// Solve the Jacobian
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, sfloat timestep, sfloat invTimestep)
{
// Predict the relative orientation at the end of the step
quaternion futureBFromA = JacobianUtilities.IntegrateOrientationBFromA(BFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep);
// Calculate the jacobian axis and angle
float3 axisAinB = math.mul(futureBFromA, AxisAinA);
float3 jacB0 = math.cross(axisAinB, AxisBinB);
float3 jacA0 = math.mul(math.inverse(futureBFromA), -jacB0);
sfloat jacLengthSq = math.lengthsq(jacB0);
sfloat invJacLength = Math.RSqrtSafe(jacLengthSq);
sfloat futureAngle;
{
sfloat sinAngle = jacLengthSq * invJacLength;
sfloat cosAngle = math.dot(axisAinB, AxisBinB);
futureAngle = math.atan2(sinAngle, cosAngle);
}
// Choose a second jacobian axis perpendicular to A
float3 jacB1 = math.cross(jacB0, axisAinB);
float3 jacA1 = math.mul(math.inverse(futureBFromA), -jacB1);
// Calculate effective mass
float2 effectiveMass; // First column of the 2x2 matrix, we don't need the second column because the second component of error is zero
{
// Calculate the inverse effective mass matrix, then invert it
sfloat invEffMassDiag0 = math.csum(jacA0 * jacA0 * velocityA.InverseInertia + jacB0 * jacB0 * velocityB.InverseInertia);
sfloat invEffMassDiag1 = math.csum(jacA1 * jacA1 * velocityA.InverseInertia + jacB1 * jacB1 * velocityB.InverseInertia);
sfloat invEffMassOffDiag = math.csum(jacA0 * jacA1 * velocityA.InverseInertia + jacB0 * jacB1 * velocityB.InverseInertia);
sfloat det = invEffMassDiag0 * invEffMassDiag1 - invEffMassOffDiag * invEffMassOffDiag;
sfloat invDet = math.select(jacLengthSq / det, sfloat.Zero, det.IsZero()); // scale by jacLengthSq because the jacs were not normalized
effectiveMass = invDet * new float2(invEffMassDiag1, -invEffMassOffDiag);
}
// Normalize the jacobians
jacA0 *= invJacLength;
jacB0 *= invJacLength;
jacA1 *= invJacLength;
jacB1 *= invJacLength;
// Calculate the error, adjust by tau and damping, and apply an impulse to correct it
sfloat futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle);
sfloat solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
float2 impulse = -effectiveMass * solveError * invTimestep;
velocityA.ApplyAngularImpulse(impulse.x * jacA0 + impulse.y * jacA1);
velocityB.ApplyAngularImpulse(impulse.x * jacB0 + impulse.y * jacB1);
}
public static bool IsTriangleDegenerate(float3 a, float3 b, float3 c)
{
sfloat defaultTriangleDegeneracyTolerance = sfloat.FromRaw(0x33d6bf95);
// Small area check
{
float3 edge1 = a - b;
float3 edge2 = a - c;
float3 cross = math.cross(edge1, edge2);
float3 edge1B = b - a;
float3 edge2B = b - c;
float3 crossB = math.cross(edge1B, edge2B);
bool cmp0 = defaultTriangleDegeneracyTolerance > math.lengthsq(cross);
bool cmp1 = defaultTriangleDegeneracyTolerance > math.lengthsq(crossB);
if (cmp0 || cmp1)
{
return(true);
}
}
// Point triangle distance check
{
float3 q = a - b;
float3 r = c - b;
sfloat qq = math.dot(q, q);
sfloat rr = math.dot(r, r);
sfloat qr = math.dot(q, r);
sfloat qqrr = qq * rr;
sfloat qrqr = qr * qr;
sfloat det = (qqrr - qrqr);
return(det.IsZero());
}
}
// Solve the Jacobian
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, sfloat timestep, sfloat invTimestep)
{
// Predict the relative orientation at the end of the step
quaternion futureMotionBFromA = JacobianUtilities.IntegrateOrientationBFromA(MotionBFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep);
// Calculate the effective mass
float3 axisInMotionB = math.mul(futureMotionBFromA, -AxisInMotionA);
sfloat effectiveMass;
{
sfloat invEffectiveMass = math.csum(AxisInMotionA * AxisInMotionA * velocityA.InverseInertia +
axisInMotionB * axisInMotionB * velocityB.InverseInertia);
effectiveMass = math.select(sfloat.One / invEffectiveMass, sfloat.Zero, invEffectiveMass.IsZero());
}
// Calculate the error, adjust by tau and damping, and apply an impulse to correct it
sfloat futureError = CalculateError(futureMotionBFromA);
sfloat solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
sfloat impulse = math.mul(effectiveMass, -solveError) * invTimestep;
velocityA.ApplyAngularImpulse(impulse * AxisInMotionA);
velocityB.ApplyAngularImpulse(impulse * axisInMotionB);
}
// Compute the barycentric coordinates of the closest point.
public float4 ComputeBarycentricCoordinates(float3 closestPoint)
{
float4 coordinates = new float4(0);
switch (NumVertices)
{
case 1:
coordinates.x = sfloat.One;
break;
case 2:
sfloat distance = math.distance(A.Xyz, B.Xyz);
UnityEngine.Assertions.Assert.AreNotEqual(distance, sfloat.Zero); // TODO just checking if this happens in my tests
if (distance.IsZero()) // Very rare case, simplex is really 1D.
{
goto case 1;
}
coordinates.x = math.distance(B.Xyz, closestPoint) / distance;
coordinates.y = sfloat.One - coordinates.x;
break;
case 3:
{
coordinates.x = math.length(math.cross(B.Xyz - closestPoint, C.Xyz - closestPoint));
coordinates.y = math.length(math.cross(C.Xyz - closestPoint, A.Xyz - closestPoint));
coordinates.z = math.length(math.cross(A.Xyz - closestPoint, B.Xyz - closestPoint));
sfloat sum = math.csum(coordinates.xyz);
if (sum.IsZero()) // Very rare case, simplex is really 2D. Happens because of int->float conversion from the hull builder.
{
// Choose the two farthest apart vertices to keep
float3 lengthsSq = new float3(math.lengthsq(A.Xyz - B.Xyz), math.lengthsq(B.Xyz - C.Xyz), math.lengthsq(C.Xyz - A.Xyz));
bool3 longest = math.cmin(lengthsSq) == lengthsSq;
if (longest.y)
{
A.Xyz = C.Xyz;
}
else if (longest.z)
{
A.Xyz = B.Xyz;
B.Xyz = C.Xyz;
}
coordinates.z = sfloat.Zero;
NumVertices = 2;
goto case 2;
}
coordinates /= sum;
break;
}
case 4:
{
coordinates.x = Det(D.Xyz, C.Xyz, B.Xyz);
coordinates.y = Det(D.Xyz, A.Xyz, C.Xyz);
coordinates.z = Det(D.Xyz, B.Xyz, A.Xyz);
coordinates.w = Det(A.Xyz, B.Xyz, C.Xyz);
sfloat sum = math.csum(coordinates.xyzw);
UnityEngine.Assertions.Assert.AreNotEqual(sum, sfloat.Zero); // TODO just checking that this doesn't happen in my tests
if (sum.IsZero()) // Unexpected case, may introduce significant error by dropping a vertex but it's better than nan
{
coordinates.zw = sfloat.Zero;
NumVertices = 3;
goto case 3;
}
coordinates /= sum;
break;
}
}
return(coordinates);
}
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, sfloat timestep, sfloat invTimestep)
{
// Predict the relative orientation at the end of the step
quaternion futureBFromA = JacobianUtilities.IntegrateOrientationBFromA(BFromA, velocityA.AngularVelocity, velocityB.AngularVelocity, timestep);
// Find the future axis and angle of rotation between the free axes
float3 jacA0, jacA1, jacA2, jacB0, jacB1, jacB2;
float3 effectiveMass; // first column of 3x3 effective mass matrix, don't need the others because only jac0 can have nonzero error
sfloat futureAngle;
{
// Calculate the relative rotation between joint spaces
quaternion jointOrientation = math.mul(math.inverse(RefBFromA), futureBFromA);
// Find the axis and angle of rotation
jacA0 = jointOrientation.value.xyz;
sfloat sinHalfAngleSq = math.lengthsq(jacA0);
sfloat invSinHalfAngle = Math.RSqrtSafe(sinHalfAngleSq);
sfloat sinHalfAngle = sinHalfAngleSq * invSinHalfAngle;
futureAngle = math.asin(sinHalfAngle) * (sfloat)2.0f;
jacA0 = math.select(jacA0 * invSinHalfAngle, new float3(sfloat.One, sfloat.Zero, sfloat.Zero), invSinHalfAngle.IsZero());
jacA0 = math.select(jacA0, -jacA0, jointOrientation.value.w < sfloat.Zero);
Math.CalculatePerpendicularNormalized(jacA0, out jacA1, out jacA2);
jacB0 = math.mul(futureBFromA, -jacA0);
jacB1 = math.mul(futureBFromA, -jacA1);
jacB2 = math.mul(futureBFromA, -jacA2);
// Calculate the effective mass
float3 invEffectiveMassDiag = new float3(
math.csum(jacA0 * jacA0 * velocityA.InverseInertia + jacB0 * jacB0 * velocityB.InverseInertia),
math.csum(jacA1 * jacA1 * velocityA.InverseInertia + jacB1 * jacB1 * velocityB.InverseInertia),
math.csum(jacA2 * jacA2 * velocityA.InverseInertia + jacB2 * jacB2 * velocityB.InverseInertia));
float3 invEffectiveMassOffDiag = new float3(
math.csum(jacA0 * jacA1 * velocityA.InverseInertia + jacB0 * jacB1 * velocityB.InverseInertia),
math.csum(jacA0 * jacA2 * velocityA.InverseInertia + jacB0 * jacB2 * velocityB.InverseInertia),
math.csum(jacA1 * jacA2 * velocityA.InverseInertia + jacB1 * jacB2 * velocityB.InverseInertia));
JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out float3 effectiveMassDiag, out float3 effectiveMassOffDiag);
effectiveMass = JacobianUtilities.BuildSymmetricMatrix(effectiveMassDiag, effectiveMassOffDiag).c0;
}
// Calculate the error, adjust by tau and damping, and apply an impulse to correct it
sfloat futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle);
sfloat solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
sfloat solveVelocity = -solveError * invTimestep;
float3 impulseA = solveVelocity * (jacA0 * effectiveMass.x + jacA1 * effectiveMass.y + jacA2 * effectiveMass.z);
float3 impulseB = solveVelocity * (jacB0 * effectiveMass.x + jacB1 * effectiveMass.y + jacB2 * effectiveMass.z);
velocityA.ApplyAngularImpulse(impulseA);
velocityB.ApplyAngularImpulse(impulseB);
}
private sfloat CalculateError(MTransform worldFromA, MTransform worldFromB, out float3 direction)
{
// Find the direction from pivot A to B and the distance between them
float3 pivotA = Mul(worldFromA, PivotAinA);
float3 pivotB = Mul(worldFromB, PivotBinB);
float3 axis = math.mul(worldFromB.Rotation, AxisInB);
direction = pivotB - pivotA;
sfloat dot = math.dot(direction, axis);
// Project for lower-dimension joints
sfloat distance;
if (Is1D)
{
// In 1D, distance is signed and measured along the axis
distance = -dot;
direction = -axis;
}
else
{
// In 2D / 3D, distance is nonnegative. In 2D it is measured perpendicular to the axis.
direction -= axis * dot;
sfloat futureDistanceSq = math.lengthsq(direction);
sfloat invFutureDistance = math.select(math.rsqrt(futureDistanceSq), sfloat.Zero, futureDistanceSq.IsZero());
distance = futureDistanceSq * invFutureDistance;
direction *= invFutureDistance;
}
// Find the difference between the future distance and the limit range
return(JacobianUtilities.CalculateError(distance, MinDistance, MaxDistance));
}