/// <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 poly = (Polygon) bodyA.Shape;
Box box = (Box) bodyB.Shape;
// TODO: this can be optimized using matrix multiplications and moving only one shape
// specifically the box, because it has fewer vertices.
Vector2f[] vertsA = poly.GetVertices(bodyA.GetPosition(), bodyA.Rotation);
Vector2f[] vertsB = box.GetPoints(bodyB.GetPosition(), bodyB.Rotation);
// TODO: use a sweepline that has the smallest projection of the box
// now we use just an arbitrary one
Vector2f sweepline = new Vector2f(vertsB[1]);
sweepline.Sub(vertsB[2]);
EdgeSweep sweep = new EdgeSweep(sweepline);
sweep.AddVerticesToSweep(true, vertsA);
sweep.AddVerticesToSweep(false, vertsB);
int[][] collEdgeCands = sweep.OverlappingEdges;
// FeaturePair[] featurePairs = getFeaturePairs(contacts.Length, vertsA, vertsB, collEdgeCands);
// return PopulateContacts(contacts, vertsA, vertsB, featurePairs);
Intersection[][] intersections = GetIntersectionPairs(vertsA, vertsB, collEdgeCands);
return PopulateContacts(contacts, vertsA, vertsB, intersections);
}
/// <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> Gets the closest point to a given point on the indefinately extended line.
/// TODO: Move this somewhere in math package
///
/// </summary>
/// <param name="startA">Starting point of the line
/// </param>
/// <param name="endA">End point of the line
/// </param>
/// <param name="point">The point to get a closes point on the line for
/// </param>
/// <returns> the closest point on the line or null if the lines are parallel
/// </returns>
public static Vector2f GetClosestPoint(Vector2f startA, Vector2f endA, Vector2f point)
{
Vector2f startB = point;
Vector2f endB = new Vector2f(endA);
endB.Sub(startA);
endB.Reconfigure(endB.y, - endB.x);
float d = endB.y * (endA.x - startA.x);
d -= endB.x * (endA.y - startA.y);
if (d == 0)
return null;
float uA = endB.x * (startA.y - startB.Y);
uA -= endB.y * (startA.x - startB.X);
uA /= d;
return new Vector2f(startA.x + uA * (endA.x - startA.x), startA.y + uA * (endA.y - startA.y));
}
/// <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)
{
Line line = (Line) bodyA.Shape;
Polygon poly = (Polygon) bodyB.Shape;
// TODO: this can be optimized using matrix multiplications and moving only one shape
// specifically the line, because it has only two vertices
Vector2f[] vertsA = line.getVertices(bodyA.GetPosition(), bodyA.Rotation);
Vector2f[] vertsB = poly.GetVertices(bodyB.GetPosition(), bodyB.Rotation);
Vector2f pos = poly.GetCentroid(bodyB.GetPosition(), bodyB.Rotation);
// using the z axis of a 3d Cross product we determine on what side B is
bool isLeftOf = 0 > (pos.x - vertsA[0].x) * (vertsA[1].y - vertsA[0].y) - (vertsA[1].x - vertsA[0].x) * (pos.y - vertsA[0].y);
// to get the proper intersection pairs we make sure
// the line's normal is pointing towards the polygon
// TODO: verify that it's not actually pointing in the opposite direction
if (isLeftOf)
{
Vector2f tmp = vertsA[0];
vertsA[0] = vertsA[1];
vertsA[1] = tmp;
}
// we use the line's normal for our sweepline projection
Vector2f normal = new Vector2f(vertsA[1]);
normal.Sub(vertsA[0]);
normal.Reconfigure(normal.y, - normal.x);
EdgeSweep sweep = new EdgeSweep(normal);
sweep.Insert(0, true, vertsA[0].Dot(normal));
sweep.Insert(0, true, vertsA[1].Dot(normal));
sweep.AddVerticesToSweep(false, vertsB);
int[][] collEdgeCands = sweep.OverlappingEdges;
IntersectionGatherer intGath = new IntersectionGatherer(vertsA, vertsB);
for (int i = 0; i < collEdgeCands.Length; i++)
intGath.Intersect(collEdgeCands[i][0], collEdgeCands[i][1]);
Intersection[] intersections = intGath.Intersections;
return PopulateContacts(contacts, vertsA, vertsB, intersections);
}
/// <summary> Get the edges from a list of vertices that can Collide with the given circle.
/// This uses a sweepline algorithm which is only efficient if some assumptions
/// are indeed true. See CPolygonCPolygonCollider for more information.
///
/// </summary>
/// <param name="vertsA">The vertices of a polygon that is Collided with a circle
/// </param>
/// <param name="centroid">The center of the polygon
/// </param>
/// <param name="radius">The radius of the circle
/// </param>
/// <param name="circlePos">The position (center) of the circle
/// </param>
/// <returns> The list of edges that can Collide with the circle
/// </returns>
protected internal virtual int[][] GetCollisionCandidates(Vector2f[] vertsA, ROVector2f centroid, float radius, ROVector2f circlePos)
{
Vector2f sweepDir = new Vector2f(centroid);
sweepDir.Sub(circlePos);
sweepDir.Normalise(); //TODO: this normalization might not be necessary
EdgeSweep sweep = new EdgeSweep(sweepDir); //vertsA[0], true, true, dist);
sweep.AddVerticesToSweep(true, vertsA);
float circProj = circlePos.Dot(sweepDir);
sweep.Insert(0, false, - radius + circProj);
sweep.Insert(0, false, radius + circProj);
return sweep.OverlappingEdges;
}
/// <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;
}
}
// 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> Reconfigure this joint
///
/// </summary>
/// <param name="b1">The first body attached to this joint
/// </param>
/// <param name="b2">The second body attached to this joint
/// </param>
/// <param name="anchor">The static anchor point between the joints
/// </param>
public virtual void Reconfigure(Body b1, Body b2, Vector2f anchor)
{
body1 = b1;
body2 = b2;
Matrix2f rot1 = new Matrix2f(body1.Rotation);
Matrix2f rot2 = new Matrix2f(body2.Rotation);
Matrix2f rot1T = rot1.Transpose();
Matrix2f rot2T = rot2.Transpose();
Vector2f a1 = new Vector2f(anchor);
a1.Sub(body1.GetPosition());
localAnchor1 = MathUtil.Mul(rot1T, a1);
Vector2f a2 = new Vector2f(anchor);
a2.Sub(body2.GetPosition());
localAnchor2 = MathUtil.Mul(rot2T, a2);
accumulatedImpulse.Reconfigure(0.0f, 0.0f);
relaxation = 1.0f;
}
/// <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)));
}
}
}
/// <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)
{
Line line = (Line) bodyA.Shape;
Circle circle = (Circle) bodyB.Shape;
Vector2f[] vertsA = line.getVertices(bodyA.GetPosition(), bodyA.Rotation);
// compute intersection of the line A and a line parallel to
// the line A's normal passing through the origin of B
Vector2f startA = vertsA[0];
Vector2f endA = vertsA[1];
ROVector2f startB = bodyB.GetPosition();
Vector2f endB = new Vector2f(endA);
endB.Sub(startA);
endB.Reconfigure(endB.y, - endB.x);
// endB.Add(startB);// TODO: inline endB into equations below, this last operation will be useless..
//TODO: reuse mathutil.Intersect
// float d = (endB.y - startB.getY()) * (endA.x - startA.x);
// d -= (endB.x - startB.getX()) * (endA.y - startA.y);
//
// float uA = (endB.x - startB.getX()) * (startA.y - startB.getY());
// uA -= (endB.y - startB.getY()) * (startA.x - startB.getX());
// uA /= d;
float d = endB.y * (endA.x - startA.x);
d -= endB.x * (endA.y - startA.y);
float uA = endB.x * (startA.y - startB.Y);
uA -= endB.y * (startA.x - startB.X);
uA /= d;
Vector2f position = null;
if (uA < 0)
{
// the intersection is somewhere before startA
position = startA;
}
else if (uA > 1)
{
// the intersection is somewhere after endA
position = endA;
}
else
{
position = new Vector2f(startA.x + uA * (endA.x - startA.x), startA.y + uA * (endA.y - startA.y));
}
Vector2f normal = endB; // reuse of vector object
normal.Reconfigure(startB);
normal.Sub(position);
float distSquared = normal.LengthSquared();
float radiusSquared = circle.Radius * circle.Radius;
if (distSquared < radiusSquared)
{
contacts[0].Position = position;
contacts[0].Feature = new FeaturePair();
normal.Normalise();
contacts[0].Normal = normal;
float separation = (float) System.Math.Sqrt(distSquared) - circle.Radius;
contacts[0].Separation = separation;
return 1;
}
return 0;
}
/// <summary> Set a contact for an intersection where the colliding line's start- or endpoint
/// is contained in the colliding polygon.
///
/// TODO: The current implementation doesn't work properly: because lines are very
/// thin, they can slide into a polygon sideways which gives a very deep penetration
/// |
/// |->
/// | +-----+
/// |-> | |
/// | | |
/// | |
/// +-----+
///
/// A possible solution would be to use the velocity of the line relative to the
/// polygon to construct a collision normal and penetration depth.
/// Another possibility is to use the line's normals (both directions) and calculate
/// proper intersection distances for them.
/// If one has multiple normals/penetration depths to choose from, the one with the
/// minimum penetration depth will probably be the best bet.
///
/// </summary>
/// <param name="contact">The contact to set
/// </param>
/// <param name="intersection">The intersection where the line enters or exits the polygon
/// </param>
/// <param name="vertsA">The line's vertices
/// </param>
/// <param name="vertsB">The polygon's vertices
/// </param>
public virtual void SetLineEndContact(Contact contact, Intersection intersection, Vector2f[] vertsA, Vector2f[] vertsB)
{
Vector2f separation = new Vector2f(intersection.position);
if (intersection.isIngoing)
separation.Sub(vertsA[1]);
else
separation.Sub(vertsA[0]);
float depthA = 0; //separation.Length();
contact.Separation = - depthA;
contact.Normal = MathUtil.GetNormal(vertsB[(intersection.edgeB + 1) % vertsB.Length], vertsB[intersection.edgeB]);
contact.Position = intersection.position;
contact.Feature = new FeaturePair(0, 0, intersection.edgeA, intersection.edgeB);
}
/// <summary> Reconfigure this joint
///
/// </summary>
/// <param name="b1">The first body attached to this joint
/// </param>
/// <param name="b2">The second body attached to this joint
/// </param>
/// <param name="anchor1">The location of the attachment to the first body, in absolute coordinates.
/// </param>
/// <param name="anchor2">The location of the attachment to the second body, in absolute coordinates.
/// </param>
public virtual void Reconfigure(Body b1, Body b2, ROVector2f anchor1, ROVector2f anchor2)
{
body1 = b1;
body2 = b2;
Matrix2f rot1 = new Matrix2f(body1.Rotation);
Matrix2f rot1T = rot1.Transpose();
Vector2f a1 = new Vector2f(anchor1);
a1.Sub(body1.GetPosition());
localAnchor1 = MathUtil.Mul(rot1T, a1);
Matrix2f rot2 = new Matrix2f(body2.Rotation);
Matrix2f rot2T = rot2.Transpose();
Vector2f a2 = new Vector2f(anchor2);
a2.Sub(body2.GetPosition());
localAnchor2 = MathUtil.Mul(rot2T, a2);
}
/// <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> Get point on this polygon's hull that is closest to p.
///
/// TODO: make this thing return a negative value when it is contained in the polygon
///
/// </summary>
/// <param name="p">The point to search the closest point for
/// </param>
/// <returns> the nearest point on this vertex' hull
/// </returns>
public override ROVector2f GetNearestPoint(ROVector2f p)
{
// TODO: this can be done with a kind of binary search
float r = System.Single.MaxValue;
float l;
Vector2f v;
int m = - 1;
for (int i = 0; i < vertices.Length; i++)
{
v = new Vector2f(vertices[i]);
v.Sub(p);
l = v.x * v.x + v.y * v.y;
if (l < r)
{
r = l;
m = i;
}
}
// the closest point could be on one of the closest point's edges
// this happens when the angle between v[m-1]-v[m] and p-v[m] is
// smaller than 90 degrees, same for v[m+1]-v[m]
int length = vertices.Length;
Vector2f pm = new Vector2f(p);
pm.Sub(vertices[m]);
Vector2f l1 = new Vector2f(vertices[(m - 1 + length) % length]);
l1.Sub(vertices[m]);
Vector2f l2 = new Vector2f(vertices[(m + 1) % length]);
l2.Sub(vertices[m]);
Vector2f normal;
if (pm.Dot(l1) > 0)
{
normal = MathUtil.GetNormal(vertices[(m - 1 + length) % length], vertices[m]);
}
else if (pm.Dot(l2) > 0)
{
normal = MathUtil.GetNormal(vertices[m], vertices[(m + 1) % length]);
}
else
{
return vertices[m];
}
normal.Scale(- pm.Dot(normal));
normal.Add(p);
return normal;
}
/// <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> Configure the line
///
/// </summary>
/// <param name="start">The start point of the line
/// </param>
/// <param name="end">The end point of the line
/// </param>
public virtual void Reconfigure(ROVector2f start, ROVector2f end)
{
this.start = start;
this.end = end;
vec = new Vector2f(end);
vec.Sub(start);
lenSquared = vec.Length();
lenSquared *= lenSquared;
}
/// <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));
}
}
/// <summary> Given two intersecting polygons, the intersection points and a collision
/// normal, get the maximum penetration Distance along the normal.
///
/// </summary>
/// <param name="in">The ingoing intersection
/// </param>
/// <param name="out">The outgoing intersection
/// </param>
/// <param name="normal">The collision normal
/// </param>
/// <param name="vertsA">The vertices of polygon A
/// </param>
/// <param name="vertsB">The vertices of polygon B
/// </param>
/// <returns> the maximum penetration depth along the given normal
/// </returns>
public static float GetPenetrationDepth(Intersection ingoing, Intersection outgoing, Vector2f normal, Vector2f[] vertsA, Vector2f[] vertsB)
{
Vector2f sweepdir = new Vector2f(outgoing.position);
sweepdir.Sub(ingoing.position);
PenetrationSweep ps = new PenetrationSweep(normal, sweepdir, ingoing.position, outgoing.position);
//TODO: most penetrations are very simple, similar to:
// \ + |
// \ / \ |
// +-----------x---x-----+
// / \
// these should be handled separately
ContourWalker walkerA = new Silver.Weight.Raw.Collide.PenetrationSweep.ContourWalker(ps, vertsA, ingoing.edgeA, outgoing.edgeA, false);
ContourWalker walkerB = new Silver.Weight.Raw.Collide.PenetrationSweep.ContourWalker(ps, vertsB, (outgoing.edgeB + 1) % vertsB.Length, (ingoing.edgeB + 1) % vertsB.Length, true);
float penetration = 0;
float lowerBound = ingoing.position.Dot(normal);
float upperBound = lowerBound;
while (walkerA.HasNext() || walkerB.HasNext())
{
// if walker a has more and the Next vertex comes before B's
// or if walker a has more but walker b hasn't, go and take a Step
if (walkerA.HasNext() && (walkerA.NextDistance < walkerB.NextDistance || !walkerB.HasNext()))
{
walkerA.Next();
if (walkerA.Distance < ps.startDist || walkerA.Distance > ps.endDist)
continue; // we don't care for vertices outside of the intersecting borders
upperBound = walkerA.GetPenetration();
lowerBound = walkerB.GetPenetration(walkerA.Distance);
}
else
{
walkerB.Next();
if (walkerB.Distance < ps.startDist || walkerB.Distance > ps.endDist)
continue;
upperBound = walkerA.GetPenetration(walkerB.Distance);
lowerBound = walkerB.GetPenetration();
}
penetration = System.Math.Max(penetration, upperBound - lowerBound);
}
return penetration;
}
/// <summary> Create a joint holding two bodies together
///
/// </summary>
/// <param name="b1">The first body attached to the joint
/// </param>
/// <param name="b2">The second body attached to the joint
/// </param>
/// <param name="anchor1">The location of the attachment to the first body, in absolute coordinates.
/// </param>
/// <param name="anchor2">The location of the attachment to the second body, in absolute coordinates.
/// </param>
public SpringJoint(Body b1, Body b2, ROVector2f anchor1, ROVector2f anchor2)
{
id = NEXT_ID++;
stretchedSpringConst = 100;
compressedSpringConst = 100;
brokenSpringConst = 100;
Vector2f spring = new Vector2f(anchor1);
spring.Sub(anchor2);
springSize = spring.Length();
minSpringSize = 0;
maxSpringSize = 2 * springSize;
Reconfigure(b1, b2, anchor1, anchor2);
}
