public cpShape SegmentQueryFirst(cpVect start, cpVect end, float radius, cpShapeFilter filter, ref cpSegmentQueryInfo output)
{
cpSegmentQueryInfo info = new cpSegmentQueryInfo(null, end, cpVect.Zero, 1.0f);
if (output == null)
{
output = info;
}
SegmentQueryContext context = new SegmentQueryContext(
start, end,
radius,
filter,
null);
this.staticShapes.SegmentQuery(context, start, end, 1.0f,
(o1, o2, o3) => SegmentQueryFirstFunc((SegmentQueryContext)o1, o2 as cpShape, (cpSegmentQueryInfo)o3)
, output);
// this.dynamicShapes.SegmentQuery(context, start, end, output.alpha , SegmentQueryFirst, ref output);
this.dynamicShapes.SegmentQuery(context, start, end, output.alpha,
(o1, o2, o3) => SegmentQueryFirstFunc((SegmentQueryContext)o1, o2 as cpShape, (cpSegmentQueryInfo)o3)
, output);
return(output.shape);
}
public void Set(cpSegmentQueryInfo info1)
{
/// The shape that was hit, NULL if no collision occured.
this.shape = info1.shape;
/// The normalized distance along the query segment in the range [0, 1].
this.alpha = info1.alpha;
/// The normal of the surface hit.
this.normal = info1.normal;
this.point = info1.point;
}
protected override void segmentQuery(cpVect a, cpVect b, float r2, ref cpSegmentQueryInfo info)
{
cpSplittingPlane[] planes = this.planes;
int count = this.Count;
float r = this.r;
float rsum = r + r2;
for (int i = 0; i < count; i++)
{
cpVect n = planes[i].n;
float an = cpVect.cpvdot(a, n);
float d = an - cpVect.cpvdot(planes[i].v0, n) - rsum;
if (d < 0.0f)
{
continue;
}
float bn = cpVect.cpvdot(b, n);
float t = d / (an - bn);
if (t < 0.0f || 1.0f < t)
{
continue;
}
cpVect point = cpVect.cpvlerp(a, b, t);
float dt = cpVect.cpvcross(n, point);
float dtMin = cpVect.cpvcross(n, planes[(i - 1 + count) % count].v0);
float dtMax = cpVect.cpvcross(n, planes[i].v0);
if (dtMin <= dt && dt <= dtMax)
{
info.shape = this;
info.point = cpVect.cpvsub(cpVect.cpvlerp(a, b, t), cpVect.cpvmult(n, r2));
info.normal = n;
info.alpha = t;
}
}
// Also check against the beveled vertexes.
if (rsum > 0.0f)
{
for (int i = 0; i < count; i++)
{
cpSegmentQueryInfo circle_info = new cpSegmentQueryInfo(null, b, cpVect.Zero, 1.0f);
cp.CircleSegmentQuery(this, planes[i].v0, r, a, b, r2, ref circle_info);
if (circle_info.alpha < info.alpha)
{
info = circle_info;
}
}
}
}
//MARK: Segment Query Functions
public float SegmentQueryFunc(SegmentQueryContext context, cpShape shape, object data)
{
cpSegmentQueryInfo info = null;
if (
!cpShapeFilter.Reject(shape.filter, context.filter) &&
shape.SegmentQuery(context.start, context.end, context.radius, ref info)
)
{
context.func(shape, info.point, info.normal, info.alpha, data);
}
return(1.0f);
}
public float SegmentQueryFirstFunc(SegmentQueryContext context, cpShape shape, cpSegmentQueryInfo output)
{
cpSegmentQueryInfo info = null;
if (
!cpShapeFilter.Reject(shape.filter, context.filter) && !shape.sensor &&
shape.SegmentQuery(context.start, context.end, context.radius, ref info) &&
info.alpha < output.alpha
)
{
output = info;
}
return(output.alpha);
}
protected override void segmentQuery(cpVect a, cpVect b, float r2, ref cpSegmentQueryInfo info)
{
cpVect n = this.tn;
float d = cpVect.cpvdot(cpVect.cpvsub(this.ta, a), n);
float r = this.r + r2;
cpVect flipped_n = (d > 0.0f ? cpVect.cpvneg(n) : n);
cpVect seg_offset = cpVect.cpvsub(cpVect.cpvmult(flipped_n, r), a);
// Make the endpoints relative to 'a' and move them by the thickness of the segment.
cpVect seg_a = cpVect.cpvadd(this.ta, seg_offset);
cpVect seg_b = cpVect.cpvadd(this.tb, seg_offset);
cpVect delta = cpVect.cpvsub(b, a);
if (cpVect.cpvcross(delta, seg_a) * cpVect.cpvcross(delta, seg_b) <= 0.0f)
{
float d_offset = d + (d > 0.0f ? -r : r);
float ad = -d_offset;
float bd = cpVect.cpvdot(delta, n) - d_offset;
if (ad * bd < 0.0f)
{
float t = ad / (ad - bd);
info.shape = (cpShape)this;
info.point = cpVect.cpvsub(cpVect.cpvlerp(a, b, t), cpVect.cpvmult(flipped_n, r2));
info.normal = flipped_n;
info.alpha = t;
}
}
else if (r != 0.0f)
{
cpSegmentQueryInfo info1 = new cpSegmentQueryInfo(null, b, cpVect.Zero, 1.0f);
cpSegmentQueryInfo info2 = new cpSegmentQueryInfo(null, b, cpVect.Zero, 1.0f);
cp.CircleSegmentQuery(this, this.ta, this.r, a, b, r2, ref info1);
cp.CircleSegmentQuery(this, this.tb, this.r, a, b, r2, ref info2);
if (info1.alpha < info2.alpha)
{
info = info1;
}
else
{
info = info2;
}
}
}
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 override void segmentQuery(cpVect a, cpVect b, float radius, ref cpSegmentQueryInfo info)
{
cp.CircleSegmentQuery(this, this.tc, this.r, a, b, radius, ref info);
}
protected override void segmentQuery(cpVect a, cpVect b, float r2, ref cpSegmentQueryInfo info)
{
cpSplittingPlane[] planes = this.planes;
int count = this.Count;
float r = this.r;
float rsum = r + r2;
for (int i = 0; i < count; i++)
{
cpVect n = planes[i].n;
float an = cpVect.cpvdot(a, n);
float d = an - cpVect.cpvdot(planes[i].v0, n) - rsum;
if (d < 0.0f) continue;
float bn = cpVect.cpvdot(b, n);
float t = d / (an - bn);
if (t < 0.0f || 1.0f < t) continue;
cpVect point = cpVect.cpvlerp(a, b, t);
float dt = cpVect.cpvcross(n, point);
float dtMin = cpVect.cpvcross(n, planes[(i - 1 + count) % count].v0);
float dtMax = cpVect.cpvcross(n, planes[i].v0);
if (dtMin <= dt && dt <= dtMax)
{
info.shape = this;
info.point = cpVect.cpvsub(cpVect.cpvlerp(a, b, t), cpVect.cpvmult(n, r2));
info.normal = n;
info.alpha = t;
}
}
// Also check against the beveled vertexes.
if (rsum > 0.0f)
{
for (int i = 0; i < count; i++)
{
cpSegmentQueryInfo circle_info = new cpSegmentQueryInfo(null, b, cpVect.Zero, 1.0f);
cp.CircleSegmentQuery(this, planes[i].v0, r, a, b, r2, ref circle_info);
if (circle_info.alpha < info.alpha)
info = circle_info;
}
}
}
public float SegmentQueryFirstFunc(SegmentQueryContext context, cpShape shape, cpSegmentQueryInfo output)
{
cpSegmentQueryInfo info = null;
if (
!cpShapeFilter.Reject(shape.filter, context.filter) && !shape.sensor &&
shape.SegmentQuery(context.start, context.end, context.radius, ref info) &&
info.alpha < output.alpha
)
{
output = info;
}
return output.alpha;
}
protected override void segmentQuery(cpVect a, cpVect b, float radius, ref cpSegmentQueryInfo info)
{
cp.CircleSegmentQuery(this, this.tc, this.r, a, b, radius, ref info);
}
protected virtual void segmentQuery(cpVect a, cpVect b, float radius, ref cpSegmentQueryInfo info)
{
throw new NotImplementedException();
}
protected virtual void segmentQuery(cpVect a, cpVect b, float radius, ref cpSegmentQueryInfo info)
{
throw new NotImplementedException();
}
protected override void segmentQuery(cpVect a, cpVect b, float r2, ref cpSegmentQueryInfo info)
{
cpVect n = this.tn;
float d = cpVect.cpvdot(cpVect.cpvsub(this.ta, a), n);
float r = this.r + r2;
cpVect flipped_n = (d > 0.0f ? cpVect.cpvneg(n) : n);
cpVect seg_offset = cpVect.cpvsub(cpVect.cpvmult(flipped_n, r), a);
// Make the endpoints relative to 'a' and move them by the thickness of the segment.
cpVect seg_a = cpVect.cpvadd(this.ta, seg_offset);
cpVect seg_b = cpVect.cpvadd(this.tb, seg_offset);
cpVect delta = cpVect.cpvsub(b, a);
if (cpVect.cpvcross(delta, seg_a) * cpVect.cpvcross(delta, seg_b) <= 0.0f)
{
float d_offset = d + (d > 0.0f ? -r : r);
float ad = -d_offset;
float bd = cpVect.cpvdot(delta, n) - d_offset;
if (ad * bd < 0.0f)
{
float t = ad / (ad - bd);
info.shape = (cpShape)this;
info.point = cpVect.cpvsub(cpVect.cpvlerp(a, b, t), cpVect.cpvmult(flipped_n, r2));
info.normal = flipped_n;
info.alpha = t;
}
}
else if (r != 0.0f)
{
cpSegmentQueryInfo info1 = new cpSegmentQueryInfo(null, b, cpVect.Zero, 1.0f);
cpSegmentQueryInfo info2 = new cpSegmentQueryInfo(null, b, cpVect.Zero, 1.0f);
cp.CircleSegmentQuery(this, this.ta, this.r, a, b, r2, ref info1);
cp.CircleSegmentQuery(this, this.tb, this.r, a, b, r2, ref info2);
if (info1.alpha < info2.alpha)
{
info = info1;
}
else
{
info = info2;
}
}
}
public static void CircleSegmentQuery(cpShape shape, cpVect center, float r1, cpVect a, cpVect b, float r2, ref cpSegmentQueryInfo info)
{
// offset the line to be relative to the circle
cpVect da = cpVect.cpvsub(a, center);
cpVect db = cpVect.cpvsub(b, center);
float rsum = r1 + r2;
float qa = cpVect.cpvdot(da, da) - 2 * cpVect.cpvdot(da, db) + cpVect.cpvdot(db, db);
float qb = cpVect.cpvdot(da, db) - cpVect.cpvdot(da, da);
float det = qb * qb - qa * (cpVect.cpvdot(da, da) - rsum * rsum);
if (det >= 0.0f)
{
float t = (-qb - cp.cpfsqrt(det)) / (qa);
if (0.0f <= t && t <= 1.0f)
{
{
cpVect n = cpVect.cpvnormalize(cpVect.cpvlerp(da, db, t));
info.shape = shape;
info.point = cpVect.cpvsub(cpVect.cpvlerp(da, db, t), cpVect.cpvmult(n, r2));
info.normal = n;
info.alpha = t;
}
}
}
}
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;
}
public cpShape SegmentQueryFirst(cpVect start, cpVect end, float radius, cpShapeFilter filter, ref cpSegmentQueryInfo output)
{
cpSegmentQueryInfo info = new cpSegmentQueryInfo(null, end, cpVect.Zero, 1.0f);
if (output == null)
output = info;
SegmentQueryContext context = new SegmentQueryContext(
start, end,
radius,
filter,
null);
this.staticShapes.SegmentQuery(context, start, end, 1.0f,
(o1, o2, o3) => SegmentQueryFirstFunc((SegmentQueryContext)o1, o2 as cpShape, (cpSegmentQueryInfo)o3)
, output);
// this.dynamicShapes.SegmentQuery(context, start, end, output.alpha , SegmentQueryFirst, ref output);
this.dynamicShapes.SegmentQuery(context, start, end, output.alpha,
(o1, o2, o3) => SegmentQueryFirstFunc((SegmentQueryContext)o1, o2 as cpShape, (cpSegmentQueryInfo)o3)
, output);
return output.shape;
}
public void Set(cpSegmentQueryInfo info1)
{
/// The shape that was hit, NULL if no collision occured.
this.shape = info1.shape;
/// The normalized distance along the query segment in the range [0, 1].
this.alpha = info1.alpha;
/// The normal of the surface hit.
this.normal = info1.normal;
this.point = info1.point;
}
public static void CircleSegmentQuery(cpShape shape, cpVect center, float r1, cpVect a, cpVect b, float r2, ref cpSegmentQueryInfo info)
{
// offset the line to be relative to the circle
cpVect da = cpVect.cpvsub(a, center);
cpVect db = cpVect.cpvsub(b, center);
float rsum = r1 + r2;
float qa = cpVect.cpvdot(da, da) - 2 * cpVect.cpvdot(da, db) + cpVect.cpvdot(db, db);
float qb = cpVect.cpvdot(da, db) - cpVect.cpvdot(da, da);
float det = qb * qb - qa * (cpVect.cpvdot(da, da) - rsum * rsum);
if (det >= 0.0f)
{
float t = (-qb - cp.cpfsqrt(det)) / (qa);
if (0.0f <= t && t <= 1.0f)
{
{
cpVect n = cpVect.cpvnormalize(cpVect.cpvlerp(da, db, t));
info.shape = shape;
info.point = cpVect.cpvsub(cpVect.cpvlerp(da, db, t), cpVect.cpvmult(n, r2));
info.normal = n;
info.alpha = t;
}
}
}
}
