// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
output = new RayCastOutput();
Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, _p);
Vector2 s = input.p1 - position;
float b = Vector2.Dot(s, s) - _radius * _radius;
// Solve quadratic equation.
Vector2 r = input.p2 - input.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_epsilon)
{
return false;
}
// 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 <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
Vector2 norm = (s + a * r);
norm.Normalize();
output.normal = norm;
return true;
}
return false;
}
/// <summary>
/// From Real-time Collision Detection, p179.
/// </summary>
/// <param name="output"></param>
/// <param name="input"></param>
/// <returns></returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input)
{
output = new RayCastOutput();
float tmin = -Settings.b2_maxFloat;
float tmax = Settings.b2_maxFloat;
Vector2 p = input.p1;
Vector2 d = input.p2 - input.p1;
Vector2 absD = MathUtils.Abs(d);
Vector2 normal = Vector2.Zero;
for (int i = 0; i < 2; ++i)
{
float absD_i = i == 0 ? absD.X : absD.Y;
float lowerBound_i = i == 0 ? lowerBound.X : lowerBound.Y;
float upperBound_i = i == 0 ? upperBound.X : upperBound.Y;
float p_i = i == 0 ? p.X : p.Y;
if (absD_i < Settings.b2_epsilon)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return false;
}
}
else
{
float d_i = i == 0 ? d.X : d.Y;
float inv_d = 1.0f / d_i;
float t1 = (lowerBound_i - p_i) * inv_d;
float t2 = (upperBound_i - p_i) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap<float>(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
if (i == 0)
{
normal.X = s;
}
else
{
normal.Y = s;
}
tmin = t1;
}
// Pull the max down
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return false;
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.maxFraction < tmin)
{
return false;
}
// Intersection.
output.fraction = tmin;
output.normal = normal;
return true;
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
/// @param callback a callback class that is called for each proxy that is hit by the ray.
/// </summary>
/// <param name="callback"></param>
/// <param name="input"></param>
internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
{
Vector2 p1 = input.p1;
Vector2 p2 = input.p2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 v = MathUtils.Cross(1.0f, r);
Vector2 abs_v = MathUtils.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.maxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.aabb, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = node.aabb.GetCenter();
Vector2 h = node.aabb.GetExtents();
float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.p1 = input.p1;
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
float value = callback(ref subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
}
else
{
if (count < k_stackSize)
{
stack[count++] = node.child1;
}
if (count < k_stackSize)
{
stack[count++] = node.child2;
}
}
}
}
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf)
{
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref xf.R, input.p1 - xf.Position);
Vector2 p2 = MathUtils.MultiplyT(ref xf.R, input.p2 - xf.Position);
Vector2 d = p2 - p1;
if (_vertexCount == 2)
{
Vector2 v1 = _vertices[0];
Vector2 v2 = _vertices[1];
Vector2 normal = _normals[0];
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -normal;
}
else
{
output.normal = normal;
}
return true;
}
else
{
float lower = 0.0f, upper = input.maxFraction;
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 false;
}
}
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 - Settings.b2_epsilon)
{
return false;
}
}
Debug.Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output.fraction = lower;
output.normal = MathUtils.Multiply(ref xf.R, _normals[index]);
return true;
}
}
return false;
}
float RayCastCallbackWrapper(ref RayCastInput input, int proxyId)
{
object userData = _contactManager._broadPhase.GetUserData(proxyId);
Fixture fixture = (Fixture)userData;
RayCastOutput output;
bool hit = fixture.RayCast(out output, ref input);
if (hit)
{
float fraction = output.fraction;
Vector2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
return _rayCastCallback(fixture, point, output.normal, fraction);
}
return input.maxFraction;
}
/// <summary>
/// Ray-cast the world for all fixtures in the path of the ray. Your callback
/// controls whether you get the closest point, any point, or n-points.
/// The ray-cast ignores shapes that contain the starting point.
/// </summary>
/// <param name="callback">a user implemented callback class.</param>
/// <param name="point1">the ray starting point</param>
/// <param name="point2">the ray ending point</param>
public void RayCast(RayCastCallback callback, Vector2 point1, Vector2 point2)
{
RayCastInput input = new RayCastInput();
input.maxFraction = 1.0f;
input.p1 = point1;
input.p2 = point2;
_rayCastCallback = callback;
_contactManager._broadPhase.RayCast(_rayCastCallbackWrapper, ref input);
_rayCastCallback = null;
}
/// Cast a ray against this Shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
{
Transform xf;
_body.GetTransform(out xf);
return _shape.RayCast(out output, ref input, ref xf, childIndex);
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
Debug.Assert(childIndex < _count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == _count)
{
i2 = 0;
}
s_edgeShape._vertex1 = _vertices[i1];
s_edgeShape._vertex2 = _vertices[i2];
return s_edgeShape.RayCast(out output, ref input, ref transform, 0);
}
/// Implement Shape.
///
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
//
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.p1 - transform.Position);
Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.p2 - transform.Position);
Vector2 d = p2 - p1;
Vector2 v1 = _vertex1;
Vector2 v2 = _vertex2;
Vector2 e = v2 - v1;
Vector2 normal = new Vector2(e.Y, -e.X);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -normal;
}
else
{
output.normal = normal;
}
return true;
}
/// Cast a ray against a child shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
/// @param transform the transform to be applied to the shape.
/// @param childIndex the child shape index
public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);
/// <summary> /// Cast a ray against this shape. /// </summary> /// <param name="output">output the ray-cast results.</param> /// <param name="input">the ray-cast input parameters.</param> /// <param name="transform">the transform to be applied to the shape.</param> /// <returns></returns> public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform);
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf)
{
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref xf.R, input.p1 - xf.Position);
Vector2 p2 = MathUtils.MultiplyT(ref xf.R, input.p2 - xf.Position);
Vector2 d = p2 - p1;
if (_vertexCount == 2)
{
Vector2 v1 = _vertices[0];
Vector2 v2 = _vertices[1];
Vector2 normal = _normals[0];
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return(false);
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -normal;
}
else
{
output.normal = normal;
}
return(true);
}
else
{
float lower = 0.0f, upper = input.maxFraction;
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(false);
}
}
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 - Settings.b2_epsilon)
{
return(false);
}
}
Debug.Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output.fraction = lower;
output.normal = MathUtils.Multiply(ref xf.R, _normals[index]);
return(true);
}
}
return(false);
}
internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
{
_tree.RayCast(callback, ref input);
}
