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

Outsource the Shape trait, wquadtree, and shape types.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-14 15:51:43 +01:00
parent 9bf1321f8f
commit cc6d1b9730
47 changed files with 444 additions and 3363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, SIMD_WIDTH};
use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use simba::simd::SimdValue;
@@ -10,17 +10,17 @@ pub(crate) struct WBallPositionConstraint {
position1: [usize; SIMD_WIDTH],
position2: [usize; SIMD_WIDTH],
local_com1: Point<SimdFloat>,
local_com2: Point<SimdFloat>,
local_com1: Point<SimdReal>,
local_com2: Point<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1: AngularInertia<SimdFloat>,
ii2: AngularInertia<SimdFloat>,
ii1: AngularInertia<SimdReal>,
ii2: AngularInertia<SimdReal>,
local_anchor1: Point<SimdFloat>,
local_anchor2: Point<SimdFloat>,
local_anchor1: Point<SimdReal>,
local_anchor2: Point<SimdReal>,
}
impl WBallPositionConstraint {
@@ -31,13 +31,13 @@ impl WBallPositionConstraint {
) -> Self {
let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
let ii2 = AngularInertia::<SimdFloat>::from(
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
@@ -97,7 +97,7 @@ impl WBallPositionConstraint {
};
let inv_lhs = lhs.inverse_unchecked();
let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
position1.translation.vector += impulse * self.im1;
position2.translation.vector -= impulse * self.im2;
@@ -120,11 +120,11 @@ impl WBallPositionConstraint {
#[derive(Debug)]
pub(crate) struct WBallPositionGroundConstraint {
position2: [usize; SIMD_WIDTH],
anchor1: Point<SimdFloat>,
im2: SimdFloat,
ii2: AngularInertia<SimdFloat>,
local_anchor2: Point<SimdFloat>,
local_com2: Point<SimdFloat>,
anchor1: Point<SimdReal>,
im2: SimdReal,
ii2: AngularInertia<SimdReal>,
local_anchor2: Point<SimdReal>,
local_com2: Point<SimdReal>,
}
impl WBallPositionGroundConstraint {
@@ -141,8 +141,8 @@ impl WBallPositionGroundConstraint {
} else {
cparams[ii].local_anchor1
}; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
@@ -186,7 +186,7 @@ impl WBallPositionGroundConstraint {
};
let inv_lhs = lhs.inverse_unchecked();
let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
position2.translation.vector -= impulse * self.im2;
let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));