public static unsafe void Solve(PhysicsWorld world, float deltaTime, float3 up, int numConstraints,
ref NativeArray <SurfaceConstraintInfo> constraints, ref float3 position, ref float3 velocity, out float integratedTime)
{
// List of planes to solve against (up to 4)
byte * supportPlanesMemory = stackalloc byte[4 * sizeof(SupportPlane)];
SupportPlane *supportPlanes = (SupportPlane *)supportPlanesMemory;
int numSupportPlanes = 0;
float remainingTime = deltaTime;
float currentTime = 0.0f;
// petarm.todo: introduce minDeltaTime after which solver breaks
while (remainingTime > 0.0f)
{
int hitIndex = -1;
float minCollisionTime = remainingTime;
// Iterate over constraints and solve them
for (int i = 0; i < numConstraints; i++)
{
if (constraints[i].Touched)
{
continue;
}
if (numSupportPlanes >= 1 && supportPlanes[0].Index == i)
{
continue;
}
if (numSupportPlanes >= 2 && supportPlanes[1].Index == i)
{
continue;
}
if (numSupportPlanes >= 3 && supportPlanes[2].Index == i)
{
continue;
}
SurfaceConstraintInfo constraint = constraints[i];
float3 relVel = velocity - constraint.Velocity;
float relProjVel = -math.dot(relVel, constraint.Plane.Normal);
if (relProjVel <= 0.0f)
{
continue;
}
// Clamp distance to 0, since penetration is handled by constraint.Velocity already
float distance = math.max(constraint.Plane.Distance, 0.0f);
if (distance <= minCollisionTime * relProjVel)
{
minCollisionTime = distance / relProjVel;
hitIndex = i;
}
}
// Integrate
{
float minCollisionTimeClamped = math.max(minCollisionTime, 0.0f);
currentTime += minCollisionTimeClamped;
remainingTime -= minCollisionTimeClamped;
position += minCollisionTime * velocity;
}
if (hitIndex < 0)
{
break;
}
// Mark constraint as touched
{
var constraint = constraints[hitIndex];
constraint.Touched = true;
constraints[hitIndex] = constraint;
}
// Add the hit to the current list of active planes
{
SupportPlane supportPlane = new SupportPlane()
{
Index = hitIndex,
SurfaceConstraintInfo = constraints[hitIndex]
};
supportPlanes[numSupportPlanes] = supportPlane;
numSupportPlanes++;
}
// Solve support planes
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
// Can't handle more than 4 support planes
if (numSupportPlanes == 4)
{
break;
}
}
integratedTime = currentTime;
}
private static unsafe void ExamineActivePlanes(float3 up, SupportPlane *supportPlanes, ref int numSupportPlanes, ref float3 velocity)
{
switch (numSupportPlanes)
{
case 1:
{
Solve1d(supportPlanes[0], ref velocity);
return;
}
case 2:
{
// Test whether we need plane 0 at all
float3 tempVelocity = velocity;
Solve1d(supportPlanes[1], ref tempVelocity);
bool plane0Used = Test1d(supportPlanes[0], tempVelocity);
if (!plane0Used)
{
// Compact the buffer and reduce size
supportPlanes[0] = supportPlanes[1];
numSupportPlanes = 1;
// Write back the result
velocity = tempVelocity;
}
else
{
Solve2d(up, supportPlanes[0], supportPlanes[1], ref velocity);
}
return;
}
case 3:
{
// Try to drop both planes
float3 tempVelocity = velocity;
Solve1d(supportPlanes[2], ref tempVelocity);
bool plane0Used = Test1d(supportPlanes[0], tempVelocity);
if (!plane0Used)
{
bool plane1Used = Test1d(supportPlanes[1], tempVelocity);
if (!plane1Used)
{
// Compact the buffer and reduce size
supportPlanes[0] = supportPlanes[2];
numSupportPlanes = 1;
goto case 1;
}
}
// Try to drop plane 0 or 1
for (int testPlane = 0; testPlane < 2; testPlane++)
{
tempVelocity = velocity;
Solve2d(up, supportPlanes[testPlane], supportPlanes[2], ref tempVelocity);
bool planeUsed = Test1d(supportPlanes[1 - testPlane], tempVelocity);
if (!planeUsed)
{
supportPlanes[0] = supportPlanes[testPlane];
supportPlanes[1] = supportPlanes[2];
numSupportPlanes--;
goto case 2;
}
}
// Try solve all three
Solve3d(up, supportPlanes[0], supportPlanes[1], supportPlanes[2], ref velocity);
return;
}
case 4:
{
for (int i = 0; i < 3; i++)
{
float3 tempVelocity = velocity;
Solve3d(up, supportPlanes[(i + 1) % 3], supportPlanes[(i + 2) % 3], supportPlanes[3], ref tempVelocity);
bool planeUsed = Test1d(supportPlanes[i], tempVelocity);
if (!planeUsed)
{
supportPlanes[i] = supportPlanes[2];
supportPlanes[2] = supportPlanes[3];
numSupportPlanes = 3;
goto case 3;
}
}
// Nothing can be dropped so we've failed to solve,
// now we do all 3d combinations
float3 tempVel = velocity;
SupportPlane sp0 = supportPlanes[0];
SupportPlane sp1 = supportPlanes[1];
SupportPlane sp2 = supportPlanes[2];
SupportPlane sp3 = supportPlanes[3];
Solve3d(up, sp0, sp1, sp2, ref tempVel);
Solve3d(up, sp0, sp1, sp3, ref tempVel);
Solve3d(up, sp0, sp2, sp3, ref tempVel);
Solve3d(up, sp1, sp2, sp3, ref tempVel);
velocity = tempVel;
return;
}
default:
{
// Can't have more than 4 and less than 1 plane
Assert.IsTrue(false);
break;
}
}
}
