private static void CreateConstraintFromHit(PhysicsWorld world, int rigidBodyIndex, ColliderKey colliderKey, float3 hitPosition, float3 normal, float distance, float skinWidth, out SurfaceConstraintInfo constraint) { bool bodyIsDynamic = 0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies; constraint = new SurfaceConstraintInfo() { Plane = new Plane { Normal = normal, Distance = distance - skinWidth, }, RigidBodyIndex = rigidBodyIndex, ColliderKey = colliderKey, HitPosition = hitPosition, Velocity = bodyIsDynamic ? world.GetLinearVelocity(rigidBodyIndex, hitPosition) : float3.zero, Priority = bodyIsDynamic ? 1 : 0 }; }
public static void CreateConstraintFromHit(PhysicsWorld world, ColliderKey key, int rigidbodyIndex, float3 position, float3 velocity, float3 normal, float distance, float deltaTime, out SurfaceConstraintInfo constraint) { bool dynamicBody = 0 <= rigidbodyIndex && rigidbodyIndex < world.NumDynamicBodies; constraint = new SurfaceConstraintInfo { Plane = new Plane { Normal = normal, Distance = distance }, RigidBodyIndex = rigidbodyIndex, ColliderKey = key, HitPosition = position, Velocity = dynamicBody ? world.MotionVelocities[rigidbodyIndex].LinearVelocity : velocity }; if (distance < 0.0f) { constraint.Velocity = constraint.Velocity - constraint.Plane.Normal * distance; } }
public void TestSolve1d() { SurfaceConstraintInfo plane = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, float3.zero); // Approaching velocity should lose all vertical speed float3 velocity = new float3(0, -1, 0); Solve1d(plane, ref velocity); Assert.AreApproximatelyEqual(velocity.x, 0); Assert.AreApproximatelyEqual(velocity.y, 0); Assert.AreApproximatelyEqual(velocity.z, 0); // Test that only vertical component of velocity is killed velocity = new float3(-1, -1, 0); Solve1d(plane, ref velocity); Assert.AreApproximatelyEqual(velocity.x, -1); Assert.AreApproximatelyEqual(velocity.y, 0); Assert.AreApproximatelyEqual(velocity.z, 0); plane = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, new float3(0, 0.5f, 0)); // Check that vertical component of velocity changes direction when ground velocity is directed towards the body velocity = new float3(0, -1, 0); Solve1d(plane, ref velocity); Assert.AreApproximatelyEqual(velocity.x, 0); Assert.AreApproximatelyEqual(velocity.y, 0.5f); Assert.AreApproximatelyEqual(velocity.z, 0); plane = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, new float3(0, -0.5f, 0)); // Check that only part of vertical component of velocity is killed when ground velocity is going away velocity = new float3(0, -1, 0); Solve1d(plane, ref velocity); Assert.AreApproximatelyEqual(velocity.x, 0); Assert.AreApproximatelyEqual(velocity.y, -0.5f); Assert.AreApproximatelyEqual(velocity.z, 0); }
public void TestSort3d() { 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, 1, 0, false, float3.zero); SurfaceConstraintInfo plane3 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 2, 0, false, float3.zero); // Leftmost plane should have smallest priority after the sort Sort3d(ref plane1, ref plane2, ref plane3); Assert.IsTrue(plane1.Priority < plane2.Priority && plane2.Priority < plane3.Priority); // Leftmost plane should have smallest priority after the sort Sort3d(ref plane1, ref plane3, ref plane2); Assert.IsTrue(plane1.Priority < plane3.Priority && plane3.Priority < plane2.Priority); // Leftmost plane should have smallest priority after the sort Sort3d(ref plane2, ref plane1, ref plane3); Assert.IsTrue(plane2.Priority < plane1.Priority && plane1.Priority < plane3.Priority); // Leftmost plane should have smallest priority after the sort Sort3d(ref plane2, ref plane3, ref plane1); Assert.IsTrue(plane2.Priority < plane3.Priority && plane3.Priority < plane1.Priority); // Leftmost plane should have smallest priority after the sort Sort3d(ref plane3, ref plane1, ref plane2); Assert.IsTrue(plane3.Priority < plane1.Priority && plane1.Priority < plane2.Priority); // Leftmost plane should have smallest priority after the sort Sort3d(ref plane3, ref plane2, ref plane1); Assert.IsTrue(plane3.Priority < plane2.Priority && plane2.Priority < plane1.Priority); }
private static unsafe void ResolveContacts(PhysicsWorld world, float deltaTime, float3 gravity, float tau, float damping, float characterMass, float3 linearVelocity, int numConstraints, ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter) { for (int i = 0; i < numConstraints; i++) { SurfaceConstraintInfo constraint = constraints[i]; int rigidBodyIndex = constraint.RigidBodyIndex; if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies) { // Invalid and static bodies should be skipped continue; } RigidBody body = world.Bodies[rigidBodyIndex]; float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition); pointRelVel -= linearVelocity; float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal); // Required velocity change float deltaVelocity = -projectedVelocity * damping; float distance = constraint.Plane.Distance; if (distance < 0.0f) { deltaVelocity += (distance / deltaTime) * tau; } // Calculate impulse MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; float3 impulse = float3.zero; if (deltaVelocity < 0.0f) { // Impulse magnitude float impulseMagnitude = 0.0f; { float3 arm = constraint.HitPosition - (body.WorldFromBody.pos + body.Collider->MassProperties.MassDistribution.Transform.pos); float3 jacAng = math.cross(arm, constraint.Plane.Normal); float3 armC = jacAng * mv.InverseInertiaAndMass.xyz; float objectMassInv = math.dot(armC, jacAng); objectMassInv += mv.InverseInertiaAndMass.w; impulseMagnitude = deltaVelocity / objectMassInv; } impulse = impulseMagnitude * constraint.Plane.Normal; } // Add gravity { // Effect of gravity on character velocity in the normal direction float3 charVelDown = gravity * deltaTime; float relVelN = math.dot(charVelDown, constraint.Plane.Normal); // Subtract separation velocity if separating contact { bool isSeparatingContact = projectedVelocity < 0.0f; float newRelVelN = relVelN - projectedVelocity; relVelN = math.select(relVelN, newRelVelN, isSeparatingContact); } // If resulting velocity is negative, an impulse is applied to stop the character // from falling into the body { float3 newImpulse = impulse; newImpulse += relVelN * characterMass * constraint.Plane.Normal; impulse = math.select(impulse, newImpulse, relVelN < 0.0f); } } // Store impulse deferredImpulseWriter.Write( new DeferredCharacterControllerImpulse() { Entity = body.Entity, Impulse = impulse, Point = constraint.HitPosition }); } }
private static void CreateConstraintFromHit(PhysicsWorld world, float3 gravity, float deltaTime, int rigidBodyIndex, ColliderKey colliderKey, float3 hitPosition, float3 normal, float distance, bool zeroVelocity, out SurfaceConstraintInfo constraint) { bool bodyIsDynamic = 0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies; constraint = new SurfaceConstraintInfo() { Plane = new Plane { Normal = normal, Distance = distance, }, RigidBodyIndex = rigidBodyIndex, ColliderKey = colliderKey, HitPosition = hitPosition, Velocity = bodyIsDynamic && !zeroVelocity ? world.MotionVelocities[rigidBodyIndex].LinearVelocity - world.MotionDatas[rigidBodyIndex].GravityFactor * gravity * deltaTime : constraint.Velocity = float3.zero, Priority = bodyIsDynamic ? 1 : 0 }; // Fix up the velocity to enable penetration recovery if (distance < 0.0f) { float3 newVel = constraint.Velocity - constraint.Plane.Normal * distance; constraint.Velocity = newVel; } }
public static unsafe void CollideAndIntegrate(PhysicsWorld world, float deltaTime, int maxIterations, float3 up, float3 gravity, float characterMass, float tau, float damping, float maxSlope, bool affectBodies, Collider *collider, ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints, ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter) { float remainingTime = deltaTime; float3 lastDisplacement = linearVelocity * remainingTime; float3 newPosition = transform.pos; quaternion orientation = transform.rot; float3 newVelocity = linearVelocity; float maxSlopeCos = math.cos(maxSlope); const float timeEpsilon = 0.000001f; for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++) { // First do distance query for penetration recovery MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(0.0f, ref distanceHits); int numConstraints = 0; { ColliderDistanceInput input = new ColliderDistanceInput() { MaxDistance = 0.0f, Transform = new RigidTransform { pos = newPosition, rot = orientation, }, Collider = collider }; world.CalculateDistance(input, ref distanceHitsCollector); // Iterate over hits and create constraints from them for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++) { DistanceHit hit = distanceHitsCollector.AllHits[hitIndex]; CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint); // Potentially add a max slope constraint AddMaxSlopeConstraint(up, maxSlopeCos, ref constraint, ref constraints, ref numConstraints); // Add original constraint to the list constraints[numConstraints++] = constraint; } } float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f; // Then do a collider cast { float3 displacement = lastDisplacement + gravityMovement; float3 endPosition = newPosition + displacement; MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits); ColliderCastInput input = new ColliderCastInput() { Collider = collider, Orientation = orientation, Position = newPosition, Direction = displacement }; world.CastCollider(input, ref collector); // Iterate over hits and create constraints from them for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++) { ColliderCastHit hit = collector.AllHits[hitIndex]; bool found = false; for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++) { DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex]; if (dHit.RigidBodyIndex == hit.RigidBodyIndex && dHit.ColliderKey.Equals(hit.ColliderKey)) { found = true; break; } } // Skip duplicate hits if (!found) { CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint); // Potentially add a max slope constraint AddMaxSlopeConstraint(up, maxSlopeCos, ref constraint, ref constraints, ref numConstraints); // Add original constraint to the list constraints[numConstraints++] = constraint; } } } // Solve float3 prevVelocity = newVelocity; float3 prevPosition = newPosition; SimplexSolver.Solve(world, remainingTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime); // Apply impulses to hit bodies if (affectBodies) { ResolveContacts(world, remainingTime, gravity, tau, damping, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter); } float3 newDisplacement = newPosition - prevPosition; // Check if we can walk to the position simplex solver has suggested MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits); int newContactIndex = -1; // If simplex solver moved the character we need to re-cast to make sure it can move to new position if (math.lengthsq(newDisplacement) > SimplexSolver.c_SimplexSolverEpsilon) { float3 displacement = newDisplacement + gravityMovement; float3 endPosition = prevPosition + displacement; ColliderCastInput input = new ColliderCastInput() { Collider = collider, Orientation = orientation, Position = prevPosition, Direction = displacement }; world.CastCollider(input, ref newCollector); for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++) { ColliderCastHit hit = newCollector.AllHits[hitIndex]; bool found = false; for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++) { SurfaceConstraintInfo constraint = constraints[constraintIndex]; if (constraint.RigidBodyIndex == hit.RigidBodyIndex && constraint.ColliderKey.Equals(hit.ColliderKey)) { found = true; break; } } if (!found) { newContactIndex = hitIndex; break; } } } // Move character along the newDisplacement direction until it reaches this new contact if (newContactIndex >= 0) { ColliderCastHit newContact = newCollector.AllHits[newContactIndex]; float fraction = newContact.Fraction / math.length(newDisplacement); integratedTime *= fraction; float3 displacement = newDisplacement * fraction; newPosition = prevPosition + displacement; } remainingTime -= integratedTime; // Remember last displacement for next iteration lastDisplacement = newVelocity * remainingTime; } // Write back position and velocity transform.pos = newPosition; linearVelocity = newVelocity; }
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); }
private static unsafe void CalculateAndStoreDeferredImpulsesAndCollisionEvents( CharacterControllerStepInput stepInput, bool affectBodies, float characterMass, float3 linearVelocity, NativeList <SurfaceConstraintInfo> constraints, ref NativeStream.Writer deferredImpulseWriter, NativeList <StatefulCollisionEvent> collisionEvents) { PhysicsWorld world = stepInput.World; for (int i = 0; i < constraints.Length; i++) { SurfaceConstraintInfo constraint = constraints[i]; int rigidBodyIndex = constraint.RigidBodyIndex; float3 impulse = float3.zero; if (rigidBodyIndex < 0) { continue; } // Skip static bodies if needed to calculate impulse if (affectBodies && (rigidBodyIndex < world.NumDynamicBodies)) { RigidBody body = world.Bodies[rigidBodyIndex]; float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition); pointRelVel -= linearVelocity; float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal); // Required velocity change float deltaVelocity = -projectedVelocity * stepInput.Damping; float distance = constraint.Plane.Distance; if (distance < 0.0f) { deltaVelocity += (distance / stepInput.DeltaTime) * stepInput.Tau; } // Calculate impulse MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; if (deltaVelocity < 0.0f) { // Impulse magnitude float impulseMagnitude = 0.0f; { float objectMassInv = GetInvMassAtPoint(constraint.HitPosition, constraint.Plane.Normal, body, mv); impulseMagnitude = deltaVelocity / objectMassInv; } impulse = impulseMagnitude * constraint.Plane.Normal; } // Add gravity { // Effect of gravity on character velocity in the normal direction float3 charVelDown = stepInput.Gravity * stepInput.DeltaTime; float relVelN = math.dot(charVelDown, constraint.Plane.Normal); // Subtract separation velocity if separating contact { bool isSeparatingContact = projectedVelocity < 0.0f; float newRelVelN = relVelN - projectedVelocity; relVelN = math.select(relVelN, newRelVelN, isSeparatingContact); } // If resulting velocity is negative, an impulse is applied to stop the character // from falling into the body { float3 newImpulse = impulse; newImpulse += relVelN * characterMass * constraint.Plane.Normal; impulse = math.select(impulse, newImpulse, relVelN < 0.0f); } } // Store impulse deferredImpulseWriter.Write( new DeferredCharacterControllerImpulse() { Entity = body.Entity, Impulse = impulse, Point = constraint.HitPosition }); } if (collisionEvents.IsCreated && constraint.Touched && !constraint.IsMaxSlope) { var collisionEvent = new StatefulCollisionEvent(world.Bodies[stepInput.RigidBodyIndex].Entity, world.Bodies[rigidBodyIndex].Entity, stepInput.RigidBodyIndex, rigidBodyIndex, ColliderKey.Empty, constraint.ColliderKey, constraint.Plane.Normal); collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details( 1, math.dot(impulse, collisionEvent.Normal), constraint.HitPosition); // check if collision event exists for the same bodyID and colliderKey // although this is a nested for, number of solved constraints shouldn't be high // if the same constraint (same entities, rigidbody indices and collider keys) // is solved in multiple solver iterations, pick the one from latest iteration bool newEvent = true; for (int j = 0; j < collisionEvents.Length; j++) { if (collisionEvents[j].CompareTo(collisionEvent) == 0) { collisionEvents[j] = collisionEvent; newEvent = false; break; } } if (newEvent) { collisionEvents.Add(collisionEvent); } } } }
private static void CreateMaxSlopeConstraint(float3 up, ref SurfaceConstraintInfo constraint, out SurfaceConstraintInfo maxSlopeConstraint) { float verticalComponent = math.dot(constraint.Plane.Normal, up); SurfaceConstraintInfo newConstraint = constraint; newConstraint.Plane.Normal = math.normalize(newConstraint.Plane.Normal - verticalComponent * up); newConstraint.IsMaxSlope = true; float distance = newConstraint.Plane.Distance; // Calculate distance to the original plane along the new normal. // Clamp the new distance to 2x the old distance to avoid penetration recovery explosions. newConstraint.Plane.Distance = distance / math.max(math.dot(newConstraint.Plane.Normal, constraint.Plane.Normal), 0.5f); if (newConstraint.Plane.Distance < 0.0f) { // Disable penetration recovery for the original plane constraint.Plane.Distance = 0.0f; // Prepare velocity to resolve penetration ResolveConstraintPenetration(ref newConstraint); } // Output max slope constraint maxSlopeConstraint = newConstraint; }
public static unsafe void CollideAndIntegrate( CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Unity.Physics.Collider *collider, ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer deferredImpulseWriter, NativeList <StatefulCollisionEvent> collisionEvents = default, NativeList <StatefulTriggerEvent> triggerEvents = default) { // Copy parameters float deltaTime = stepInput.DeltaTime; float3 up = stepInput.Up; PhysicsWorld world = stepInput.World; float remainingTime = deltaTime; float3 newPosition = transform.pos; quaternion orientation = transform.rot; float3 newVelocity = linearVelocity; float maxSlopeCos = math.cos(stepInput.MaxSlope); const float timeEpsilon = 0.000001f; for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++) { NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp); // Do a collider cast { float3 displacement = newVelocity * remainingTime; NativeList <ColliderCastHit> triggerHits = default; if (triggerEvents.IsCreated) { triggerHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity / 4, Allocator.Temp); } NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp); CharacterControllerAllHitsCollector <ColliderCastHit> collector = new CharacterControllerAllHitsCollector <ColliderCastHit>( stepInput.RigidBodyIndex, 1.0f, ref castHits, world, triggerHits); ColliderCastInput input = new ColliderCastInput() { Collider = collider, Orientation = orientation, Start = newPosition, End = newPosition + displacement }; world.CastCollider(input, ref collector); // Iterate over hits and create constraints from them for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++) { ColliderCastHit hit = collector.AllHits[hitIndex]; CreateConstraint(stepInput.World, stepInput.Up, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, math.dot(-hit.SurfaceNormal, hit.Fraction * displacement), stepInput.SkinWidth, maxSlopeCos, ref constraints); } // Update trigger events if (triggerEvents.IsCreated) { UpdateTriggersSeen(stepInput, triggerHits, triggerEvents, collector.MinHitFraction); } } // Then do a collider distance for penetration recovery, // but only fix up penetrating hits { // Collider distance query NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(k_DefaultQueryHitsCapacity, Allocator.Temp); CharacterControllerAllHitsCollector <DistanceHit> distanceHitsCollector = new CharacterControllerAllHitsCollector <DistanceHit>( stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits, world); { ColliderDistanceInput input = new ColliderDistanceInput() { MaxDistance = stepInput.ContactTolerance, Transform = transform, Collider = collider }; world.CalculateDistance(input, ref distanceHitsCollector); } // Iterate over penetrating hits and fix up distance and normal int numConstraints = constraints.Length; for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++) { DistanceHit hit = distanceHitsCollector.AllHits[hitIndex]; if (hit.Distance < stepInput.SkinWidth) { bool found = false; // Iterate backwards to locate the original constraint before the max slope constraint for (int constraintIndex = numConstraints - 1; constraintIndex >= 0; constraintIndex--) { SurfaceConstraintInfo constraint = constraints[constraintIndex]; if (constraint.RigidBodyIndex == hit.RigidBodyIndex && constraint.ColliderKey.Equals(hit.ColliderKey)) { // Fix up the constraint (normal, distance) { // Create new constraint CreateConstraintFromHit(world, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance, stepInput.SkinWidth, out SurfaceConstraintInfo newConstraint); // Resolve its penetration ResolveConstraintPenetration(ref newConstraint); // Write back constraints[constraintIndex] = newConstraint; } found = true; break; } } // Add penetrating hit not caught by collider cast if (!found) { CreateConstraint(stepInput.World, stepInput.Up, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance, stepInput.SkinWidth, maxSlopeCos, ref constraints); } } } } // Min delta time for solver to break float minDeltaTime = 0.0f; if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq) { // Min delta time to travel at least 1cm minDeltaTime = 0.01f / math.length(newVelocity); } // Solve float3 prevVelocity = newVelocity; float3 prevPosition = newPosition; SimplexSolver.Solve(remainingTime, minDeltaTime, up, stepInput.MaxMovementSpeed, constraints, ref newPosition, ref newVelocity, out float integratedTime); // Apply impulses to hit bodies and store collision events if (affectBodies || collisionEvents.IsCreated) { CalculateAndStoreDeferredImpulsesAndCollisionEvents(stepInput, affectBodies, characterMass, prevVelocity, constraints, ref deferredImpulseWriter, collisionEvents); } // Calculate new displacement float3 newDisplacement = newPosition - prevPosition; // If simplex solver moved the character we need to re-cast to make sure it can move to new position if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon) { // Check if we can walk to the position simplex solver has suggested var newCollector = new CharacterControllerClosestHitCollector <ColliderCastHit>(constraints, world, stepInput.RigidBodyIndex, 1.0f); ColliderCastInput input = new ColliderCastInput() { Collider = collider, Orientation = orientation, Start = prevPosition, End = prevPosition + newDisplacement }; world.CastCollider(input, ref newCollector); if (newCollector.NumHits > 0) { ColliderCastHit hit = newCollector.ClosestHit; // Move character along the newDisplacement direction until it reaches this new contact { Assert.IsTrue(hit.Fraction >= 0.0f && hit.Fraction <= 1.0f); integratedTime *= hit.Fraction; newPosition = prevPosition + newDisplacement * hit.Fraction; } } } // Reduce remaining time remainingTime -= integratedTime; // Write back position so that the distance query will update results transform.pos = newPosition; } // Write back final velocity linearVelocity = newVelocity; }
private static unsafe void CalculateAndStoreDeferredImpulses( CharacterControllerStepInput stepInput, float characterMass, float3 linearVelocity, int numConstraints, ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter) { PhysicsWorld world = stepInput.World; for (int i = 0; i < numConstraints; i++) { SurfaceConstraintInfo constraint = constraints[i]; int rigidBodyIndex = constraint.RigidBodyIndex; if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies) { // Invalid and static bodies should be skipped continue; } RigidBody body = world.Bodies[rigidBodyIndex]; float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition); pointRelVel -= linearVelocity; float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal); // Required velocity change float deltaVelocity = -projectedVelocity * stepInput.Damping; float distance = constraint.Plane.Distance; if (distance < 0.0f) { deltaVelocity += (distance / stepInput.DeltaTime) * stepInput.Tau; } // Calculate impulse MotionVelocity mv = world.MotionVelocities[rigidBodyIndex]; float3 impulse = float3.zero; if (deltaVelocity < 0.0f) { // Impulse magnitude float impulseMagnitude = 0.0f; { float objectMassInv = GetInvMassAtPoint(constraint.HitPosition, constraint.Plane.Normal, body, mv); impulseMagnitude = deltaVelocity / objectMassInv; } impulse = impulseMagnitude * constraint.Plane.Normal; } // Add gravity { // Effect of gravity on character velocity in the normal direction float3 charVelDown = stepInput.Gravity * stepInput.DeltaTime; float relVelN = math.dot(charVelDown, constraint.Plane.Normal); // Subtract separation velocity if separating contact { bool isSeparatingContact = projectedVelocity < 0.0f; float newRelVelN = relVelN - projectedVelocity; relVelN = math.select(relVelN, newRelVelN, isSeparatingContact); } // If resulting velocity is negative, an impulse is applied to stop the character // from falling into the body { float3 newImpulse = impulse; newImpulse += relVelN * characterMass * constraint.Plane.Normal; impulse = math.select(impulse, newImpulse, relVelN < 0.0f); } } // Store impulse deferredImpulseWriter.Write( new DeferredCharacterControllerImpulse() { Entity = body.Entity, Impulse = impulse, Point = constraint.HitPosition }); } }
public static unsafe void CollideAndIntegrate( CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider, MaxHitsCollector <DistanceHit> distanceHitsCollector, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints, ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter) { // Copy parameters float deltaTime = stepInput.DeltaTime; float3 gravity = stepInput.Gravity; float3 up = stepInput.Up; PhysicsWorld world = stepInput.World; float remainingTime = deltaTime; float3 lastDisplacement = linearVelocity * remainingTime; float3 newPosition = transform.pos; quaternion orientation = transform.rot; float3 newVelocity = linearVelocity; float maxSlopeCos = math.cos(stepInput.MaxSlope); // Iterate over hits and create constraints from them int numDistanceConstraints = 0; for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++) { DistanceHit hit = distanceHitsCollector.AllHits[hitIndex]; CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint); // Check if max slope plane is required float verticalComponent = math.dot(constraint.Plane.Normal, up); bool shouldAddPlane = verticalComponent > SimplexSolver.c_SimplexSolverEpsilon && verticalComponent < maxSlopeCos; if (shouldAddPlane) { AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numDistanceConstraints); } // Add original constraint to the list constraints[numDistanceConstraints++] = constraint; } const float timeEpsilon = 0.000001f; for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++) { int numConstraints = numDistanceConstraints; float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f; // Then do a collider cast (but not in first iteration) if (i > 0) { float3 displacement = lastDisplacement + gravityMovement; MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits); ColliderCastInput input = new ColliderCastInput() { Collider = collider, Orientation = orientation, Start = newPosition, End = newPosition + displacement, }; world.CastCollider(input, ref collector); // Iterate over hits and create constraints from them for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++) { ColliderCastHit hit = collector.AllHits[hitIndex]; bool found = false; for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++) { DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex]; if (dHit.RigidBodyIndex == hit.RigidBodyIndex && dHit.ColliderKey.Equals(hit.ColliderKey)) { found = true; break; } } // Skip duplicate hits if (!found) { CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint); // Check if max slope plane is required float verticalComponent = math.dot(constraint.Plane.Normal, up); bool shouldAddPlane = verticalComponent > SimplexSolver.c_SimplexSolverEpsilon && verticalComponent < maxSlopeCos; if (shouldAddPlane) { AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numConstraints); } // Add original constraint to the list constraints[numConstraints++] = constraint; } } } // Solve float3 prevVelocity = newVelocity; float3 prevPosition = newPosition; SimplexSolver.Solve(world, remainingTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime); // Apply impulses to hit bodies if (affectBodies) { CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter); } float3 newDisplacement = newPosition - prevPosition; // Check if we can walk to the position simplex solver has suggested MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits); int newContactIndex = -1; // If simplex solver moved the character we need to re-cast to make sure it can move to new position if (math.lengthsq(newDisplacement) > SimplexSolver.c_SimplexSolverEpsilon) { float3 displacement = newDisplacement + gravityMovement; ColliderCastInput input = new ColliderCastInput() { Collider = collider, Orientation = orientation, Start = prevPosition, End = prevPosition + displacement }; world.CastCollider(input, ref newCollector); for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++) { ColliderCastHit hit = newCollector.AllHits[hitIndex]; bool found = false; for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++) { SurfaceConstraintInfo constraint = constraints[constraintIndex]; if (constraint.RigidBodyIndex == hit.RigidBodyIndex && constraint.ColliderKey.Equals(hit.ColliderKey)) { found = true; break; } } if (!found) { newContactIndex = hitIndex; break; } } } // Move character along the newDisplacement direction until it reaches this new contact if (newContactIndex >= 0) { ColliderCastHit newContact = newCollector.AllHits[newContactIndex]; Assert.IsTrue(newContact.Fraction >= 0.0f && newContact.Fraction <= 1.0f); integratedTime *= newContact.Fraction; newPosition = prevPosition + newDisplacement * newContact.Fraction; } remainingTime -= integratedTime; // Remember last displacement for next iteration lastDisplacement = newVelocity * remainingTime; } // Write back position and velocity transform.pos = newPosition; linearVelocity = newVelocity; }
public static unsafe void CollideAndIntegrate( CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints, int numConstraints, ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter) { // Copy parameters float deltaTime = stepInput.DeltaTime; float3 gravity = stepInput.Gravity; float3 up = stepInput.Up; PhysicsWorld world = stepInput.World; float remainingTime = deltaTime; float3 lastDisplacement = linearVelocity * remainingTime; float3 newPosition = transform.pos; quaternion orientation = transform.rot; float3 newVelocity = linearVelocity; float maxSlopeCos = math.cos(stepInput.MaxSlope); const float timeEpsilon = 0.000001f; for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++) { float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f; // Then do a collider cast (but not in first iteration) if (i > 0) { int numCastConstraints = 0; float3 displacement = lastDisplacement + gravityMovement; MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits); ColliderCastInput input = new ColliderCastInput() { Collider = collider, Orientation = orientation, Start = newPosition, End = newPosition + displacement * (1.0f + stepInput.ContactTolerance), }; world.CastCollider(input, ref collector); // Iterate over hits and create constraints from them for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++) { ColliderCastHit hit = collector.AllHits[hitIndex]; CreateConstraint(stepInput.World, stepInput.Up, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(lastDisplacement), stepInput.SkinWidth, maxSlopeCos, ref constraints, ref numCastConstraints); } numConstraints = numCastConstraints; } // Min delta time for solver to break float minDeltaTime = 0.0f; if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq) { // Min delta time to travel at least 1cm minDeltaTime = 0.01f / math.length(newVelocity); } // Solve float3 prevVelocity = newVelocity; float3 prevPosition = newPosition; SimplexSolver.Solve(world, remainingTime, minDeltaTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime); // Apply impulses to hit bodies if (affectBodies) { CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter); } float3 newDisplacement = newPosition - prevPosition; // Check if we can walk to the position simplex solver has suggested MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits); int newContactIndex = -1; // If simplex solver moved the character we need to re-cast to make sure it can move to new position if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon) { float3 displacement = newDisplacement + gravityMovement; ColliderCastInput input = new ColliderCastInput() { Collider = collider, Orientation = orientation, Start = prevPosition, End = prevPosition + displacement * (1.0f + stepInput.ContactTolerance) }; world.CastCollider(input, ref newCollector); float minFraction = float.MaxValue; for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++) { ColliderCastHit hit = newCollector.AllHits[hitIndex]; if (hit.Fraction < minFraction) { bool found = false; for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++) { SurfaceConstraintInfo constraint = constraints[constraintIndex]; if (constraint.RigidBodyIndex == hit.RigidBodyIndex && constraint.ColliderKey.Equals(hit.ColliderKey)) { found = true; break; } } if (!found) { minFraction = hit.Fraction; newContactIndex = hitIndex; } } } } // Move character along the newDisplacement direction until it reaches this new contact if (newContactIndex >= 0) { ColliderCastHit newContact = newCollector.AllHits[newContactIndex]; Assert.IsTrue(newContact.Fraction >= 0.0f && newContact.Fraction <= 1.0f); integratedTime *= newContact.Fraction; newPosition = prevPosition + newDisplacement * newContact.Fraction; } remainingTime -= integratedTime; // Remember last displacement for next iteration lastDisplacement = newVelocity * remainingTime; } // Write back position and velocity transform.pos = newPosition; linearVelocity = newVelocity; }
public static unsafe void SolveCollisionConstraints(PhysicsWorld world, float deltaTime, int maxIterations, float skinWidth, float maxSlope, Collider *collider, ref RigidTransform transform, ref float3 velocity, ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> colliderHits, ref NativeArray <SurfaceConstraintInfo> surfaceConstraints) { float remainingTime = deltaTime; float3 previousDisplacement = velocity * remainingTime; float3 outPosition = transform.pos; float3 outVelocity = velocity; quaternion orientation = transform.rot; const float timeEpsilon = 0.000001f; for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++) { MaxHitCollector <DistanceHit> distanceHitCollector = new MaxHitCollector <DistanceHit>(skinWidth, ref distanceHits); int constraintCount = 0; // Handle distance checks { ColliderDistanceInput input = new ColliderDistanceInput { Collider = collider, MaxDistance = skinWidth, Transform = new RigidTransform { pos = outPosition, rot = orientation } }; world.CalculateDistance(input, ref distanceHitCollector); for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++) { DistanceHit hit = distanceHitCollector.AllHits[hitIndex]; CreateConstraintFromHit(world, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, float3.zero, hit.SurfaceNormal, hit.Distance, deltaTime, out SurfaceConstraintInfo constraint); CreateSlopeConstraint(math.up(), math.cos(maxSlope), ref constraint, ref surfaceConstraints, ref constraintCount); surfaceConstraints[constraintCount++] = constraint; } } // Handle Collider { float3 displacement = previousDisplacement; MaxHitCollector <ColliderCastHit> colliderHitCollector = new MaxHitCollector <ColliderCastHit>(1.0f, ref colliderHits); ColliderCastInput input = new ColliderCastInput { Collider = collider, Position = outPosition, Direction = velocity, Orientation = orientation }; world.CastCollider(input, ref colliderHitCollector); for (int hitIndex = 0; hitIndex < colliderHitCollector.NumHits; hitIndex++) { ColliderCastHit hit = colliderHitCollector.AllHits[hitIndex]; bool duplicate = false; for (int distanceHitIndex = 0; distanceHitIndex < distanceHitCollector.NumHits; distanceHitIndex++) { DistanceHit distanceHit = distanceHitCollector.AllHits[distanceHitIndex]; if (distanceHit.RigidBodyIndex == hit.RigidBodyIndex && distanceHit.ColliderKey.Equals(hit.ColliderKey)) { duplicate = true; break; } } if (!duplicate) { CreateConstraintFromHit(world, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, outVelocity, hit.SurfaceNormal, hit.Fraction * math.length(previousDisplacement), deltaTime, out SurfaceConstraintInfo constraint); CreateSlopeConstraint(math.up(), math.cos(maxSlope), ref constraint, ref surfaceConstraints, ref constraintCount); surfaceConstraints[constraintCount++] = constraint; } } } float3 previousPosition = outPosition; float3 previousVelocity = outVelocity; SimplexSolver.Solve(world, remainingTime, math.up(), constraintCount, ref surfaceConstraints, ref outPosition, ref outVelocity, out float integratedTime); float3 currentDisplacement = outPosition - previousPosition; MaxHitCollector <ColliderCastHit> displacementHitCollector = new MaxHitCollector <ColliderCastHit>(1.0f, ref colliderHits); int displacementContactIndex = -1; if (math.lengthsq(currentDisplacement) > SimplexSolver.c_SimplexSolverEpsilon) { ColliderCastInput input = new ColliderCastInput { Collider = collider, Position = previousPosition, Direction = currentDisplacement, Orientation = orientation }; world.CastCollider(input, ref displacementHitCollector); for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++) { ColliderCastHit hit = displacementHitCollector.AllHits[hitIndex]; bool duplicate = false; for (int constrainIndex = 0; constrainIndex < constraintCount; constrainIndex++) { SurfaceConstraintInfo constraint = surfaceConstraints[constrainIndex]; if (constraint.RigidBodyIndex == hit.RigidBodyIndex && constraint.ColliderKey.Equals(hit.ColliderKey)) { duplicate = true; break; } if (!duplicate) { displacementContactIndex = hitIndex; break; } } } if (displacementContactIndex >= 0) { ColliderCastHit newContact = displacementHitCollector.AllHits[displacementContactIndex]; float fraction = newContact.Fraction / math.length(currentDisplacement); integratedTime *= fraction; float3 displacement = currentDisplacement * fraction; outPosition = previousPosition + displacement; } } remainingTime -= integratedTime; previousDisplacement = outVelocity * remainingTime; } transform.pos = outPosition; velocity = outVelocity; }
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; }