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; }