cpPolyShapeNearestPointQuery(cpPolyShape poly, cpVect p, cpNearestPointQueryInfo* info)
{
int count = poly.numVerts;
cpSplittingPlane planes = poly.tPlanes;
cpVect[] verts = poly.tVerts;
cpVect v0 = verts[count - 1];
double minDist = double.PositiveInfinity;
cpVect closestPoint = cpvzero;
bool outside = false;
for (int i = 0; i < count; i++)
{
if (cpSplittingPlaneCompare(planes[i], p) > 0.0f) outside = true;
cpVect v1 = verts[i];
cpVect closest = cpClosetPointOnSegment(p, v0, v1);
double dist = cpVect.Distance(p, closest);
if (dist < minDist)
{
minDist = dist;
closestPoint = closest;
}
v0 = v1;
}
info.shape = (cpShape)poly;
info.p = closestPoint; // TODO div/0
info.d = (outside ? minDist : -minDist);
}
cpSegmentShapeNearestPointQuery(cpSegmentShape seg, cpVect p, cpNearestPointQueryInfo info)
{
cpVect closest = cpClosetPointOnSegment(p, seg.ta, seg.tb);
cpVect delta = cpVect.Sub(p, closest);
double d = cpvlength(delta);
double r = seg.r;
info.shape = (cpShape )seg;
info.p = (d ? cpVect.Add(closest, cpVect.Multiply(delta, r/d)) : closest);
info.d = d - r;
}
cpShapeNearestPointQuery(cpShape shape, cpVect p, ref cpNearestPointQueryInfo info)
{
cpNearestPointQueryInfo blank = new cpNearestPointQueryInfo() {null, cpvzero, double.PositiveInfinity};
info = blank;
shape.klass.nearestPointQuery(shape, p, info);
return info.d;
}
cpCicleShapeNearestPointQuery(cpCircleShape circle, cpVect p, cpNearestPointQueryInfo info)
{
cpVect delta = cpVect.Sub(p, circle.tc);
double d = cpvlength(delta);
double r = circle.r;
info.shape = (cpShape )circle;
info.p = cpVect.Add(circle.tc, cpVect.Multiply(delta, r/d)); // TODO div/0
info.d = d - r;
}
cpShapePointQuery(cpShape shape, cpVect p){
cpNearestPointQueryInfo info = new cpNearestPointQueryInfo()
{
null,
cpvzero,
double.PositiveInfinity
};
cpShapeNearestPointQuery(shape, p, ref info);
return (info.d < 0.0f);
}