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

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

182
src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
@@ -34,33 +35,46 @@ impl WBallVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
let anchor_world1 = position1 * local_anchor1;
let anchor_world2 = position2 * local_anchor2;
@@ -114,20 +128,16 @@ impl WBallVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
mj_lambda1.linear += self.impulse * self.im1;
@@ -147,20 +157,16 @@ impl WBallVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -214,33 +220,49 @@ impl WBallVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
let anchor_world1 = position1 * local_anchor1;
let anchor_world2 = position2 * local_anchor2;
@@ -287,12 +309,10 @@ impl WBallVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
mj_lambda2.linear -= self.impulse * self.im2;
@@ -306,12 +326,10 @@ impl WBallVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);