Experiment with incremental island computation.

13 files changed, 40 insertions, 40 deletions

diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
index 744c00d..6708d44 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
@@ -33,8 +33,8 @@ impl BallPositionConstraint {
             ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
+            position1: rb1.island_offset,
+            position2: rb2.island_offset,
         }
     }
 
@@ -118,7 +118,7 @@ impl BallPositionGroundConstraint {
                 im2: rb2.effective_inv_mass,
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor1,
-                position2: rb2.active_set_offset,
+                position2: rb2.island_offset,
                 local_com2: rb2.mass_properties.local_com,
             }
         } else {
@@ -127,7 +127,7 @@ impl BallPositionGroundConstraint {
                 im2: rb2.effective_inv_mass,
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor2,
-                position2: rb2.active_set_offset,
+                position2: rb2.island_offset,
                 local_com2: rb2.mass_properties.local_com,
             }
         }
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..08fb96c 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
@@ -43,8 +43,8 @@ impl WBallPositionConstraint {
         .squared();
         let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
         let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
-        let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
-        let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let position1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH];
+        let position2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
 
         Self {
             local_com1,
@@ -151,7 +151,7 @@ impl WBallPositionGroundConstraint {
         } else {
             cparams[ii].local_anchor2
         }; SIMD_WIDTH]);
-        let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let position2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
         let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
 
         Self {
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index e75f978..2791747 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
@@ -79,8 +79,8 @@ impl BallVelocityConstraint {
 
         BallVelocityConstraint {
             joint_id,
-            mj_lambda1: rb1.active_set_offset,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda1: rb1.island_offset,
+            mj_lambda2: rb2.island_offset,
             im1,
             im2,
             impulse: cparams.impulse * params.warmstart_coeff,
@@ -201,7 +201,7 @@ impl BallVelocityGroundConstraint {
 
         BallVelocityGroundConstraint {
             joint_id,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda2: rb2.island_offset,
             im2,
             impulse: cparams.impulse * params.warmstart_coeff,
             r2: anchor2,
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
index 95b0bb5..a18bccb 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
@@ -46,7 +46,7 @@ impl WBallVelocityConstraint {
         let ii1_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH];
 
         let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
@@ -56,7 +56,7 @@ impl WBallVelocityConstraint {
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
 
         let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
         let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
@@ -232,7 +232,7 @@ impl WBallVelocityGroundConstraint {
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
 
         let local_anchor2 = Point::from(
             array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
index 7e8fc97..5e0b485 100644
--- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
@@ -31,8 +31,8 @@ impl FixedPositionConstraint {
         Self {
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
+            position1: rb1.island_offset,
+            position2: rb2.island_offset,
             im1,
             im2,
             ii1,
@@ -110,7 +110,7 @@ impl FixedPositionGroundConstraint {
         Self {
             anchor1,
             local_anchor2,
-            position2: rb2.active_set_offset,
+            position2: rb2.island_offset,
             im2: rb2.effective_inv_mass,
             ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
             local_com2: rb2.mass_properties.local_com,
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
index 2868d4b..2f284a7 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
@@ -112,8 +112,8 @@ impl FixedVelocityConstraint {
 
         FixedVelocityConstraint {
             joint_id,
-            mj_lambda1: rb1.active_set_offset,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda1: rb1.island_offset,
+            mj_lambda2: rb2.island_offset,
             im1,
             im2,
             ii1,
@@ -301,7 +301,7 @@ impl FixedVelocityGroundConstraint {
 
         FixedVelocityGroundConstraint {
             joint_id,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda2: rb2.island_offset,
             im2,
             ii2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
index c423272..0090a8b 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
@@ -65,7 +65,7 @@ impl WFixedVelocityConstraint {
         let ii1_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH];
 
         let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
@@ -75,7 +75,7 @@ impl WFixedVelocityConstraint {
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
 
         let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
         let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
@@ -319,7 +319,7 @@ impl WFixedVelocityGroundConstraint {
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
 
         let local_anchor1 = Isometry::from(
             array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
index ea7e927..f03804a 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
@@ -46,8 +46,8 @@ impl PrismaticPositionConstraint {
             local_frame2: cparams.local_frame2(),
             local_axis1: cparams.local_axis1,
             local_axis2: cparams.local_axis2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
+            position1: rb1.island_offset,
+            position2: rb2.island_offset,
             limits: cparams.limits,
         }
     }
@@ -135,7 +135,7 @@ impl PrismaticPositionGroundConstraint {
             local_frame2,
             axis1,
             local_axis2,
-            position2: rb2.active_set_offset,
+            position2: rb2.island_offset,
             limits: cparams.limits,
         }
     }
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
index d1943fc..6c8e9b6 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
@@ -173,8 +173,8 @@ impl PrismaticVelocityConstraint {
 
         PrismaticVelocityConstraint {
             joint_id,
-            mj_lambda1: rb1.active_set_offset,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda1: rb1.island_offset,
+            mj_lambda2: rb2.island_offset,
             im1,
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             im2,
@@ -463,7 +463,7 @@ impl PrismaticVelocityGroundConstraint {
 
         PrismaticVelocityGroundConstraint {
             joint_id,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda2: rb2.island_offset,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
index f24cfa5..f084d60 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
@@ -77,7 +77,7 @@ impl WPrismaticVelocityConstraint {
         let ii1_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH];
 
         let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
@@ -87,7 +87,7 @@ impl WPrismaticVelocityConstraint {
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
 
         let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
         let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
@@ -435,7 +435,7 @@ impl WPrismaticVelocityGroundConstraint {
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
 
         #[cfg(feature = "dim2")]
         let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
index 19dd451..d9f6cff 100644
--- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
@@ -44,8 +44,8 @@ impl RevolutePositionConstraint {
             local_anchor2: cparams.local_anchor2,
             local_axis1: cparams.local_axis1,
             local_axis2: cparams.local_axis2,
-            position1: rb1.active_set_offset,
-            position2: rb2.active_set_offset,
+            position1: rb1.island_offset,
+            position2: rb2.island_offset,
         }
     }
 
@@ -118,7 +118,7 @@ impl RevolutePositionGroundConstraint {
             local_anchor2,
             axis1,
             local_axis2,
-            position2: rb2.active_set_offset,
+            position2: rb2.island_offset,
         }
     }
 
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
index 38f56d9..22fd916 100644
--- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
@@ -84,8 +84,8 @@ impl RevoluteVelocityConstraint {
 
         RevoluteVelocityConstraint {
             joint_id,
-            mj_lambda1: rb1.active_set_offset,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda1: rb1.island_offset,
+            mj_lambda2: rb2.island_offset,
             im1,
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             basis1,
@@ -238,7 +238,7 @@ impl RevoluteVelocityGroundConstraint {
 
         RevoluteVelocityGroundConstraint {
             joint_id,
-            mj_lambda2: rb2.active_set_offset,
+            mj_lambda2: rb2.island_offset,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
index 822c2ac..fbfec3b 100644
--- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
@@ -47,7 +47,7 @@ impl WRevoluteVelocityConstraint {
         let ii1_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH];
 
         let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
@@ -57,7 +57,7 @@ impl WRevoluteVelocityConstraint {
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
 
         let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
         let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
@@ -266,7 +266,7 @@ impl WRevoluteVelocityGroundConstraint {
         let ii2_sqrt = AngularInertia::<SimdReal>::from(
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
-        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH];
         let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
 
         let local_anchor1 = Point::from(