///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
Vector3f.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Vector3f.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
Vector3f.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1);
Vector3f.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2);
Vector3f error;
Vector3f.Cross(ref worldAxisA, ref worldAxisB, out error);
Vector3f.Dot(ref error, ref worldConstrainedAxis1, out this.error.X);
Vector3f.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
errorReduction = -errorReduction;
biasVelocity.X = errorReduction * this.error.X;
biasVelocity.Y = errorReduction * this.error.Y;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared;
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
}
Vector3f axis1I, axis2I;
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3f inertiaTensorSum;
Matrix3f.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum);
Vector3f.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I);
Vector3f.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Vector3f.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I);
Vector3f.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Vector3f.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I);
Vector3f.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
Vector3f.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11);
Vector3f.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12);
Vector3f.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21);
Vector3f.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22);
effectiveMassMatrix.M11 += softness;
effectiveMassMatrix.M22 += softness;
Matrix2f.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
Matrix2f.Negate(ref effectiveMassMatrix, out effectiveMassMatrix);
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
entityADynamic = entityA != null && entityA.isDynamic;
entityBDynamic = entityB != null && entityB.isDynamic;
contactCount = contactManifoldConstraint.penetrationConstraints.Count;
switch (contactCount)
{
case 1:
manifoldCenter = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position;
break;
case 2:
Vector3f.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
manifoldCenter.X *= .5f;
manifoldCenter.Y *= .5f;
manifoldCenter.Z *= .5f;
break;
case 3:
Vector3f.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
Vector3f.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
ref manifoldCenter,
out manifoldCenter);
manifoldCenter.X *= .333333333f;
manifoldCenter.Y *= .333333333f;
manifoldCenter.Z *= .333333333f;
break;
case 4:
//This isn't actually the center of the manifold. Is it good enough? Sure seems like it.
Vector3f.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
Vector3f.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
ref manifoldCenter,
out manifoldCenter);
Vector3f.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[3].contact.Position,
ref manifoldCenter,
out manifoldCenter);
manifoldCenter.X *= .25f;
manifoldCenter.Y *= .25f;
manifoldCenter.Z *= .25f;
break;
default:
manifoldCenter = VectorHelper.NoVector;
break;
}
//Compute the three dimensional relative velocity at the point.
Vector3f velocityA, velocityB;
if (entityA != null)
{
Vector3f.Subtract(ref manifoldCenter, ref entityA.position, out ra);
Vector3f.Cross(ref entityA.angularVelocity, ref ra, out velocityA);
Vector3f.Add(ref velocityA, ref entityA.linearVelocity, out velocityA);
}
else
{
velocityA = new Vector3f();
}
if (entityB != null)
{
Vector3f.Subtract(ref manifoldCenter, ref entityB.position, out rb);
Vector3f.Cross(ref entityB.angularVelocity, ref rb, out velocityB);
Vector3f.Add(ref velocityB, ref entityB.linearVelocity, out velocityB);
}
else
{
velocityB = new Vector3f();
}
Vector3f.Subtract(ref velocityA, ref velocityB, out relativeVelocity);
//Get rid of the normal velocity.
Vector3f normal = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Normal;
float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y + normal.Z * relativeVelocity.Z;
relativeVelocity.X -= normalVelocityScalar * normal.X;
relativeVelocity.Y -= normalVelocityScalar * normal.Y;
relativeVelocity.Z -= normalVelocityScalar * normal.Z;
//Create the jacobian entry and decide the friction coefficient.
float length = relativeVelocity.LengthSquared;
if (length > MathHelper.Epsilon)
{
length = (float)Math.Sqrt(length);
float inverseLength = 1 / length;
linearA.M11 = relativeVelocity.X * inverseLength;
linearA.M12 = relativeVelocity.Y * inverseLength;
linearA.M13 = relativeVelocity.Z * inverseLength;
friction = length > CollisionResponseSettings.StaticFrictionVelocityThreshold ?
contactManifoldConstraint.materialInteraction.KineticFriction :
contactManifoldConstraint.materialInteraction.StaticFriction;
}
else
{
friction = contactManifoldConstraint.materialInteraction.StaticFriction;
//If there was no velocity, try using the previous frame's jacobian... if it exists.
//Reusing an old one is okay since jacobians are cleared when a contact is initialized.
if (!(linearA.M11 != 0 || linearA.M12 != 0 || linearA.M13 != 0))
{
//Otherwise, just redo it all.
//Create arbitrary axes.
Vector3f axis1;
Vector3f.Cross(ref normal, ref Toolbox.RightVector, out axis1);
length = axis1.LengthSquared;
if (length > MathHelper.Epsilon)
{
length = (float)Math.Sqrt(length);
float inverseLength = 1 / length;
linearA.M11 = axis1.X * inverseLength;
linearA.M12 = axis1.Y * inverseLength;
linearA.M13 = axis1.Z * inverseLength;
}
else
{
Vector3f.Cross(ref normal, ref Toolbox.UpVector, out axis1);
axis1.Normalize();
linearA.M11 = axis1.X;
linearA.M12 = axis1.Y;
linearA.M13 = axis1.Z;
}
}
}
//Second axis is first axis x normal
linearA.M21 = (linearA.M12 * normal.Z) - (linearA.M13 * normal.Y);
linearA.M22 = (linearA.M13 * normal.X) - (linearA.M11 * normal.Z);
linearA.M23 = (linearA.M11 * normal.Y) - (linearA.M12 * normal.X);
//Compute angular jacobians
if (entityA != null)
{
//angularA 1 = ra x linear axis 1
angularA.M11 = (ra.Y * linearA.M13) - (ra.Z * linearA.M12);
angularA.M12 = (ra.Z * linearA.M11) - (ra.X * linearA.M13);
angularA.M13 = (ra.X * linearA.M12) - (ra.Y * linearA.M11);
//angularA 2 = ra x linear axis 2
angularA.M21 = (ra.Y * linearA.M23) - (ra.Z * linearA.M22);
angularA.M22 = (ra.Z * linearA.M21) - (ra.X * linearA.M23);
angularA.M23 = (ra.X * linearA.M22) - (ra.Y * linearA.M21);
}
//angularB 1 = linear axis 1 x rb
if (entityB != null)
{
angularB.M11 = (linearA.M12 * rb.Z) - (linearA.M13 * rb.Y);
angularB.M12 = (linearA.M13 * rb.X) - (linearA.M11 * rb.Z);
angularB.M13 = (linearA.M11 * rb.Y) - (linearA.M12 * rb.X);
//angularB 2 = linear axis 2 x rb
angularB.M21 = (linearA.M22 * rb.Z) - (linearA.M23 * rb.Y);
angularB.M22 = (linearA.M23 * rb.X) - (linearA.M21 * rb.Z);
angularB.M23 = (linearA.M21 * rb.Y) - (linearA.M22 * rb.X);
}
//Compute inverse effective mass matrix
Matrix2f entryA, entryB;
//these are the transformed coordinates
Matrix2x3f transform;
Matrix3x2f transpose;
if (entityADynamic)
{
Matrix2x3f.Multiply(ref angularA, ref entityA.inertiaTensorInverse, out transform);
Matrix2x3f.Transpose(ref angularA, out transpose);
Matrix2f.Multiply(ref transform, ref transpose, out entryA);
entryA.M11 += entityA.inverseMass;
entryA.M22 += entityA.inverseMass;
}
else
{
entryA = new Matrix2f();
}
if (entityBDynamic)
{
Matrix2x3f.Multiply(ref angularB, ref entityB.inertiaTensorInverse, out transform);
Matrix2x3f.Transpose(ref angularB, out transpose);
Matrix2f.Multiply(ref transform, ref transpose, out entryB);
entryB.M11 += entityB.inverseMass;
entryB.M22 += entityB.inverseMass;
}
else
{
entryB = new Matrix2f();
}
velocityToImpulse.M11 = -entryA.M11 - entryB.M11;
velocityToImpulse.M12 = -entryA.M12 - entryB.M12;
velocityToImpulse.M21 = -entryA.M21 - entryB.M21;
velocityToImpulse.M22 = -entryA.M22 - entryB.M22;
Matrix2f.Invert(ref velocityToImpulse, out velocityToImpulse);
}
/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
isTryingToMove = movementDirection3d.LengthSquared > 0;
maxForceDt = MaximumForce * dt;
maxAccelerationForceDt = MaximumAccelerationForce * dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
Vector3f downDirection = characterBody.orientationMatrix.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
Vector3f velocityDirection;
Vector3f offVelocityDirection;
//Project the movement direction onto the support plane defined by the support normal.
//This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
//Instead, project along the sweep direction to the plane.
//For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
Vector3f lineStart = movementDirection3d;
Vector3f lineEnd;
Vector3f.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(supportData.Normal, 0);
float t;
//This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);
//The origin->intersection line direction defines the horizontal velocity direction in 3d space.
velocityDirection.Normalize();
//The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
Vector3f.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);
linearJacobianA1 = velocityDirection;
linearJacobianA2 = offVelocityDirection;
linearJacobianB1 = -velocityDirection;
linearJacobianB2 = -offVelocityDirection;
}
else
{
//If the character isn't trying to move, then the velocity directions are not well defined.
//Instead, pick two arbitrary vectors on the support plane.
//First guess will be based on the previous jacobian.
//Project the old linear jacobian onto the support normal plane.
float dot;
Vector3f.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
Vector3f toRemove;
Vector3f.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3f.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3f.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared;
if (length < MathHelper.Epsilon)
{
//First guess failed. Try the right vector.
Vector3f.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared;
if (length < MathHelper.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3f.Cross(ref Toolbox.ForwardVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared;
//Unless something really weird is happening, we do not need to test any more axes.
}
}
Vector3f.Divide(ref linearJacobianA1, (float)Math.Sqrt(length), out linearJacobianA1);
//Pick another perpendicular vector. Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
Vector3f.Cross(ref linearJacobianA1, ref supportData.Normal, out linearJacobianA2);
//B's linear jacobians are just -A's.
linearJacobianB1 = -linearJacobianA1;
linearJacobianB2 = -linearJacobianA2;
}
if (supportEntity != null)
{
//Compute the angular jacobians.
Vector3f supportToContact = supportData.Position - supportEntity.Position;
//Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
//Note the order of the cross product- it is reversed to negate the result.
Vector3f.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3f.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new Vector3f();
angularJacobianB2 = new Vector3f();
}
}
else
{
//If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
linearJacobianA1 = movementDirection3d;
linearJacobianA2 = Vector3f.Cross(linearJacobianA1, characterBody.orientationMatrix.Down);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
targetVelocity.X = isTryingToMove ? TargetSpeed : 0;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
Vector3f intermediate;
inverseMass = characterBody.InverseMass;
m11 = inverseMass;
m22 = inverseMass;
//Scale the inertia and mass of the support. This will make the solver view the object as 'heavier' with respect to horizontal motion.
Matrix3f inertiaInverse = supportEntity.InertiaTensorInverse;
Matrix3f.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
float extra;
inverseMass = supportForceFactor * supportEntity.InverseMass;
Vector3f.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
Vector3f.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3f.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Vector3f.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3f.Dot(ref intermediate, ref angularJacobianB2, out extra);
m22 += inverseMass + extra;
massMatrix.M11 = m11;
massMatrix.M12 = m1221;
massMatrix.M21 = m1221;
massMatrix.M22 = m22;
Matrix2f.Invert(ref massMatrix, out massMatrix);
}
else
{
//If we're not standing on a dynamic entity, then the mass matrix is defined entirely by the character.
Matrix2f.CreateScale(characterBody.Mass, out massMatrix);
}
//If we're trying to stand still on an object that's moving, use a position correction term to keep the character
//from drifting due to accelerations.
//First thing to do is to check to see if we're moving into a traction/trying to stand still state from a
//non-traction || trying to move state. Either that, or we've switched supports and need to update the offset.
if (supportEntity != null && ((wasTryingToMove && !isTryingToMove) || (!hadTraction && supportFinder.HasTraction) || supportEntity != previousSupportEntity))
{
//We're transitioning into a new 'use position correction' state.
//Force a recomputation of the local offset.
//The time since transition is used as a flag.
timeSinceTransition = 0;
}
//The state is now up to date. Compute an error and velocity bias, if needed.
if (!isTryingToMove && MovementMode == MovementMode.Traction && supportEntity != null)
{
var distanceToBottomOfCharacter = supportFinder.BottomDistance;
if (timeSinceTransition >= 0 && timeSinceTransition < timeUntilPositionAnchor)
{
timeSinceTransition += dt;
}
if (timeSinceTransition >= timeUntilPositionAnchor)
{
Vector3f.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Vector3f.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (timeSinceTransition < 0)
{
Vector3f targetPosition;
Vector3f.Multiply(ref downDirection, distanceToBottomOfCharacter, out targetPosition);
targetPosition += characterBody.Position;
Vector3f worldSupportLocation = Vector3f.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
Vector3f error;
Vector3f.Subtract(ref targetPosition, ref worldSupportLocation, out error);
//If the error is too large, then recompute the offset. We don't want the character rubber banding around.
if (error.LengthSquared > PositionAnchorDistanceThreshold * PositionAnchorDistanceThreshold)
{
Vector3f.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Vector3f.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new Vector2f();
}
else
{
//The error in world space is now available. We can't use this error to directly create a velocity bias, though.
//It needs to be transformed into constraint space where the constraint operates.
//Use the jacobians!
Vector3f.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3f.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2f.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new Vector2f();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportFinder.HasTraction;
previousSupportEntity = supportEntity;
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform local axes into world space
Vector3f.Transform(ref localRestrictedAxis1, ref connectionA.orientationMatrix, out worldRestrictedAxis1);
Vector3f.Transform(ref localRestrictedAxis2, ref connectionA.orientationMatrix, out worldRestrictedAxis2);
Vector3f.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix, out worldLineAnchor);
Vector3f.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor);
Vector3f.Transform(ref localLineDirection, ref connectionA.orientationMatrix, out worldLineDirection);
//Transform local
Vector3f.Transform(ref localPoint, ref connectionB.orientationMatrix, out rB);
Vector3f.Add(ref rB, ref connectionB.position, out worldPoint);
//Find the point on the line closest to the world point.
Vector3f offset;
Vector3f.Subtract(ref worldPoint, ref worldLineAnchor, out offset);
float distanceAlongAxis;
Vector3f.Dot(ref offset, ref worldLineDirection, out distanceAlongAxis);
Vector3f worldNearPoint;
Vector3f.Multiply(ref worldLineDirection, distanceAlongAxis, out offset);
Vector3f.Add(ref worldLineAnchor, ref offset, out worldNearPoint);
Vector3f.Subtract(ref worldNearPoint, ref connectionA.position, out rA);
//Error
Vector3f error3D;
Vector3f.Subtract(ref worldPoint, ref worldNearPoint, out error3D);
Vector3f.Dot(ref error3D, ref worldRestrictedAxis1, out error.X);
Vector3f.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
float bias = -errorReduction;
biasVelocity.X = bias * error.X;
biasVelocity.Y = bias * error.Y;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared;
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
}
//Set up the jacobians
Vector3f.Cross(ref rA, ref worldRestrictedAxis1, out angularA1);
Vector3f.Cross(ref worldRestrictedAxis1, ref rB, out angularB1);
Vector3f.Cross(ref rA, ref worldRestrictedAxis2, out angularA2);
Vector3f.Cross(ref worldRestrictedAxis2, ref rB, out angularB2);
float m11 = 0, m22 = 0, m1221 = 0;
float inverseMass;
Vector3f intermediate;
//Compute the effective mass matrix.
if (connectionA.isDynamic)
{
inverseMass = connectionA.inverseMass;
Vector3f.Transform(ref angularA1, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3f.Dot(ref intermediate, ref angularA1, out m11);
m11 += inverseMass;
Vector3f.Dot(ref intermediate, ref angularA2, out m1221);
Vector3f.Transform(ref angularA2, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3f.Dot(ref intermediate, ref angularA2, out m22);
m22 += inverseMass;
}
#region Mass Matrix B
if (connectionB.isDynamic)
{
float extra;
inverseMass = connectionB.inverseMass;
Vector3f.Transform(ref angularB1, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3f.Dot(ref intermediate, ref angularB1, out extra);
m11 += inverseMass + extra;
Vector3f.Dot(ref intermediate, ref angularB2, out extra);
m1221 += extra;
Vector3f.Transform(ref angularB2, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3f.Dot(ref intermediate, ref angularB2, out extra);
m22 += inverseMass + extra;
}
#endregion
negativeEffectiveMassMatrix.M11 = m11 + softness;
negativeEffectiveMassMatrix.M12 = m1221;
negativeEffectiveMassMatrix.M21 = m1221;
negativeEffectiveMassMatrix.M22 = m22 + softness;
Matrix2f.Invert(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
Matrix2f.Negate(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
}