aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics/solver/joint_constraint/fixed_position_constraint.rs')
-rw-r--r--src/dynamics/solver/joint_constraint/fixed_position_constraint.rs52
1 files changed, 26 insertions, 26 deletions
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
index 24001cd..7e8fc97 100644
--- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
@@ -1,30 +1,30 @@
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
-use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
use crate::utils::WAngularInertia;
#[derive(Debug)]
pub(crate) struct FixedPositionConstraint {
position1: usize,
position2: usize,
- local_anchor1: Isometry<f32>,
- local_anchor2: Isometry<f32>,
- local_com1: Point<f32>,
- local_com2: Point<f32>,
- im1: f32,
- im2: f32,
- ii1: AngularInertia<f32>,
- ii2: AngularInertia<f32>,
-
- lin_inv_lhs: f32,
- ang_inv_lhs: AngularInertia<f32>,
+ local_anchor1: Isometry<Real>,
+ local_anchor2: Isometry<Real>,
+ local_com1: Point<Real>,
+ local_com2: Point<Real>,
+ im1: Real,
+ im2: Real,
+ ii1: AngularInertia<Real>,
+ ii2: AngularInertia<Real>,
+
+ lin_inv_lhs: Real,
+ ang_inv_lhs: AngularInertia<Real>,
}
impl FixedPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
- let ii1 = rb1.world_inv_inertia_sqrt.squared();
- let ii2 = rb2.world_inv_inertia_sqrt.squared();
- let im1 = rb1.mass_properties.inv_mass;
- let im2 = rb2.mass_properties.inv_mass;
+ let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+ let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+ let im1 = rb1.effective_inv_mass;
+ let im2 = rb2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();
@@ -44,7 +44,7 @@ impl FixedPositionConstraint {
}
}
- pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+ pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position1 = positions[self.position1 as usize];
let mut position2 = positions[self.position2 as usize];
@@ -81,12 +81,12 @@ impl FixedPositionConstraint {
#[derive(Debug)]
pub(crate) struct FixedPositionGroundConstraint {
position2: usize,
- anchor1: Isometry<f32>,
- local_anchor2: Isometry<f32>,
- local_com2: Point<f32>,
- im2: f32,
- ii2: AngularInertia<f32>,
- impulse: f32,
+ anchor1: Isometry<Real>,
+ local_anchor2: Isometry<Real>,
+ local_com2: Point<Real>,
+ im2: Real,
+ ii2: AngularInertia<Real>,
+ impulse: Real,
}
impl FixedPositionGroundConstraint {
@@ -111,14 +111,14 @@ impl FixedPositionGroundConstraint {
anchor1,
local_anchor2,
position2: rb2.active_set_offset,
- im2: rb2.mass_properties.inv_mass,
- ii2: rb2.world_inv_inertia_sqrt.squared(),
+ im2: rb2.effective_inv_mass,
+ ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com2: rb2.mass_properties.local_com,
impulse: 0.0,
}
}
- pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+ pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
// Angular correction.