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

Use WReal instead of SimdRealField everywhere

Browse Source
This commit is contained in:
Sébastien Crozet
2022-02-21 21:19:30 +01:00
parent 19a00885d6
commit 15b165893c
7 changed files with 41 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/dynamics/solver/delta_vel.rs
View File

@@ -1,6 +1,6 @@
use crate::math::{AngVector, Vector, SPATIAL_DIM};
use na::{DVectorSlice, DVectorSliceMut};
use na::{Scalar, SimdRealField};
use crate::utils::WReal;
use na::{DVectorSlice, DVectorSliceMut, Scalar};
use std::ops::{AddAssign, Sub};
#[derive(Copy, Clone, Debug, Default)]
@@ -29,7 +29,7 @@ impl<N: Scalar + Copy> DeltaVel<N> {
}
}
impl<N: SimdRealField + Copy> DeltaVel<N> {
impl<N: WReal> DeltaVel<N> {
pub fn zero() -> Self {
Self {
linear: na::zero(),
@@ -38,14 +38,14 @@ impl<N: SimdRealField + Copy> DeltaVel<N> {
}
}
impl<N: SimdRealField + Copy> AddAssign for DeltaVel<N> {
impl<N: WReal> AddAssign for DeltaVel<N> {
fn add_assign(&mut self, rhs: Self) {
self.linear += rhs.linear;
self.angular += rhs.angular;
}
}
impl<N: SimdRealField + Copy> Sub for DeltaVel<N> {
impl<N: WReal> Sub for DeltaVel<N> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {

11
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -3,7 +3,6 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
use crate::utils::{WDot, WReal};
use simba::simd::SimdRealField;
#[cfg(feature = "simd-is-enabled")]
use {
@@ -12,7 +11,7 @@ use {
};
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct MotorParameters<N: SimdRealField> {
pub struct MotorParameters<N: WReal> {
pub stiffness: N,
pub damping: N,
pub gamma: N,
@@ -22,7 +21,7 @@ pub struct MotorParameters<N: SimdRealField> {
pub max_impulse: N,
}
impl<N: SimdRealField> Default for MotorParameters<N> {
impl<N: WReal> Default for MotorParameters<N> {
fn default() -> Self {
Self {
stiffness: N::zero(),
@@ -48,7 +47,7 @@ pub enum WritebackId {
// the solver, to avoid fetching data from the rigid-body set
// every time.
#[derive(Copy, Clone)]
pub struct SolverBody<N: SimdRealField, const LANES: usize> {
pub struct SolverBody<N: WReal, const LANES: usize> {
pub linvel: Vector<N>,
pub angvel: AngVector<N>,
pub im: Vector<N>,
@@ -58,7 +57,7 @@ pub struct SolverBody<N: SimdRealField, const LANES: usize> {
}
#[derive(Debug, Copy, Clone)]
pub struct JointVelocityConstraint<N: SimdRealField, const LANES: usize> {
pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
pub mj_lambda1: [usize; LANES],
pub mj_lambda2: [usize; LANES],
@@ -339,7 +338,7 @@ impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
}
#[derive(Debug, Copy, Clone)]
pub struct JointVelocityGroundConstraint<N: SimdRealField, const LANES: usize> {
pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
pub mj_lambda2: [usize; LANES],
pub joint_id: [JointIndex; LANES],

3
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
View File

@@ -7,10 +7,9 @@ use crate::dynamics::{IntegrationParameters, JointIndex};
use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
use na::SMatrix;
use simba::simd::SimdRealField;
#[derive(Debug, Copy, Clone)]
pub struct JointVelocityConstraintBuilder<N: SimdRealField> {
pub struct JointVelocityConstraintBuilder<N: WReal> {
pub basis: Matrix<N>,
pub cmat1_basis: SMatrix<N, ANG_DIM, DIM>,
pub cmat2_basis: SMatrix<N, ANG_DIM, DIM>,

9
src/dynamics/solver/velocity_constraint.rs
View File

@@ -5,7 +5,7 @@ use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot, WReal};
use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
@@ -373,8 +373,7 @@ pub(crate) fn compute_tangent_contact_directions<N>(
linvel2: &Vector<N>,
) -> [Vector<N>; DIM - 1]
where
N: na::SimdRealField + Copy,
N::Element: na::RealField + Copy,
N: WReal,
Vector<N>: WBasis,
{
use na::SimdValue;
@@ -392,8 +391,8 @@ where
tangent_relative_linvel.normalize_mut()
};
let threshold: N::Element = na::convert(1.0e-4);
let use_fallback = tangent_linvel_norm.simd_lt(N::splat(threshold));
const THRESHOLD: Real = 1.0e-4;
let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD));
let tangent_fallback = force_dir1.orthonormal_vector();
let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);

17
src/dynamics/solver/velocity_constraint_element.rs
View File

@@ -1,10 +1,9 @@
use super::DeltaVel;
use crate::math::{AngVector, Vector, DIM};
use crate::utils::{WBasis, WDot};
use na::SimdRealField;
use crate::utils::{WBasis, WDot, WReal};
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
pub(crate) struct VelocityConstraintTangentPart<N: WReal> {
pub gcross1: [AngVector<N>; DIM - 1],
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
@@ -18,7 +17,7 @@ pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
pub r: [N; DIM],
}
impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
impl<N: WReal> VelocityConstraintTangentPart<N> {
fn zero() -> Self {
Self {
gcross1: [na::zero(); DIM - 1],
@@ -43,7 +42,6 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
mj_lambda2: &mut DeltaVel<N>,
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
N::Element: SimdRealField + Copy,
{
#[cfg(feature = "dim2")]
{
@@ -107,7 +105,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraintNormalPart<N: SimdRealField + Copy> {
pub(crate) struct VelocityConstraintNormalPart<N: WReal> {
pub gcross1: AngVector<N>,
pub gcross2: AngVector<N>,
pub rhs: N,
@@ -116,7 +114,7 @@ pub(crate) struct VelocityConstraintNormalPart<N: SimdRealField + Copy> {
pub r: N,
}
impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
impl<N: WReal> VelocityConstraintNormalPart<N> {
fn zero() -> Self {
Self {
gcross1: na::zero(),
@@ -157,12 +155,12 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityConstraintElement<N: SimdRealField + Copy> {
pub(crate) struct VelocityConstraintElement<N: WReal> {
pub normal_part: VelocityConstraintNormalPart<N>,
pub tangent_part: VelocityConstraintTangentPart<N>,
}
impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
impl<N: WReal> VelocityConstraintElement<N> {
pub fn zero() -> Self {
Self {
normal_part: VelocityConstraintNormalPart::zero(),
@@ -186,7 +184,6 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
) where
Vector<N>: WBasis,
AngVector<N>: WDot<AngVector<N>, Result = N>,
N::Element: SimdRealField + Copy,
{
// Solve penetration.
if solve_normal {

17
src/dynamics/solver/velocity_ground_constraint_element.rs
View File

@@ -1,10 +1,9 @@
use super::DeltaVel;
use crate::math::{AngVector, Vector, DIM};
use crate::utils::{WBasis, WDot};
use na::SimdRealField;
use crate::utils::{WBasis, WDot, WReal};
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
pub(crate) struct VelocityGroundConstraintTangentPart<N: WReal> {
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
#[cfg(feature = "dim2")]
@@ -17,7 +16,7 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
pub r: [N; DIM],
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
impl<N: WReal> VelocityGroundConstraintTangentPart<N> {
fn zero() -> Self {
Self {
gcross2: [na::zero(); DIM - 1],
@@ -39,7 +38,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
mj_lambda2: &mut DeltaVel<N>,
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
N::Element: SimdRealField + Copy,
{
#[cfg(feature = "dim2")]
{
@@ -89,7 +87,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
pub(crate) struct VelocityGroundConstraintNormalPart<N: WReal> {
pub gcross2: AngVector<N>,
pub rhs: N,
pub rhs_wo_bias: N,
@@ -97,7 +95,7 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
pub r: N,
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
fn zero() -> Self {
Self {
gcross2: na::zero(),
@@ -129,12 +127,12 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct VelocityGroundConstraintElement<N: SimdRealField + Copy> {
pub(crate) struct VelocityGroundConstraintElement<N: WReal> {
pub normal_part: VelocityGroundConstraintNormalPart<N>,
pub tangent_part: VelocityGroundConstraintTangentPart<N>,
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
impl<N: WReal> VelocityGroundConstraintElement<N> {
pub fn zero() -> Self {
Self {
normal_part: VelocityGroundConstraintNormalPart::zero(),
@@ -156,7 +154,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
) where
Vector<N>: WBasis,
AngVector<N>: WDot<AngVector<N>, Result = N>,
N::Element: SimdRealField + Copy,
{
// Solve penetration.
if solve_normal {