private PPolygonPolygonCollider.PWContactedVertex [] ClipEdge(PPolygonPolygonCollider.PWContactedVertex [] clips,
Vector2f normal, float dist) {
PPolygonPolygonCollider.PWContactedVertex [] line = new PPolygonPolygonCollider.PWContactedVertex [2];
int numClips = 0;
float dist0 = normal.Dot(clips[0].v) - dist;
float dist1 = normal.Dot(clips[1].v) - dist;
if (dist0 < 0.0F) {
line[numClips] = clips[0];
numClips++;
}
if (dist1 < 0.0F) {
line[numClips] = clips[1];
numClips++;
}
if (numClips == 0)
return null;
if (numClips == 2)
return line;
int c = 0;
if (dist0 < 0.0F && dist1 > 0.0F)
c = 1;
float d = dist0 / (dist0 - dist1);
line[1] = new PPolygonPolygonCollider.PWContactedVertex ();
line[1].v = clips[1].v.Sub(clips[0].v).Clone();
line[1].v.MulLocal(d);
line[1].v.AddLocal(clips[0].v);
line[1].data = clips[c].data;
return line;
}
public bool Intersects(Line other)
{
Vector2f lineSegmentStart = new Vector2f(other.GetX1(), other.GetY1());
Vector2f lineSegmentEnd = new Vector2f(other.GetX2(), other.GetY2());
Vector2f circleCenter = new Vector2f(GetCenterX(), GetCenterY());
Vector2f closest;
Vector2f segv = lineSegmentEnd.Cpy().Sub(lineSegmentStart);
Vector2f ptv = circleCenter.Cpy().Sub(lineSegmentStart);
float segvLength = segv.Len();
float projvl = ptv.Dot(segv) / segvLength;
if (projvl < 0)
{
closest = lineSegmentStart;
}
else if (projvl > segvLength)
{
closest = lineSegmentEnd;
}
else
{
Vector2f projv = segv.Cpy().Scale(projvl / segvLength);
closest = lineSegmentStart.Cpy().Add(projv);
}
bool intersects = circleCenter.Cpy().Sub(closest).LengthSquared() <= GetRadius()
* GetRadius();
return(intersects);
}
public void GetClosestPoint(Vector2f point, Vector2f result)
{
loc.Set(point);
loc.Sub(start);
float projDistance = vec.Dot(loc);
projDistance /= vec.LengthSquared();
if (projDistance < 0)
{
result.Set(start);
return;
}
if (projDistance > 1)
{
result.Set(end);
return;
}
result.x = start.GetX() + projDistance * vec.GetX();
result.y = start.GetY() + projDistance * vec.GetY();
}
public static float CalcEffectiveMass(PBody b1, PBody b2, Vector2f r1,
Vector2f r2, Vector2f normal) {
float rn1 = normal.Dot(r1);
float rn2 = normal.Dot(r2);
return 1.0F / (b1.invM + b2.invM + b1.invI
* ((r1.x * r1.x + r1.y * r1.y) - rn1 * rn1) + b2.invI
* ((r2.x * r2.x + r2.y * r2.y) - rn2 * rn2));
}
public virtual int Collide(PShape s1, PShape s2, PContact[] cs) {
if (s1._type != PShapeType.CONVEX_SHAPE
&& s1._type != PShapeType.BOX_SHAPE
|| s2._type != PShapeType.CONVEX_SHAPE
&& s2._type != PShapeType.BOX_SHAPE) {
return 0;
}
PConvexPolygonShape p1 = (PConvexPolygonShape) s1;
PConvexPolygonShape p2 = (PConvexPolygonShape) s2;
PPolygonPolygonCollider.PWDistanceData dis1 = GetDistance(p1, p2);
if (dis1.dist > 0.0F)
return 0;
PPolygonPolygonCollider.PWDistanceData dis2 = GetDistance(p2, p1);
if (dis2.dist > 0.0F)
return 0;
float error = 0.008F;
int edgeA;
PConvexPolygonShape pa;
PConvexPolygonShape pb;
bool flip;
if (dis1.dist > dis2.dist + error) {
pa = p1;
pb = p2;
edgeA = dis1.edge;
flip = false;
} else {
pa = p2;
pb = p1;
edgeA = dis2.edge;
flip = true;
}
Vector2f normal = pa.nors[edgeA];
Vector2f tangent = new Vector2f(-normal.y, normal.x);
Vector2f[] paVers = pa.vers;
PPolygonPolygonCollider.PWContactedVertex [] cv = GetEdgeOfPotentialCollision(pa, pb, edgeA,
flip);
cv = ClipEdge(cv, tangent.Negate(), -tangent.Dot(paVers[edgeA]));
if (cv == null)
return 0;
cv = ClipEdge(cv, tangent,
tangent.Dot(paVers[(edgeA + 1) % pa.numVertices]));
if (cv == null)
return 0;
Vector2f contactNormal = (flip) ? normal : normal.Negate();
int numContacts = 0;
for (int i = 0; i < 2; i++) {
float dist = normal.Dot(cv[i].v) - normal.Dot(paVers[edgeA]);
if (dist < 0.0F) {
PContact c = new PContact();
c.normal.Set(contactNormal.x, contactNormal.y);
c.pos.Set(cv[i].v.x, cv[i].v.y);
c.overlap = dist;
c.data = cv[i].data;
c.data.flip = flip;
cs[numContacts] = c;
numContacts++;
}
}
return numContacts;
}
