void CollisionResponse(float deltaTime) { for (int i = 0; i < intersections.Length; i++) { Intersection intersection = intersections[i]; PhysicsBodyComponent p1 = intersection.Object1; PhysicsBodyComponent p2 = intersection.Object2; AxisAlignedBoundingBox p1Collider = p1.GetCollider(); AxisAlignedBoundingBox p1DeltaCollider = p1.GetColliderAt(p1.Delta.FinalPosition); AxisAlignedBoundingBox p2Collider = p2.GetCollider(); AxisAlignedBoundingBox p2DeltaCollider = p2.GetColliderAt(p2.Delta.FinalPosition); if (intersection.Type == IntersectionType.Rigid) { // Update the intersection data, so that previous updates for // these objects are applied intersection.UpdateFromDelta(); // If both entry times are invalid, the objects aren't intersecting anymore, // so ignore the collision. if (intersection.Object1EntryTime == 1 && intersection.Object2EntryTime == 1) { continue; } // Handle the collision based on static states if (intersection.Object2.IsStatic) { // If we are past the first sweep pass for this object, // and this intersection is no longer valid, then ignore it. if (p1.Delta.DeltaPass > 0) { if (!p1DeltaCollider.Intersects(p2Collider)) { continue; } } // Gather intersection data Vector3 surfaceNormal = intersection.Object2Normal; float collisionTime = intersection.Object1EntryTime; float remainingTime = 1f - collisionTime; float stepDist = intersection.Resolver.StepDistance(p1DeltaCollider, p2Collider); //if (surfaceNormal.Y == 0) // Diagnostics.DashCMD.WriteLine("{0} | {1}", stepDist.ToString(), Math.Min(p1.Delta.MaxStep, p1.MaxStep)); //Diagnostics.DashCMD.WriteLine(surfaceNormal); // Try to step on object if (p1.CanStep && p2.CanBeSteppedOn && surfaceNormal.Y == 0 && stepDist >= 0 && stepDist <= Math.Min(p1.Delta.MaxStep, p1.MaxStep)) { // Step onto object p1.Delta.FinalPosition.Y += stepDist + 0.001f; p1.Delta.FinalVelocity.Y = 0; p1.Delta.IsGrounded = p1.Delta.IsGrounded || true; p1.Delta.Stepped = true; } else // Normal collision resolve { // Calculate the compensation in position from the collision Vector3 compensation = surfaceNormal * (Maths.Abs(p1.Delta.FinalVelocity * deltaTime)) * remainingTime; p1.Delta.FinalPosition += compensation; // If this normal moved the object anywhere upward, // that object is now considered grounded if (surfaceNormal.Y > 0) { p1.Delta.IsGrounded = p1.Delta.IsGrounded || true; } // Fix the velocity if (p1.BounceOnWallCollision || p1.BounceOnVerticalCollision) { if ((surfaceNormal.X != 0 || surfaceNormal.Z != 0) && p1.BounceOnWallCollision) { if (surfaceNormal.X != 0) { p1.Delta.FinalVelocity.X *= -(1f - p1.HorizontalBounceFalloff); } if (surfaceNormal.Z != 0) { p1.Delta.FinalVelocity.Z *= -(1f - p1.HorizontalBounceFalloff); } } else if (surfaceNormal.Y != 0 && p1.BounceOnVerticalCollision) { p1.Delta.FinalVelocity.Y *= -(1f - p1.VerticalBounceFalloff); p1.Delta.FinalVelocity.X *= p1.InverseFriction; p1.Delta.FinalVelocity.Z *= p1.InverseFriction; } else { intersection.Resolver.FixVelocity(ref p1.Delta.FinalVelocity, surfaceNormal); } } else { intersection.Resolver.FixVelocity(ref p1.Delta.FinalVelocity, surfaceNormal); } } // Another delta pass has occured p1.Delta.DeltaPass++; p1.OnCollide(p2); p2.OnCollide(p1); } else if (intersection.Object1.IsStatic) { // If we are past the first sweep pass for this object, // and this intersection is no longer valid, then ignore it. if (p2.Delta.DeltaPass > 0) { if (!p2DeltaCollider.Intersects(p1Collider)) { continue; } } // Gather intersection data Vector3 surfaceNormal = intersection.Object1Normal; float collisionTime = intersection.Object2EntryTime; float remainingTime = 1f - collisionTime; float stepDist = intersection.Resolver.StepDistance(p2DeltaCollider, p1Collider); // Try to step on object if (p2.CanStep && p1.CanBeSteppedOn && surfaceNormal.Y == 0 && stepDist >= 0 && stepDist <= Math.Min(p2.Delta.MaxStep, p2.MaxStep)) { // Step onto object p2.Delta.FinalPosition.Y += stepDist + 0.001f; p2.Delta.FinalVelocity.Y = 0; p2.Delta.IsGrounded = p2.Delta.IsGrounded || true; p2.Delta.Stepped = true; } else // Normal collision resolve { // Calculate the compensation in position from the collision Vector3 compensation = surfaceNormal * (Maths.Abs(p2.Delta.FinalVelocity * deltaTime)) * remainingTime; p2.Delta.FinalPosition += compensation; // If this normal moved the object anywhere upward, // that object is now considered grounded if (surfaceNormal.Y > 0) { p2.Delta.IsGrounded = p2.Delta.IsGrounded || true; } // Fix the velocity if (p2.BounceOnWallCollision || p2.BounceOnVerticalCollision) { if ((surfaceNormal.X != 0 || surfaceNormal.Z != 0) && p2.BounceOnWallCollision) { if (surfaceNormal.X != 0) { p2.Delta.FinalVelocity.X *= -(1f - p2.HorizontalBounceFalloff); } if (surfaceNormal.Z != 0) { p2.Delta.FinalVelocity.Z *= -(1f - p2.HorizontalBounceFalloff); } } else if (surfaceNormal.Y != 0 && p2.BounceOnVerticalCollision) { p2.Delta.FinalVelocity.Y *= -(1f - p2.VerticalBounceFalloff); p1.Delta.FinalVelocity.X *= p1.InverseFriction; p1.Delta.FinalVelocity.Z *= p1.InverseFriction; } else { intersection.Resolver.FixVelocity(ref p2.Delta.FinalVelocity, surfaceNormal); } } else { intersection.Resolver.FixVelocity(ref p2.Delta.FinalVelocity, surfaceNormal); } } // Another delta pass has occured p2.Delta.DeltaPass++; p1.OnCollide(p2); p2.OnCollide(p1); } } else { // If we are past the first sweep pass for this object, // and this intersection is no longer valid, then ignore it. if ((!p1DeltaCollider.Intersects(p2Collider)) && (!p2DeltaCollider.Intersects(p1Collider))) { continue; } float p1DeltaMiddleX = p1.Delta.FinalPosition.X + p1.Size.X / 2f; float p2DeltaMiddleX = p2.Delta.FinalPosition.X + p2.Size.X / 2f; float p1DeltaMiddleZ = p1.Delta.FinalPosition.Z + p1.Size.Z / 2f; float p2DeltaMiddleZ = p2.Delta.FinalPosition.Z + p2.Size.Z / 2f; Vector3 p1Move = new Vector3((p2DeltaMiddleX - p1DeltaMiddleX), 0, (p2DeltaMiddleZ - p1DeltaMiddleZ)); Vector3 p2Move = new Vector3((p1DeltaMiddleX - p2DeltaMiddleX), 0, (p1DeltaMiddleZ - p2DeltaMiddleZ)); if (p1Move.X == p2Move.X) { p1Move.X += Maths.Random.Next((int)-p2.Size.X, (int)p2.Size.X) / deltaTime; } Vector2 masses = new Vector2((p2.Mass / p1.Mass), (p1.Mass / p2.Mass)) * 0.5f; if (p1.CanBePushedBySoft) { p1.Delta.FinalVelocity -= p1Move * masses.X; p1.Delta.FinalPosition = p1.Transform.Position + p1.Delta.FinalVelocity * deltaTime; } if (p2.CanBePushedBySoft) { p2.Delta.FinalVelocity -= p2Move * masses.Y; p2.Delta.FinalPosition = p2.Transform.Position + p2.Delta.FinalVelocity * deltaTime; } p1.OnCollide(p2); p2.OnCollide(p1); } } }
void Broadphase(IntersectionType forType, PhysicsBodyComponent p1, float deltaTime, int start) { if (!p1.IsEnabled || !p1.GameObject.IsEnabled) { return; } if (!p1.CanCollide) { return; } // For every object, check if it CAN intersect with any other object AxisAlignedBoundingBox p1Collider = p1.GetCollider(); AxisAlignedBoundingBox p1Broad = p1.GetBroadphase(); AxisAlignedBoundingBox?p1StepCollider = null; if (p1.CanStep) { p1StepCollider = p1.GetColliderAt(p1.Delta.StepPosition); } if (RecordProcessedBoundingBoxes) { LastProcessedBoundingBoxes.Add(p1Collider); LastProcessedBoundingBoxes.Add(p1Broad); } for (int k = start; k < physicsBodies.Count; k++) { PhysicsBodyComponent p2 = physicsBodies[k]; if (!p2.IsEnabled || !p2.GameObject.IsEnabled) { continue; } if (p2 == p1) { continue; } if (!p2.CanCollide) { continue; } // Get the type of intersection to determine how it should be handled IntersectionType type = GetIntersectionType(p1.IsStatic, p2.IsStatic); if (type != forType) { continue; } if (forType == IntersectionType.Soft && (!p2.CanCollideWithSoft || !p1.CanCollideWithSoft)) { continue; } AxisAlignedBoundingBox p2Collider = p2.GetCollider(); AxisAlignedBoundingBox p2Broad = p2.GetBroadphase(); AxisAlignedBoundingBox?p2StepCollider = null; if (p2.CanStep) { p2StepCollider = p2.GetColliderAt(p2.Delta.StepPosition); } if (RecordProcessedBoundingBoxes) { LastProcessedBoundingBoxes.Add(p2Collider); LastProcessedBoundingBoxes.Add(p2Broad); } // Check if the objects CAN intersect if (CanIntersect(p1.IsStatic, p1Collider, p1Broad, p2.IsStatic, p2Collider, p2Broad)) { // Calculate max step if (type == IntersectionType.Rigid) { CalculateMaxStep(p1, p2, p1Collider, p1StepCollider, p2Collider, p2StepCollider); } // Add the possible intersection Intersection intersect = new Intersection(p1, p2, sweepResolver, deltaTime, type); broadIntersections.Add(intersect); } } // Let extensions run foreach (IPhysicsEngineExtension ext in extensions) { if (!ext.IsActive || !ext.CanCheck(forType, p1.IsStatic)) { continue; } // Get intersections from extensions IEnumerable <PhysicsBodyComponent> intersections = ext.GetBroadphaseIntersections(p1Broad); foreach (PhysicsBodyComponent physOb in intersections) { AxisAlignedBoundingBox?physObStepCollider = null; if (physOb.CanStep) { physObStepCollider = physOb.GetColliderAt(physOb.Delta.StepPosition); } IntersectionType type = GetIntersectionType(p1.IsStatic, physOb.IsStatic); // Calculate max step if (type == IntersectionType.Rigid) { CalculateMaxStep(p1, physOb, p1Collider, p1StepCollider, physOb.GetCollider(), physObStepCollider); } // Add the possible intersection Intersection intersection = new Intersection(p1, physOb, sweepResolver, deltaTime, IntersectionType.Rigid); broadIntersections.Add(intersection); } } }