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

Add support of 64-bits reals.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-01-04 15:14:25 +01:00
parent a1aa8855f7
commit aa61fe65e3
55 changed files with 656 additions and 518 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
};
use crate::math::{AngularInertia, Vector};
use crate::math::{AngularInertia, Real, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
@@ -13,20 +13,20 @@ pub(crate) struct RevoluteVelocityConstraint {
joint_id: JointIndex,
r1: Vector<f32>,
r2: Vector<f32>,
r1: Vector<Real>,
r2: Vector<Real>,
inv_lhs: Matrix5<f32>,
rhs: Vector5<f32>,
impulse: Vector5<f32>,
inv_lhs: Matrix5<Real>,
rhs: Vector5<Real>,
impulse: Vector5<Real>,
basis1: Matrix3x2<f32>,
basis1: Matrix3x2<Real>,
im1: f32,
im2: f32,
im1: Real,
im2: Real,
ii1_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<f32>,
ii1_sqrt: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
}
impl RevoluteVelocityConstraint {
@@ -99,7 +99,7 @@ impl RevoluteVelocityConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -120,7 +120,7 @@ impl RevoluteVelocityConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
@@ -165,17 +165,17 @@ pub(crate) struct RevoluteVelocityGroundConstraint {
joint_id: JointIndex,
r2: Vector<f32>,
r2: Vector<Real>,
inv_lhs: Matrix5<f32>,
rhs: Vector5<f32>,
impulse: Vector5<f32>,
inv_lhs: Matrix5<Real>,
rhs: Vector5<Real>,
impulse: Vector5<Real>,
basis1: Matrix3x2<f32>,
basis1: Matrix3x2<Real>,
im2: f32,
im2: Real,
ii2_sqrt: AngularInertia<f32>,
ii2_sqrt: AngularInertia<Real>,
}
impl RevoluteVelocityGroundConstraint {
@@ -249,7 +249,7 @@ impl RevoluteVelocityGroundConstraint {
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
@@ -263,7 +263,7 @@ impl RevoluteVelocityGroundConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);