nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: implement new "small-steps" solver + joint improvements

Browse Source
This commit is contained in:
Sébastien Crozet
2024-01-21 21:02:23 +01:00
parent 9ac3503b87
commit 9b87f06a85
76 changed files with 6672 additions and 4305 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/dynamics/rigid_body_components.rs
View File

@@ -7,7 +7,7 @@ use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
};
use crate::parry::partitioning::IndexedData;
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
/// The unique handle of a rigid body added to a `RigidBodySet`.
@@ -307,11 +307,52 @@ impl RigidBodyMassProps {
self.effective_inv_mass.map(crate::utils::inv)
}
/// The square root of the effective world-space angular inertia (that takes the potential rotation locking into account) of
/// this rigid-body.
#[must_use]
pub fn effective_angular_inertia_sqrt(&self) -> AngularInertia<Real> {
#[allow(unused_mut)] // mut needed in 3D.
let mut ang_inertia = self.effective_world_inv_inertia_sqrt;
// Make the matrix invertible.
#[cfg(feature = "dim3")]
{
if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
ang_inertia.m11 = 1.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
ang_inertia.m22 = 1.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
ang_inertia.m33 = 1.0;
}
}
#[allow(unused_mut)] // mut needed in 3D.
let mut result = ang_inertia.inverse();
// Remove the locked axes again.
#[cfg(feature = "dim3")]
{
if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
result.m11 = 0.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
result.m22 = 0.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
result.m33 = 0.0;
}
}
result
}
/// The effective world-space angular inertia (that takes the potential rotation locking into account) of
/// this rigid-body.
#[must_use]
pub fn effective_angular_inertia(&self) -> AngularInertia<Real> {
self.effective_world_inv_inertia_sqrt.squared().inverse()
self.effective_angular_inertia_sqrt().squared()
}
/// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders.