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