public cpShape PointQueryNearest(cpVect point, float maxDistance, cpShapeFilter filter, ref cpPointQueryInfo output)
{
cpPointQueryInfo info = new cpPointQueryInfo(null, cpVect.Zero, maxDistance, cpVect.Zero);
if (output == null)
{
output = info;
}
PointQueryContext context = new PointQueryContext(
point, maxDistance,
filter,
null
);
cpBB bb = cpBB.cpBBNewForCircle(point, cp.cpfmax(maxDistance, 0.0f));
object outp = (object)output;
this.dynamicShapes.Query(context, bb,
(o1, o2, s, o3) => NearestPointQueryNearest(o1, (cpShape)o2, s, ref outp)
, null);
this.staticShapes.Query(context, bb,
(o1, o2, s, o3) => NearestPointQueryNearest(o1, (cpShape)o2, s, ref outp)
, null);
output = (cpPointQueryInfo)outp;
return(output.shape);
}
public void Set(cpPointQueryInfo newPointInfo)
{
/// The nearest shape, NULL if no shape was within range.
shape = newPointInfo.shape;
point = newPointInfo.point;
distance = newPointInfo.distance;
gradient = newPointInfo.gradient;
// g = newPointInfo.g;
}
public float PointQuery(cpVect p, ref cpPointQueryInfo info)
{
if (info == null)
{
info = new cpPointQueryInfo(null, cpVect.Zero, cp.Infinity, cpVect.Zero);
}
pointQuery(p, ref info);
return(info.distance);
}
protected override void pointQuery(cpVect p, ref cpPointQueryInfo info)
{
// base.pointQuery(p, ref info);
cpVect delta = cpVect.cpvsub(p, this.tc);
float d = cpVect.cpvlength(delta);
float r = this.r;
info.shape = this;
info.point = cpVect.cpvadd(this.tc, cpVect.cpvmult(delta, r / d)); // TODO: div/0
info.distance = d - r;
// Use up for the gradient if the distance is very small.
info.gradient = (d > cp.MAGIC_EPSILON ? cpVect.cpvmult(delta, 1.0f / d) : new cpVect(0.0f, 1.0f));
}
protected override void pointQuery(cpVect p, ref cpPointQueryInfo info)
{
cpVect closest = cp.closestPointOnSegment(p, this.ta, this.tb);
cpVect delta = cpVect.cpvsub(p, closest);
float d = cpVect.cpvlength(delta);
float r = this.r;
cpVect g = cpVect.cpvmult(delta, 1.0f / d);
info.shape = (cpShape)this;
info.point = (d != 0 ? cpVect.cpvadd(closest, cpVect.cpvmult(g, r)) : closest);
info.distance = d - r;
// Use the segment's normal if the distance is very small.
info.gradient = (d > cp.MAGIC_EPSILON ? g : this.n);
}
public ulong NearestPointQuery(PointQueryContext context, cpShape shape, ulong id, object data)
{
if (
!cpShapeFilter.Reject(shape.filter, context.filter)
)
{
cpPointQueryInfo info = null;
shape.PointQuery(context.point, ref info);
if (info.shape != null && info.distance < context.maxDistance)
{
context.func(shape, info.point, info.distance, info.gradient, data);
}
}
return(id);
}
protected override void pointQuery(cpVect p, ref cpPointQueryInfo info)
{
int count = Count;
cpSplittingPlane[] planes = this.planes;
float r = this.r;
cpVect v0 = planes[count - 1].v0;
float minDist = cp.Infinity;
cpVect closestPoint = cpVect.Zero;
cpVect closestNormal = cpVect.Zero;
bool outside = false;
for (int i = 0; i < count; i++)
{
cpVect v1 = planes[i].v0;
if (cpVect.cpvdot(planes[i].n, cpVect.cpvsub(p, v1)) > 0.0f)
{
outside = true;
}
cpVect closest = cp.closestPointOnSegment(p, v0, v1);
{
float dista = cpVect.cpvdist(p, closest);
if (dista < minDist)
{
minDist = dista;
closestPoint = closest;
closestNormal = planes[i].n;
}
}
v0 = v1;
}
float dist = (outside ? minDist : -minDist);
cpVect g = cpVect.cpvmult(cpVect.cpvsub(p, closestPoint), 1.0f / dist);
info.shape = this;
info.point = cpVect.cpvadd(closestPoint, cpVect.cpvmult(g, r));
info.distance = dist - r;
// Use the normal of the closest segment if the distance is small.
info.gradient = (minDist > cp.MAGIC_EPSILON ? g : closestNormal);
}
public ulong NearestPointQueryNearest(object ctx, cpShape shape, ulong id, ref object outp)
{
PointQueryContext context = (PointQueryContext)ctx;
cpPointQueryInfo output = (cpPointQueryInfo)outp;
if (
!cpShapeFilter.Reject(shape.filter, context.filter) && !shape.sensor
)
{
cpPointQueryInfo info = null;
shape.PointQuery(context.point, ref info);
if (info.distance < output.distance)
{
outp = (object)info;
}
}
return(id);
}
public bool SegmentQuery(cpVect a, cpVect b, float radius, ref cpSegmentQueryInfo info)
{
if (info == null)
{
info = new cpSegmentQueryInfo(null, b, cpVect.Zero, 1.0f);
}
cpPointQueryInfo nearest = null;
PointQuery(a, ref nearest);
if (nearest.distance <= radius)
{
info.shape = this;
info.alpha = 0.0f;
info.normal = cpVect.cpvnormalize(cpVect.cpvsub(a, nearest.point));
}
else
{
segmentQuery(a, b, radius, ref info);
}
return(info.shape != null);
}
protected virtual void pointQuery(cpVect p, ref cpPointQueryInfo info)
{
throw new NotImplementedException();
}
