/// <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 virtual int Collide(Contact[] contacts, Body bodyA, Body bodyB)
{
int count = collider.Collide(contacts, bodyB, bodyA);
// Reverse the collision results by inverting normals
for (int i = 0; i < count; i++)
{
Vector2f vec = MathUtil.Scale(contacts[i].Normal, - 1);
contacts[i].Normal = vec;
}
return count;
}
/// <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;
}
/// <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);
}
/// <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 circleBody, Body boxBody)
{
int count = base.Collide(contacts, boxBody, circleBody);
// Reverse the collision results by inverting normals
// and projecting the results onto the circle
for (int i = 0; i < count; i++)
{
Vector2f vec = MathUtil.Scale(contacts[i].Normal, - 1);
contacts[i].Normal = vec;
Vector2f pt = MathUtil.Sub(contacts[i].Position, circleBody.GetPosition());
pt.Normalise();
pt.Scale(((Circle) circleBody.Shape).Radius);
pt.Add(circleBody.GetPosition());
contacts[i].Position = pt;
}
return count;
}
/// <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)
{
float x1 = bodyA.GetPosition().X;
float y1 = bodyA.GetPosition().Y;
float x2 = bodyB.GetPosition().X;
float y2 = bodyB.GetPosition().Y;
bool touches = bodyA.Shape.Bounds.Touches(x1, y1, bodyB.Shape.Bounds, x2, y2);
if (!touches)
{
return 0;
}
Circle circleA = (Circle) bodyA.Shape;
Circle circleB = (Circle) bodyB.Shape;
touches = circleA.Touches(x1, y1, circleB, x2, y2);
if (!touches)
{
return 0;
}
Vector2f normal = MathUtil.Sub(bodyB.GetPosition(), bodyA.GetPosition());
float sep = (circleA.Radius + circleB.Radius) - normal.Length();
normal.Normalise();
Vector2f pt = MathUtil.Scale(normal, circleA.Radius);
pt.Add(bodyA.GetPosition());
contacts[0].Separation = - sep;
contacts[0].Position = pt;
contacts[0].Normal = normal;
FeaturePair fp = new FeaturePair();
contacts[0].Feature = fp;
return 1;
}
/// <summary> Create a collider for two bodies. The decision depends on
/// the body's shapes.
///
/// </summary>
/// <param name="bodyA">First body in the collision test
/// </param>
/// <param name="bodyB">Second body in the collision test
/// </param>
/// <returns> A collider that can test wether the two bodies actually Collide
/// </returns>
/// <throws> ColliderUnavailableException </throws>
/// <summary> This exception will be thrown if no suitable collider can be found.
/// </summary>
public virtual Collider CreateCollider(Body bodyA, Body bodyB)
{
Shape shapeA = bodyA.Shape;
Shape shapeB = bodyB.Shape;
if (shapeA is Circle)
{
return CreateColliderFor((Circle) shapeA, shapeB);
}
else if (shapeA is Box)
{
return CreateColliderFor((Box) shapeA, shapeB);
}
else if (shapeA is Line)
{
return CreateColliderFor((Line) shapeA, shapeB);
}
else if (shapeA is Polygon)
{
return CreateColliderFor((Polygon) shapeA, shapeB);
}
throw new ColliderUnavailableException(shapeA, shapeB);
}
/// <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)
{
// TODO: function disabled until we can remember on what side of A,
// B used to be, which is crucial to determine a proper collision normal
return 0;
// Line lineA = (Line) bodyA.getShape();
// Line lineB = (Line) bodyB.getShape();
//
// Vector2f[] vertsA = lineA.GetVertices(bodyA.GetPosition(), bodyA.getRotation());
// Vector2f[] vertsB = lineB.GetVertices(bodyB.GetPosition(), bodyB.getRotation());
//
// Vector2f startA = vertsA[0];
// Vector2f endA = vertsA[1];
// Vector2f startB = vertsB[0];
// Vector2f endB = vertsB[1];
//
// //TODO: reuse mathutil.Intersect?
// float d = (endB.y - startB.y) * (endA.x - startA.x) - (endB.x - startB.x) * (endA.y - startA.y);
//
// if ( d == 0 ) // parallel lines
// return 0;
//
// float uA = (endB.x - startB.x) * (startA.y - startB.y) - (endB.y - startB.y) * (startA.x - startB.x);
// uA /= d;
// float uB = (endA.x - startA.x) * (startA.y - startB.y) - (endA.y - startA.y) * (startA.x - startB.x);
// uB /= d;
//
// if ( uA < 0 || uA > 1 || uB < 0 || uB > 1 )
// return 0; // intersection point isn't between the start and endpoints
//
// // there must be a collision, let's determine our contact information
// // we're searching for a contact with the smallest penetration depth
// Vector2f[][] closestPoints = {
// {startB, GetClosestPoint(startA, endA, startB)},
// {endB, GetClosestPoint(startA, endA, endB)},
// {startA, GetClosestPoint(startB, endB, startA)},
// {endA, GetClosestPoint(startB, endB, endA)}
// };
//
// float distSquared = Float.MAX_VALUE;
// Vector2f position = null;
// Vector2f normal = new Vector2f();
//
// for ( int i = 0; i < 4; i++ ) {
// Vector2f l;
// if ( i < 2 ) {
// l = closestPoints[i][1];
// l.Sub(closestPoints[i][0]);
// } else {
// l = closestPoints[i][0];
// l.Sub(closestPoints[i][1]);
// }
//
// float newDistSquared = l.LengthSquared();
// if ( newDistSquared < distSquared ) {
// distSquared = newDistSquared;
// position = closestPoints[i][0];
// normal.set(l);
// }
// }
//
// normal.Normalise();
// contacts[0].setNormal(normal);
// contacts[0].SetPosition(position);
// if ( Math.sqrt(distSquared) > 10f )
// System.out.println(Math.sqrt(distSquared));
// contacts[0].setSeparation((float) -Math.sqrt(distSquared));
//
// return 1;
}
/// <summary> Add a pody to the space
///
/// </summary>
/// <param name="body">The body to Add to the space
/// </param>
public virtual void AddBody(Body body)
{
Add(body);
}
/// <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;
}
/// <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;
}
/// <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);
}
