public void RayCast(Func <RayCastInput, int, FP> callback, ref RayCastInput input) { TSVector2 point = input.Point1; TSVector2 point2 = input.Point2; TSVector2 tSVector = point2 - point; Debug.Assert(tSVector.LengthSquared() > 0f); tSVector.Normalize(); TSVector2 value = MathUtils.Abs(new TSVector2(-tSVector.y, tSVector.x)); FP fP = input.MaxFraction; AABB aABB = default(AABB); TSVector2 tSVector2 = point + fP * (point2 - point); TSVector2.Min(ref point, ref tSVector2, out aABB.LowerBound); TSVector2.Max(ref point, ref tSVector2, out aABB.UpperBound); this._raycastStack.Clear(); this._raycastStack.Push(this._root); while (this._raycastStack.Count > 0) { int num = this._raycastStack.Pop(); bool flag = num == -1; if (!flag) { TreeNode <T> treeNode = this._nodes[num]; bool flag2 = !AABB.TestOverlap(ref treeNode.AABB, ref aABB); if (!flag2) { TSVector2 center = treeNode.AABB.Center; TSVector2 extents = treeNode.AABB.Extents; FP x = FP.Abs(TSVector2.Dot(new TSVector2(-tSVector.y, tSVector.x), point - center)) - TSVector2.Dot(value, extents); bool flag3 = x > 0f; if (!flag3) { bool flag4 = treeNode.IsLeaf(); if (flag4) { RayCastInput arg; arg.Point1 = input.Point1; arg.Point2 = input.Point2; arg.MaxFraction = fP; FP fP2 = callback(arg, num); bool flag5 = fP2 == 0f; if (flag5) { break; } bool flag6 = fP2 > 0f; if (flag6) { fP = fP2; TSVector2 value2 = point + fP * (point2 - point); aABB.LowerBound = TSVector2.Min(point, value2); aABB.UpperBound = TSVector2.Max(point, value2); } } else { this._raycastStack.Push(treeNode.Child1); this._raycastStack.Push(treeNode.Child2); } } } } } }
public void Solve(ref TimeStep step, ref FPVector2 gravity) { FP h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; FPVector2 c = b._sweep.C; FP a = b._sweep.A; FPVector2 v = b._linearVelocity; FP w = b._angularVelocity; // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. // FPE: Only apply gravity if the body wants it. if (b.IgnoreGravity) { v += h * (b._invMass * b._force); } else { v += h * (b.GravityScale * gravity + b._invMass * b._force); } w += h * b._invI * b._torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 //v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); //w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); v *= 1 / (1 + h * b.LinearDamping); w *= 1 / (1 + h * b.AngularDamping); } _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solver data SolverData solverData = new SolverData(); solverData.step = step; solverData.positions = _positions; solverData.velocities = _velocities; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint2D joint = _joints[j]; if (!joint.Enabled) { continue; } joint.SolveVelocityConstraints(ref solverData); joint.Validate(step.inv_dt); } _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { FPVector2 c = _positions[i].c; FP a = _positions[i].a; FPVector2 v = _velocities[i].v; FP w = _velocities[i].w; // Check for large velocities FPVector2 translation = h * v; if (FPVector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { FP ratio = Settings.MaxTranslation / translation.magnitude; v *= ratio; } FP rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { FP ratio = Settings.MaxRotation / FP.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solve position constraints bool positionSolved = false; for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint2D joint = _joints[j]; if (!joint.Enabled) { continue; } bool jointOkay = joint.SolvePositionConstraints(ref solverData); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body._sweep.C = _positions[i].c; body._sweep.A = _positions[i].a; body._linearVelocity = _velocities[i].v; body._angularVelocity = _velocities[i].w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); if (Settings.AllowSleep) { FP minSleepTime = Settings.MaxFP; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || FPVector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += h; minSleepTime = KBEngine.FPMath.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; _positions[i].c = b._sweep.C; _positions[i].a = b._sweep.A; _velocities[i].v = b._linearVelocity; _velocities[i].w = b._angularVelocity; } _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting); // Solve position constraints. for (int i = 0; i < Settings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // Leap of faith to new safe state. Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c; Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a; Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c; Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. _contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < Settings.TOIVelocityIterations; ++i) { _contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. FP h = subStep.dt; // Integrate positions. for (int i = 0; i < BodyCount; ++i) { FPVector2 c = _positions[i].c; FP a = _positions[i].a; FPVector2 v = _velocities[i].v; FP w = _velocities[i].w; // Check for large velocities FPVector2 translation = h * v; if (FPVector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { FP ratio = Settings.MaxTranslation / translation.magnitude; v *= ratio; } FP rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { FP ratio = Settings.MaxRotation / FP.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; // Sync bodies Body body = Bodies[i]; body._sweep.C = c; body._sweep.A = a; body._linearVelocity = v; body._angularVelocity = w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); }
internal override bool SolvePositionConstraints(ref SolverData data) { TSVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; TSVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FP positionError, angularError; Mat33 K = new Mat33(); K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; K.ez.x = -rA.y * iA - rB.y * iB; K.ex.y = K.ey.x; K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; K.ez.y = rA.x * iA + rB.x * iB; K.ex.z = K.ez.x; K.ey.z = K.ez.y; K.ez.z = iA + iB; if (FrequencyHz > 0.0f) { TSVector2 C1 = cB + rB - cA - rA; positionError = C1.magnitude; angularError = 0.0f; TSVector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { TSVector2 C1 = cB + rB - cA - rA; FP C2 = aB - aA - ReferenceAngle; positionError = C1.magnitude; angularError = FP.Abs(C2); TSVector C = new TSVector(C1.x, C1.y, C2); TSVector impulse = K.Solve33(C) * -1; TSVector2 P = new TSVector2(impulse.x, impulse.y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
public static FPVector2 Abs(FPVector2 v) { return(new FPVector2(FP.Abs(v.x), FP.Abs(v.y))); }
public static bool FPEquals(FP value1, FP value2) { return(FP.Abs(value1 - value2) <= Settings.Epsilon); }