public unsafe void TestExamineActivePlanes()
{
float3 up = new float3(0, 1, 0);
SurfaceConstraintInfo plane1 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane2 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, -1, 0), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane3 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(1, 0, 0), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane4 = CreateConstraint(ColliderKey.Empty, float3.zero, math.normalize(new float3(1, 1, 0)), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane5 = CreateConstraint(ColliderKey.Empty, float3.zero, math.normalize(new float3(-1, 1, 0)), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane6 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 0, 1), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane7 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 0, -1), 1, 0, 0, false, new float3(0, 0, 1));
// Test the single plane
SurfaceConstraintInfo *supportPlanes = stackalloc SurfaceConstraintInfo[4];
int numSupportPlanes = 1;
supportPlanes[0] = plane1;
float3 velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(1, numSupportPlanes);
// 2 planes, one unnecessary
numSupportPlanes = 2;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(1, numSupportPlanes);
// 2 planes, both necessary
numSupportPlanes = 2;
supportPlanes[0] = plane5;
supportPlanes[1] = plane4;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(2, numSupportPlanes);
// 3 planes, 2 unnecessary
numSupportPlanes = 3;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
supportPlanes[2] = plane1;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(1, numSupportPlanes);
// 3 planes, 1 unnecessary
numSupportPlanes = 3;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
supportPlanes[2] = plane3;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(2, numSupportPlanes);
// 3 planes, all necessary
numSupportPlanes = 3;
supportPlanes[0] = plane4;
supportPlanes[1] = plane5;
supportPlanes[2] = plane6;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(3, numSupportPlanes);
// 4 planes, 3 unnecessary
numSupportPlanes = 4;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
supportPlanes[2] = plane1;
supportPlanes[3] = plane1;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(1, numSupportPlanes);
// 4 planes, 2 unnecessary
numSupportPlanes = 4;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
supportPlanes[2] = plane1;
supportPlanes[3] = plane3;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(2, numSupportPlanes);
// 4 planes, 1 unnecessary
numSupportPlanes = 4;
supportPlanes[0] = plane1;
supportPlanes[1] = plane4;
supportPlanes[2] = plane5;
supportPlanes[3] = plane6;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(3, numSupportPlanes);
// 4 planes, all necessary
numSupportPlanes = 4;
supportPlanes[0] = plane4;
supportPlanes[1] = plane5;
supportPlanes[2] = plane6;
supportPlanes[3] = plane7;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(4, numSupportPlanes);
}
public static unsafe void ExamineActivePlanes(float3 up, SurfaceConstraintInfo *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;
SurfaceConstraintInfo sp0 = supportPlanes[0];
SurfaceConstraintInfo sp1 = supportPlanes[1];
SurfaceConstraintInfo sp2 = supportPlanes[2];
SurfaceConstraintInfo 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;
}
}
}
public static unsafe void Solve(
float deltaTime, float minDeltaTime, float3 up, float maxVelocity,
NativeList <SurfaceConstraintInfo> constraints, ref float3 position, ref float3 velocity, out float integratedTime, bool useConstraintVelocities = true
)
{
// List of planes to solve against (up to 4)
SurfaceConstraintInfo *supportPlanes = stackalloc SurfaceConstraintInfo[4];
int numSupportPlanes = 0;
float remainingTime = deltaTime;
float currentTime = 0.0f;
// Clamp the input velocity to max movement speed
ClampToMaxLength(maxVelocity, ref velocity);
while (remainingTime > 0.0f)
{
int hitIndex = -1;
float minCollisionTime = remainingTime;
// Iterate over constraints and solve them
for (int i = 0; i < constraints.Length; i++)
{
if (constraints[i].Touched)
{
continue;
}
SurfaceConstraintInfo constraint = constraints[i];
float3 relVel = velocity - (useConstraintVelocities ? constraint.Velocity : float3.zero);
float relProjVel = -math.dot(relVel, constraint.Plane.Normal);
if (relProjVel < k_Epsilon)
{
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
{
currentTime += minCollisionTime;
remainingTime -= minCollisionTime;
position += minCollisionTime * velocity;
}
if (hitIndex < 0 || currentTime > minDeltaTime)
{
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
supportPlanes[numSupportPlanes] = constraints[hitIndex];
if (!useConstraintVelocities)
{
supportPlanes[numSupportPlanes].Velocity = float3.zero;
}
numSupportPlanes++;
// Solve support planes
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
// Clamp the solved velocity to max movement speed
ClampToMaxLength(maxVelocity, ref velocity);
// Can't handle more than 4 support planes
if (numSupportPlanes == 4)
{
break;
}
}
integratedTime = currentTime;
}
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)
SurfaceConstraintInfo *supportPlanes = stackalloc SurfaceConstraintInfo[4];
int numSupportPlanes = 0;
float remainingTime = deltaTime;
float currentTime = 0.0f;
float minDeltaTime = 0.0f;
if (math.lengthsq(velocity) > c_SimplexSolverEpsilon)
{
// Min delta time to travel at least 1cm
minDeltaTime = 0.01f / math.length(velocity);
}
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;
}
SurfaceConstraintInfo constraint = constraints[i];
float3 relVel = velocity - constraint.Velocity;
float relProjVel = -math.dot(relVel, constraint.Plane.Normal);
if (relProjVel < c_SimplexSolverEpsilon)
{
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 if at least 100 microseconds to hit
if (minCollisionTime > 1e-4f)
{
currentTime += minCollisionTime;
remainingTime -= minCollisionTime;
position += minCollisionTime * velocity;
}
if (hitIndex < 0 || currentTime > minDeltaTime)
{
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
supportPlanes[numSupportPlanes++] = constraints[hitIndex];
// Solve support planes
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
// Can't handle more than 4 support planes
if (numSupportPlanes == 4)
{
break;
}
}
integratedTime = currentTime;
}
