public static void ComputeEffectiveMass(ref BodyInertias inertiaA, ref BodyInertias inertiaB,
ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector <float> effectiveMassCFMScale, out Symmetric3x3Wide effectiveMass)
{
//Anchor points attached to each body are constrained to stay in the same position, yielding a position constraint of:
//C = positionA + anchorOffsetA - (positionB + anchorOffsetB) = 0
//C' = velocityA + d/dt(anchorOffsetA) - (velocityB + d/dt(anchorOffsetB)) = 0
//C' = velocityA + angularVelocity x anchorOffsetA - (velocityB + angularVelocityB x anchorOffsetB) = 0
//C' = velocityA * I + angularVelocity * skewSymmetric(anchorOffsetA) - velocityB * I - angularVelocityB * skewSymmetric(anchorOffsetB) = 0
//So, the jacobians:
//LinearA: I
//AngularA: skewSymmetric(anchorOffsetA)
//LinearB: -I
//AngularB: skewSymmetric(-anchorOffsetB)
//Each of these is a 3x3 matrix. However, we don't need to explicitly compute or store any of these.
//Not storing the identity matrix is obvious enough, but we can also just store the offset rather than the full skew symmetric matrix for the angular jacobians.
//The skew symmetric matrix is replaced by a simple cross product.
//The projection should strive to contain the smallest possible representation necessary for the constraint to work.
//We can sorta-kinda make use of the baking-inertia-into-jacobian trick, because I * softenedEffectiveMass is... softenedEffectiveMass.
//Likewise, J * inverseInertia is just the scalar inverseMasses for the two linear contributions.
//Note that there's no reason to try to bake the softenedEffectiveMass into the angular jacobian, because we already have to perform at least one
//softenedEffectiveMass multiply due to the linear components. We can just store the raw angular jacobians and piggyback on the linear component effective mass multiply.
//So, the question becomes... is there a way to bundle inverseInertia into the angularJacobians?
//Yes, but it's not useful. If we premultiply the skew(offset) * inverseInertia for CSIToWSV, the result is no longer symmetric.
//That means we would have to store 3 additional scalars per body compared to the symmetric inverse inertias. That's not particularly useful;
//it only saves a cross product. Loading 6 more scalars to save 2 cross products (12 multiplies, 6 adds) is a terrible trade, even at SIMD128.
Symmetric3x3Wide.SkewSandwichWithoutOverlap(offsetA, inertiaA.InverseInertiaTensor, out var inverseEffectiveMass);
//Note that the jacobian is technically skewSymmetric(-OffsetB), but the sign doesn't matter due to the sandwich.
Symmetric3x3Wide.SkewSandwichWithoutOverlap(offsetB, inertiaB.InverseInertiaTensor, out var angularBContribution);
Symmetric3x3Wide.Add(inverseEffectiveMass, angularBContribution, out inverseEffectiveMass);
//Linear contributions are simply I * inverseMass * I, which is just boosting the diagonal.
var linearContribution = inertiaA.InverseMass + inertiaB.InverseMass;
inverseEffectiveMass.XX += linearContribution;
inverseEffectiveMass.YY += linearContribution;
inverseEffectiveMass.ZZ += linearContribution;
Symmetric3x3Wide.Invert(inverseEffectiveMass, out effectiveMass);
Symmetric3x3Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass);
//NOTE:
//It's possible to use local inertia tensors diagonalized to only 3 scalars per body. Given that we load orientations as a part of the prestep and so could reconstruct
//the world inertia tensor, this would save 6 scalar loads. In isolation, that's a likely win. However, there are some other considerations:
//1) Some common constraints, notably including the contact constraints, do not load pose. They can either load diagonalized inertia with orientation (7 total scalars per body)
//or a symmetric world inertia tensor (6 scalars). Given the relatively high cost of incoherent gathers, the 6 scalar world inertia gather would be used.
//2) If constraints are loading from both local and world inertias rather than one or the other, the number of cache misses increases.
//3) It requires that the local space of physicsShapes3D is aligned with the moments of inertia. While this could be done automatically when creating physicsShapes3D, and while
//all primitive physicsShapes3D are built with diagonal inertia tensors, it is likely that the local 'reorientation' required by diagonalization applied to things like
//convex hulls or meshes would be confusing for users. Recentering certainly was in V1.
//4) It cannot be used to save space on explicitly stored inertias in constraints, because the warmstart/solve do not load the pose (and, as above, loading orientation and
//local inertia is 1 scalar more than just the world inertia).
//5) While the local diagonal representation does offer some possibilities for ALU-level optimizations in the prestep, the effective mass matrix will be in world space,
//and in general it will not be any smaller than the usual symmetric representation.
}
public void Do()
{
Symmetric3x3Wide.SkewSandwichWithoutOverlap(v, triangular, out result);
}