Implement non-linear position stabilization for the generic constraint.

1 files changed, 251 insertions, 19 deletions

diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
index 9d74bf3..1502403 100644
--- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
@@ -5,7 +5,7 @@ use crate::math::{
     AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
     DIM,
 };
-use crate::utils::{WAngularInertia, WCross};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Vector3, Vector6};
 
 // FIXME: review this code for the case where the center of masses are not the origin.
@@ -48,16 +48,136 @@ impl GenericPositionConstraint {
     }
 
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        return;
-    }
+        let mut position1 = positions[self.position1];
+        let mut position2 = positions[self.position2];
+        let mut params = *params;
+        params.joint_erp = 0.8;
+
+        /*
+         *
+         * Translation part.
+         *
+         */
+        {
+            let anchor1 = position1 * self.joint.local_anchor1;
+            let anchor2 = position2 * self.joint.local_anchor2;
+            let basis = anchor1.rotation;
+            let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1;
+            let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
+            let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
+            let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
+
+            let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.xyz(),
+                &self.joint.max_position.xyz(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let rmat1 = r1.gcross_matrix() * rotmat;
+            let rmat2 = r2.gcross_matrix() * rotmat;
+
+            // Will be actually inverted right after.
+            // TODO: we should keep the SdpMatrix3 type.
+            let delassus = (self.ii1.quadform(&rmat1).add_diagonal(self.im1)
+                + self.ii2.quadform(&rmat2).add_diagonal(self.im2))
+            .into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot1 = self.ii1.transform_vector(r1.gcross(impulse));
+            let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
+
+            position1.translation.vector += self.im1 * impulse;
+            position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
+            position2.translation.vector -= self.im2 * impulse;
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        /*
+         *
+         * Rotation part
+         *
+         */
+        {
+            let anchor1 = position1 * self.joint.local_anchor1;
+            let anchor2 = position2 * self.joint.local_anchor2;
+            let basis = anchor1.rotation;
+            let mut min_pos_impulse = self
+                .joint
+                .min_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+            let mut max_pos_impulse = self
+                .joint
+                .max_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+
+            let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
+                &self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            // Will be actually inverted right after.
+            // TODO: we should keep the SdpMatrix3 type.
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot1 = self.ii1.transform_vector(impulse);
+            let rot2 = self.ii2.transform_vector(impulse);
+
+            position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
 
-    pub fn solve2(
-        &self,
-        params: &IntegrationParameters,
-        positions: &mut [Isometry<Real>],
-        dpos: &mut [DeltaVel<Real>],
-    ) {
-        return;
+        positions[self.position1] = position1;
+        positions[self.position2] = position2;
     }
 }
 
@@ -102,15 +222,127 @@ impl GenericPositionGroundConstraint {
     }
 
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
-        return;
-    }
+        let mut position2 = positions[self.position2];
+        let mut params = *params;
+        params.joint_erp = 0.8;
+
+        /*
+         *
+         * Translation part.
+         *
+         */
+        {
+            let anchor1 = self.anchor1;
+            let anchor2 = position2 * self.local_anchor2;
+            let basis = anchor1.rotation;
+            let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
+            let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
+            let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
+
+            let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.xyz(),
+                &self.joint.max_position.xyz(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let rmat2 = r2.gcross_matrix() * rotmat;
+
+            // Will be actually inverted right after.
+            // TODO: we should keep the SdpMatrix3 type.
+            let delassus = self
+                .ii2
+                .quadform(&rmat2)
+                .add_diagonal(self.im2)
+                .into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
+
+            position2.translation.vector -= self.im2 * impulse;
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        /*
+         *
+         * Rotation part
+         *
+         */
+        {
+            let anchor1 = self.anchor1;
+            let anchor2 = position2 * self.local_anchor2;
+            let basis = anchor1.rotation;
+            let mut min_pos_impulse = self
+                .joint
+                .min_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+            let mut max_pos_impulse = self
+                .joint
+                .max_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+
+            let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
+                &self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            // Will be actually inverted right after.
+            // TODO: we should keep the SdpMatrix3 type.
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let delassus = self.ii2.quadform(&rotmat).into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+            let rot2 = self.ii2.transform_vector(impulse);
+
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
 
-    pub fn solve2(
-        &self,
-        params: &IntegrationParameters,
-        positions: &mut [Isometry<Real>],
-        dpos: &mut [DeltaVel<Real>],
-    ) {
-        return;
+        positions[self.position2] = position2;
     }
 }