First working version of non-linear CCD based on single-substep motion-clamping.

8 files changed, 23 insertions, 23 deletions

diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index d0866bf..6ebf402 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -59,7 +59,7 @@ impl IslandSolver {
 
             counters.solver.velocity_update_time.resume();
             bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-                rb.integrate(params.dt)
+                rb.integrate_next_position(params.dt, true)
             });
             counters.solver.velocity_update_time.pause();
 
@@ -77,7 +77,7 @@ impl IslandSolver {
             bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
                 // Since we didn't run the velocity solver we need to integrate the accelerations here
                 rb.integrate_accelerations(params.dt);
-                rb.integrate(params.dt);
+                rb.integrate_next_position(params.dt, true);
             });
             counters.solver.velocity_update_time.pause();
         }
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
index 744c00d..93996f4 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
@@ -114,7 +114,7 @@ impl BallPositionGroundConstraint {
             // are the local_anchors. The rb1 and rb2 have
             // already been flipped by the caller.
             Self {
-                anchor1: rb1.predicted_position * cparams.local_anchor2,
+                anchor1: rb1.next_position * cparams.local_anchor2,
                 im2: rb2.effective_inv_mass,
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor1,
@@ -123,7 +123,7 @@ impl BallPositionGroundConstraint {
             }
         } else {
             Self {
-                anchor1: rb1.predicted_position * cparams.local_anchor1,
+                anchor1: rb1.next_position * cparams.local_anchor1,
                 im2: rb2.effective_inv_mass,
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor2,
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
index 043eea7..e9162a4 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
@@ -134,7 +134,7 @@ impl WBallPositionGroundConstraint {
         cparams: [&BallJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
-        let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
+        let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]);
         let anchor1 = position1
             * Point::from(array![|ii| if flipped[ii] {
                 cparams[ii].local_anchor2
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
index 7e8fc97..c98660f 100644
--- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
@@ -100,10 +100,10 @@ impl FixedPositionGroundConstraint {
         let local_anchor2;
 
         if flipped {
-            anchor1 = rb1.predicted_position * cparams.local_anchor2;
+            anchor1 = rb1.next_position * cparams.local_anchor2;
             local_anchor2 = cparams.local_anchor1;
         } else {
-            anchor1 = rb1.predicted_position * cparams.local_anchor1;
+            anchor1 = rb1.next_position * cparams.local_anchor1;
             local_anchor2 = cparams.local_anchor2;
         };
 
diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
index ea7e927..ed52a52 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
@@ -119,14 +119,14 @@ impl PrismaticPositionGroundConstraint {
         let local_axis2;
 
         if flipped {
-            frame1 = rb1.predicted_position * cparams.local_frame2();
+            frame1 = rb1.next_position * cparams.local_frame2();
             local_frame2 = cparams.local_frame1();
-            axis1 = rb1.predicted_position * cparams.local_axis2;
+            axis1 = rb1.next_position * cparams.local_axis2;
             local_axis2 = cparams.local_axis1;
         } else {
-            frame1 = rb1.predicted_position * cparams.local_frame1();
+            frame1 = rb1.next_position * cparams.local_frame1();
             local_frame2 = cparams.local_frame2();
-            axis1 = rb1.predicted_position * cparams.local_axis1;
+            axis1 = rb1.next_position * cparams.local_axis1;
             local_axis2 = cparams.local_axis2;
         };
 
diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
index 2da49e6..96391a2 100644
--- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
@@ -145,23 +145,23 @@ impl RevolutePositionGroundConstraint {
         let local_basis2;
 
         if flipped {
-            anchor1 = rb1.predicted_position * cparams.local_anchor2;
+            anchor1 = rb1.next_position * cparams.local_anchor2;
             local_anchor2 = cparams.local_anchor1;
-            axis1 = rb1.predicted_position * cparams.local_axis2;
+            axis1 = rb1.next_position * cparams.local_axis2;
             local_axis2 = cparams.local_axis1;
             basis1 = [
-                rb1.predicted_position * cparams.basis2[0],
-                rb1.predicted_position * cparams.basis2[1],
+                rb1.next_position * cparams.basis2[0],
+                rb1.next_position * cparams.basis2[1],
             ];
             local_basis2 = cparams.basis1;
         } else {
-            anchor1 = rb1.predicted_position * cparams.local_anchor1;
+            anchor1 = rb1.next_position * cparams.local_anchor1;
             local_anchor2 = cparams.local_anchor2;
-            axis1 = rb1.predicted_position * cparams.local_axis1;
+            axis1 = rb1.next_position * cparams.local_axis1;
             local_axis2 = cparams.local_axis2;
             basis1 = [
-                rb1.predicted_position * cparams.basis1[0],
-                rb1.predicted_position * cparams.basis1[1],
+                rb1.next_position * cparams.basis1[0],
+                rb1.next_position * cparams.basis1[1],
             ];
             local_basis2 = cparams.basis2;
         };
diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index 99c1ec5..f32f49f 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/src/dynamics/solver/parallel_island_solver.rs
@@ -277,7 +277,7 @@ impl ParallelIslandSolver {
                         rb.linvel += dvel.linear;
                         rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);
                         rb.integrate(params.dt);
-                        positions[rb.active_set_offset] = rb.position;
+                        positions[rb.active_set_offset] = rb.next_position;
                     }
                 }
 
@@ -298,7 +298,7 @@ impl ParallelIslandSolver {
                     let batch_size = thread.batch_size;
                     for handle in active_bodies[thread.position_writeback_index] {
                         let rb = &mut bodies[handle.0];
-                        rb.set_position_internal(positions[rb.active_set_offset]);
+                        rb.set_next_position(positions[rb.active_set_offset]);
                     }
                 }
             })
diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs
index df0e3fc..ea92c59 100644
--- a/src/dynamics/solver/position_solver.rs
+++ b/src/dynamics/solver/position_solver.rs
@@ -25,7 +25,7 @@ impl PositionSolver {
         self.positions.extend(
             bodies
                 .iter_active_island(island_id)
-                .map(|(_, b)| b.position),
+                .map(|(_, b)| b.next_position),
         );
 
         for _ in 0..params.max_position_iterations {
@@ -39,7 +39,7 @@ impl PositionSolver {
         }
 
         bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-            rb.set_position_internal(self.positions[rb.active_set_offset])
+            rb.set_next_position(self.positions[rb.active_set_offset])
         });
     }
 }