/// <seealso cref="Silver.Weight.Raw.Collide.Collider.Collide(Silver.Weight.Raw.Contact[], Silver.Weight.Raw.Body, Silver.Weight.Raw.Body)"> /// </seealso> public override int Collide(Contact[] contacts, Body bodyA, Body bodyB) { Polygon polyA = (Polygon) bodyA.Shape; Circle circle = (Circle) bodyB.Shape; // TODO: this can be optimized using matrix multiplications and moving only the circle Vector2f[] vertsA = polyA.GetVertices(bodyA.GetPosition(), bodyA.Rotation); Vector2f centroidA = new Vector2f(polyA.GetCentroid()); centroidA.Add(bodyA.GetPosition()); int[][] collPairs = GetCollisionCandidates(vertsA, centroidA, circle.Radius, bodyB.GetPosition()); int noContacts = 0; for (int i = 0; i < collPairs.Length; i++) { if (noContacts >= contacts.Length) return contacts.Length; Vector2f lineStartA = vertsA[collPairs[i][0]]; Vector2f lineEndA = vertsA[(collPairs[i][0] + 1) % vertsA.Length]; Line line = new Line(lineStartA, lineEndA); float dis2 = line.distanceSquared(bodyB.GetPosition()); float r2 = circle.Radius * circle.Radius; if (dis2 < r2) { Vector2f pt = new Vector2f(); line.getClosestPoint(bodyB.GetPosition(), pt); Vector2f normal = new Vector2f(bodyB.GetPosition()); normal.Sub(pt); float sep = circle.Radius - normal.Length(); normal.Normalise(); contacts[noContacts].Separation = - sep; contacts[noContacts].Position = pt; contacts[noContacts].Normal = normal; contacts[noContacts].Feature = new FeaturePair(); noContacts++; } } return noContacts; }
/// <summary> Move this line a certain amount /// /// </summary> /// <param name="v">The amount to Move the line /// </param> public virtual void move(ROVector2f v) { Vector2f temp = new Vector2f(start); temp.Add(v); start = temp; temp = new Vector2f(end); temp.Add(v); end = temp; }
// private static Vector2f r1 = new Vector2f(); // private static Vector2f r2 = new Vector2f(); // private static Vector2f relativeVelocity = new Vector2f(); // private static Vector2f impulse = new Vector2f(); // private static Vector2f Pb = new Vector2f(); // private static Vector2f tmp = new Vector2f(); // // /** // * Apply the impulse accumlated at the contact points maintained // * by this arbiter. // */ // void ApplyImpulse() { // Body b1 = body1; // Body b2 = body2; // // for (int i = 0; i < numContacts; ++i) // { // Contact c = contacts[i]; // // r1.set(c.position); // r1.Sub(b1.GetPosition()); // r2.set(c.position); // r2.Sub(b2.GetPosition()); // // // Relative velocity at contact // relativeVelocity.set(b2.getVelocity()); // relativeVelocity.Add(MathUtil.Cross(b2.getAngularVelocity(), r2)); // relativeVelocity.Sub(b1.getVelocity()); // relativeVelocity.Sub(MathUtil.Cross(b1.getAngularVelocity(), r1)); // // // Compute normal impulse with bias. // float vn = relativeVelocity.Dot(c.normal); // // // bias caculations are now handled seperately hence we only // // handle the real impulse caculations here // //float normalImpulse = c.massNormal * ((c.restitution - vn) + c.bias); // float normalImpulse = c.massNormal * (c.restitution - vn); // // // Clamp the accumulated impulse // float oldNormalImpulse = c.accumulatedNormalImpulse; // c.accumulatedNormalImpulse = Math.max(oldNormalImpulse + normalImpulse, 0.0f); // normalImpulse = c.accumulatedNormalImpulse - oldNormalImpulse; // // if (normalImpulse != 0) { // // Apply contact impulse // impulse.set(c.normal); // impulse.Scale(normalImpulse); // // tmp.set(impulse); // tmp.Scale(-b1.getInvMass()); // b1.AdjustVelocity(tmp); // b1.AdjustAngularVelocity(-(b1.getInvI() * MathUtil.Cross(r1, impulse))); // // tmp.set(impulse); // tmp.Scale(b2.getInvMass()); // b2.AdjustVelocity(tmp); // b2.AdjustAngularVelocity(b2.getInvI() * MathUtil.Cross(r2, impulse)); // } // // // Compute bias impulse // // NEW STUFF FOR SEPERATING BIAS // relativeVelocity.set(b2.getBiasedVelocity()); // relativeVelocity.Add(MathUtil.Cross(b2.getBiasedAngularVelocity(), r2)); // relativeVelocity.Sub(b1.getBiasedVelocity()); // relativeVelocity.Sub(MathUtil.Cross(b1.getBiasedAngularVelocity(), r1)); // float vnb = relativeVelocity.Dot(c.normal); // // float biasImpulse = c.massNormal * (-vnb + c.bias); // float oldBiasImpulse = c.biasImpulse; // c.biasImpulse = Math.max(oldBiasImpulse + biasImpulse, 0.0f); // biasImpulse = c.biasImpulse - oldBiasImpulse; // // if (biasImpulse != 0) { // Pb.set(c.normal); // Pb.Scale(biasImpulse); // // tmp.set(Pb); // tmp.Scale(-b1.getInvMass()); // b1.AdjustBiasedVelocity(tmp); // b1.AdjustBiasedAngularVelocity(-(b1.getInvI() * MathUtil.Cross(r1, Pb))); // // tmp.set(Pb); // tmp.Scale(b2.getInvMass()); // b2.AdjustBiasedVelocity(tmp); // b2.AdjustBiasedAngularVelocity((b2.getInvI() * MathUtil.Cross(r2, Pb))); // } // // END NEW STUFF // // // // // Compute friction (tangent) impulse // // // float maxTangentImpulse = friction * c.accumulatedNormalImpulse; // // // Relative velocity at contact // relativeVelocity.set(b2.getVelocity()); // relativeVelocity.Add(MathUtil.Cross(b2.getAngularVelocity(), r2)); // relativeVelocity.Sub(b1.getVelocity()); // relativeVelocity.Sub(MathUtil.Cross(b1.getAngularVelocity(), r1)); // // Vector2f tangent = MathUtil.Cross(c.normal, 1.0f); // float vt = relativeVelocity.Dot(tangent); // float tangentImpulse = c.massTangent * (-vt); // // // Clamp friction // float oldTangentImpulse = c.accumulatedTangentImpulse; // c.accumulatedTangentImpulse = MathUtil.Clamp(oldTangentImpulse + tangentImpulse, -maxTangentImpulse, maxTangentImpulse); // tangentImpulse = c.accumulatedTangentImpulse - oldTangentImpulse; // // // Apply contact impulse // if ((tangentImpulse > 0.1f) || (tangentImpulse < -0.1f)) { // impulse = MathUtil.Scale(tangent, tangentImpulse); // // tmp.set(impulse); // tmp.Scale(-b1.getInvMass()); // b1.AdjustVelocity(tmp); // b1.AdjustAngularVelocity(-b1.getInvI() * MathUtil.Cross(r1, impulse)); // // tmp.set(impulse); // tmp.Scale(b2.getInvMass()); // b2.AdjustVelocity(tmp); // b2.AdjustAngularVelocity(b2.getInvI() * MathUtil.Cross(r2, impulse)); // } // } // } /// <summary> Apply the impulse accumlated at the contact points maintained /// by this arbiter. /// </summary> internal virtual void ApplyImpulse() { Body b1 = body1; Body b2 = body2; for (int i = 0; i < numContacts; ++i) { Contact c = contacts[i]; Vector2f r1 = new Vector2f(c.position); r1.Sub(b1.GetPosition()); Vector2f r2 = new Vector2f(c.position); r2.Sub(b2.GetPosition()); // Relative velocity at contact Vector2f relativeVelocity = new Vector2f(b2.Velocity); relativeVelocity.Add(MathUtil.Cross(b2.AngularVelocity, r2)); relativeVelocity.Sub(b1.Velocity); relativeVelocity.Sub(MathUtil.Cross(b1.AngularVelocity, r1)); // Compute normal impulse with bias. float vn = relativeVelocity.Dot(c.normal); // bias caculations are now handled seperately hence we only // handle the real impulse caculations here //float normalImpulse = c.massNormal * ((c.restitution - vn) + c.bias); float normalImpulse = c.massNormal * (c.restitution - vn); // Clamp the accumulated impulse float oldNormalImpulse = c.accumulatedNormalImpulse; c.accumulatedNormalImpulse = System.Math.Max(oldNormalImpulse + normalImpulse, 0.0f); normalImpulse = c.accumulatedNormalImpulse - oldNormalImpulse; // Apply contact impulse Vector2f impulse = MathUtil.Scale(c.normal, normalImpulse); b1.AdjustVelocity(MathUtil.Scale(impulse, - b1.InvMass)); b1.AdjustAngularVelocity(- (b1.InvI * MathUtil.Cross(r1, impulse))); b2.AdjustVelocity(MathUtil.Scale(impulse, b2.InvMass)); b2.AdjustAngularVelocity(b2.InvI * MathUtil.Cross(r2, impulse)); // Compute bias impulse // NEW STUFF FOR SEPERATING BIAS relativeVelocity.Reconfigure(b2.BiasedVelocity); relativeVelocity.Add(MathUtil.Cross(b2.BiasedAngularVelocity, r2)); relativeVelocity.Sub(b1.BiasedVelocity); relativeVelocity.Sub(MathUtil.Cross(b1.BiasedAngularVelocity, r1)); float vnb = relativeVelocity.Dot(c.normal); float biasImpulse = c.massNormal * (- vnb + c.bias); float oldBiasImpulse = c.biasImpulse; c.biasImpulse = System.Math.Max(oldBiasImpulse + biasImpulse, 0.0f); biasImpulse = c.biasImpulse - oldBiasImpulse; Vector2f Pb = MathUtil.Scale(c.normal, biasImpulse); b1.AdjustBiasedVelocity(MathUtil.Scale(Pb, - b1.InvMass)); b1.AdjustBiasedAngularVelocity(- (b1.InvI * MathUtil.Cross(r1, Pb))); b2.AdjustBiasedVelocity(MathUtil.Scale(Pb, b2.InvMass)); b2.AdjustBiasedAngularVelocity((b2.InvI * MathUtil.Cross(r2, Pb))); // END NEW STUFF // // Compute friction (tangent) impulse // float maxTangentImpulse = friction * c.accumulatedNormalImpulse; // Relative velocity at contact relativeVelocity.Reconfigure(b2.Velocity); relativeVelocity.Add(MathUtil.Cross(b2.AngularVelocity, r2)); relativeVelocity.Sub(b1.Velocity); relativeVelocity.Sub(MathUtil.Cross(b1.AngularVelocity, r1)); Vector2f tangent = MathUtil.Cross(c.normal, 1.0f); float vt = relativeVelocity.Dot(tangent); float tangentImpulse = c.massTangent * (- vt); // Clamp friction float oldTangentImpulse = c.accumulatedTangentImpulse; c.accumulatedTangentImpulse = MathUtil.Clamp(oldTangentImpulse + tangentImpulse, - maxTangentImpulse, maxTangentImpulse); tangentImpulse = c.accumulatedTangentImpulse - oldTangentImpulse; // Apply contact impulse impulse = MathUtil.Scale(tangent, tangentImpulse); b1.AdjustVelocity(MathUtil.Scale(impulse, - b1.InvMass)); b1.AdjustAngularVelocity((- b1.InvI) * MathUtil.Cross(r1, impulse)); b2.AdjustVelocity(MathUtil.Scale(impulse, b2.InvMass)); b2.AdjustAngularVelocity(b2.InvI * MathUtil.Cross(r2, impulse)); } }
/// <summary> Apply the friction impulse from each contact. /// /// </summary> /// <param name="dt">The amount of time to Step the simulation by /// </param> /// <param name="invDT">The inverted time /// </param> /// <param name="damping">The percentage of energy to retain through out /// collision. (1 = no loss, 0 = total loss) /// </param> internal virtual void PreStep(float invDT, float dt, float damping) { float allowedPenetration = 0.01f; float biasFactor = 0.8f; for (int i = 0; i < numContacts; ++i) { Contact c = contacts[i]; c.normal.Normalise(); Vector2f r1 = new Vector2f(c.position); r1.Sub(body1.GetPosition()); Vector2f r2 = new Vector2f(c.position); r2.Sub(body2.GetPosition()); // Precompute normal mass, tangent mass, and bias. float rn1 = r1.Dot(c.normal); float rn2 = r2.Dot(c.normal); float kNormal = body1.InvMass + body2.InvMass; kNormal += body1.InvI * (r1.Dot(r1) - rn1 * rn1) + body2.InvI * (r2.Dot(r2) - rn2 * rn2); c.massNormal = damping / kNormal; Vector2f tangent = MathUtil.Cross(c.normal, 1.0f); float rt1 = r1.Dot(tangent); float rt2 = r2.Dot(tangent); float kTangent = body1.InvMass + body2.InvMass; kTangent += body1.InvI * (r1.Dot(r1) - rt1 * rt1) + body2.InvI * (r2.Dot(r2) - rt2 * rt2); c.massTangent = damping / kTangent; // Compute restitution // Relative velocity at contact Vector2f relativeVelocity = new Vector2f(body2.Velocity); relativeVelocity.Add(MathUtil.Cross(r2, body2.AngularVelocity)); relativeVelocity.Sub(body1.Velocity); relativeVelocity.Sub(MathUtil.Cross(r1, body1.AngularVelocity)); float combinedRestitution = (body1.Restitution * body2.Restitution); float relVel = c.normal.Dot(relativeVelocity); c.restitution = combinedRestitution * (- relVel); c.restitution = System.Math.Max(c.restitution, 0); float penVel = (- c.separation) / dt; if (c.restitution >= penVel) { c.bias = 0; } else { c.bias = (- biasFactor) * invDT * System.Math.Min(0.0f, c.separation + allowedPenetration); } // apply damping c.accumulatedNormalImpulse *= damping; // Apply normal + friction impulse Vector2f impulse = MathUtil.Scale(c.normal, c.accumulatedNormalImpulse); impulse.Add(MathUtil.Scale(tangent, c.accumulatedTangentImpulse)); body1.AdjustVelocity(MathUtil.Scale(impulse, - body1.InvMass)); body1.AdjustAngularVelocity((- body1.InvI) * MathUtil.Cross(r1, impulse)); body2.AdjustVelocity(MathUtil.Scale(impulse, body2.InvMass)); body2.AdjustAngularVelocity(body2.InvI * MathUtil.Cross(r2, impulse)); // rest bias c.biasImpulse = 0; } }
/// <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> 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> Resolve the collision math around an end point /// /// </summary> /// <param name="pos">The position of the contact /// </param> /// <param name="bodyA">The first body in the collision /// </param> /// <param name="bodyB">The second body in the collision /// </param> /// <param name="leftLine">The line to the left of the vertex of collision /// </param> /// <param name="rightLine">The line to the right of the vertex of collision /// </param> /// <param name="contact">The contact to populate /// </param> /// <param name="norm">The normal determined for the line /// </param> /// <param name="i">The index of teh face we're resolving for feature ID /// </param> private void ResolveEndPointCollision(Vector2f pos, Body bodyA, Body bodyB, Vector2f norm, Line leftLine, Line rightLine, Contact contact, int i) { Vector2f start = new Vector2f(pos); Vector2f end = new Vector2f(start); end.Add(norm); rightLine.move(bodyA.GetPosition()); leftLine.move(bodyA.GetPosition()); Line normLine = new Line(start, end); Vector2f rightPoint = normLine.intersect(rightLine); Vector2f leftPoint = normLine.intersect(leftLine); float dis1 = System.Single.MaxValue; if (rightPoint != null) { dis1 = rightPoint.Distance(start) - norm.Length(); } float dis2 = System.Single.MaxValue; if (leftPoint != null) { dis2 = leftPoint.Distance(start) - norm.Length(); } norm.Normalise(); float dis = System.Math.Min(dis1, dis2); contact.Separation = - dis; contact.Position = pos; contact.Normal = norm; contact.Feature = new FeaturePair(i); }
/// <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(); } } }
/// <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) { // calculate the spring's vector (pointing from body1 to body2) and Length spring = new Vector2f(body2.GetPosition()); spring.Add(r2); spring.Sub(body1.GetPosition()); spring.Sub(r1); springLength = spring.Length(); // the spring vector needs to be normalized for ApplyImpulse as well! spring.Normalise(); // calculate the spring's forces // note that although theoretically invDT could never be 0 // but here it can float springConst; if (springLength < minSpringSize || springLength > maxSpringSize) { // 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); // the mass normal or 'k' float rn1 = r1.Dot(spring); float rn2 = r2.Dot(spring); float kNormal = body1.InvMass + body2.InvMass; kNormal += body1.InvI * (r1.Dot(r1) - rn1 * rn1) + body2.InvI * (r2.Dot(r2) - rn2 * rn2); massNormal = 1 / kNormal; // The spring is broken so apply force to correct it // note that we use biased velocities for this float springImpulse = invDT != 0?brokenSpringConst * (springLength - springSize) / invDT:0; Vector2f impulse = MathUtil.Scale(spring, springImpulse); body1.AdjustBiasedVelocity(MathUtil.Scale(impulse, body1.InvMass)); body1.AdjustBiasedAngularVelocity((body1.InvI * MathUtil.Cross(r1, impulse))); body2.AdjustBiasedVelocity(MathUtil.Scale(impulse, - body2.InvMass)); body2.AdjustBiasedAngularVelocity(- (body2.InvI * MathUtil.Cross(r2, impulse))); isBroken = true; return ; } else if (springLength < springSize) { springConst = compressedSpringConst; isBroken = false; } else { // if ( springLength >= springSize ) springConst = stretchedSpringConst; isBroken = false; } float springImpulse2 = invDT != 0?springConst * (springLength - springSize) / invDT:0; // apply the spring's forces Vector2f impulse2 = MathUtil.Scale(spring, springImpulse2); body1.AdjustVelocity(MathUtil.Scale(impulse2, body1.InvMass)); body1.AdjustAngularVelocity((body1.InvI * MathUtil.Cross(r1, impulse2))); body2.AdjustVelocity(MathUtil.Scale(impulse2, - body2.InvMass)); body2.AdjustAngularVelocity(- (body2.InvI * MathUtil.Cross(r2, impulse2))); }
/// <summary> Apply the impulse caused by the joint to the bodies attached.</summary> public virtual void ApplyImpulse() { if (isBroken) { // calculate difference in velocity // TODO: share this code with BasicJoint and Arbiter Vector2f relativeVelocity = new Vector2f(body2.Velocity); relativeVelocity.Add(MathUtil.Cross(body2.AngularVelocity, r2)); relativeVelocity.Sub(body1.Velocity); relativeVelocity.Sub(MathUtil.Cross(body1.AngularVelocity, r1)); // project the relative velocity onto the spring vector and apply the mass normal float normalImpulse = massNormal * relativeVelocity.Dot(spring); // // TODO: Clamp the accumulated impulse? // float oldNormalImpulse = accumulatedNormalImpulse; // accumulatedNormalImpulse = Math.max(oldNormalImpulse + normalImpulse, 0.0f); // normalImpulse = accumulatedNormalImpulse - oldNormalImpulse; // only apply the impulse if we are pushing or pulling in the right way // i.e. pulling if the string is overstretched and pushing if it is too compressed if (springLength < minSpringSize && normalImpulse < 0 || springLength > maxSpringSize && normalImpulse > 0) { // now apply the impulses to the bodies Vector2f impulse = MathUtil.Scale(spring, normalImpulse); body1.AdjustVelocity(MathUtil.Scale(impulse, body1.InvMass)); body1.AdjustAngularVelocity((body1.InvI * MathUtil.Cross(r1, impulse))); body2.AdjustVelocity(MathUtil.Scale(impulse, - body2.InvMass)); body2.AdjustAngularVelocity(- (body2.InvI * MathUtil.Cross(r2, impulse))); } } }