unsafe static void SolveSingleJoint(JointData *jointData, int numIterations, float timestep,
ref MotionVelocity velocityA, ref MotionVelocity velocityB, ref MotionData motionA, ref MotionData motionB, out NativeStream jacobiansOut)
{
var stepInput = new Solver.StepInput
{
IsLastIteration = false,
InvNumSolverIterations = 1.0f / numIterations,
Timestep = timestep,
InvTimestep = timestep > 0.0f ? 1.0f / timestep : 0.0f
};
// Build jacobians
jacobiansOut = new NativeStream(1, Allocator.Temp);
{
NativeStream.Writer jacobianWriter = jacobiansOut.AsWriter();
jacobianWriter.BeginForEachIndex(0);
Solver.BuildJointJacobian(jointData, new BodyIndexPair(), velocityA, velocityB, motionA, motionB, timestep, numIterations, ref jacobianWriter);
jacobianWriter.EndForEachIndex();
}
var eventWriter = new NativeStream.Writer(); // no events expected
// Solve the joint
for (int iIteration = 0; iIteration < numIterations; iIteration++)
{
stepInput.IsLastIteration = (iIteration == numIterations - 1);
NativeStream.Reader jacobianReader = jacobiansOut.AsReader();
var jacIterator = new JacobianIterator(jacobianReader, 0);
while (jacIterator.HasJacobiansLeft())
{
ref JacobianHeader header = ref jacIterator.ReadJacobianHeader();
header.Solve(ref velocityA, ref velocityB, stepInput, ref eventWriter, ref eventWriter);
}
}
// 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 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);
}
}
// 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
sfloat sumImpulses = sfloat.Zero;
sfloat totalAccumulatedImpulse = sfloat.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
sfloat relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac,
tempVelocityA.LinearVelocity, tempVelocityA.AngularVelocity, tempVelocityB.LinearVelocity, tempVelocityB.AngularVelocity);
sfloat dv = jacAngular.VelToReachCp - relativeVelocity;
sfloat impulse = dv * jacAngular.Jac.EffectiveMass;
sfloat accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, sfloat.Zero);
if (accumulatedImpulse != jacAngular.Jac.Impulse)
{
sfloat 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 > sfloat.Zero;
}
// 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 SolveTest()
{
var jacobian = new ContactJacobian();
var jacHeader = new JacobianHeader();
var velocityA = MotionVelocity.Zero;
var velocityB = MotionVelocity.Zero;
var stepInput = new Solver.StepInput();
var collisionEventsWriter = new BlockStream.Writer();
jacobian.Solve(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter);
Assert.AreEqual(new JacobianHeader(), jacHeader);
Assert.AreEqual(MotionVelocity.Zero, velocityA);
Assert.AreEqual(MotionVelocity.Zero, velocityB);
Assert.AreEqual(new BlockStream.Writer(), collisionEventsWriter);
}
public void SolveTest()
{
var jacobian = new ContactJacobian();
var jacHeader = new JacobianHeader();
var velocityA = MotionVelocity.Zero;
var velocityB = MotionVelocity.Zero;
var stepInput = new Solver.StepInput();
var collisionEventsWriter = new NativeStream.Writer();
jacobian.SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter,
false, Solver.MotionStabilizationInput.Default, Solver.MotionStabilizationInput.Default);
Assert.AreEqual(new JacobianHeader(), jacHeader);
Assert.AreEqual(MotionVelocity.Zero, velocityA);
Assert.AreEqual(MotionVelocity.Zero, velocityB);
Assert.AreEqual(new NativeStream.Writer(), collisionEventsWriter);
}
