public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ContactManifold *manifold, out PairMaterialProperties pairMaterial)
{
pairMaterial.FrictionCoefficient = 1;
pairMaterial.MaximumRecoveryVelocity = float.MaxValue;
pairMaterial.SpringSettings.NaturalFrequency = MathHelper.Pi * 30;
pairMaterial.SpringSettings.DampingRatio = 100f;// 100000;
return(true);
}
unsafe static void ResolveSubstepManifold(ref SubstepManifolds substeps, ContactManifold *outManifold)
{
//Scan the substeps looking for the first substep that contains any contacts.
//TODO: There are situations involving very high angular velocity where the first contacts are not the best choice.
//If this turns out to be a problem in practice, you may want to change the heuristic to prefer *approaching* contacts over merely existing contacts.
//However, determining whether a contact is approaching requires computing the relative velocity at its position, which isn't free. (Not super expensive, but not free.)
//For the sake of simplicity, just use 'first contact' for now.
//TODO: If the first manifold is not full, you could also pull contacts from later substeps. They might help post-collision rotate-through-the-ground type penetration.
float inverseCount = 1f / substeps.Manifolds.Count;
for (int i = 0; i < substeps.Manifolds.Count; ++i)
{
ref var manifold = ref substeps.Manifolds[i];
var contactCount = manifold.ContactCount;
if (contactCount > 0)
{
*outManifold = manifold;
//Once the best substep is selected, transform the contact positions and depths to be relative to the poses at t=0.
//Since the contact position offsets are not rotated, all we have to do is add the offset from t=0 to the current time to each contact position
//and modify the penetration depths according to that offset along the normal.
var offset = substeps.RelativeOffsetChange * (i * inverseCount);
var offsets = &outManifold->Offset0;
var depths = &outManifold->Depth0;
//TODO: these two TransformY's could be optimized with knowledge that it's unit length.
//That would be pretty useful generally- I'm not sure we've ever used those functions without it being unit length.
//Pretty micro-optimizey, though.
if (outManifold->Convex)
{
var penetrationOffset = Vector3.Dot(offset, outManifold->ConvexNormal);
for (int j = 0; j < contactCount; ++j)
{
offsets[j] += offset;
depths[j] += penetrationOffset;
}
}
else
{
var normals = &outManifold->Normal0;
for (int j = 0; j < contactCount; ++j)
{
var penetrationOffset = Vector3.Dot(offset, normals[j]);
offsets[j] += offset;
depths[j] += penetrationOffset;
}
}
return;
}
}
public static void ConvexFastRemoveAt(ContactManifold *manifold, int index)
{
Debug.Assert(manifold->Convex);
var count = manifold->ContactCount;
var lastIndex = count - 1;
if (index < lastIndex)
{
var offsets = &manifold->Offset0;
var depths = &manifold->Depth0;
var featureIds = &manifold->FeatureId0;
offsets[index] = offsets[lastIndex];
depths[index] = depths[lastIndex];
featureIds[index] = featureIds[lastIndex];
}
manifold->SetConvexityAndCount(lastIndex, true);
}
public unsafe void Notify(ContinuationIndex continuationId, ContactManifold *manifold)
{
var todoTestCollisionCache = default(EmptyCollisionCache);
Debug.Assert(continuationId.Exists);
var continuationIndex = continuationId.Index;
switch ((ConstraintGeneratorType)continuationId.Type)
{
case ConstraintGeneratorType.Discrete:
{
//Direct has no need for accumulating multiple reports; we can immediately dispatch.
ref var continuation = ref discrete.Caches[continuationIndex];
//Discrete manifolds should obey the speculative margin limitation on speculative contacts.
//Note that we cannot modify the manifold provided to this function; we generate our own version.
ContactManifold reducedManifold;
ReduceDistantContacts(manifold, continuation.SpeculativeMargin, &reducedManifold);
narrowPhase.UpdateConstraintsForPair(workerIndex, ref continuation.Pair, &reducedManifold, ref todoTestCollisionCache);
discrete.Return(continuationIndex, pool);
}
break;
public unsafe void Notify(ContinuationIndex continuationId, ContactManifold *manifold)
{
//Console.WriteLine($"Completed {continuationId}:");
//var normals = &manifold->Normal0;
//var offsets = &manifold->Offset0;
//var depths = &manifold->Depth0;
//if (manifold->Convex)
//{
// for (int i = 0; i < manifold->ContactCount; ++i)
// {
// Console.WriteLine($"{i}: P: {offsets[i]}, N: {manifold->ConvexNormal}, D: {depths[i]}");
// }
//}
//else
//{
// for (int i = 0; i < manifold->ContactCount; ++i)
// {
// Console.WriteLine($"{i}: P: {offsets[i]}, N: {normals[i]}, D: {depths[i]}");
// }
//}
var extra = 1e-16 * (manifold->Depth0 + manifold->Offset0.X + manifold->Normal0.X);
Count += 1 + (int)extra;
}
public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ContactManifold *manifold)
{
return(true);
}
public unsafe void Configure(CollidablePair parent, int childA, int childB, ContactManifold *manifold)
{
}
private unsafe void RedistributeImpulses <TContactImpulses>(int oldContactCount, float *oldImpulses, int *oldFeatureIds, ContactManifold *manifold, ref TContactImpulses newImpulsesContainer)
{
//Map the new contacts to the old contacts.
var newFeatureIds = &manifold->FeatureId0;
var newContactCount = manifold->ContactCount;
ref var newImpulses = ref Unsafe.As <TContactImpulses, float>(ref newImpulsesContainer);
private unsafe void ReduceDistantContacts(ContactManifold *manifold, float speculativeMargin, ContactManifold *outputManifold)
{
var contactCount = manifold->ContactCount;
var sourceDepths = &manifold->Depth0;
//Negative depths correspond to separation.
Debug.Assert(speculativeMargin >= 0, "Negative speculative margins are nonsensical. Is something busted?");
speculativeMargin = -speculativeMargin;
if (manifold->Convex)
{
int count = 0;
var sourceOffsets = &manifold->Offset0;
var sourceIds = &manifold->FeatureId0;
var targetOffsets = &outputManifold->Offset0;
var targetDepths = &outputManifold->Depth0;
var targetIds = &outputManifold->FeatureId0;
for (int i = 0; i < contactCount; ++i)
{
if (sourceDepths[i] >= speculativeMargin)
{
var index = count++;
targetOffsets[index] = sourceOffsets[i];
targetDepths[index] = sourceDepths[i];
targetIds[index] = sourceIds[i];
}
}
if (count > 0)
{
outputManifold->ConvexNormal = manifold->ConvexNormal;
outputManifold->OffsetB = manifold->OffsetB;
}
outputManifold->SetConvexityAndCount(count, true);
}
else
{
int count = 0;
var sourceOffsets = &manifold->Offset0;
var sourceIds = &manifold->FeatureId0;
var sourceNormals = &manifold->Normal0;
var targetOffsets = &outputManifold->Offset0;
var targetDepths = &outputManifold->Depth0;
var targetIds = &outputManifold->FeatureId0;
var targetNormals = &outputManifold->Normal0;
for (int i = 0; i < contactCount; ++i)
{
if (sourceDepths[i] >= speculativeMargin)
{
var index = count++;
targetOffsets[index] = sourceOffsets[i];
targetDepths[index] = sourceDepths[i];
targetIds[index] = sourceIds[i];
targetNormals[index] = sourceNormals[i];
}
}
if (count > 0)
{
outputManifold->OffsetB = manifold->OffsetB;
}
outputManifold->SetConvexityAndCount(count, false);
}
}
private unsafe void FillLinearManifoldSlotB(ref ContactManifold linearManifold, ContactManifold *manifold)
{
Debug.Assert(manifold->ContactCount == 1);
linearManifold.Offset1 = manifold->Offset0;
linearManifold.Depth1 = manifold->Depth0;
linearManifold.FeatureId1 = CCDFeatureIdOffsets.LinearB;
linearManifold.Normal1 = manifold->Normal0;
}
private unsafe void FillLinearManifoldSlotA(ref ContactManifold linearManifold, ContactManifold *manifold)
{
//Note that linear A is always in slot 0, and linear B is always in slot 1.
//This is allowed because a linear pair is guaranteed to have two contacts generated from CCD.
//There is no separation limit on inner sphere pairs, and you never just create one sphere-collidable pair- it's always bilateral.
//Also note that offsetB is only used from linear A, not linear B. They should be identical.
Debug.Assert(manifold->ContactCount == 1);
linearManifold.OffsetB = manifold->OffsetB;
linearManifold.Offset0 = manifold->Offset0;
linearManifold.Depth0 = manifold->Depth0;
linearManifold.FeatureId0 = CCDFeatureIdOffsets.LinearA;
linearManifold.Normal0 = manifold->Normal0;
}
unsafe static void ResolveLinearManifold(ContactManifold *mainManifold, ContactManifold *linearManifold, ContactManifold *outManifold)
{
var linearCount = linearManifold->ContactCount;
if (linearCount > 0)
{
Debug.Assert(linearCount <= 2, "Inner sphere derived contacts should only ever contribute one contact per involved body.");
var mainCount = mainManifold->ContactCount;
if (mainCount > 0)
{
var totalCount = mainCount + linearCount;
var contactsToPotentiallyReplaceCount = totalCount - 4;
if (contactsToPotentiallyReplaceCount > 0)
{
//There are more contacts than slots. A subset must be prioritized.
//We want the deepest set of contacts from all available manifolds.
//(This isn't necessarily an ideal heuristic- consider a bunch of deep contacts all in the same place,
//causing the removal of a distant but less redundant contact. In practice, though, it works okay.)
//To find the deepest contacts, simply sort both sets and pop the minimums.
//Rather than shuffling the contact manifold memory around, just sort indices.
//TODO: This entire hardcoded sort is a bit gross and silly. You could do better.
//TODO: It's not clear that this is actually even useful. It may be that just picking the deepest of the linear contacts and filling any open space
//is the best and simplest option in the end.
var linearIndices = stackalloc int[linearCount];
var mainIndices = stackalloc int[mainCount];
if (linearCount == 2)
{
if (linearManifold->Depth0 < linearManifold->Depth1)
{
linearIndices[0] = 0;
linearIndices[1] = 1;
}
else
{
linearIndices[0] = 0;
linearIndices[1] = 1;
}
}
else
{
linearIndices[0] = 0;
}
for (int i = 0; i < mainCount; ++i)
{
mainIndices[i] = i;
}
outManifold->SetConvexityAndCount(totalCount, false);
var mainDepths = &mainManifold->Depth0;
for (int i = 1; i <= mainCount; ++i)
{
var originalIndex = mainIndices[i];
var depth = mainDepths[originalIndex];
int compareIndex;
for (compareIndex = i - 1; compareIndex >= 0; --compareIndex)
{
var compareDepth = mainDepths[compareIndex];
if (compareDepth < depth)
{
//Move the element up one slot.
var upperSlotIndex = compareIndex + 1;
mainIndices[upperSlotIndex] = mainIndices[compareIndex];
}
else
{
break;
}
}
var targetIndex = compareIndex + 1;
if (targetIndex != i)
{
//Move the original index down.
mainIndices[targetIndex] = originalIndex;
}
}
var outIndex = 0;
var mainIndex = 0;
var linearIndex = 0;
var outOffsets = &outManifold->Offset0;
var outDepths = &outManifold->Depth0;
var outBases = &outManifold->Normal0;
var outIds = &outManifold->FeatureId0;
var linearOffsets = &linearManifold->Offset0;
var linearDepths = &linearManifold->Depth0;
var linearBases = &linearManifold->Normal0;
var linearIds = &linearManifold->FeatureId0;
var mainOffsets = &mainManifold->Offset0;
var mainIds = &mainManifold->FeatureId0;
if (mainManifold->Convex)
{
//While the linear and output manifolds are nonconvex, the main one is convex.
while (outIndex < 4)
{
if (linearIndex == linearCount ||
(mainIndex < mainCount &&
mainDepths[mainIndex] > linearDepths[linearIndex]))
{
outOffsets[outIndex] = mainOffsets[mainIndex];
outDepths[outIndex] = mainDepths[mainIndex];
outBases[outIndex] = mainManifold->ConvexNormal;
outIds[outIndex] = mainIds[mainIndex];
++mainIndex;
}
else
{
outOffsets[outIndex] = linearOffsets[linearIndex];
outDepths[outIndex] = linearDepths[linearIndex];
outBases[outIndex] = linearBases[linearIndex];
outIds[outIndex] = linearIds[linearIndex];
++linearIndex;
}
}
}
else
{
//Both manifolds are nonconvex.
var mainBases = &mainManifold->Normal0;
while (outIndex < 4)
{
if (linearIndex == linearCount ||
(mainIndex < mainCount &&
mainDepths[mainIndex] > linearDepths[linearIndex]))
{
outOffsets[outIndex] = mainOffsets[mainIndex];
outDepths[outIndex] = mainDepths[mainIndex];
outBases[outIndex] = mainBases[mainIndex];
outIds[outIndex] = mainIds[mainIndex];
++mainIndex;
}
else
{
outOffsets[outIndex] = linearOffsets[linearIndex];
outDepths[outIndex] = linearDepths[linearIndex];
outBases[outIndex] = linearBases[linearIndex];
outIds[outIndex] = linearIds[linearIndex];
++linearIndex;
}
++outIndex;
}
}
}
else
{
//There is sufficient room in the manifold to include all the new contacts, so there is no need for prioritization.
outManifold->SetConvexityAndCount(totalCount, false);
//Add all existing contacts. Note that the use of inner sphere contacts forces the manifold to be nonconvex unconditionally.
//While there are cases in which the normals could actually be planar, we don't spend the time figuring that out-
//this state will be extremely brief regardless, and there isn't much value in trying to tease out convexity for one or two frames.
var outOffsets = &outManifold->Offset0;
var outDepths = &outManifold->Depth0;
var outBases = &outManifold->Normal0;
var outIds = &outManifold->FeatureId0;
var mainOffsets = &mainManifold->Offset0;
var mainDepths = &mainManifold->Depth0;
var mainIds = &mainManifold->FeatureId0;
if (mainManifold->Convex)
{
for (int i = 0; i < mainCount; ++i)
{
outOffsets[i] = mainOffsets[i];
outDepths[i] = mainDepths[i];
outBases[i] = mainManifold->ConvexNormal;
outIds[i] = mainIds[i];
}
}
else
{
var mainBases = &mainManifold->Normal0;
for (int i = 0; i < mainCount; ++i)
{
outOffsets[i] = mainOffsets[i];
outDepths[i] = mainDepths[i];
outBases[i] = mainBases[i];
outIds[i] = mainIds[i];
}
}
//Now add the linear contacts. Both manifolds are known to be nonconvex.
var linearOffsets = &linearManifold->Offset0;
var linearDepths = &linearManifold->Depth0;
var linearBases = &linearManifold->Normal0;
var linearIds = &linearManifold->FeatureId0;
for (int linearIndex = 0; linearIndex < linearCount; ++linearIndex)
{
var outIndex = mainCount + linearIndex;
outOffsets[outIndex] = linearOffsets[linearIndex];
outDepths[outIndex] = linearDepths[linearIndex];
outBases[outIndex] = linearBases[linearIndex];
outIds[outIndex] = linearIds[linearIndex];
}
}
}
else
{
outManifold = linearManifold;
}
}
else
{
outManifold = mainManifold;
}
}
public unsafe void Configure(CollidablePair parent, int childA, int childB, ContactManifold *manifold)
{
narrowPhase.Callbacks.ConfigureContactManifold(workerIndex, parent, childA, childB, manifold);
}
