// 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 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;
}
public unsafe static void Execute(ref JacobiansJobData <T> jobData, IntPtr additionalData,
IntPtr bufferRangePatchData, ref JobRanges ranges, int jobIndex)
{
ModifiableJacobianHeader modifiableHeader;
ModifiableContactJacobian modifiableContact;
byte *headerBuffer = stackalloc byte[JacobianHeader.CalculateSize(JacobianType.Contact, (JacobianFlags)0xff, 4)]; //<todo.eoin.modifier How to verify correct sizes?
byte *contactBuffer = stackalloc byte[sizeof(ContactJacobian)];
Havok.Physics.HpGrid *curGrid = jobData.FixedJacobianGrid;
int *pluginIndexToLocal = jobData.PluginIndexToLocal->Data;
for (int g = 0; g < 2; g++)
{
for (int i = 0; i < curGrid->m_size; i++)
{
HpCsContactJacRange *gridRange = curGrid->m_entries + i;
var range = (Havok.Physics.HpLinkedRange *)UnsafeUtility.AddressOf(ref gridRange->m_range);
while (range != null)
{
var reader = new Havok.Physics.HpBlockStreamReader(range);
while (reader.HasItems)
{
var hpHeader = (Havok.Physics.HpJacHeader *)reader.Peek();
modifiableHeader = new ModifiableJacobianHeader {
};
modifiableContact = new ModifiableContactJacobian {
};
modifiableHeader.m_Header = (JacobianHeader *)headerBuffer;
modifiableContact.m_ContactJacobian = (ContactJacobian *)contactBuffer;
int bodyIndexA = pluginIndexToLocal[hpHeader->m_bodyIdA & 0x00ffffff];
int bodyIndexB = pluginIndexToLocal[hpHeader->m_bodyIdB & 0x00ffffff];
modifiableHeader.m_Header->BodyPair = new BodyIndexPair
{
BodyIndexA = bodyIndexA,
BodyIndexB = bodyIndexB
};
modifiableHeader.EntityPair = new EntityPair
{
EntityA = jobData.Bodies[bodyIndexA].Entity,
EntityB = jobData.Bodies[bodyIndexB].Entity
};
modifiableHeader.m_Header->Type = JacobianType.Contact;
modifiableContact.m_ContactJacobian->BaseJacobian.NumContacts = hpHeader->m_numPoints;
modifiableContact.m_ContactJacobian->BaseJacobian.Normal = hpHeader->m_normal.xyz;
Havok.Physics.HPManifoldCollisionCache *manifoldCache = hpHeader->m_manifoldCollisionCache;
modifiableContact.m_ContactJacobian->CoefficientOfFriction = manifoldCache->m_friction.Value;
// Fill in friction data
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
modifiableContact.m_ContactJacobian->Friction0.AngularA = jf->m_jacDir0_angular0.xyz;
modifiableContact.m_ContactJacobian->Friction0.AngularB = jf->m_jacDir0_angular1.xyz;
modifiableContact.m_ContactJacobian->Friction1.AngularA = jf->m_jacDir1_angular0.xyz;
modifiableContact.m_ContactJacobian->Friction1.AngularB = jf->m_jacDir1_angular1.xyz;
modifiableContact.m_ContactJacobian->AngularFriction.AngularA = jf->m_jacAng_angular0.xyz;
modifiableContact.m_ContactJacobian->AngularFriction.AngularB = jf->m_jacAng_angular1.xyz;
}
Havok.Physics.HpPerManifoldProperty *cdp = manifoldCache->GetCustomPropertyStorage();
modifiableHeader.m_Header->Flags = (JacobianFlags)cdp->m_jacobianFlags;
if ((cdp->m_jacobianFlags & (byte)JacobianFlags.EnableMassFactors) != 0)
{
modifiableHeader.MassFactors = *hpHeader->accessMassFactors();
}
for (int p = 0; p < hpHeader->m_numPoints; p++)
{
Havok.Physics.HpJacAngular *hpAng = hpHeader->accessJacAngular(p);
var ang = new ContactJacAngAndVelToReachCp
{
Jac = new ContactJacobianAngular
{
AngularA = hpAng->m_angular0.xyz,
AngularB = hpAng->m_angular1.xyz,
EffectiveMass = hpAng->m_angular0.w,
},
VelToReachCp = hpAng->m_angular1.w,
};
// Access the angular jacobian from the header directly,
// to avoid the modifiable header marking itself dirty.
modifiableHeader.m_Header->AccessAngularJacobian(p) = ang;
}
jobData.UserJobData.Execute(ref modifiableHeader, ref modifiableContact);
if (((byte)modifiableHeader.Flags & (byte)JacobianFlags.Disabled) != 0)
{
// Don't check the "changed" state of the jacobian - this flag is set on the contact
hpHeader->m_flagsAndDimB |= 1 << 10; // JH_MANIFOLD_IS_NOT_NORMAL
hpHeader->m_manifoldType = 3; // hknpManifoldType::DISABLED
}
if (modifiableHeader.AngularChanged || modifiableHeader.ModifiersChanged)
{
// Need to disable jacobian caching, since we can't tell what modifications the user has done
manifoldCache->m_qualityFlags &= (0xffff ^ (1 << 10)); //hknpBodyQuality::ENABLE_CONTACT_CACHING
}
if (modifiableHeader.AngularChanged)
{
for (int p = 0; p < hpHeader->m_numPoints; p++)
{
Havok.Physics.HpJacAngular * hpAng = hpHeader->accessJacAngular(p);
ContactJacAngAndVelToReachCp ang = modifiableHeader.GetAngularJacobian(p);
hpAng->m_angular0 = new float4(ang.Jac.AngularA, ang.Jac.EffectiveMass);
hpAng->m_angular1 = new float4(ang.Jac.AngularB, ang.VelToReachCp);
}
}
if (modifiableHeader.ModifiersChanged && (cdp->m_jacobianFlags & (byte)JacobianFlags.EnableMassFactors) != 0)
{
*hpHeader->accessMassFactors() = modifiableHeader.MassFactors;
}
if (modifiableHeader.ModifiersChanged && (cdp->m_jacobianFlags & (byte)JacobianFlags.EnableSurfaceVelocity) != 0)
{
var surfVel = modifiableHeader.SurfaceVelocity;
float angVelProj = math.dot(surfVel.AngularVelocity, modifiableContact.Normal);
if (manifoldCache != null)
{
float frictionRhs = manifoldCache->getFrictionRhsMultiplierValue();
float frictRhsMul = frictionRhs * jobData.TimeStep;
// Update cached integrated friction rhs
float4 vel = new float4(-surfVel.LinearVelocity, -angVelProj);
float4 rhs4 = manifoldCache->m_integratedFrictionRhs;
rhs4 += frictRhsMul * vel;
manifoldCache->m_integratedFrictionRhs = rhs4;
}
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Math.CalculatePerpendicularNormalized(modifiableContact.Normal, out float3 dir0, out float3 dir1);
float linVel0 = math.dot(surfVel.LinearVelocity, dir0);
float linVel1 = math.dot(surfVel.LinearVelocity, dir1);
// Check JH_SURFACE_VELOCITY_DIRTY flag and clear it if it was set
const ushort jhSurfaceVelocityDirty = 1 << 3;
if ((hpHeader->m_flagsAndDimB & jhSurfaceVelocityDirty) != 0)
{
*hpHeader->accessSurfaceVelocity() = new float3(linVel0, linVel1, angVelProj);
hpHeader->m_flagsAndDimB &= 0xffff ^ jhSurfaceVelocityDirty;
}
else
{
*hpHeader->accessSurfaceVelocity() += new float3(linVel0, linVel1, angVelProj);
}
// Update friction Jacobian
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
float dRhs0 = jf->m_jacDir0_linear0.w;
float dRhs1 = jf->m_jacDir1_linear0.w;
float dRhs = jf->m_jacAng_angular1.w;
float frictRhsMul = hpHeader->m_manifoldCollisionCache->getFrictionRhsMultiplierValue();
dRhs0 -= frictRhsMul * linVel0;
dRhs1 -= frictRhsMul * linVel1;
dRhs -= frictRhsMul * angVelProj;
jf->m_jacDir0_linear0.w = dRhs0;
jf->m_jacDir1_linear0.w = dRhs1;
jf->m_jacAng_angular1.w = dRhs;
}
}
}
if (modifiableContact.Modified)
{
hpHeader->m_normal.xyz = modifiableContact.Normal;
manifoldCache->m_friction.Value = modifiableContact.CoefficientOfFriction;
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
jf->m_jacDir0_angular0.xyz = modifiableContact.Friction0.AngularA;
jf->m_jacDir0_angular1.xyz = modifiableContact.Friction0.AngularB;
jf->m_jacDir1_angular0.xyz = modifiableContact.Friction1.AngularA;
jf->m_jacDir1_angular1.xyz = modifiableContact.Friction1.AngularB;
jf->m_jacAng_angular0.xyz = modifiableContact.AngularFriction.AngularA;
jf->m_jacAng_angular1.xyz = modifiableContact.AngularFriction.AngularB;
}
}
reader.Advance(hpHeader->m_sizeDiv16 * 16);
}
range = range->m_next;
}
}
curGrid = jobData.MovingJacobianGrid;
}
}
