public void Execute(ref ModifiableJacobianHeader header, ref ModifiableContactJacobian jacobian)
{
// Header verification
Assert.IsFalse(header.AngularChanged);
Assert.AreNotEqual(header.BodyPair.BodyAIndex, header.BodyPair.BodyBIndex);
Assert.AreEqual(header.ColliderKeys.ColliderKeyA.Value, ColliderKey.Empty.Value);
Assert.AreEqual(header.ColliderKeys.ColliderKeyB.Value, ColliderKey.Empty.Value);
Assert.AreEqual(header.Entities.EntityA, Bodies[header.BodyPair.BodyAIndex].Entity);
Assert.AreEqual(header.Entities.EntityB, Bodies[header.BodyPair.BodyBIndex].Entity);
Assert.AreEqual(header.Flags, (JacobianFlags)0);
Assert.IsFalse(header.HasColliderKeys);
Assert.IsFalse(header.HasMassFactors);
Assert.IsFalse(header.HasSurfaceVelocity);
Assert.IsFalse(header.ModifiersChanged);
Assert.AreEqual(header.Type, JacobianType.Contact);
// Jacobian verification
Assert.AreApproximatelyEqual(jacobian.CoefficientOfFriction, 0.5f, 0.01f);
Assert.IsFalse(jacobian.Modified);
Assert.AreEqual(jacobian.NumContacts, 4);
for (int i = 0; i < jacobian.NumContacts; i++)
{
ContactJacAngAndVelToReachCp jacAng = header.GetAngularJacobian(i);
Assert.AreEqual(jacAng.Jac.Impulse, 0.0f);
}
}
// 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 Execute(ref ModifiableJacobianHeader jacHeader, ref ModifiableContactJacobian contactJacobian)
{
Entity entityA = jacHeader.EntityA;
Entity entityB = jacHeader.EntityB;
ModifyContactJacobians.ModificationType typeA = ModifyContactJacobians.ModificationType.None;
if (modificationData.HasComponent(entityA))
{
typeA = modificationData[entityA].type;
}
ModifyContactJacobians.ModificationType typeB = ModifyContactJacobians.ModificationType.None;
if (modificationData.HasComponent(entityB))
{
typeB = modificationData[entityB].type;
}
{
// Check for jacobians we want to ignore:
if (typeA == ModifyContactJacobians.ModificationType.DisabledContact || typeB == ModifyContactJacobians.ModificationType.DisabledContact)
{
jacHeader.Flags = jacHeader.Flags | JacobianFlags.Disabled;
}
// Check if NoTorque modifier, or friction should be disabled through jacobian
if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects ||
typeA == ModifyContactJacobians.ModificationType.DisabledAngularFriction || typeB == ModifyContactJacobians.ModificationType.DisabledAngularFriction)
{
// Disable all friction angular effects
var friction0 = contactJacobian.Friction0;
friction0.AngularA = 0.0f;
friction0.AngularB = 0.0f;
contactJacobian.Friction0 = friction0;
var friction1 = contactJacobian.Friction1;
friction1.AngularA = 0.0f;
friction1.AngularB = 0.0f;
contactJacobian.Friction1 = friction1;
var angularFriction = contactJacobian.AngularFriction;
angularFriction.AngularA = 0.0f;
angularFriction.AngularB = 0.0f;
contactJacobian.AngularFriction = angularFriction;
}
// Check if SurfaceVelocity present
if (jacHeader.HasSurfaceVelocity &&
(typeA == ModifyContactJacobians.ModificationType.SurfaceVelocity || typeB == ModifyContactJacobians.ModificationType.SurfaceVelocity))
{
// Since surface normal can change, make sure angular velocity is always relative to it, not independent
jacHeader.SurfaceVelocity = new SurfaceVelocity
{
LinearVelocity = float3.zero,
AngularVelocity = contactJacobian.Normal * (new float3(0.0f, 1.0f, 0.0f))
};
}
// Check if MassFactors present
if (jacHeader.HasMassFactors &&
(typeA == ModifyContactJacobians.ModificationType.InfiniteInertia || typeB == ModifyContactJacobians.ModificationType.InfiniteInertia))
{
// Give both bodies infinite inertia
jacHeader.MassFactors = new MassFactors
{
InverseInertiaFactorA = float3.zero,
InverseMassFactorA = 1.0f,
InverseInertiaFactorB = float3.zero,
InverseMassFactorB = 1.0f
};
}
}
// Angular jacobian modifications
for (int i = 0; i < contactJacobian.NumContacts; i++)
{
ContactJacAngAndVelToReachCp jacobianAngular = jacHeader.GetAngularJacobian(i);
// Check if NoTorque modifier
if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects)
{
// Disable all angular effects
jacobianAngular.Jac.AngularA = 0.0f;
jacobianAngular.Jac.AngularB = 0.0f;
}
// Check if SoftContact modifier
if (typeA == ModifyContactJacobians.ModificationType.SoftContact || typeB == ModifyContactJacobians.ModificationType.SoftContact)
{
jacobianAngular.Jac.EffectiveMass *= 0.1f;
if (jacobianAngular.VelToReachCp > 0.0f)
{
jacobianAngular.VelToReachCp *= 0.5f;
}
}
jacHeader.SetAngularJacobian(i, jacobianAngular);
}
}
public void Execute(ref ModifiableJacobianHeader jacHeader, ref ModifiableContactJacobian contactJacobian)
{
Entity entityA = jacHeader.Entities.EntityA;
Entity entityB = jacHeader.Entities.EntityB;
ModifyContactJacobians.ModificationType typeA = ModifyContactJacobians.ModificationType.None;
if (modificationData.Exists(entityA))
{
typeA = modificationData[entityA].type;
}
ModifyContactJacobians.ModificationType typeB = ModifyContactJacobians.ModificationType.None;
if (modificationData.Exists(entityB))
{
typeB = modificationData[entityB].type;
}
{
// Check for jacobians we want to ignore:
if (typeA == ModifyContactJacobians.ModificationType.DisabledContact || typeB == ModifyContactJacobians.ModificationType.DisabledContact)
{
jacHeader.Flags = jacHeader.Flags | JacobianFlags.Disabled;
}
// Check if NoTorque modifier
if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects)
{
// Disable all friction angular effects
var friction0 = contactJacobian.Friction0;
friction0.AngularA = 0.0f;
friction0.AngularB = 0.0f;
contactJacobian.Friction0 = friction0;
var friction1 = contactJacobian.Friction1;
friction1.AngularA = 0.0f;
friction1.AngularB = 0.0f;
contactJacobian.Friction1 = friction1;
var angularFriction = contactJacobian.AngularFriction;
angularFriction.AngularA = 0.0f;
angularFriction.AngularB = 0.0f;
contactJacobian.AngularFriction = angularFriction;
}
// Check if SurfaceVelocity present
if (jacHeader.HasSurfaceVelocity &&
(typeA == ModifyContactJacobians.ModificationType.SurfaceVelocity || typeB == ModifyContactJacobians.ModificationType.SurfaceVelocity))
{
// Since surface normal can change, make sure angular velocity is always relative to it, not independent
float3 angVel = contactJacobian.Normal * (new float3(0.0f, 1.0f, 0.0f));
float3 linVel = float3.zero;
Math.CalculatePerpendicularNormalized(contactJacobian.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.Normal);
//jacHeader.SurfaceVelocity = new SurfaceVelocity { ExtraFrictionDv = new float3(linVel0, linVel1, angVelProj) };
}
/*
* // Check if MaxImpulse present
* if (jacHeader.HasMaxImpulse &&
* (typeA == ModifyContactJacobians.ModificationType.ClippedImpulse || typeB == ModifyContactJacobians.ModificationType.ClippedImpulse))
* {
* // Max impulse in Ns (Newton-second)
* jacHeader.MaxImpulse = 20.0f;
* }
*/
// Check if MassFactors present
if (jacHeader.HasMassFactors &&
(typeA == ModifyContactJacobians.ModificationType.InfiniteInertia || typeB == ModifyContactJacobians.ModificationType.InfiniteInertia))
{
// Give both bodies infinite inertia
jacHeader.MassFactors = new MassFactors
{
InvInertiaAndMassFactorA = new float4(0.0f, 0.0f, 0.0f, 1.0f),
InvInertiaAndMassFactorB = new float4(0.0f, 0.0f, 0.0f, 1.0f)
};
}
}
// Angular jacobian modifications
for (int i = 0; i < contactJacobian.NumContacts; i++)
{
ContactJacAngAndVelToReachCp jacobianAngular = jacHeader.GetAngularJacobian(i);
// Check if NoTorque modifier
if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects)
{
// Disable all angular effects
jacobianAngular.Jac.AngularA = 0.0f;
jacobianAngular.Jac.AngularB = 0.0f;
}
// Check if SoftContact modifier
if (typeA == ModifyContactJacobians.ModificationType.SoftContact || typeB == ModifyContactJacobians.ModificationType.SoftContact)
{
jacobianAngular.Jac.EffectiveMass *= 0.1f;
if (jacobianAngular.VelToReachCp > 0.0f)
{
jacobianAngular.VelToReachCp *= 0.5f;
}
}
jacHeader.SetAngularJacobian(i, jacobianAngular);
}
}
