rsbullet_core/
client.rs

1use std::{
2    borrow::Cow,
3    convert::{TryFrom, TryInto},
4    env,
5    ffi::{CStr, CString},
6    mem::MaybeUninit,
7    os::raw::c_char,
8    path::{Path, PathBuf},
9    ptr, slice,
10    sync::atomic::{AtomicUsize, Ordering},
11    time::Duration,
12};
13
14use nalgebra as na;
15use robot_behavior::utils::*;
16use rsbullet_sys as ffi;
17
18use crate::error::{BulletError, BulletResult};
19use crate::mode::Mode;
20use crate::types::*;
21
22// Mirror selected PyBullet limits for reference/statistics
23pub const B3_MAX_NUM_END_EFFECTORS: usize = 128;
24pub const MAX_PHYSICS_CLIENTS: usize = 1024;
25
26pub struct PhysicsClient {
27    handle: ffi::b3PhysicsClientHandle,
28    mode: Mode,
29}
30
31// Per-process GUI guard: 0 = free, 1 = occupied
32static GUI_GUARD: AtomicUsize = AtomicUsize::new(0);
33// Process-wide counters to mirror PyBullet statistics
34static TOTAL_CLIENTS: AtomicUsize = AtomicUsize::new(0);
35static GUI_CLIENTS: AtomicUsize = AtomicUsize::new(0);
36
37#[derive(Default)]
38struct GeometryScratch {
39    cstrings: Vec<CString>,
40    float64_buffers: Vec<Vec<f64>>,
41    float32_buffers: Vec<Vec<f32>>,
42    int32_buffers: Vec<Vec<i32>>,
43}
44
45impl GeometryScratch {
46    fn push_c_string(&mut self, value: &str) -> BulletResult<*const c_char> {
47        let c_string = CString::new(value)?;
48        self.cstrings.push(c_string);
49        Ok(self.cstrings.last().unwrap().as_ptr())
50    }
51
52    fn push_f64_buffer(&mut self, data: Vec<f64>) -> *const f64 {
53        self.float64_buffers.push(data);
54        self.float64_buffers.last().unwrap().as_ptr()
55    }
56
57    #[allow(dead_code)]
58    fn push_f64_buffer_mut(&mut self, data: Vec<f64>) -> *mut f64 {
59        self.float64_buffers.push(data);
60        self.float64_buffers.last_mut().unwrap().as_mut_ptr()
61    }
62
63    fn push_f32_buffer_mut(&mut self, data: Vec<f32>) -> *mut f32 {
64        self.float32_buffers.push(data);
65        self.float32_buffers.last_mut().unwrap().as_mut_ptr()
66    }
67
68    fn push_i32_buffer(&mut self, data: Vec<i32>) -> *const i32 {
69        self.int32_buffers.push(data);
70        self.int32_buffers.last().unwrap().as_ptr()
71    }
72}
73
74/// ! =====================================================================================================================================
75/// ### Connection & Session
76///
77/// | API | Status | Notes |
78/// | --- | --- | --- |
79/// | connect | **Implemented** | `PhysicsClient::connect` covers all modes in scope |
80/// | disconnect | **Implemented** | `PhysicsClient::disconnectPhysicsServer` + `Drop` |
81/// | getConnectionInfo | **Implemented** | Requires client/server introspection |
82/// | isConnected | **Implemented** | Uses `b3CanSubmitCommand` |
83impl PhysicsClient {
84    /// Connect to an existing physics server (using shared memory by default).  
85    /// - connect(method, `key=SHARED_MEMORY_KEY`, options='')
86    /// - connect(method, hostname='localhost', port=1234, options='')  
87    pub fn connect(mode: Mode) -> BulletResult<Self> {
88        // Allow only a single GUI instance per process
89
90        let mut reserved_gui = false;
91        if let Mode::Gui | Mode::GuiServer = mode {
92            // Atomically reserve if free; otherwise fail
93            if GUI_GUARD
94                .fetch_update(Ordering::SeqCst, Ordering::SeqCst, |v| {
95                    if v == 0 { Some(1) } else { None }
96                })
97                .is_ok()
98            {
99                reserved_gui = true;
100            } else {
101                return Err(BulletError::ServerUnavailable(
102                    "Only one GUI instance is allowed per process",
103                ));
104            }
105        }
106
107        let handle = match mode {
108            Mode::Direct => unsafe { ffi::b3ConnectPhysicsDirect() },
109            Mode::Gui => {
110                if cfg!(target_os = "macos") {
111                    unsafe {
112                        ffi::b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(
113                            0,
114                            ptr::null_mut(),
115                        )
116                    }
117                } else {
118                    unsafe {
119                        ffi::b3CreateInProcessPhysicsServerAndConnectSharedMemory(
120                            0,
121                            ptr::null_mut(),
122                        )
123                    }
124                }
125            }
126            Mode::GuiMainThread => unsafe {
127                ffi::b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(
128                    0,
129                    ptr::null_mut(),
130                )
131            },
132            Mode::GuiServer => {
133                if cfg!(target_os = "macos") {
134                    unsafe {
135                        ffi::b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(
136                            0,
137                            ptr::null_mut(),
138                        )
139                    }
140                } else {
141                    unsafe {
142                        ffi::b3CreateInProcessPhysicsServerAndConnectSharedMemory(
143                            0,
144                            ptr::null_mut(),
145                        )
146                    }
147                }
148            }
149            Mode::Udp { hostname, port } => {
150                if cfg!(feature = "enet") {
151                    let host_name = CString::new(hostname)?;
152                    unsafe {
153                        ffi::b3ConnectPhysicsUDP(host_name.as_ptr(), port.unwrap_or(1234) as i32)
154                    }
155                } else {
156                    return Err(BulletError::ServerUnavailable(
157                        "UDP is not enabled in this pybullet build",
158                    ));
159                }
160            }
161            Mode::Tcp { hostname, port } => {
162                if cfg!(feature = "clsocket") {
163                    let host_name = CString::new(hostname)?;
164                    unsafe {
165                        ffi::b3ConnectPhysicsTCP(host_name.as_ptr(), port.unwrap_or(6667) as i32)
166                    }
167                } else {
168                    return Err(BulletError::ServerUnavailable(
169                        "TCP is not enabled in this pybullet build",
170                    ));
171                }
172            }
173            Mode::GraphicsServerMainThread { port } => unsafe {
174                ffi::b3CreateInProcessGraphicsServerAndConnectMainThreadSharedMemory(
175                    port.unwrap_or(6667) as i32,
176                )
177            },
178            Mode::GraphicsServer { port } => {
179                if cfg!(target_os = "macos") {
180                    unsafe {
181                        ffi::b3CreateInProcessGraphicsServerAndConnectMainThreadSharedMemory(
182                            port.unwrap_or(6667) as i32,
183                        )
184                    }
185                } else {
186                    unsafe {
187                        ffi::b3CreateInProcessGraphicsServerAndConnectSharedMemory(
188                            port.unwrap_or(6667) as i32,
189                        )
190                    }
191                }
192            }
193            Mode::GraphicsServerTcp { hostname, port } => {
194                if cfg!(feature = "clsocket") {
195                    let host_name = CString::new(hostname)?;
196                    unsafe {
197                        ffi::b3ConnectPhysicsTCP(host_name.as_ptr(), port.unwrap_or(6667) as i32)
198                    }
199                } else {
200                    return Err(BulletError::ServerUnavailable(
201                        "TCP is not enabled in this pybullet build",
202                    ));
203                }
204            }
205            Mode::SharedMemory => unsafe { ffi::b3ConnectSharedMemory(ffi::SHARED_MEMORY_KEY) },
206            Mode::SharedMemoryServer => unsafe {
207                ffi::b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(
208                    ptr::null_mut(),
209                    ffi::SHARED_MEMORY_KEY,
210                )
211            },
212            Mode::SharedMemoryGui | Mode::GraphicsClient => unsafe {
213                ffi::b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect4(
214                    ptr::null_mut(),
215                    ffi::SHARED_MEMORY_KEY,
216                )
217            },
218
219            #[cfg(feature = "dart")]
220            Mode::Dart => unsafe { ffi::b3ConnectPhysicsDART() },
221            #[cfg(feature = "physx")]
222            Mode::PhysX => unsafe { ffi::b3ConnectPhysicsPhysX(0, ptr::null_mut()) },
223            #[cfg(feature = "mujoco")]
224            Mode::MuJoCo => unsafe { ffi::b3ConnectPhysicsMuJoCo() },
225            #[cfg(feature = "grpc")]
226            Mode::Grpc { hostname, port } => {
227                let host_name = CString::new(hostname)?;
228                unsafe { ffi::b3ConnectPhysicsGRPC(host_name.as_ptr(), port.unwrap_or(-1)) }
229            }
230        }
231        .ok_or(BulletError::NullPointer(
232            "Bullet returned a null physics client handle",
233        ))?;
234
235        let mut client = PhysicsClient { handle, mode };
236        client.ensure_can_submit()?;
237
238        unsafe {
239            let command = ffi::b3InitSyncBodyInfoCommand(client.handle);
240            client.submit_simple_command(
241                command,
242                ffi::EnumSharedMemoryServerStatus::CMD_SYNC_BODY_INFO_COMPLETED,
243            )?;
244
245            let command = ffi::b3InitSyncUserDataCommand(client.handle);
246            client.submit_simple_command(
247                command,
248                ffi::EnumSharedMemoryServerStatus::CMD_SYNC_USER_DATA_COMPLETED,
249            )?;
250        }
251
252        // Successful connect: update counters
253        TOTAL_CLIENTS.fetch_add(1, Ordering::SeqCst);
254        if reserved_gui {
255            GUI_CLIENTS.fetch_add(1, Ordering::SeqCst);
256        }
257
258        Ok(client)
259    }
260
261    pub fn get_connection_info(&mut self) -> BulletResult<String> {
262        let is_connected = self.is_connected();
263        let mode = &self.mode;
264        Ok(format!(
265            "{{\"isConnected\": {is_connected}, \"mode\": \"{mode:?}\"}}"
266        ))
267    }
268
269    /// "Disconnect from the physics server."
270    pub fn disconnect(self) {
271        drop(self);
272    }
273
274    pub fn is_connected(&mut self) -> bool {
275        self.can_submit_command()
276    }
277
278    /// Returns whether or not this client can submit a command.
279    pub(crate) fn can_submit_command(&mut self) -> bool {
280        unsafe { ffi::b3CanSubmitCommand(self.handle) != 0 }
281    }
282}
283
284/// ! =====================================================================================================================================
285/// ### Simulation Parameters
286///
287/// | API | Status | Notes |
288/// | --- | --- | --- |
289/// | resetSimulation | **Implemented** | Core reset command |
290/// | stepSimulation | **Implemented** | Core stepping command |
291/// | performCollisionDetection | **Implemented** | Extra collision command |
292/// | setGravity | **Implemented** | Physics param command |
293/// | setTimeStep | **Implemented** | Physics param command |
294/// | setDefaultContactERP | **Implemented** | Physics param command |
295/// | setRealTimeSimulation | **Implemented** | Physics param command |
296/// | setPhysicsEngineParameter | **Implemented** | Bulk physics-param configuration |
297/// | getPhysicsEngineParameters | **Implemented** | Query mirror of above |
298/// | setInternalSimFlags | **Implemented** | Expert-only flagging |
299impl PhysicsClient {
300    /// `reset_simulation` will remove all objects from the world and reset the world to initial conditions.
301    pub fn reset_simulation(&mut self) -> BulletResult<()> {
302        self.ensure_can_submit()?;
303        let command = unsafe { ffi::b3InitResetSimulationCommand(self.handle) };
304        unsafe { ffi::b3InitResetSimulationSetFlags(command, 0) };
305        let _status_handle =
306            unsafe { ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command) };
307        Ok(())
308    }
309
310    /// Performs all the actions in a single forward dynamics simulation step such as collision
311    /// detection, constraint solving, and integration.
312    /// TODO: Return analytics data?
313    pub fn step_simulation(&mut self) -> BulletResult<()> {
314        self.ensure_can_submit()?;
315        let command = unsafe { ffi::b3InitStepSimulationCommand(self.handle) };
316        self.submit_simple_command(
317            command,
318            ffi::EnumSharedMemoryServerStatus::CMD_STEP_FORWARD_SIMULATION_COMPLETED,
319        )?;
320        Ok(())
321    }
322
323    pub fn perform_collision_detection(&mut self) -> BulletResult<()> {
324        self.ensure_can_submit()?;
325        let command = unsafe { ffi::b3InitPerformCollisionDetectionCommand(self.handle) };
326        let _ = self.submit_command(command)?;
327        Ok(())
328    }
329
330    /// Sets the default gravity force for all objects.
331    ///
332    /// By default, there is no gravitational force enabled.
333    ///
334    /// # Arguments
335    /// * `gravity` - a gravity vector. Can be a \[f64;3\]-array or anything else that can be
336    ///   converted into [f64; 3].
337    pub fn set_gravity(&mut self, grav: impl Into<[f64; 3]>) -> BulletResult<&mut Self> {
338        self.ensure_can_submit()?;
339        let grav = grav.into();
340        let command = unsafe { ffi::b3InitPhysicsParamCommand(self.handle) };
341        unsafe {
342            ffi::b3PhysicsParamSetGravity(command, grav[0], grav[1], grav[2]);
343        }
344        self.submit_simple_command(
345            command,
346            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
347        )?;
348        Ok(self)
349    }
350
351    /// Warning: in many cases it is best to leave the timeStep to default, which is 240Hz.
352    /// Several parameters are tuned with this value in mind. For example the number of solver
353    /// iterations and the error reduction parameters (erp) for contact, friction and non-contact
354    /// joints are related to the time step. If you change the time step, you may need to re-tune
355    /// those values accordingly, especially the erp values.
356    ///
357    /// You can set the physics engine timestep that is used when calling
358    /// [`step_simulation`](`Self::step_simulation()`).
359    /// It is best to only call this method at the start of a simulation.
360    /// Don't change this time step regularly.
361    pub fn set_time_step(&mut self, time_step: Duration) -> BulletResult<&mut Self> {
362        self.ensure_can_submit()?;
363        let command = unsafe { ffi::b3InitPhysicsParamCommand(self.handle) };
364        unsafe {
365            ffi::b3PhysicsParamSetTimeStep(command, time_step.as_secs_f64());
366        }
367        self.submit_simple_command(
368            command,
369            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
370        )?;
371        Ok(self)
372    }
373
374    /// Update the global default contact ERP (error reduction parameter).
375    pub fn set_default_contact_erp(&mut self, default_contact_erp: f64) -> BulletResult<&mut Self> {
376        self.ensure_can_submit()?;
377        let command = unsafe { ffi::b3InitPhysicsParamCommand(self.handle) };
378        unsafe {
379            ffi::b3PhysicsParamSetDefaultContactERP(command, default_contact_erp);
380        }
381        self.submit_simple_command(
382            command,
383            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
384        )?;
385        Ok(self)
386    }
387
388    /// By default, the physics server will not step the simulation, unless you explicitly send a
389    /// [`step_simulation`](`Self::step_simulation()`) command.
390    /// This way you can maintain control determinism of the simulation
391    /// It is possible to run the simulation in real-time by letting the physics server
392    /// automatically step the simulation according to its real-time-clock (RTC) using the
393    /// `set_real_time_simulation` command. If you enable the real-time simulation,
394    /// you don't need to call [`step_simulation`](`Self::step_simulation()`).
395    ///
396    /// Note that `set_real_time_simulation` has no effect in
397    /// [`Direct mode`](`crate::mode::Mode::Direct`) :
398    /// in [`Direct mode`](`crate::mode::Mode::Direct`) mode the physics
399    /// server and client happen in the same thread and you trigger every command.
400    /// In [`Gui mode`](`crate::mode::Mode::Gui`) and in Virtual Reality mode, and TCP/UDP mode,
401    /// the physics server runs in a separate thread from the client (`RuBullet`),
402    /// and `set_real_time_simulation` allows the physics server thread
403    /// to add additional calls to  [`step_simulation`](`Self::step_simulation()`).
404    ///
405    /// # Arguments
406    /// * `enable_real_time_simulation` - activates or deactivates real-time simulation
407    pub fn set_real_time_simulation(&mut self, enable: bool) -> BulletResult<&mut Self> {
408        self.ensure_can_submit()?;
409        let command = unsafe { ffi::b3InitPhysicsParamCommand(self.handle) };
410        unsafe {
411            ffi::b3PhysicsParamSetRealTimeSimulation(command, enable as i32);
412        }
413        self.submit_simple_command(
414            command,
415            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
416        )?;
417        Ok(self)
418    }
419
420    /// Apply a batch of physics engine parameters, mirroring `PyBullet`'s `setPhysicsEngineParameter`.
421    pub fn set_physics_engine_parameter(
422        &mut self,
423        update: &PhysicsEngineParametersUpdate,
424    ) -> BulletResult<&mut Self> {
425        self.ensure_can_submit()?;
426        let command = unsafe { ffi::b3InitPhysicsParamCommand(self.handle) };
427        Self::apply_physics_engine_update(command, update);
428        self.submit_simple_command(
429            command,
430            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
431        )?;
432        Ok(self)
433    }
434
435    /// Query the current physics simulation parameters from the server.
436    pub fn get_physics_engine_parameters(&mut self) -> BulletResult<PhysicsEngineParameters> {
437        self.ensure_can_submit()?;
438        let command = unsafe { ffi::b3InitRequestPhysicsParamCommand(self.handle) };
439        let status = self.submit_simple_command(
440            command,
441            ffi::EnumSharedMemoryServerStatus::CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED,
442        )?;
443
444        let mut raw = MaybeUninit::<ffi::b3PhysicsSimulationParameters>::uninit();
445        let result =
446            unsafe { ffi::b3GetStatusPhysicsSimulationParameters(status.handle, raw.as_mut_ptr()) };
447        if result == 0 {
448            return Err(BulletError::CommandFailed {
449                message: "Failed to fetch physics simulation parameters",
450                code: result,
451            });
452        }
453        unsafe { raw.assume_init() }.try_into()
454    }
455
456    /// Set internal simulation flags (expert-level API).
457    pub fn set_internal_sim_flags(&mut self, flags: i32) -> BulletResult<&mut Self> {
458        self.ensure_can_submit()?;
459        let command = unsafe { ffi::b3InitPhysicsParamCommand(self.handle) };
460        unsafe {
461            ffi::b3PhysicsParamSetInternalSimFlags(command, flags);
462        }
463        self.submit_simple_command(
464            command,
465            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
466        )?;
467        Ok(self)
468    }
469}
470
471// ! =====================================================================================================================================
472// ### World Authoring & Persistence
473//
474// | API | Status | Notes |
475// | --- | --- | --- |
476// | loadURDF | **Implemented** | Supports position/orientation/options |
477// | loadSDF | **Implemented** | Returns body list |
478// | loadSoftBody | Optional | Requires soft-body build support |
479// | createSoftBodyAnchor | Optional | Soft-body specific |
480// | loadBullet | **Implemented** | World snapshot load |
481// | saveBullet | **Implemented** | World snapshot save |
482// | restoreState | **Implemented** | In-memory state restore |
483// | saveState | **Implemented** | In-memory state save |
484// | removeState | **Implemented** | Pair with saveState |
485// | loadMJCF | **Implemented** | Returns body list |
486// | saveWorld | **Implemented** | Script export helper |
487// | setAdditionalSearchPath | **Implemented** | Search path registry |
488// | vhacd | Optional | Requires VHACD build flag |
489
490impl PhysicsClient {
491    /// Sends a command to the physics server to load a physics model from a Unified Robot
492    /// Description Format (URDF) model.
493    ///
494    /// # Arguments
495    /// * `file` - a relative or absolute path to the URDF file on the file system of the physics server
496    /// * `options` - use additional options like `global_scaling` and `use_maximal_coordinates` for
497    ///   loading the URDF-file. See [`UrdfOptions`](`crate::types::UrdfOptions`).
498    /// # Example
499    /// ```rust
500    /// use rsbullet_core::*;
501    /// fn main() -> BulletResult<&mut Self> {
502    ///     let mut physics_client = PhysicsClient::connect(Mode::Direct)?;
503    ///     physics_client.set_additional_search_path(PhysicsClient::bullet_data_path())?;
504    ///     let cube = physics_client.load_urdf("cube.urdf", None)?;
505    ///     assert_eq!("baseLink", physics_client.get_body_info(cube)?.base_name);
506    ///     for i in 0..10 {
507    ///         let _cube = physics_client.load_urdf(
508    ///             "cube.urdf",
509    ///             None,
510    ///         )?;
511    ///     }
512    ///     assert_eq!(11, physics_client.get_num_bodies());
513    ///     Ok(())
514    /// }
515    /// ```
516    pub fn load_urdf(
517        &mut self,
518        file: impl AsRef<Path>,
519        options: Option<impl Into<UrdfOptions>>,
520    ) -> BulletResult<i32> {
521        self.ensure_can_submit()?;
522        let file_c = Self::path_to_cstring(file.as_ref())?;
523        let command = unsafe { ffi::b3LoadUrdfCommandInit(self.handle, file_c.as_ptr()) };
524
525        if let Some(options) = options {
526            let options = options.into();
527
528            if let Some(flags) = options.flags {
529                unsafe {
530                    ffi::b3LoadUrdfCommandSetFlags(command, flags.bits());
531                }
532            }
533
534            if let Some(base) = options.base.as_ref() {
535                let ([x, y, z], [ox, oy, oz, ow]) = isometry_to_raw_parts(base);
536                unsafe {
537                    ffi::b3LoadUrdfCommandSetStartPosition(command, x, y, z);
538                    ffi::b3LoadUrdfCommandSetStartOrientation(command, ox, oy, oz, ow);
539                }
540            }
541
542            if let Some(use_maximal) = options.use_maximal_coordinates {
543                unsafe {
544                    ffi::b3LoadUrdfCommandSetUseMultiBody(command, (!use_maximal) as i32);
545                }
546            }
547
548            if options.use_fixed_base {
549                unsafe {
550                    ffi::b3LoadUrdfCommandSetUseFixedBase(command, 1);
551                }
552            }
553
554            if let Some(global_scaling) = options.global_scaling {
555                unsafe {
556                    ffi::b3LoadUrdfCommandSetGlobalScaling(command, global_scaling);
557                }
558            }
559        }
560
561        let status = self.submit_simple_command(
562            command,
563            ffi::EnumSharedMemoryServerStatus::CMD_URDF_LOADING_COMPLETED,
564        )?;
565
566        let body_id = unsafe { ffi::b3GetStatusBodyIndex(status.handle) };
567        Ok(body_id)
568    }
569
570    /// Sends a command to the physics server to load a physics model from
571    /// a Simulation Description Format (SDF) model.
572    /// # Arguments
573    /// * `file` - a relative or absolute path to the SDF file on the file system of the physics server.
574    /// * `options` -  use additional options like `global_scaling` and `use_maximal_coordinates` for
575    ///   loading the SDF-file. See [`SdfOptions`](`crate::types::SdfOptions`).
576    ///    
577    /// # Return
578    /// Returns a list of unique body id's
579    ///
580    /// # Example
581    /// ```rust
582    /// use rsbullet_core::*;
583    /// fn main() -> BulletResult<&mut Self> {
584    ///     let mut physics_client = PhysicsClient::connect(Mode::Direct)?;
585    ///     physics_client.set_additional_search_path(PhysicsClient::bullet_data_path())?;
586    ///     let cubes = physics_client.load_sdf("two_cubes.sdf", None)?;
587    ///     assert_eq!(3, cubes.len()); // 2 cubes + 1 plane
588    ///     Ok(())
589    /// }
590    /// ```
591    pub fn load_sdf(
592        &mut self,
593        file: impl AsRef<Path>,
594        options: impl Into<Option<SdfOptions>>,
595    ) -> BulletResult<Vec<i32>> {
596        self.ensure_can_submit()?;
597        let file_c = Self::path_to_cstring(file.as_ref())?;
598        let command = unsafe { ffi::b3LoadSdfCommandInit(self.handle, file_c.as_ptr()) };
599
600        let options = options.into().unwrap_or_default();
601
602        if let Some(use_maximal) = options.use_maximal_coordinates {
603            unsafe {
604                ffi::b3LoadSdfCommandSetUseMultiBody(command, (!use_maximal) as i32);
605            }
606        }
607
608        if let Some(global_scaling) = options.global_scaling {
609            unsafe {
610                ffi::b3LoadSdfCommandSetUseGlobalScaling(command, global_scaling);
611            }
612        }
613
614        let status = self.submit_simple_command(
615            command,
616            ffi::EnumSharedMemoryServerStatus::CMD_SDF_LOADING_COMPLETED,
617        )?;
618
619        Self::collect_body_indices(status.handle)
620    }
621
622    /// Sends a command to the physics server to load a physics model from
623    /// a `MuJoCo` model.
624    /// # Arguments
625    /// * `file` - a relative or absolute path to the `MuJoCo` file on the file system of the physics server.
626    /// * `flags` -  Flags for loading the model. Set to None if you do not whish to provide any.
627    ///
628    /// # Return
629    /// Returns a list of unique body id's
630    ///
631    /// # Example
632    /// ```rust
633    /// use rsbullet_core::*;
634    /// fn main() -> BulletResult<&mut Self> {
635    ///     let mut physics_client = PhysicsClient::connect(Mode::Direct)?;
636    ///     physics_client.set_additional_search_path(PhysicsClient::bullet_data_path())?;
637    ///     let stadium = physics_client.load_mjcf("mjcf/hello_mjcf.xml", None)?;
638    ///     assert_eq!(2, stadium.len()); // 1 cube + 1 plane
639    ///
640    ///     let plane = physics_client.load_mjcf("mjcf/ground_plane.xml", LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES)?;
641    ///     assert_eq!(1, plane.len());
642    ///     Ok(())
643    /// }
644    /// ```
645    pub fn load_mjcf(
646        &mut self,
647        file: impl AsRef<Path>,
648        options: impl Into<Option<MjcfOptions>>,
649    ) -> BulletResult<Vec<i32>> {
650        self.ensure_can_submit()?;
651        let file_c = Self::path_to_cstring(file.as_ref())?;
652        let command = unsafe { ffi::b3LoadMJCFCommandInit(self.handle, file_c.as_ptr()) };
653
654        let options = options.into().unwrap_or_default();
655
656        if let Some(flags) = options.flags {
657            unsafe {
658                ffi::b3LoadMJCFCommandSetFlags(command, flags.bits());
659            }
660        }
661
662        if let Some(use_multi_body) = options.use_multi_body {
663            unsafe {
664                ffi::b3LoadMJCFCommandSetUseMultiBody(command, use_multi_body as i32);
665            }
666        }
667
668        let status = self.submit_simple_command(
669            command,
670            ffi::EnumSharedMemoryServerStatus::CMD_MJCF_LOADING_COMPLETED,
671        )?;
672
673        Self::collect_body_indices(status.handle)
674    }
675
676    /// Loads Bodies from a `.bullet` file. These can be created with [`save_bullet`](`Self::save_bullet`).
677    ///
678    /// Returns a list of `BodyId`'s.
679    /// # Arguments
680    /// * `bullet_filename` - location of the `.bullet`
681    /// # Example
682    /// ```rust
683    ///# use rsbullet::*;
684    ///#
685    ///# fn main() -> BulletResult<&mut Self> {
686    ///#     let mut physics_client = PhysicsClient::connect(Mode::Direct)?;
687    ///#     physics_client.set_additional_search_path(PhysicsClient::bullet_data_path())?;
688    ///     let points = physics_client.load_bullet("spider.bullet")?;
689    ///#     Ok(())
690    ///# }
691    /// ```
692    /// See also `save_and_restore.rs` example.
693    pub fn load_bullet(&mut self, file: impl AsRef<Path>) -> BulletResult<Vec<i32>> {
694        self.ensure_can_submit()?;
695        let file_c = Self::path_to_cstring(file.as_ref())?;
696        let command = unsafe { ffi::b3LoadBulletCommandInit(self.handle, file_c.as_ptr()) };
697        let status = self.submit_simple_command(
698            command,
699            ffi::EnumSharedMemoryServerStatus::CMD_BULLET_LOADING_COMPLETED,
700        )?;
701
702        Self::collect_body_indices(status.handle)
703    }
704    /// Saves all bodies and the current state into a `.bullet` file which can then be read by
705    /// [`load_bullet`](`Self::load_bullet`) or [`restore_state_from_file`](`Self::restore_state_from_file`).
706    /// See also `save_and_restore.rs` example.
707    pub fn save_bullet(&mut self, file: impl AsRef<Path>) -> BulletResult<()> {
708        self.ensure_can_submit()?;
709        let file_c = Self::path_to_cstring(file.as_ref())?;
710        let command = unsafe { ffi::b3SaveBulletCommandInit(self.handle, file_c.as_ptr()) };
711        self.submit_simple_command(
712            command,
713            ffi::EnumSharedMemoryServerStatus::CMD_BULLET_SAVING_COMPLETED,
714        )?;
715        Ok(())
716    }
717
718    pub fn save_world(&mut self, file: impl AsRef<Path>) -> BulletResult<()> {
719        self.ensure_can_submit()?;
720        let file_c = Self::path_to_cstring(file.as_ref())?;
721        let command = unsafe { ffi::b3SaveWorldCommandInit(self.handle, file_c.as_ptr()) };
722        self.submit_simple_command(
723            command,
724            ffi::EnumSharedMemoryServerStatus::CMD_SAVE_WORLD_COMPLETED,
725        )?;
726        Ok(())
727    }
728
729    /// restores a state from memory using a state id which was created with [`save_state`](`Self::save_state`).
730    /// See `save_and_restore.rs` example.
731    pub fn restore_state(
732        &mut self,
733        state_id: Option<i32>,
734        file: Option<impl AsRef<Path>>,
735    ) -> BulletResult<()> {
736        self.ensure_can_submit()?;
737        let command = unsafe { ffi::b3LoadStateCommandInit(self.handle) };
738
739        if let Some(state_id) = state_id {
740            unsafe {
741                ffi::b3LoadStateSetStateId(command, state_id);
742            }
743        }
744
745        if let Some(ref file) = file {
746            let file_c = Self::path_to_cstring(file.as_ref())?;
747            unsafe { ffi::b3LoadStateSetFileName(command, file_c.as_ptr()) };
748        }
749
750        self.submit_simple_command(
751            command,
752            ffi::EnumSharedMemoryServerStatus::CMD_RESTORE_STATE_COMPLETED,
753        )?;
754        Ok(())
755    }
756    /// Saves the current state in memory and returns a `StateId` which can be used by [`restore_state`](`Self::restore_state`)
757    /// to restore this state.  Use [`save_bullet`](`Self::save_bullet`) if you want to save a state
758    /// to a file.
759    /// See `save_and_restore.rs` example.
760    pub fn save_state(&mut self) -> BulletResult<i32> {
761        self.ensure_can_submit()?;
762        let command = unsafe { ffi::b3SaveStateCommandInit(self.handle) };
763        let status = self.submit_simple_command(
764            command,
765            ffi::EnumSharedMemoryServerStatus::CMD_SAVE_STATE_COMPLETED,
766        )?;
767
768        let state_id = unsafe { ffi::b3GetStatusGetStateId(status.handle) };
769        Ok(state_id)
770    }
771    /// Removes a state from memory.
772    pub fn remove_state(&mut self, state_id: i32) -> BulletResult<()> {
773        if state_id < 0 {
774            return Ok(());
775        }
776        self.ensure_can_submit()?;
777        let command = unsafe { ffi::b3InitRemoveStateCommand(self.handle, state_id) };
778        self.submit_simple_command(
779            command,
780            ffi::EnumSharedMemoryServerStatus::CMD_REMOVE_STATE_COMPLETED,
781        )?;
782        Ok(())
783    }
784
785    pub fn set_default_search_path(&mut self) -> BulletResult<&mut Self> {
786        self.set_additional_search_path(Self::bullet_data_path())
787    }
788
789    pub fn set_additional_search_path(
790        &mut self,
791        path: impl AsRef<Path>,
792    ) -> BulletResult<&mut Self> {
793        self.ensure_can_submit()?;
794        let path_c = Self::path_to_cstring(path.as_ref())?;
795        let command = unsafe { ffi::b3SetAdditionalSearchPath(self.handle, path_c.as_ptr()) };
796        self.submit_simple_command(
797            command,
798            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
799        )?;
800        Ok(self)
801    }
802
803    pub fn bullet_data_path() -> PathBuf {
804        {
805            #[cfg(target_os = "windows")]
806            {
807                env::var_os("LOCALAPPDATA")
808                    .or_else(|| env::var_os("USERPROFILE"))
809                    .map(PathBuf::from)
810            }
811
812            #[cfg(target_os = "macos")]
813            {
814                use std::path::PathBuf;
815                env::var_os("HOME")
816                    .map(PathBuf::from)
817                    .map(|home| home.join("Library").join("Application Support"))
818            }
819
820            #[cfg(all(not(target_os = "windows"), not(target_os = "macos")))]
821            {
822                use std::path::PathBuf;
823                env::var_os("HOME")
824                    .map(PathBuf::from)
825                    .map(|home| home.join(".local").join("share"))
826            }
827        }
828        .map(|path| path.join("bullet"))
829        .unwrap()
830    }
831}
832
833/// ! =====================================================================================================================================
834/// ### Asset Creation & Mutation
835///
836/// | API | Status | Notes |
837/// | --- | --- | --- |
838/// | createCollisionShape | **Implemented** | Supports primitive/mesh geometry |
839/// | createCollisionShapeArray | **Implemented** | Compound shape builder |
840/// | removeCollisionShape | **Implemented** | Clean-up helper |
841/// | getMeshData | Pending | Mesh inspection |
842/// | getTetraMeshData | Pending | Soft-body mesh |
843/// | resetMeshData | Optional | Deformable specific |
844/// | createVisualShape | **Implemented** | Visual geometry authoring |
845/// | createVisualShapeArray | **Implemented** | Bulk visual authoring |
846/// | createMultiBody | **Implemented** | Procedural multibody build |
847/// | createConstraint | **Implemented** | Constraint authoring |
848/// | changeConstraint | **Implemented** | Constraint mutation |
849/// | removeConstraint | **Implemented** | Constraint teardown |
850/// | enableJointForceTorqueSensor | **Implemented** | Sensor toggle |
851/// | removeBody | **Implemented** | Body teardown |
852/// | getNumConstraints | **Implemented** | Constraint enumeration |
853/// | getConstraintInfo | **Implemented** | Constraint query |
854/// | getConstraintState | **Implemented** | Constraint forces |
855/// | getConstraintUniqueId | **Implemented** | Constraint enumeration |
856/// | changeVisualShape | **Implemented** | Visual mutation |
857/// | resetVisualShapeData | Pending | Legacy alias |
858/// | loadTexture | **Implemented** | Visual assets |
859/// | changeTexture | **Implemented** | Visual assets |
860impl PhysicsClient {
861    /// You can create a collision shape in a similar way to creating a visual shape. If you have
862    /// both you can use them to create objects in `RuBullet`.
863    /// # Arguments
864    /// * `shape` - A geometric body from which to create the shape
865    /// * `frame_offset` - offset of the shape with respect to the link frame. Default is no offset.
866    ///
867    /// # Return
868    /// Returns a unique [`CollisionId`](crate::CollisionId) which can then be used to create a body.
869    /// # See also
870    /// * [`create_visual_shape`](`Self::create_visual_shape`)
871    /// * [`create_multi_body`](`Self::create_multi_body`)
872    pub fn create_collision_shape(
873        &mut self,
874        geometry: &CollisionGeometry<'_>,
875        options: Option<impl Into<CollisionShapeOptions>>,
876    ) -> BulletResult<i32> {
877        self.ensure_can_submit()?;
878        let mut scratch = GeometryScratch::default();
879        let command = unsafe { ffi::b3CreateCollisionShapeCommandInit(self.handle) };
880        let shape_index = self.add_collision_geometry(command, geometry, &mut scratch)?;
881
882        let CollisionShapeOptions { transform, flags } =
883            options.map_or(CollisionShapeOptions::default(), Into::into);
884
885        if let Some(flags) = flags {
886            unsafe { ffi::b3CreateCollisionSetFlag(command, shape_index, flags) };
887        }
888
889        let (position, orientation) = isometry_to_raw_parts(&transform);
890        unsafe {
891            ffi::b3CreateCollisionShapeSetChildTransform(
892                command,
893                shape_index,
894                position.as_ptr(),
895                orientation.as_ptr(),
896            );
897        }
898
899        let status = self.submit_simple_command(
900            command,
901            ffi::EnumSharedMemoryServerStatus::CMD_CREATE_COLLISION_SHAPE_COMPLETED,
902        )?;
903        let shape_id = unsafe { ffi::b3GetStatusCollisionShapeUniqueId(status.handle) };
904        if shape_id < 0 {
905            return Err(BulletError::CommandFailed {
906                message: "Bullet failed to create collision shape",
907                code: shape_id,
908            });
909        }
910        Ok(shape_id)
911    }
912
913    pub fn create_collision_shape_array(
914        &mut self,
915        options: &'_ [(
916            CollisionGeometry<'_>,
917            impl Clone + Into<CollisionShapeOptions>,
918        )],
919    ) -> BulletResult<i32> {
920        if options.is_empty() {
921            return Err(BulletError::CommandFailed {
922                message: "Collision shape array requires at least one entry",
923                code: -1,
924            });
925        }
926
927        self.ensure_can_submit()?;
928        let mut scratch = GeometryScratch::default();
929        let command = unsafe { ffi::b3CreateCollisionShapeCommandInit(self.handle) };
930
931        for child in options {
932            let (geometry, option) = child;
933            let CollisionShapeOptions { transform, flags } = option.clone().into();
934            let index = self.add_collision_geometry(command, geometry, &mut scratch)?;
935            if let Some(flags) = flags {
936                unsafe { ffi::b3CreateCollisionSetFlag(command, index, flags) };
937            }
938            let (position, orientation) = isometry_to_raw_parts(&transform);
939            unsafe {
940                ffi::b3CreateCollisionShapeSetChildTransform(
941                    command,
942                    index,
943                    position.as_ptr(),
944                    orientation.as_ptr(),
945                );
946            }
947        }
948
949        let status = self.submit_simple_command(
950            command,
951            ffi::EnumSharedMemoryServerStatus::CMD_CREATE_COLLISION_SHAPE_COMPLETED,
952        )?;
953        let shape_id = unsafe { ffi::b3GetStatusCollisionShapeUniqueId(status.handle) };
954        if shape_id < 0 {
955            return Err(BulletError::CommandFailed {
956                message: "Bullet failed to create collision shape array",
957                code: shape_id,
958            });
959        }
960        Ok(shape_id)
961    }
962
963    pub fn remove_collision_shape(&mut self, collision_shape_id: i32) -> BulletResult<()> {
964        self.ensure_can_submit()?;
965        let command =
966            unsafe { ffi::b3InitRemoveCollisionShapeCommand(self.handle, collision_shape_id) };
967        self.submit_simple_command(
968            command,
969            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
970        )?;
971        Ok(())
972    }
973
974    /// You can create a visual shape in a similar way to creating a collision shape, with some
975    /// additional arguments to control the visual appearance, such as diffuse and specular color.
976    /// When you use the [`GeometricVisualShape::MeshFile`](`crate::GeometricVisualShape::MeshFile`)
977    /// type, you can point to a Wavefront OBJ file, and the
978    /// visual shape will parse some parameters from the material file (.mtl) and load a texture.
979    /// Note that large textures (above 1024x1024 pixels)
980    /// can slow down the loading and run-time performance.
981    ///
982    /// # Arguments
983    /// * `shape` - A geometric body from which to create the shape
984    /// * `options` - additional options to specify, like colors. See [`VisualShapeOptions`](crate::VisualShapeOptions)
985    ///   for details.
986    /// # Return
987    /// Returns a unique [`VisualId`](crate::VisualId) which can then be used to create a body.
988    /// # See also
989    /// * [`create_collision_shape`](`Self::create_collision_shape`)
990    /// * [`create_multi_body`](`Self::create_multi_body`)
991    pub fn create_visual_shape(
992        &mut self,
993        geometry: &VisualGeometry<'_>,
994        options: Option<impl Into<VisualShapeOptions>>,
995    ) -> BulletResult<i32> {
996        self.ensure_can_submit()?;
997        let mut scratch = GeometryScratch::default();
998        let command = unsafe { ffi::b3CreateVisualShapeCommandInit(self.handle) };
999
1000        let shape_index = self.add_visual_geometry(command, geometry, &mut scratch)?;
1001
1002        let options = options.map_or(VisualShapeOptions::default(), Into::into);
1003        if let Some(flags) = options.flags {
1004            unsafe { ffi::b3CreateVisualSetFlag(command, shape_index, flags.bits()) };
1005        }
1006        let (position, orientation) = isometry_to_raw_parts(&options.transform);
1007        unsafe {
1008            ffi::b3CreateVisualShapeSetChildTransform(
1009                command,
1010                shape_index,
1011                position.as_ptr(),
1012                orientation.as_ptr(),
1013            );
1014        }
1015        unsafe {
1016            ffi::b3CreateVisualShapeSetRGBAColor(command, shape_index, options.rgba.as_ptr());
1017        }
1018        unsafe {
1019            ffi::b3CreateVisualShapeSetSpecularColor(
1020                command,
1021                shape_index,
1022                options.specular.as_ptr(),
1023            );
1024        }
1025
1026        let status = self.submit_simple_command(
1027            command,
1028            ffi::EnumSharedMemoryServerStatus::CMD_CREATE_VISUAL_SHAPE_COMPLETED,
1029        )?;
1030        let visual_id = unsafe { ffi::b3GetStatusVisualShapeUniqueId(status.handle) };
1031        if visual_id < 0 {
1032            return Err(BulletError::CommandFailed {
1033                message: "Bullet failed to create visual shape",
1034                code: visual_id,
1035            });
1036        }
1037        Ok(visual_id)
1038    }
1039
1040    pub fn create_visual_shape_array(
1041        &mut self,
1042        options: &'_ [(VisualGeometry, Option<VisualShapeOptions>)],
1043    ) -> BulletResult<i32> {
1044        if options.is_empty() {
1045            return Err(BulletError::CommandFailed {
1046                message: "Visual shape array requires at least one entry",
1047                code: -1,
1048            });
1049        }
1050
1051        self.ensure_can_submit()?;
1052        let mut scratch = GeometryScratch::default();
1053        let command = unsafe { ffi::b3CreateVisualShapeCommandInit(self.handle) };
1054
1055        for child in options {
1056            let (geometry, options) = child;
1057
1058            let shape_index = self.add_visual_geometry(command, geometry, &mut scratch)?;
1059
1060            let options = options.unwrap_or_default();
1061            if let Some(flags) = &options.flags {
1062                unsafe { ffi::b3CreateVisualSetFlag(command, shape_index, flags.bits()) };
1063            }
1064            let (position, orientation) = isometry_to_raw_parts(&options.transform);
1065            unsafe {
1066                ffi::b3CreateVisualShapeSetChildTransform(
1067                    command,
1068                    shape_index,
1069                    position.as_ptr(),
1070                    orientation.as_ptr(),
1071                );
1072            }
1073            unsafe {
1074                ffi::b3CreateVisualShapeSetRGBAColor(command, shape_index, options.rgba.as_ptr());
1075            }
1076            unsafe {
1077                ffi::b3CreateVisualShapeSetSpecularColor(
1078                    command,
1079                    shape_index,
1080                    options.specular.as_ptr(),
1081                );
1082            }
1083        }
1084
1085        let status = self.submit_simple_command(
1086            command,
1087            ffi::EnumSharedMemoryServerStatus::CMD_CREATE_VISUAL_SHAPE_COMPLETED,
1088        )?;
1089        let visual_id = unsafe { ffi::b3GetStatusVisualShapeUniqueId(status.handle) };
1090        if visual_id < 0 {
1091            return Err(BulletError::CommandFailed {
1092                message: "Bullet failed to create visual shape array",
1093                code: visual_id,
1094            });
1095        }
1096        Ok(visual_id)
1097    }
1098
1099    pub fn change_visual_shape(
1100        &mut self,
1101        body_unique_id: i32,
1102        link_index: i32,
1103        options: &ChangeVisualShapeOptions,
1104    ) -> BulletResult<&mut Self> {
1105        self.ensure_can_submit()?;
1106        let command = unsafe {
1107            ffi::b3InitUpdateVisualShape2(
1108                self.handle,
1109                body_unique_id,
1110                link_index,
1111                options.shape_index.0,
1112            )
1113        };
1114
1115        if let Some(texture_id) = options.texture_unique_id {
1116            unsafe { ffi::b3UpdateVisualShapeTexture(command, texture_id) };
1117        }
1118        if let Some(rgba) = options.rgba_color {
1119            unsafe { ffi::b3UpdateVisualShapeRGBAColor(command, rgba.as_ptr()) };
1120        }
1121        if let Some(flags) = options.flags {
1122            unsafe { ffi::b3UpdateVisualShapeFlags(command, flags) };
1123        }
1124        if let Some(specular) = options.specular_color {
1125            unsafe { ffi::b3UpdateVisualShapeSpecularColor(command, specular.as_ptr()) };
1126        }
1127
1128        self.submit_simple_command(
1129            command,
1130            ffi::EnumSharedMemoryServerStatus::CMD_VISUAL_SHAPE_UPDATE_COMPLETED,
1131        )?;
1132        Ok(self)
1133    }
1134
1135    pub fn create_multi_body(&mut self, options: &MultiBodyCreateOptions<'_>) -> BulletResult<i32> {
1136        self.ensure_can_submit()?;
1137        let command = unsafe { ffi::b3CreateMultiBodyCommandInit(self.handle) };
1138
1139        let (base_position, base_orientation) = isometry_to_raw_parts(&options.base.pose);
1140        let (inertial_position, inertial_orientation) =
1141            isometry_to_raw_parts(&options.base.inertial_pose);
1142
1143        unsafe {
1144            ffi::b3CreateMultiBodyBase(
1145                command,
1146                options.base.mass,
1147                options.base.collision_shape.0,
1148                options.base.visual_shape.0,
1149                base_position.as_ptr(),
1150                base_orientation.as_ptr(),
1151                inertial_position.as_ptr(),
1152                inertial_orientation.as_ptr(),
1153            );
1154        }
1155
1156        for link in options.links {
1157            let parent_index = match link.parent_index {
1158                Some(index) => Self::usize_to_i32(index)?,
1159                None => -1,
1160            };
1161
1162            let (link_position, link_orientation) = isometry_to_raw_parts(&link.parent_transform);
1163            let (link_inertial_position, link_inertial_orientation) =
1164                isometry_to_raw_parts(&link.inertial_transform);
1165
1166            unsafe {
1167                ffi::b3CreateMultiBodyLink(
1168                    command,
1169                    link.mass,
1170                    link.collision_shape.0 as f64,
1171                    link.visual_shape.0 as f64,
1172                    link_position.as_ptr(),
1173                    link_orientation.as_ptr(),
1174                    link_inertial_position.as_ptr(),
1175                    link_inertial_orientation.as_ptr(),
1176                    parent_index,
1177                    link.joint_type as i32,
1178                    link.joint_axis.as_ptr(),
1179                );
1180            }
1181        }
1182
1183        if let Some(flags) = options.flags {
1184            unsafe { ffi::b3CreateMultiBodySetFlags(command, flags) };
1185        }
1186        if options.use_maximal_coordinates {
1187            unsafe { ffi::b3CreateMultiBodyUseMaximalCoordinates(command) };
1188        }
1189        let mut _batch_storage: Option<Vec<f64>> = None;
1190        if let Some(batch) = options.batch_transforms {
1191            let mut flattened = Self::flatten_isometries(batch);
1192            unsafe {
1193                ffi::b3CreateMultiBodySetBatchPositions(
1194                    self.handle,
1195                    command,
1196                    flattened.as_mut_ptr(),
1197                    batch.len() as i32,
1198                );
1199            }
1200            _batch_storage = Some(flattened);
1201        }
1202
1203        let status = self.submit_simple_command(
1204            command,
1205            ffi::EnumSharedMemoryServerStatus::CMD_CREATE_MULTI_BODY_COMPLETED,
1206        )?;
1207        let body_id = unsafe { ffi::b3GetStatusBodyIndex(status.handle) };
1208        if body_id < 0 {
1209            return Err(BulletError::CommandFailed {
1210                message: "Bullet failed to create multibody",
1211                code: body_id,
1212            });
1213        }
1214        Ok(body_id)
1215    }
1216
1217    pub fn create_constraint(&mut self, options: &ConstraintCreateOptions) -> BulletResult<i32> {
1218        self.ensure_can_submit()?;
1219        let parent_link = match options.parent_link {
1220            Some(index) => Self::usize_to_i32(index)?,
1221            None => -1,
1222        };
1223        let child_link = match options.child_link {
1224            Some(index) => Self::usize_to_i32(index)?,
1225            None => -1,
1226        };
1227        let mut joint_info = ffi::b3JointInfo {
1228            m_joint_type: options.joint_type as i32,
1229            m_joint_axis: options.joint_axis,
1230            m_joint_max_force: options.max_applied_force,
1231            ..Default::default()
1232        };
1233        Self::write_transform_to_frame(&options.parent_frame, &mut joint_info.m_parent_frame);
1234        Self::write_transform_to_frame(&options.child_frame, &mut joint_info.m_child_frame);
1235
1236        let command = unsafe {
1237            ffi::b3InitCreateUserConstraintCommand(
1238                self.handle,
1239                options.parent_body,
1240                parent_link,
1241                options.child_body,
1242                child_link,
1243                &raw mut joint_info,
1244            )
1245        };
1246
1247        let status = self.submit_simple_command(
1248            command,
1249            ffi::EnumSharedMemoryServerStatus::CMD_USER_CONSTRAINT_COMPLETED,
1250        )?;
1251        let constraint_id = unsafe { ffi::b3GetStatusUserConstraintUniqueId(status.handle) };
1252        if constraint_id < 0 {
1253            return Err(BulletError::CommandFailed {
1254                message: "Bullet failed to create user constraint",
1255                code: constraint_id,
1256            });
1257        }
1258
1259        let update = ConstraintUpdate {
1260            max_force: Some(options.max_applied_force),
1261            gear_ratio: options.gear_ratio,
1262            gear_aux_link: options.gear_aux_link,
1263            relative_position_target: options.relative_position_target,
1264            erp: options.erp,
1265            ..Default::default()
1266        };
1267
1268        if Self::constraint_update_has_changes(&update) {
1269            self.change_constraint(constraint_id, &update)?;
1270        }
1271
1272        Ok(constraint_id)
1273    }
1274
1275    pub fn change_constraint(
1276        &mut self,
1277        constraint_id: i32,
1278        update: &ConstraintUpdate,
1279    ) -> BulletResult<&mut Self> {
1280        if !Self::constraint_update_has_changes(update) {
1281            return Ok(self);
1282        }
1283
1284        self.ensure_can_submit()?;
1285        let command = unsafe { ffi::b3InitChangeUserConstraintCommand(self.handle, constraint_id) };
1286
1287        if let Some(frame) = update.child_frame.as_ref() {
1288            let (pivot, orientation) = isometry_to_raw_parts(frame);
1289            unsafe {
1290                ffi::b3InitChangeUserConstraintSetPivotInB(command, pivot.as_ptr());
1291                ffi::b3InitChangeUserConstraintSetFrameInB(command, orientation.as_ptr());
1292            }
1293        }
1294        if let Some(force) = update.max_force {
1295            unsafe { ffi::b3InitChangeUserConstraintSetMaxForce(command, force) };
1296        }
1297        if let Some(ratio) = update.gear_ratio {
1298            unsafe { ffi::b3InitChangeUserConstraintSetGearRatio(command, ratio) };
1299        }
1300        if let Some(aux) = update.gear_aux_link {
1301            let aux = Self::usize_to_i32(aux)?;
1302            unsafe { ffi::b3InitChangeUserConstraintSetGearAuxLink(command, aux) };
1303        }
1304        if let Some(target) = update.relative_position_target {
1305            unsafe { ffi::b3InitChangeUserConstraintSetRelativePositionTarget(command, target) };
1306        }
1307        if let Some(erp) = update.erp {
1308            unsafe { ffi::b3InitChangeUserConstraintSetERP(command, erp) };
1309        }
1310
1311        self.submit_simple_command(
1312            command,
1313            ffi::EnumSharedMemoryServerStatus::CMD_CHANGE_USER_CONSTRAINT_COMPLETED,
1314        )?;
1315        Ok(self)
1316    }
1317
1318    pub fn remove_constraint(&mut self, constraint_id: i32) -> BulletResult<()> {
1319        self.ensure_can_submit()?;
1320        let command = unsafe { ffi::b3InitRemoveUserConstraintCommand(self.handle, constraint_id) };
1321        self.submit_simple_command(
1322            command,
1323            ffi::EnumSharedMemoryServerStatus::CMD_REMOVE_USER_CONSTRAINT_COMPLETED,
1324        )?;
1325        Ok(())
1326    }
1327
1328    pub fn enable_joint_force_torque_sensor(
1329        &mut self,
1330        body_unique_id: i32,
1331        joint_index: i32,
1332        enable: bool,
1333    ) -> BulletResult<()> {
1334        self.ensure_can_submit()?;
1335        let command = unsafe { ffi::b3CreateSensorCommandInit(self.handle, body_unique_id) };
1336        unsafe {
1337            ffi::b3CreateSensorEnable6DofJointForceTorqueSensor(
1338                command,
1339                joint_index,
1340                enable.into(),
1341            );
1342        }
1343        self.submit_simple_command(
1344            command,
1345            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
1346        )?;
1347        Ok(())
1348    }
1349
1350    pub fn remove_body(&mut self, body_unique_id: i32) -> BulletResult<()> {
1351        self.ensure_can_submit()?;
1352        let command = unsafe { ffi::b3InitRemoveBodyCommand(self.handle, body_unique_id) };
1353        self.submit_simple_command(
1354            command,
1355            ffi::EnumSharedMemoryServerStatus::CMD_REMOVE_BODY_COMPLETED,
1356        )?;
1357        Ok(())
1358    }
1359
1360    pub fn get_num_constraints(&self) -> i32 {
1361        unsafe { ffi::b3GetNumUserConstraints(self.handle) }
1362    }
1363
1364    pub fn get_constraint_info(&self, constraint_id: i32) -> BulletResult<ConstraintInfo> {
1365        let mut raw = MaybeUninit::<ffi::b3UserConstraint>::uninit();
1366        let success =
1367            unsafe { ffi::b3GetUserConstraintInfo(self.handle, constraint_id, raw.as_mut_ptr()) };
1368        if success == 0 {
1369            return Err(BulletError::CommandFailed {
1370                message: "Unable to query constraint info",
1371                code: success,
1372            });
1373        }
1374        let raw = unsafe { raw.assume_init() };
1375        Ok(ConstraintInfo {
1376            parent_body: raw.m_parentBodyIndex,
1377            parent_link: raw.m_parentJointIndex,
1378            child_body: raw.m_childBodyIndex,
1379            child_link: raw.m_childJointIndex,
1380            parent_frame: Self::read_frame_transform(&raw.m_parentFrame),
1381            child_frame: Self::read_frame_transform(&raw.m_childFrame),
1382            joint_axis: raw.m_jointAxis,
1383            joint_type: raw.m_jointType,
1384            max_applied_force: raw.m_maxAppliedForce,
1385            constraint_unique_id: raw.m_userConstraintUniqueId,
1386            gear_ratio: raw.m_gearRatio,
1387            gear_aux_link: raw.m_gearAuxLink,
1388            relative_position_target: raw.m_relativePositionTarget,
1389            erp: raw.m_erp,
1390        })
1391    }
1392
1393    pub fn get_constraint_state(&mut self, constraint_id: i32) -> BulletResult<ConstraintState> {
1394        self.ensure_can_submit()?;
1395        let command =
1396            unsafe { ffi::b3InitGetUserConstraintStateCommand(self.handle, constraint_id) };
1397        let status = self.submit_simple_command(
1398            command,
1399            ffi::EnumSharedMemoryServerStatus::CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED,
1400        )?;
1401        let mut raw = MaybeUninit::<ffi::b3UserConstraintState>::uninit();
1402        let success =
1403            unsafe { ffi::b3GetStatusUserConstraintState(status.handle, raw.as_mut_ptr()) };
1404        if success == 0 {
1405            return Err(BulletError::CommandFailed {
1406                message: "Unable to read constraint state",
1407                code: success,
1408            });
1409        }
1410        let raw = unsafe { raw.assume_init() };
1411        Ok(ConstraintState {
1412            applied_forces: raw.m_appliedConstraintForces,
1413            dof_count: raw.m_numDofs,
1414        })
1415    }
1416
1417    pub fn get_constraint_unique_id(&self, index: i32) -> Option<i32> {
1418        let id = unsafe { ffi::b3GetUserConstraintId(self.handle, index) };
1419        if id < 0 { None } else { Some(id) }
1420    }
1421
1422    pub fn load_texture(&mut self, filename: &str) -> BulletResult<i32> {
1423        self.ensure_can_submit()?;
1424        let filename_c = CString::new(filename)?;
1425        let command = unsafe { ffi::b3InitLoadTexture(self.handle, filename_c.as_ptr()) };
1426        let status = self.submit_simple_command(
1427            command,
1428            ffi::EnumSharedMemoryServerStatus::CMD_LOAD_TEXTURE_COMPLETED,
1429        )?;
1430        let texture_id = unsafe { ffi::b3GetStatusTextureUniqueId(status.handle) };
1431        if texture_id < 0 {
1432            return Err(BulletError::CommandFailed {
1433                message: "Bullet failed to load texture",
1434                code: texture_id,
1435            });
1436        }
1437        Ok(texture_id)
1438    }
1439
1440    pub fn change_texture(&mut self, texture_id: i32, data: &TextureData<'_>) -> BulletResult<()> {
1441        if data.width <= 0 || data.height <= 0 {
1442            return Err(BulletError::CommandFailed {
1443                message: "Texture dimensions must be positive",
1444                code: -1,
1445            });
1446        }
1447
1448        let expected_len = (data.width as usize)
1449            .saturating_mul(data.height as usize)
1450            .saturating_mul(3);
1451        if expected_len != data.rgb_pixels.len() {
1452            return Err(BulletError::CommandFailed {
1453                message: "Texture data length mismatch",
1454                code: data.rgb_pixels.len() as i32,
1455            });
1456        }
1457
1458        self.ensure_can_submit()?;
1459        let command = unsafe {
1460            ffi::b3CreateChangeTextureCommandInit(
1461                self.handle,
1462                texture_id,
1463                data.width,
1464                data.height,
1465                data.rgb_pixels.as_ptr().cast(),
1466            )
1467        };
1468        self.submit_simple_command(
1469            command,
1470            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
1471        )?;
1472        Ok(())
1473    }
1474}
1475
1476/// ! =====================================================================================================================================
1477/// ### Bodies, Joints & Base State
1478///
1479/// | API | Status | Notes |
1480/// | --- | --- | --- |
1481/// | getNumBodies | **Implemented** | Enumeration helper |
1482/// | getBodyUniqueId | **Implemented** | Enumeration helper |
1483/// | getBodyInfo | **Implemented** | Names cached |
1484/// | computeDofCount | **Implemented** | Useful with dynamics |
1485/// | syncBodyInfo | **Implemented** | Multi-client support |
1486/// | syncUserData | **Implemented** | Multi-client support |
1487/// | addUserData | **Implemented** | User data authoring |
1488/// | getUserData | **Implemented** | User data query |
1489/// | removeUserData | **Implemented** | User data cleanup |
1490/// | getUserDataId | **Implemented** | User data query |
1491/// | getNumUserData | **Implemented** | User data query |
1492/// | getUserDataInfo | **Implemented** | User data query |
1493/// | getBasePositionAndOrientation | **Implemented** | Uses actual state request |
1494/// | getAABB | **Implemented** | Contact bounds |
1495/// | resetBasePositionAndOrientation | **Implemented** | World authoring priority |
1496/// | unsupportedChangeScaling | Optional | Rudimentary scaling |
1497/// | getBaseVelocity | **Implemented** | Uses actual state request |
1498/// | resetBaseVelocity | **Implemented** | Dynamics priority |
1499/// | getNumJoints | **Implemented** | Joint enumeration |
1500/// | getJointInfo | **Implemented** | Joint metadata |
1501/// | getJointState | **Implemented** | Single joint sensor |
1502/// | getJointStates | **Implemented** | Batch sensor support |
1503/// | getJointStateMultiDof | **Implemented** | Multi-DoF sensor |
1504/// | getJointStatesMultiDof | **Implemented** | Multi-DoF batch |
1505/// | getLinkState | **Implemented** | Forward kinematics |
1506/// | getLinkStates | **Implemented** | Batch link state |
1507/// | resetJointState | **Implemented** | World authoring priority |
1508/// | resetJointStateMultiDof | **Implemented** | Multi-DoF reset |
1509/// | resetJointStatesMultiDof | **Implemented** | Multi-DoF batch reset |
1510impl PhysicsClient {
1511    pub fn compute_dof_count(&self, body_unique_id: i32) -> i32 {
1512        unsafe { ffi::b3ComputeDofCount(self.handle, body_unique_id) }
1513    }
1514
1515    pub fn sync_body_info(&mut self) -> BulletResult<()> {
1516        self.ensure_can_submit()?;
1517        let command = unsafe { ffi::b3InitSyncBodyInfoCommand(self.handle) };
1518        self.submit_simple_command(
1519            command,
1520            ffi::EnumSharedMemoryServerStatus::CMD_SYNC_BODY_INFO_COMPLETED,
1521        )?;
1522        Ok(())
1523    }
1524
1525    pub fn sync_user_data(&mut self, body_unique_ids: Option<&[i32]>) -> BulletResult<()> {
1526        self.ensure_can_submit()?;
1527        let command = unsafe { ffi::b3InitSyncUserDataCommand(self.handle) };
1528        if let Some(ids) = body_unique_ids {
1529            for &id in ids {
1530                unsafe {
1531                    ffi::b3AddBodyToSyncUserDataRequest(command, id);
1532                }
1533            }
1534        }
1535        self.submit_simple_command(
1536            command,
1537            ffi::EnumSharedMemoryServerStatus::CMD_SYNC_USER_DATA_COMPLETED,
1538        )?;
1539        Ok(())
1540    }
1541
1542    pub fn add_user_data(
1543        &mut self,
1544        body_unique_id: i32,
1545        key: &str,
1546        value: &str,
1547        link_index: Option<i32>,
1548        visual_shape_index: Option<i32>,
1549    ) -> BulletResult<i32> {
1550        self.ensure_can_submit()?;
1551        let key_c = CString::new(key)?;
1552        let value_c = CString::new(value)?;
1553        let link_index = link_index.unwrap_or(-1);
1554        let visual_shape_index = visual_shape_index.unwrap_or(-1);
1555        let value_len = i32::try_from(value_c.as_bytes_with_nul().len()).map_err(|_| {
1556            BulletError::CommandFailed { message: "User data value is too long", code: -1 }
1557        })?;
1558
1559        let command = unsafe {
1560            ffi::b3InitAddUserDataCommand(
1561                self.handle,
1562                body_unique_id,
1563                link_index,
1564                visual_shape_index,
1565                key_c.as_ptr(),
1566                ffi::USER_DATA_VALUE_TYPE_STRING,
1567                value_len,
1568                value_c.as_ptr().cast(),
1569            )
1570        };
1571
1572        let status = self.submit_simple_command(
1573            command,
1574            ffi::EnumSharedMemoryServerStatus::CMD_ADD_USER_DATA_COMPLETED,
1575        )?;
1576        let user_data_id = unsafe { ffi::b3GetUserDataIdFromStatus(status.handle) };
1577        Ok(user_data_id)
1578    }
1579
1580    pub fn remove_user_data(&mut self, user_data_id: i32) -> BulletResult<()> {
1581        self.ensure_can_submit()?;
1582        let command = unsafe { ffi::b3InitRemoveUserDataCommand(self.handle, user_data_id) };
1583        self.submit_simple_command(
1584            command,
1585            ffi::EnumSharedMemoryServerStatus::CMD_REMOVE_USER_DATA_COMPLETED,
1586        )?;
1587        Ok(())
1588    }
1589
1590    pub fn get_user_data_id(
1591        &self,
1592        body_unique_id: i32,
1593        key: &str,
1594        link_index: Option<i32>,
1595        visual_shape_index: Option<i32>,
1596    ) -> BulletResult<Option<i32>> {
1597        let key_c = CString::new(key)?;
1598        let link_index = link_index.unwrap_or(-1);
1599        let visual_shape_index = visual_shape_index.unwrap_or(-1);
1600        let id = unsafe {
1601            ffi::b3GetUserDataId(
1602                self.handle,
1603                body_unique_id,
1604                link_index,
1605                visual_shape_index,
1606                key_c.as_ptr(),
1607            )
1608        };
1609        if id < 0 { Ok(None) } else { Ok(Some(id)) }
1610    }
1611
1612    pub fn get_user_data(&self, user_data_id: i32) -> BulletResult<Option<String>> {
1613        let mut raw = MaybeUninit::<ffi::b3UserDataValue>::uninit();
1614        let success = unsafe { ffi::b3GetUserData(self.handle, user_data_id, raw.as_mut_ptr()) };
1615        if success == 0 {
1616            return Ok(None);
1617        }
1618        let raw = unsafe { raw.assume_init() };
1619        if raw.m_type != ffi::USER_DATA_VALUE_TYPE_STRING {
1620            return Err(BulletError::CommandFailed {
1621                message: "User data value has unsupported type",
1622                code: raw.m_type,
1623            });
1624        }
1625        if raw.m_data1.is_null() {
1626            return Ok(None);
1627        }
1628        let value = unsafe { CStr::from_ptr(raw.m_data1) };
1629        Ok(Some(value.to_string_lossy().into_owned()))
1630    }
1631
1632    pub fn get_num_user_data(&self, body_unique_id: i32) -> BulletResult<i32> {
1633        let count = unsafe { ffi::b3GetNumUserData(self.handle, body_unique_id) };
1634        if count < 0 {
1635            Err(BulletError::CommandFailed {
1636                message: "Failed to query user data count",
1637                code: count,
1638            })
1639        } else {
1640            Ok(count)
1641        }
1642    }
1643
1644    pub fn get_user_data_info(
1645        &self,
1646        body_unique_id: i32,
1647        user_data_index: i32,
1648    ) -> BulletResult<UserDataInfo> {
1649        let mut key_ptr: *const i8 = ptr::null();
1650        let mut user_data_id = -1;
1651        let mut link_index = -1;
1652        let mut visual_shape_index = -1;
1653        unsafe {
1654            ffi::b3GetUserDataInfo(
1655                self.handle,
1656                body_unique_id,
1657                user_data_index,
1658                &raw mut key_ptr,
1659                &raw mut user_data_id,
1660                &raw mut link_index,
1661                &raw mut visual_shape_index,
1662            );
1663        }
1664
1665        if key_ptr.is_null() || user_data_id == -1 {
1666            return Err(BulletError::CommandFailed {
1667                message: "Could not fetch user data info",
1668                code: -1,
1669            });
1670        }
1671
1672        let key = unsafe { CStr::from_ptr(key_ptr) }
1673            .to_string_lossy()
1674            .into_owned();
1675
1676        Ok(UserDataInfo {
1677            user_data_id,
1678            key,
1679            body_unique_id,
1680            link_index,
1681            visual_shape_index,
1682        })
1683    }
1684
1685    pub fn get_aabb(&mut self, body_unique_id: i32, link_index: i32) -> BulletResult<Aabb> {
1686        if body_unique_id < 0 {
1687            return Err(BulletError::CommandFailed {
1688                message: "Invalid body id",
1689                code: body_unique_id,
1690            });
1691        }
1692        if link_index < -1 {
1693            return Err(BulletError::CommandFailed {
1694                message: "Invalid link index",
1695                code: link_index,
1696            });
1697        }
1698        self.ensure_can_submit()?;
1699        let command =
1700            unsafe { ffi::b3RequestCollisionInfoCommandInit(self.handle, body_unique_id) };
1701        let status = self.submit_simple_command(
1702            command,
1703            ffi::EnumSharedMemoryServerStatus::CMD_REQUEST_COLLISION_INFO_COMPLETED,
1704        )?;
1705
1706        let mut aabb_min = [0.0; 3];
1707        let mut aabb_max = [0.0; 3];
1708        let success = unsafe {
1709            ffi::b3GetStatusAABB(
1710                status.handle,
1711                link_index,
1712                aabb_min.as_mut_ptr(),
1713                aabb_max.as_mut_ptr(),
1714            )
1715        };
1716        if success == 0 {
1717            return Err(BulletError::CommandFailed {
1718                message: "Failed to fetch AABB",
1719                code: success,
1720            });
1721        }
1722
1723        Ok(Aabb { min: aabb_min, max: aabb_max })
1724    }
1725
1726    pub fn reset_base_position_and_orientation(
1727        &mut self,
1728        body_unique_id: i32,
1729        position: [f64; 3],
1730        orientation: [f64; 4],
1731    ) -> BulletResult<()> {
1732        self.ensure_can_submit()?;
1733        let command = unsafe { ffi::b3CreatePoseCommandInit(self.handle, body_unique_id) };
1734        unsafe {
1735            ffi::b3CreatePoseCommandSetBasePosition(command, position[0], position[1], position[2]);
1736            ffi::b3CreatePoseCommandSetBaseOrientation(
1737                command,
1738                orientation[0],
1739                orientation[1],
1740                orientation[2],
1741                orientation[3],
1742            );
1743        }
1744        self.submit_simple_command(
1745            command,
1746            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
1747        )?;
1748        Ok(())
1749    }
1750
1751    pub fn reset_base_velocity(
1752        &mut self,
1753        body_unique_id: i32,
1754        linear_velocity: Option<[f64; 3]>,
1755        angular_velocity: Option<[f64; 3]>,
1756    ) -> BulletResult<()> {
1757        if linear_velocity.is_none() && angular_velocity.is_none() {
1758            return Err(BulletError::CommandFailed {
1759                message: "Expected linear and/or angular velocity",
1760                code: -1,
1761            });
1762        }
1763        self.ensure_can_submit()?;
1764        let command = unsafe { ffi::b3CreatePoseCommandInit(self.handle, body_unique_id) };
1765        if let Some(vel) = linear_velocity {
1766            unsafe {
1767                ffi::b3CreatePoseCommandSetBaseLinearVelocity(command, vel.as_ptr());
1768            }
1769        }
1770        if let Some(vel) = angular_velocity {
1771            unsafe {
1772                ffi::b3CreatePoseCommandSetBaseAngularVelocity(command, vel.as_ptr());
1773            }
1774        }
1775        self.submit_simple_command(
1776            command,
1777            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
1778        )?;
1779        Ok(())
1780    }
1781
1782    pub fn get_num_bodies(&self) -> i32 {
1783        unsafe { ffi::b3GetNumBodies(self.handle) }
1784    }
1785
1786    pub fn get_body_unique_id(&self, serial_index: i32) -> i32 {
1787        unsafe { ffi::b3GetBodyUniqueId(self.handle, serial_index) }
1788    }
1789
1790    pub fn get_body_info(&self, body_unique_id: i32) -> BulletResult<BodyInfo> {
1791        let mut raw = MaybeUninit::<ffi::b3BodyInfo>::uninit();
1792        let result = unsafe { ffi::b3GetBodyInfo(self.handle, body_unique_id, raw.as_mut_ptr()) };
1793        if result == 0 {
1794            return Err(BulletError::CommandFailed {
1795                message: "Cannot query body info",
1796                code: result,
1797            });
1798        }
1799        let raw = unsafe { raw.assume_init() };
1800        let base_name = Self::read_c_string(&raw.m_baseName);
1801        let body_name = Self::read_c_string(&raw.m_bodyName);
1802        Ok(BodyInfo { base_name, body_name })
1803    }
1804
1805    pub fn get_num_joints(&self, body_unique_id: i32) -> i32 {
1806        unsafe { ffi::b3GetNumJoints(self.handle, body_unique_id) }
1807    }
1808
1809    pub fn get_joint_info(&self, body_unique_id: i32, joint_index: i32) -> BulletResult<JointInfo> {
1810        let mut raw = MaybeUninit::<ffi::b3JointInfo>::uninit();
1811        let success = unsafe {
1812            ffi::b3GetJointInfo(self.handle, body_unique_id, joint_index, raw.as_mut_ptr())
1813        };
1814        if success == 0 {
1815            return Err(BulletError::CommandFailed {
1816                message: "Cannot query joint info",
1817                code: success,
1818            });
1819        }
1820        unsafe { raw.assume_init() }.try_into()
1821    }
1822
1823    pub fn get_joint_state(
1824        &mut self,
1825        body_unique_id: i32,
1826        joint_index: i32,
1827    ) -> BulletResult<JointState> {
1828        let status = self.request_actual_state_status(body_unique_id)?;
1829        self.read_joint_state(status.handle, joint_index)
1830    }
1831
1832    pub fn get_joint_states(
1833        &mut self,
1834        body_unique_id: i32,
1835        joint_indices: &[i32],
1836    ) -> BulletResult<Vec<JointState>> {
1837        if joint_indices.is_empty() {
1838            return Ok(Vec::new());
1839        }
1840        let status = self.request_actual_state_status(body_unique_id)?;
1841        joint_indices
1842            .iter()
1843            .map(|&index| self.read_joint_state(status.handle, index))
1844            .collect()
1845    }
1846
1847    pub fn get_joint_state_multi_dof(
1848        &mut self,
1849        body_unique_id: i32,
1850        joint_index: i32,
1851    ) -> BulletResult<JointStateMultiDof> {
1852        let status = self.request_actual_state_status(body_unique_id)?;
1853        self.read_joint_state_multi_dof(status.handle, joint_index)
1854    }
1855
1856    pub fn get_joint_states_multi_dof(
1857        &mut self,
1858        body_unique_id: i32,
1859        joint_indices: &[i32],
1860    ) -> BulletResult<Vec<JointStateMultiDof>> {
1861        if joint_indices.is_empty() {
1862            return Ok(Vec::new());
1863        }
1864        let status = self.request_actual_state_status(body_unique_id)?;
1865        joint_indices
1866            .iter()
1867            .map(|&index| self.read_joint_state_multi_dof(status.handle, index))
1868            .collect()
1869    }
1870
1871    pub fn get_base_position_and_orientation(
1872        &mut self,
1873        body_unique_id: i32,
1874    ) -> BulletResult<na::Isometry3<f64>> {
1875        let status = self.request_actual_state_status(body_unique_id)?;
1876        let (base, _) = Self::extract_base_state(status.handle)?;
1877        Ok(base)
1878    }
1879
1880    pub fn get_base_velocity(&mut self, body_unique_id: i32) -> BulletResult<[f64; 6]> {
1881        let status = self.request_actual_state_status(body_unique_id)?;
1882        let (_, velocity) = Self::extract_base_state(status.handle)?;
1883        Ok(velocity)
1884    }
1885
1886    pub fn get_link_state(
1887        &mut self,
1888        body_unique_id: i32,
1889        link_index: i32,
1890        compute_forward_kinematics: bool,
1891        compute_link_velocity: bool,
1892    ) -> BulletResult<LinkState> {
1893        let status = self.request_actual_state_status_with_flags(
1894            body_unique_id,
1895            compute_link_velocity,
1896            compute_forward_kinematics,
1897        )?;
1898        self.read_link_state(status.handle, link_index)
1899    }
1900
1901    pub fn get_link_states(
1902        &mut self,
1903        body_unique_id: i32,
1904        link_indices: &[i32],
1905        compute_forward_kinematics: bool,
1906        compute_link_velocity: bool,
1907    ) -> BulletResult<Vec<LinkState>> {
1908        if link_indices.is_empty() {
1909            return Ok(Vec::new());
1910        }
1911        let status = self.request_actual_state_status_with_flags(
1912            body_unique_id,
1913            compute_link_velocity,
1914            compute_forward_kinematics,
1915        )?;
1916        link_indices
1917            .iter()
1918            .map(|&index| self.read_link_state(status.handle, index))
1919            .collect()
1920    }
1921
1922    pub fn reset_joint_state(
1923        &mut self,
1924        body_unique_id: i32,
1925        joint_index: i32,
1926        target_position: f64,
1927        target_velocity: Option<f64>,
1928    ) -> BulletResult<()> {
1929        self.ensure_can_submit()?;
1930        let command = unsafe { ffi::b3CreatePoseCommandInit(self.handle, body_unique_id) };
1931        unsafe {
1932            ffi::b3CreatePoseCommandSetJointPosition(
1933                self.handle,
1934                command,
1935                joint_index,
1936                target_position,
1937            );
1938            ffi::b3CreatePoseCommandSetJointVelocity(
1939                self.handle,
1940                command,
1941                joint_index,
1942                target_velocity.unwrap_or(0.0),
1943            );
1944        }
1945        self.submit_simple_command(
1946            command,
1947            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
1948        )?;
1949        Ok(())
1950    }
1951
1952    pub fn reset_joint_state_multi_dof(
1953        &mut self,
1954        body_unique_id: i32,
1955        joint_index: i32,
1956        positions: Option<&[f64]>,
1957        velocities: Option<&[f64]>,
1958    ) -> BulletResult<()> {
1959        let target = MultiDofTarget { joint_index, positions, velocities };
1960        self.reset_joint_states_multi_dof(body_unique_id, slice::from_ref(&target))
1961    }
1962
1963    pub fn reset_joint_states_multi_dof(
1964        &mut self,
1965        body_unique_id: i32,
1966        targets: &[MultiDofTarget<'_>],
1967    ) -> BulletResult<()> {
1968        if targets.is_empty() {
1969            return Ok(());
1970        }
1971        self.ensure_can_submit()?;
1972        let command = unsafe { ffi::b3CreatePoseCommandInit(self.handle, body_unique_id) };
1973
1974        for target in targets {
1975            let mut position_buf = [0.0_f64; 4];
1976            let mut velocity_buf = [0.0_f64; 3];
1977            let mut position_size = 0_i32;
1978            let mut velocity_size = 0_i32;
1979
1980            if let Some(positions) = target.positions {
1981                if positions.len() > position_buf.len() {
1982                    return Err(BulletError::CommandFailed {
1983                        message: "Position vector too large for multi-DoF joint",
1984                        code: positions.len() as i32,
1985                    });
1986                }
1987                if !positions.is_empty() {
1988                    position_buf[..positions.len()].copy_from_slice(positions);
1989                    position_size = positions.len() as i32;
1990                }
1991            }
1992
1993            if let Some(velocities) = target.velocities {
1994                if velocities.len() > velocity_buf.len() {
1995                    return Err(BulletError::CommandFailed {
1996                        message: "Velocity vector too large for multi-DoF joint",
1997                        code: velocities.len() as i32,
1998                    });
1999                }
2000                if !velocities.is_empty() {
2001                    velocity_buf[..velocities.len()].copy_from_slice(velocities);
2002                    velocity_size = velocities.len() as i32;
2003                }
2004            }
2005
2006            if position_size == 0 && velocity_size == 0 {
2007                return Err(BulletError::CommandFailed {
2008                    message: "Expected position and/or velocity data for multi-DoF joint",
2009                    code: target.joint_index,
2010                });
2011            }
2012
2013            if position_size > 0 {
2014                unsafe {
2015                    ffi::b3CreatePoseCommandSetJointPositionMultiDof(
2016                        self.handle,
2017                        command,
2018                        target.joint_index,
2019                        position_buf.as_ptr(),
2020                        position_size,
2021                    );
2022                }
2023            }
2024
2025            if velocity_size > 0 {
2026                unsafe {
2027                    ffi::b3CreatePoseCommandSetJointVelocityMultiDof(
2028                        self.handle,
2029                        command,
2030                        target.joint_index,
2031                        velocity_buf.as_ptr(),
2032                        velocity_size,
2033                    );
2034                }
2035            }
2036        }
2037
2038        self.submit_simple_command(
2039            command,
2040            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
2041        )?;
2042        Ok(())
2043    }
2044}
2045
2046/// ! =====================================================================================================================================
2047/// ### Dynamics & Control
2048///
2049/// | API | Status | Notes |
2050/// | --- | --- | --- |
2051/// | changeDynamics | **Implemented** | Mutation wrapper |
2052/// | getDynamicsInfo | **Implemented** | Mirrors Bullet query |
2053/// | setJointMotorControl | Optional | Legacy single-call (deprecated) |
2054/// | setJointMotorControl2 | **Implemented** | Primary motor control path |
2055/// | setJointMotorControlMultiDof | **Implemented** | Multi-DoF control |
2056/// | setJointMotorControlMultiDofArray | **Implemented** | Multi-DoF batch control |
2057/// | setJointMotorControlArray | **Implemented** | Batch joint control |
2058/// | applyExternalForce | **Implemented** | Core dynamics action |
2059/// | applyExternalTorque | **Implemented** | Core dynamics action |
2060/// | calculateInverseDynamics | **Implemented** | Advanced dynamics |
2061/// | calculateJacobian | **Implemented** | Advanced dynamics |
2062/// | calculateMassMatrix | **Implemented** | Advanced dynamics |
2063/// | calculateInverseKinematics | **Implemented** | Depends on jacobians |
2064/// | calculateInverseKinematics2 | **Implemented** | Multi-end-effector variant |
2065impl PhysicsClient {
2066    pub fn change_dynamics(
2067        &mut self,
2068        body_unique_id: i32,
2069        link_index: i32,
2070        update: &DynamicsUpdate,
2071    ) -> BulletResult<()> {
2072        self.ensure_can_submit()?;
2073        let command = unsafe { ffi::b3InitChangeDynamicsInfo(self.handle) };
2074
2075        unsafe {
2076            if let Some(mass) = update.mass {
2077                ffi::b3ChangeDynamicsInfoSetMass(command, body_unique_id, link_index, mass);
2078            }
2079            if let Some(local_inertia) = update.local_inertia_diagonal {
2080                ffi::b3ChangeDynamicsInfoSetLocalInertiaDiagonal(
2081                    command,
2082                    body_unique_id,
2083                    link_index,
2084                    local_inertia.as_ptr(),
2085                );
2086            }
2087            if let Some(anisotropic) = update.anisotropic_friction {
2088                ffi::b3ChangeDynamicsInfoSetAnisotropicFriction(
2089                    command,
2090                    body_unique_id,
2091                    link_index,
2092                    anisotropic.as_ptr(),
2093                );
2094            }
2095            if let Some((lower, upper)) = update.joint_limit {
2096                ffi::b3ChangeDynamicsInfoSetJointLimit(
2097                    command,
2098                    body_unique_id,
2099                    link_index,
2100                    lower,
2101                    upper,
2102                );
2103            }
2104            if let Some(limit_force) = update.joint_limit_force {
2105                ffi::b3ChangeDynamicsInfoSetJointLimitForce(
2106                    command,
2107                    body_unique_id,
2108                    link_index,
2109                    limit_force,
2110                );
2111            }
2112            if let Some(dynamic_type) = update.dynamic_type {
2113                ffi::b3ChangeDynamicsInfoSetDynamicType(
2114                    command,
2115                    body_unique_id,
2116                    link_index,
2117                    dynamic_type,
2118                );
2119            }
2120            if let Some(lateral) = update.lateral_friction {
2121                ffi::b3ChangeDynamicsInfoSetLateralFriction(
2122                    command,
2123                    body_unique_id,
2124                    link_index,
2125                    lateral,
2126                );
2127            }
2128            if let Some(spinning) = update.spinning_friction {
2129                ffi::b3ChangeDynamicsInfoSetSpinningFriction(
2130                    command,
2131                    body_unique_id,
2132                    link_index,
2133                    spinning,
2134                );
2135            }
2136            if let Some(rolling) = update.rolling_friction {
2137                ffi::b3ChangeDynamicsInfoSetRollingFriction(
2138                    command,
2139                    body_unique_id,
2140                    link_index,
2141                    rolling,
2142                );
2143            }
2144            if let Some(restitution) = update.restitution {
2145                ffi::b3ChangeDynamicsInfoSetRestitution(
2146                    command,
2147                    body_unique_id,
2148                    link_index,
2149                    restitution,
2150                );
2151            }
2152            if let Some(linear_damping) = update.linear_damping {
2153                ffi::b3ChangeDynamicsInfoSetLinearDamping(command, body_unique_id, linear_damping);
2154            }
2155            if let Some(angular_damping) = update.angular_damping {
2156                ffi::b3ChangeDynamicsInfoSetAngularDamping(
2157                    command,
2158                    body_unique_id,
2159                    angular_damping,
2160                );
2161            }
2162            if let Some(joint_damping) = update.joint_damping {
2163                ffi::b3ChangeDynamicsInfoSetJointDamping(
2164                    command,
2165                    body_unique_id,
2166                    link_index,
2167                    joint_damping,
2168                );
2169            }
2170            if let Some((stiffness, damping)) = update.contact_stiffness_and_damping {
2171                ffi::b3ChangeDynamicsInfoSetContactStiffnessAndDamping(
2172                    command,
2173                    body_unique_id,
2174                    link_index,
2175                    stiffness,
2176                    damping,
2177                );
2178            }
2179            if let Some(anchor) = update.friction_anchor {
2180                ffi::b3ChangeDynamicsInfoSetFrictionAnchor(
2181                    command,
2182                    body_unique_id,
2183                    link_index,
2184                    anchor as i32,
2185                );
2186            }
2187            if let Some(radius) = update.ccd_swept_sphere_radius {
2188                ffi::b3ChangeDynamicsInfoSetCcdSweptSphereRadius(
2189                    command,
2190                    body_unique_id,
2191                    link_index,
2192                    radius,
2193                );
2194            }
2195            if let Some(threshold) = update.contact_processing_threshold {
2196                ffi::b3ChangeDynamicsInfoSetContactProcessingThreshold(
2197                    command,
2198                    body_unique_id,
2199                    link_index,
2200                    threshold,
2201                );
2202            }
2203            if let Some(state) = update.activation_state {
2204                ffi::b3ChangeDynamicsInfoSetActivationState(command, body_unique_id, state);
2205            }
2206            if let Some(max_velocity) = update.max_joint_velocity {
2207                ffi::b3ChangeDynamicsInfoSetMaxJointVelocity(command, body_unique_id, max_velocity);
2208            }
2209            if let Some(collision_margin) = update.collision_margin {
2210                ffi::b3ChangeDynamicsInfoSetCollisionMargin(
2211                    command,
2212                    body_unique_id,
2213                    collision_margin,
2214                );
2215            }
2216        }
2217
2218        self.submit_simple_command(
2219            command,
2220            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
2221        )?;
2222        Ok(())
2223    }
2224
2225    pub fn get_dynamics_info(
2226        &mut self,
2227        body_unique_id: i32,
2228        link_index: i32,
2229    ) -> BulletResult<DynamicsInfo> {
2230        self.ensure_can_submit()?;
2231        let command =
2232            unsafe { ffi::b3GetDynamicsInfoCommandInit(self.handle, body_unique_id, link_index) };
2233        let status = self.submit_simple_command(
2234            command,
2235            ffi::EnumSharedMemoryServerStatus::CMD_GET_DYNAMICS_INFO_COMPLETED,
2236        )?;
2237
2238        let mut info = MaybeUninit::<ffi::b3DynamicsInfo>::uninit();
2239        let success = unsafe { ffi::b3GetDynamicsInfo(status.handle, info.as_mut_ptr()) };
2240        if success == 0 {
2241            return Err(BulletError::CommandFailed {
2242                message: "Failed to retrieve dynamics info",
2243                code: success,
2244            });
2245        }
2246        let info = unsafe { info.assume_init() };
2247        Ok(DynamicsInfo {
2248            mass: info.m_mass,
2249            lateral_friction: info.m_lateralFrictionCoeff,
2250            local_inertia_diagonal: info.m_localInertialDiagonal,
2251            local_inertia_position: [
2252                info.m_localInertialFrame[0],
2253                info.m_localInertialFrame[1],
2254                info.m_localInertialFrame[2],
2255            ],
2256            local_inertia_orientation: [
2257                info.m_localInertialFrame[3],
2258                info.m_localInertialFrame[4],
2259                info.m_localInertialFrame[5],
2260                info.m_localInertialFrame[6],
2261            ],
2262            restitution: info.m_restitution,
2263            rolling_friction: info.m_rollingFrictionCoeff,
2264            spinning_friction: info.m_spinningFrictionCoeff,
2265            contact_damping: info.m_contactDamping,
2266            contact_stiffness: info.m_contactStiffness,
2267            body_type: info.m_bodyType.try_into()?,
2268            collision_margin: info.m_collisionMargin,
2269            angular_damping: info.m_angularDamping,
2270            linear_damping: info.m_linearDamping,
2271            ccd_swept_sphere_radius: info.m_ccdSweptSphereRadius,
2272            contact_processing_threshold: info.m_contactProcessingThreshold,
2273            activation_state: info.m_activationState,
2274            friction_anchor: info.m_frictionAnchor != 0,
2275            dynamic_type: info.m_dynamicType,
2276        })
2277    }
2278
2279    pub fn set_joint_motor_control(
2280        &mut self,
2281        body_unique_id: i32,
2282        joint_index: i32,
2283        control_mode: ControlMode,
2284        maximum_force: Option<f64>,
2285    ) -> BulletResult<()> {
2286        self.ensure_can_submit()?;
2287        let info = self.get_joint_info(body_unique_id, joint_index)?;
2288        let command = unsafe {
2289            ffi::b3JointControlCommandInit2(self.handle, body_unique_id, control_mode.as_raw())
2290        };
2291
2292        let u_index = info.u_index;
2293        if u_index < 0 {
2294            return Err(BulletError::CommandFailed {
2295                message: "Joint has no velocity DOF for motor control",
2296                code: u_index,
2297            });
2298        }
2299
2300        let force = maximum_force.unwrap_or(100_000.);
2301        let kp = 0.1;
2302        let kd = 1.0;
2303        let target_velocity = 0.;
2304
2305        unsafe {
2306            match control_mode {
2307                ControlMode::Position(target_position) => {
2308                    ffi::b3JointControlSetDesiredPosition(command, info.q_index, target_position);
2309
2310                    ffi::b3JointControlSetKp(command, info.u_index, kp);
2311                    ffi::b3JointControlSetDesiredVelocity(command, info.u_index, target_velocity);
2312
2313                    ffi::b3JointControlSetKd(command, info.u_index, kd);
2314                    ffi::b3JointControlSetMaximumForce(command, info.u_index, force);
2315                }
2316                ControlMode::Pd {
2317                    target_position: pos,
2318                    target_velocity: vel,
2319                    position_gain: kp,
2320                    velocity_gain: kd,
2321                    maximum_velocity: max_vel,
2322                }
2323                | ControlMode::StablePd {
2324                    target_position: pos,
2325                    target_velocity: vel,
2326                    position_gain: kp,
2327                    velocity_gain: kd,
2328                    maximum_velocity: max_vel,
2329                } => {
2330                    if let Some(max_vel) = max_vel {
2331                        ffi::b3JointControlSetMaximumVelocity(command, info.u_index, max_vel);
2332                    }
2333                    ffi::b3JointControlSetDesiredPosition(command, info.q_index, pos);
2334
2335                    ffi::b3JointControlSetKp(command, info.u_index, kp);
2336                    ffi::b3JointControlSetDesiredVelocity(command, info.u_index, vel);
2337
2338                    ffi::b3JointControlSetKd(command, info.u_index, kd);
2339                    ffi::b3JointControlSetMaximumForce(command, info.u_index, force);
2340                }
2341                ControlMode::Velocity(vel) => {
2342                    ffi::b3JointControlSetDesiredVelocity(command, info.u_index, vel);
2343                    ffi::b3JointControlSetKd(command, info.u_index, kd);
2344                    ffi::b3JointControlSetMaximumForce(command, info.u_index, force);
2345                }
2346                ControlMode::Torque(f) => {
2347                    ffi::b3JointControlSetDesiredForceTorque(command, info.u_index, f);
2348                }
2349            }
2350        }
2351        self.submit_simple_command(
2352            command,
2353            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
2354        )?;
2355        Ok(())
2356    }
2357
2358    pub fn set_joint_motor_control_array(
2359        &mut self,
2360        body_unique_id: i32,
2361        joint_index: &[i32],
2362        control_mode: ControlModeArray<'_>,
2363        maximum_force: Option<&[f64]>,
2364    ) -> BulletResult<()> {
2365        const DEFAULT_FORCE: f64 = 100_000.0;
2366        const DEFAULT_KP: f64 = 0.1;
2367        const DEFAULT_KD: f64 = 1.0;
2368        const DEFAULT_TARGET_VELOCITY: f64 = 0.0;
2369
2370        if joint_index.is_empty() {
2371            return Ok(());
2372        }
2373        self.ensure_can_submit()?;
2374        let command = unsafe {
2375            ffi::b3JointControlCommandInit2(self.handle, body_unique_id, control_mode.as_raw())
2376        };
2377
2378        let joint_count = joint_index.len();
2379
2380        match control_mode {
2381            ControlModeArray::Position(targets) => {
2382                if targets.len() != joint_count {
2383                    return Err(BulletError::CommandFailed {
2384                        message: "Joint index and position target lengths differ",
2385                        code: -1,
2386                    });
2387                }
2388                if let Some(forces) = maximum_force
2389                    && forces.len() != joint_count
2390                {
2391                    return Err(BulletError::CommandFailed {
2392                        message: "Joint index and maximum force lengths differ",
2393                        code: -1,
2394                    });
2395                }
2396
2397                for (i, joint) in joint_index.iter().copied().enumerate() {
2398                    let info = self.get_joint_info(body_unique_id, joint)?;
2399                    unsafe {
2400                        ffi::b3JointControlSetDesiredPosition(command, info.q_index, targets[i]);
2401                        ffi::b3JointControlSetKp(command, info.u_index, DEFAULT_KP);
2402                        ffi::b3JointControlSetDesiredVelocity(
2403                            command,
2404                            info.u_index,
2405                            DEFAULT_TARGET_VELOCITY,
2406                        );
2407                        ffi::b3JointControlSetKd(command, info.u_index, DEFAULT_KD);
2408                        ffi::b3JointControlSetMaximumForce(
2409                            command,
2410                            info.u_index,
2411                            maximum_force.map_or(DEFAULT_FORCE, |forces| forces[i]),
2412                        );
2413                    }
2414                }
2415            }
2416            ControlModeArray::Pd {
2417                target_position: pos,
2418                target_velocity: vel,
2419                position_gain: kp,
2420                velocity_gain: kd,
2421                maximum_velocity: max_vel,
2422            }
2423            | ControlModeArray::StablePd {
2424                target_position: pos,
2425                target_velocity: vel,
2426                position_gain: kp,
2427                velocity_gain: kd,
2428                maximum_velocity: max_vel,
2429            } => {
2430                if pos.len() != joint_count
2431                    || vel.len() != joint_count
2432                    || kp.len() != joint_count
2433                    || kd.len() != joint_count
2434                {
2435                    return Err(BulletError::CommandFailed {
2436                        message: "Joint index and PD target lengths differ",
2437                        code: -1,
2438                    });
2439                }
2440                if let Some(max_velocities) = max_vel
2441                    && max_velocities.len() != joint_count
2442                {
2443                    return Err(BulletError::CommandFailed {
2444                        message: "Joint index and maximum velocity lengths differ",
2445                        code: -1,
2446                    });
2447                }
2448
2449                if let Some(forces) = maximum_force
2450                    && forces.len() != joint_count
2451                {
2452                    return Err(BulletError::CommandFailed {
2453                        message: "Joint index and maximum force lengths differ",
2454                        code: -1,
2455                    });
2456                }
2457
2458                for (i, joint) in joint_index.iter().copied().enumerate() {
2459                    let info = self.get_joint_info(body_unique_id, joint)?;
2460                    if let Some(max_velocities) = max_vel {
2461                        unsafe {
2462                            ffi::b3JointControlSetMaximumVelocity(
2463                                command,
2464                                info.u_index,
2465                                max_velocities[i],
2466                            );
2467                        }
2468                    }
2469                    unsafe {
2470                        ffi::b3JointControlSetDesiredPosition(command, info.q_index, pos[i]);
2471                        ffi::b3JointControlSetKp(command, info.u_index, kp[i]);
2472                        ffi::b3JointControlSetDesiredVelocity(command, info.u_index, vel[i]);
2473                        ffi::b3JointControlSetKd(command, info.u_index, kd[i]);
2474                        ffi::b3JointControlSetMaximumForce(
2475                            command,
2476                            info.u_index,
2477                            maximum_force.map_or(DEFAULT_FORCE, |forces| forces[i]),
2478                        );
2479                    }
2480                }
2481            }
2482            ControlModeArray::Velocity(velocities) => {
2483                if velocities.len() != joint_count {
2484                    return Err(BulletError::CommandFailed {
2485                        message: "Joint index and velocity target lengths differ",
2486                        code: -1,
2487                    });
2488                }
2489                if let Some(forces) = maximum_force
2490                    && forces.len() != joint_count
2491                {
2492                    return Err(BulletError::CommandFailed {
2493                        message: "Joint index and maximum force lengths differ",
2494                        code: -1,
2495                    });
2496                }
2497
2498                for (i, joint) in joint_index.iter().copied().enumerate() {
2499                    let info = self.get_joint_info(body_unique_id, joint)?;
2500                    unsafe {
2501                        ffi::b3JointControlSetDesiredVelocity(command, info.u_index, velocities[i]);
2502                        ffi::b3JointControlSetKd(command, info.u_index, DEFAULT_KD);
2503                        ffi::b3JointControlSetMaximumForce(
2504                            command,
2505                            info.u_index,
2506                            maximum_force.map_or(DEFAULT_FORCE, |forces| forces[i]),
2507                        );
2508                    }
2509                }
2510            }
2511            ControlModeArray::Torque(forces) => {
2512                if forces.len() != joint_count {
2513                    return Err(BulletError::CommandFailed {
2514                        message: "Joint index and torque lengths differ",
2515                        code: -1,
2516                    });
2517                }
2518
2519                for (force, joint) in forces.iter().copied().zip(joint_index.iter().copied()) {
2520                    let info = self.get_joint_info(body_unique_id, joint)?;
2521                    unsafe {
2522                        ffi::b3JointControlSetDesiredForceTorque(command, info.u_index, force);
2523                    }
2524                }
2525            }
2526        }
2527
2528        self.submit_command(command)?;
2529        Ok(())
2530    }
2531
2532    pub fn set_joint_motor_control_multi_dof(
2533        &mut self,
2534        body_unique_id: i32,
2535        joint_index: i32,
2536        control: &MultiDofControl<'_>,
2537    ) -> BulletResult<()> {
2538        self.ensure_can_submit()?;
2539        let info = self.get_joint_info(body_unique_id, joint_index)?;
2540        let command = unsafe {
2541            ffi::b3JointControlCommandInit2(self.handle, body_unique_id, control.as_raw())
2542        };
2543
2544        Self::configure_multi_dof_control(command, &info, control)?;
2545
2546        self.submit_simple_command(
2547            command,
2548            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
2549        )?;
2550        Ok(())
2551    }
2552
2553    pub fn set_joint_motor_control_multi_dof_array(
2554        &mut self,
2555        body_unique_id: i32,
2556        entries: &[MultiDofControlEntry<'_>],
2557    ) -> BulletResult<()> {
2558        if entries.is_empty() {
2559            return Ok(());
2560        }
2561        self.ensure_can_submit()?;
2562        // 选择第一个条目的控制模式初始化命令（Bullet 要求一次命令同一模式）。
2563        let command = unsafe {
2564            ffi::b3JointControlCommandInit2(
2565                self.handle,
2566                body_unique_id,
2567                entries[0].control.as_raw(),
2568            )
2569        };
2570
2571        // 校验所有条目控制模式一致，以符合底层一次命令的限制。
2572        let expected_mode = entries[0].control.as_raw();
2573        for entry in entries {
2574            if entry.control.as_raw() != expected_mode {
2575                return Err(BulletError::CommandFailed {
2576                    message: "All Multi-DoF array entries must use the same control mode",
2577                    code: -1,
2578                });
2579            }
2580            let info = self.get_joint_info(body_unique_id, entry.joint_index)?;
2581            Self::configure_multi_dof_control(command, &info, &entry.control)?;
2582        }
2583
2584        self.submit_simple_command(
2585            command,
2586            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
2587        )?;
2588        Ok(())
2589    }
2590
2591    pub fn apply_external_force(
2592        &mut self,
2593        body_unique_id: i32,
2594        link_index: i32,
2595        force: impl Into<[f64; 3]>,
2596        position: impl Into<[f64; 3]>,
2597        frame: ForceFrame,
2598    ) -> BulletResult<()> {
2599        self.ensure_can_submit()?;
2600        let command = unsafe { ffi::b3ApplyExternalForceCommandInit(self.handle) };
2601        let force = force.into();
2602        let position = position.into();
2603        unsafe {
2604            ffi::b3ApplyExternalForce(
2605                command,
2606                body_unique_id,
2607                link_index,
2608                force.as_ptr(),
2609                position.as_ptr(),
2610                frame.as_raw(),
2611            );
2612        }
2613        self.submit_simple_command(
2614            command,
2615            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
2616        )?;
2617        Ok(())
2618    }
2619
2620    pub fn apply_external_torque(
2621        &mut self,
2622        body_unique_id: i32,
2623        link_index: i32,
2624        torque: impl Into<[f64; 3]>,
2625        frame: ForceFrame,
2626    ) -> BulletResult<()> {
2627        self.ensure_can_submit()?;
2628        let command = unsafe { ffi::b3ApplyExternalForceCommandInit(self.handle) };
2629        let torque = torque.into();
2630        unsafe {
2631            ffi::b3ApplyExternalTorque(
2632                command,
2633                body_unique_id,
2634                link_index,
2635                torque.as_ptr(),
2636                frame.as_raw(),
2637            );
2638        }
2639        self.submit_simple_command(
2640            command,
2641            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
2642        )?;
2643        Ok(())
2644    }
2645
2646    pub fn calculate_inverse_dynamics(
2647        &mut self,
2648        body_unique_id: i32,
2649        joint_positions: &[f64],
2650        joint_velocities: &[f64],
2651        joint_accelerations: &[f64],
2652    ) -> BulletResult<Vec<f64>> {
2653        if joint_positions.len() != joint_velocities.len()
2654            || joint_positions.len() != joint_accelerations.len()
2655        {
2656            return Err(BulletError::CommandFailed {
2657                message: "Inverse dynamics expects position/velocity/acceleration arrays of equal length",
2658                code: joint_positions.len() as i32,
2659            });
2660        }
2661
2662        self.ensure_can_submit()?;
2663        let dof = joint_positions.len() as i32;
2664        let command = unsafe {
2665            ffi::b3CalculateInverseDynamicsCommandInit2(
2666                self.handle,
2667                body_unique_id,
2668                joint_positions.as_ptr(),
2669                dof,
2670                joint_velocities.as_ptr(),
2671                joint_accelerations.as_ptr(),
2672                dof,
2673            )
2674        };
2675
2676        let status = self.submit_simple_command(
2677            command,
2678            ffi::EnumSharedMemoryServerStatus::CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
2679        )?;
2680
2681        let mut result_body = 0;
2682        let mut result_dof = 0;
2683        let mut joint_forces = vec![0.0_f64; joint_positions.len()];
2684        let success = unsafe {
2685            ffi::b3GetStatusInverseDynamicsJointForces(
2686                status.handle,
2687                &raw mut result_body,
2688                &raw mut result_dof,
2689                joint_forces.as_mut_ptr(),
2690            )
2691        };
2692        if success == 0 {
2693            return Err(BulletError::CommandFailed {
2694                message: "Failed to calculate inverse dynamics",
2695                code: success,
2696            });
2697        }
2698        joint_forces.truncate(result_dof as usize);
2699        Ok(joint_forces)
2700    }
2701
2702    pub fn calculate_jacobian(
2703        &mut self,
2704        body_unique_id: i32,
2705        link_index: i32,
2706        local_position: [f64; 3],
2707        joint_positions: &[f64],
2708        joint_velocities: &[f64],
2709        joint_accelerations: &[f64],
2710    ) -> BulletResult<Jacobian> {
2711        let dof = joint_positions.len();
2712        if dof == 0 || joint_velocities.len() != dof || joint_accelerations.len() != dof {
2713            return Err(BulletError::CommandFailed {
2714                message: "Jacobian computation expects non-empty arrays of equal length",
2715                code: dof as i32,
2716            });
2717        }
2718
2719        self.ensure_can_submit()?;
2720        let command = unsafe {
2721            ffi::b3CalculateJacobianCommandInit(
2722                self.handle,
2723                body_unique_id,
2724                link_index,
2725                local_position.as_ptr(),
2726                joint_positions.as_ptr(),
2727                joint_velocities.as_ptr(),
2728                joint_accelerations.as_ptr(),
2729            )
2730        };
2731
2732        let status = self.submit_simple_command(
2733            command,
2734            ffi::EnumSharedMemoryServerStatus::CMD_CALCULATED_JACOBIAN_COMPLETED,
2735        )?;
2736
2737        let mut out_dof = 0;
2738        let mut linear = vec![0.0_f64; 3 * dof];
2739        let mut angular = vec![0.0_f64; 3 * dof];
2740        let success = unsafe {
2741            ffi::b3GetStatusJacobian(
2742                status.handle,
2743                &raw mut out_dof,
2744                linear.as_mut_ptr(),
2745                angular.as_mut_ptr(),
2746            )
2747        };
2748        if success == 0 {
2749            return Err(BulletError::CommandFailed {
2750                message: "Failed to calculate Jacobian",
2751                code: success,
2752            });
2753        }
2754        let out_cols = out_dof as usize;
2755        if out_cols == 0 {
2756            return Ok(Jacobian::zeros(0));
2757        }
2758
2759        let mut linear_matrix = na::Matrix3xX::zeros(out_cols);
2760        let mut angular_matrix = na::Matrix3xX::zeros(out_cols);
2761        for col in 0..out_cols {
2762            for row in 0..3 {
2763                linear_matrix[(row, col)] = linear[row * out_cols + col];
2764                angular_matrix[(row, col)] = angular[row * out_cols + col];
2765            }
2766        }
2767
2768        Ok(Jacobian { linear: linear_matrix, angular: angular_matrix })
2769    }
2770
2771    pub fn calculate_mass_matrix(
2772        &mut self,
2773        body_unique_id: i32,
2774        joint_positions: &[f64],
2775    ) -> BulletResult<MassMatrix> {
2776        let dof = joint_positions.len();
2777        if dof == 0 {
2778            return Err(BulletError::CommandFailed {
2779                message: "Mass matrix requires at least one joint coordinate",
2780                code: dof as i32,
2781            });
2782        }
2783
2784        self.ensure_can_submit()?;
2785        let command = unsafe {
2786            ffi::b3CalculateMassMatrixCommandInit(
2787                self.handle,
2788                body_unique_id,
2789                joint_positions.as_ptr(),
2790                dof as i32,
2791            )
2792        };
2793
2794        let status = self.submit_simple_command(
2795            command,
2796            ffi::EnumSharedMemoryServerStatus::CMD_CALCULATED_MASS_MATRIX_COMPLETED,
2797        )?;
2798
2799        let mut out_dof = 0;
2800        let mut raw = vec![0.0_f64; dof * dof];
2801        let success = unsafe {
2802            ffi::b3GetStatusMassMatrix(
2803                self.handle,
2804                status.handle,
2805                &raw mut out_dof,
2806                raw.as_mut_ptr(),
2807            )
2808        };
2809        if success == 0 {
2810            return Err(BulletError::CommandFailed {
2811                message: "Failed to calculate mass matrix",
2812                code: success,
2813            });
2814        }
2815        let matrix_size = out_dof as usize;
2816        if matrix_size == 0 {
2817            return Ok(MassMatrix { matrix: na::DMatrix::zeros(0, 0) });
2818        }
2819        raw.truncate(matrix_size * matrix_size);
2820        let matrix = na::DMatrix::from_column_slice(matrix_size, matrix_size, &raw);
2821        Ok(MassMatrix { matrix })
2822    }
2823
2824    pub fn calculate_inverse_kinematics(
2825        &mut self,
2826        body_unique_id: i32,
2827        end_effector_link_index: i32,
2828        target_position: impl Into<na::Isometry3<f64>>,
2829        options: &InverseKinematicsOptions<'_>,
2830    ) -> BulletResult<Vec<f64>> {
2831        self.ensure_can_submit()?;
2832        let dof = unsafe { ffi::b3ComputeDofCount(self.handle, body_unique_id) };
2833        if dof <= 0 {
2834            return Err(BulletError::CommandFailed {
2835                message: "Body has no movable joints for inverse kinematics",
2836                code: dof,
2837            });
2838        }
2839        let dof_usize = dof as usize;
2840        let (tran, rot) = isometry_to_raw_parts(&target_position.into());
2841
2842        let command =
2843            unsafe { ffi::b3CalculateInverseKinematicsCommandInit(self.handle, body_unique_id) };
2844
2845        unsafe { ffi::b3CalculateInverseKinematicsSelectSolver(command, options.solver as i32) };
2846
2847        if let Some(current_positions) = options.current_positions {
2848            if current_positions.len() != dof_usize {
2849                return Err(BulletError::CommandFailed {
2850                    message: "current_positions length must match degrees of freedom",
2851                    code: current_positions.len() as i32,
2852                });
2853            }
2854            unsafe {
2855                ffi::b3CalculateInverseKinematicsSetCurrentPositions(
2856                    command,
2857                    dof,
2858                    current_positions.as_ptr(),
2859                );
2860            }
2861        }
2862        if let Some(max_iterations) = options.max_iterations
2863            && max_iterations > 0
2864        {
2865            unsafe {
2866                ffi::b3CalculateInverseKinematicsSetMaxNumIterations(command, max_iterations);
2867            }
2868        }
2869        if let Some(residual_threshold) = options.residual_threshold
2870            && residual_threshold >= 0.0
2871        {
2872            unsafe {
2873                ffi::b3CalculateInverseKinematicsSetResidualThreshold(command, residual_threshold);
2874            }
2875        }
2876
2877        let has_nullspace = options.lower_limits.map_or(0, <[f64]>::len) == dof_usize
2878            && options.upper_limits.map_or(0, <[f64]>::len) == dof_usize
2879            && options.joint_ranges.map_or(0, <[f64]>::len) == dof_usize
2880            && options.rest_poses.map_or(0, <[f64]>::len) == dof_usize;
2881
2882        unsafe {
2883            if has_nullspace {
2884                ffi::b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(
2885                    command,
2886                    dof,
2887                    end_effector_link_index,
2888                    tran.as_ptr(),
2889                    rot.as_ptr(),
2890                    options.lower_limits.unwrap().as_ptr(),
2891                    options.upper_limits.unwrap().as_ptr(),
2892                    options.joint_ranges.unwrap().as_ptr(),
2893                    options.rest_poses.unwrap().as_ptr(),
2894                );
2895            } else {
2896                ffi::b3CalculateInverseKinematicsAddTargetPositionWithOrientation(
2897                    command,
2898                    end_effector_link_index,
2899                    tran.as_ptr(),
2900                    rot.as_ptr(),
2901                );
2902            }
2903        }
2904
2905        if let Some(joint_damping) = options.joint_damping {
2906            if joint_damping.len() < dof_usize {
2907                return Err(BulletError::CommandFailed {
2908                    message: "joint_damping length must be >= degrees of freedom",
2909                    code: joint_damping.len() as i32,
2910                });
2911            }
2912            unsafe {
2913                ffi::b3CalculateInverseKinematicsSetJointDamping(
2914                    command,
2915                    dof,
2916                    joint_damping.as_ptr(),
2917                );
2918            }
2919        }
2920
2921        let status = self.submit_simple_command(
2922            command,
2923            ffi::EnumSharedMemoryServerStatus::CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
2924        )?;
2925
2926        let mut result_body = 0;
2927        let mut result_dof = 0;
2928        let mut results = vec![0.0_f64; dof_usize];
2929        let success = unsafe {
2930            ffi::b3GetStatusInverseKinematicsJointPositions(
2931                status.handle,
2932                &raw mut result_body,
2933                &raw mut result_dof,
2934                results.as_mut_ptr(),
2935            )
2936        };
2937        if success == 0 {
2938            return Err(BulletError::CommandFailed {
2939                message: "Failed to compute inverse kinematics",
2940                code: success,
2941            });
2942        }
2943        results.truncate(result_dof as usize);
2944        Ok(results)
2945    }
2946
2947    pub fn calculate_inverse_kinematics2(
2948        &mut self,
2949        body_unique_id: i32,
2950        options: &InverseKinematicsMultiTargetOptions<'_>,
2951    ) -> BulletResult<Vec<f64>> {
2952        self.ensure_can_submit()?;
2953        let num_targets = options.end_effector_link_indices.len();
2954        if num_targets == 0 {
2955            return Err(BulletError::CommandFailed {
2956                message: "inverse_kinematics2 requires at least one end-effector target",
2957                code: 0,
2958            });
2959        }
2960        if num_targets != options.target_positions.len() {
2961            return Err(BulletError::CommandFailed {
2962                message: "endEffectorLinkIndices and targetPositions must have matching lengths",
2963                code: num_targets as i32,
2964            });
2965        }
2966
2967        let dof = unsafe { ffi::b3ComputeDofCount(self.handle, body_unique_id) };
2968        if dof <= 0 {
2969            return Err(BulletError::CommandFailed {
2970                message: "Body has no movable joints for inverse kinematics",
2971                code: dof,
2972            });
2973        }
2974        let dof_usize = dof as usize;
2975
2976        let command =
2977            unsafe { ffi::b3CalculateInverseKinematicsCommandInit(self.handle, body_unique_id) };
2978        if let Some(solver) = options.solver {
2979            unsafe { ffi::b3CalculateInverseKinematicsSelectSolver(command, solver) };
2980        }
2981        if let Some(current_positions) = options.current_positions {
2982            if current_positions.len() != dof_usize {
2983                return Err(BulletError::CommandFailed {
2984                    message: "current_positions length must match degrees of freedom",
2985                    code: current_positions.len() as i32,
2986                });
2987            }
2988            unsafe {
2989                ffi::b3CalculateInverseKinematicsSetCurrentPositions(
2990                    command,
2991                    dof,
2992                    current_positions.as_ptr(),
2993                );
2994            }
2995        }
2996        if let Some(max_iterations) = options.max_iterations
2997            && max_iterations > 0
2998        {
2999            unsafe {
3000                ffi::b3CalculateInverseKinematicsSetMaxNumIterations(command, max_iterations);
3001            }
3002        }
3003        if let Some(residual_threshold) = options.residual_threshold
3004            && residual_threshold >= 0.0
3005        {
3006            unsafe {
3007                ffi::b3CalculateInverseKinematicsSetResidualThreshold(command, residual_threshold);
3008            }
3009        }
3010
3011        let mut flattened_positions = Vec::with_capacity(num_targets * 3);
3012        for pos in options.target_positions {
3013            flattened_positions.extend_from_slice(pos);
3014        }
3015        let link_indices: Vec<i32> = options.end_effector_link_indices.to_vec();
3016
3017        unsafe {
3018            ffi::b3CalculateInverseKinematicsAddTargetsPurePosition(
3019                command,
3020                link_indices.len() as i32,
3021                link_indices.as_ptr(),
3022                flattened_positions.as_ptr(),
3023            );
3024        }
3025
3026        let has_nullspace = options.lower_limits.map_or(0, <[f64]>::len) == dof_usize
3027            && options.upper_limits.map_or(0, <[f64]>::len) == dof_usize
3028            && options.joint_ranges.map_or(0, <[f64]>::len) == dof_usize
3029            && options.rest_poses.map_or(0, <[f64]>::len) == dof_usize;
3030
3031        unsafe {
3032            if has_nullspace {
3033                ffi::b3CalculateInverseKinematicsPosWithNullSpaceVel(
3034                    command,
3035                    dof,
3036                    link_indices[0],
3037                    options.target_positions[0].as_ptr(),
3038                    options.lower_limits.unwrap().as_ptr(),
3039                    options.upper_limits.unwrap().as_ptr(),
3040                    options.joint_ranges.unwrap().as_ptr(),
3041                    options.rest_poses.unwrap().as_ptr(),
3042                );
3043            }
3044        }
3045
3046        if let Some(joint_damping) = options.joint_damping {
3047            if joint_damping.len() < dof_usize {
3048                return Err(BulletError::CommandFailed {
3049                    message: "joint_damping length must be >= degrees of freedom",
3050                    code: joint_damping.len() as i32,
3051                });
3052            }
3053            unsafe {
3054                ffi::b3CalculateInverseKinematicsSetJointDamping(
3055                    command,
3056                    dof,
3057                    joint_damping.as_ptr(),
3058                );
3059            }
3060        }
3061
3062        let status = self.submit_simple_command(
3063            command,
3064            ffi::EnumSharedMemoryServerStatus::CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
3065        )?;
3066
3067        let mut result_body = 0;
3068        let mut result_dof = 0;
3069        let mut results = vec![0.0_f64; dof_usize];
3070        let success = unsafe {
3071            ffi::b3GetStatusInverseKinematicsJointPositions(
3072                status.handle,
3073                &raw mut result_body,
3074                &raw mut result_dof,
3075                results.as_mut_ptr(),
3076            )
3077        };
3078        if success == 0 {
3079            return Err(BulletError::CommandFailed {
3080                message: "Failed to compute inverse kinematics",
3081                code: success,
3082            });
3083        }
3084        results.truncate(result_dof as usize);
3085        Ok(results)
3086    }
3087}
3088
3089/// ! =====================================================================================================================================
3090/// ### Collision Queries & Contact Data
3091///
3092/// | API | Status | Notes |
3093/// | --- | --- | --- |
3094/// | getContactPoints | **Implemented** | Returns per-contact data |
3095/// | getClosestPoints | **Implemented** | Distance queries |
3096/// | getOverlappingObjects | **Implemented** | AABB queries |
3097/// | setCollisionFilterPair | **Implemented** | Collision filtering |
3098/// | setCollisionFilterGroupMask | **Implemented** | Collision filtering |
3099/// | rayTest | **Implemented** | Single raycast |
3100/// | rayTestBatch | **Implemented** | Batch raycasts |
3101impl PhysicsClient {
3102    pub fn get_contact_points(
3103        &mut self,
3104        body_a: Option<i32>,
3105        body_b: Option<i32>,
3106        link_a: Option<i32>,
3107        link_b: Option<i32>,
3108    ) -> BulletResult<Vec<ContactPoint>> {
3109        self.ensure_can_submit()?;
3110        let command = unsafe { ffi::b3InitRequestContactPointInformation(self.handle) };
3111        unsafe {
3112            if let Some(body) = body_a {
3113                ffi::b3SetContactFilterBodyA(command, body);
3114            }
3115            if let Some(body) = body_b {
3116                ffi::b3SetContactFilterBodyB(command, body);
3117            }
3118            if let Some(link) = link_a {
3119                ffi::b3SetContactFilterLinkA(command, link);
3120            }
3121            if let Some(link) = link_b {
3122                ffi::b3SetContactFilterLinkB(command, link);
3123            }
3124        }
3125
3126        self.submit_simple_command(
3127            command,
3128            ffi::EnumSharedMemoryServerStatus::CMD_CONTACT_POINT_INFORMATION_COMPLETED,
3129        )?;
3130
3131        let mut info = MaybeUninit::<ffi::b3ContactInformation>::uninit();
3132        unsafe {
3133            ffi::b3GetContactPointInformation(self.handle, info.as_mut_ptr());
3134            let info = info.assume_init();
3135            Ok(Self::collect_contact_points(&info))
3136        }
3137    }
3138
3139    pub fn get_closest_points(
3140        &mut self,
3141        body_a: i32,
3142        body_b: i32,
3143        distance: f64,
3144        options: &ClosestPointsOptions,
3145    ) -> BulletResult<Vec<ClosestPoint>> {
3146        self.ensure_can_submit()?;
3147        let command = unsafe { ffi::b3InitClosestDistanceQuery(self.handle) };
3148        unsafe {
3149            ffi::b3SetClosestDistanceFilterBodyA(command, body_a);
3150            ffi::b3SetClosestDistanceFilterBodyB(command, body_b);
3151            ffi::b3SetClosestDistanceThreshold(command, distance);
3152            if let Some(link) = options.link_index_a {
3153                ffi::b3SetClosestDistanceFilterLinkA(command, link);
3154            }
3155            if let Some(link) = options.link_index_b {
3156                ffi::b3SetClosestDistanceFilterLinkB(command, link);
3157            }
3158            if let Some(shape) = options.collision_shape_a {
3159                ffi::b3SetClosestDistanceFilterCollisionShapeA(command, shape);
3160            }
3161            if let Some(shape) = options.collision_shape_b {
3162                ffi::b3SetClosestDistanceFilterCollisionShapeB(command, shape);
3163            }
3164            if let Some(position) = options.collision_shape_position_a {
3165                ffi::b3SetClosestDistanceFilterCollisionShapePositionA(command, position.as_ptr());
3166            }
3167            if let Some(position) = options.collision_shape_position_b {
3168                ffi::b3SetClosestDistanceFilterCollisionShapePositionB(command, position.as_ptr());
3169            }
3170            if let Some(orientation) = options.collision_shape_orientation_a {
3171                ffi::b3SetClosestDistanceFilterCollisionShapeOrientationA(
3172                    command,
3173                    orientation.as_ptr(),
3174                );
3175            }
3176            if let Some(orientation) = options.collision_shape_orientation_b {
3177                ffi::b3SetClosestDistanceFilterCollisionShapeOrientationB(
3178                    command,
3179                    orientation.as_ptr(),
3180                );
3181            }
3182        }
3183
3184        self.submit_simple_command(
3185            command,
3186            ffi::EnumSharedMemoryServerStatus::CMD_CONTACT_POINT_INFORMATION_COMPLETED,
3187        )?;
3188
3189        let mut info = MaybeUninit::<ffi::b3ContactInformation>::uninit();
3190        unsafe {
3191            ffi::b3GetClosestPointInformation(self.handle, info.as_mut_ptr());
3192            let info = info.assume_init();
3193            Ok(Self::collect_contact_points(&info))
3194        }
3195    }
3196
3197    pub fn get_overlapping_objects(
3198        &mut self,
3199        aabb_min: [f64; 3],
3200        aabb_max: [f64; 3],
3201    ) -> BulletResult<Vec<OverlappingObject>> {
3202        self.ensure_can_submit()?;
3203        let command = unsafe {
3204            ffi::b3InitAABBOverlapQuery(self.handle, aabb_min.as_ptr(), aabb_max.as_ptr())
3205        };
3206        self.submit_simple_command(
3207            command,
3208            ffi::EnumSharedMemoryServerStatus::CMD_REQUEST_AABB_OVERLAP_COMPLETED,
3209        )?;
3210
3211        let mut data = MaybeUninit::<ffi::b3AABBOverlapData>::uninit();
3212        unsafe {
3213            ffi::b3GetAABBOverlapResults(self.handle, data.as_mut_ptr());
3214            let data = data.assume_init();
3215            Ok(Self::collect_overlapping_objects(&data))
3216        }
3217    }
3218
3219    pub fn set_collision_filter_pair(
3220        &mut self,
3221        body_a: i32,
3222        body_b: i32,
3223        link_a: i32,
3224        link_b: i32,
3225        enable_collision: bool,
3226    ) -> BulletResult<&mut Self> {
3227        self.ensure_can_submit()?;
3228        let command = unsafe { ffi::b3CollisionFilterCommandInit(self.handle) };
3229        unsafe {
3230            ffi::b3SetCollisionFilterPair(
3231                command,
3232                body_a,
3233                body_b,
3234                link_a,
3235                link_b,
3236                enable_collision as i32,
3237            );
3238        }
3239        self.submit_simple_command(
3240            command,
3241            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
3242        )?;
3243        Ok(self)
3244    }
3245
3246    pub fn set_collision_filter_group_mask(
3247        &mut self,
3248        body_unique_id: i32,
3249        link_index: i32,
3250        collision_filter_group: i32,
3251        collision_filter_mask: i32,
3252    ) -> BulletResult<&mut Self> {
3253        self.ensure_can_submit()?;
3254        let command = unsafe { ffi::b3CollisionFilterCommandInit(self.handle) };
3255        unsafe {
3256            ffi::b3SetCollisionFilterGroupMask(
3257                command,
3258                body_unique_id,
3259                link_index,
3260                collision_filter_group,
3261                collision_filter_mask,
3262            );
3263        }
3264        self.submit_simple_command(
3265            command,
3266            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
3267        )?;
3268        Ok(self)
3269    }
3270
3271    pub fn ray_test(
3272        &mut self,
3273        ray_from: [f64; 3],
3274        ray_to: [f64; 3],
3275        collision_filter_mask: Option<i32>,
3276        report_hit_number: Option<i32>,
3277    ) -> BulletResult<Vec<RayHit>> {
3278        self.ensure_can_submit()?;
3279        let command = unsafe {
3280            ffi::b3CreateRaycastCommandInit(
3281                self.handle,
3282                ray_from[0],
3283                ray_from[1],
3284                ray_from[2],
3285                ray_to[0],
3286                ray_to[1],
3287                ray_to[2],
3288            )
3289        };
3290        unsafe {
3291            if let Some(mask) = collision_filter_mask {
3292                ffi::b3RaycastBatchSetCollisionFilterMask(command, mask);
3293            }
3294            if let Some(hit_number) = report_hit_number {
3295                ffi::b3RaycastBatchSetReportHitNumber(command, hit_number);
3296            }
3297        }
3298
3299        self.submit_simple_command(
3300            command,
3301            ffi::EnumSharedMemoryServerStatus::CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
3302        )?;
3303
3304        let mut info = MaybeUninit::<ffi::b3RaycastInformation>::uninit();
3305        unsafe {
3306            ffi::b3GetRaycastInformation(self.handle, info.as_mut_ptr());
3307            let info = info.assume_init();
3308            Ok(Self::collect_ray_hits(&info))
3309        }
3310    }
3311
3312    pub fn ray_test_batch(
3313        &mut self,
3314        options: &RayTestBatchOptions<'_>,
3315    ) -> BulletResult<Vec<RayHit>> {
3316        let from_len = options.ray_from_positions.len();
3317        let to_len = options.ray_to_positions.len();
3318        if from_len == 0 || from_len != to_len {
3319            return Err(BulletError::CommandFailed {
3320                message: "rayTestBatch requires non-empty rayFrom/rayTo arrays of equal length",
3321                code: from_len as i32,
3322            });
3323        }
3324
3325        self.ensure_can_submit()?;
3326        let command = unsafe { ffi::b3CreateRaycastBatchCommandInit(self.handle) };
3327        unsafe {
3328            if let Some(num_threads) = options.num_threads {
3329                ffi::b3RaycastBatchSetNumThreads(command, num_threads);
3330            }
3331            if let Some(parent_id) = options.parent_object_unique_id {
3332                ffi::b3RaycastBatchSetParentObject(
3333                    command,
3334                    parent_id,
3335                    options.parent_link_index.unwrap_or(-1),
3336                );
3337            }
3338            if let Some(hit_number) = options.report_hit_number {
3339                ffi::b3RaycastBatchSetReportHitNumber(command, hit_number);
3340            }
3341            if let Some(mask) = options.collision_filter_mask {
3342                ffi::b3RaycastBatchSetCollisionFilterMask(command, mask);
3343            }
3344            if let Some(epsilon) = options.fraction_epsilon {
3345                ffi::b3RaycastBatchSetFractionEpsilon(command, epsilon);
3346            }
3347        }
3348
3349        for (from, to) in options
3350            .ray_from_positions
3351            .iter()
3352            .zip(options.ray_to_positions.iter())
3353        {
3354            unsafe { ffi::b3RaycastBatchAddRay(command, from.as_ptr(), to.as_ptr()) };
3355        }
3356
3357        self.submit_simple_command(
3358            command,
3359            ffi::EnumSharedMemoryServerStatus::CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
3360        )?;
3361
3362        let mut info = MaybeUninit::<ffi::b3RaycastInformation>::uninit();
3363        unsafe {
3364            ffi::b3GetRaycastInformation(self.handle, info.as_mut_ptr());
3365            let info = info.assume_init();
3366            Ok(Self::collect_ray_hits(&info))
3367        }
3368    }
3369
3370    fn configure_multi_dof_control(
3371        command: ffi::b3SharedMemoryCommandHandle,
3372        info: &JointInfo,
3373        control: &MultiDofControl<'_>,
3374    ) -> BulletResult<()> {
3375        let q_index = info.q_index;
3376        let u_index = info.u_index;
3377        let q_size = info.q_size.max(0) as usize;
3378        let u_size = info.u_size.max(0) as usize;
3379
3380        // 提取通用字段（damping、max_velocity），随后根据模式填充细节。
3381        let (damping_opt, max_velocity_opt) = match control {
3382            MultiDofControl::Position { damping, max_velocity, .. }
3383            | MultiDofControl::Pd { damping, max_velocity, .. }
3384            | MultiDofControl::StablePd { damping, max_velocity, .. } => (*damping, *max_velocity),
3385            MultiDofControl::Velocity { damping, .. } | MultiDofControl::Torque { damping, .. } => {
3386                (*damping, None)
3387            }
3388        };
3389
3390        if let Some(max_velocity) = max_velocity_opt {
3391            if u_index < 0 || u_size == 0 {
3392                return Err(BulletError::CommandFailed {
3393                    message: "Joint has no velocity DOF for max velocity control",
3394                    code: u_index,
3395                });
3396            }
3397            for dof in 0..u_size {
3398                unsafe {
3399                    ffi::b3JointControlSetMaximumVelocity(
3400                        command,
3401                        u_index + dof as i32,
3402                        max_velocity,
3403                    )
3404                };
3405            }
3406        }
3407        if let Some(damping) = damping_opt {
3408            if u_index < 0 || u_size == 0 {
3409                return Err(BulletError::CommandFailed {
3410                    message: "Joint has no velocity DOF for damping",
3411                    code: u_index,
3412                });
3413            }
3414            Self::ensure_slice_len("damping", damping.len(), u_size)?;
3415            unsafe {
3416                ffi::b3JointControlSetDampingMultiDof(
3417                    command,
3418                    u_index,
3419                    damping.as_ptr(),
3420                    u_size as i32,
3421                );
3422            }
3423        }
3424        if control.is_position_based() {
3425            if q_index < 0 || q_size == 0 {
3426                return Err(BulletError::CommandFailed {
3427                    message: "Joint has no position DOF for position control",
3428                    code: q_index,
3429                });
3430            }
3431            if u_index < 0 || u_size == 0 {
3432                return Err(BulletError::CommandFailed {
3433                    message: "Joint has no velocity DOF for position control",
3434                    code: u_index,
3435                });
3436            }
3437
3438            // 解构各模式下需要的参数
3439            let (target_positions, target_velocities, position_gains, velocity_gains, forces) =
3440                match control {
3441                    MultiDofControl::Position {
3442                        target_positions,
3443                        target_velocities,
3444                        position_gains,
3445                        velocity_gains,
3446                        forces,
3447                        ..
3448                    }
3449                    | MultiDofControl::Pd {
3450                        target_positions,
3451                        target_velocities,
3452                        position_gains,
3453                        velocity_gains,
3454                        forces,
3455                        ..
3456                    }
3457                    | MultiDofControl::StablePd {
3458                        target_positions,
3459                        target_velocities,
3460                        position_gains,
3461                        velocity_gains,
3462                        forces,
3463                        ..
3464                    } => (
3465                        *target_positions,
3466                        *target_velocities,
3467                        *position_gains,
3468                        *velocity_gains,
3469                        *forces,
3470                    ),
3471                    _ => unreachable!(),
3472                };
3473
3474            let positions =
3475                Self::slice_or_default(target_positions, q_size, 0.0, "targetPositions")?;
3476            unsafe {
3477                ffi::b3JointControlSetDesiredPositionMultiDof(
3478                    command,
3479                    q_index,
3480                    positions.as_ptr(),
3481                    q_size as i32,
3482                );
3483            }
3484
3485            let velocities =
3486                Self::slice_or_default(target_velocities, u_size, 0.0, "targetVelocities")?;
3487            unsafe {
3488                ffi::b3JointControlSetDesiredVelocityMultiDof(
3489                    command,
3490                    u_index,
3491                    velocities.as_ptr(),
3492                    u_size as i32,
3493                );
3494            }
3495
3496            let kps = Self::slice_or_default(position_gains, u_size, 0.1, "positionGains")?;
3497            unsafe {
3498                ffi::b3JointControlSetKpMultiDof(command, u_index, kps.as_ptr(), u_size as i32);
3499            }
3500
3501            let kds = Self::slice_or_default(velocity_gains, u_size, 1.0, "velocityGains")?;
3502            unsafe {
3503                ffi::b3JointControlSetKdMultiDof(command, u_index, kds.as_ptr(), u_size as i32);
3504            }
3505
3506            let forces = Self::slice_or_default(forces, u_size, info.max_force, "forces")?;
3507            for (dof, &force) in forces.iter().enumerate() {
3508                unsafe {
3509                    ffi::b3JointControlSetMaximumForce(command, u_index + dof as i32, force);
3510                }
3511            }
3512        } else if control.is_velocity_based() {
3513            if u_index < 0 || u_size == 0 {
3514                return Err(BulletError::CommandFailed {
3515                    message: "Joint has no velocity DOF for velocity control",
3516                    code: u_index,
3517                });
3518            }
3519
3520            let (target_velocities, velocity_gains, forces) = match control {
3521                MultiDofControl::Velocity { target_velocities, velocity_gains, forces, .. } => {
3522                    (*target_velocities, *velocity_gains, *forces)
3523                }
3524                _ => unreachable!(),
3525            };
3526
3527            let velocities =
3528                Self::slice_or_default(target_velocities, u_size, 0.0, "targetVelocities")?;
3529            unsafe {
3530                ffi::b3JointControlSetDesiredVelocityMultiDof(
3531                    command,
3532                    u_index,
3533                    velocities.as_ptr(),
3534                    u_size as i32,
3535                );
3536            }
3537
3538            let kds = Self::slice_or_default(velocity_gains, u_size, 1.0, "velocityGains")?;
3539            unsafe {
3540                ffi::b3JointControlSetKdMultiDof(command, u_index, kds.as_ptr(), u_size as i32);
3541            }
3542
3543            let forces = Self::slice_or_default(forces, u_size, info.max_force, "forces")?;
3544            for (dof, &force) in forces.iter().enumerate() {
3545                unsafe {
3546                    ffi::b3JointControlSetMaximumForce(command, u_index + dof as i32, force);
3547                }
3548            }
3549        } else if control.is_torque_based() {
3550            if u_index < 0 || u_size == 0 {
3551                return Err(BulletError::CommandFailed {
3552                    message: "Joint has no velocity DOF for torque control",
3553                    code: u_index,
3554                });
3555            }
3556
3557            let forces = match control {
3558                MultiDofControl::Torque { forces, .. } => *forces,
3559                _ => None,
3560            };
3561            let forces = Self::slice_or_default(forces, u_size, 0.0, "forces")?;
3562            unsafe {
3563                ffi::b3JointControlSetDesiredForceTorqueMultiDof(
3564                    command,
3565                    u_index,
3566                    forces.as_ptr(),
3567                    u_size as i32,
3568                );
3569            }
3570        }
3571
3572        Ok(())
3573    }
3574
3575    fn ensure_slice_len(name: &'static str, actual: usize, expected: usize) -> BulletResult<()> {
3576        if expected != actual {
3577            return Err(BulletError::CommandFailed {
3578                message: match name {
3579                    "targetPositions" => "targetPositions length mismatch",
3580                    "targetVelocities" => "targetVelocities length mismatch",
3581                    "positionGains" => "positionGains length mismatch",
3582                    "velocityGains" => "velocityGains length mismatch",
3583                    "forces" => "forces length mismatch",
3584                    "damping" => "damping length mismatch",
3585                    other => {
3586                        debug_assert!(false, "Unexpected slice name: {other}");
3587                        "slice length mismatch"
3588                    }
3589                },
3590                code: actual as i32,
3591            });
3592        }
3593        Ok(())
3594    }
3595
3596    fn slice_or_default<'a>(
3597        data: Option<&'a [f64]>,
3598        expected_len: usize,
3599        default_value: f64,
3600        name: &'static str,
3601    ) -> BulletResult<Cow<'a, [f64]>> {
3602        if expected_len == 0 {
3603            return Ok(Cow::Owned(Vec::new()));
3604        }
3605
3606        if let Some(slice) = data {
3607            Self::ensure_slice_len(name, slice.len(), expected_len)?;
3608            Ok(Cow::Borrowed(slice))
3609        } else {
3610            Ok(Cow::Owned(vec![default_value; expected_len]))
3611        }
3612    }
3613
3614    fn collect_contact_points(info: &ffi::b3ContactInformation) -> Vec<ContactPoint> {
3615        if info.m_contactPointData.is_null() || info.m_numContactPoints <= 0 {
3616            return Vec::new();
3617        }
3618        let count = info.m_numContactPoints as usize;
3619        let raw = unsafe { slice::from_raw_parts(info.m_contactPointData, count) };
3620        raw.iter()
3621            .map(|cp| ContactPoint {
3622                contact_flags: cp.m_contactFlags,
3623                body_a: cp.m_bodyUniqueIdA,
3624                body_b: cp.m_bodyUniqueIdB,
3625                link_a: cp.m_linkIndexA,
3626                link_b: cp.m_linkIndexB,
3627                position_on_a: cp.m_positionOnAInWS,
3628                position_on_b: cp.m_positionOnBInWS,
3629                contact_normal_on_b: cp.m_contactNormalOnBInWS,
3630                contact_distance: cp.m_contactDistance,
3631                normal_force: cp.m_normalForce,
3632                linear_friction_force_1: cp.m_linearFrictionForce1,
3633                linear_friction_direction_1: cp.m_linearFrictionDirection1,
3634                linear_friction_force_2: cp.m_linearFrictionForce2,
3635                linear_friction_direction_2: cp.m_linearFrictionDirection2,
3636            })
3637            .collect()
3638    }
3639
3640    fn collect_overlapping_objects(data: &ffi::b3AABBOverlapData) -> Vec<OverlappingObject> {
3641        if data.m_overlappingObjects.is_null() || data.m_numOverlappingObjects <= 0 {
3642            return Vec::new();
3643        }
3644        let count = data.m_numOverlappingObjects as usize;
3645        let raw = unsafe { slice::from_raw_parts(data.m_overlappingObjects, count) };
3646        raw.iter()
3647            .map(|obj| OverlappingObject {
3648                object_unique_id: obj.m_objectUniqueId,
3649                link_index: obj.m_linkIndex,
3650            })
3651            .collect()
3652    }
3653
3654    fn collect_ray_hits(info: &ffi::b3RaycastInformation) -> Vec<RayHit> {
3655        if info.m_rayHits.is_null() || info.m_numRayHits <= 0 {
3656            return Vec::new();
3657        }
3658        let count = info.m_numRayHits as usize;
3659        let raw = unsafe { slice::from_raw_parts(info.m_rayHits, count) };
3660        raw.iter()
3661            .map(|hit| RayHit {
3662                object_unique_id: hit.m_hitObjectUniqueId,
3663                link_index: hit.m_hitObjectLinkIndex,
3664                hit_fraction: hit.m_hitFraction,
3665                hit_position_world: hit.m_hitPositionWorld,
3666                hit_normal_world: hit.m_hitNormalWorld,
3667            })
3668            .collect()
3669    }
3670
3671    fn decode_camera_image(raw: &ffi::b3CameraImageData) -> BulletResult<CameraImage> {
3672        let width = raw.m_pixel_width;
3673        let height = raw.m_pixel_height;
3674        if width <= 0 || height <= 0 {
3675            return Err(BulletError::CommandFailed {
3676                message: "Camera returned an empty image",
3677                code: -1,
3678            });
3679        }
3680
3681        let pixel_count = (width as usize) * (height as usize);
3682        let mut rgba = Vec::with_capacity(pixel_count * 4);
3683        if !raw.m_rgb_color_data.is_null() && pixel_count > 0 {
3684            let data = unsafe { slice::from_raw_parts(raw.m_rgb_color_data, pixel_count * 4) };
3685            rgba.extend_from_slice(data);
3686        } else {
3687            rgba.resize(pixel_count * 4, 0);
3688        }
3689
3690        let mut depth = Vec::with_capacity(pixel_count);
3691        if !raw.m_depth_values.is_null() && pixel_count > 0 {
3692            let data = unsafe { slice::from_raw_parts(raw.m_depth_values, pixel_count) };
3693            depth.extend_from_slice(data);
3694        } else {
3695            depth.resize(pixel_count, 0.0);
3696        }
3697
3698        let mut segmentation_mask = Vec::with_capacity(pixel_count);
3699        if !raw.m_segmentation_mask_values.is_null() && pixel_count > 0 {
3700            let data =
3701                unsafe { slice::from_raw_parts(raw.m_segmentation_mask_values, pixel_count) };
3702            segmentation_mask.extend(data.iter().copied());
3703        } else {
3704            segmentation_mask.resize(pixel_count, -1);
3705        }
3706
3707        Ok(CameraImage { width, height, rgba, depth, segmentation_mask })
3708    }
3709
3710    fn collect_vr_events(
3711        data: &ffi::b3VREventsData,
3712        include_aux_analog_axes: bool,
3713    ) -> Vec<VrControllerEvent> {
3714        if data.m_controllerEvents.is_null() || data.m_numControllerEvents <= 0 {
3715            return Vec::new();
3716        }
3717        let count = data.m_numControllerEvents as usize;
3718        let raw = unsafe { slice::from_raw_parts(data.m_controllerEvents, count) };
3719        raw.iter()
3720            .map(|event| {
3721                let mut position = [0.0_f32; 3];
3722                position.copy_from_slice(&event.m_pos[..3]);
3723                let orientation = event.m_orn;
3724                let mut aux = [0.0_f32; VR_MAX_ANALOG_AXIS * 2];
3725                if include_aux_analog_axes {
3726                    aux.copy_from_slice(&event.m_auxAnalogAxis);
3727                }
3728                let mut buttons = [0_i32; VR_MAX_BUTTONS];
3729                buttons.copy_from_slice(&event.m_buttons);
3730                VrControllerEvent {
3731                    controller_id: event.m_controllerId,
3732                    device_type: event.m_deviceType,
3733                    num_move_events: event.m_numMoveEvents,
3734                    num_button_events: event.m_numButtonEvents,
3735                    position,
3736                    orientation,
3737                    analog_axis: event.m_analogAxis,
3738                    aux_analog_axis: aux,
3739                    buttons,
3740                }
3741            })
3742            .collect()
3743    }
3744
3745    fn collect_keyboard_events(data: &ffi::b3KeyboardEventsData) -> Vec<KeyboardEvent> {
3746        if data.m_keyboardEvents.is_null() || data.m_numKeyboardEvents <= 0 {
3747            return Vec::new();
3748        }
3749        let count = data.m_numKeyboardEvents as usize;
3750        let raw = unsafe { slice::from_raw_parts(data.m_keyboardEvents, count) };
3751        raw.iter().copied().map(Into::into).collect()
3752    }
3753
3754    fn collect_mouse_events(data: &ffi::b3MouseEventsData) -> Vec<MouseEvent> {
3755        if data.m_mouseEvents.is_null() || data.m_numMouseEvents <= 0 {
3756            return Vec::new();
3757        }
3758        let count = data.m_numMouseEvents as usize;
3759        let raw = unsafe { slice::from_raw_parts(data.m_mouseEvents, count) };
3760        raw.iter().copied().map(Into::into).collect()
3761    }
3762
3763    fn collect_visual_shapes(info: &ffi::b3VisualShapeInformation) -> Vec<VisualShapeData> {
3764        if info.m_visualShapeData.is_null() || info.m_numVisualShapes <= 0 {
3765            return Vec::new();
3766        }
3767        let count = info.m_numVisualShapes as usize;
3768        let raw = unsafe { slice::from_raw_parts(info.m_visualShapeData, count) };
3769        raw.iter()
3770            .map(|shape| {
3771                let mut frame_position = [0.0_f64; 3];
3772                frame_position.copy_from_slice(&shape.m_localVisualFrame[..3]);
3773                let mut frame_orientation = [0.0_f64; 4];
3774                frame_orientation.copy_from_slice(&shape.m_localVisualFrame[3..7]);
3775                VisualShapeData {
3776                    object_unique_id: shape.m_objectUniqueId,
3777                    link_index: shape.m_linkIndex,
3778                    geometry_type: shape.m_visualGeometryType,
3779                    dimensions: shape.m_dimensions,
3780                    mesh_asset_file_name: Self::read_c_string(&shape.m_meshAssetFileName),
3781                    local_visual_frame_position: frame_position,
3782                    local_visual_frame_orientation: frame_orientation,
3783                    rgba_color: shape.m_rgbaColor,
3784                    tiny_renderer_texture_id: shape.m_tinyRendererTextureId,
3785                    texture_unique_id: shape.m_textureUniqueId,
3786                    opengl_texture_id: shape.m_openglTextureId,
3787                }
3788            })
3789            .collect()
3790    }
3791
3792    fn collect_collision_shapes(
3793        info: &ffi::b3CollisionShapeInformation,
3794    ) -> Vec<CollisionShapeData> {
3795        if info.m_collisionShapeData.is_null() || info.m_numCollisionShapes <= 0 {
3796            return Vec::new();
3797        }
3798        let count = info.m_numCollisionShapes as usize;
3799        let raw = unsafe { slice::from_raw_parts(info.m_collisionShapeData, count) };
3800        raw.iter()
3801            .map(|shape| {
3802                let mut frame_position = [0.0_f64; 3];
3803                frame_position.copy_from_slice(&shape.m_localCollisionFrame[..3]);
3804                let mut frame_orientation = [0.0_f64; 4];
3805                frame_orientation.copy_from_slice(&shape.m_localCollisionFrame[3..7]);
3806                CollisionShapeData {
3807                    object_unique_id: shape.m_objectUniqueId,
3808                    link_index: shape.m_linkIndex,
3809                    geometry_type: shape.m_collisionGeometryType,
3810                    dimensions: shape.m_dimensions,
3811                    mesh_asset_file_name: Self::read_c_string(&shape.m_meshAssetFileName),
3812                    local_collision_frame_position: frame_position,
3813                    local_collision_frame_orientation: frame_orientation,
3814                }
3815            })
3816            .collect()
3817    }
3818
3819    fn usize_to_i32(value: usize) -> BulletResult<i32> {
3820        value.try_into().map_err(|_| BulletError::CommandFailed {
3821            message: "Index exceeds Bullet i32 range",
3822            code: -1,
3823        })
3824    }
3825
3826    fn path_to_cstring(path: &Path) -> BulletResult<CString> {
3827        let utf8 = path
3828            .to_str()
3829            .ok_or(BulletError::CommandFailed { message: "Path must be valid UTF-8", code: -1 })?;
3830        Ok(CString::new(utf8)?)
3831    }
3832}
3833
3834/// ! =====================================================================================================================================
3835/// ### Rendering, Debug & Visualization
3836///
3837/// | API | Status | Notes |
3838/// | --- | --- | --- |
3839/// | renderImage | Pending | Obsolete; low priority |
3840/// | getCameraImage | Implemented | High effort (buffers) |
3841/// | isNumpyEnabled | Pending | Simple flag query |
3842/// | computeViewMatrix | Implemented | Math helper |
3843/// | computeViewMatrixFromYawPitchRoll | Implemented | Math helper |
3844/// | computeProjectionMatrix | Implemented | Math helper |
3845/// | computeProjectionMatrixFOV | Implemented | Math helper |
3846/// | addUserDebugLine | Implemented | Debug draw |
3847/// | addUserDebugPoints | Implemented | Debug draw |
3848/// | addUserDebugText | Implemented | Debug draw |
3849/// | addUserDebugParameter | Implemented | GUI slider/button |
3850/// | readUserDebugParameter | Implemented | GUI feedback |
3851/// | removeUserDebugItem | Implemented | Debug cleanup |
3852/// | removeAllUserDebugItems | Implemented | Debug cleanup |
3853/// | removeAllUserParameters | Implemented | Debug cleanup |
3854/// | setDebugObjectColor | Implemented | Debug override |
3855/// | getDebugVisualizerCamera | Implemented | Visualizer query |
3856/// | configureDebugVisualizer | Implemented | Visualizer toggle |
3857/// | resetDebugVisualizerCamera | Implemented | Visualizer camera |
3858/// | getVisualShapeData | Implemented | Visual query |
3859/// | getCollisionShapeData | Implemented | Collision query |
3860impl PhysicsClient {
3861    pub fn get_camera_image(&mut self, request: &CameraImageOptions) -> BulletResult<CameraImage> {
3862        if request.width <= 0 || request.height <= 0 {
3863            return Err(BulletError::CommandFailed {
3864                message: "Camera image width/height must be positive",
3865                code: -1,
3866            });
3867        }
3868
3869        self.ensure_can_submit()?;
3870        let command = unsafe { ffi::b3InitRequestCameraImage(self.handle) };
3871        unsafe {
3872            ffi::b3RequestCameraImageSetPixelResolution(command, request.width, request.height);
3873        }
3874
3875        if let (Some(mut view), Some(mut projection)) =
3876            (request.view_matrix, request.projection_matrix)
3877        {
3878            unsafe {
3879                ffi::b3RequestCameraImageSetCameraMatrices(
3880                    command,
3881                    view.as_mut_ptr(),
3882                    projection.as_mut_ptr(),
3883                );
3884            }
3885        }
3886        if let Some(mut direction) = request.light_direction {
3887            unsafe { ffi::b3RequestCameraImageSetLightDirection(command, direction.as_mut_ptr()) };
3888        }
3889        if let Some(mut color) = request.light_color {
3890            unsafe { ffi::b3RequestCameraImageSetLightColor(command, color.as_mut_ptr()) };
3891        }
3892        if let Some(distance) = request.light_distance
3893            && distance >= 0.0
3894        {
3895            unsafe { ffi::b3RequestCameraImageSetLightDistance(command, distance) };
3896        }
3897        if let Some(has_shadow) = request.shadow {
3898            unsafe { ffi::b3RequestCameraImageSetShadow(command, has_shadow as i32) };
3899        }
3900        if let Some(coeff) = request.light_ambient_coeff
3901            && coeff >= 0.0
3902        {
3903            unsafe { ffi::b3RequestCameraImageSetLightAmbientCoeff(command, coeff) };
3904        }
3905        if let Some(coeff) = request.light_diffuse_coeff
3906            && coeff >= 0.0
3907        {
3908            unsafe { ffi::b3RequestCameraImageSetLightDiffuseCoeff(command, coeff) };
3909        }
3910        if let Some(coeff) = request.light_specular_coeff
3911            && coeff >= 0.0
3912        {
3913            unsafe { ffi::b3RequestCameraImageSetLightSpecularCoeff(command, coeff) };
3914        }
3915        if let Some(renderer) = request.renderer {
3916            unsafe { ffi::b3RequestCameraImageSelectRenderer(command, renderer as i32) };
3917        }
3918        if let Some(flags) = request.flags {
3919            unsafe { ffi::b3RequestCameraImageSetFlags(command, flags) };
3920        }
3921        if let (Some(mut view), Some(mut projection)) = (
3922            request.projective_texture_view,
3923            request.projective_texture_projection,
3924        ) {
3925            unsafe {
3926                ffi::b3RequestCameraImageSetProjectiveTextureMatrices(
3927                    command,
3928                    view.as_mut_ptr(),
3929                    projection.as_mut_ptr(),
3930                );
3931            }
3932        }
3933
3934        let _status = self.submit_simple_command(
3935            command,
3936            ffi::EnumSharedMemoryServerStatus::CMD_CAMERA_IMAGE_COMPLETED,
3937        )?;
3938
3939        let mut raw = ffi::b3CameraImageData::default();
3940        unsafe { ffi::b3GetCameraImageData(self.handle, &raw mut raw) };
3941        Self::decode_camera_image(&raw)
3942    }
3943
3944    pub fn compute_view_matrix(
3945        camera_eye_position: [f32; 3],
3946        camera_target_position: [f32; 3],
3947        camera_up_vector: [f32; 3],
3948    ) -> [f32; 16] {
3949        let mut matrix = [0.0_f32; 16];
3950        unsafe {
3951            ffi::b3ComputeViewMatrixFromPositions(
3952                camera_eye_position.as_ptr(),
3953                camera_target_position.as_ptr(),
3954                camera_up_vector.as_ptr(),
3955                matrix.as_mut_ptr(),
3956            );
3957        }
3958        matrix
3959    }
3960
3961    pub fn compute_view_matrix_from_yaw_pitch_roll(
3962        camera_target_position: [f32; 3],
3963        distance: f32,
3964        yaw: f32,
3965        pitch: f32,
3966        roll: f32,
3967        up_axis: i32,
3968    ) -> [f32; 16] {
3969        let mut matrix = [0.0_f32; 16];
3970        unsafe {
3971            ffi::b3ComputeViewMatrixFromYawPitchRoll(
3972                camera_target_position.as_ptr(),
3973                distance,
3974                yaw,
3975                pitch,
3976                roll,
3977                up_axis,
3978                matrix.as_mut_ptr(),
3979            );
3980        }
3981        matrix
3982    }
3983
3984    pub fn compute_projection_matrix(
3985        left: f32,
3986        right: f32,
3987        bottom: f32,
3988        top: f32,
3989        near_val: f32,
3990        far_val: f32,
3991    ) -> [f32; 16] {
3992        let mut matrix = [0.0_f32; 16];
3993        unsafe {
3994            ffi::b3ComputeProjectionMatrix(
3995                left,
3996                right,
3997                bottom,
3998                top,
3999                near_val,
4000                far_val,
4001                matrix.as_mut_ptr(),
4002            );
4003        }
4004        matrix
4005    }
4006
4007    pub fn compute_projection_matrix_fov(
4008        fov: f32,
4009        aspect: f32,
4010        near_val: f32,
4011        far_val: f32,
4012    ) -> [f32; 16] {
4013        let mut matrix = [0.0_f32; 16];
4014        unsafe {
4015            ffi::b3ComputeProjectionMatrixFOV(fov, aspect, near_val, far_val, matrix.as_mut_ptr());
4016        }
4017        matrix
4018    }
4019
4020    pub fn add_user_debug_line(&mut self, options: &DebugLineOptions) -> BulletResult<i32> {
4021        self.ensure_can_submit()?;
4022        let color = options.color.unwrap_or([1.0; 3]);
4023        let command = unsafe {
4024            ffi::b3InitUserDebugDrawAddLine3D(
4025                self.handle,
4026                options.from.as_ptr(),
4027                options.to.as_ptr(),
4028                color.as_ptr(),
4029                options.line_width,
4030                options.life_time,
4031            )
4032        };
4033
4034        if let Some(parent_id) = options.parent_object_unique_id {
4035            let link_index = options.parent_link_index.unwrap_or(-1);
4036            unsafe { ffi::b3UserDebugItemSetParentObject(command, parent_id, link_index) };
4037        }
4038        if let Some(replace_id) = options.replace_item_unique_id {
4039            unsafe { ffi::b3UserDebugItemSetReplaceItemUniqueId(command, replace_id) };
4040        }
4041
4042        let status = self.submit_simple_command(
4043            command,
4044            ffi::EnumSharedMemoryServerStatus::CMD_USER_DEBUG_DRAW_COMPLETED,
4045        )?;
4046        let unique_id = unsafe { ffi::b3GetDebugItemUniqueId(status.handle) };
4047        Ok(unique_id)
4048    }
4049
4050    pub fn add_user_debug_points(&mut self, options: &DebugPointsOptions<'_>) -> BulletResult<i32> {
4051        if options.positions.len() != options.colors.len() {
4052            return Err(BulletError::CommandFailed {
4053                message: "pointColors length must match pointPositions",
4054                code: options.colors.len() as i32,
4055            });
4056        }
4057        if options.positions.is_empty() {
4058            return Err(BulletError::CommandFailed {
4059                message: "At least one point is required",
4060                code: 0,
4061            });
4062        }
4063
4064        self.ensure_can_submit()?;
4065        let positions = Self::flatten_points(options.positions);
4066        let colors = Self::flatten_points(options.colors);
4067        let command = unsafe {
4068            ffi::b3InitUserDebugDrawAddPoints3D(
4069                self.handle,
4070                positions.as_ptr(),
4071                colors.as_ptr(),
4072                options.point_size,
4073                options.life_time,
4074                options.positions.len() as i32,
4075            )
4076        };
4077
4078        if let Some(parent_id) = options.parent_object_unique_id {
4079            let link_index = options.parent_link_index.unwrap_or(-1);
4080            unsafe { ffi::b3UserDebugItemSetParentObject(command, parent_id, link_index) };
4081        }
4082        if let Some(replace_id) = options.replace_item_unique_id {
4083            unsafe { ffi::b3UserDebugItemSetReplaceItemUniqueId(command, replace_id) };
4084        }
4085
4086        let status = self.submit_simple_command(
4087            command,
4088            ffi::EnumSharedMemoryServerStatus::CMD_USER_DEBUG_DRAW_COMPLETED,
4089        )?;
4090        let unique_id = unsafe { ffi::b3GetDebugItemUniqueId(status.handle) };
4091        Ok(unique_id)
4092    }
4093
4094    pub fn add_user_debug_text(&mut self, options: &DebugTextOptions<'_>) -> BulletResult<i32> {
4095        self.ensure_can_submit()?;
4096        let color = options.color.unwrap_or([1.0; 3]);
4097        let text = CString::new(options.text)?;
4098        let command = unsafe {
4099            ffi::b3InitUserDebugDrawAddText3D(
4100                self.handle,
4101                text.as_ptr(),
4102                options.position.as_ptr(),
4103                color.as_ptr(),
4104                options.size,
4105                options.life_time,
4106            )
4107        };
4108
4109        if let Some(parent_id) = options.parent_object_unique_id {
4110            let link_index = options.parent_link_index.unwrap_or(-1);
4111            unsafe { ffi::b3UserDebugItemSetParentObject(command, parent_id, link_index) };
4112        }
4113        if let Some(orientation) = options.orientation {
4114            const USER_DEBUG_HAS_TEXT_ORIENTATION: i32 = 512;
4115            unsafe {
4116                ffi::b3UserDebugTextSetOptionFlags(command, USER_DEBUG_HAS_TEXT_ORIENTATION);
4117                ffi::b3UserDebugTextSetOrientation(command, orientation.as_ptr());
4118            }
4119        }
4120        if let Some(replace_id) = options.replace_item_unique_id {
4121            unsafe { ffi::b3UserDebugItemSetReplaceItemUniqueId(command, replace_id) };
4122        }
4123
4124        let status = self.submit_simple_command(
4125            command,
4126            ffi::EnumSharedMemoryServerStatus::CMD_USER_DEBUG_DRAW_COMPLETED,
4127        )?;
4128        let unique_id = unsafe { ffi::b3GetDebugItemUniqueId(status.handle) };
4129        Ok(unique_id)
4130    }
4131
4132    pub fn add_user_debug_parameter(
4133        &mut self,
4134        options: &DebugParameterOptions<'_>,
4135    ) -> BulletResult<i32> {
4136        self.ensure_can_submit()?;
4137        let name = CString::new(options.name)?;
4138        let command = unsafe {
4139            ffi::b3InitUserDebugAddParameter(
4140                self.handle,
4141                name.as_ptr(),
4142                options.range_min,
4143                options.range_max,
4144                options.start_value,
4145            )
4146        };
4147
4148        let status = self.submit_simple_command(
4149            command,
4150            ffi::EnumSharedMemoryServerStatus::CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED,
4151        )?;
4152        let unique_id = unsafe { ffi::b3GetDebugItemUniqueId(status.handle) };
4153        Ok(unique_id)
4154    }
4155
4156    pub fn read_user_debug_parameter(&mut self, parameter_id: i32) -> BulletResult<f64> {
4157        self.ensure_can_submit()?;
4158        let command = unsafe { ffi::b3InitUserDebugReadParameter(self.handle, parameter_id) };
4159        let status = self.submit_simple_command(
4160            command,
4161            ffi::EnumSharedMemoryServerStatus::CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED,
4162        )?;
4163
4164        let mut value = 0.0_f64;
4165        let success = unsafe { ffi::b3GetStatusDebugParameterValue(status.handle, &raw mut value) };
4166        if success == 0 {
4167            return Err(BulletError::CommandFailed {
4168                message: "Unable to read user debug parameter",
4169                code: success,
4170            });
4171        }
4172        Ok(value)
4173    }
4174
4175    pub fn remove_user_debug_item(&mut self, item_unique_id: i32) -> BulletResult<()> {
4176        self.ensure_can_submit()?;
4177        let command = unsafe { ffi::b3InitUserDebugDrawRemove(self.handle, item_unique_id) };
4178        let _ = self.submit_command(command)?;
4179        Ok(())
4180    }
4181
4182    pub fn remove_all_user_debug_items(&mut self) -> BulletResult<()> {
4183        self.ensure_can_submit()?;
4184        let command = unsafe { ffi::b3InitUserDebugDrawRemoveAll(self.handle) };
4185        let _ = self.submit_command(command)?;
4186        Ok(())
4187    }
4188
4189    pub fn remove_all_user_parameters(&mut self) -> BulletResult<()> {
4190        self.ensure_can_submit()?;
4191        let command = unsafe { ffi::b3InitUserRemoveAllParameters(self.handle) };
4192        let _ = self.submit_command(command)?;
4193        Ok(())
4194    }
4195
4196    pub fn set_debug_object_color(
4197        &mut self,
4198        object_unique_id: i32,
4199        link_index: i32,
4200        color: Option<[f64; 3]>,
4201    ) -> BulletResult<&mut Self> {
4202        self.ensure_can_submit()?;
4203        let command = unsafe { ffi::b3InitDebugDrawingCommand(self.handle) };
4204        unsafe {
4205            if let Some(color) = color {
4206                ffi::b3SetDebugObjectColor(command, object_unique_id, link_index, color.as_ptr());
4207            } else {
4208                ffi::b3RemoveDebugObjectColor(command, object_unique_id, link_index);
4209            }
4210        }
4211        let _ = self.submit_command(command)?;
4212        Ok(self)
4213    }
4214
4215    pub fn get_debug_visualizer_camera(&mut self) -> BulletResult<DebugVisualizerCamera> {
4216        self.ensure_can_submit()?;
4217        let command = unsafe { ffi::b3InitRequestOpenGLVisualizerCameraCommand(self.handle) };
4218        let status = self.submit_command(command)?;
4219
4220        let mut camera = ffi::b3OpenGLVisualizerCameraInfo::default();
4221        let success =
4222            unsafe { ffi::b3GetStatusOpenGLVisualizerCamera(status.handle, &raw mut camera) };
4223        if success == 0 {
4224            return Err(BulletError::CommandFailed {
4225                message: "Unable to fetch debug visualizer camera",
4226                code: success,
4227            });
4228        }
4229
4230        Ok(DebugVisualizerCamera {
4231            width: camera.m_width,
4232            height: camera.m_height,
4233            view_matrix: camera.m_viewMatrix,
4234            projection_matrix: camera.m_projectionMatrix,
4235            camera_up: camera.m_camUp,
4236            camera_forward: camera.m_camForward,
4237            horizontal: camera.m_horizontal,
4238            vertical: camera.m_vertical,
4239            yaw: camera.m_yaw,
4240            pitch: camera.m_pitch,
4241            distance: camera.m_dist,
4242            target: camera.m_target,
4243        })
4244    }
4245
4246    pub fn configure_debug_visualizer(
4247        &mut self,
4248        options: &DebugVisualizerOptions,
4249    ) -> BulletResult<&mut Self> {
4250        self.ensure_can_submit()?;
4251        let command = unsafe { ffi::b3InitConfigureOpenGLVisualizer(self.handle) };
4252
4253        match options {
4254            DebugVisualizerOptions::Flag(flag, enabled) => unsafe {
4255                ffi::b3ConfigureOpenGLVisualizerSetVisualizationFlags(
4256                    command,
4257                    *flag as i32,
4258                    *enabled as i32,
4259                );
4260            },
4261            DebugVisualizerOptions::LightPosition(position) => unsafe {
4262                let mut pos_copy = *position;
4263                ffi::b3ConfigureOpenGLVisualizerSetLightPosition(command, pos_copy.as_mut_ptr());
4264            },
4265            DebugVisualizerOptions::ShadowMapResolution(resolution) => unsafe {
4266                ffi::b3ConfigureOpenGLVisualizerSetShadowMapResolution(command, *resolution);
4267            },
4268            DebugVisualizerOptions::ShadowMapWorldSize(world_size) => unsafe {
4269                ffi::b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(command, *world_size);
4270            },
4271            DebugVisualizerOptions::RemoteSyncTransformInterval(interval) => unsafe {
4272                ffi::b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(command, *interval);
4273            },
4274            DebugVisualizerOptions::ShadowMapIntensity(intensity) => unsafe {
4275                ffi::b3ConfigureOpenGLVisualizerSetShadowMapIntensity(command, *intensity);
4276            },
4277            DebugVisualizerOptions::RgbBackground(rgb) => unsafe {
4278                let mut rgb_copy = *rgb;
4279                ffi::b3ConfigureOpenGLVisualizerSetLightRgbBackground(
4280                    command,
4281                    rgb_copy.as_mut_ptr(),
4282                );
4283            },
4284        }
4285
4286        let _ = self.submit_command(command)?;
4287        Ok(self)
4288    }
4289
4290    pub fn reset_debug_visualizer_camera(
4291        &mut self,
4292        options: &ResetDebugVisualizerCameraOptions,
4293    ) -> BulletResult<&mut Self> {
4294        if options.distance < 0.0 {
4295            return Err(BulletError::CommandFailed {
4296                message: "cameraDistance must be non-negative",
4297                code: -1,
4298            });
4299        }
4300
4301        self.ensure_can_submit()?;
4302        let command = unsafe { ffi::b3InitConfigureOpenGLVisualizer(self.handle) };
4303        unsafe {
4304            ffi::b3ConfigureOpenGLVisualizerSetViewMatrix(
4305                command,
4306                options.distance,
4307                options.pitch,
4308                options.yaw,
4309                options.target.as_ptr(),
4310            );
4311        }
4312        let _ = self.submit_command(command)?;
4313        Ok(self)
4314    }
4315
4316    pub fn get_visual_shape_data(
4317        &mut self,
4318        object_unique_id: i32,
4319    ) -> BulletResult<Vec<VisualShapeData>> {
4320        self.ensure_can_submit()?;
4321        let command =
4322            unsafe { ffi::b3InitRequestVisualShapeInformation(self.handle, object_unique_id) };
4323        let _status = self.submit_simple_command(
4324            command,
4325            ffi::EnumSharedMemoryServerStatus::CMD_VISUAL_SHAPE_INFO_COMPLETED,
4326        )?;
4327        let mut info = ffi::b3VisualShapeInformation::default();
4328        unsafe { ffi::b3GetVisualShapeInformation(self.handle, &raw mut info) };
4329        Ok(Self::collect_visual_shapes(&info))
4330    }
4331
4332    pub fn get_collision_shape_data(
4333        &mut self,
4334        object_unique_id: i32,
4335        link_index: Option<i32>,
4336    ) -> BulletResult<Vec<CollisionShapeData>> {
4337        self.ensure_can_submit()?;
4338        let link_index = link_index.unwrap_or(-1);
4339        let command = unsafe {
4340            ffi::b3InitRequestCollisionShapeInformation(self.handle, object_unique_id, link_index)
4341        };
4342        let _status = self.submit_simple_command(
4343            command,
4344            ffi::EnumSharedMemoryServerStatus::CMD_COLLISION_SHAPE_INFO_COMPLETED,
4345        )?;
4346        let mut info = ffi::b3CollisionShapeInformation::default();
4347        unsafe { ffi::b3GetCollisionShapeInformation(self.handle, &raw mut info) };
4348        Ok(Self::collect_collision_shapes(&info))
4349    }
4350}
4351
4352/// ! =====================================================================================================================================
4353/// ### Math & Transform Utilities
4354///
4355/// | API | Status | Notes |
4356/// | --- | --- | --- |
4357/// | getQuaternionFromEuler | Pending | Utility |
4358/// | getEulerFromQuaternion | Pending | Utility |
4359/// | multiplyTransforms | Pending | Utility |
4360/// | invertTransform | Pending | Utility |
4361/// | getMatrixFromQuaternion | Pending | Utility |
4362/// | getQuaternionSlerp | Pending | Utility |
4363/// | getQuaternionFromAxisAngle | Pending | Utility |
4364/// | getAxisAngleFromQuaternion | Pending | Utility |
4365/// | getDifferenceQuaternion | Pending | Utility |
4366/// | getAxisDifferenceQuaternion | Pending | Utility |
4367/// | calculateVelocityQuaternion | Pending | Utility |
4368/// | rotateVector | Pending | Utility |
4369impl PhysicsClient {
4370    fn _math_transform_utilities() {}
4371}
4372
4373/// ! =====================================================================================================================================
4374/// ### VR, Input, Logging, Plugins & Misc
4375///
4376/// | API | Status | Notes |
4377/// | --- | --- | --- |
4378/// | getVREvents | Implemented | VR input |
4379/// | setVRCameraState | Implemented | VR camera |
4380/// | getKeyboardEvents | Implemented | Input polling |
4381/// | getMouseEvents | Implemented | Input polling |
4382/// | startStateLogging | Implemented | Logging |
4383/// | stopStateLogging | Implemented | Logging |
4384/// | loadPlugin | Implemented | Plugin system |
4385/// | unloadPlugin | Implemented | Plugin system |
4386/// | executePluginCommand | Implemented | Plugin system |
4387/// | submitProfileTiming | Implemented | Profiling |
4388/// | setTimeOut | Implemented | Client timeout |
4389/// | getAPIVersion | Implemented | Static constant |
4390impl PhysicsClient {
4391    pub fn get_vr_events(
4392        &mut self,
4393        options: &VrEventsOptions,
4394    ) -> BulletResult<Vec<VrControllerEvent>> {
4395        self.ensure_can_submit()?;
4396        let command = unsafe { ffi::b3RequestVREventsCommandInit(self.handle) };
4397        let filter = options
4398            .device_type_filter
4399            .unwrap_or(ffi::VR_DEVICE_CONTROLLER);
4400        unsafe {
4401            ffi::b3VREventsSetDeviceTypeFilter(command, filter);
4402        }
4403
4404        let _status = self.submit_simple_command(
4405            command,
4406            ffi::EnumSharedMemoryServerStatus::CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
4407        )?;
4408
4409        let mut raw = ffi::b3VREventsData::default();
4410        unsafe { ffi::b3GetVREventsData(self.handle, &raw mut raw) };
4411        Ok(Self::collect_vr_events(
4412            &raw,
4413            options.include_aux_analog_axes,
4414        ))
4415    }
4416
4417    pub fn set_vr_camera_state(
4418        &mut self,
4419        options: &VrCameraStateOptions,
4420    ) -> BulletResult<&mut Self> {
4421        self.ensure_can_submit()?;
4422        let command = unsafe { ffi::b3SetVRCameraStateCommandInit(self.handle) };
4423
4424        if let Some(position) = options.root_position {
4425            unsafe { ffi::b3SetVRCameraRootPosition(command, position.as_ptr()) };
4426        }
4427        if let Some(orientation) = options.root_orientation {
4428            unsafe { ffi::b3SetVRCameraRootOrientation(command, orientation.as_ptr()) };
4429        }
4430        if let Some(object_id) = options.tracking_object_unique_id {
4431            unsafe { ffi::b3SetVRCameraTrackingObject(command, object_id) };
4432        }
4433        if let Some(flag) = options.tracking_flag {
4434            unsafe { ffi::b3SetVRCameraTrackingObjectFlag(command, flag) };
4435        }
4436
4437        self.submit_simple_command(
4438            command,
4439            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
4440        )?;
4441        Ok(self)
4442    }
4443
4444    pub fn get_keyboard_events(&mut self) -> BulletResult<Vec<KeyboardEvent>> {
4445        self.ensure_can_submit()?;
4446        let command = unsafe { ffi::b3RequestKeyboardEventsCommandInit(self.handle) };
4447        let _status = self.submit_simple_command(
4448            command,
4449            ffi::EnumSharedMemoryServerStatus::CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED,
4450        )?;
4451
4452        let mut raw = ffi::b3KeyboardEventsData::default();
4453        unsafe { ffi::b3GetKeyboardEventsData(self.handle, &raw mut raw) };
4454        Ok(Self::collect_keyboard_events(&raw))
4455    }
4456
4457    pub fn get_mouse_events(&mut self) -> BulletResult<Vec<MouseEvent>> {
4458        self.ensure_can_submit()?;
4459        let command = unsafe { ffi::b3RequestMouseEventsCommandInit(self.handle) };
4460        let _status = self.submit_simple_command(
4461            command,
4462            ffi::EnumSharedMemoryServerStatus::CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED,
4463        )?;
4464
4465        let mut raw = ffi::b3MouseEventsData::default();
4466        unsafe { ffi::b3GetMouseEventsData(self.handle, &raw mut raw) };
4467        Ok(Self::collect_mouse_events(&raw))
4468    }
4469
4470    pub fn start_state_logging(
4471        &mut self,
4472        logging_type: LoggingType,
4473        file_name: impl AsRef<Path>,
4474        options: Option<impl Into<StateLoggingOptions>>,
4475    ) -> BulletResult<i32> {
4476        self.ensure_can_submit()?;
4477        let file_c = Self::path_to_cstring(file_name.as_ref())?;
4478        let command = unsafe { ffi::b3StateLoggingCommandInit(self.handle) };
4479        unsafe { ffi::b3StateLoggingStart(command, logging_type as i32, file_c.as_ptr()) };
4480
4481        let options = options.map(Into::into).unwrap_or_default();
4482        if let Some(object_ids) = options.object_unique_ids {
4483            for object_id in object_ids {
4484                unsafe { ffi::b3StateLoggingAddLoggingObjectUniqueId(command, object_id) };
4485            }
4486        }
4487        if let Some(max_log_dof) = options.max_log_dof {
4488            unsafe { ffi::b3StateLoggingSetMaxLogDof(command, max_log_dof) };
4489        }
4490        if let Some(body_a) = options.body_unique_id_a {
4491            unsafe { ffi::b3StateLoggingSetBodyAUniqueId(command, body_a) };
4492        }
4493        if let Some(body_b) = options.body_unique_id_b {
4494            unsafe { ffi::b3StateLoggingSetBodyBUniqueId(command, body_b) };
4495        }
4496        if let Some(link_a) = options.link_index_a {
4497            unsafe { ffi::b3StateLoggingSetLinkIndexA(command, link_a) };
4498        }
4499        if let Some(link_b) = options.link_index_b {
4500            unsafe { ffi::b3StateLoggingSetLinkIndexB(command, link_b) };
4501        }
4502        if let Some(device_filter) = options.device_type_filter {
4503            unsafe { ffi::b3StateLoggingSetDeviceTypeFilter(command, device_filter) };
4504        }
4505        if let Some(flags) = options.log_flags {
4506            unsafe { ffi::b3StateLoggingSetLogFlags(command, flags.bits()) };
4507        }
4508
4509        let status = self.submit_simple_command(
4510            command,
4511            ffi::EnumSharedMemoryServerStatus::CMD_STATE_LOGGING_START_COMPLETED,
4512        )?;
4513
4514        let logging_id = unsafe { ffi::b3GetStatusLoggingUniqueId(status.handle) };
4515        if logging_id < 0 {
4516            return Err(BulletError::CommandFailed {
4517                message: "Bullet failed to start state logging",
4518                code: logging_id,
4519            });
4520        }
4521        Ok(logging_id)
4522    }
4523
4524    pub fn stop_state_logging(&mut self, logging_id: i32) -> BulletResult<&mut Self> {
4525        if logging_id < 0 {
4526            return Ok(self);
4527        }
4528        self.ensure_can_submit()?;
4529        let command = unsafe { ffi::b3StateLoggingCommandInit(self.handle) };
4530        unsafe { ffi::b3StateLoggingStop(command, logging_id) };
4531        self.submit_simple_command(
4532            command,
4533            ffi::EnumSharedMemoryServerStatus::CMD_STATE_LOGGING_COMPLETED,
4534        )?;
4535        Ok(self)
4536    }
4537
4538    pub fn load_plugin(&mut self, plugin_path: &str, postfix: Option<&str>) -> BulletResult<i32> {
4539        self.ensure_can_submit()?;
4540        let path_c = CString::new(plugin_path)?;
4541        let postfix_c = postfix.map(CString::new).transpose()?;
4542        let command = unsafe { ffi::b3CreateCustomCommand(self.handle) };
4543        unsafe { ffi::b3CustomCommandLoadPlugin(command, path_c.as_ptr()) };
4544        if let Some(ref postfix_c) = postfix_c {
4545            unsafe { ffi::b3CustomCommandLoadPluginSetPostFix(command, postfix_c.as_ptr()) };
4546        }
4547
4548        let status = self.submit_command(command)?;
4549        let plugin_id = unsafe { ffi::b3GetStatusPluginUniqueId(status.handle) };
4550        if plugin_id < 0 {
4551            return Err(BulletError::CommandFailed {
4552                message: "Bullet failed to load plugin",
4553                code: plugin_id,
4554            });
4555        }
4556        Ok(plugin_id)
4557    }
4558
4559    pub fn unload_plugin(&mut self, plugin_unique_id: i32) -> BulletResult<()> {
4560        self.ensure_can_submit()?;
4561        let command = unsafe { ffi::b3CreateCustomCommand(self.handle) };
4562        unsafe { ffi::b3CustomCommandUnloadPlugin(command, plugin_unique_id) };
4563        self.submit_command(command)?;
4564        Ok(())
4565    }
4566
4567    pub fn execute_plugin_command(
4568        &mut self,
4569        options: &PluginCommandOptions<'_>,
4570    ) -> BulletResult<PluginCommandResult> {
4571        self.ensure_can_submit()?;
4572        let command = unsafe { ffi::b3CreateCustomCommand(self.handle) };
4573        let text_c = options.text_argument.map(CString::new).transpose()?;
4574        let text_ptr = text_c.as_ref().map_or(ptr::null(), |value| value.as_ptr());
4575        unsafe {
4576            ffi::b3CustomCommandExecutePluginCommand(command, options.plugin_unique_id, text_ptr);
4577        }
4578
4579        if let Some(int_args) = options.int_args {
4580            for &value in int_args {
4581                unsafe { ffi::b3CustomCommandExecuteAddIntArgument(command, value) };
4582            }
4583        }
4584        if let Some(float_args) = options.float_args {
4585            for &value in float_args {
4586                unsafe { ffi::b3CustomCommandExecuteAddFloatArgument(command, value) };
4587            }
4588        }
4589
4590        let status = self.submit_simple_command(
4591            command,
4592            ffi::EnumSharedMemoryServerStatus::CMD_CUSTOM_COMMAND_COMPLETED,
4593        )?;
4594
4595        let status_code = unsafe { ffi::b3GetStatusPluginCommandResult(status.handle) };
4596        let mut raw = ffi::b3UserDataValue { m_type: 0, m_length: 0, m_data1: ptr::null() };
4597        let has_data =
4598            unsafe { ffi::b3GetStatusPluginCommandReturnData(self.handle, &raw mut raw) } != 0;
4599        let return_data = if has_data && raw.m_length > 0 && !raw.m_data1.is_null() {
4600            let len = raw.m_length as usize;
4601            let bytes = unsafe { slice::from_raw_parts(raw.m_data1, len) };
4602            let data = bytes.iter().map(|&b| b as i32).collect();
4603            Some(PluginCommandReturnData { value_type: raw.m_type, data })
4604        } else {
4605            None
4606        };
4607
4608        Ok(PluginCommandResult { status: status_code, return_data })
4609    }
4610
4611    pub fn submit_profile_timing(&mut self, event_name: Option<&str>) -> BulletResult<()> {
4612        self.ensure_can_submit()?;
4613        let name_c = event_name.map(CString::new).transpose()?;
4614        let command = unsafe {
4615            ffi::b3ProfileTimingCommandInit(
4616                self.handle,
4617                name_c.as_ref().map_or(ptr::null(), |value| value.as_ptr()),
4618            )
4619        };
4620        unsafe {
4621            ffi::b3SetProfileTimingType(command, name_c.is_some().into());
4622        }
4623        self.submit_simple_command(
4624            command,
4625            ffi::EnumSharedMemoryServerStatus::CMD_CLIENT_COMMAND_COMPLETED,
4626        )?;
4627        Ok(())
4628    }
4629
4630    pub fn set_time_out(&mut self, time_out_seconds: f64) -> BulletResult<&mut Self> {
4631        if time_out_seconds < 0.0 {
4632            return Err(BulletError::CommandFailed {
4633                message: "Timeout must be non-negative",
4634                code: -1,
4635            });
4636        }
4637        self.ensure_can_submit()?;
4638        unsafe { ffi::b3SetTimeOut(self.handle, time_out_seconds) };
4639        Ok(self)
4640    }
4641
4642    pub fn get_api_version(&self) -> i32 {
4643        ffi::SHARED_MEMORY_MAGIC_NUMBER
4644    }
4645}
4646
4647/// ! =====================================================================================================================================
4648/// ### tool functions
4649impl PhysicsClient {
4650    fn ensure_can_submit(&mut self) -> BulletResult<()> {
4651        if self.can_submit_command() {
4652            Ok(())
4653        } else {
4654            Err(BulletError::ServerUnavailable(
4655                "Physics server is not running",
4656            ))
4657        }
4658    }
4659
4660    fn apply_physics_engine_update(
4661        command: ffi::b3SharedMemoryCommandHandle,
4662        update: &PhysicsEngineParametersUpdate,
4663    ) {
4664        unsafe {
4665            if let Some(num_solver_iterations) = update.num_solver_iterations {
4666                ffi::b3PhysicsParamSetNumSolverIterations(command, num_solver_iterations as i32);
4667            }
4668            if let Some(minimum_solver_island_size) = update.minimum_solver_island_size {
4669                ffi::b3PhysicsParameterSetMinimumSolverIslandSize(
4670                    command,
4671                    minimum_solver_island_size as i32,
4672                );
4673            }
4674            if let Some(solver_residual_threshold) = update.solver_residual_threshold {
4675                assert!(solver_residual_threshold.is_sign_positive());
4676                ffi::b3PhysicsParamSetSolverResidualThreshold(command, solver_residual_threshold);
4677            }
4678            if let Some(collision_filter_mode) = update.collision_filter_mode {
4679                ffi::b3PhysicsParamSetCollisionFilterMode(command, collision_filter_mode as i32);
4680            }
4681            if let Some(num_sub_steps) = update.num_sub_steps {
4682                ffi::b3PhysicsParamSetNumSubSteps(command, num_sub_steps as i32);
4683            }
4684            if let Some(fixed_time_step) = update.fixed_time_step {
4685                ffi::b3PhysicsParamSetTimeStep(command, fixed_time_step.as_secs_f64());
4686            }
4687            if let Some(use_split_impulse) = update.use_split_impulse {
4688                if use_split_impulse {
4689                    ffi::b3PhysicsParamSetUseSplitImpulse(command, 1);
4690                } else {
4691                    ffi::b3PhysicsParamSetUseSplitImpulse(command, 0);
4692                }
4693            }
4694            if let Some(split_impulse_penetration_threshold) =
4695                update.split_impulse_penetration_threshold
4696            {
4697                assert!(split_impulse_penetration_threshold.is_sign_positive());
4698                ffi::b3PhysicsParamSetSplitImpulsePenetrationThreshold(
4699                    command,
4700                    split_impulse_penetration_threshold,
4701                );
4702            }
4703            if let Some(contact_breaking_threshold) = update.contact_breaking_threshold {
4704                assert!(contact_breaking_threshold.is_sign_positive());
4705                ffi::b3PhysicsParamSetContactBreakingThreshold(command, contact_breaking_threshold);
4706            }
4707            if let Some(contact_slop) = update.contact_slop {
4708                assert!(contact_slop.is_sign_positive());
4709                ffi::b3PhysicsParamSetContactSlop(command, contact_slop);
4710            }
4711            if let Some(max_num_cmd_per_1_ms) = update.max_num_cmd_per_1_ms {
4712                assert!(max_num_cmd_per_1_ms >= -1);
4713                ffi::b3PhysicsParamSetMaxNumCommandsPer1ms(command, max_num_cmd_per_1_ms);
4714            }
4715            if let Some(restitution_velocity_threshold) = update.restitution_velocity_threshold {
4716                assert!(restitution_velocity_threshold.is_sign_positive());
4717                ffi::b3PhysicsParamSetRestitutionVelocityThreshold(
4718                    command,
4719                    restitution_velocity_threshold,
4720                );
4721            }
4722            if let Some(enable_file_caching) = update.enable_file_caching {
4723                if enable_file_caching {
4724                    ffi::b3PhysicsParamSetEnableFileCaching(command, 1);
4725                } else {
4726                    ffi::b3PhysicsParamSetEnableFileCaching(command, 0);
4727                }
4728            }
4729            if let Some(erp) = update.erp {
4730                assert!(erp.is_sign_positive());
4731                ffi::b3PhysicsParamSetDefaultNonContactERP(command, erp);
4732            }
4733            if let Some(contact_erp) = update.contact_erp {
4734                assert!(contact_erp.is_sign_positive());
4735                ffi::b3PhysicsParamSetDefaultContactERP(command, contact_erp);
4736            }
4737            if let Some(friction_erp) = update.friction_erp {
4738                assert!(friction_erp.is_sign_positive());
4739                ffi::b3PhysicsParamSetDefaultFrictionERP(command, friction_erp);
4740            }
4741            if let Some(enable_cone_friction) = update.enable_cone_friction {
4742                if enable_cone_friction {
4743                    ffi::b3PhysicsParamSetEnableConeFriction(command, 1);
4744                } else {
4745                    ffi::b3PhysicsParamSetEnableConeFriction(command, 0);
4746                }
4747            }
4748            if let Some(deterministic_overlapping_pairs) = update.deterministic_overlapping_pairs {
4749                if deterministic_overlapping_pairs {
4750                    ffi::b3PhysicsParameterSetDeterministicOverlappingPairs(command, 1);
4751                } else {
4752                    ffi::b3PhysicsParameterSetDeterministicOverlappingPairs(command, 0);
4753                }
4754            }
4755            if let Some(allowed_ccd_penetration) = update.allowed_ccd_penetration {
4756                assert!(allowed_ccd_penetration.is_sign_positive());
4757                ffi::b3PhysicsParameterSetAllowedCcdPenetration(command, allowed_ccd_penetration);
4758            }
4759            if let Some(joint_feedback_mode) = update.joint_feedback_mode {
4760                ffi::b3PhysicsParameterSetJointFeedbackMode(command, joint_feedback_mode as i32);
4761            }
4762            if let Some(enable_sat) = update.enable_sat {
4763                if enable_sat {
4764                    ffi::b3PhysicsParameterSetEnableSAT(command, 1);
4765                } else {
4766                    ffi::b3PhysicsParameterSetEnableSAT(command, 0);
4767                }
4768            }
4769            if let Some(constraint_solver_type) = update.constraint_solver_type {
4770                let val = constraint_solver_type as i32;
4771                println!("{val:?}");
4772                ffi::b3PhysicsParameterSetConstraintSolverType(command, val);
4773            }
4774            if let Some(global_cfm) = update.global_cfm {
4775                assert!(global_cfm.is_sign_positive());
4776                ffi::b3PhysicsParamSetDefaultGlobalCFM(command, global_cfm);
4777            }
4778            if let Some(report_solver_analytics) = update.report_solver_analytics {
4779                if report_solver_analytics {
4780                    ffi::b3PhysicsParamSetSolverAnalytics(command, 1);
4781                } else {
4782                    ffi::b3PhysicsParamSetSolverAnalytics(command, 0);
4783                }
4784            }
4785            if let Some(warm_starting_factor) = update.warm_starting_factor {
4786                assert!(warm_starting_factor.is_sign_positive());
4787                ffi::b3PhysicsParamSetWarmStartingFactor(command, warm_starting_factor);
4788            }
4789            if let Some(sparse_sdf_voxel_size) = update.sparse_sdf_voxel_size {
4790                assert!(sparse_sdf_voxel_size.is_sign_positive());
4791                ffi::b3PhysicsParameterSetSparseSdfVoxelSize(command, sparse_sdf_voxel_size);
4792            }
4793            if let Some(num_non_contact_inner_iterations) = update.num_non_contact_inner_iterations
4794            {
4795                assert!(num_non_contact_inner_iterations >= 1);
4796                ffi::b3PhysicsParamSetNumNonContactInnerIterations(
4797                    command,
4798                    num_non_contact_inner_iterations as i32,
4799                );
4800            }
4801        }
4802    }
4803
4804    fn submit_command(
4805        &mut self,
4806        command: ffi::b3SharedMemoryCommandHandle,
4807    ) -> BulletResult<CommandStatus> {
4808        unsafe {
4809            let handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command);
4810            if handle.is_null() {
4811                return Err(BulletError::NullPointer(
4812                    "Bullet returned a null status handle",
4813                ));
4814            }
4815            let status_type = ffi::b3GetStatusType(handle);
4816            Ok(CommandStatus { handle, status_type })
4817        }
4818    }
4819
4820    fn submit_simple_command(
4821        &mut self,
4822        command: ffi::b3SharedMemoryCommandHandle,
4823        expected_status: ffi::EnumSharedMemoryServerStatus,
4824    ) -> BulletResult<CommandStatus> {
4825        let status = self.submit_command(command)?;
4826        let expected = expected_status as i32;
4827        if status.status_type != expected {
4828            return Err(BulletError::UnexpectedStatus { expected, actual: status.status_type });
4829        }
4830        Ok(status)
4831    }
4832
4833    fn collect_body_indices(
4834        status_handle: ffi::b3SharedMemoryStatusHandle,
4835    ) -> BulletResult<Vec<i32>> {
4836        if status_handle.is_null() {
4837            return Err(BulletError::NullPointer(
4838                "Bullet returned a null status handle while collecting body indices",
4839            ));
4840        }
4841
4842        let capacity = usize::try_from(ffi::MAX_SDF_BODIES).unwrap_or(512).max(1);
4843        let mut buffer = vec![0_i32; capacity];
4844        let count = unsafe {
4845            ffi::b3GetStatusBodyIndices(status_handle, buffer.as_mut_ptr(), capacity as i32)
4846        };
4847        if count < 0 {
4848            return Err(BulletError::CommandFailed {
4849                message: "Failed to read body indices from status",
4850                code: count,
4851            });
4852        }
4853        buffer.truncate(count as usize);
4854        Ok(buffer)
4855    }
4856
4857    fn add_collision_geometry(
4858        &self,
4859        command: ffi::b3SharedMemoryCommandHandle,
4860        geometry: &CollisionGeometry<'_>,
4861        scratch: &mut GeometryScratch,
4862    ) -> BulletResult<i32> {
4863        use CollisionGeometry::*;
4864
4865        let index = match geometry {
4866            Sphere { radius } => {
4867                if *radius <= 0.0 {
4868                    return Err(BulletError::CommandFailed {
4869                        message: "Sphere radius must be positive",
4870                        code: -1,
4871                    });
4872                }
4873                unsafe { ffi::b3CreateCollisionShapeAddSphere(command, *radius) }
4874            }
4875            Box { half_extents } => unsafe {
4876                ffi::b3CreateCollisionShapeAddBox(command, half_extents.as_ptr())
4877            },
4878            Capsule { radius, height } => {
4879                if *radius <= 0.0 || *height < 0.0 {
4880                    return Err(BulletError::CommandFailed {
4881                        message: "Capsule dimensions must be positive",
4882                        code: -1,
4883                    });
4884                }
4885                unsafe { ffi::b3CreateCollisionShapeAddCapsule(command, *radius, *height) }
4886            }
4887            Cylinder { radius, height } => {
4888                if *radius <= 0.0 || *height <= 0.0 {
4889                    return Err(BulletError::CommandFailed {
4890                        message: "Cylinder dimensions must be positive",
4891                        code: -1,
4892                    });
4893                }
4894                unsafe { ffi::b3CreateCollisionShapeAddCylinder(command, *radius, *height) }
4895            }
4896            Plane { normal, constant } => unsafe {
4897                ffi::b3CreateCollisionShapeAddPlane(command, normal.as_ptr(), *constant)
4898            },
4899            &MeshFile { file, scale } => {
4900                let file_ptr = scratch.push_c_string(file)?;
4901                unsafe { ffi::b3CreateCollisionShapeAddMesh(command, file_ptr, scale.as_ptr()) }
4902            }
4903            ConvexMesh { vertices, scale } => {
4904                if vertices.is_empty() {
4905                    return Err(BulletError::CommandFailed {
4906                        message: "Convex mesh requires at least one vertex",
4907                        code: -1,
4908                    });
4909                }
4910                let flattened = Self::flatten_points(vertices);
4911                let vertices_ptr = scratch.push_f64_buffer(flattened);
4912                unsafe {
4913                    ffi::b3CreateCollisionShapeAddConvexMesh(
4914                        self.handle,
4915                        command,
4916                        scale.as_ptr(),
4917                        vertices_ptr,
4918                        vertices.len() as i32,
4919                    )
4920                }
4921            }
4922            Mesh { vertices, indices, scale } => {
4923                if vertices.is_empty() || indices.is_empty() {
4924                    return Err(BulletError::CommandFailed {
4925                        message: "Concave mesh requires vertices and indices",
4926                        code: -1,
4927                    });
4928                }
4929                let vertex_buffer = Self::flatten_points(vertices);
4930                let index_buffer = indices.to_vec();
4931                let vertices_ptr = scratch.push_f64_buffer(vertex_buffer);
4932                let indices_ptr = scratch.push_i32_buffer(index_buffer);
4933                unsafe {
4934                    ffi::b3CreateCollisionShapeAddConcaveMesh(
4935                        self.handle,
4936                        command,
4937                        scale.as_ptr(),
4938                        vertices_ptr,
4939                        vertices.len() as i32,
4940                        indices_ptr,
4941                        indices.len() as i32,
4942                    )
4943                }
4944            }
4945            HeightfieldFile { file, mesh_scale, texture_scaling } => {
4946                let file_ptr = scratch.push_c_string(file)?;
4947                unsafe {
4948                    ffi::b3CreateCollisionShapeAddHeightfield(
4949                        command,
4950                        file_ptr,
4951                        mesh_scale.as_ptr(),
4952                        *texture_scaling,
4953                    )
4954                }
4955            }
4956            HeightfieldData {
4957                mesh_scale,
4958                texture_scaling,
4959                samples,
4960                rows,
4961                columns,
4962                replace_index,
4963            } => {
4964                if *rows <= 0 || *columns <= 0 {
4965                    return Err(BulletError::CommandFailed {
4966                        message: "Heightfield dimensions must be positive",
4967                        code: -1,
4968                    });
4969                }
4970                let expected_len = (*rows as usize).saturating_mul(*columns as usize);
4971                if expected_len != samples.len() {
4972                    return Err(BulletError::CommandFailed {
4973                        message: "Heightfield sample count mismatch",
4974                        code: expected_len as i32,
4975                    });
4976                }
4977                let buffer = samples.to_vec();
4978                let data_ptr = scratch.push_f32_buffer_mut(buffer);
4979                unsafe {
4980                    ffi::b3CreateCollisionShapeAddHeightfield2(
4981                        self.handle,
4982                        command,
4983                        mesh_scale.as_ptr(),
4984                        *texture_scaling,
4985                        data_ptr,
4986                        *rows,
4987                        *columns,
4988                        replace_index.unwrap_or(-1),
4989                    )
4990                }
4991            }
4992        };
4993
4994        if index < 0 {
4995            Err(BulletError::CommandFailed {
4996                message: "Bullet rejected collision geometry",
4997                code: index,
4998            })
4999        } else {
5000            Ok(index)
5001        }
5002    }
5003
5004    fn add_visual_geometry(
5005        &self,
5006        command: ffi::b3SharedMemoryCommandHandle,
5007        geometry: &VisualGeometry<'_>,
5008        scratch: &mut GeometryScratch,
5009    ) -> BulletResult<i32> {
5010        use VisualGeometry::*;
5011
5012        let index = match geometry {
5013            Sphere { radius } => {
5014                if *radius <= 0.0 {
5015                    return Err(BulletError::CommandFailed {
5016                        message: "Sphere radius must be positive",
5017                        code: -1,
5018                    });
5019                }
5020                unsafe { ffi::b3CreateVisualShapeAddSphere(command, *radius) }
5021            }
5022            Box { half_extents } => unsafe {
5023                ffi::b3CreateVisualShapeAddBox(command, half_extents.as_ptr())
5024            },
5025            Capsule { radius, height } => {
5026                if *radius <= 0.0 || *height < 0.0 {
5027                    return Err(BulletError::CommandFailed {
5028                        message: "Capsule dimensions must be positive",
5029                        code: -1,
5030                    });
5031                }
5032                unsafe { ffi::b3CreateVisualShapeAddCapsule(command, *radius, *height) }
5033            }
5034            Cylinder { radius, height } => {
5035                if *radius <= 0.0 || *height <= 0.0 {
5036                    return Err(BulletError::CommandFailed {
5037                        message: "Cylinder dimensions must be positive",
5038                        code: -1,
5039                    });
5040                }
5041                unsafe { ffi::b3CreateVisualShapeAddCylinder(command, *radius, *height) }
5042            }
5043            Plane { normal, constant } => unsafe {
5044                ffi::b3CreateVisualShapeAddPlane(command, normal.as_ptr(), *constant)
5045            },
5046            Mesh { file, scale } => {
5047                let file_ptr = scratch.push_c_string(file)?;
5048                unsafe { ffi::b3CreateVisualShapeAddMesh(command, file_ptr, scale.as_ptr()) }
5049            }
5050            MeshData { vertices, indices, normals, uvs, scale } => {
5051                if vertices.is_empty() {
5052                    return Err(BulletError::CommandFailed {
5053                        message: "Visual mesh requires vertices",
5054                        code: -1,
5055                    });
5056                }
5057                let vertex_buffer = Self::flatten_points(vertices);
5058                let vertices_ptr = scratch.push_f64_buffer(vertex_buffer);
5059
5060                let (indices_ptr, num_indices) = if let Some(indices) = indices {
5061                    let buffer = indices.to_vec();
5062                    (scratch.push_i32_buffer(buffer), indices.len() as i32)
5063                } else {
5064                    (ptr::null::<i32>(), 0)
5065                };
5066
5067                let (normals_ptr, num_normals) = if let Some(normals) = normals {
5068                    let buffer = Self::flatten_points(normals);
5069                    (scratch.push_f64_buffer(buffer), normals.len() as i32)
5070                } else {
5071                    (ptr::null::<f64>(), 0)
5072                };
5073
5074                let (uvs_ptr, num_uvs) = if let Some(uvs) = uvs {
5075                    let buffer = Self::flatten_points(uvs);
5076                    (scratch.push_f64_buffer(buffer), (uvs.len() * 2) as i32)
5077                } else {
5078                    (ptr::null::<f64>(), 0)
5079                };
5080
5081                unsafe {
5082                    ffi::b3CreateVisualShapeAddMesh2(
5083                        self.handle,
5084                        command,
5085                        scale.as_ptr(),
5086                        vertices_ptr,
5087                        vertices.len() as i32,
5088                        indices_ptr,
5089                        num_indices,
5090                        normals_ptr,
5091                        num_normals,
5092                        uvs_ptr,
5093                        num_uvs,
5094                    )
5095                }
5096            }
5097        };
5098
5099        if index < 0 {
5100            Err(BulletError::CommandFailed {
5101                message: "Bullet rejected visual geometry",
5102                code: index,
5103            })
5104        } else {
5105            Ok(index)
5106        }
5107    }
5108
5109    fn flatten_points<const N: usize>(values: &[[f64; N]]) -> Vec<f64> {
5110        let mut out = Vec::with_capacity(values.len() * N);
5111        for value in values {
5112            out.extend_from_slice(value);
5113        }
5114        out
5115    }
5116
5117    fn flatten_isometries(values: &[na::Isometry3<f64>]) -> Vec<f64> {
5118        let mut out = Vec::with_capacity(values.len() * 7);
5119        for transform in values {
5120            let (position, orientation) = isometry_to_raw_parts(transform);
5121            out.extend_from_slice(&position);
5122            out.extend_from_slice(&orientation);
5123        }
5124        out
5125    }
5126
5127    fn write_transform_to_frame(transform: &na::Isometry3<f64>, frame: &mut [f64; 7]) {
5128        isometry_write_to_frame(transform, frame);
5129    }
5130
5131    fn read_frame_transform(frame: &[f64; 7]) -> na::Isometry3<f64> {
5132        isometry_from_frame(frame)
5133    }
5134
5135    fn constraint_update_has_changes(update: &ConstraintUpdate) -> bool {
5136        update.child_frame.is_some()
5137            || update.max_force.is_some()
5138            || update.gear_ratio.is_some()
5139            || update.gear_aux_link.is_some()
5140            || update.relative_position_target.is_some()
5141            || update.erp.is_some()
5142    }
5143
5144    pub(crate) fn read_c_string(raw: &[std::os::raw::c_char]) -> String {
5145        if raw.is_empty() || raw[0] == 0 {
5146            return String::new();
5147        }
5148        unsafe { CStr::from_ptr(raw.as_ptr()) }
5149            .to_string_lossy()
5150            .into_owned()
5151    }
5152
5153    fn request_actual_state_status(&mut self, body_unique_id: i32) -> BulletResult<CommandStatus> {
5154        self.request_actual_state_status_with_flags(body_unique_id, false, false)
5155    }
5156
5157    fn request_actual_state_status_with_flags(
5158        &mut self,
5159        body_unique_id: i32,
5160        compute_link_velocity: bool,
5161        compute_forward_kinematics: bool,
5162    ) -> BulletResult<CommandStatus> {
5163        self.ensure_can_submit()?;
5164        let command = unsafe { ffi::b3RequestActualStateCommandInit(self.handle, body_unique_id) };
5165        unsafe {
5166            let velocity_flag = compute_link_velocity.into();
5167            let kinematics_flag = compute_forward_kinematics.into();
5168            ffi::b3RequestActualStateCommandComputeLinkVelocity(command, velocity_flag);
5169            ffi::b3RequestActualStateCommandComputeForwardKinematics(command, kinematics_flag);
5170        }
5171        self.submit_simple_command(
5172            command,
5173            ffi::EnumSharedMemoryServerStatus::CMD_ACTUAL_STATE_UPDATE_COMPLETED,
5174        )
5175    }
5176
5177    fn read_joint_state(
5178        &mut self,
5179        status_handle: ffi::b3SharedMemoryStatusHandle,
5180        joint_index: i32,
5181    ) -> BulletResult<JointState> {
5182        let mut raw = MaybeUninit::<ffi::b3JointSensorState>::uninit();
5183        let success = unsafe {
5184            ffi::b3GetJointState(self.handle, status_handle, joint_index, raw.as_mut_ptr())
5185        };
5186        if success == 0 {
5187            return Err(BulletError::CommandFailed {
5188                message: "Cannot query joint state",
5189                code: success,
5190            });
5191        }
5192        let raw = unsafe { raw.assume_init() };
5193        Ok(JointState {
5194            position: raw.m_joint_position,
5195            velocity: raw.m_joint_velocity,
5196            force_torque: raw.m_joint_force_torque,
5197            motor_torque: raw.m_joint_motor_torque,
5198        })
5199    }
5200
5201    fn read_joint_state_multi_dof(
5202        &mut self,
5203        status_handle: ffi::b3SharedMemoryStatusHandle,
5204        joint_index: i32,
5205    ) -> BulletResult<JointStateMultiDof> {
5206        let mut raw = MaybeUninit::<ffi::b3JointSensorState2>::uninit();
5207        let success = unsafe {
5208            ffi::b3GetJointStateMultiDof(self.handle, status_handle, joint_index, raw.as_mut_ptr())
5209        };
5210        if success == 0 {
5211            return Err(BulletError::CommandFailed {
5212                message: "Cannot query multi-DoF joint state",
5213                code: success,
5214            });
5215        }
5216        let raw = unsafe { raw.assume_init() };
5217
5218        let q_len = raw.m_q_dof_size as usize;
5219        let u_len = raw.m_u_dof_size as usize;
5220        if q_len > raw.m_joint_position.len() || u_len > raw.m_joint_velocity.len() {
5221            return Err(BulletError::CommandFailed {
5222                message: "Bullet reported invalid multi-DoF state lengths",
5223                code: (q_len.max(u_len)) as i32,
5224            });
5225        }
5226
5227        Ok(JointStateMultiDof {
5228            positions: raw.m_joint_position[..q_len].to_vec(),
5229            velocities: raw.m_joint_velocity[..u_len].to_vec(),
5230            reaction_forces: raw.m_joint_reaction_force_torque,
5231            motor_torques: raw.m_joint_motor_torque_multi_dof[..u_len].to_vec(),
5232        })
5233    }
5234
5235    fn read_link_state(
5236        &mut self,
5237        status_handle: ffi::b3SharedMemoryStatusHandle,
5238        link_index: i32,
5239    ) -> BulletResult<LinkState> {
5240        let mut raw = MaybeUninit::<ffi::b3LinkState>::uninit();
5241        let success = unsafe {
5242            ffi::b3GetLinkState(self.handle, status_handle, link_index, raw.as_mut_ptr())
5243        };
5244        if success == 0 {
5245            return Err(BulletError::CommandFailed {
5246                message: "Cannot query link state",
5247                code: success,
5248            });
5249        }
5250        unsafe { raw.assume_init() }.try_into()
5251    }
5252
5253    fn extract_base_state(
5254        status_handle: ffi::b3SharedMemoryStatusHandle,
5255    ) -> BulletResult<(na::Isometry3<f64>, [f64; 6])> {
5256        if status_handle.is_null() {
5257            return Err(BulletError::NullPointer(
5258                "Bullet returned a null status handle while reading base state",
5259            ));
5260        }
5261
5262        let mut actual_state_q: *const f64 = ptr::null();
5263        let mut actual_state_qdot: *const f64 = ptr::null();
5264
5265        let result = unsafe {
5266            ffi::b3GetStatusActualState(
5267                status_handle,
5268                ptr::null_mut(),
5269                ptr::null_mut(),
5270                ptr::null_mut(),
5271                ptr::null_mut(),
5272                &raw mut actual_state_q,
5273                &raw mut actual_state_qdot,
5274                ptr::null_mut(),
5275            )
5276        };
5277
5278        if result == 0 {
5279            return Err(BulletError::CommandFailed {
5280                message: "Failed to read actual state",
5281                code: result,
5282            });
5283        }
5284
5285        if actual_state_q.is_null() {
5286            return Err(BulletError::NullPointer(
5287                "Bullet returned a null actual-state position pointer",
5288            ));
5289        }
5290
5291        let mut position = [0.0; 3];
5292        let mut orientation = [0.0; 4];
5293        unsafe {
5294            ptr::copy_nonoverlapping(actual_state_q, position.as_mut_ptr(), 3);
5295            ptr::copy_nonoverlapping(actual_state_q.add(3), orientation.as_mut_ptr(), 4);
5296        }
5297
5298        let mut velocity = [0.0; 6];
5299        if !actual_state_qdot.is_null() {
5300            unsafe {
5301                ptr::copy_nonoverlapping(actual_state_qdot, velocity.as_mut_ptr(), 6);
5302            }
5303        }
5304
5305        Ok((
5306            na::Isometry3::from_parts(
5307                na::Translation3::from(position),
5308                na::UnitQuaternion::from_quaternion(orientation.into()),
5309            ),
5310            velocity,
5311        ))
5312    }
5313}
5314
5315impl Drop for PhysicsClient {
5316    fn drop(&mut self) {
5317        unsafe {
5318            ffi::b3DisconnectSharedMemory(self.handle);
5319        }
5320        // Decrement totals
5321        let _ = TOTAL_CLIENTS.fetch_update(Ordering::SeqCst, Ordering::SeqCst, |v| {
5322            Some(v.saturating_sub(1))
5323        });
5324        if let Mode::Gui | Mode::GuiServer = self.mode {
5325            // Release GUI slot
5326            GUI_GUARD.store(0, Ordering::SeqCst);
5327            let _ = GUI_CLIENTS.fetch_update(Ordering::SeqCst, Ordering::SeqCst, |v| {
5328                Some(v.saturating_sub(1))
5329            });
5330        }
5331    }
5332}
5333
5334// ---- Process-wide statistics ----
5335/// Return the number of active physics clients in this process (PyBullet-compatible name).
5336pub fn get_num_physics_clients() -> usize {
5337    TOTAL_CLIENTS.load(Ordering::SeqCst)
5338}
5339
5340/// Return the number of active GUI/graphics clients in this process (PyBullet-compatible name).
5341pub fn get_num_gui_physics_clients() -> usize {
5342    GUI_CLIENTS.load(Ordering::SeqCst)
5343}
rsbullet_core/client.rs

rsbullet_core/
client.rs
