public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, FP t1) { _localPoint = FPVector2.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; FPVector2 localPointA = _proxyA.Vertices[cache.IndexA[0]]; FPVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; FPVector2 pointA = MathUtils.Mul(ref xfA, localPointA); FPVector2 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; FPVector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; FPVector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; FPVector2 a = localPointB2 - localPointB1; _axis = new FPVector2(a.y, -a.x); _axis.Normalize(); FPVector2 normal = MathUtils.Mul(ref xfB.q, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); FPVector2 pointB = MathUtils.Mul(ref xfB, _localPoint); FPVector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; FPVector2 pointA = MathUtils.Mul(ref xfA, localPointA); FP s = FPVector2.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; FPVector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]]; FPVector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]]; FPVector2 a = localPointA2 - localPointA1; _axis = new FPVector2(a.y, -a.x); _axis.Normalize(); FPVector2 normal = MathUtils.Mul(ref xfA.q, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); FPVector2 pointA = MathUtils.Mul(ref xfA, _localPoint); FPVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; FPVector2 pointB = MathUtils.Mul(ref xfB, localPointB); FP s = FPVector2.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. }
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); } }
/// <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 = Spax.FPMath.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); } }