/// <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)));
}
}
}