private static void SwapPlanes(ref SupportPlane plane0, ref SupportPlane plane1) { var temp = plane0; plane0 = plane1; plane1 = temp; }
private static bool Test1d(SupportPlane supportPlane, float3 velocity) { SurfaceConstraintInfo constraint = supportPlane.SurfaceConstraintInfo; float3 relVel = velocity - constraint.Velocity; float planeVel = math.dot(relVel, constraint.Plane.Normal); return(planeVel < -c_SimplexSolverEpsilon); }
private static void Solve2d(float3 up, SupportPlane supportPlane0, SupportPlane supportPlane1, ref float3 velocity) { SurfaceConstraintInfo constraint0 = supportPlane0.SurfaceConstraintInfo; SurfaceConstraintInfo constraint1 = supportPlane1.SurfaceConstraintInfo; float3 plane0 = constraint0.Plane.Normal; float3 plane1 = constraint1.Plane.Normal; // Calculate the free axis float3 axis = math.cross(plane0, plane1); float axisLen2 = math.lengthsq(axis); // Check for parallel planes if (axisLen2 < c_SimplexSolverEpsilon) { // Do the planes sequentially Sort2d(ref supportPlane0, ref supportPlane1); Solve1d(supportPlane1, ref velocity); Solve1d(supportPlane0, ref velocity); return; } float invAxisLen = math.rsqrt(axisLen2); axis *= invAxisLen; // Calculate the velocity of the free axis float3 axisVel; { float4x4 m = new float4x4(); float3 r0 = math.cross(plane0, plane1); float3 r1 = math.cross(plane1, axis); float3 r2 = math.cross(axis, plane0); m.c0 = new float4(r0, 0.0f); m.c1 = new float4(r1, 0.0f); m.c2 = new float4(r2, 0.0f); m.c3 = new float4(0.0f, 0.0f, 0.0f, 1.0f); float3 sVel = constraint0.Velocity + constraint1.Velocity; float3 t = new float3( math.dot(axis, sVel) * 0.5f, math.dot(plane0, constraint0.Velocity), math.dot(plane1, constraint1.Velocity)); axisVel = math.rotate(m, t); axisVel *= invAxisLen; } float3 groundVelocity = axisVel; float3 relVel = velocity - groundVelocity; float vel2 = math.lengthsq(relVel); float axisVert = math.dot(up, axis); float axisProjVel = math.dot(relVel, axis); velocity = groundVelocity + axis * axisProjVel; }
private static void Sort2d(ref SupportPlane plane0, ref SupportPlane plane1) { int priority0 = plane0.SurfaceConstraintInfo.Priority; int priority1 = plane1.SurfaceConstraintInfo.Priority; if (priority0 > priority1) { SwapPlanes(ref plane0, ref plane1); } }
private static void Solve1d(SupportPlane supportPlane, ref float3 velocity) { SurfaceConstraintInfo constraint = supportPlane.SurfaceConstraintInfo; float3 groundVelocity = constraint.Velocity; float3 relVel = velocity - groundVelocity; float planeVel = math.dot(relVel, constraint.Plane.Normal); relVel -= planeVel * constraint.Plane.Normal; velocity = relVel + groundVelocity; }
private static void Solve3d(float3 up, SupportPlane supportPlane0, SupportPlane supportPlane1, SupportPlane supportPlane2, ref float3 velocity) { SurfaceConstraintInfo constraint0 = supportPlane0.SurfaceConstraintInfo; SurfaceConstraintInfo constraint1 = supportPlane1.SurfaceConstraintInfo; SurfaceConstraintInfo constraint2 = supportPlane2.SurfaceConstraintInfo; float3 plane0 = constraint0.Plane.Normal; float3 plane1 = constraint1.Plane.Normal; float3 plane2 = constraint2.Plane.Normal; float4x4 m = new float4x4(); float3 r0 = math.cross(plane1, plane2); float3 r1 = math.cross(plane2, plane0); float3 r2 = math.cross(plane0, plane1); m.c0 = new float4(r0, 0.0f); m.c1 = new float4(r1, 0.0f); m.c2 = new float4(r2, 0.0f); m.c3 = new float4(0.0f, 0.0f, 0.0f, 1.0f); float det = m.c0.x * m.c1.y * m.c2.z; float tst = math.abs(det); if (tst < c_SimplexSolverEpsilon) { Sort3d(ref supportPlane0, ref supportPlane1, ref supportPlane2); Solve2d(up, supportPlane1, supportPlane2, ref velocity); Solve2d(up, supportPlane0, supportPlane2, ref velocity); Solve2d(up, supportPlane0, supportPlane1, ref velocity); return; } float3 sVel = constraint0.Velocity + constraint1.Velocity; float3 t = new float3( math.dot(plane0, constraint0.Velocity), math.dot(plane1, constraint1.Velocity), math.dot(plane2, constraint2.Velocity)); float3 pointVel = math.rotate(m, t); pointVel /= det; velocity = pointVel; }
private static void Sort3d(ref SupportPlane plane0, ref SupportPlane plane1, ref SupportPlane plane2) { int priority0 = plane0.SurfaceConstraintInfo.Priority; int priority1 = plane1.SurfaceConstraintInfo.Priority; int priority2 = plane2.SurfaceConstraintInfo.Priority; if (priority0 <= priority1) { if (priority1 <= priority2) { // 0, 1, 2 } else if (priority0 <= priority2) { // 0, 2, 1 SwapPlanes(ref plane1, ref plane2); } else { // 1, 2, 0 SwapPlanes(ref plane0, ref plane1); SwapPlanes(ref plane0, ref plane2); } } else { if (priority2 < priority1) { // 2, 1, 0 SwapPlanes(ref plane0, ref plane2); } else if (priority2 > priority0) { // 1, 0, 2 SwapPlanes(ref plane0, ref plane1); } else { // 2, 0, 1 SwapPlanes(ref plane0, ref plane1); SwapPlanes(ref plane1, ref plane2); } } }
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; } } }