diff options
30 files changed, 634 insertions, 86 deletions
diff --git a/.circleci/config.yml b/.circleci/config.yml new file mode 100644 index 0000000..c0722a9 --- /dev/null +++ b/.circleci/config.yml @@ -0,0 +1,82 @@ +version: 2.1 + +executors: + rust-executor: + docker: + - image: rust:latest + +jobs: + check-fmt: + executor: rust-executor + steps: + - checkout + - run: + name: install rustfmt + command: rustup component add rustfmt + - run: + name: check formatting + command: cargo fmt -- --check + build-native: + executor: rust-executor + steps: + - checkout + - run: apt-get update + - run: apt-get install -y cmake libxcb-composite0-dev + - run: + name: build rapier2d + command: cargo build --verbose -p rapier2d; + - run: + name: build rapier3d + command: cargo build --verbose -p rapier3d; + - run: + name: build rapier2d SIMD + command: cd build/rapier2d; cargo build --verbose --features simd-stable; + - run: + name: build rapier3d SIMD + command: cd build/rapier3d; cargo build --verbose --features simd-stable; + - run: + name: build rapier2d SIMD Parallel + command: cd build/rapier2d; cargo build --verbose --features simd-stable --features parallel; + - run: + name: build rapier3d SIMD Parallel + command: cd build/rapier3d; cargo build --verbose --features simd-stable --features parallel; + - run: + name: test + command: cargo test + - run: + name: check rapier_testbed2d + command: cargo check --verbose -p rapier_testbed2d; + - run: + name: check rapier_testbed3d + command: cargo check --verbose -p rapier_testbed3d; + - run: + name: check rapier-examples-2d + command: cargo check -j 1 --verbose -p rapier-examples-2d; + - run: + name: check rapier-examples-3d + command: cargo check -j 1 --verbose -p rapier-examples-3d; + build-wasm: + executor: rust-executor + steps: + - checkout + - run: + name: install cargo-web + command: cargo install -f cargo-web; + - run: + name: build rapier2d + command: cd build/rapier2d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; + - run: + name: build rapier3d + command: cd build/rapier3d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown; + +workflows: + version: 2 + build: + jobs: + - check-fmt + - build-native: + requires: + - check-fmt + - build-wasm: + requires: + - check-fmt @@ -10,3 +10,10 @@ debug = false codegen-units = 1 #opt-level = 1 #lto = true + + +#[profile.dev.package.rapier3d] +#opt-level = 3 +# +#[profile.dev.package.kiss3d] +#opt-level = 3
\ No newline at end of file diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml index 7e97948..ad63958 100644 --- a/examples2d/Cargo.toml +++ b/examples2d/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "nphysics-examples-2d" +name = "rapier-examples-2d" version = "0.1.0" authors = [ "Sébastien Crozet <developer@crozet.re>" ] edition = "2018" diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 3176696..efc3cce 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "nphysics-examples-3d" +name = "rapier-examples-3d" version = "0.1.0" authors = [ "Sébastien Crozet <developer@crozet.re>" ] edition = "2018" @@ -24,4 +24,4 @@ path = "../build/rapier3d" [[bin]] name = "all_examples3" -path = "./all_examples3.rs"
\ No newline at end of file +path = "./all_examples3.rs" diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 04b9b36..2b89efa 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -14,6 +14,7 @@ mod add_remove3; mod balls3; mod boxes3; mod capsules3; +mod compound3; mod debug_boxes3; mod debug_triangle3; mod domino3; @@ -71,6 +72,7 @@ pub fn main() { ("Balls", balls3::init_world), ("Boxes", boxes3::init_world), ("Capsules", capsules3::init_world), + ("Compound", compound3::init_world), ("Domino", domino3::init_world), ("Heightfield", heightfield3::init_world), ("Joints", joints3::init_world), diff --git a/examples3d/compound3.rs b/examples3d/compound3.rs new file mode 100644 index 0000000..a8a9939 --- /dev/null +++ b/examples3d/compound3.rs @@ -0,0 +1,76 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 200.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.2; + + let shift = rad * 4.0 + rad; + let centerx = shift * (num / 2) as f32; + let centery = shift / 2.0; + let centerz = shift * (num / 2) as f32; + + let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; + + for j in 0usize..25 { + for i in 0..num { + for k in 0usize..num { + let x = i as f32 * shift * 5.0 - centerx + offset; + let y = j as f32 * (shift * 5.0) + centery + 3.0; + let z = k as f32 * shift * 2.0 - centerz + offset; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build(); + let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad) + .translation(rad * 10.0, rad * 10.0, 0.0) + .build(); + let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad) + .translation(-rad * 10.0, rad * 10.0, 0.0) + .build(); + colliders.insert(collider1, handle, &mut bodies); + colliders.insert(collider2, handle, &mut bodies); + colliders.insert(collider3, handle, &mut bodies); + } + } + + offset -= 0.05 * rad * (num as f32 - 1.0); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/rustfmt.toml b/rustfmt.toml new file mode 100644 index 0000000..a71ae5f --- /dev/null +++ b/rustfmt.toml @@ -0,0 +1,3 @@ +#indent_style = "Block" +#where_single_line = true +#brace_style = "PreferSameLine"
\ No newline at end of file diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index cc2979c..22e7da5 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -1,7 +1,7 @@ use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector}; use crate::utils; use num::Zero; -use std::ops::{Add, AddAssign}; +use std::ops::{Add, AddAssign, Sub, SubAssign}; #[cfg(feature = "dim3")] use {na::Matrix3, std::ops::MulAssign}; @@ -91,6 +91,18 @@ impl MassProperties { } #[cfg(feature = "dim3")] + /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii. + pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> { + let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); + self.principal_inertia_local_frame.to_rotation_matrix() + * Matrix3::from_diagonal(&inv_principal_inertia) + * self + .principal_inertia_local_frame + .inverse() + .to_rotation_matrix() + } + + #[cfg(feature = "dim3")] /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> { let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); @@ -125,6 +137,19 @@ impl MassProperties { Matrix3::zeros() } } + + /// Transform each element of the mass properties. + pub fn transform_by(&self, m: &Isometry<f32>) -> Self { + // NOTE: we don't apply the parallel axis theorem here + // because the center of mass is also transformed. + Self { + local_com: m * self.local_com, + inv_mass: self.inv_mass, + inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt, + #[cfg(feature = "dim3")] + principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame, + } + } } impl Zero for MassProperties { @@ -143,6 +168,68 @@ impl Zero for MassProperties { } } +impl Sub<MassProperties> for MassProperties { + type Output = Self; + + #[cfg(feature = "dim2")] + fn sub(self, other: MassProperties) -> Self { + if self.is_zero() || other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 - m2); + + let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 - i2; + // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. + let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt()); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + } + } + + #[cfg(feature = "dim3")] + fn sub(self, other: MassProperties) -> Self { + if self.is_zero() || other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 - m2); + let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 - i2; + let eigen = inertia.symmetric_eigen(); + let principal_inertia_local_frame = + Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one()); + let principal_inertia = eigen.eigenvalues; + // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. + let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt())); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + principal_inertia_local_frame, + } + } +} + +impl SubAssign<MassProperties> for MassProperties { + fn sub_assign(&mut self, rhs: MassProperties) { + *self = *self - rhs + } +} + impl Add<MassProperties> for MassProperties { type Output = Self; @@ -186,7 +273,8 @@ impl Add<MassProperties> for MassProperties { let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); let inertia = i1 + i2; let eigen = inertia.symmetric_eigen(); - let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors); + let principal_inertia_local_frame = + Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one()); let principal_inertia = eigen.eigenvalues; let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); @@ -204,3 +292,101 @@ impl AddAssign<MassProperties> for MassProperties { *self = *self + rhs } } + +impl approx::AbsDiffEq for MassProperties { + type Epsilon = f32; + fn default_epsilon() -> Self::Epsilon { + f32::default_epsilon() + } + + fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { + #[cfg(feature = "dim2")] + let inertia_is_ok = self + .inv_principal_inertia_sqrt + .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon); + + #[cfg(feature = "dim3")] + let inertia_is_ok = self + .reconstruct_inverse_inertia_matrix() + .abs_diff_eq(&other.reconstruct_inverse_inertia_matrix(), epsilon); + + inertia_is_ok + && self.local_com.abs_diff_eq(&other.local_com, epsilon) + && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon) + && self + .inv_principal_inertia_sqrt + .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon) + } +} + +impl approx::RelativeEq for MassProperties { + fn default_max_relative() -> Self::Epsilon { + f32::default_max_relative() + } + + fn relative_eq( + &self, + other: &Self, + epsilon: Self::Epsilon, + max_relative: Self::Epsilon, + ) -> bool { + #[cfg(feature = "dim2")] + let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq( + &other.inv_principal_inertia_sqrt, + epsilon, + max_relative, + ); + + #[cfg(feature = "dim3")] + let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq( + &other.reconstruct_inverse_inertia_matrix(), + epsilon, + max_relative, + ); + + inertia_is_ok + && self + .local_com + .relative_eq(&other.local_com, epsilon, max_relative) + && self + .inv_mass + .relative_eq(&other.inv_mass, epsilon, max_relative) + } +} + +#[cfg(test)] +mod test { + use super::MassProperties; + use crate::geometry::ColliderBuilder; + use approx::assert_relative_eq; + use num::Zero; + + #[test] + fn mass_properties_add_sub() { + // Check that addition and subtraction of mass properties behave as expected. + let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build(); + let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build(); + let c3 = ColliderBuilder::ball(5.0).build(); + + let m1 = c1.mass_properties(); + let m2 = c2.mass_properties(); + let m3 = c3.mass_properties(); + let m1m2m3 = m1 + m2 + m3; + + assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6); + assert_relative_eq!( + m1m2m3 - m1 - m2 - m3, + MassProperties::zero(), + epsilon = 1.0e-6 + ); + } +} diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 584dc96..a2fcacc 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -1,5 +1,5 @@ use crate::dynamics::MassProperties; -use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex}; +use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex}; use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector}; use crate::utils::{WCross, WDot}; use num::Zero; @@ -137,6 +137,28 @@ impl RigidBody { crate::utils::inv(self.mass_properties.inv_mass) } + /// Adds a collider to this rigid-body. + pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { + let mass_properties = coll + .mass_properties() + .transform_by(coll.position_wrt_parent()); + self.colliders.push(handle); + self.mass_properties += mass_properties; + self.update_world_mass_properties(); + } + + /// Removes a collider from this rigid-body. + pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { + if let Some(i) = self.colliders.iter().position(|e| *e == handle) { + self.colliders.swap_remove(i); + let mass_properties = coll + .mass_properties() + .transform_by(coll.position_wrt_parent()); + self.mass_properties -= mass_properties; + self.update_world_mass_properties(); + } + } + /// Put this rigid body to sleep. /// /// A sleeping body no longer moves and is no longer simulated by the physics engine unless @@ -171,7 +193,7 @@ impl RigidBody { } fn integrate_velocity(&self, dt: f32) -> Isometry<f32> { - let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses. + let com = &self.position * self.mass_properties.local_com; let shift = Translation::from(com.coords); shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 608a342..69fcf57 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -104,8 +104,8 @@ impl PositionConstraint { let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; for l in 0..manifold_points.len() { - local_p1[l] = manifold_points[l].local_p1 + shift1; - local_p2[l] = manifold_points[l].local_p2 + shift2; + local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1); + local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2); } let constraint = PositionConstraint { diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 8828c3d..0633926 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -51,6 +51,9 @@ impl WPositionConstraint { let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -85,8 +88,8 @@ impl WPositionConstraint { let local_p2 = Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]); - constraint.local_p1[i] = local_p1 + shift1; - constraint.local_p2[i] = local_p2 + shift2; + constraint.local_p1[i] = delta1 * (local_p1 + shift1); + constraint.local_p2[i] = delta2 * (local_p2 + shift2); } if push { diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index e6e83c6..dcd2d64 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -34,22 +34,30 @@ impl PositionGroundConstraint { let local_n1; let local_n2; + let delta1; + let delta2; if flip { std::mem::swap(&mut rb1, &mut rb2); local_n1 = manifold.local_n2; local_n2 = manifold.local_n1; + delta1 = &manifold.delta2; + delta2 = &manifold.delta1; } else { local_n1 = manifold.local_n1; local_n2 = manifold.local_n2; + delta1 = &manifold.delta1; + delta2 = &manifold.delta2; }; + let coll_pos1 = rb1.position * delta1; let shift1 = local_n1 * -manifold.kinematics.radius1; let shift2 = local_n2 * -manifold.kinematics.radius2; + let n1 = coll_pos1 * local_n1; let radius = manifold.kinematics.radius1 + manifold.kinematics.radius2 /* - params.allowed_linear_error */; - for (l, manifold_points) in manifold + for (l, manifold_contacts) in manifold .active_contacts() .chunks(MAX_MANIFOLD_POINTS) .enumerate() @@ -59,16 +67,16 @@ impl PositionGroundConstraint { if flip { // Don't forget that we already swapped rb1 and rb2 above. - // So if we flip, only manifold_points[k].{local_p1,local_p2} have to + // So if we flip, only manifold_contacts[k].{local_p1,local_p2} have to // be swapped. - for k in 0..manifold_points.len() { - p1[k] = rb1.predicted_position * (manifold_points[k].local_p2 + shift1); - local_p2[k] = manifold_points[k].local_p1 + shift2; + for k in 0..manifold_contacts.len() { + p1[k] = coll_pos1 * (manifold_contacts[k].local_p2 + shift1); + local_p2[k] = delta2 * (manifold_contacts[k].local_p1 + shift2); } } else { - for k in 0..manifold_points.len() { - p1[k] = rb1.predicted_position * (manifold_points[k].local_p1 + shift1); - local_p2[k] = manifold_points[k].local_p2 + shift2; + for k in 0..manifold_contacts.len() { + p1[k] = coll_pos1 * (manifold_contacts[k].local_p1 + shift1); + local_p2[k] = delta2 * (manifold_contacts[k].local_p2 + shift2); } } @@ -76,11 +84,11 @@ impl PositionGroundConstraint { rb2: rb2.active_set_offset, p1, local_p2, - n1: rb1.predicted_position * local_n1, + n1, radius, im2: rb2.mass_properties.inv_mass, ii2: rb2.world_inv_inertia_sqrt.squared(), - num_contacts: manifold_points.len() as u8, + num_contacts: manifold_contacts.len() as u8, erp: params.erp, max_linear_correction: params.max_linear_correction, }; diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index faa928b..e423c0a 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -54,16 +54,24 @@ impl WPositionGroundConstraint { array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH], ); + let delta1 = Isometry::from( + array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], + ); + let delta2 = Isometry::from( + array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], + ); + let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); - let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + let coll_pos1 = + delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; - let n1 = position1 * local_n1; + let n1 = coll_pos1 * local_n1; for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; @@ -90,8 +98,8 @@ impl WPositionGroundConstraint { array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH], ); - constraint.p1[i] = position1 * local_p1 - n1 * radius1; - constraint.local_p2[i] = local_p2 - local_n2 * radius2; + constraint.p1[i] = coll_pos1 * local_p1 - n1 * radius1; + constraint.local_p2[i] = delta2 * (local_p2 - local_n2 * radius2); } if push { diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 9212e89..190076d 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -148,7 +148,9 @@ impl VelocityConstraint { let rb2 = &bodies[manifold.body_pair.body2]; let mj_lambda1 = rb1.active_set_offset; let mj_lambda2 = rb2.active_set_offset; - let force_dir1 = rb1.position * (-manifold.local_n1); + let pos_coll1 = rb1.position * manifold.delta1; + let pos_coll2 = rb2.position * manifold.delta2; + let force_dir1 = pos_coll1 * (-manifold.local_n1); let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold @@ -215,10 +217,8 @@ impl VelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = (rb1.position * manifold_point.local_p1).coords - - rb1.position.translation.vector; - let dp2 = (rb2.position * manifold_point.local_p2).coords - - rb2.position.translation.vector; + let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com; + let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); @@ -355,7 +355,7 @@ impl VelocityConstraint { } } - // Solve penetration. + // Solve non-penetration. for i in 0..self.num_contacts as usize { let elt = &mut self.elements[i].normal_part; let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular) diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 8f387a5..f515d5e 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -72,6 +72,9 @@ impl WVelocityConstraint { let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let ii1: AngularInertia<SimdFloat> = AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); @@ -79,7 +82,8 @@ impl WVelocityConstraint { let linvel1 = Vector::from(array![|ii |