/// <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);
}
/// <summary> Create a new joint
///
/// </summary>
/// <param name="body1">The first body jointed
/// </param>
/// <param name="body2">The second body jointed
/// </param>
/// <param name="anchor">The anchor point for the underlying basic joint
/// </param>
/// <param name="Distance">The Distance the bodies must be apart before force is applied
/// </param>
public ConstrainingJoint(Body body1, Body body2, Vector2f anchor, float distance)
{
this.distance = distance;
this.body1 = body1;
this.body2 = body2;
realJoint = new BasicJoint(body1, body2, anchor);
}
/// <summary> Construct an intersection with all its attributes set.
///
/// </summary>
/// <param name="edgeA">The edge of polygon A that intersects
/// </param>
/// <param name="edgeB">The edge of polygon B that intersects
/// </param>
/// <param name="position">The position of the intersection in world (absolute) coordinates
/// </param>
/// <param name="isIngoing">True iff this is an intersection where polygon A enters B
/// </param>
public Intersection(int edgeA, int edgeB, Vector2f position, bool isIngoing)
: base()
{
this.edgeA = edgeA;
this.edgeB = edgeB;
this.position = position;
this.isIngoing = isIngoing;
}
/// <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>Construct the polygon with a list of vertices
/// sorted in counterclockwise order.
/// Note that all the vector values will be copied.
///
/// Throws an exception when too few vertices (<3) are supplied.
/// TODO: throw an exception when the vertices arent counterclockwise?
///
/// </summary>
/// <param name="vertices">Vertices sorted in counterclockwise order
/// </param>
public Polygon(ROVector2f[] vertices)
{
if (vertices.Length < 3)
throw new System.ArgumentException("A polygon can not have fewer than 3 edges!");
this.vertices = new Vector2f[vertices.Length];
for (int i = 0; i < vertices.Length; i++)
{
this.vertices[i] = new Vector2f(vertices[i]);
}
float r = ComputeBoundingCircleRadius();
this.bounds = new AABox(r * 2, r * 2);
this.area = ComputeArea();
this.centroid = ComputeCentroid();
}
/// <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> 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));
}
/// <summary> Add a force to this body
///
/// </summary>
/// <param name="f">The force to be applied
/// </param>
public virtual void AddForce(Vector2f f)
{
force.Add(f);
}
/// <summary> Notification that we've started an Update frame/iteration</summary>
internal virtual void StartFrame()
{
if (!CanRest)
{
return ;
}
oldPosition = new Vector2f(GetPosition());
hitByAnother = false;
hitCount = 0;
touching.Clear();
}
/// <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> 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;
}
/// <summary> Return a translated and rotated line.
///
/// </summary>
/// <param name="displacement">The displacement of the line
/// </param>
/// <param name="rotation">The rotation of the line in radians
/// </param>
/// <returns> The two endpoints of this line
/// </returns>
public virtual Vector2f[] getVertices(ROVector2f displacement, float rotation)
{
float cos = (float) System.Math.Cos(rotation);
float sin = (float) System.Math.Sin(rotation);
Vector2f[] endPoints = new Vector2f[2];
endPoints[0] = new Vector2f(X1 * cos - Y1 * sin, Y1 * cos + X1 * sin);
endPoints[0].Add(displacement);
endPoints[1] = new Vector2f(X2 * cos - Y2 * sin, Y2 * cos + X2 * sin);
endPoints[1].Add(displacement);
return endPoints;
}
/// <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="anchor">The anchor point which movement/rotation will occur
/// arround.
/// </param>
public BasicJoint(Body b1, Body b2, Vector2f anchor)
{
id = NEXT_ID++;
accumulatedImpulse.Reconfigure(0.0f, 0.0f);
relaxation = 1.0f;
Reconfigure(b1, b2, anchor);
}
/// <summary> Adjust the velocity of this body
///
/// </summary>
/// <param name="delta">The amount to change the velocity by
/// </param>
public virtual void AdjustVelocity(Vector2f delta)
{
if (!IsMoveable())
{
return ;
}
lastVelocity.Reconfigure(velocity);
velocity.Add(delta);
}
/// <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> 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 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> 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> 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> Adjust the bias velocity of this body
///
/// </summary>
/// <param name="delta">The amount to change the velocity by
/// </param>
public virtual void AdjustBiasedVelocity(Vector2f delta)
{
if (!IsMoveable())
{
return ;
}
biasedVelocity.Add(delta);
}
/// <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 boxBody, Body circleBody)
{
float x1 = boxBody.GetPosition().X;
float y1 = boxBody.GetPosition().Y;
float x2 = circleBody.GetPosition().X;
float y2 = circleBody.GetPosition().Y;
bool touches = boxBody.Shape.Bounds.Touches(x1, y1, circleBody.Shape.Bounds, x2, y2);
if (!touches)
{
return 0;
}
Box box = (Box) boxBody.Shape;
Circle circle = (Circle) circleBody.Shape;
Vector2f[] pts = box.GetPoints(boxBody.GetPosition(), boxBody.Rotation);
Line[] lines = new Line[4];
lines[0] = new Line(pts[0], pts[1]);
lines[1] = new Line(pts[1], pts[2]);
lines[2] = new Line(pts[2], pts[3]);
lines[3] = new Line(pts[3], pts[0]);
float r2 = circle.Radius * circle.Radius;
int closest = - 1;
float closestDistance = System.Single.MaxValue;
for (int i = 0; i < 4; i++)
{
float dis = lines[i].distanceSquared(circleBody.GetPosition());
if (dis < r2)
{
if (closestDistance > dis)
{
closestDistance = dis;
closest = i;
}
}
}
if (closest > - 1)
{
float dis = (float) System.Math.Sqrt(closestDistance);
contacts[0].Separation = dis - circle.Radius;
// this should really be where the edge and the line
// between the two elements Cross?
Vector2f contactPoint = new Vector2f();
lines[closest].getClosestPoint(circleBody.GetPosition(), contactPoint);
Vector2f normal = MathUtil.Sub(circleBody.GetPosition(), contactPoint);
normal.Normalise();
contacts[0].Normal = normal;
contacts[0].Position = contactPoint;
contacts[0].Feature = new FeaturePair();
return 1;
}
return 0;
}
/// <summary> Adjust the position of this body
/// The previous position will be set to the position before
/// this function was called.
///
/// </summary>
/// <param name="delta">The amount to change the position by
/// </param>
public virtual void AdjustPosition(Vector2f delta)
{
lastPosition.Reconfigure(position);
position.Add(delta);
}
/// <summary> Get the closest point on the line to a given point
///
/// </summary>
/// <param name="point">The point which we want to project
/// </param>
/// <param name="result">The point on the line closest to the given point
/// </param>
public virtual void getClosestPoint(ROVector2f point, Vector2f result)
{
loc.Reconfigure(point);
loc.Sub(start);
v.Reconfigure(vec);
v2.Reconfigure(vec);
v2.Scale(- 1);
v.Normalise();
loc.ProjectOntoUnit(v, proj);
if (proj.LengthSquared() > vec.LengthSquared())
{
result.Reconfigure(end);
return ;
}
proj.Add(start);
other.Reconfigure(proj);
other.Sub(end);
if (other.LengthSquared() > vec.LengthSquared())
{
result.Reconfigure(start);
return ;
}
result.Reconfigure(proj);
return ;
}
/// <summary> Notification that we've ended an Update frame/iteration</summary>
public virtual void EndFrame()
{
if (!CanRest)
{
return ;
}
if ((hitCount == 0) || (touchingCount != touching.Size()))
{
isResting = false;
SetMass(originalMass);
touchingStatic = false;
touchingCount = touching.Size();
}
else
{
newPosition = new Vector2f(GetPosition());
if (!hitByAnother)
{
if (true && (newPosition.DistanceSquared(oldPosition) <= positionTolerance) && (velocity.LengthSquared() <= 0.001f) && (biasedVelocity.LengthSquared() <= 0.001f) && (System.Math.Abs(angularVelocity) <= rotationTolerance))
{
if (!touchingStatic)
{
touchingStatic = IsTouchingStatic(new List<Body>());
}
if (touchingStatic)
{
isResting = true;
SetMass(INFINITE_MASS);
velocity.Reconfigure(0.0f, 0.0f);
biasedVelocity.Reconfigure(0, 0);
angularVelocity = 0.0f;
biasedAngularVelocity = 0;
force.Reconfigure(0.0f, 0.0f);
torque = 0.0f;
}
}
}
else
{
isResting = false;
SetMass(originalMass);
}
if ((newPosition.DistanceSquared(oldPosition) > positionTolerance) && (System.Math.Abs(angularVelocity) > rotationTolerance))
{
touchingStatic = false;
}
}
}
/// <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> Constructs a Penetration Sweep object, with all its attributes set.
/// This constructor is public only for testing purposes. The static method
/// {@link PenetrationSweep#GetPenetrationDepth(Intersection, Intersection, Vector2f, Vector2f[], Vector2f[])}
/// should be called to get the penetration depth.
///
/// </summary>
/// <param name="normal">The collision normal
/// </param>
/// <param name="sweepDir">The sweep direction
/// </param>
/// <param name="intersectionStart">The start bound of the intersection Area
/// </param>
/// <param name="intersectionEnd">The end bound of the intersection Area.
/// </param>
public PenetrationSweep(Vector2f normal, Vector2f sweepDir, Vector2f intersectionStart, Vector2f intersectionEnd)
: base()
{
this.normal = normal;
this.sweepDir = sweepDir;
this.startDist = intersectionStart.Dot(sweepDir);
this.endDist = intersectionEnd.Dot(sweepDir);
}
/// <summary> Test whether or not the point p is in this polygon in O(n),
/// where n is the number of vertices in this polygon.
///
/// </summary>
/// <param name="p">The point to be tested for inclusion in this polygon
/// </param>
/// <returns> true iff the p is in this polygon (not on a border)
/// </returns>
public virtual bool Contains(Vector2f p)
{
// p is in the polygon if it is left of all the edges
int l = vertices.Length;
for (int i = 0; i < vertices.Length; i++)
{
Vector2f x = vertices[i];
Vector2f y = vertices[(i + 1) % l];
Vector2f z = p;
// does the 3d Cross product point up or down?
if ((z.x - x.x) * (y.y - x.y) - (y.x - x.x) * (z.y - x.y) >= 0)
return false;
}
return true;
}
/// <summary> Construct a contourwalker.
///
/// </summary>
/// <param name="verts">The vertices of the polygon which's contour is being followed
/// </param>
/// <param name="firstVert">The index of the vertex where the contour's subsection which we
/// walk on starts
/// </param>
/// <param name="lastVert">The index of the vertex where the contour's subsection which we
/// walk on ends
/// </param>
/// <param name="isBackwards">True iff we're walking backwards over the contour
/// </param>
public ContourWalker(PenetrationSweep sweep, Vector2f[] verts, int firstVert, int lastVert, bool isBackwards)
{
InitBlock(sweep);
if (firstVert < 0 || lastVert < 0)
throw new System.ArgumentException("Vertex numbers cannot be negative.");
if (firstVert > verts.Length || lastVert > verts.Length)
throw new System.ArgumentException("The given vertex array doesn't include the first or the last vertex.");
this.isBackwards = isBackwards;
this.verts = verts;
this.firstVert = firstVert;
this.lastVert = lastVert;
this.currentVert = isBackwards?lastVert:firstVert;
this.distance = verts[currentVert].Dot(Parent.sweepDir);
this.penetration = verts[currentVert].Dot(Parent.normal);
CalculateNextValues();
}
/// <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;
}
