///<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:
Vector3.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:
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
Vector3.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.
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
ref manifoldCenter,
out manifoldCenter);
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[3].contact.Position,
ref manifoldCenter,
out manifoldCenter);
manifoldCenter.X *= .25f;
manifoldCenter.Y *= .25f;
manifoldCenter.Z *= .25f;
break;
default:
manifoldCenter = Toolbox.NoVector;
break;
}
//Compute the three dimensional relative velocity at the point.
Vector3 velocityA, velocityB;
if (entityA != null)
{
Vector3.Subtract(ref manifoldCenter, ref entityA.position, out ra);
Vector3.Cross(ref entityA.angularVelocity, ref ra, out velocityA);
Vector3.Add(ref velocityA, ref entityA.linearVelocity, out velocityA);
}
else
{
velocityA = new Vector3();
}
if (entityB != null)
{
Vector3.Subtract(ref manifoldCenter, ref entityB.position, out rb);
Vector3.Cross(ref entityB.angularVelocity, ref rb, out velocityB);
Vector3.Add(ref velocityB, ref entityB.linearVelocity, out velocityB);
}
else
{
velocityB = new Vector3();
}
Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity);
//Get rid of the normal velocity.
Vector3 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 > Toolbox.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.
Vector3 axis1;
Vector3.Cross(ref normal, ref Toolbox.RightVector, out axis1);
length = axis1.LengthSquared();
if (length > Toolbox.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
{
Vector3.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
Matrix2X2 entryA, entryB;
//these are the transformed coordinates
Matrix2X3 transform;
Matrix3X2 transpose;
if (entityADynamic)
{
Matrix2X3.Multiply(ref angularA, ref entityA.inertiaTensorInverse, out transform);
Matrix2X3.Transpose(ref angularA, out transpose);
Matrix2X2.Multiply(ref transform, ref transpose, out entryA);
entryA.M11 += entityA.inverseMass;
entryA.M22 += entityA.inverseMass;
}
else
{
entryA = new Matrix2X2();
}
if (entityBDynamic)
{
Matrix2X3.Multiply(ref angularB, ref entityB.inertiaTensorInverse, out transform);
Matrix2X3.Transpose(ref angularB, out transpose);
Matrix2X2.Multiply(ref transform, ref transpose, out entryB);
entryB.M11 += entityB.inverseMass;
entryB.M22 += entityB.inverseMass;
}
else
{
entryB = new Matrix2X2();
}
velocityToImpulse.M11 = -entryA.M11 - entryB.M11;
velocityToImpulse.M12 = -entryA.M12 - entryB.M12;
velocityToImpulse.M21 = -entryA.M21 - entryB.M21;
velocityToImpulse.M22 = -entryA.M22 - entryB.M22;
Matrix2X2.Invert(ref velocityToImpulse, out velocityToImpulse);
}
private bool Get2X2InverseMassMatrix(int indexA, int indexB, out Matrix2X2 massMatrix)
{
massMatrix.M11 = GetMassMatrixEntry(indexA, indexA);
massMatrix.M12 = GetMassMatrixEntry(indexA, indexB);
massMatrix.M21 = massMatrix.M12; // getMassMatrixEntry(indexB, indexA);
massMatrix.M22 = GetMassMatrixEntry(indexB, indexB);
return ComputeNorm(ref massMatrix) < ConditionNumberLimit;
}
private static float ComputeNorm(ref Matrix2X2 m)
{
//Would a square-based norm be faster and sufficient ?
//Huge number of branches in this
float norm = MathHelper.Max(Math.Abs(m.M11), Math.Abs(m.M12));
norm = MathHelper.Max(norm, Math.Abs(m.M21));
norm = MathHelper.Max(norm, Math.Abs(m.M22));
return norm;
}
/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
//Collect references, pick the mode, and configure the coefficients to be used by the solver.
bool isTryingToMove = movementDirection.LengthSquared() > 0;
if (supportData.SupportObject != null)
{
if (supportData.HasTraction)
{
MovementMode = MovementMode.Traction;
maxSpeed = speed;
maxForce = maximumForce;
}
else
{
MovementMode = MovementMode.Sliding;
maxSpeed = slidingSpeed;
maxForce = maximumSlidingForce;
}
}
else
{
MovementMode = MovementMode.Floating;
maxSpeed = airSpeed;
maxForce = maximumAirForce;
supportEntity = null;
}
if (!isTryingToMove)
{
maxSpeed = 0;
}
maxForce *= dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
Vector3 downDirection = character.Body.OrientationMatrix.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
Vector3 velocityDirection;
Vector3 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.
Vector3 lineStart = new Vector3(movementDirection.X, 0, movementDirection.Y);
Vector3 lineEnd;
Vector3.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(character.SupportFinder.HasTraction ? supportData.Normal : 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.
Vector3.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);
linearJacobianA1 = velocityDirection;
linearJacobianA2 = offVelocityDirection;
linearJacobianB1 = -velocityDirection;
linearJacobianB2 = -offVelocityDirection;
}
else
{
Vector3 previousLinearJacobianA1 = linearJacobianA1;
Vector3 previousLinearJacobianA2 = linearJacobianA2;
//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;
Vector3.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
Vector3 toRemove;
Vector3.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3.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.
}
}
Vector3.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.
Vector3.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.
Vector3 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.
Vector3.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new Vector3();
angularJacobianB2 = new Vector3();
}
}
else
{
Vector3 previousLinearJacobianA1 = linearJacobianA1;
Vector3 previousLinearJacobianA2 = linearJacobianA2;
//If the character is floating, then the jacobians are simply the movement direction.
//Note that in a 6DOF character, this will change- but it will still be trivial.
//In that case, the movement direction will be a 3d vector, and the A2 jacobian will just be
//linearJacobianA1 x downDirection.
linearJacobianA1 = new Vector3(movementDirection.X, 0, movementDirection.Y);
linearJacobianA2 = new Vector3(movementDirection.Y, 0, -movementDirection.X);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
targetVelocity.X = maxSpeed;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
Vector3 intermediate;
inverseMass = character.Body.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.
Matrix3X3 inertiaInverse = supportEntity.InertiaTensorInverse;
Matrix3X3.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
float extra;
inverseMass = supportForceFactor * supportEntity.InverseMass;
Matrix3X3.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Matrix3X3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m22 += inverseMass + extra;
massMatrix.M11 = m11;
massMatrix.M12 = m1221;
massMatrix.M21 = m1221;
massMatrix.M22 = m22;
Matrix2X2.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.
Matrix2X2.CreateScale(character.Body.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 && supportData.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 && supportData.HasTraction && supportEntity != null)
{
if (timeSinceTransition >= 0 && timeSinceTransition < tractionDecelerationTime)
{
timeSinceTransition += dt;
}
if (timeSinceTransition >= tractionDecelerationTime)
{
Vector3.Multiply(ref downDirection, character.Body.Radius, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + character.Body.Position) - supportEntity.Position;
positionLocalOffset = Matrix3X3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (timeSinceTransition < 0)
{
Vector3 targetPosition;
Vector3.Multiply(ref downDirection, character.Body.Radius, out targetPosition);
targetPosition += character.Body.Position;
Vector3 worldSupportLocation = Matrix3X3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
Vector3 error;
Vector3.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() > .15f * .15f)
{
Vector3.Multiply(ref downDirection, character.Body.Radius, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + character.Body.Position) - supportEntity.Position;
positionLocalOffset = Matrix3X3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new Vector2();
}
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!
Vector3.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new Vector2();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportData.HasTraction;
previousSupportEntity = supportEntity;
}
/// <summary>
/// Computes a solution to the constraint.
/// </summary>
/// <returns>Impulse magnitude computed by the iteration.</returns>
public override float SolveIteration()
{
Vector2 relativeVelocity = RelativeVelocity;
Vector2.Add(ref relativeVelocity, ref positionCorrectionBias, out relativeVelocity);
//Create the full velocity change, and convert it to an impulse in constraint space.
Vector2 lambda;
Vector2.Subtract(ref targetVelocity, ref relativeVelocity, out lambda);
Matrix2X2.Transform(ref lambda, ref massMatrix, out lambda);
//Add and clamp the impulse.
Vector2 previousAccumulatedImpulse = accumulatedImpulse;
if (MovementMode == MovementMode.Floating)
{
//If it's floating, clamping rules are different.
//The constraint is not permitted to slow down the character; only speed it up.
//This offers a hole for an exploit; by jumping and curving just right,
//the character can accelerate beyond its maximum speed. A bit like an HL2 speed run.
accumulatedImpulse.X = MathHelper.Clamp(accumulatedImpulse.X + lambda.X, 0, maxForce);
accumulatedImpulse.Y = 0;
}
else
{
Vector2.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
float length = accumulatedImpulse.LengthSquared();
if (length > maxForce * maxForce)
{
Vector2.Multiply(ref accumulatedImpulse, maxForce / (float)Math.Sqrt(length), out accumulatedImpulse);
}
}
Vector2.Subtract(ref accumulatedImpulse, ref previousAccumulatedImpulse, out lambda);
//Use the jacobians to put the impulse into world space.
#if !WINDOWS
Vector3 impulse = new Vector3();
Vector3 torque = new Vector3();
#else
Vector3 impulse;
Vector3 torque;
#endif
float x = lambda.X;
float y = lambda.Y;
impulse.X = linearJacobianA1.X * x + linearJacobianA2.X * y;
impulse.Y = linearJacobianA1.Y * x + linearJacobianA2.Y * y;
impulse.Z = linearJacobianA1.Z * x + linearJacobianA2.Z * y;
character.Body.ApplyLinearImpulse(ref impulse);
if (supportEntity != null && supportEntity.IsDynamic)
{
Vector3.Multiply(ref impulse, -supportForceFactor, out impulse);
x *= supportForceFactor;
y *= supportForceFactor;
torque.X = x * angularJacobianB1.X + y * angularJacobianB2.X;
torque.Y = x * angularJacobianB1.Y + y * angularJacobianB2.Y;
torque.Z = x * angularJacobianB1.Z + y * angularJacobianB2.Z;
supportEntity.ApplyLinearImpulse(ref impulse);
supportEntity.ApplyAngularImpulse(ref torque);
}
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
Matrix3X3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Matrix3X3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
Matrix3X3.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1);
Matrix3X3.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2);
Vector3 error;
Vector3.Cross(ref worldAxisA, ref worldAxisB, out error);
Vector3.Dot(ref error, ref worldConstrainedAxis1, out this.error.X);
Vector3.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(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;
}
Vector3 axis1I, axis2I;
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3X3 inertiaTensorSum;
Matrix3X3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum);
Matrix3X3.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I);
Matrix3X3.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3X3.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I);
Matrix3X3.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3X3.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I);
Matrix3X3.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
Vector3.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11);
Vector3.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12);
Vector3.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21);
Vector3.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22);
effectiveMassMatrix.M11 += softness;
effectiveMassMatrix.M22 += softness;
Matrix2X2.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
Matrix2X2.Negate(ref effectiveMassMatrix, out effectiveMassMatrix);
}
/// <summary>
/// Gets the mass matrix of the constraint.
/// </summary>
/// <param name="massMatrix">Constraint's mass matrix.</param>
public void GetMassMatrix(out Matrix2X2 massMatrix)
{
massMatrix = effectiveMassMatrix;
}
///<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
Matrix3X3.Transform(ref localRestrictedAxis1, ref connectionA.orientationMatrix, out worldRestrictedAxis1);
Matrix3X3.Transform(ref localRestrictedAxis2, ref connectionA.orientationMatrix, out worldRestrictedAxis2);
Matrix3X3.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix, out worldLineAnchor);
Vector3.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor);
Matrix3X3.Transform(ref localLineDirection, ref connectionA.orientationMatrix, out worldLineDirection);
//Transform local
Matrix3X3.Transform(ref localPoint, ref connectionB.orientationMatrix, out rB);
Vector3.Add(ref rB, ref connectionB.position, out worldPoint);
//Find the closest point worldAxis line to worldPoint on the line.
Vector3 offset;
Vector3.Subtract(ref worldPoint, ref worldLineAnchor, out offset);
float distanceAlongAxis;
Vector3.Dot(ref offset, ref worldLineDirection, out distanceAlongAxis);
//Find the point on the line closest to the world point.
Vector3 worldNearPoint;
Vector3.Multiply(ref worldLineDirection, distanceAlongAxis, out offset);
Vector3.Add(ref worldLineAnchor, ref offset, out worldNearPoint);
Vector3.Subtract(ref worldNearPoint, ref connectionA.position, out rA);
//Error
Vector3 error3D;
Vector3.Subtract(ref worldPoint, ref worldNearPoint, out error3D);
Vector3.Dot(ref error3D, ref worldRestrictedAxis1, out error.X);
Vector3.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(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
Vector3.Cross(ref rA, ref worldRestrictedAxis1, out angularA1);
Vector3.Cross(ref worldRestrictedAxis1, ref rB, out angularB1);
Vector3.Cross(ref rA, ref worldRestrictedAxis2, out angularA2);
Vector3.Cross(ref worldRestrictedAxis2, ref rB, out angularB2);
float m11 = 0, m22 = 0, m1221 = 0;
float inverseMass;
Vector3 intermediate;
//Compute the effective mass matrix.
if (connectionA.isDynamic)
{
inverseMass = connectionA.inverseMass;
Matrix3X3.Transform(ref angularA1, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularA1, out m11);
m11 += inverseMass;
Vector3.Dot(ref intermediate, ref angularA2, out m1221);
Matrix3X3.Transform(ref angularA2, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularA2, out m22);
m22 += inverseMass;
}
#region Mass Matrix B
if (connectionB.isDynamic)
{
float extra;
inverseMass = connectionB.inverseMass;
Matrix3X3.Transform(ref angularB1, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularB2, out extra);
m1221 += extra;
Matrix3X3.Transform(ref angularB2, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3.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;
Matrix2X2.Invert(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
Matrix2X2.Negate(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override float SolveIteration()
{
#region Theory
//lambda = -mc * (Jv + b)
// PraT = [ bx by bz ] * [ 0 raz -ray ] = [ (-by * raz + bz * ray) (bx * raz - bz * rax) (-bx * ray + by * rax) ]
// [ cx cy cz ] [ -raz 0 rax ] [ (-cy * raz + cz * ray) (cx * raz - cz * rax) (-cx * ray + cy * rax) ]
// [ ray -rax 0 ]
//
// PrbT = [ bx by bz ] * [ 0 rbz -rby ] = [ (-by * rbz + bz * rby) (bx * rbz - bz * rbx) (-bx * rby + by * rbx) ]
// [ cx cy cz ] [ -rbz 0 rbx ] [ (-cy * rbz + cz * rby) (cx * rbz - cz * rbx) (-cx * rby + cy * rbx) ]
// [ rby -rbx 0 ]
// Jv = [ bx by bz PraT -bx -by -bz -Prbt ] * [ vax ]
// [ cx cy cz -cx -cy -cz ] [ vay ]
// [ vaz ]
// [ wax ]
// [ way ]
// [ waz ]
// [ vbx ]
// [ vby ]
// [ vbz ]
// [ wbx ]
// [ wby ]
// [ wbz ]
// va' = [ bx * vax + by * vay + bz * vaz ] = [ b * va ]
// [ cx * vax + cy * vay + cz * vaz ] [ c * va ]
// wa' = [ (PraT row 1) * wa ]
// [ (PraT row 2) * wa ]
// vb' = [ -bx * vbx - by * vby - bz * vbz ] = [ -b * vb ]
// [ -cx * vbx - cy * vby - cz * vbz ] [ -c * vb ]
// wb' = [ -(PrbT row 1) * wb ]
// [ -(PrbT row 2) * wb ]
// Jv = [ b * va + (PraT row 1) * wa - b * vb - (PrbT row 1) * wb ]
// [ c * va + (PraT row 2) * wa - c * vb - (PrbT row 2) * wb ]
// Jv = [ b * (va + wa x ra - vb - wb x rb) ]
// [ c * (va + wa x ra - vb - wb x rb) ]
//P = JT * lambda
#endregion
#if !WINDOWS
Vector2 lambda = new Vector2();
#else
Vector2 lambda;
#endif
//float va1, va2, wa1, wa2, vb1, vb2, wb1, wb2;
//Vector3.Dot(ref worldAxis1, ref myParentA.myInternalLinearVelocity, out va1);
//Vector3.Dot(ref worldAxis2, ref myParentA.myInternalLinearVelocity, out va2);
//wa1 = prAT.M11 * myParentA.myInternalAngularVelocity.X + prAT.M12 * myParentA.myInternalAngularVelocity.Y + prAT.M13 * myParentA.myInternalAngularVelocity.Z;
//wa2 = prAT.M21 * myParentA.myInternalAngularVelocity.X + prAT.M22 * myParentA.myInternalAngularVelocity.Y + prAT.M23 * myParentA.myInternalAngularVelocity.Z;
//Vector3.Dot(ref worldAxis1, ref myParentB.myInternalLinearVelocity, out vb1);
//Vector3.Dot(ref worldAxis2, ref myParentB.myInternalLinearVelocity, out vb2);
//wb1 = prBT.M11 * myParentB.myInternalAngularVelocity.X + prBT.M12 * myParentB.myInternalAngularVelocity.Y + prBT.M13 * myParentB.myInternalAngularVelocity.Z;
//wb2 = prBT.M21 * myParentB.myInternalAngularVelocity.X + prBT.M22 * myParentB.myInternalAngularVelocity.Y + prBT.M23 * myParentB.myInternalAngularVelocity.Z;
//lambda.X = va1 + wa1 - vb1 - wb1 + biasVelocity.X + mySoftness * accumulatedImpulse.X;
//lambda.Y = va2 + wa2 - vb2 - wb2 + biasVelocity.Y + mySoftness * accumulatedImpulse.Y;
Vector3 dv;
Vector3 aVel, bVel;
Vector3.Cross(ref connectionA.angularVelocity, ref rA, out aVel);
Vector3.Add(ref aVel, ref connectionA.linearVelocity, out aVel);
Vector3.Cross(ref connectionB.angularVelocity, ref rB, out bVel);
Vector3.Add(ref bVel, ref connectionB.linearVelocity, out bVel);
Vector3.Subtract(ref aVel, ref bVel, out dv);
Vector3.Dot(ref dv, ref worldRestrictedAxis1, out lambda.X);
Vector3.Dot(ref dv, ref worldRestrictedAxis2, out lambda.Y);
lambda.X += biasVelocity.X + softness * accumulatedImpulse.X;
lambda.Y += biasVelocity.Y + softness * accumulatedImpulse.Y;
//Convert to impulse
Matrix2X2.Transform(ref lambda, ref negativeEffectiveMassMatrix, out lambda);
Vector2.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
float x = lambda.X;
float y = lambda.Y;
//Apply impulse
#if !WINDOWS
Vector3 impulse = new Vector3();
Vector3 torque = new Vector3();
#else
Vector3 impulse;
Vector3 torque;
#endif
impulse.X = worldRestrictedAxis1.X * x + worldRestrictedAxis2.X * y;
impulse.Y = worldRestrictedAxis1.Y * x + worldRestrictedAxis2.Y * y;
impulse.Z = worldRestrictedAxis1.Z * x + worldRestrictedAxis2.Z * y;
if (connectionA.isDynamic)
{
torque.X = x * angularA1.X + y * angularA2.X;
torque.Y = x * angularA1.Y + y * angularA2.Y;
torque.Z = x * angularA1.Z + y * angularA2.Z;
connectionA.ApplyLinearImpulse(ref impulse);
connectionA.ApplyAngularImpulse(ref torque);
}
if (connectionB.isDynamic)
{
impulse.X = -impulse.X;
impulse.Y = -impulse.Y;
impulse.Z = -impulse.Z;
torque.X = x * angularB1.X + y * angularB2.X;
torque.Y = x * angularB1.Y + y * angularB2.Y;
torque.Z = x * angularB1.Z + y * angularB2.Z;
connectionB.ApplyLinearImpulse(ref impulse);
connectionB.ApplyAngularImpulse(ref torque);
}
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
/// <summary>
/// Gets the mass matrix of the constraint.
/// </summary>
/// <param name="massMatrix">Constraint's mass matrix.</param>
public void GetMassMatrix(out Matrix2X2 massMatrix)
{
Matrix2X2.Negate(ref negativeEffectiveMassMatrix, out massMatrix);
}
