public override void Update(float dt)
{
entityADynamic = entityA != null && entityA.isDynamic;
entityBDynamic = entityB != null && entityB.isDynamic;
//Set up the jacobians.
linearAX = -contact.Normal.X;
linearAY = -contact.Normal.Y;
linearAZ = -contact.Normal.Z;
//linearBX = -linearAX;
//linearBY = -linearAY;
//linearBZ = -linearAZ;
//angular A = Ra x N
if (entityA != null)
{
Vector3.Subtract(ref contact.Position, ref entityA.position, out ra);
angularAX = (ra.Y * linearAZ) - (ra.Z * linearAY);
angularAY = (ra.Z * linearAX) - (ra.X * linearAZ);
angularAZ = (ra.X * linearAY) - (ra.Y * linearAX);
}
//Angular B = N x Rb
if (entityB != null)
{
Vector3.Subtract(ref contact.Position, ref entityB.position, out rb);
angularBX = (linearAY * rb.Z) - (linearAZ * rb.Y);
angularBY = (linearAZ * rb.X) - (linearAX * rb.Z);
angularBZ = (linearAX * rb.Y) - (linearAY * rb.X);
}
//Compute inverse effective mass matrix
float entryA, entryB;
//these are the transformed coordinates
float tX, tY, tZ;
if (entityADynamic)
{
tX = angularAX * entityA.inertiaTensorInverse.M11 + angularAY * entityA.inertiaTensorInverse.M21 + angularAZ * entityA.inertiaTensorInverse.M31;
tY = angularAX * entityA.inertiaTensorInverse.M12 + angularAY * entityA.inertiaTensorInverse.M22 + angularAZ * entityA.inertiaTensorInverse.M32;
tZ = angularAX * entityA.inertiaTensorInverse.M13 + angularAY * entityA.inertiaTensorInverse.M23 + angularAZ * entityA.inertiaTensorInverse.M33;
entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + entityA.inverseMass;
}
else
entryA = 0;
if (entityBDynamic)
{
tX = angularBX * entityB.inertiaTensorInverse.M11 + angularBY * entityB.inertiaTensorInverse.M21 + angularBZ * entityB.inertiaTensorInverse.M31;
tY = angularBX * entityB.inertiaTensorInverse.M12 + angularBY * entityB.inertiaTensorInverse.M22 + angularBZ * entityB.inertiaTensorInverse.M32;
tZ = angularBX * entityB.inertiaTensorInverse.M13 + angularBY * entityB.inertiaTensorInverse.M23 + angularBZ * entityB.inertiaTensorInverse.M33;
entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + entityB.inverseMass;
}
else
entryB = 0;
velocityToImpulse = -1 / (entryA + entryB); //Softness?
//Bounciness and bias (penetration correction)
if (contact.PenetrationDepth >= 0)
{
bias = MathHelper.Min(
MathHelper.Max(0, contact.PenetrationDepth - CollisionDetectionSettings.AllowedPenetration) *
CollisionResponseSettings.PenetrationRecoveryStiffness / dt,
CollisionResponseSettings.MaximumPenetrationCorrectionSpeed);
if (contactManifoldConstraint.materialInteraction.Bounciness > 0)
{
//Target a velocity which includes a portion of the incident velocity.
float relativeVelocity = -RelativeVelocity;
if (relativeVelocity > CollisionResponseSettings.BouncinessVelocityThreshold)
bias = MathHelper.Max(relativeVelocity * contactManifoldConstraint.materialInteraction.Bounciness, bias);
}
}
else
{
//The contact is actually separated right now. Allow the solver to target a position that is just barely in collision.
//If the solver finds that an accumulated negative impulse is required to hit this target, then no work will be done.
bias = contact.PenetrationDepth / dt;
//This implementation is going to ignore bounciness for now.
//Since it's not being used for CCD, these negative-depth contacts
//only really occur in situations where no bounce should occur.
//if (contactManifoldConstraint.materialInteraction.Bounciness > 0)
//{
// //Target a velocity which includes a portion of the incident velocity.
// //The contact isn't colliding currently, but go ahead and target the post-bounce velocity.
// //The bias is added to the bounce velocity to simulate the object continuing to the surface and then bouncing off.
// float relativeVelocity = -RelativeVelocity;
// if (relativeVelocity > CollisionResponseSettings.BouncinessVelocityThreshold)
// bias = relativeVelocity * contactManifoldConstraint.materialInteraction.Bounciness + bias;
//}
}
}
public static unsafe void Test()
{
ContactPenetrationConstraint constraint = new ContactPenetrationConstraint();
Contact contact = new Contact { Normal = new Vector3(0, 1, 0), PenetrationDepth = 0, Position = new Vector3() };
var pairHandler = new BoxPairHandler();
var a = new Entity(new BoxShape(1, 1, 1), 1)
{
Position = new Vector3(0, 0, 0),
Orientation = Quaternion.Identity,
LinearVelocity = new Vector3(0, 0, 0)
};
var b = new Entity(new BoxShape(1, 1, 1), 1)
{
Position = new Vector3(0, 1, 0),
Orientation = Quaternion.Identity,
LinearVelocity = new Vector3(0, 0, 0)
};
pairHandler.Initialize(a.CollisionInformation, b.CollisionInformation);
ContactManifoldConstraint manifoldConstraint = new ConvexContactManifoldConstraint(pairHandler);
manifoldConstraint.Initialize(a, b);
constraint.Setup(manifoldConstraint, contact);
float dt = 1 / 60f;
float inverseDt = 1 / dt;
constraint.Update(dt);
constraint.ExclusiveUpdate();
constraint.SolveIteration();
const int testCount = VectorizedConstraintTest.TestCount * 4;
const int iterationCount = VectorizedConstraintTest.IterationCount;
var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < testCount; ++i)
{
constraint.Update(dt);
constraint.ExclusiveUpdate();
for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
{
constraint.SolveIteration();
}
}
var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
Console.WriteLine($"Scalar Old: {endtime - startTime}");
}