public void RayCast(out RayCastOutput output, RayCastInput input)
{
output = new RayCastOutput();
float tmin = -Settings.FLT_MAX;
float tmax = Settings.FLT_MAX;
output.Hit = false;
Vec2 p = input.P1;
Vec2 d = input.P2 - input.P1;
Vec2 absD = Math.Abs(d);
Vec2 normal = new Vec2();
if (absD.X < Settings.FLT_EPSILON)
{
// Parallel.
if (p.X < LowerBound.X || UpperBound.X < p.X)
{
return;
}
}
else
{
float inv_d = 1.0f / d.X;
float t1 = (LowerBound.X - p.X) * inv_d;
float t2 = (UpperBound.X - p.X) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
Common.Math.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.X = s;
tmin = t1;
}
// Pull the max down
tmax = Common.Math.Min(tmax, t2);
if (tmin > tmax)
{
return;
}
}
if (absD.Y < Settings.FLT_EPSILON)
{
// Parallel.
if (p.Y < LowerBound.Y || UpperBound.Y < p.Y)
{
return;
}
}
else
{
float inv_d = 1.0f / d.Y;
float t1 = (LowerBound.Y - p.Y) * inv_d;
float t2 = (UpperBound.Y - p.Y) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
Common.Math.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.Y = s;
tmin = t1;
}
// Pull the max down
tmax = Common.Math.Min(tmax, t2);
if (tmin > tmax)
{
return;
}
}
//// Does the ray start inside the box?
//// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.MaxFraction < tmin)
{
return;
}
//// Intersection.
output.Fraction = tmin;
output.Normal = normal;
output.Hit = true;
}
// 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 void RayCast(out RayCastOutput output, ref RayCastInput input, Transform transform)
{
output = new RayCastOutput();
Vec2 position = transform.Position + Math.Mul(transform.R, _p);
Vec2 s = input.P1 - position;
float b = Vec2.Dot(s, s) - _radius * _radius;
// Solve quadratic equation.
Vec2 r = input.P2 - input.P1;
float c = Vec2.Dot(s, r);
float rr = Vec2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.FLT_EPSILON)
{
output.Hit = false;
return;
}
// Find the point of intersection of the line with the circle.
float a = -(c + Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Hit = true;
output.Fraction = a;
output.Normal = s + a*r;
output.Normal.Normalize();
return;
}
output.Hit = false;
return;
}
/// Cast a ray against this shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
public void RayCast(out RayCastOutput output, ref RayCastInput input)
{
Shape.RayCast(out output, ref input, Body.GetTransform());
}
/// <summary>
// From Real-time Collision Detection, p179.
/// </summary>
public void RayCast(out RayCastOutput output, RayCastInput input)
{
float tmin = -Common.Settings.FLT_MAX;
float tmax = Common.Settings.FLT_MAX;
output = new RayCastOutput();
output.Hit = false;
Vec2 p = input.P1;
Vec2 d = input.P2 - input.P1;
Vec2 absD = Common.Math.Abs(d);
Vec2 normal = new Vec2(0);
for (int i = 0; i < 2; ++i)
{
if (absD[i] < Common.Settings.FLT_EPSILON)
{
// Parallel.
if (p[i] < LowerBound[i] || UpperBound[i] < p[i])
{
return;
}
}
else
{
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)
{
Common.Math.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal[i] = s;
tmin = t1;
}
// Pull the max down
tmax = Common.Math.Min(tmax, t2);
if (tmin > tmax)
{
return;
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.MaxFraction < tmin)
{
return;
}
// Intersection.
output.Fraction = tmin;
output.Normal = normal;
output.Hit = true;
}
/// Cast a ray against this shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. public abstract void RayCast(out RayCastOutput output, ref RayCastInput input, Transform transform);
public override void RayCast(out RayCastOutput output, ref RayCastInput input, Transform xf)
{
output = new RayCastOutput();
float lower = 0.0f, upper = input.MaxFraction;
// Put the ray into the polygon's frame of reference.
Vec2 p1 = Math.MulT(xf.R, input.P1 - xf.Position);
Vec2 p2 = Math.MulT(xf.R, input.P2 - xf.Position);
Vec2 d = p2 - p1;
int index = -1;
output.Hit = false;
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 = Vec2.Dot(Normals[i], Vertices[i] - p1);
float denominator = Vec2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return;
}
}
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;
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Hit = true;
output.Fraction = lower;
output.Normal = Math.Mul(xf.R, Normals[index]);
return;
}
}
/// 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.
public void RayCast(IRayCastEnabled callback, RayCastInput input)
{
Vec2 p1 = input.P1;
Vec2 p2 = input.P2;
Vec2 r = p2 - p1;
Box2DXDebug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vec2 v = Vec2.Cross(1.0f, r);
Vec2 abs_v = Math.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();
{
Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Math.Min(p1, t);
segmentAABB.UpperBound = Math.Max(p1, t);
}
const int k_stackSize = 128;
int[] stack = new int[k_stackSize];
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode node = _nodes[nodeId];
if (Collision.TestOverlap(node.Aabb, segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vec2 c = node.Aabb.GetCenter();
Vec2 h = node.Aabb.GetExtents();
float separation = Math.Abs(Vec2.Dot(v, p1 - c)) - Vec2.Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput = new RayCastInput();
subInput.P1 = input.P1;
subInput.P2 = input.P2;
subInput.MaxFraction = maxFraction;
maxFraction = callback.RayCastCallback(subInput, nodeId);
if (maxFraction == 0.0f)
{
return;
}
// Update segment bounding box.
{
Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Math.Min(p1, t);
segmentAABB.UpperBound = Math.Max(p1, t);
}
}
else
{
Box2DXDebug.Assert(count + 1 < k_stackSize);
stack[count++] = node.Child1;
stack[count++] = node.Child2;
}
}
}
/// 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.
/// @param callback a user implemented callback class.
/// @param point1 the ray starting point
/// @param point2 the ray ending point
public void RayCast(RayCastCallback callback, Vec2 point1, Vec2 point2)
{
WorldRayCastWrapper wrapper = new WorldRayCastWrapper();
wrapper.broadPhase = _contactManager._broadPhase;
wrapper.callback = callback;
RayCastInput input = new RayCastInput();
input.MaxFraction = 1.0f;
input.P1 = point1;
input.P2 = point2;
_contactManager._broadPhase.RayCast(wrapper, input);
}
public float RayCastCallback(RayCastInput input, int proxyId)
{
object userData = broadPhase.GetUserData(proxyId);
Fixture fixture = (Fixture)userData;
RayCastOutput output;
fixture.RayCast(out output, ref input);
if (output.Hit)
{
float fraction = output.Fraction;
Vec2 point = (1.0f - fraction) * input.P1 + fraction * input.P2;
return callback.ReportFixture(fixture, point, output.Normal, fraction);
}
return input.MaxFraction;
}
