public override void Update(float dt)
{
base.Update(dt);
if (CCMouse.Instance.rightclick)
{
QUERY_START = CCMouse.Instance.Position;
}
start = QUERY_START;
end = CCMouse.Instance.Position;
float radius = 10;
segInfo = null;
m_debugDraw.DrawSegment(start, end, 1, cpColor.Green);
shapeInfo = this.space.SegmentQueryFirst(start, end, radius, cpShape.FILTER_ALL, ref segInfo);
if (shapeInfo != null)
{
font.Text = string.Format("Segment Query: Dist({0}) Normal({1},{2})", segInfo.alpha * cpVect.cpvdist(start, end), segInfo.normal.x, segInfo.normal.y);
}
else
{
font.Text = string.Format("Segment Query (None)");
}
nearestInfo = null;
space.PointQueryNearest(CCMouse.Instance.Position, 100, cpShape.FILTER_ALL, ref nearestInfo);
if (nearestInfo != null)
{
// Draw a grey line to the closest shape.
m_debugDraw.Draw(CCMouse.Instance.Position, new cpColor(127, 127, 127, 255));
m_debugDraw.DrawSegment(CCMouse.Instance.Position, nearestInfo.point, 1, new cpColor(127, 127, 127, 255));
// Draw a red bounding box around the shape under the mouse.
if (nearestInfo.distance < 0)
{
m_debugDraw.Draw(nearestInfo.shape.bb, cpColor.Red);
}
}
space.Step(dt);
}
cpPolyShapeSegmentQuery(cpPolyShape poly, cpVect a, cpVect b, cpSegmentQueryInfo* info)
{
cpSplittingPlane axes = poly.tPlanes;
cpVect[] verts = poly.tVerts;
int numVerts = poly.numVerts;
for (int i = 0; i < numVerts; i++)
{
cpVect n = axes[i].n;
double an = cpVect.Dot(a, n);
if (axes[i].d > an) continue;
double bn = cpVect.Dot(b, n);
double t = (axes[i].d - an) / (bn - an);
if (t < 0.0f || 1.0f < t) continue;
cpVect point = cpvlerp(a, b, t);
double dt = -cpVect.CrossProduct(n, point);
double dtMin = -cpVect.CrossProduct(n, verts[i]);
double dtMax = -cpVect.CrossProduct(n, verts[(i + 1) % numVerts]);
if (dtMin <= dt && dt <= dtMax)
{
info.shape = (cpShape)poly;
info.t = t;
info.n = n;
}
}
}
cpSegmentShapeSegmentQuery(cpSegmentShape seg, cpVect a, cpVect b, cpSegmentQueryInfo info)
{
cpVect n = seg.tn;
double d = cpVect.Dot(cpVect.Sub(seg.ta, a), n);
double r = seg.r;
cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n);
cpVect seg_offset = cpVect.Sub(cpVect.Multiply(flipped_n, r), a);
// Make the endpoints relative to 'a' and move them by the thickness of the segment.
cpVect seg_a = cpVect.Add(seg.ta, seg_offset);
cpVect seg_b = cpVect.Add(seg.tb, seg_offset);
cpVect delta = cpVect.Sub(b, a);
if(cpVect.CrossProduct(delta, seg_a)*cpVect.CrossProduct(delta, seg_b) <= 0.0f){
double d_offset = d + (d > 0.0f ? -r : r);
double ad = -d_offset;
double bd = cpVect.Dot(delta, n) - d_offset;
if(ad*bd < 0.0f){
info.shape = (cpShape )seg;
info.t = ad/(ad - bd);
info.n = flipped_n;
}
} else if(r != 0.0f){
cpSegmentQueryInfo info1 = {null, 1.0f, cpvzero};
cpSegmentQueryInfo info2 = {null, 1.0f, cpvzero};
circleSegmentQuery((cpShape )seg, seg.ta, seg.r, a, b, ref info1);
circleSegmentQuery((cpShape )seg, seg.tb, seg.r, a, b, ref info2);
if(info1.t < info2.t){
(*info) = info1;
} else {
(*info) = info2;
}
}
}
cpCircleShapeSegmentQuery(cpCircleShape circle, cpVect a, cpVect b, cpSegmentQueryInfo info)
{
circleSegmentQuery((cpShape )circle, circle.tc, circle.r, a, b, info);
}
circleSegmentQuery(cpShape shape, cpVect center, double r, cpVect a, cpVect b, cpSegmentQueryInfo info)
{
cpVect da = cpVect.Sub(a, center);
cpVect db = cpVect.Sub(b, center);
double qa = cpVect.Dot(da, da) - 2.0f*cpVect.Dot(da, db) + cpVect.Dot(db, db);
double qb = -2.0f*cpVect.Dot(da, da) + 2.0f*cpVect.Dot(da, db);
double qc = cpVect.Dot(da, da) - r*r;
double det = qb*qb - 4.0f*qa*qc;
if(det >= 0.0f){
double t = (-qb - System.Math.Sqrt(det))/(2.0f*qa);
if(0.0f<= t && t <= 1.0f){
info.shape = shape;
info.t = t;
info.n = cpvnormalize(cpvlerp(da, db, t));
}
}
}
cpShapeSegmentQuery(cpShape shape, cpVect a, cpVect b, ref cpSegmentQueryInfo info){
cpSegmentQueryInfo blank = new cpSegmentQueryInfo() {null, 0.0f, cpvzero};
info = blank;
cpNearestPointQueryInfo nearest;
shape.klass.nearestPointQuery(shape, a, &nearest);
if(nearest.d <= 0.0){
info.shape = shape;
info.t = 0.0;
info.n = cpvnormalize(cpVect.Sub(a, nearest.p));
} else {
shape.klass.segmentQuery(shape, a, b, info);
}
return (info.shape != null);
}
