internal override void SolveVelocityConstraints(ref SolverData data) { FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; FPVector2 vpA = vA + MathUtils.Cross(wA, _rA); FPVector2 vpB = vB + MathUtils.Cross(wB, _rB); FP Cdot = -FPVector2.Dot(_uA, vpA) - Ratio * FPVector2.Dot(_uB, vpB); FP impulse = -_mass * Cdot; _impulse += impulse; FPVector2 PA = -impulse * _uA; FPVector2 PB = -Ratio * impulse * _uB; vA += _invMassA * PA; wA += _invIA * MathUtils.Cross(_rA, PA); vB += _invMassB * PB; wB += _invIB * MathUtils.Cross(_rB, PB); data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void SolveVelocityConstraints(ref SolverData data) { FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; // Cdot = dot(u, v + cross(w, r)) FPVector2 vpA = vA + MathUtils.Cross(wA, _rA); FPVector2 vpB = vB + MathUtils.Cross(wB, _rB); FP Cdot = FPVector2.Dot(_u, vpB - vpA); FP impulse = -_mass * (Cdot + _bias + _gamma * _impulse); _impulse += impulse; FPVector2 P = impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(_rA, P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(_rB, P); data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
public override bool TestPoint(ref Transform transform, ref FPVector2 point) { FPVector2 center = transform.p + MathUtils.Mul(transform.q, Position); FPVector2 d = point - center; return(FPVector2.Dot(d, d) <= _2radius); }
public override FP ComputeSubmergedArea(ref FPVector2 normal, FP offset, ref Transform xf, out FPVector2 sc) { sc = FPVector2.zero; FPVector2 p = MathUtils.Mul(ref xf, Position); FP l = -(FPVector2.Dot(normal, p) - offset); if (l < -Radius + Settings.Epsilon) { //Completely dry return(0); } if (l > Radius) { //Completely wet sc = p; return(Settings.Pi * _2radius); } //Magic FP l2 = l * l; FP area = _2radius * (FP)((FPMath.Asin((l / Radius)) + FPMath.PiOver2) + l * FPMath.Sqrt(_2radius - l2)); // TODO - PORT //FP com = -2.0f / 3.0f * (FP)Math.Pow(_2radius - l2, 1.5f) / area; FP com = new FP(-2) / new FP(3) * (FP)Math.Pow((_2radius - l2).AsFloat(), 1.5f) / area; sc.x = p.x + normal.x * com; sc.y = p.y + normal.y * com; return(area); }
public static FP DistanceBetweenPointAndLineSegment(ref FPVector2 point, ref FPVector2 start, ref FPVector2 end) { if (start == end) { return(FPVector2.Distance(point, start)); } FPVector2 v = FPVector2.Subtract(end, start); FPVector2 w = FPVector2.Subtract(point, start); FP c1 = FPVector2.Dot(w, v); if (c1 <= 0) { return(FPVector2.Distance(point, start)); } FP c2 = FPVector2.Dot(v, v); if (c2 <= c1) { return(FPVector2.Distance(point, end)); } FP b = c1 / c2; FPVector2 pointOnLine = FPVector2.Add(start, FPVector2.Multiply(v, b)); return(FPVector2.Distance(point, pointOnLine)); }
/// <summary> /// Tests if a point lies on a line segment. /// </summary> /// <remarks>Used by method <c>CalculateBeta()</c>.</remarks> private static bool PointOnLineSegment(FPVector2 start, FPVector2 end, FPVector2 point) { FPVector2 segment = end - start; return(MathUtils.Area(ref start, ref end, ref point) == 0f && FPVector2.Dot(point - start, segment) >= 0f && FPVector2.Dot(point - end, segment) <= 0f); }
protected override sealed void ComputeProperties() { FP area = Settings.Pi * _2radius; MassData.Area = area; MassData.Mass = Density * area; MassData.Centroid = Position; // inertia about the local origin MassData.Inertia = MassData.Mass * (0.5f * _2radius + FPVector2.Dot(Position, Position)); }
public static FP Evaluate(int indexA, int indexB, FP t) { Transform xfA, xfB; _sweepA.GetTransform(out xfA, t); _sweepB.GetTransform(out xfB, t); switch (_type) { case SeparationFunctionType.Points: { FPVector2 localPointA = _proxyA.Vertices[indexA]; FPVector2 localPointB = _proxyB.Vertices[indexB]; FPVector2 pointA = MathUtils.Mul(ref xfA, localPointA); FPVector2 pointB = MathUtils.Mul(ref xfB, localPointB); FP separation = FPVector2.Dot(pointB - pointA, _axis); return(separation); } case SeparationFunctionType.FaceA: { FPVector2 normal = MathUtils.Mul(ref xfA.q, _axis); FPVector2 pointA = MathUtils.Mul(ref xfA, _localPoint); FPVector2 localPointB = _proxyB.Vertices[indexB]; FPVector2 pointB = MathUtils.Mul(ref xfB, localPointB); FP separation = FPVector2.Dot(pointB - pointA, normal); return(separation); } case SeparationFunctionType.FaceB: { FPVector2 normal = MathUtils.Mul(ref xfB.q, _axis); FPVector2 pointB = MathUtils.Mul(ref xfB, _localPoint); FPVector2 localPointA = _proxyA.Vertices[indexA]; FPVector2 pointA = MathUtils.Mul(ref xfA, localPointA); FP separation = FPVector2.Dot(pointA - pointB, normal); return(separation); } default: Debug.Assert(false); return(0.0f); } }
public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out FPVector2 normal, out FPVector2 point, out FP separation) { Debug.Assert(pc.pointCount > 0); switch (pc.type) { case ManifoldType.Circles: { FPVector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint); FPVector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]); normal = pointB - pointA; normal.Normalize(); point = 0.5f * (pointA + pointB); separation = FPVector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB; } break; case ManifoldType.FaceA: { normal = MathUtils.Mul(xfA.q, pc.localNormal); FPVector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint); FPVector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]); separation = FPVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; point = clipPoint; } break; case ManifoldType.FaceB: { normal = MathUtils.Mul(xfB.q, pc.localNormal); FPVector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint); FPVector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]); separation = FPVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; point = clipPoint; // Ensure normal points from A to B normal = -normal; } break; default: normal = FPVector2.zero; point = FPVector2.zero; separation = 0; break; } }
internal override bool SolvePositionConstraints(ref SolverData data) { FPVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; FPVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FPVector2 d = (cB - cA) + rB - rA; FPVector2 ay = MathUtils.Mul(qA, _localYAxis); FP sAy = MathUtils.Cross(d + rA, ay); FP sBy = MathUtils.Cross(rB, ay); FP C = FPVector2.Dot(d, ay); FP k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy; FP impulse; if (k != 0.0f) { impulse = -C / k; } else { impulse = 0.0f; } FPVector2 P = impulse * ay; FP LA = impulse * sAy; FP LB = impulse * sBy; cA -= _invMassA * P; aA -= _invIA * LA; cB += _invMassB * P; aB += _invIB * LB; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(FP.Abs(C) <= Settings.LinearSlop); }
public override bool TestPoint(ref Transform transform, ref FPVector2 point) { FPVector2 pLocal = MathUtils.MulT(transform.q, point - transform.p); for (int i = 0; i < Vertices.Count; ++i) { FP dot = FPVector2.Dot(Normals[i], pLocal - Vertices[i]); if (dot > 0.0f) { return(false); } } return(true); }
/// <summary> /// Get the supporting vertex in the given direction. /// </summary> /// <param name="direction">The direction.</param> /// <returns></returns> public FPVector2 GetSupportVertex(FPVector2 direction) { int bestIndex = 0; FP bestValue = FPVector2.Dot(Vertices[0], direction); for (int i = 1; i < Vertices.Count; ++i) { FP value = FPVector2.Dot(Vertices[i], direction); if (value > bestValue) { bestIndex = i; bestValue = value; } } return(Vertices[bestIndex]); }
// Solve a line segment using barycentric coordinates. // // p = a1 * w1 + a2 * w2 // a1 + a2 = 1 // // The vector from the origin to the closest point on the line is // perpendicular to the line. // e12 = w2 - w1 // dot(p, e) = 0 // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 // // 2-by-2 linear system // [1 1 ][a1] = [1] // [w1.e12 w2.e12][a2] = [0] // // Define // d12_1 = dot(w2, e12) // d12_2 = -dot(w1, e12) // d12 = d12_1 + d12_2 // // Solution // a1 = d12_1 / d12 // a2 = d12_2 / d12 internal void Solve2() { FPVector2 w1 = V[0].W; FPVector2 w2 = V[1].W; FPVector2 e12 = w2 - w1; // w1 region FP d12_2 = -FPVector2.Dot(w1, e12); if (d12_2 <= 0.0f) { // a2 <= 0, so we clamp it to 0 SimplexVertex v0 = V[0]; v0.A = 1.0f; V[0] = v0; Count = 1; return; } // w2 region FP d12_1 = FPVector2.Dot(w2, e12); if (d12_1 <= 0.0f) { // a1 <= 0, so we clamp it to 0 SimplexVertex v1 = V[1]; v1.A = 1.0f; V[1] = v1; Count = 1; V[0] = V[1]; return; } // Must be in e12 region. FP inv_d12 = 1.0f / (d12_1 + d12_2); SimplexVertex v0_2 = V[0]; SimplexVertex v1_2 = V[1]; v0_2.A = d12_1 * inv_d12; v1_2.A = d12_2 * inv_d12; V[0] = v0_2; V[1] = v1_2; Count = 2; }
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { // Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius output = new RayCastOutput(); FPVector2 position = transform.p + MathUtils.Mul(transform.q, Position); FPVector2 s = input.Point1 - position; FP b = FPVector2.Dot(s, s) - _2radius; // Solve quadratic equation. FPVector2 r = input.Point2 - input.Point1; FP c = FPVector2.Dot(s, r); FP rr = FPVector2.Dot(r, r); FP sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < Settings.Epsilon) { return(false); } // Find the point of intersection of the line with the circle. FP a = -(c + FP.Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= input.MaxFraction * rr) { a /= rr; output.Fraction = a; //TODO: Check results here output.Normal = s + a * r; output.Normal.Normalize(); return(true); } return(false); }
internal override void SolveVelocityConstraints(ref SolverData data) { FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; FPVector2 vC = data.velocities[_indexC].v; FP wC = data.velocities[_indexC].w; FPVector2 vD = data.velocities[_indexD].v; FP wD = data.velocities[_indexD].w; FP Cdot = FPVector2.Dot(_JvAC, vA - vC) + FPVector2.Dot(_JvBD, vB - vD); Cdot += (_JwA * wA - _JwC * wC) + (_JwB * wB - _JwD * wD); FP impulse = -_mass * Cdot; _impulse += impulse; vA += (_mA * impulse) * _JvAC; wA += _iA * impulse * _JwA; vB += (_mB * impulse) * _JvBD; wB += _iB * impulse * _JwB; vC -= (_mC * impulse) * _JvAC; wC -= _iC * impulse * _JwC; vD -= (_mD * impulse) * _JvBD; wD -= _iD * impulse * _JwD; data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; data.velocities[_indexC].v = vC; data.velocities[_indexC].w = wC; data.velocities[_indexD].v = vD; data.velocities[_indexD].w = wD; }
internal override void SolveVelocityConstraints(ref SolverData data) { FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; // Cdot = dot(u, v + cross(w, r)) FPVector2 vpA = vA + MathUtils.Cross(wA, _rA); FPVector2 vpB = vB + MathUtils.Cross(wB, _rB); FP C = _length - MaxLength; FP Cdot = FPVector2.Dot(_u, vpB - vpA); // Predictive constraint. if (C < 0.0f) { Cdot += data.step.inv_dt * C; } FP impulse = -_mass * Cdot; FP oldImpulse = _impulse; _impulse = Spax.FPMath.Min(0.0f, _impulse + impulse); impulse = _impulse - oldImpulse; FPVector2 P = impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(_rA, P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(_rB, P); data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
/// <summary> /// Requires two existing revolute or prismatic joints (any combination will work). /// The provided joints must attach a dynamic body to a static body. /// </summary> /// <param name="jointA">The first joint.</param> /// <param name="jointB">The second joint.</param> /// <param name="ratio">The ratio.</param> /// <param name="bodyA">The first body</param> /// <param name="bodyB">The second body</param> // TS - public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, FP ratio = 1f) public GearJoint(Body bodyA, Body bodyB, Joint2D jointA, Joint2D jointB, FP ratio) { JointType = JointType.Gear; BodyA = bodyA; BodyB = bodyB; JointA = jointA; JointB = jointB; Ratio = ratio; _typeA = jointA.JointType; _typeB = jointB.JointType; Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic); Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic); FP coordinateA, coordinateB; // TODO_ERIN there might be some problem with the joint edges in b2Joint. _bodyC = JointA.BodyA; _bodyA = JointA.BodyB; // Get geometry of joint1 Transform xfA = _bodyA._xf; FP aA = _bodyA._sweep.A; Transform xfC = _bodyC._xf; FP aC = _bodyC._sweep.A; if (_typeA == JointType.Revolute) { RevoluteJoint revolute = (RevoluteJoint)jointA; _localAnchorC = revolute.LocalAnchorA; _localAnchorA = revolute.LocalAnchorB; _referenceAngleA = revolute.ReferenceAngle; _localAxisC = FPVector2.zero; coordinateA = aA - aC - _referenceAngleA; } else { PrismaticJoint prismatic = (PrismaticJoint)jointA; _localAnchorC = prismatic.LocalAnchorA; _localAnchorA = prismatic.LocalAnchorB; _referenceAngleA = prismatic.ReferenceAngle; _localAxisC = prismatic.LocalXAxis; FPVector2 pC = _localAnchorC; FPVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p)); coordinateA = FPVector2.Dot(pA - pC, _localAxisC); } _bodyD = JointB.BodyA; _bodyB = JointB.BodyB; // Get geometry of joint2 Transform xfB = _bodyB._xf; FP aB = _bodyB._sweep.A; Transform xfD = _bodyD._xf; FP aD = _bodyD._sweep.A; if (_typeB == JointType.Revolute) { RevoluteJoint revolute = (RevoluteJoint)jointB; _localAnchorD = revolute.LocalAnchorA; _localAnchorB = revolute.LocalAnchorB; _referenceAngleB = revolute.ReferenceAngle; _localAxisD = FPVector2.zero; coordinateB = aB - aD - _referenceAngleB; } else { PrismaticJoint prismatic = (PrismaticJoint)jointB; _localAnchorD = prismatic.LocalAnchorA; _localAnchorB = prismatic.LocalAnchorB; _referenceAngleB = prismatic.ReferenceAngle; _localAxisD = prismatic.LocalXAxis; FPVector2 pD = _localAnchorD; FPVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p)); coordinateB = FPVector2.Dot(pB - pD, _localAxisD); } _ratio = ratio; _constant = coordinateA + _ratio * coordinateB; _impulse = 0.0f; }
//Cutting a shape into two is based on the work of Daid and his prototype BoxCutter: http://www.box2d.org/forum/viewtopic.php?f=3&t=1473 /// <summary> /// Split a fixture into 2 vertice collections using the given entry and exit-point. /// </summary> /// <param name="fixture">The Fixture to split</param> /// <param name="entryPoint">The entry point - The start point</param> /// <param name="exitPoint">The exit point - The end point</param> /// <param name="first">The first collection of vertexes</param> /// <param name="second">The second collection of vertexes</param> public static void SplitShape(Fixture fixture, FPVector2 entryPoint, FPVector2 exitPoint, out Vertices first, out Vertices second) { FPVector2 localEntryPoint = fixture.Body.GetLocalPoint(ref entryPoint); FPVector2 localExitPoint = fixture.Body.GetLocalPoint(ref exitPoint); PolygonShape shape = fixture.Shape as PolygonShape; //We can only cut polygons at the moment if (shape == null) { first = new Vertices(); second = new Vertices(); return; } //Offset the entry and exit points if they are too close to the vertices foreach (FPVector2 vertex in shape.Vertices) { if (vertex.Equals(localEntryPoint)) { localEntryPoint -= new FPVector2(0, Settings.Epsilon); } if (vertex.Equals(localExitPoint)) { localExitPoint += new FPVector2(0, Settings.Epsilon); } } Vertices vertices = new Vertices(shape.Vertices); Vertices[] newPolygon = new Vertices[2]; for (int i = 0; i < newPolygon.Length; i++) { newPolygon[i] = new Vertices(vertices.Count); } int[] cutAdded = { -1, -1 }; int last = -1; for (int i = 0; i < vertices.Count; i++) { int n; //Find out if this vertex is on the old or new shape. if (FPVector2.Dot(MathUtils.Cross(localExitPoint - localEntryPoint, 1), vertices[i] - localEntryPoint) > Settings.Epsilon) { n = 0; } else { n = 1; } if (last != n) { //If we switch from one shape to the other add the cut vertices. if (last == 0) { Debug.Assert(cutAdded[0] == -1); cutAdded[0] = newPolygon[last].Count; newPolygon[last].Add(localExitPoint); newPolygon[last].Add(localEntryPoint); } if (last == 1) { Debug.Assert(cutAdded[last] == -1); cutAdded[last] = newPolygon[last].Count; newPolygon[last].Add(localEntryPoint); newPolygon[last].Add(localExitPoint); } } newPolygon[n].Add(vertices[i]); last = n; } //Add the cut in case it has not been added yet. if (cutAdded[0] == -1) { cutAdded[0] = newPolygon[0].Count; newPolygon[0].Add(localExitPoint); newPolygon[0].Add(localEntryPoint); } if (cutAdded[1] == -1) { cutAdded[1] = newPolygon[1].Count; newPolygon[1].Add(localEntryPoint); newPolygon[1].Add(localExitPoint); } for (int n = 0; n < 2; n++) { FPVector2 offset; if (cutAdded[n] > 0) { offset = (newPolygon[n][cutAdded[n] - 1] - newPolygon[n][cutAdded[n]]); } else { offset = (newPolygon[n][newPolygon[n].Count - 1] - newPolygon[n][0]); } offset.Normalize(); if (!offset.IsValid()) { offset = FPVector2.one; } newPolygon[n][cutAdded[n]] += Settings.Epsilon * offset; if (cutAdded[n] < newPolygon[n].Count - 2) { offset = (newPolygon[n][cutAdded[n] + 2] - newPolygon[n][cutAdded[n] + 1]); } else { offset = (newPolygon[n][0] - newPolygon[n][newPolygon[n].Count - 1]); } offset.Normalize(); if (!offset.IsValid()) { offset = FPVector2.one; } newPolygon[n][cutAdded[n] + 1] += Settings.Epsilon * offset; } first = newPolygon[0]; second = newPolygon[1]; }
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 = Spax.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 override void SolveVelocityConstraints(ref SolverData data) { FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { FP Cdot = FPVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; FP impulse = _motorMass * (_motorSpeed - Cdot); FP oldImpulse = MotorImpulse; FP maxImpulse = data.step.dt * _maxMotorForce; MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = MotorImpulse - oldImpulse; FPVector2 P = impulse * _axis; FP LA = impulse * _a1; FP LB = impulse * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } FPVector2 Cdot1 = new FPVector2(); Cdot1.x = FPVector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA; Cdot1.y = wB - wA; if (_enableLimit && _limitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. FP Cdot2; Cdot2 = FPVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; FPVector Cdot = new FPVector(Cdot1.x, Cdot1.y, Cdot2); FPVector f1 = _impulse; FPVector df = _K.Solve33(Cdot * -1); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.z = KBEngine.FPMath.Max(_impulse.z, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.z = KBEngine.FPMath.Min(_impulse.z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) FPVector2 b = -Cdot1 - (_impulse.z - f1.z) * new FPVector2(_K.ez.x, _K.ez.y); FPVector2 f2r = _K.Solve22(b) + new FPVector2(f1.x, f1.y); _impulse.x = f2r.x; _impulse.y = f2r.y; df = _impulse - f1; FPVector2 P = df.x * _perp + df.z * _axis; FP LA = df.x * _s1 + df.y + df.z * _a1; FP LB = df.x * _s2 + df.y + df.z * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { // Limit is inactive, just solve the prismatic constraint in block form. FPVector2 df = _K.Solve22(-Cdot1); _impulse.x += df.x; _impulse.y += df.y; FPVector2 P = df.x * _perp; FP LA = df.x * _s1 + df.y; FP LB = df.x * _s2 + df.y; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
/// <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) { FPVector2 p1 = input.Point1; FPVector2 p2 = input.Point2; FPVector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. FPVector2 absV = MathUtils.Abs(new FPVector2(-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(); { FPVector2 t = p1 + maxFraction * (p2 - p1); FPVector2.Min(ref p1, ref t, out segmentAABB.LowerBound); FPVector2.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) FPVector2 c = node.AABB.Center; FPVector2 h = node.AABB.Extents; FP separation = FP.Abs(FPVector2.Dot(new FPVector2(-r.y, r.x), p1 - c)) - FPVector2.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; FPVector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = FPVector2.Min(p1, t); segmentAABB.UpperBound = FPVector2.Max(p1, t); } } else { _raycastStack.Push(node.Child1); _raycastStack.Push(node.Child2); } } }
// Possible regions: // - points[2] // - edge points[0]-points[2] // - edge points[1]-points[2] // - inside the triangle internal void Solve3() { FPVector2 w1 = V[0].W; FPVector2 w2 = V[1].W; FPVector2 w3 = V[2].W; // Edge12 // [1 1 ][a1] = [1] // [w1.e12 w2.e12][a2] = [0] // a3 = 0 FPVector2 e12 = w2 - w1; FP w1e12 = FPVector2.Dot(w1, e12); FP w2e12 = FPVector2.Dot(w2, e12); FP d12_1 = w2e12; FP d12_2 = -w1e12; // Edge13 // [1 1 ][a1] = [1] // [w1.e13 w3.e13][a3] = [0] // a2 = 0 FPVector2 e13 = w3 - w1; FP w1e13 = FPVector2.Dot(w1, e13); FP w3e13 = FPVector2.Dot(w3, e13); FP d13_1 = w3e13; FP d13_2 = -w1e13; // Edge23 // [1 1 ][a2] = [1] // [w2.e23 w3.e23][a3] = [0] // a1 = 0 FPVector2 e23 = w3 - w2; FP w2e23 = FPVector2.Dot(w2, e23); FP w3e23 = FPVector2.Dot(w3, e23); FP d23_1 = w3e23; FP d23_2 = -w2e23; // Triangle123 FP n123 = MathUtils.Cross(e12, e13); FP d123_1 = n123 * MathUtils.Cross(w2, w3); FP d123_2 = n123 * MathUtils.Cross(w3, w1); FP d123_3 = n123 * MathUtils.Cross(w1, w2); // w1 region if (d12_2 <= 0.0f && d13_2 <= 0.0f) { SimplexVertex v0_1 = V[0]; v0_1.A = 1.0f; V[0] = v0_1; Count = 1; return; } // e12 if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) { FP inv_d12 = 1.0f / (d12_1 + d12_2); SimplexVertex v0_2 = V[0]; SimplexVertex v1_2 = V[1]; v0_2.A = d12_1 * inv_d12; v1_2.A = d12_2 * inv_d12; V[0] = v0_2; V[1] = v1_2; Count = 2; return; } // e13 if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) { FP inv_d13 = 1.0f / (d13_1 + d13_2); SimplexVertex v0_3 = V[0]; SimplexVertex v2_3 = V[2]; v0_3.A = d13_1 * inv_d13; v2_3.A = d13_2 * inv_d13; V[0] = v0_3; V[2] = v2_3; Count = 2; V[1] = V[2]; return; } // w2 region if (d12_1 <= 0.0f && d23_2 <= 0.0f) { SimplexVertex v1_4 = V[1]; v1_4.A = 1.0f; V[1] = v1_4; Count = 1; V[0] = V[1]; return; } // w3 region if (d13_1 <= 0.0f && d23_1 <= 0.0f) { SimplexVertex v2_5 = V[2]; v2_5.A = 1.0f; V[2] = v2_5; Count = 1; V[0] = V[2]; return; } // e23 if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) { FP inv_d23 = 1.0f / (d23_1 + d23_2); SimplexVertex v1_6 = V[1]; SimplexVertex v2_6 = V[2]; v1_6.A = d23_1 * inv_d23; v2_6.A = d23_2 * inv_d23; V[1] = v1_6; V[2] = v2_6; Count = 2; V[0] = V[2]; return; } // Must be in triangle123 FP inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); SimplexVertex v0_7 = V[0]; SimplexVertex v1_7 = V[1]; SimplexVertex v2_7 = V[2]; v0_7.A = d123_1 * inv_d123; v1_7.A = d123_2 * inv_d123; v2_7.A = d123_3 * inv_d123; V[0] = v0_7; V[1] = v1_7; V[2] = v2_7; Count = 3; }
internal override void SolveVelocityConstraints(ref SolverData data) { FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; // Solve spring constraint { FP Cdot = FPVector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA; FP impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse); _springImpulse += impulse; FPVector2 P = impulse * _ax; FP LA = impulse * _sAx; FP LB = impulse * _sBx; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } // Solve rotational motor constraint { 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 point to line constraint { FP Cdot = FPVector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA; FP impulse = -_mass * Cdot; _impulse += impulse; FPVector2 P = impulse * _ay; FP LA = impulse * _sAy; FP LB = impulse * _sBy; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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 mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; FPVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); // Compute the effective masses. FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FPVector2 d1 = cB + rB - cA - rA; // Point to line constraint { _ay = MathUtils.Mul(qA, _localYAxis); _sAy = MathUtils.Cross(d1 + rA, _ay); _sBy = MathUtils.Cross(rB, _ay); _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy; if (_mass > 0.0f) { _mass = 1.0f / _mass; } } // Spring constraint _springMass = 0.0f; _bias = 0.0f; _gamma = 0.0f; if (Frequency > 0.0f) { _ax = MathUtils.Mul(qA, LocalXAxis); _sAx = MathUtils.Cross(d1 + rA, _ax); _sBx = MathUtils.Cross(rB, _ax); FP invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx; if (invMass > 0.0f) { _springMass = 1.0f / invMass; FP C = FPVector2.Dot(d1, _ax); // Frequency FP omega = 2.0f * Settings.Pi * Frequency; // Damping coefficient FP d = 2.0f * _springMass * DampingRatio * omega; // Spring stiffness FP k = _springMass * omega * omega; // magic formulas FP h = data.step.dt; _gamma = h * (d + h * k); if (_gamma > 0.0f) { _gamma = 1.0f / _gamma; } _bias = C * h * k * _gamma; _springMass = invMass + _gamma; if (_springMass > 0.0f) { _springMass = 1.0f / _springMass; } } } else { _springImpulse = 0.0f; } // Rotational motor if (_enableMotor) { _motorMass = iA + iB; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } } else { _motorMass = 0.0f; _motorImpulse = 0.0f; } if (Settings.EnableWarmstarting) { // Account for variable time step. _impulse *= data.step.dtRatio; _springImpulse *= data.step.dtRatio; _motorImpulse *= data.step.dtRatio; FPVector2 P = _impulse * _ay + _springImpulse * _ax; FP LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse; FP LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; vA -= _invMassA * P; wA -= _invIA * LA; vB += _invMassB * P; wB += _invIB * LB; } else { _impulse = 0.0f; _springImpulse = 0.0f; _motorImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { // p = p1 + t * d // v = v1 + s * e // p1 + t * d = v1 + s * e // s * e - t * d = p1 - v1 output = new RayCastOutput(); // Put the ray into the edge's frame of reference. FPVector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p); FPVector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p); FPVector2 d = p2 - p1; FPVector2 v1 = _vertex1; FPVector2 v2 = _vertex2; FPVector2 e = v2 - v1; FPVector2 normal = new FPVector2(e.y, -e.x); //TODO: Could possibly cache the normal. normal.Normalize(); // q = p1 + t * d // dot(normal, q - v1) = 0 // dot(normal, p1 - v1) + t * dot(normal, d) = 0 FP numerator = FPVector2.Dot(normal, v1 - p1); FP denominator = FPVector2.Dot(normal, d); if (denominator == 0.0f) { return(false); } FP t = numerator / denominator; if (t < 0.0f || input.MaxFraction < t) { return(false); } FPVector2 q = p1 + t * d; // q = v1 + s * r // s = dot(q - v1, r) / dot(r, r) FPVector2 r = v2 - v1; FP rr = FPVector2.Dot(r, r); if (rr == 0.0f) { return(false); } FP s = FPVector2.Dot(q - v1, r) / rr; if (s < 0.0f || 1.0f < s) { return(false); } output.Fraction = t; if (numerator > 0.0f) { output.Normal = -normal; } else { output.Normal = normal; } return(true); }
internal override bool SolvePositionConstraints(ref SolverData data) { FPVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; FPVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; FPVector2 cC = data.positions[_indexC].c; FP aC = data.positions[_indexC].a; FPVector2 cD = data.positions[_indexD].c; FP aD = data.positions[_indexD].a; Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); FP linearError = 0.0f; FP coordinateA, coordinateB; FPVector2 JvAC, JvBD; FP JwA, JwB, JwC, JwD; FP mass = 0.0f; if (_typeA == JointType.Revolute) { JvAC = FPVector2.zero; JwA = 1.0f; JwC = 1.0f; mass += _iA + _iC; coordinateA = aA - aC - _referenceAngleA; } else { FPVector2 u = MathUtils.Mul(qC, _localAxisC); FPVector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); FPVector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); JvAC = u; JwC = MathUtils.Cross(rC, u); JwA = MathUtils.Cross(rA, u); mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA; FPVector2 pC = _localAnchorC - _lcC; FPVector2 pA = MathUtils.MulT(qC, rA + (cA - cC)); coordinateA = FPVector2.Dot(pA - pC, _localAxisC); } if (_typeB == JointType.Revolute) { JvBD = FPVector2.zero; JwB = _ratio; JwD = _ratio; mass += _ratio * _ratio * (_iB + _iD); coordinateB = aB - aD - _referenceAngleB; } else { FPVector2 u = MathUtils.Mul(qD, _localAxisD); FPVector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); FPVector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); JvBD = _ratio * u; JwD = _ratio * MathUtils.Cross(rD, u); JwB = _ratio * MathUtils.Cross(rB, u); mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB; FPVector2 pD = _localAnchorD - _lcD; FPVector2 pB = MathUtils.MulT(qD, rB + (cB - cD)); coordinateB = FPVector2.Dot(pB - pD, _localAxisD); } FP C = (coordinateA + _ratio * coordinateB) - _constant; FP impulse = 0.0f; if (mass > 0.0f) { impulse = -C / mass; } cA += _mA * impulse * JvAC; aA += _iA * impulse * JwA; cB += _mB * impulse * JvBD; aB += _iB * impulse * JwB; cC -= _mC * impulse * JvAC; aC -= _iC * impulse * JwC; cD -= _mD * impulse * JvBD; aD -= _iD * impulse * JwD; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; data.positions[_indexC].c = cC; data.positions[_indexC].a = aC; data.positions[_indexD].c = cD; data.positions[_indexD].a = aD; // TODO_ERIN not implemented return(linearError < Settings.LinearSlop); }
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; FPVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); // Compute the effective masses. FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FPVector2 d = (cB - cA) + rB - rA; FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; // Compute motor Jacobian and effective mass. { _axis = MathUtils.Mul(qA, LocalXAxis); _a1 = MathUtils.Cross(d + rA, _axis); _a2 = MathUtils.Cross(rB, _axis); _motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } } // Prismatic constraint. { _perp = MathUtils.Mul(qA, _localYAxisA); _s1 = MathUtils.Cross(d + rA, _perp); _s2 = MathUtils.Cross(rB, _perp); FP k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2; FP k12 = iA * _s1 + iB * _s2; FP k13 = iA * _s1 * _a1 + iB * _s2 * _a2; FP k22 = iA + iB; if (k22 == 0.0f) { // For bodies with fixed rotation. k22 = 1.0f; } FP k23 = iA * _a1 + iB * _a2; FP k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; _K.ex = new FPVector(k11, k12, k13); _K.ey = new FPVector(k12, k22, k23); _K.ez = new FPVector(k13, k23, k33); } // Compute motor and limit terms. if (_enableLimit) { FP jointTranslation = FPVector2.Dot(_axis, d); if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { _limitState = LimitState.Equal; } else if (jointTranslation <= _lowerTranslation) { if (_limitState != LimitState.AtLower) { _limitState = LimitState.AtLower; _impulse.z = 0.0f; } } else if (jointTranslation >= _upperTranslation) { if (_limitState != LimitState.AtUpper) { _limitState = LimitState.AtUpper; _impulse.z = 0.0f; } } else { _limitState = LimitState.Inactive; _impulse.z = 0.0f; } } else { _limitState = LimitState.Inactive; _impulse.z = 0.0f; } if (_enableMotor == false) { MotorImpulse = 0.0f; } if (Settings.EnableWarmstarting) { // Account for variable time step. _impulse *= data.step.dtRatio; MotorImpulse *= data.step.dtRatio; FPVector2 P = _impulse.x * _perp + (MotorImpulse + _impulse.z) * _axis; FP LA = _impulse.x * _s1 + _impulse.y + (MotorImpulse + _impulse.z) * _a1; FP LB = _impulse.x * _s2 + _impulse.y + (MotorImpulse + _impulse.z) * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { _impulse = FPVector.zero; MotorImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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. }
internal override bool SolvePositionConstraints(ref SolverData data) { FPVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; FPVector2 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; // Compute fresh Jacobians FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FPVector2 d = cB + rB - cA - rA; FPVector2 axis = MathUtils.Mul(qA, LocalXAxis); FP a1 = MathUtils.Cross(d + rA, axis); FP a2 = MathUtils.Cross(rB, axis); FPVector2 perp = MathUtils.Mul(qA, _localYAxisA); FP s1 = MathUtils.Cross(d + rA, perp); FP s2 = MathUtils.Cross(rB, perp); FPVector impulse; FPVector2 C1 = new FPVector2(); C1.x = FPVector2.Dot(perp, d); C1.y = aB - aA - ReferenceAngle; FP linearError = FP.Abs(C1.x); FP angularError = FP.Abs(C1.y); bool active = false; FP C2 = 0.0f; if (_enableLimit) { FP translation = FPVector2.Dot(axis, d); if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = KBEngine.FPMath.Max(linearError, FP.Abs(translation)); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = KBEngine.FPMath.Max(linearError, _lowerTranslation - translation); active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = KBEngine.FPMath.Max(linearError, translation - _upperTranslation); active = true; } } if (active) { FP k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; FP k12 = iA * s1 + iB * s2; FP k13 = iA * s1 * a1 + iB * s2 * a2; FP k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } FP k23 = iA * a1 + iB * a2; FP k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = new Mat33(); K.ex = new FPVector(k11, k12, k13); K.ey = new FPVector(k12, k22, k23); K.ez = new FPVector(k13, k23, k33); FPVector C = new FPVector(); C.x = C1.x; C.y = C1.y; C.z = C2; impulse = K.Solve33(C * -1); } else { FP k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; FP k12 = iA * s1 + iB * s2; FP k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = new Mat22(); K.ex = new FPVector2(k11, k12); K.ey = new FPVector2(k12, k22); FPVector2 impulse1 = K.Solve(-C1); impulse = new FPVector(); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } FPVector2 P = impulse.x * perp + impulse.z * axis; FP LA = impulse.x * s1 + impulse.y + impulse.z * a1; FP LB = impulse.x * s2 + impulse.y + impulse.z * a2; cA -= mA * P; aA -= iA * LA; cB += mB * P; aB += iB * LB; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
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); }