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