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; } } }