Attempt to combine the position constraints initialization with the velocity constraints initialization.

src/dynamics/solver/categorization.rs

@@ -5,9 +5,7 @@ pub(crate) fn categorize_position_contacts(
     bodies: &RigidBodySet,
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
-    out_point_point_ground: &mut Vec<ContactManifoldIndex>,
     out_plane_point_ground: &mut Vec<ContactManifoldIndex>,
-    out_point_point: &mut Vec<ContactManifoldIndex>,
     out_plane_point: &mut Vec<ContactManifoldIndex>,
 ) {
     for manifold_i in manifold_indices {
@@ -16,15 +14,9 @@ pub(crate) fn categorize_position_contacts(
         let rb2 = &bodies[manifold.data.body_pair.body2];
 
         if !rb1.is_dynamic() || !rb2.is_dynamic() {
-            match manifold.kinematics.category {
+            out_plane_point_ground.push(*manifold_i)
-                KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i),
-                KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i),
-            }
         } else {
-            match manifold.kinematics.category {
+            out_plane_point.push(*manifold_i)
-                KinematicsCategory::PointPoint => out_point_point.push(*manifold_i),
-                KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i),
-            }
         }
     }
 }

src/dynamics/solver/island_solver.rs

@@ -29,6 +29,7 @@ impl IslandSolver {
     ) {
         if manifold_indices.len() != 0 || joint_indices.len() != 0 {
             counters.solver.velocity_assembly_time.resume();
+            self.position_solver.part.constraints.clear();
             self.velocity_solver.init_constraints(
                 island_id,
                 params,
@@ -37,6 +38,7 @@ impl IslandSolver {
                 &manifold_indices,
                 joints,
                 &joint_indices,
+                &mut self.position_solver.part.constraints,
             );
             counters.solver.velocity_assembly_time.pause();
 

src/dynamics/solver/position_solver.rs

@@ -33,13 +33,9 @@ impl PositionSolverJointPart {
 }
 
 pub(crate) struct PositionSolverPart {
-    pub point_point_manifolds: Vec<ContactManifoldIndex>,
     pub plane_point_manifolds: Vec<ContactManifoldIndex>,
-    pub point_point_ground_manifolds: Vec<ContactManifoldIndex>,
     pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>,
-    pub point_point_groups: InteractionGroups,
     pub plane_point_groups: InteractionGroups,
-    pub point_point_ground_groups: InteractionGroups,
     pub plane_point_ground_groups: InteractionGroups,
     pub constraints: Vec<AnyPositionConstraint>,
 }
@@ -47,13 +43,9 @@ pub(crate) struct PositionSolverPart {
 impl PositionSolverPart {
     pub fn new() -> Self {
         Self {
-            point_point_manifolds: Vec::new(),
             plane_point_manifolds: Vec::new(),
-            point_point_ground_manifolds: Vec::new(),
             plane_point_ground_manifolds: Vec::new(),
-            point_point_groups: InteractionGroups::new(),
             plane_point_groups: InteractionGroups::new(),
-            point_point_ground_groups: InteractionGroups::new(),
             plane_point_ground_groups: InteractionGroups::new(),
             constraints: Vec::new(),
         }
@@ -62,7 +54,7 @@ impl PositionSolverPart {
 
 pub(crate) struct PositionSolver {
     positions: Vec<Isometry<f32>>,
-    part: PositionSolverPart,
+    pub part: PositionSolverPart,
     joint_part: PositionSolverJointPart,
 }
 
@@ -85,8 +77,8 @@ impl PositionSolver {
         joints: &[JointGraphEdge],
         joint_constraint_indices: &[JointIndex],
     ) {
-        self.part
+        // self.part
-            .init_constraints(island_id, params, bodies, manifolds, manifold_indices);
+        //     .init_constraints(island_id, params, bodies, manifolds, manifold_indices);
         self.joint_part.init_constraints(
             island_id,
             params,
@@ -134,27 +126,16 @@ impl PositionSolverPart {
         manifolds_all: &[&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
     ) {
-        self.point_point_ground_manifolds.clear();
         self.plane_point_ground_manifolds.clear();
-        self.point_point_manifolds.clear();
         self.plane_point_manifolds.clear();
         categorize_position_contacts(
             bodies,
             manifolds_all,
             manifold_indices,
-            &mut self.point_point_ground_manifolds,
             &mut self.plane_point_ground_manifolds,
-            &mut self.point_point_manifolds,
             &mut self.plane_point_manifolds,
         );
 
-        self.point_point_groups.clear_groups();
-        self.point_point_groups.group_manifolds(
-            island_id,
-            bodies,
-            manifolds_all,
-            &self.point_point_manifolds,
-        );
         self.plane_point_groups.clear_groups();
         self.plane_point_groups.group_manifolds(
             island_id,
@@ -163,13 +144,6 @@ impl PositionSolverPart {
             &self.plane_point_manifolds,
         );
 
-        self.point_point_ground_groups.clear_groups();
-        self.point_point_ground_groups.group_manifolds(
-            island_id,
-            bodies,
-            manifolds_all,
-            &self.point_point_ground_manifolds,
-        );
         self.plane_point_ground_groups.clear_groups();
         self.plane_point_ground_groups.group_manifolds(
             island_id,
@@ -185,13 +159,6 @@ impl PositionSolverPart {
          */
         #[cfg(feature = "simd-is-enabled")]
         {
-            compute_grouped_constraints(
-                params,
-                bodies,
-                manifolds_all,
-                &self.point_point_groups.grouped_interactions,
-                &mut self.constraints,
-            );
             compute_grouped_constraints(
                 params,
                 bodies,
@@ -200,13 +167,6 @@ impl PositionSolverPart {
                 &mut self.constraints,
             );
         }
-        compute_nongrouped_constraints(
-            params,
-            bodies,
-            manifolds_all,
-            &self.point_point_groups.nongrouped_interactions,
-            &mut self.constraints,
-        );
         compute_nongrouped_constraints(
             params,
             bodies,
@@ -220,13 +180,6 @@ impl PositionSolverPart {
          */
         #[cfg(feature = "simd-is-enabled")]
         {
-            compute_grouped_ground_constraints(
-                params,
-                bodies,
-                manifolds_all,
-                &self.point_point_ground_groups.grouped_interactions,
-                &mut self.constraints,
-            );
             compute_grouped_ground_constraints(
                 params,
                 bodies,
@@ -235,13 +188,6 @@ impl PositionSolverPart {
                 &mut self.constraints,
             );
         }
-        compute_nongrouped_ground_constraints(
-            params,
-            bodies,
-            manifolds_all,
-            &self.point_point_ground_groups.nongrouped_interactions,
-            &mut self.constraints,
-        );
         compute_nongrouped_ground_constraints(
             params,
             bodies,

src/dynamics/solver/velocity_solver.rs

@@ -4,6 +4,10 @@ use super::{
 #[cfg(feature = "simd-is-enabled")]
 use super::{WVelocityConstraint, WVelocityGroundConstraint};
 use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts};
+use crate::dynamics::solver::{
+    AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, WPositionConstraint,
+    WPositionGroundConstraint,
+};
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
     IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
@@ -37,9 +41,16 @@ impl VelocitySolver {
         manifold_indices: &[ContactManifoldIndex],
         joints: &[JointGraphEdge],
         joint_constraint_indices: &[JointIndex],
+        position_constraints: &mut Vec<AnyPositionConstraint>,
     ) {
-        self.contact_part
+        self.contact_part.init_constraints(
-            .init_constraints(island_id, params, bodies, manifolds, manifold_indices);
+            island_id,
+            params,
+            bodies,
+            manifolds,
+            manifold_indices,
+            position_constraints,
+        );
         self.joint_part.init_constraints(
             island_id,
             params,
@@ -173,19 +184,25 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
         bodies: &RigidBodySet,
         manifolds: &[&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
+        position_constraints: &mut Vec<AnyPositionConstraint>,
     ) {
         self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices);
         self.constraints.clear();
         #[cfg(feature = "simd-is-enabled")]
         {
-            self.compute_grouped_constraints(params, bodies, manifolds);
+            self.compute_grouped_constraints(params, bodies, manifolds, position_constraints);
         }
-        self.compute_nongrouped_constraints(params, bodies, manifolds);
+        self.compute_nongrouped_constraints(params, bodies, manifolds, position_constraints);
         #[cfg(feature = "simd-is-enabled")]
         {
-            self.compute_grouped_ground_constraints(params, bodies, manifolds);
+            self.compute_grouped_ground_constraints(
+                params,
+                bodies,
+                manifolds,
+                position_constraints,
+            );
         }
-        self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
+        self.compute_nongrouped_ground_constraints(params, bodies, manifolds, position_constraints);
     }
 
     #[cfg(feature = "simd-is-enabled")]
@@ -194,6 +211,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
         params: &IntegrationParameters,
         bodies: &RigidBodySet,
         manifolds_all: &[&mut ContactManifold],
+        constraints_all: &mut Vec<AnyPositionConstraint>,
     ) {
         for manifolds_i in self
             .interaction_groups
@@ -210,6 +228,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
                 &mut self.constraints,
                 true,
             );
+            WPositionConstraint::generate(params, manifolds, bodies, constraints_all, true);
         }
     }
 
@@ -218,6 +237,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
         params: &IntegrationParameters,
         bodies: &RigidBodySet,
         manifolds_all: &[&mut ContactManifold],
+        constraints_all: &mut Vec<AnyPositionConstraint>,
     ) {
         for manifold_i in &self.interaction_groups.nongrouped_interactions {
             let manifold = &manifolds_all[*manifold_i];
@@ -229,6 +249,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
                 &mut self.constraints,
                 true,
             );
+            PositionConstraint::generate(params, manifold, bodies, constraints_all, true);
         }
     }
 
@@ -238,6 +259,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
         params: &IntegrationParameters,
         bodies: &RigidBodySet,
         manifolds_all: &[&mut ContactManifold],
+        constraints_all: &mut Vec<AnyPositionConstraint>,
     ) {
         for manifolds_i in self
             .ground_interaction_groups
@@ -254,6 +276,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
                 &mut self.constraints,
                 true,
             );
+            WPositionGroundConstraint::generate(params, manifolds, bodies, constraints_all, true);
         }
     }
 
@@ -262,6 +285,7 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
         params: &IntegrationParameters,
         bodies: &RigidBodySet,
         manifolds_all: &[&mut ContactManifold],
+        constraints_all: &mut Vec<AnyPositionConstraint>,
     ) {
         for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
             let manifold = &manifolds_all[*manifold_i];
@@ -272,7 +296,8 @@ impl VelocitySolverPart<AnyVelocityConstraint> {
                 bodies,
                 &mut self.constraints,
                 true,
-            )
+            );
+            PositionGroundConstraint::generate(params, manifold, bodies, constraints_all, true)
         }
     }
 }