// Solve the Jacobian
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep)
{
// 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);
float jacLengthSq = math.lengthsq(jacB0);
float invJacLength = Math.RSqrtSafe(jacLengthSq);
float futureAngle;
{
float sinAngle = jacLengthSq * invJacLength;
float 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
float invEffMassDiag0 = math.csum(jacA0 * jacA0 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB0 * velocityB.InverseInertiaAndMass.xyz);
float invEffMassDiag1 = math.csum(jacA1 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB1 * jacB1 * velocityB.InverseInertiaAndMass.xyz);
float invEffMassOffDiag = math.csum(jacA0 * jacA1 * velocityA.InverseInertiaAndMass.xyz + jacB0 * jacB1 * velocityB.InverseInertiaAndMass.xyz);
float det = invEffMassDiag0 * invEffMassDiag1 - invEffMassOffDiag * invEffMassOffDiag;
float invDet = math.select(jacLengthSq / det, 0.0f, det == 0.0f); // 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
float futureError = JacobianUtilities.CalculateError(futureAngle, MinAngle, MaxAngle);
float solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
float2 impulse = -effectiveMass * solveError * (1.0f / timestep);
velocityA.ApplyAngularImpulse(impulse.x * jacA0 + impulse.y * jacA1);
velocityB.ApplyAngularImpulse(impulse.x * jacB0 + impulse.y * jacB1);
}
// Generic solve method that dispatches to specific ones
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref BlockStream.Writer collisionEventsWriter)
{
bool bothBodiesWithInfInertiaAndMass = math.all(velocityA.InverseInertiaAndMass == float4.zero) && math.all(velocityB.InverseInertiaAndMass == float4.zero);
if (bothBodiesWithInfInertiaAndMass)
{
SolveInfMassPair(ref jacHeader, velocityA, velocityB, stepInput, ref collisionEventsWriter);
}
else
{
SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter);
}
}
// Solve the Jacobian
public void SolveContact(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter, bool enableFrictionVelocitiesHeuristic,
Solver.MotionStabilizationInput motionStabilizationSolverInputA, Solver.MotionStabilizationInput motionStabilizationSolverInputB)
{
// Copy velocity data
MotionVelocity tempVelocityA = velocityA;
MotionVelocity tempVelocityB = velocityB;
if (jacHeader.HasMassFactors)
{
MassFactors jacMod = jacHeader.AccessMassFactors();
tempVelocityA.InverseInertia *= jacMod.InverseInertiaFactorA;
tempVelocityA.InverseMass *= jacMod.InverseMassFactorA;
tempVelocityB.InverseInertia *= jacMod.InverseInertiaFactorB;
tempVelocityB.InverseMass *= jacMod.InverseMassFactorB;
}
// Solve normal impulses
float sumImpulses = 0.0f;
float totalAccumulatedImpulse = 0.0f;
bool forceCollisionEvent = false;
for (int j = 0; j < BaseJacobian.NumContacts; j++)
{
ref ContactJacAngAndVelToReachCp jacAngular = ref jacHeader.AccessAngularJacobian(j);
// Solve velocity so that predicted contact distance is greater than or equal to zero
float relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac,
tempVelocityA.LinearVelocity, tempVelocityA.AngularVelocity, tempVelocityB.LinearVelocity, tempVelocityB.AngularVelocity);
float dv = jacAngular.VelToReachCp - relativeVelocity;
float impulse = dv * jacAngular.Jac.EffectiveMass;
float accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, 0.0f);
if (accumulatedImpulse != jacAngular.Jac.Impulse)
{
float deltaImpulse = accumulatedImpulse - jacAngular.Jac.Impulse;
ApplyImpulse(deltaImpulse, BaseJacobian.Normal, jacAngular.Jac, ref tempVelocityA, ref tempVelocityB,
motionStabilizationSolverInputA.InverseInertiaScale, motionStabilizationSolverInputB.InverseInertiaScale);
}
jacAngular.Jac.Impulse = accumulatedImpulse;
sumImpulses += accumulatedImpulse;
totalAccumulatedImpulse += jacAngular.Jac.Impulse;
// Force contact event even when no impulse is applied, but there is penetration.
forceCollisionEvent |= jacAngular.VelToReachCp > 0.0f;
}
private static void BuildContactJacobian(
int contactPointIndex,
float3 normal,
MTransform worldFromA,
MTransform worldFromB,
float invTimestep,
MotionVelocity velocityA,
MotionVelocity velocityB,
float sumInvMass,
float maxDepenetrationVelocity,
ref JacobianHeader jacobianHeader,
ref float3 centerA,
ref float3 centerB,
ref NativeStream.Reader contactReader)
{
ref ContactJacAngAndVelToReachCp jacAngular = ref jacobianHeader.AccessAngularJacobian(contactPointIndex);
// Generic solve method that dispatches to specific ones
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter, bool enableFrictionVelocitiesHeuristic,
Solver.MotionStabilizationInput motionStabilizationSolverInputA, Solver.MotionStabilizationInput motionStabilizationSolverInputB)
{
bool bothBodiesWithInfInertiaAndMass = velocityA.HasInfiniteInertiaAndMass && velocityB.HasInfiniteInertiaAndMass;
if (bothBodiesWithInfInertiaAndMass)
{
SolveInfMassPair(ref jacHeader, velocityA, velocityB, stepInput, ref collisionEventsWriter);
}
else
{
SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter,
enableFrictionVelocitiesHeuristic, motionStabilizationSolverInputA, motionStabilizationSolverInputB);
}
}
public void Execute(int i)
{
MotionData motionData = MotionDatas[i];
MotionVelocity motionVelocity = MotionVelocities[i];
// Apply gravity
motionVelocity.LinearVelocity += GravityAcceleration * motionData.GravityFactor;
// Write back
MotionVelocities[i] = motionVelocity;
// Make a copy
InputVelocities[i] = new Velocity
{
Linear = motionVelocity.LinearVelocity,
Angular = motionVelocity.AngularVelocity
};
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
RefBFromA = new quaternion(math.mul(bFromConstraint.Rotation, aFromConstraint.InverseRotation));
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
quaternion jointOrientation = math.mul(math.inverse(RefBFromA), BFromA);
float initialAngle = math.asin(math.length(jointOrientation.value.xyz)) * 2.0f;
InitialError = JacobianUtilities.CalculateError(initialAngle, MinAngle, MaxAngle);
}
internal static void ExecuteImpl(int i, float3 gravityAcceleration,
NativeSlice <MotionData> motionDatas, NativeSlice <MotionVelocity> motionVelocities, NativeSlice <Velocity> inputVelocities)
{
MotionData motionData = motionDatas[i];
MotionVelocity motionVelocity = motionVelocities[i];
// Apply gravity
motionVelocity.LinearVelocity += gravityAcceleration * motionData.GravityFactor;
// Write back
motionVelocities[i] = motionVelocity;
// Make a copy
inputVelocities[i] = new Velocity
{
Linear = motionVelocity.LinearVelocity,
Angular = motionVelocity.AngularVelocity
};
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
// Copy the constraint into the jacobian
AxisIndex = constraint.ConstrainedAxis1D;
AxisInMotionA = aFromConstraint.Rotation[AxisIndex];
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
MotionBFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
MotionAFromJoint = new quaternion(aFromConstraint.Rotation);
MotionBFromJoint = new quaternion(bFromConstraint.Rotation);
// Calculate the current error
InitialError = CalculateError(MotionBFromA);
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
this = default(AngularLimit3DJacobian);
BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
MotionAFromJoint = new quaternion(aFromConstraint.Rotation);
MotionBFromJoint = new quaternion(bFromConstraint.Rotation);
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
float initialAngle = math.atan2(math.length(BFromA.value.xyz), BFromA.value.w) * 2.0f;
InitialError = JacobianUtilities.CalculateError(initialAngle, MinAngle, MaxAngle);
}
// Gets a body's motion, even if the body is static
// TODO - share code with Solver.GetMotions()?
private static void GetMotion(ref PhysicsWorld world, int bodyIndex, out MotionVelocity velocity, out MotionData motion)
{
if (bodyIndex >= world.MotionVelocities.Length)
{
// Body is static
RigidBody body = world.Bodies[bodyIndex];
velocity = MotionVelocity.Zero;
motion = new MotionData
{
WorldFromMotion = body.WorldFromBody,
BodyFromMotion = RigidTransform.identity
// remaining fields all zero
};
}
else
{
// Body is dynamic
velocity = world.MotionVelocities[bodyIndex];
motion = world.MotionDatas[bodyIndex];
}
}
// Solve the Jacobian
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep, float 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);
float effectiveMass;
{
float invEffectiveMass = math.csum(AxisInMotionA * AxisInMotionA * velocityA.InverseInertia +
axisInMotionB * axisInMotionB * velocityB.InverseInertia);
effectiveMass = math.select(1.0f / invEffectiveMass, 0.0f, invEffectiveMass == 0.0f);
}
// Calculate the error, adjust by tau and damping, and apply an impulse to correct it
float futureError = CalculateError(futureMotionBFromA);
float solveError = JacobianUtilities.CalculateCorrection(futureError, InitialError, Tau, Damping);
float impulse = math.mul(effectiveMass, -solveError) * invTimestep;
velocityA.ApplyAngularImpulse(impulse * AxisInMotionA);
velocityB.ApplyAngularImpulse(impulse * axisInMotionB);
}
private static void ApplyImpulse(float3 impulse, float3 ang0, float3 ang1, float3 ang2, ref MotionVelocity velocity)
{
velocity.ApplyLinearImpulse(impulse);
velocity.ApplyAngularImpulse(impulse.x * ang0 + impulse.y * ang1 + impulse.z * ang2);
}
// Write a set of contact manifolds for a pair of bodies to the given stream.
public static unsafe void BodyBody(RigidBody rigidBodyA, RigidBody rigidBodyB, MotionVelocity motionVelocityA, MotionVelocity motionVelocityB,
float collisionTolerance, float timeStep, BodyIndexPair pair, ref NativeStream.Writer contactWriter)
{
Collider *colliderA = rigidBodyA.Collider;
Collider *colliderB = rigidBodyB.Collider;
if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter))
{
return;
}
// Build combined motion expansion
MotionExpansion expansion;
{
MotionExpansion expansionA = motionVelocityA.CalculateExpansion(timeStep);
MotionExpansion expansionB = motionVelocityB.CalculateExpansion(timeStep);
expansion = new MotionExpansion
{
Linear = expansionA.Linear - expansionB.Linear,
Uniform = expansionA.Uniform + expansionB.Uniform + collisionTolerance
};
}
var context = new Context
{
BodyIndices = pair,
BodyCustomTags = new CustomTagsPair {
CustomTagsA = rigidBodyA.CustomTags, CustomTagsB = rigidBodyB.CustomTags
},
BodiesHaveInfiniteMass =
!math.any(motionVelocityA.InverseInertiaAndMass) &&
!math.any(motionVelocityB.InverseInertiaAndMass),
ContactWriter = (NativeStream.Writer *)UnsafeUtility.AddressOf(ref contactWriter)
};
var worldFromA = new MTransform(rigidBodyA.WorldFromBody);
var worldFromB = new MTransform(rigidBodyB.WorldFromBody);
// Dispatch to appropriate manifold generator
switch (colliderA->CollisionType)
{
case CollisionType.Convex:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
ConvexTerrain(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Composite:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Composite:
CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
CompositeTerrain(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Terrain:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
TerrainConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
TerrainComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Terrain:
UnityEngine.Assertions.Assert.IsTrue(false);
break;
}
break;
}
}
public float3 FrictionEffectiveMassOffDiag; // Effective mass matrix (0, 1), (0, 2), (1, 2) == (1, 0), (2, 0), (2, 1)
// Solve the Jacobian
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref BlockStream.Writer collisionEventsWriter)
{
// Copy velocity data
MotionVelocity tempVelocityA = velocityA;
MotionVelocity tempVelocityB = velocityB;
if (jacHeader.HasMassFactors)
{
MassFactors jacMod = jacHeader.AccessMassFactors();
tempVelocityA.InverseInertiaAndMass *= jacMod.InvInertiaAndMassFactorA;
tempVelocityB.InverseInertiaAndMass *= jacMod.InvInertiaAndMassFactorB;
}
// Calculate maximum impulse per sub step
float maxImpulseToApply;
if (jacHeader.HasMaxImpulse)
{
maxImpulseToApply = jacHeader.AccessMaxImpulse() * stepInput.Timestep * stepInput.InvNumSolverIterations;
}
else
{
maxImpulseToApply = float.MaxValue;
}
// Solve normal impulses
float sumImpulses = 0.0f;
float frictionFactor = 1.0f;
float4 totalAccumulatedImpulses = float4.zero;
bool forceCollisionEvent = false;
for (int j = 0; j < BaseJacobian.NumContacts; j++)
{
ref ContactJacAngAndVelToReachCp jacAngular = ref jacHeader.AccessAngularJacobian(j);
// Solve velocity so that predicted contact distance is greater than or equal to zero
float relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac, tempVelocityA, tempVelocityB);
float dv = jacAngular.VelToReachCp - relativeVelocity;
// Restitution (typically set to zero)
if (dv > 0.0f && CoefficientOfRestitution > 0.0f)
{
float negContactRestingVelocity = -stepInput.GravityLength * stepInput.Timestep * 1.5f;
if (relativeVelocity < negContactRestingVelocity)
{
float invMassA = tempVelocityA.InverseInertiaAndMass.w;
float invMassB = tempVelocityB.InverseInertiaAndMass.w;
float effInvMassAtCenter = invMassA + invMassB;
jacAngular.VelToReachCp = -(CoefficientOfRestitution * (relativeVelocity - negContactRestingVelocity)) *
(jacAngular.Jac.EffectiveMass * effInvMassAtCenter);
dv = jacAngular.VelToReachCp - relativeVelocity;
// reduce friction to 1/4 of the impulse
frictionFactor = 0.25f;
}
}
float impulse = dv * jacAngular.Jac.EffectiveMass;
bool clipped = impulse > maxImpulseToApply;
impulse = math.min(impulse, maxImpulseToApply);
float accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, 0.0f);
if (accumulatedImpulse != jacAngular.Jac.Impulse)
{
float deltaImpulse = accumulatedImpulse - jacAngular.Jac.Impulse;
ApplyImpulse(deltaImpulse, BaseJacobian.Normal, jacAngular.Jac, ref tempVelocityA, ref tempVelocityB);
}
jacAngular.Jac.Impulse = accumulatedImpulse;
sumImpulses += accumulatedImpulse;
// If there are more than 4 contacts, accumulate their impulses to the last contact point
totalAccumulatedImpulses[math.min(j, 3)] += jacAngular.Jac.Impulse;
// Force contact event even when no impulse is applied, but there is penetration.
// Also force when impulse was clipped, even if clipped to 0.
forceCollisionEvent |= jacAngular.VelToReachCp > 0.0f || clipped;
}
internal static float GetJacVelocity(float3 linear, ContactJacobianAngular jacAngular, MotionVelocity velocityA, MotionVelocity velocityB)
{
float3 temp = (velocityA.LinearVelocity - velocityB.LinearVelocity) * linear;
temp += velocityA.AngularVelocity * jacAngular.AngularA;
temp += velocityB.AngularVelocity * jacAngular.AngularB;
return(math.csum(temp));
}
