// From Real-time Collision Detection, p179. public void RayCast(out RayCastOutput output, ref RayCastInput input) { output = new RayCastOutput(); float tmin = -Settings.b2_FLT_MAX; float tmax = Settings.b2_FLT_MAX; output.hit = false; 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_FLT_EPSILON) { // Parallel. if (p_i < lowerBound_i || upperBound_i < p_i) { return; } } 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; } } } // 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; }
/// 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(RayCastCallback 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; RayCastOutput output; callback(out output, ref subInput, node.userData); if (output.hit) { // Early exit. if (output.fraction == 0.0f) { return; } maxFraction = output.fraction; // Update segment bounding box. { Vector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.lowerBound = Vector2.Min(p1, t); segmentAABB.upperBound = Vector2.Max(p1, t); } } } else { Debug.Assert(count + 1 < k_stackSize); stack[count++] = node.child1; stack[count++] = node.child2; } } }