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

Add prelude + use vectors for setting linvel/translation in builders

Browse Source
This commit is contained in:
Crozet Sébastien
2021-05-25 11:00:13 +02:00
parent 47139323e0
commit 1bef66fea9
93 changed files with 1528 additions and 1259 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
View File

@@ -8,8 +8,8 @@ use crate::utils::WAngularInertia;
pub(crate) struct FixedPositionConstraint {
position1: usize,
position2: usize,
local_anchor1: Isometry<Real>,
local_anchor2: Isometry<Real>,
local_frame1: Isometry<Real>,
local_frame2: Isometry<Real>,
local_com1: Point<Real>,
local_com2: Point<Real>,
im1: Real,
@@ -38,8 +38,8 @@ impl FixedPositionConstraint {
let ang_inv_lhs = (ii1 + ii2).inverse();
Self {
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
local_frame1: cparams.local_frame1,
local_frame2: cparams.local_frame2,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
im1,
@@ -58,8 +58,8 @@ impl FixedPositionConstraint {
let mut position2 = positions[self.position2 as usize];
// Angular correction.
let anchor1 = position1 * self.local_anchor1;
let anchor2 = position2 * self.local_anchor2;
let anchor1 = position1 * self.local_frame1;
let anchor2 = position2 * self.local_frame2;
let ang_err = anchor2.rotation * anchor1.rotation.inverse();
#[cfg(feature = "dim3")]
let ang_impulse = self
@@ -75,8 +75,8 @@ impl FixedPositionConstraint {
Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
// Linear correction.
let anchor1 = position1 * Point::from(self.local_anchor1.translation.vector);
let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector);
let anchor1 = position1 * Point::from(self.local_frame1.translation.vector);
let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
let err = anchor2 - anchor1;
let impulse = err * (self.lin_inv_lhs * params.joint_erp);
position1.translation.vector += self.im1 * impulse;
@@ -91,7 +91,7 @@ impl FixedPositionConstraint {
pub(crate) struct FixedPositionGroundConstraint {
position2: usize,
anchor1: Isometry<Real>,
local_anchor2: Isometry<Real>,
local_frame2: Isometry<Real>,
local_com2: Point<Real>,
im2: Real,
ii2: AngularInertia<Real>,
@@ -109,19 +109,19 @@ impl FixedPositionGroundConstraint {
let (mprops2, ids2) = rb2;
let anchor1;
let local_anchor2;
let local_frame2;
if flipped {
anchor1 = poss1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
anchor1 = poss1.next_position * cparams.local_frame2;
local_frame2 = cparams.local_frame1;
} else {
anchor1 = poss1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
anchor1 = poss1.next_position * cparams.local_frame1;
local_frame2 = cparams.local_frame2;
};
Self {
anchor1,
local_anchor2,
local_frame2,
position2: ids2.active_set_offset,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
@@ -134,13 +134,13 @@ impl FixedPositionGroundConstraint {
let mut position2 = positions[self.position2 as usize];
// Angular correction.
let anchor2 = position2 * self.local_anchor2;
let anchor2 = position2 * self.local_frame2;
let ang_err = anchor2.rotation * self.anchor1.rotation.inverse();
position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
// Linear correction.
let anchor1 = Point::from(self.anchor1.translation.vector);
let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector);
let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
let err = anchor2 - anchor1;
// NOTE: no need to divide by im2 just to multiply right after.
let impulse = err * params.joint_erp;

12
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -63,8 +63,8 @@ impl FixedVelocityConstraint {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor1 = poss1.position * cparams.local_anchor1;
let anchor2 = poss2.position * cparams.local_anchor2;
let anchor1 = poss1.position * cparams.local_frame1;
let anchor2 = poss2.position * cparams.local_frame2;
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
@@ -280,13 +280,13 @@ impl FixedVelocityGroundConstraint {
let (anchor1, anchor2) = if flipped {
(
poss1.position * cparams.local_anchor2,
poss2.position * cparams.local_anchor1,
poss1.position * cparams.local_frame2,
poss2.position * cparams.local_frame1,
)
} else {
(
poss1.position * cparams.local_anchor1,
poss2.position * cparams.local_anchor2,
poss1.position * cparams.local_frame1,
poss2.position * cparams.local_frame2,
)
};

24
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

@@ -91,12 +91,12 @@ impl WFixedVelocityConstraint {
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1]);
let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
let anchor1 = position1 * local_frame1;
let anchor2 = position2 * local_frame2;
let ii1 = ii1_sqrt.squared();
let ii2 = ii2_sqrt.squared();
let r1 = anchor1.translation.vector - world_com1.coords;
@@ -363,20 +363,20 @@ impl WFixedVelocityGroundConstraint {
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
let local_frame1 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_frame2
} else {
cparams[ii].local_anchor1
cparams[ii].local_frame1
}]);
let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
let local_frame2 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_frame1
} else {
cparams[ii].local_anchor2
cparams[ii].local_frame2
}]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
let anchor1 = position1 * local_frame1;
let anchor2 = position2 * local_frame2;
let ii2 = ii2_sqrt.squared();
let r1 = anchor1.translation.vector - world_com1.coords;
let r2 = anchor2.translation.vector - world_com2.coords;