public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, FP t1) { SeparationFunction._localPoint = TSVector2.zero; SeparationFunction._proxyA = proxyA; SeparationFunction._proxyB = proxyB; int count = (int)cache.Count; Debug.Assert(0 < count && count < 3); SeparationFunction._sweepA = sweepA; SeparationFunction._sweepB = sweepB; Transform transform; SeparationFunction._sweepA.GetTransform(out transform, t1); Transform transform2; SeparationFunction._sweepB.GetTransform(out transform2, t1); bool flag = count == 1; if (flag) { SeparationFunction._type = SeparationFunctionType.Points; TSVector2 v = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[0]]; TSVector2 v2 = SeparationFunction._proxyB.Vertices[(int)cache.IndexB[0]]; TSVector2 value = MathUtils.Mul(ref transform, v); TSVector2 value2 = MathUtils.Mul(ref transform2, v2); SeparationFunction._axis = value2 - value; SeparationFunction._axis.Normalize(); } else { bool flag2 = cache.IndexA[0] == cache.IndexA[1]; if (flag2) { SeparationFunction._type = SeparationFunctionType.FaceB; TSVector2 tSVector = proxyB.Vertices[(int)cache.IndexB[0]]; TSVector2 tSVector2 = proxyB.Vertices[(int)cache.IndexB[1]]; TSVector2 tSVector3 = tSVector2 - tSVector; SeparationFunction._axis = new TSVector2(tSVector3.y, -tSVector3.x); SeparationFunction._axis.Normalize(); TSVector2 value3 = MathUtils.Mul(ref transform2.q, SeparationFunction._axis); SeparationFunction._localPoint = 0.5f * (tSVector + tSVector2); TSVector2 value4 = MathUtils.Mul(ref transform2, SeparationFunction._localPoint); TSVector2 v3 = proxyA.Vertices[(int)cache.IndexA[0]]; TSVector2 value5 = MathUtils.Mul(ref transform, v3); FP x = TSVector2.Dot(value5 - value4, value3); bool flag3 = x < 0f; if (flag3) { SeparationFunction._axis = -SeparationFunction._axis; } } else { SeparationFunction._type = SeparationFunctionType.FaceA; TSVector2 tSVector4 = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[0]]; TSVector2 tSVector5 = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[1]]; TSVector2 tSVector6 = tSVector5 - tSVector4; SeparationFunction._axis = new TSVector2(tSVector6.y, -tSVector6.x); SeparationFunction._axis.Normalize(); TSVector2 value6 = MathUtils.Mul(ref transform.q, SeparationFunction._axis); SeparationFunction._localPoint = 0.5f * (tSVector4 + tSVector5); TSVector2 value7 = MathUtils.Mul(ref transform, SeparationFunction._localPoint); TSVector2 v4 = SeparationFunction._proxyB.Vertices[(int)cache.IndexB[0]]; TSVector2 value8 = MathUtils.Mul(ref transform2, v4); FP x2 = TSVector2.Dot(value8 - value7, value6); bool flag4 = x2 < 0f; if (flag4) { SeparationFunction._axis = -SeparationFunction._axis; } } } }
public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input) { output = default(TOIOutput); output.State = TOIOutputState.Unknown; output.T = input.TMax; Sweep sweepA = input.SweepA; Sweep sweepB = input.SweepB; sweepA.Normalize(); sweepB.Normalize(); FP tMax = input.TMax; FP x = input.ProxyA.Radius + input.ProxyB.Radius; FP fP = TSMath.Max(Settings.LinearSlop, x - 3f * Settings.LinearSlop); FP y = 0.25f * Settings.LinearSlop; Debug.Assert(fP > y); FP fP2 = 0f; int num = 0; TimeOfImpact._distanceInput = (TimeOfImpact._distanceInput ?? new DistanceInput()); TimeOfImpact._distanceInput.ProxyA = input.ProxyA; TimeOfImpact._distanceInput.ProxyB = input.ProxyB; TimeOfImpact._distanceInput.UseRadii = false; while (true) { Transform transformA; sweepA.GetTransform(out transformA, fP2); Transform transformB; sweepB.GetTransform(out transformB, fP2); TimeOfImpact._distanceInput.TransformA = transformA; TimeOfImpact._distanceInput.TransformB = transformB; DistanceOutput distanceOutput; SimplexCache simplexCache; Distance.ComputeDistance(out distanceOutput, out simplexCache, TimeOfImpact._distanceInput); bool flag = distanceOutput.Distance <= 0f; if (flag) { break; } bool flag2 = distanceOutput.Distance < fP + y; if (flag2) { goto Block_3; } SeparationFunction.Set(ref simplexCache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, fP2); bool flag3 = false; FP fP3 = tMax; int num2 = 0; while (true) { int indexA; int indexB; FP x2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, fP3); bool flag4 = x2 > fP + y; if (flag4) { goto Block_4; } bool flag5 = x2 > fP - y; if (flag5) { goto Block_5; } FP fP4 = SeparationFunction.Evaluate(indexA, indexB, fP2); bool flag6 = fP4 < fP - y; if (flag6) { goto Block_6; } bool flag7 = fP4 <= fP + y; if (flag7) { goto Block_7; } int num3 = 0; FP fP5 = fP2; FP fP6 = fP3; FP fP7; bool flag11; do { bool flag8 = (num3 & 1) != 0; if (flag8) { fP7 = fP5 + (fP - fP4) * (fP6 - fP5) / (x2 - fP4); } else { fP7 = 0.5f * (fP5 + fP6); } num3++; FP fP8 = SeparationFunction.Evaluate(indexA, indexB, fP7); bool flag9 = FP.Abs(fP8 - fP) < y; if (flag9) { goto Block_9; } bool flag10 = fP8 > fP; if (flag10) { fP5 = fP7; fP4 = fP8; } else { fP6 = fP7; x2 = fP8; } flag11 = (num3 == 50); }while (!flag11); IL_373: num2++; bool flag12 = num2 == Settings.MaxPolygonVertices; if (flag12) { break; } continue; Block_9: fP3 = fP7; goto IL_373; } IL_396: num++; bool flag13 = flag3; if (flag13) { goto Block_13; } bool flag14 = num == 20; if (flag14) { goto Block_14; } continue; Block_4: output.State = TOIOutputState.Seperated; output.T = tMax; flag3 = true; goto IL_396; Block_5: fP2 = fP3; goto IL_396; Block_6: output.State = TOIOutputState.Failed; output.T = fP2; flag3 = true; goto IL_396; Block_7: output.State = TOIOutputState.Touching; output.T = fP2; flag3 = true; goto IL_396; } output.State = TOIOutputState.Overlapped; output.T = 0f; return; Block_3: output.State = TOIOutputState.Touching; output.T = fP2; Block_13: return; Block_14: output.State = TOIOutputState.Failed; output.T = fP2; }
public void Clone(Body body) { this._angularDamping = body._angularDamping; this._bodyType = body._bodyType; this._inertia = body._inertia; this._linearDamping = body._linearDamping; this._mass = body._mass; this._sleepingAllowed = body._sleepingAllowed; this._awake = body._awake; this._fixedRotation = body._fixedRotation; this._enabled = body._enabled; this._angularVelocity = body._angularVelocity; this._linearVelocity = body._linearVelocity; this._force = body._force; this._invI = body._invI; this._invMass = body._invMass; this._sleepTime = body._sleepTime; this._sweep.A = body._sweep.A; this._sweep.A0 = body._sweep.A0; this._sweep.Alpha0 = body._sweep.Alpha0; this._sweep.C = body._sweep.C; this._sweep.C0 = body._sweep.C0; this._sweep.LocalCenter = body._sweep.LocalCenter; this._torque = body._torque; this._xf.p = body._xf.p; this._xf.q = body._xf.q; this._island = body._island; this.disabled = body.disabled; this.GravityScale = body.GravityScale; this.IsBullet = body.IsBullet; this.IgnoreCCD = body.IgnoreCCD; this.IgnoreGravity = body.IgnoreGravity; this.prevKinematicMass = body.prevKinematicMass; this.prevKinematicInvMass = body.prevKinematicInvMass; this.prevKinematicInertia = body.prevKinematicInertia; this.prevKinematicInvI = body.prevKinematicInvI; this.prevKinematicSweep = body.prevKinematicSweep; for (int index = 0, length = shapesClone.Count; index < length; index++) { poolClone2D.GiveBack(shapesClone[index]); } this.shapesClone.Clear(); List <Physics2D.Fixture> fixtureList = body.FixtureList; for (int index = 0, length = fixtureList.Count; index < length; index++) { GenericShapeClone2D shapeClone = poolClone2D.GetNew(); shapeClone.Clone(body.FixtureList[index].Shape); this.shapesClone.Add(shapeClone); } if (body.ContactList == null) { this.contactEdgeClone = null; } else { this.contactEdgeClone = WorldClone2D.poolContactEdgeClone.GetNew(); this.contactEdgeClone.Clone(body.ContactList); } }
private void SolveTOI(ref TimeStep step) { this.Island.Reset(64, 32, 0, this.ContactManager); bool stepComplete = this._stepComplete; if (stepComplete) { for (int i = 0; i < this.BodyList.Count; i++) { this.BodyList[i]._island = false; this.BodyList[i]._sweep.Alpha0 = 0f; } for (int j = 0; j < this.ContactManager.ContactList.Count; j++) { Contact contact = this.ContactManager.ContactList[j]; contact.IslandFlag = false; contact.TOIFlag = false; contact._toiCount = 0; contact._toi = 1f; } } while (true) { Contact contact2 = null; FP fP = 1f; for (int k = 0; k < this.ContactManager.ContactList.Count; k++) { Contact contact3 = this.ContactManager.ContactList[k]; bool flag = !contact3.Enabled; if (!flag) { bool flag2 = contact3._toiCount > 8; if (!flag2) { bool tOIFlag = contact3.TOIFlag; FP fP2; if (tOIFlag) { fP2 = contact3._toi; } else { Fixture fixtureA = contact3.FixtureA; Fixture fixtureB = contact3.FixtureB; bool flag3 = fixtureA.IsSensor || fixtureB.IsSensor; if (flag3) { goto IL_424; } Body body = fixtureA.Body; Body body2 = fixtureB.Body; BodyType bodyType = body.BodyType; BodyType bodyType2 = body2.BodyType; Debug.Assert(bodyType == BodyType.Dynamic || bodyType2 == BodyType.Dynamic); bool flag4 = body.Awake && bodyType > BodyType.Static; bool flag5 = body2.Awake && bodyType2 > BodyType.Static; bool flag6 = !flag4 && !flag5; if (flag6) { goto IL_424; } bool flag7 = (body.IsBullet || bodyType != BodyType.Dynamic) && (fixtureA.IgnoreCCDWith & fixtureB.CollisionCategories) == Category.None && !body.IgnoreCCD; bool flag8 = (body2.IsBullet || bodyType2 != BodyType.Dynamic) && (fixtureB.IgnoreCCDWith & fixtureA.CollisionCategories) == Category.None && !body2.IgnoreCCD; bool flag9 = !flag7 && !flag8; if (flag9) { goto IL_424; } FP alpha = body._sweep.Alpha0; bool flag10 = body._sweep.Alpha0 < body2._sweep.Alpha0; if (flag10) { alpha = body2._sweep.Alpha0; body._sweep.Advance(alpha); } else { bool flag11 = body2._sweep.Alpha0 < body._sweep.Alpha0; if (flag11) { alpha = body._sweep.Alpha0; body2._sweep.Advance(alpha); } } Debug.Assert(alpha < 1f); this._input.ProxyA.Set(fixtureA.Shape, contact3.ChildIndexA); this._input.ProxyB.Set(fixtureB.Shape, contact3.ChildIndexB); this._input.SweepA = body._sweep; this._input.SweepB = body2._sweep; this._input.TMax = 1f; TOIOutput tOIOutput; TimeOfImpact.CalculateTimeOfImpact(out tOIOutput, this._input); FP t = tOIOutput.T; bool flag12 = tOIOutput.State == TOIOutputState.Touching; if (flag12) { fP2 = TSMath.Min(alpha + (1f - alpha) * t, 1f); } else { fP2 = 1f; } contact3._toi = fP2; contact3.TOIFlag = true; } bool flag13 = fP2 < fP; if (flag13) { contact2 = contact3; fP = fP2; } } } IL_424 :; } bool flag14 = contact2 == null || 1f - 10f * Settings.Epsilon < fP; if (flag14) { break; } Fixture fixtureA2 = contact2.FixtureA; Fixture fixtureB2 = contact2.FixtureB; Body body3 = fixtureA2.Body; Body body4 = fixtureB2.Body; Sweep sweep = body3._sweep; Sweep sweep2 = body4._sweep; body3.Advance(fP); body4.Advance(fP); contact2.Update(this.ContactManager); contact2.TOIFlag = false; contact2._toiCount++; bool flag15 = !contact2.Enabled || !contact2.IsTouching; if (flag15) { contact2.Enabled = false; body3._sweep = sweep; body4._sweep = sweep2; body3.SynchronizeTransform(); body4.SynchronizeTransform(); } else { body3.Awake = true; body4.Awake = true; this.Island.Clear(); this.Island.Add(body3); this.Island.Add(body4); this.Island.Add(contact2); body3._island = true; body4._island = true; contact2.IslandFlag = true; Body[] array = new Body[] { body3, body4 }; for (int l = 0; l < 2; l++) { Body body5 = array[l]; bool flag16 = body5.BodyType == BodyType.Dynamic; if (flag16) { for (ContactEdge contactEdge = body5.ContactList; contactEdge != null; contactEdge = contactEdge.Next) { Contact contact4 = contactEdge.Contact; bool flag17 = this.Island.BodyCount == this.Island.BodyCapacity; if (flag17) { break; } bool flag18 = this.Island.ContactCount == this.Island.ContactCapacity; if (flag18) { break; } bool islandFlag = contact4.IslandFlag; if (!islandFlag) { Body other = contactEdge.Other; bool flag19 = other.BodyType == BodyType.Dynamic && !body5.IsBullet && !other.IsBullet; if (!flag19) { bool flag20 = contact4.FixtureA.IsSensor || contact4.FixtureB.IsSensor; if (!flag20) { Sweep sweep3 = other._sweep; bool flag21 = !other._island; if (flag21) { other.Advance(fP); } contact4.Update(this.ContactManager); bool flag22 = !contact4.Enabled; if (flag22) { other._sweep = sweep3; other.SynchronizeTransform(); } else { bool flag23 = !contact4.IsTouching; if (flag23) { other._sweep = sweep3; other.SynchronizeTransform(); } else { contact4.IslandFlag = true; this.Island.Add(contact4); bool island = other._island; if (!island) { other._island = true; bool flag24 = other.BodyType > BodyType.Static; if (flag24) { other.Awake = true; } this.Island.Add(other); } } } } } } } } } TimeStep timeStep; timeStep.dt = (1f - fP) * step.dt; timeStep.inv_dt = 1f / timeStep.dt; timeStep.dtRatio = 1f; this.Island.SolveTOI(ref timeStep, body3.IslandIndex, body4.IslandIndex, false); for (int m = 0; m < this.Island.BodyCount; m++) { Body body6 = this.Island.Bodies[m]; body6._island = false; bool flag25 = body6.BodyType != BodyType.Dynamic; if (!flag25) { body6.SynchronizeFixtures(); for (ContactEdge contactEdge2 = body6.ContactList; contactEdge2 != null; contactEdge2 = contactEdge2.Next) { contactEdge2.Contact.TOIFlag = false; contactEdge2.Contact.IslandFlag = false; } } } this.ContactManager.FindNewContacts(); } } this._stepComplete = true; }
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, FP t1) { _localPoint = TSVector2.zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; TSVector2 localPointA = _proxyA.Vertices[cache.IndexA[0]]; TSVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; TSVector2 pointA = MathUtils.Mul(ref xfA, localPointA); TSVector2 pointB = MathUtils.Mul(ref xfB, localPointB); _axis = pointB - pointA; _axis.Normalize(); } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; TSVector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; TSVector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; TSVector2 a = localPointB2 - localPointB1; _axis = new TSVector2(a.y, -a.x); _axis.Normalize(); TSVector2 normal = MathUtils.Mul(ref xfB.q, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); TSVector2 pointB = MathUtils.Mul(ref xfB, _localPoint); TSVector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; TSVector2 pointA = MathUtils.Mul(ref xfA, localPointA); FP s = TSVector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; } } else { // Two points on A and one or two points on B. _type = SeparationFunctionType.FaceA; TSVector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]]; TSVector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]]; TSVector2 a = localPointA2 - localPointA1; _axis = new TSVector2(a.y, -a.x); _axis.Normalize(); TSVector2 normal = MathUtils.Mul(ref xfA.q, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); TSVector2 pointA = MathUtils.Mul(ref xfA, _localPoint); TSVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; TSVector2 pointB = MathUtils.Mul(ref xfB, localPointB); FP s = TSVector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; } } //FPE note: the returned value that used to be here has been removed, as it was not used. }
/// <summary> /// Compute the upper bound on time before two shapes penetrate. Time is represented as /// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, /// non-tunneling collision. If you change the time interval, you should call this function /// again. /// Note: use Distance() to compute the contact point and normal at the time of impact. /// </summary> /// <param name="output">The output.</param> /// <param name="input">The input.</param> public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input) { if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++TOICalls; } output = new TOIOutput(); output.State = TOIOutputState.Unknown; output.T = input.TMax; Sweep sweepA = input.SweepA; Sweep sweepB = input.SweepB; // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); FP tMax = input.TMax; FP totalRadius = input.ProxyA.Radius + input.ProxyB.Radius; FP target = TrueSync.TSMath.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); FP tolerance = 0.25f * Settings.LinearSlop; Debug.Assert(target > tolerance); FP t1 = 0.0f; const int k_maxIterations = 20; int iter = 0; // Prepare input for distance query. _distanceInput = _distanceInput ?? new DistanceInput(); _distanceInput.ProxyA = input.ProxyA; _distanceInput.ProxyB = input.ProxyB; _distanceInput.UseRadii = false; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). for (; ;) { Transform xfA, xfB; sweepA.GetTransform(out xfA, t1); sweepB.GetTransform(out xfB, t1); // Get the distance between shapes. We can also use the results // to get a separating axis. _distanceInput.TransformA = xfA; _distanceInput.TransformB = xfB; DistanceOutput distanceOutput; SimplexCache cache; Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.Distance <= 0.0f) { // Failure! output.State = TOIOutputState.Overlapped; output.T = 0.0f; break; } if (distanceOutput.Distance < target + tolerance) { // Victory! output.State = TOIOutputState.Touching; output.T = t1; break; } SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1); // Compute the TOI on the separating axis. We do this by successively // resolving the deepest point. This loop is bounded by the number of vertices. bool done = false; FP t2 = tMax; int pushBackIter = 0; for (; ;) { // Find the deepest point at t2. Store the witness point indices. int indexA, indexB; FP s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! output.State = TOIOutputState.Seperated; output.T = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. FP s1 = SeparationFunction.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. if (s1 < target - tolerance) { output.State = TOIOutputState.Failed; output.T = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // Victory! t1 should hold the TOI (could be 0.0). output.State = TOIOutputState.Touching; output.T = t1; done = true; break; } // Compute 1D root of: f(x) - target = 0 int rootIterCount = 0; FP a1 = t1, a2 = t2; for (; ;) { // Use a mix of the secant rule and bisection. FP t; if ((rootIterCount & 1) != 0) { // Secant rule to improve convergence. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. t = 0.5f * (a1 + a2); } ++rootIterCount; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++TOIRootIters; } FP s = SeparationFunction.Evaluate(indexA, indexB, t); if (FP.Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = t; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = t; s1 = s; } else { a2 = t; s2 = s; } if (rootIterCount == 50) { break; } } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); } ++pushBackIter; if (pushBackIter == Settings.MaxPolygonVertices) { break; } } ++iter; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++TOIIters; } if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.T = t1; break; } } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { TOIMaxIters = Math.Max(TOIMaxIters, iter); } }