/// <summary> Precaculate everything and apply initial impulse before the /// simulation Step takes place /// /// </summary> /// <param name="invDT">The amount of time the simulation is being stepped by /// </param> public virtual void PreStep(float invDT) { // Pre-compute anchors, mass matrix, and bias. Matrix2f rot1 = new Matrix2f(body1.Rotation); Matrix2f rot2 = new Matrix2f(body2.Rotation); r1 = MathUtil.Mul(rot1, localAnchor1); r2 = MathUtil.Mul(rot2, localAnchor2); // deltaV = deltaV0 + K * impulse // invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] Matrix2f K1 = new Matrix2f(); K1.col1.x = body1.InvMass + body2.InvMass; K1.col2.x = 0.0f; K1.col1.y = 0.0f; K1.col2.y = body1.InvMass + body2.InvMass; Matrix2f K2 = new Matrix2f(); K2.col1.x = body1.InvI * r1.y * r1.y; K2.col2.x = (- body1.InvI) * r1.x * r1.y; K2.col1.y = (- body1.InvI) * r1.x * r1.y; K2.col2.y = body1.InvI * r1.x * r1.x; Matrix2f K3 = new Matrix2f(); K3.col1.x = body2.InvI * r2.y * r2.y; K3.col2.x = (- body2.InvI) * r2.x * r2.y; K3.col1.y = (- body2.InvI) * r2.x * r2.y; K3.col2.y = body2.InvI * r2.x * r2.x; Matrix2f K = MathUtil.Add(MathUtil.Add(K1, K2), K3); M = K.Invert(); Vector2f p1 = new Vector2f(body1.GetPosition()); p1.Add(r1); Vector2f p2 = new Vector2f(body2.GetPosition()); p2.Add(r2); Vector2f dp = new Vector2f(p2); dp.Sub(p1); bias = new Vector2f(dp); bias.Scale(- 0.1f); bias.Scale(invDT); // Apply accumulated impulse. accumulatedImpulse.Scale(relaxation); if (!body1.Static) { Vector2f accum1 = new Vector2f(accumulatedImpulse); accum1.Scale(- body1.InvMass); body1.AdjustVelocity(accum1); body1.AdjustAngularVelocity(- (body1.InvI * MathUtil.Cross(r1, accumulatedImpulse))); } if (!body2.Static) { Vector2f accum2 = new Vector2f(accumulatedImpulse); accum2.Scale(body2.InvMass); body2.AdjustVelocity(accum2); body2.AdjustAngularVelocity(body2.InvI * MathUtil.Cross(r2, accumulatedImpulse)); } }
/// <seealso cref="Silver.Weight.Raw.Collide.Collider.Collide(Silver.Weight.Raw.Contact[], Silver.Weight.Raw.Body, Silver.Weight.Raw.Body)"> /// </seealso> public virtual int Collide(Contact[] contacts, Body bodyA, Body bodyB) { int numContacts = 0; Line line = (Line) bodyA.Shape; Box box = (Box) bodyB.Shape; Vector2f lineVec = new Vector2f(line.DX, line.DY); lineVec.Normalise(); Vector2f axis = new Vector2f(- line.DY, line.DX); axis.Normalise(); Vector2f res = new Vector2f(); line.Start.ProjectOntoUnit(axis, res); float linePos = GetProp(res, axis); Vector2f c = MathUtil.Sub(bodyB.GetPosition(), bodyA.GetPosition()); c.ProjectOntoUnit(axis, res); float centre = GetProp(res, axis); Vector2f[] pts = box.GetPoints(bodyB.GetPosition(), bodyB.Rotation); float[] tangent = new float[4]; float[] proj = new float[4]; int outOfRange = 0; for (int i = 0; i < 4; i++) { pts[i].Sub(bodyA.GetPosition()); pts[i].ProjectOntoUnit(axis, res); tangent[i] = GetProp(res, axis); pts[i].ProjectOntoUnit(lineVec, res); proj[i] = GetProp(res, new Vector2f(line.DX, line.DY)); if ((proj[i] >= 1) || (proj[i] <= 0)) { outOfRange++; } } if (outOfRange == 4) { return 0; } Vector2f normal = new Vector2f(axis); if (centre < linePos) { if (!line.BlocksInnerEdge) { return 0; } normal.Scale(- 1); for (int i = 0; i < 4; i++) { if (tangent[i] > linePos) { if (proj[i] < 0) { Vector2f onAxis = new Vector2f(); Line leftLine = new Line(GetPt(pts, i - 1), pts[i]); Line rightLine = new Line(GetPt(pts, i + 1), pts[i]); leftLine.getClosestPoint(line.Start, res); res.ProjectOntoUnit(axis, onAxis); float left = GetProp(onAxis, axis); rightLine.getClosestPoint(line.Start, res); res.ProjectOntoUnit(axis, onAxis); float right = GetProp(onAxis, axis); if ((left > 0) && (right > 0)) { Vector2f pos = new Vector2f(bodyA.GetPosition()); pos.Add(line.Start); ResolveEndPointCollision(pos, bodyA, bodyB, normal, leftLine, rightLine, contacts[numContacts], i); numContacts++; } } else if (proj[i] > 1) { Vector2f onAxis = new Vector2f(); Line leftLine = new Line(GetPt(pts, i - 1), pts[i]); Line rightLine = new Line(GetPt(pts, i + 1), pts[i]); leftLine.getClosestPoint(line.End, res); res.ProjectOntoUnit(axis, onAxis); float left = GetProp(onAxis, axis); rightLine.getClosestPoint(line.End, res); res.ProjectOntoUnit(axis, onAxis); float right = GetProp(onAxis, axis); if ((left > 0) && (right > 0)) { Vector2f pos = new Vector2f(bodyA.GetPosition()); pos.Add(line.End); ResolveEndPointCollision(pos, bodyA, bodyB, normal, leftLine, rightLine, contacts[numContacts], i); numContacts++; } } else { pts[i].ProjectOntoUnit(lineVec, res); res.Add(bodyA.GetPosition()); contacts[numContacts].Separation = - (tangent[i] - linePos); contacts[numContacts].Position = new Vector2f(res); contacts[numContacts].Normal = normal; contacts[numContacts].Feature = new FeaturePair(i); numContacts++; } } } } else { if (!line.BlocksOuterEdge) { return 0; } for (int i = 0; i < 4; i++) { if (tangent[i] < linePos) { if (proj[i] < 0) { Vector2f onAxis = new Vector2f(); Line leftLine = new Line(GetPt(pts, i - 1), pts[i]); Line rightLine = new Line(GetPt(pts, i + 1), pts[i]); leftLine.getClosestPoint(line.Start, res); res.ProjectOntoUnit(axis, onAxis); float left = GetProp(onAxis, axis); rightLine.getClosestPoint(line.Start, res); res.ProjectOntoUnit(axis, onAxis); float right = GetProp(onAxis, axis); if ((left < 0) && (right < 0)) { Vector2f pos = new Vector2f(bodyA.GetPosition()); pos.Add(line.Start); ResolveEndPointCollision(pos, bodyA, bodyB, normal, leftLine, rightLine, contacts[numContacts], i); numContacts++; } } else if (proj[i] > 1) { Vector2f onAxis = new Vector2f(); Line leftLine = new Line(GetPt(pts, i - 1), pts[i]); Line rightLine = new Line(GetPt(pts, i + 1), pts[i]); leftLine.getClosestPoint(line.End, res); res.ProjectOntoUnit(axis, onAxis); float left = GetProp(onAxis, axis); rightLine.getClosestPoint(line.End, res); res.ProjectOntoUnit(axis, onAxis); float right = GetProp(onAxis, axis); if ((left < 0) && (right < 0)) { Vector2f pos = new Vector2f(bodyA.GetPosition()); pos.Add(line.End); ResolveEndPointCollision(pos, bodyA, bodyB, normal, leftLine, rightLine, contacts[numContacts], i); numContacts++; } } else { pts[i].ProjectOntoUnit(lineVec, res); res.Add(bodyA.GetPosition()); contacts[numContacts].Separation = - (linePos - tangent[i]); contacts[numContacts].Position = new Vector2f(res); contacts[numContacts].Normal = normal; contacts[numContacts].Feature = new FeaturePair(); numContacts++; } } } } if (numContacts > 2) { throw new System.SystemException("LineBoxCollision: > 2 contacts"); } return numContacts; }
/// <summary> Apply the impulse caused by the joint to the bodies attached.</summary> public virtual void ApplyImpulse() { Vector2f dv = new Vector2f(body2.Velocity); dv.Add(MathUtil.Cross(body2.AngularVelocity, r2)); dv.Sub(body1.Velocity); dv.Sub(MathUtil.Cross(body1.AngularVelocity, r1)); dv.Scale(- 1); dv.Add(bias); // TODO: is this baumgarte stabilization? if (dv.LengthSquared() == 0) { return ; } Vector2f impulse = MathUtil.Mul(M, dv); if (!body1.Static) { Vector2f delta1 = new Vector2f(impulse); delta1.Scale(- body1.InvMass); body1.AdjustVelocity(delta1); body1.AdjustAngularVelocity((- body1.InvI) * MathUtil.Cross(r1, impulse)); } if (!body2.Static) { Vector2f delta2 = new Vector2f(impulse); delta2.Scale(body2.InvMass); body2.AdjustVelocity(delta2); body2.AdjustAngularVelocity(body2.InvI * MathUtil.Cross(r2, impulse)); } accumulatedImpulse.Add(impulse); }
/// <summary> Step the simulation. Currently anything other than 1/60f as a /// Step leads to unpredictable results - hence the default Step /// fixes us to this Step /// /// </summary> /// <param name="dt">The amount of time to Step /// </param> public virtual void Step(float dt) { BodyList bodies = ActiveBodies; JointList joints = ActiveJoints; float invDT = dt > 0.0f?1.0f / dt:0.0f; if (restingBodyDetection) { for (int i = 0; i < bodies.Size(); ++i) { Body b = bodies.Item(i); b.StartFrame(); } for (int i = 0; i < joints.Size(); ++i) { Joint j = joints.Item(i); j.Body1.IsResting = false; j.Body2.IsResting = false; } } BroadPhase(dt); for (int i = 0; i < bodies.Size(); ++i) { Body b = bodies.Item(i); if (b.InvMass == 0.0f) { continue; } if (b.Resting && restingBodyDetection) { continue; } Vector2f temp = new Vector2f(b.GetForce()); temp.Scale(b.InvMass); if (b.GravityEffected) { temp.Add(gravity); } temp.Scale(dt); b.AdjustVelocity(temp); Vector2f damping = new Vector2f(b.Velocity); damping.Scale((- b.Damping) * b.InvMass); b.AdjustVelocity(damping); b.AdjustAngularVelocity(dt * b.InvI * b.Torque); b.AdjustAngularVelocity((- b.AngularVelocity) * b.InvI * b.RotDamping); } for (int i = 0; i < arbiters.size(); i++) { Arbiter arb = arbiters.Item(i); if (!restingBodyDetection || !arb.HasRestingPair()) { arb.PreStep(invDT, dt, damping); } } for (int i = 0; i < joints.Size(); ++i) { Joint j = joints.Item(i); j.PreStep(invDT); } for (int i = 0; i < iterations; ++i) { for (int k = 0; k < arbiters.size(); k++) { Arbiter arb = arbiters.Item(k); if (!restingBodyDetection || !arb.HasRestingPair()) { arb.ApplyImpulse(); } else { arb.Body1.Collided(arb.Body2); arb.Body2.Collided(arb.Body1); } } for (int k = 0; k < joints.Size(); ++k) { Joint j = joints.Item(k); j.ApplyImpulse(); } } for (int i = 0; i < bodies.Size(); ++i) { Body b = bodies.Item(i); if (b.InvMass == 0.0f) { continue; } if (restingBodyDetection) { if (b.Resting) { continue; } } b.AdjustPosition(b.Velocity, dt); b.AdjustPosition(b.BiasedVelocity, dt); b.AdjustRotation(dt * b.AngularVelocity); b.AdjustRotation(dt * b.BiasedAngularVelocity); b.ResetBias(); b.SetForce(0, 0); b.Torque = 0; } if (restingBodyDetection) { for (int i = 0; i < bodies.Size(); ++i) { Body b = bodies.Item(i); b.EndFrame(); } } }