/// <summary>
/// Computes one iteration of the constraint to meet the solver updateable's goal.
/// </summary>
/// <returns>The rough applied impulse magnitude.</returns>
public override float SolveIteration()
{
// lambda = -mc * (Jv + b)
// P = JT * lambda
Vector3 velocity;
Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out velocity);
#if !WINDOWS
Vector2 lambda = new Vector2();
#else
Vector2 lambda;
#endif
Vector3.Dot(ref worldConstrainedAxis1, ref velocity, out lambda.X);
Vector3.Dot(ref worldConstrainedAxis2, ref velocity, out lambda.Y);
Vector2.Add(ref lambda, ref biasVelocity, out lambda);
Vector2 softnessImpulse;
Vector2.Multiply(ref accumulatedImpulse, softness, out softnessImpulse);
Vector2.Add(ref lambda, ref softnessImpulse, out lambda);
Matrix2x2.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
Vector2.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse);
#if !WINDOWS
Vector3 impulse = new Vector3();
#else
Vector3 impulse;
#endif
impulse.X = worldConstrainedAxis1.X * lambda.X + worldConstrainedAxis2.X * lambda.Y;
impulse.Y = worldConstrainedAxis1.Y * lambda.X + worldConstrainedAxis2.Y * lambda.Y;
impulse.Z = worldConstrainedAxis1.Z * lambda.X + worldConstrainedAxis2.Z * lambda.Y;
if (connectionA.isDynamic)
{
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3.Negate(ref impulse, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
/// <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, maxForceDt);
accumulatedImpulse.Y = 0;
}
else
{
Vector2.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
float length = accumulatedImpulse.LengthSquared();
if (length > maxForceDt * maxForceDt)
{
Vector2.Multiply(ref accumulatedImpulse, maxForceDt / (float)Math.Sqrt(length), out accumulatedImpulse);
}
if (isTryingToMove && accumulatedImpulse.X > maxAccelerationForceDt)
{
accumulatedImpulse.X = maxAccelerationForceDt;
}
}
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;
characterBody.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>
/// 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));
}
