First public release of Rapier.v0.1.0

34 files changed, 9763 insertions, 0 deletions

diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
new file mode 100644
index 0000000..9ddf791
--- /dev/null
+++ b/src/dynamics/solver/categorization.rs
@@ -0,0 +1,70 @@
+use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory};
+
+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 {
+        let manifold = &manifolds[*manifold_i];
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            match manifold.kinematics.category {
+                KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i),
+                KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i),
+            }
+        } else {
+            match manifold.kinematics.category {
+                KinematicsCategory::PointPoint => out_point_point.push(*manifold_i),
+                KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i),
+            }
+        }
+    }
+}
+
+pub(crate) fn categorize_velocity_contacts(
+    bodies: &RigidBodySet,
+    manifolds: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    out_ground: &mut Vec<ContactManifoldIndex>,
+    out_not_ground: &mut Vec<ContactManifoldIndex>,
+) {
+    for manifold_i in manifold_indices {
+        let manifold = &manifolds[*manifold_i];
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            out_ground.push(*manifold_i)
+        } else {
+            out_not_ground.push(*manifold_i)
+        }
+    }
+}
+
+pub(crate) fn categorize_joints(
+    bodies: &RigidBodySet,
+    joints: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    ground_joints: &mut Vec<JointIndex>,
+    nonground_joints: &mut Vec<JointIndex>,
+) {
+    for joint_i in joint_indices {
+        let joint = &joints[*joint_i].weight;
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            ground_joints.push(*joint_i);
+        } else {
+            nonground_joints.push(*joint_i);
+        }
+    }
+}
diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs
new file mode 100644
index 0000000..c4a424b
--- /dev/null
+++ b/src/dynamics/solver/delta_vel.rs
@@ -0,0 +1,18 @@
+use crate::math::{AngVector, Vector};
+use na::{Scalar, SimdRealField};
+
+#[derive(Copy, Clone, Debug)]
+//#[repr(align(64))]
+pub(crate) struct DeltaVel<N: Scalar> {
+    pub linear: Vector<N>,
+    pub angular: AngVector<N>,
+}
+
+impl<N: SimdRealField> DeltaVel<N> {
+    pub fn zero() -> Self {
+        Self {
+            linear: na::zero(),
+            angular: na::zero(),
+        }
+    }
+}
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
new file mode 100644
index 0000000..04acaaf
--- /dev/null
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -0,0 +1,434 @@
+use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
+    vec_map::VecMap,
+};
+
+pub(crate) trait PairInteraction {
+    fn body_pair(&self) -> BodyPair;
+}
+
+impl<'a> PairInteraction for &'a mut ContactManifold {
+    fn body_pair(&self) -> BodyPair {
+        self.body_pair
+    }
+}
+
+impl<'a> PairInteraction for JointGraphEdge {
+    fn body_pair(&self) -> BodyPair {
+        BodyPair::new(self.weight.body1, self.weight.body2)
+    }
+}
+
+#[cfg(feature = "parallel")]
+pub(crate) struct ParallelInteractionGroups {
+    bodies_color: Vec<u128>,         // Workspace.
+    interaction_indices: Vec<usize>, // Workspace.
+    interaction_colors: Vec<usize>,  // Workspace.
+    sorted_interactions: Vec<usize>,
+    groups: Vec<usize>,
+}
+
+#[cfg(feature = "parallel")]
+impl ParallelInteractionGroups {
+    pub fn new() -> Self {
+        Self {
+            bodies_color: Vec::new(),
+            interaction_indices: Vec::new(),
+            interaction_colors: Vec::new(),
+            sorted_interactions: Vec::new(),
+            groups: Vec::new(),
+        }
+    }
+
+    pub fn group(&self, i: usize) -> &[usize] {
+        let range = self.groups[i]..self.groups[i + 1];
+        &self.sorted_interactions[range]
+    }
+    pub fn num_groups(&self) -> usize {
+        self.groups.len() - 1
+    }
+
+    pub fn group_interactions<Interaction: PairInteraction>(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[Interaction],
+        interaction_indices: &[usize],
+    ) {
+        let num_island_bodies = bodies.active_island(island_id).len();
+        self.bodies_color.clear();
+        self.interaction_indices.clear();
+        self.groups.clear();
+        self.sorted_interactions.clear();
+        self.interaction_colors.clear();
+
+        let mut color_len = [0; 128];
+        self.bodies_color.resize(num_island_bodies, 0u128);
+        self.interaction_indices
+            .extend_from_slice(interaction_indices);
+        self.interaction_colors.resize(interaction_indices.len(), 0);
+        let bcolors = &mut self.bodies_color;
+
+        for (interaction_id, color) in self
+            .interaction_indices
+            .iter()
+            .zip(self.interaction_colors.iter_mut())
+        {
+            let body_pair = interactions[*interaction_id].body_pair();
+            let rb1 = &bodies[body_pair.body1];
+            let rb2 = &bodies[body_pair.body2];
+
+            match (rb1.is_static(), rb2.is_static()) {
+                (false, false) => {
+                    let color_mask =
+                        bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb1.active_set_offset] |= 1 << *color;
+                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                }
+                (true, false) => {
+                    let color_mask = bcolors[rb2.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                }
+                (false, true) => {
+                    let color_mask = bcolors[rb1.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb1.active_set_offset] |= 1 << *color;
+                }
+                (true, true) => unreachable!(),
+            }
+        }
+
+        let mut sort_offsets = [0; 128];
+        let mut last_offset = 0;
+
+        for i in 0..128 {
+            if color_len[i] == 0 {
+                break;
+            }
+
+            self.groups.push(last_offset);
+            sort_offsets[i] = last_offset;
+            last_offset += color_len[i];
+        }
+
+        self.sorted_interactions
+            .resize(interaction_indices.len(), 0);
+
+        for (interaction_id, color) in interaction_indices
+            .iter()
+            .zip(self.interaction_colors.iter())
+        {
+            self.sorted_interactions[sort_offsets[*color]] = *interaction_id;
+            sort_offsets[*color] += 1;
+        }
+
+        self.groups.push(self.sorted_interactions.len());
+    }
+}
+
+pub(crate) struct InteractionGroups {
+    #[cfg(feature = "simd-is-enabled")]
+    buckets: VecMap<([usize; SIMD_WIDTH], usize)>,
+    #[cfg(feature = "simd-is-enabled")]
+    body_masks: Vec<u128>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub grouped_interactions: Vec<usize>,
+    pub nongrouped_interactions: Vec<usize>,
+}
+
+impl InteractionGroups {
+    pub fn new() -> Self {
+        Self {
+            #[cfg(feature = "simd-is-enabled")]
+            buckets: VecMap::new(),
+            #[cfg(feature = "simd-is-enabled")]
+            body_masks: Vec::new(),
+            #[cfg(feature = "simd-is-enabled")]
+            grouped_interactions: Vec::new(),
+            nongrouped_interactions: Vec::new(),
+        }
+    }
+
+    // FIXME: there is a lot of duplicated code with group_manifolds here.
+    // But we don't refactor just now because we may end up with distinct
+    // grouping strategies in the future.
+    #[cfg(not(feature = "simd-is-enabled"))]
+    pub fn group_joints(
+        &mut self,
+        _island_id: usize,
+        _bodies: &RigidBodySet,
+        _interactions: &[JointGraphEdge],
+        interaction_indices: &[JointIndex],
+    ) {
+        self.nongrouped_interactions
+            .extend_from_slice(interaction_indices);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn group_joints(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[JointGraphEdge],
+        interaction_indices: &[JointIndex],
+    ) {
+        // NOTE: in 3D we have up to 10 different joint types.
+        // In 2D we only have 5 joint types.
+        #[cfg(feature = "dim3")]
+        const NUM_JOINT_TYPES: usize = 10;
+        #[cfg(feature = "dim2")]
+        const NUM_JOINT_TYPES: usize = 5;
+
+        // The j-th bit of joint_type_conflicts[i] indicates that the
+        // j-th bucket contains a joint with a type different than `i`.
+        let mut joint_type_conflicts = [0u128; NUM_JOINT_TYPES];
+
+        // Note: each bit of a body mask indicates what bucket already contains
+        // a constraints involving this body.
+        // FIXME: currently, this is a bit overconservative because when a bucket
+        // is full, we don't clear the corresponding body mask bit. This may result
+        // in less grouped constraints.
+        self.body_masks
+            .resize(bodies.active_island(island_id).len(), 0u128);
+
+        // NOTE: each bit of the occupied mask indicates what bucket already
+        // contains at least one constraint.
+        let mut occupied_mask = 0u128;
+
+        for interaction_i in interaction_indices {
+            let interaction = &interactions[*interaction_i].weight;
+            let body1 = &bodies[interaction.body1];
+            let body2 = &bodies[interaction.body2];
+            let is_static1 = !body1.is_dynamic();
+            let is_static2 = !body2.is_dynamic();
+
+            if is_static1 && is_static2 {
+                continue;
+            }
+
+            let ijoint = interaction.params.type_id();
+            let i1 = body1.active_set_offset;
+            let i2 = body2.active_set_offset;
+            let conflicts =
+                self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
+            let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
+            let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
+
+            let target_index = if conflictfree_occupied_targets != 0 {
+                // Try to fill partial WContacts first.
+                conflictfree_occupied_targets.trailing_zeros()
+            } else {
+                conflictfree_targets.trailing_zeros()
+            };
+
+            if target_index == 128 {
+                // The interaction conflicts with every bucket we can manage.
+                // So push it in an nongrouped interaction list that won't be combined with
+                // any other interactions.
+                self.nongrouped_interactions.push(*interaction_i);
+                continue;
+            }
+
+            let target_mask_bit = 1 << target_index;
+
+            let bucket = self
+                .buckets
+                .entry(target_index as usize)
+                .or_insert_with(|| ([0; SIMD_WIDTH], 0));
+
+            if bucket.1 == SIMD_LAST_INDEX {
+                // We completed our group.
+                (bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
+                self.grouped_interactions.extend_from_slice(&bucket.0);
+                bucket.1 = 0;
+                occupied_mask &= !target_mask_bit;
+
+                for k in 0..NUM_JOINT_TYPES {
+                    joint_type_conflicts[k] &= !target_mask_bit;
+                }
+            } else {
+                (bucket.0)[bucket.1] = *interaction_i;
+                bucket.1 += 1;
+                occupied_mask |= target_mask_bit;
+
+                for k in 0..ijoint {
+                    joint_type_conflicts[k] |= target_mask_bit;
+                }
+                for k in ijoint + 1..NUM_JOINT_TYPES {
+                    joint_type_conflicts[k] |= target_mask_bit;
+                }
+            }
+
+            // NOTE: static bodies don't transmit forces. Therefore they don't
+            // imply any interaction conflicts.
+            if !is_static1 {
+                self.body_masks[i1] |= target_mask_bit;
+            }
+
+            if !is_static2 {
+                self.body_masks[i2] |= target_mask_bit;
+            }
+        }
+
+        self.nongrouped_interactions.extend(
+            self.buckets
+                .values()
+                .flat_map(|e| e.0.iter().take(e.1).copied()),
+        );
+        self.buckets.clear();
+        self.body_masks.iter_mut().for_each(|e| *e = 0);
+
+        assert!(
+            self.grouped_interactions.len() % SIMD_WIDTH == 0,
+            "Invalid SIMD contact grouping."
+        );
+
+        //        println!(
+        //            "Num grouped interactions: {}, nongrouped: {}",
+        //            self.grouped_interactions.len(),
+        //            self.nongrouped_interactions.len()
+        //        );
+    }
+
+    pub fn clear_groups(&mut self) {
+        #[cfg(feature = "simd-is-enabled")]
+        self.grouped_interactions.clear();
+        self.nongrouped_interactions.clear();
+    }
+
+    #[cfg(not(feature = "simd-is-enabled"))]
+    pub fn group_manifolds(
+        &mut self,
+        _island_id: usize,
+        _bodies: &RigidBodySet,
+        _interactions: &[&mut ContactManifold],
+        interaction_indices: &[ContactManifoldIndex],
+    ) {
+        self.nongrouped_interactions
+            .extend_from_slice(interaction_indices);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn group_manifolds(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[&mut ContactManifold],
+        interaction_indices: &[ContactManifoldIndex],
+    ) {
+        // Note: each bit of a body mask indicates what bucket already contains
+        // a constraints involving this body.
+        // FIXME: currently, this is a bit overconservative because when a bucket
+        // is full, we don't clear the corresponding body mask bit. This may result
+        // in less grouped contacts.
+        // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
+        self.body_masks
+            .resize(bodies.active_island(island_id).len(), 0u128);
+
+        // NOTE: each bit of the occupied mask indicates what bucket already
+        // contains at least one constraint.
+        let mut occupied_mask = 0u128;
+        let max_interaction_points = interaction_indices
+            .iter()
+            .map(|i| interactions[*i].num_active_contacts())
+            .max()
+            .unwrap_or(1);
+
+        // FIXME: find a way to reduce the number of iteration.
+        // There must be a way to iterate just once on every interaction indices
+        // instead of MAX_MANIFOLD_POINTS times.
+        for k in 1..=max_interaction_points {
+            for interaction_i in interaction_indices {
+                let interaction = &interactions[*interaction_i];
+
+                // FIXME: how could we avoid iterating
+                // on each interaction at every iteration on k?
+                if interaction.num_active_contacts() != k {
+                    continue;
+                }
+
+                let body1 = &bodies[interaction.body_pair.body1];
+                let body2 = &bodies[interaction.body_pair.body2];
+                let is_static1 = !body1.is_dynamic();
+                let is_static2 = !body2.is_dynamic();
+
+                // FIXME: don't generate interactions between static bodies in the first place.
+                if is_static1 && is_static2 {
+                    continue;
+                }
+
+                let i1 = body1.active_set_offset;
+                let i2 = body2.active_set_offset;
+                let conflicts = self.body_masks[i1] | self.body_masks[i2];
+                let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
+                let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
+
+                let target_index = if conflictfree_occupied_targets != 0 {
+                    // Try to fill partial WContacts first.
+                    conflictfree_occupied_targets.trailing_zeros()
+                } else {
+                    conflictfree_targets.trailing_zeros()
+                };
+
+                if target_index == 128 {
+                    // The interaction conflicts with every bucket we can manage.
+                    // So push it in an nongrouped interaction list that won't be combined with
+                    // any other interactions.
+                    self.nongrouped_interactions.push(*interaction_i);
+                    continue;
+                }
+
+                let target_mask_bit = 1 << target_index;
+
+                let bucket = self
+                    .buckets
+                    .entry(target_index as usize)
+                    .or_insert_with(|| ([0; SIMD_WIDTH], 0));
+
+                if bucket.1 == SIMD_LAST_INDEX {
+                    // We completed our group.
+                    (bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
+                    self.grouped_interactions.extend_from_slice(&bucket.0);
+                    bucket.1 = 0;
+                    occupied_mask = occupied_mask & (!target_mask_bit);
+                } else {
+                    (bucket.0)[bucket.1] = *interaction_i;
+                    bucket.1 += 1;
+                    occupied_mask = occupied_mask | target_mask_bit;
+                }
+
+                // NOTE: static bodies don't transmit forces. Therefore they don't
+                // imply any interaction conflicts.
+                if !is_static1 {
+                    self.body_masks[i1] |= target_mask_bit;
+                }
+
+                if !is_static2 {
+                    self.body_masks[i2] |= target_mask_bit;
+                }
+            }
+
+            self.nongrouped_interactions.extend(
+                self.buckets
+                    .values()
+                    .flat_map(|e| e.0.iter().take(e.1).copied()),
+            );
+            self.buckets.clear();
+            self.body_masks.iter_mut().for_each(|e| *e = 0);
+            occupied_mask = 0u128;
+        }
+
+        assert!(
+            self.grouped_interactions.len() % SIMD_WIDTH == 0,
+            "Invalid SIMD contact grouping."
+        );
+    }
+}
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
new file mode 100644
index 0000000..7ce142a
--- /dev/null
+++ b/src/dynamics/solver/island_solver.rs
@@ -0,0 +1,73 @@
+use super::{PositionSolver, VelocitySolver};
+use crate::counters::Counters;
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+
+pub struct IslandSolver {
+    velocity_solver: VelocitySolver,
+    position_solver: PositionSolver,
+}
+
+impl IslandSolver {
+    pub fn new() -> Self {
+        Self {
+            velocity_solver: VelocitySolver::new(),
+            position_solver: PositionSolver::new(),
+        }
+    }
+
+    pub fn solve_island(
+        &mut self,
+        island_id: usize,
+        counters: &mut Counters,
+        params: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+        joints: &mut [JointGraphEdge],
+        joint_indices: &[JointIndex],
+    ) {
+        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
+            counters.solver.velocity_assembly_time.resume();
+            self.velocity_solver.init_constraints(
+                island_id,
+                params,
+                bodies,
+                manifolds,
+                &manifold_indices,
+                joints,
+                &joint_indices,
+            );
+            counters.solver.velocity_assembly_time.pause();
+
+            counters.solver.velocity_resolution_time.resume();
+            self.velocity_solver
+                .solve_constraints(island_id, params, bodies, manifolds, joints);
+            counters.solver.velocity_resolution_time.pause();
+
+            counters.solver.position_assembly_time.resume();
+            self.position_solver.init_constraints(
+                island_id,
+                params,
+                bodies,
+                manifolds,
+                &manifold_indices,
+                joints,
+                &joint_indices,
+            );
+            counters.solver.position_assembly_time.pause();
+        }
+
+        counters.solver.velocity_update_time.resume();
+        bodies
+            .foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt()));
+        counters.solver.velocity_update_time.pause();
+
+        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
+            counters.solver.position_resolution_time.resume();
+            self.position_solver
+                .solve_constraints(island_id, params, bodies);
+            counters.solver.position_resolution_time.pause();
+        }
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
new file mode 100644
index 0000000..21a537e
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
@@ -0,0 +1,165 @@
+use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
+#[cfg(feature = "dim2")]
+use crate::math::SdpMatrix;
+use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+
+#[derive(Debug)]
+pub(crate) struct BallPositionConstraint {
+    position1: usize,
+    position2: usize,
+
+    local_com1: Point<f32>,
+    local_com2: Point<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    local_anchor1: Point<f32>,
+    local_anchor2: Point<f32>,
+}
+
+impl BallPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
+        Self {
+            local_com1: rb1.mass_properties.local_com,
+            local_com2: rb2.mass_properties.local_com,
+            im1: rb1.mass_properties.inv_mass,
+            im2: rb2.mass_properties.inv_mass,
+            ii1: rb1.world_inv_inertia_sqrt.squared(),
+            ii2: rb2.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,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = positions[self.position1 as usize];
+        let mut position2 = positions[self.position2 as usize];
+
+        let anchor1 = position1 * self.local_anchor1;
+        let anchor2 = position2 * self.local_anchor2;
+
+        let com1 = position1 * self.local_com1;
+        let com2 = position2 * self.local_com2;
+
+        let err = anchor1 - anchor2;
+
+        let centered_anchor1 = anchor1 - com1;
+        let centered_anchor2 = anchor2 - com2;
+
+        let cmat1 = centered_anchor1.gcross_matrix();
+        let cmat2 = centered_anchor2.gcross_matrix();
+
+        // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose()
+        // because it is anti-symmetric.
+        #[cfg(feature = "dim3")]
+        let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1)
+            + self.ii2.quadform(&cmat2).add_diagonal(self.im2);
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way. It is also
+        // faster because in 2D lhs will be symmetric.
+        #[cfg(feature = "dim2")]
+        let lhs = {
+            let m11 =
+                self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2;
+            let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2;
+            let m22 =
+                self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2;
+            SdpMatrix::new(m11, m12, m22)
+        };
+