public void Query(object context, cpBB bb, cpSpatialIndexQueryFunc func, object node)
{
if (root != null)
{
SubtreeQuery(root, context, bb, func, ref node);
}
}
/// Returns the fraction along the segment query the cpBB is hit. Returns INFINITY if it doesn't hit.
public static float SegmentQuery(cpBB bb, cpVect a, cpVect b)
{
float idx = 1.0f / (b.x - a.x);
float tx1 = (bb.l == a.x ? -cp.Infinity : (bb.l - a.x) * idx);
float tx2 = (bb.r == a.x ? cp.Infinity : (bb.r - a.x) * idx);
float txmin = cp.cpfmin(tx1, tx2);
float txmax = cp.cpfmax(tx1, tx2);
float idy = 1.0f / (b.y - a.y);
float ty1 = (bb.b == a.y ? -cp.Infinity : (bb.b - a.y) * idy);
float ty2 = (bb.t == a.y ? cp.Infinity : (bb.t - a.y) * idy);
float tymin = cp.cpfmin(ty1, ty2);
float tymax = cp.cpfmax(ty1, ty2);
if (tymin <= txmax && txmin <= tymax)
{
float min = cp.cpfmax(txmin, tymin);
float max = cp.cpfmin(txmax, tymax);
if (0.0 <= max && min <= 1.0)
{
return(cp.cpfmax(min, 0.0f));
}
}
return(cp.Infinity);
}
public static cpTransform Ortho(cpBB bb)
{
return(NewTranspose(
2.0f / (bb.r - bb.l), 0.0f, -(bb.r + bb.l) / (bb.r - bb.l),
0.0f, 2.0f / (bb.t - bb.b), -(bb.t + bb.b) / (bb.t - bb.b)
));
}
public BBQueryContext(cpBB bb1, cpShapeFilter filter1, cpSpaceBBQueryFunc func1)
{
// TODO: Complete member initialization
this.bb = bb1;
this.filter = filter1;
this.func = func1;
}
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 static cpBB Expand(cpBB bb, cpVect v)
{
return(new cpBB(
cp.cpfmin(bb.l, v.x),
cp.cpfmin(bb.b, v.y),
cp.cpfmax(bb.r, v.x),
cp.cpfmax(bb.t, v.y)
));
}
public static cpBB Merge(cpBB a, cpBB b)
{
return(new cpBB(
cp.cpfmin(a.l, b.l),
cp.cpfmin(a.b, b.b),
cp.cpfmax(a.r, b.r),
cp.cpfmax(a.t, b.t)
));
}
//public static cpVect CentroidForPoly(cpVect[] verts)
//{
// return CentroidForPoly(verts.Length, verts);
//}
public static float MomentForBox2(float m, cpBB box)
{
var width = box.r - box.l;
var height = box.t - box.b;
var offset = cpVect.cpvmult(new cpVect(box.l + box.r, box.b + box.t), 0.5f);
// TODO NaN when offset is 0 and m is INFINITY
return(MomentForBox(m, width, height) + m * cpVect.cpvlengthsq(offset));
}
public void PointQuery(cpVect point, float maxDistance, cpShapeFilter filter, cpSpacePointQueryFunc func, object data)
{
PointQueryContext context = new PointQueryContext(point, maxDistance, filter, func);
cpBB bb = cpBB.cpBBNewForCircle(point, cp.cpfmax(maxDistance, 0.0f));
Lock();
{
this.staticShapes.Query(context, bb, (ctx, shape, colid, o) => NearestPointQuery((PointQueryContext)ctx, shape as cpShape, colid, o), data);
this.dynamicShapes.Query(context, bb, (ctx, shape, colid, o) => NearestPointQuery((PointQueryContext)ctx, shape as cpShape, colid, o), data);
} Unlock(true);
}
public static cpPolyShape BoxShape2(cpBody body, cpBB box, float radius)
{
cpVect[] verts = new cpVect[] {
new cpVect(box.r, box.b),
new cpVect(box.r, box.t),
new cpVect(box.l, box.t),
new cpVect(box.l, box.b),
};
return(new cpPolyShape(body, 4, verts, radius));
}
/// Transform a cpBB.
public static cpBB BB(cpTransform t, cpBB bb)
{
cpVect center = cpBB.Center(bb);
float hw = (bb.r - bb.l) * 0.5f;
float hh = (bb.t - bb.b) * 0.5f;
float a = t.a * hw, b = t.c * hh, d = t.b * hw, e = t.d * hh;
float hw_max = cp.cpfmax(cp.cpfabs(a + b), cp.cpfabs(a - b));
float hh_max = cp.cpfmax(cp.cpfabs(d + e), cp.cpfabs(d - e));
return(cpBB.NewForExtents(Point(t, center), hw_max, hh_max));
}
/// Wrap a vector to a bounding box.
public static cpVect WrapVect(cpBB bb, cpVect v)
{
// wrap a vector to a bbox
float ix = cp.cpfabs(bb.r - bb.l);
float modx = (v.x - bb.l) % ix;
float x = (modx > 0) ? modx : modx + ix;
float iy = cp.cpfabs(bb.t - bb.b);
float mody = (v.y - bb.b) % iy;
float y = (mody > 0) ? mody : mody + iy;
return(new cpVect(x + bb.l, y + bb.b));
}
public void SubtreeQuery(Node subtree, object obj, cpBB bb, cpSpatialIndexQueryFunc func, ref object data)
{
//if(bbIntersectsBB(subtree.bb, bb)){
if (subtree.bb.Intersects(bb))
{
if (subtree.isLeaf)
{
func(obj, subtree.obj, 0, data);
}
else
{
SubtreeQuery(subtree.A, obj, bb, func, ref data);
SubtreeQuery(subtree.B, obj, bb, func, ref data);
}
}
}
public void BBQuery(cpBB bb, cpShapeFilter filter, cpSpaceBBQueryFunc func, object data)
{
BBQueryContext context = new BBQueryContext(bb, filter, func);
Lock();
{
this.staticShapes.Query(context, bb, (o1, o2, s, o3) =>
BBQueryFunc((BBQueryContext)o1, o2 as cpShape, s, o3)
, data);
this.dynamicShapes.Query(context, bb, (o1, o2, s, o3) =>
BBQueryFunc((BBQueryContext)o1, o2 as cpShape, s, o3)
, data);
} Unlock(true);
}
public cpBB GetBB(IObjectBox obj)
{
cpBB bb = obj.bb;
var velocityFunc = this.velocityFunc; // tree->velocityFunc;
if (velocityFunc != null)
{
float coef = 0.1f;
float x = (bb.r - bb.l) * coef;
float y = (bb.t - bb.b) * coef;
cpVect v = cpVect.cpvmult(velocityFunc(obj), 0.1f);
return(new cpBB(bb.l + cp.cpfmin(-x, v.x), bb.b + cp.cpfmin(-y, v.y), bb.r + cp.cpfmax(x, v.x), bb.t + cp.cpfmax(y, v.y)));
}
else
{
return(bb);
}
}
public bool ShapeQuery(cpShape shape, cpSpaceShapeQueryFunc func, object data)
{
cpBody body = shape.body;
cpBB bb = (body != null ? shape.Update(body.transform) : shape.bb);
ShapeQueryContext context = new ShapeQueryContext(func, data, false);
object ctx = (object)context;
Lock();
{
this.staticShapes.Query(shape, bb,
(o1, o2, s, o3) => ShapeQueryFunc(o1 as cpShape, o2 as cpShape, s, (ShapeQueryContext)o3)
, ctx);
this.dynamicShapes.Query(shape, bb,
(o1, o2, s, o3) => ShapeQueryFunc(o1 as cpShape, o2 as cpShape, s, (ShapeQueryContext)o3)
, ctx);
} Unlock(true);
return(((ShapeQueryContext)ctx).anyCollision);
}
public float Proximity(cpBB b)
{
return(Proximity(this, b));
}
/// Returns a bounding box that holds both bounding boxes.
public cpBB Merge(cpBB a)
{
return(Merge(this, a));
}
public float MergedArea(cpBB a)
{
return(MergedArea(this, a));
}
public static bool ContainsBB(cpBB bb, cpBB other)
{
return(bb.l <= other.l && bb.r >= other.r && bb.b <= other.b && bb.t >= other.t);
}
public static bool ContainsVect(cpBB bb, cpVect v)
{
return(bb.l <= v.x && bb.r >= v.x && bb.b <= v.y && bb.t >= v.y);
}
/// Returns true if @c a and @c b intersect.
public bool Intersects(cpBB a)
{
return(Intersects(this, a));
}
/// Returns true if @c other lies completely within @c bb.
public bool ContainsBB(cpBB other)
{
return(ContainsBB(this, other));
}
public static float Proximity(cpBB a, cpBB b)
{
return(cp.cpfabs(a.l + a.r - b.l - b.r) + cp.cpfabs(a.b + a.t - b.b - b.t));
}
/// Merges @c a and @c b and returns the area of the merged bounding box.
public static float MergedArea(cpBB a, cpBB b)
{
return((cp.cpfmax(a.r, b.r) - cp.cpfmin(a.l, b.l)) * (cp.cpfmax(a.t, b.t) - cp.cpfmin(a.b, b.b)));
}
/// Convenience constructor for cpBB structs.
public static cpBB cpBBNew(float l, float b, float r, float t)
{
cpBB bb = new cpBB(l, b, r, t);
return(bb);
}
//// **** All Important cpSpaceStep() Function
/// Returns true if @c a and @c b intersect.
public static bool bbIntersects(cpBB a, cpBB b)
{
return(a.l <= b.r && b.l <= a.r && a.b <= b.t && b.b <= a.t);;
}
/// Clamp a vector to a bounding box.
public static cpVect ClampVect(cpBB bb, cpVect v)
{
return(cpVect.cpv(cp.cpfclamp(v.x, bb.l, bb.r), cp.cpfclamp(v.y, bb.b, bb.t)));
}
public static bool bbIntersects2(cpBB bb, float l, float b, float r, float t)
{
return(bb.l <= r && l <= bb.r && bb.b <= t && b <= bb.t);
}
/// Return true if the bounding box intersects the line segment with ends @c a and @c b.
public static bool IntersectsSegment(cpBB bb, cpVect a, cpVect b)
{
return(SegmentQuery(bb, a, b) != cp.Infinity);
}