/// @see Shape.TestSegment
public override SegmentCollide TestSegment( ref XForm transform,
out float lambda,
out Vector2 normal,
ref Segment segment,
float maxLambda)
{
lambda = 0.0f;
normal = Vector2.Zero;
Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, _p);
Vector2 s = segment.p1 - position;
float b = Vector2.Dot(s, s) - _radius * _radius;
// Does the segment start inside the circle?
if (b < 0.0f)
{
return SegmentCollide.StartsInside;
}
// Solve quadratic equation.
Vector2 r = segment.p2 - segment.p1;
float c = Vector2.Dot(s, r);
float rr = Vector2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.b2_FLT_EPSILON)
{
return SegmentCollide.Miss;
}
// Find the point of intersection of the line with the circle.
float a = -(c + (float)Math.Sqrt((double)sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= maxLambda * rr)
{
a /= rr;
lambda = a;
normal = s + a * r;
normal.Normalize();
return SegmentCollide.Hit;
}
return SegmentCollide.Miss;
}
/// Perform a ray cast against this shape.
/// @param xf the shape world transform.
/// @param lambda returns the hit fraction. You can use this to compute the contact point
/// p = (1 - lambda) * segment.p1 + lambda * segment.p2.
/// @param normal returns the normal at the contact point. If there is no intersection, the normal
/// is not set.
/// @param segment defines the begin and end point of the ray cast.
/// @param maxLambda a number typically in the range [0,1].
public SegmentCollide TestSegment(out float lambda, out Vector2 normal, ref Segment segment, float maxLambda)
{
XForm xf;
_body.GetXForm(out xf);
return _shape.TestSegment(ref xf, out lambda, out normal, ref segment, maxLambda);
}
/// @see Shape.TestSegment
public override SegmentCollide TestSegment(ref XForm xf,
out float lambda,
out Vector2 normal,
ref Segment segment,
float maxLambda)
{
lambda = 0;
normal = Vector2.Zero;
float lower = 0.0f, upper = maxLambda;
Vector2 p1 = MathUtils.MultiplyT(ref xf.R, segment.p1 - xf.Position);
Vector2 p2 = MathUtils.MultiplyT(ref xf.R, segment.p2 - xf.Position);
Vector2 d = p2 - p1;
int index = -1;
for (int i = 0; i < _vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(_normals[i], _vertices[i] - p1);
float denominator = Vector2.Dot(_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return SegmentCollide.Miss;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return SegmentCollide.Miss;
}
}
Debug.Assert(0.0f <= lower && lower <= maxLambda);
if (index >= 0)
{
lambda = lower;
normal = MathUtils.Multiply(ref xf.R, _normals[index]);
return SegmentCollide.Hit;
}
lambda = 0;
return SegmentCollide.StartsInside;
}
public Vector2 p2; ///< the ending point
#endregion Fields
#region Methods
/// Ray cast against this segment with another segment.
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.4.1
// x = mu1 * p1 + mu2 * p2
// mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
// mu1 = 1 - mu2;
// x = (1 - mu2) * p1 + mu2 * p2
// = p1 + mu2 * (p2 - p1)
// x = s + a * r (s := start, r := end - start)
// s + a * r = p1 + mu2 * d (d := p2 - p1)
// -a * r + mu2 * d = b (b := s - p1)
// [-r d] * [a; mu2] = b
// Cramer's rule:
// denom = det[-r d]
// a = det[b d] / denom
// mu2 = det[-r b] / denom
public bool TestSegment(out float lambda, out Vector2 normal, ref Segment segment, float maxLambda)
{
lambda = 0;
normal = Vector2.Zero;
Vector2 s = segment.p1;
Vector2 r = segment.p2 - s;
Vector2 d = p2 - p1;
Vector2 n = MathUtils.Cross(d, 1.0f);
float k_slop = 100.0f * Settings.b2_FLT_EPSILON;
float denom = -Vector2.Dot(r, n);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop)
{
// Does the segment intersect the infinite line associated with this segment?
Vector2 b = s - p1;
float a = Vector2.Dot(b, n);
if (0.0f <= a && a <= maxLambda * denom)
{
float mu2 = -r.X * b.Y + r.Y * b.X;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
{
a /= denom;
n.Normalize();
lambda = a;
normal = n;
return true;
}
}
}
return false;
}
/// Perform a ray cast against this shape.
/// @param xf the shape world transform.
/// @param lambda returns the hit fraction. You can use this to compute the contact point
/// p = (1 - lambda) * segment.p1 + lambda * segment.p2.
/// @param normal returns the normal at the contact point. If there is no intersection, the normal
/// is not set.
/// @param segment defines the begin and end point of the ray cast.
/// @param maxLambda a number typically in the range [0,1].
public abstract SegmentCollide TestSegment( ref XForm xf,
out float lambda,
out Vector2 normal,
ref Segment segment,
float maxLambda);
