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);
}
}
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);
}
// Solve the Jacobian
public void SolveContact(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter)
{
// 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, tempVelocityB);
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);
}
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;
}
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);
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
if (m_ContactModifierGroup.CalculateLength() == 0)
{
return(inputDeps);
}
if (m_StepPhysicsWorld.Simulation.Type == SimulationType.NoPhysics)
{
return(inputDeps);
}
SimulationCallbacks.Callback preparationCallback = (ref ISimulation simulation, JobHandle inDeps) =>
{
inDeps.Complete(); // shouldn't be needed (jobify the below)
SimulationData.Contacts.Iterator iterator = simulation.Contacts.GetIterator();
while (iterator.HasItemsLeft())
{
ContactHeader manifold = iterator.GetNextContactHeader();
// JacobianModifierFlags format for this example
// UserData 0 - soft contact
// UserData 1 - surface velocity
// UserData 2 - infinite inertia
// UserData 3 - no torque
// UserData 4 - clip impulse
// UserData 5 - disabled contact
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 0)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 0)))
{
manifold.JacobianFlags |= JacobianFlags.UserFlag0; // Soft Contact
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 1)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 1)))
{
manifold.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 2)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 2)))
{
manifold.JacobianFlags |= JacobianFlags.EnableMassFactors;
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 3)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 3)))
{
manifold.JacobianFlags |= JacobianFlags.UserFlag1; // No Torque
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 4)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 4)))
{
manifold.JacobianFlags |= JacobianFlags.EnableMaxImpulse; // No Torque
}
if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 5)) ||
0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 5)))
{
manifold.JacobianFlags |= JacobianFlags.Disabled;
}
iterator.UpdatePreviousContactHeader(manifold);
// Just read contacts
for (int i = 0; i < manifold.NumContacts; i++)
{
iterator.GetNextContact();
}
}
return(inDeps);
};
SimulationCallbacks.Callback jacobianModificationCallback = (ref ISimulation simulation, JobHandle inDeps) =>
{
inDeps.Complete(); // shouldn't be needed (jobify the below)
JacobianIterator iterator = simulation.Jacobians.Iterator;
while (iterator.HasJacobiansLeft())
{
// JacobianModifierFlags format for this example
// UserFlag0 - soft contact
// UserFlag1 - no torque
// Jacobian header
ref JacobianHeader jacHeader = ref iterator.ReadJacobianHeader();
// Triggers can only be disabled, other modifiers have no effect
if (jacHeader.Type == JacobianType.Contact)
{
// Contact jacobian modification
ref ContactJacobian contactJacobian = ref jacHeader.AccessBaseJacobian <ContactJacobian>();
{
// Check if NoTorque modifier
if ((jacHeader.Flags & JacobianFlags.UserFlag1) != 0)
{
// Disable all friction angular effects
contactJacobian.Friction0.AngularA = 0.0f;
contactJacobian.Friction1.AngularA = 0.0f;
contactJacobian.AngularFriction.AngularA = 0.0f;
contactJacobian.Friction0.AngularB = 0.0f;
contactJacobian.Friction1.AngularB = 0.0f;
contactJacobian.AngularFriction.AngularB = 0.0f;
}
// Check if SurfaceVelocity present
if (jacHeader.HasSurfaceVelocity)
{
// Since surface normal can change, make sure angular velocity is always relative to it, not independent
float3 angVel = contactJacobian.BaseJacobian.Normal * (new float3(0.0f, 1.0f, 0.0f));
float3 linVel = float3.zero;
Math.CalculatePerpendicularNormalized(contactJacobian.BaseJacobian.Normal, out float3 dir0, out float3 dir1);
float linVel0 = math.dot(linVel, dir0);
float linVel1 = math.dot(linVel, dir1);
float angVelProj = math.dot(angVel, contactJacobian.BaseJacobian.Normal);
jacHeader.SurfaceVelocity = new SurfaceVelocity {
ExtraFrictionDv = new float3(linVel0, linVel1, angVelProj)
};
}
