public void Solve(ref TimeStep step, ref TSVector2 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]; TSVector2 c = b._sweep.C; FP a = b._sweep.A; TSVector2 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) { TSVector2 c = _positions[i].c; FP a = _positions[i].a; TSVector2 v = _velocities[i].v; FP w = _velocities[i].w; // Check for large velocities TSVector2 translation = h * v; if (TSVector2.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 || TSVector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += h; minSleepTime = SyncFrame.TSMath.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
/// <summary> /// Attaches the bodies with revolute joints. /// </summary> /// <param name="world">The world.</param> /// <param name="bodies">The bodies.</param> /// <param name="localAnchorA">The local anchor A.</param> /// <param name="localAnchorB">The local anchor B.</param> /// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param> /// <param name="collideConnected">if set to <c>true</c> [collide connected].</param> public static List <RevoluteJoint> AttachBodiesWithRevoluteJoint(World world, List <Body> bodies, TSVector2 localAnchorA, TSVector2 localAnchorB, bool connectFirstAndLast, bool collideConnected) { List <RevoluteJoint> joints = new List <RevoluteJoint>(bodies.Count + 1); for (int i = 1; i < bodies.Count; i++) { RevoluteJoint joint = new RevoluteJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB); joint.CollideConnected = collideConnected; world.AddJoint(joint); joints.Add(joint); } if (connectFirstAndLast) { RevoluteJoint lastjoint = new RevoluteJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB); lastjoint.CollideConnected = collideConnected; world.AddJoint(lastjoint); joints.Add(lastjoint); } return(joints); }
public override bool TestPoint(ref Transform transform, ref TSVector2 point) { return(false); }
/// <summary> /// Draw a line segment. /// </summary> /// <param name="start">The start.</param> /// <param name="end">The end.</param> /// <param name="red">The red value.</param> /// <param name="blue">The blue value.</param> /// <param name="green">The green value.</param> public abstract void DrawSegment(TSVector2 start, TSVector2 end, FP red, FP blue, FP green);
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; FP aA = data.positions[_indexA].a; TSVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FP aB = data.positions[_indexB].a; TSVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; 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) { K.GetInverse22(ref _mass); FP invM = iA + iB; FP m = invM > 0.0f ? 1.0f / invM : 0.0f; FP C = aB - aA - ReferenceAngle; // Frequency FP omega = 2.0f * Settings.Pi * FrequencyHz; // Damping coefficient FP d = 2.0f * m * DampingRatio * omega; // Spring stiffness FP k = m * omega * omega; // magic formulas FP h = data.step.dt; _gamma = h * (d + h * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * h * k * _gamma; invM += _gamma; _mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(ref _mass); _gamma = 0.0f; _bias = 0.0f; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; TSVector2 P = new TSVector2(_impulse.x, _impulse.y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _impulse.z); } else { _impulse = TSVector.zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
public BuoyancyController(AABB container, FP density, FP linearDragCoefficient, FP rotationalDragCoefficient, TSVector2 gravity) : base(ControllerType.BuoyancyController) { this.Container = container; this._normal = new TSVector2(0, 1); this.Density = density; this.LinearDragCoefficient = linearDragCoefficient; this.AngularDragCoefficient = rotationalDragCoefficient; this._gravity = gravity; }
internal override void InitVelocityConstraints(ref SolverData data) { this._indexA = base.BodyA.IslandIndex; this._indexB = base.BodyB.IslandIndex; this._localCenterA = base.BodyA._sweep.LocalCenter; this._localCenterB = base.BodyB._sweep.LocalCenter; this._invMassA = base.BodyA._invMass; this._invMassB = base.BodyB._invMass; this._invIA = base.BodyA._invI; this._invIB = base.BodyB._invI; TSVector2 c = data.positions[this._indexA].c; FP a = data.positions[this._indexA].a; TSVector2 tSVector = data.velocities[this._indexA].v; FP fP = data.velocities[this._indexA].w; TSVector2 c2 = data.positions[this._indexB].c; FP a2 = data.positions[this._indexB].a; TSVector2 tSVector2 = data.velocities[this._indexB].v; FP fP2 = data.velocities[this._indexB].w; Rot q = new Rot(a); Rot q2 = new Rot(a2); this._rA = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA); this._rB = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB); this._u = c2 + this._rB - c - this._rA; this._length = this._u.magnitude; FP x = this._length - this.MaxLength; bool flag = x > 0f; if (flag) { this.State = LimitState.AtUpper; } else { this.State = LimitState.Inactive; } bool flag2 = this._length > Settings.LinearSlop; if (flag2) { this._u *= 1f / this._length; FP y = MathUtils.Cross(this._rA, this._u); FP y2 = MathUtils.Cross(this._rB, this._u); FP fP3 = this._invMassA + this._invIA * y * y + this._invMassB + this._invIB * y2 * y2; this._mass = ((fP3 != 0f) ? (1f / fP3) : 0f); this._impulse *= data.step.dtRatio; TSVector2 tSVector3 = this._impulse * this._u; tSVector -= this._invMassA * tSVector3; fP -= this._invIA * MathUtils.Cross(this._rA, tSVector3); tSVector2 += this._invMassB * tSVector3; fP2 += this._invIB * MathUtils.Cross(this._rB, tSVector3); data.velocities[this._indexA].v = tSVector; data.velocities[this._indexA].w = fP; data.velocities[this._indexB].v = tSVector2; data.velocities[this._indexB].w = fP2; } else { this._u = TSVector2.zero; this._mass = 0f; this._impulse = 0f; } }
public static BreakableBody CreateBreakableBody(World world, IEnumerable <Shape> shapes, TSVector2 position) { BreakableBody breakableBody = new BreakableBody(shapes, world); breakableBody.MainBody.Position = position; world.AddBreakableBody(breakableBody); return(breakableBody); }
public static Body CreateLineArc(World world, FP radians, int sides, FP radius, TSVector2 position, FP angle, bool closed) { Body body = CreateBody(world); FixtureFactory.AttachLineArc(radians, sides, radius, position, angle, closed, body); return(body); }
/// <summary> /// Creates a rounded rectangle. /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices) /// </summary> /// <param name="world">The world.</param> /// <param name="width">The width.</param> /// <param name="height">The height.</param> /// <param name="xRadius">The x radius.</param> /// <param name="yRadius">The y radius.</param> /// <param name="segments">The segments.</param> /// <param name="density">The density.</param> /// <param name="position">The position.</param> /// <returns></returns> public static Body CreateRoundedRectangle(World world, FP width, FP height, FP xRadius, FP yRadius, int segments, FP density, TSVector2 position, object userData = null) { Vertices verts = PolygonTools.CreateRoundedRectangle(width, height, xRadius, yRadius, segments); //There are too many vertices in the capsule. We decompose it. if (verts.Count >= Settings.MaxPolygonVertices) { List <Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip, true, FP.EN3); Body body = CreateCompoundPolygon(world, vertList, density, userData); body.Position = position; return(body); } return(CreatePolygon(world, verts, density, null)); }
/// <summary> /// Creates a breakable body. You would want to remove collinear points before using this. /// </summary> /// <param name="world">The world.</param> /// <param name="vertices">The vertices.</param> /// <param name="density">The density.</param> /// <param name="position">The position.</param> /// <returns></returns> public static BreakableBody CreateBreakableBody(World world, Vertices vertices, FP density, TSVector2 position) { List <Vertices> triangles = Triangulate.ConvexPartition(vertices, TriangulationAlgorithm.Earclip, true, FP.EN3); BreakableBody breakableBody = new BreakableBody(triangles, world, density); breakableBody.MainBody.Position = position; world.AddBreakableBody(breakableBody); return(breakableBody); }
/// <summary> /// Creates a capsule. /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices) /// </summary> /// <returns></returns> public static Body CreateCapsule(World world, FP height, FP topRadius, int topEdges, FP bottomRadius, int bottomEdges, FP density, TSVector2 position, object userData = null) { Vertices verts = PolygonTools.CreateCapsule(height, topRadius, topEdges, bottomRadius, bottomEdges); Body body; //There are too many vertices in the capsule. We decompose it. if (verts.Count >= Settings.MaxPolygonVertices) { List <Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip, true, FP.EN3); body = CreateCompoundPolygon(world, vertList, density, userData); body.Position = position; return(body); } body = CreatePolygon(world, verts, density, userData); body.Position = position; return(body); }
// TS - public static Body CreateBody(World world, Vector2 position, FP rotation = 0, object userData = null) public static Body CreateBody(World world, TSVector2 position, FP rotation, object userData = null) { Body body = new Body(world, position, rotation, userData); return(body); }
public static Body CreateCompoundPolygon(World world, List <Vertices> list, FP density, TSVector2 position, object userData = null) { //We create a single body Body polygonBody = CreateBody(world, position); FixtureFactory.AttachCompoundPolygon(list, density, polygonBody, userData); return(polygonBody); }
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 angularError = 0.0f; FP positionError; bool fixedRotation = (_invIA + _invIB == 0.0f); // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { FP angle = aB - aA - ReferenceAngle; FP limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections FP C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = FP.Abs(C); } else if (_limitState == LimitState.AtLower) { FP C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { FP C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } aA -= _invIA * limitImpulse; aB += _invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); TSVector2 C = cB + rB - cA - rA; positionError = C.magnitude; FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; Mat22 K = new Mat22(); K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; K.ey.x = K.ex.y; K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; TSVector2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * MathUtils.Cross(rA, impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(rB, impulse); } 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 Body CreateSolidArc(World world, FP density, FP radians, int sides, FP radius, TSVector2 position, FP angle) { Body body = CreateBody(world); FixtureFactory.AttachSolidArc(density, radians, sides, radius, position, angle, body); return(body); }
Vector3 ToXZ(TSVector2 target) { return(new Vector3(target.x.AsFloat(), 0, target.y.AsFloat())); }
public static Body CreateRectangle(World world, FP width, FP height, FP density, TSVector2 position, object userData = null) { if (width <= 0) { throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters"); } if (height <= 0) { throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters"); } Body newBody = CreateBody(world, position); newBody.UserData = userData; Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2); PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density); newBody.CreateFixture(rectangleShape); return(newBody); }
public override void Update(FP dt) { this._uniqueBodies.Clear(); this.World.QueryAABB(delegate(Fixture fixture) { bool flag3 = fixture.Body.IsStatic || !fixture.Body.Awake; bool result; if (flag3) { result = true; } else { bool flag4 = !this._uniqueBodies.ContainsKey(fixture.Body.BodyId); if (flag4) { this._uniqueBodies.Add(fixture.Body.BodyId, fixture.Body); } result = true; } return(result); }, ref this._container); foreach (KeyValuePair <int, Body> current in this._uniqueBodies) { Body value = current.Value; TSVector2 zero = TSVector2.zero; TSVector2 zero2 = TSVector2.zero; FP fP = 0; FP fP2 = 0; for (int i = 0; i < value.FixtureList.Count; i++) { Fixture fixture2 = value.FixtureList[i]; bool flag = fixture2.Shape.ShapeType != ShapeType.Polygon && fixture2.Shape.ShapeType > ShapeType.Circle; if (!flag) { Shape shape = fixture2.Shape; TSVector2 tSVector; FP fP3 = shape.ComputeSubmergedArea(ref this._normal, this._offset, ref value._xf, out tSVector); fP += fP3; zero.x += fP3 * tSVector.x; zero.y += fP3 * tSVector.y; fP2 += fP3 * shape.Density; zero2.x += fP3 * tSVector.x * shape.Density; zero2.y += fP3 * tSVector.y * shape.Density; } } zero.x /= fP; zero.y /= fP; zero2.x /= fP2; zero2.y /= fP2; bool flag2 = fP < Settings.Epsilon; if (!flag2) { TSVector2 force = -this.Density * fP * this._gravity; value.ApplyForce(force, zero2); TSVector2 tSVector2 = value.GetLinearVelocityFromWorldPoint(zero) - this.Velocity; tSVector2 *= -this.LinearDragCoefficient * fP; value.ApplyForce(tSVector2, zero); value.ApplyTorque(-value.Inertia / value.Mass * fP * value.AngularVelocity * this.AngularDragCoefficient); } } }
/// <summary> /// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a Shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// </summary> /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(Func <RayCastInput, int, FP> callback, ref RayCastInput input) { TSVector2 p1 = input.Point1; TSVector2 p2 = input.Point2; TSVector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. TSVector2 absV = MathUtils.Abs(new TSVector2(-r.y, r.x)); //FPE: Inlined the 'v' variable // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) FP maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { TSVector2 t = p1 + maxFraction * (p2 - p1); TSVector2.Min(ref p1, ref t, out segmentAABB.LowerBound); TSVector2.Max(ref p1, ref t, out segmentAABB.UpperBound); } _raycastStack.Clear(); _raycastStack.Push(_root); while (_raycastStack.Count > 0) { int nodeId = _raycastStack.Pop(); if (nodeId == NullNode) { continue; } TreeNode <T> node = _nodes[nodeId]; if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) TSVector2 c = node.AABB.Center; TSVector2 h = node.AABB.Extents; FP separation = FP.Abs(TSVector2.Dot(new TSVector2(-r.y, r.x), p1 - c)) - TSVector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; FP value = callback(subInput, nodeId); if (value == 0.0f) { // the client has terminated the raycast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; TSVector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = TSVector2.Min(p1, t); segmentAABB.UpperBound = TSVector2.Max(p1, t); } } else { _raycastStack.Push(node.Child1); _raycastStack.Push(node.Child2); } } }
/// <summary> /// Draw a solid circle. /// </summary> /// <param name="center">The center.</param> /// <param name="radius">The radius.</param> /// <param name="axis">The axis.</param> /// <param name="red">The red value.</param> /// <param name="blue">The blue value.</param> /// <param name="green">The green value.</param> public abstract void DrawSolidCircle(TSVector2 center, FP radius, TSVector2 axis, FP red, FP blue, FP green);
/// <summary> /// Constructor of RevoluteJoint. /// </summary> /// <param name="bodyA">The first body.</param> /// <param name="bodyB">The second body.</param> /// <param name="anchor">The shared anchor.</param> /// <param name="useWorldCoordinates"></param> public RevoluteJoint(Body bodyA, Body bodyB, TSVector2 anchor, bool useWorldCoordinates = false) : this(bodyA, bodyB, anchor, anchor, useWorldCoordinates) { }
public FP SDValue(TSVector2 pos) { return(SDFUtils.SDCircle(pos, Center, Radius)); }
public override TSVector2 GetReactionForce(FP invDt) { TSVector2 p = new TSVector2(_impulse.x, _impulse.y); return(invDt * p); }
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); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; FP aA = data.positions[_indexA].a; TSVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FP aB = data.positions[_indexB].a; TSVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; bool fixedRotation = (iA + iB == 0.0f); _mass.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB; _mass.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB; _mass.ez.x = -_rA.y * iA - _rB.y * iB; _mass.ex.y = _mass.ey.x; _mass.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB; _mass.ez.y = _rA.x * iA + _rB.x * iB; _mass.ex.z = _mass.ez.x; _mass.ey.z = _mass.ez.y; _mass.ez.z = iA + iB; _motorMass = iA + iB; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } if (_enableMotor == false || fixedRotation) { _motorImpulse = 0.0f; } if (_enableLimit && fixedRotation == false) { FP jointAngle = aB - aA - ReferenceAngle; if (FP.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) { _limitState = LimitState.Equal; } else if (jointAngle <= _lowerAngle) { if (_limitState != LimitState.AtLower) { _impulse.z = 0.0f; } _limitState = LimitState.AtLower; } else if (jointAngle >= _upperAngle) { if (_limitState != LimitState.AtUpper) { _impulse.z = 0.0f; } _limitState = LimitState.AtUpper; } else { _limitState = LimitState.Inactive; _impulse.z = 0.0f; } } else { _limitState = LimitState.Inactive; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; _motorImpulse *= data.step.dtRatio; TSVector2 P = new TSVector2(_impulse.x, _impulse.y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.z); } else { _impulse = TSVector.zero; _motorImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
private static List <Vertices> TriangulatePolygon(Vertices vertices, FP tolerance) { bool flag = vertices.Count < 3; List <Vertices> result; if (flag) { result = new List <Vertices>(); } else { List <Vertices> list = new List <Vertices>(); Vertices pin = new Vertices(vertices); Vertices vertices2; Vertices vertices3; bool flag2 = EarclipDecomposer.ResolvePinchPoint(pin, out vertices2, out vertices3, tolerance); if (flag2) { List <Vertices> list2 = EarclipDecomposer.TriangulatePolygon(vertices2, tolerance); List <Vertices> list3 = EarclipDecomposer.TriangulatePolygon(vertices3, tolerance); bool flag3 = list2.Count == -1 || list3.Count == -1; if (flag3) { throw new Exception("Can't triangulate your polygon."); } for (int i = 0; i < list2.Count; i++) { list.Add(new Vertices(list2[i])); } for (int j = 0; j < list3.Count; j++) { list.Add(new Vertices(list3[j])); } result = list; } else { Vertices[] array = new Vertices[vertices.Count - 2]; int num = 0; FP[] array2 = new FP[vertices.Count]; FP[] array3 = new FP[vertices.Count]; for (int k = 0; k < vertices.Count; k++) { array2[k] = vertices[k].x; array3[k] = vertices[k].y; } int l = vertices.Count; while (l > 3) { int num2 = -1; FP y = -10f; for (int m = 0; m < l; m++) { bool flag4 = EarclipDecomposer.IsEar(m, array2, array3, l); if (flag4) { int num3 = EarclipDecomposer.Remainder(m - 1, l); int num4 = EarclipDecomposer.Remainder(m + 1, l); TSVector2 tSVector = new TSVector2(array2[num4] - array2[m], array3[num4] - array3[m]); TSVector2 tSVector2 = new TSVector2(array2[m] - array2[num3], array3[m] - array3[num3]); TSVector2 tSVector3 = new TSVector2(array2[num3] - array2[num4], array3[num3] - array3[num4]); tSVector.Normalize(); tSVector2.Normalize(); tSVector3.Normalize(); FP fP; MathUtils.Cross(ref tSVector, ref tSVector2, out fP); fP = FP.Abs(fP); FP fP2; MathUtils.Cross(ref tSVector2, ref tSVector3, out fP2); fP2 = FP.Abs(fP2); FP fP3; MathUtils.Cross(ref tSVector3, ref tSVector, out fP3); fP3 = FP.Abs(fP3); FP fP4 = TSMath.Min(fP, TSMath.Min(fP2, fP3)); bool flag5 = fP4 > y; if (flag5) { num2 = m; y = fP4; } } } bool flag6 = num2 == -1; if (flag6) { for (int n = 0; n < num; n++) { list.Add(array[n]); } result = list; return(result); } l--; FP[] array4 = new FP[l]; FP[] array5 = new FP[l]; int num5 = 0; for (int num6 = 0; num6 < l; num6++) { bool flag7 = num5 == num2; if (flag7) { num5++; } array4[num6] = array2[num5]; array5[num6] = array3[num5]; num5++; } int num7 = (num2 == 0) ? l : (num2 - 1); int num8 = (num2 == l) ? 0 : (num2 + 1); EarclipDecomposer.Triangle triangle = new EarclipDecomposer.Triangle(array2[num2], array3[num2], array2[num8], array3[num8], array2[num7], array3[num7]); array[num] = triangle; num++; array2 = array4; array3 = array5; } EarclipDecomposer.Triangle triangle2 = new EarclipDecomposer.Triangle(array2[1], array3[1], array2[2], array3[2], array2[0], array3[0]); array[num] = triangle2; num++; for (int num9 = 0; num9 < num; num9++) { list.Add(new Vertices(array[num9])); } result = list; } } return(result); }
internal override void SolveVelocityConstraints(ref SolverData data) { TSVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; TSVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false) { FP Cdot = wB - wA - _motorSpeed; FP impulse = _motorMass * (-Cdot); FP oldImpulse = _motorImpulse; FP maxImpulse = data.step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { TSVector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); FP Cdot2 = wB - wA; TSVector Cdot = new TSVector(Cdot1.x, Cdot1.y, Cdot2); TSVector impulse = _mass.Solve33(Cdot) * -1; if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { FP newImpulse = _impulse.z + impulse.z; if (newImpulse < 0.0f) { TSVector2 rhs = -Cdot1 + _impulse.z * new TSVector2(_mass.ez.x, _mass.ez.y); TSVector2 reduced = _mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -_impulse.z; _impulse.x += reduced.x; _impulse.y += reduced.y; _impulse.z = 0.0f; } else { _impulse += impulse; } } else if (_limitState == LimitState.AtUpper) { FP newImpulse = _impulse.z + impulse.z; if (newImpulse > 0.0f) { TSVector2 rhs = -Cdot1 + _impulse.z * new TSVector2(_mass.ez.x, _mass.ez.y); TSVector2 reduced = _mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -_impulse.z; _impulse.x += reduced.x; _impulse.y += reduced.y; _impulse.z = 0.0f; } else { _impulse += impulse; } } TSVector2 P = new TSVector2(impulse.x, impulse.y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + impulse.z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + impulse.z); } else { // Solve point-to-point constraint TSVector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); TSVector2 impulse = _mass.Solve22(-Cdot); _impulse.x += impulse.x; _impulse.y += impulse.y; vA -= mA * impulse; wA -= iA * MathUtils.Cross(_rA, impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(_rB, impulse); } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc) { sc = TSVector2.zero; return(0); }
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) { TSVector2 c = _positions[i].c; FP a = _positions[i].a; TSVector2 v = _velocities[i].v; FP w = _velocities[i].w; // Check for large velocities TSVector2 translation = h * v; if (TSVector2.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); }