/// <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; Vector2 p = input.P1; Vector2 d = input.P2 - input.P1; Vector2 absD = d.Abs(); Vector2 normal = Vector2.Zero; for (int i = 0; i < 2; ++i) { if (absD.GetByIndex(i) < Common.Settings.FLT_EPSILON) { // Parallel. if (p.GetByIndex(i) < LowerBound.GetByIndex(i) || UpperBound.GetByIndex(i) < p.GetByIndex(i)) { return; } } else { float inv_d = 1.0f / d.GetByIndex(i); float t1 = (LowerBound.GetByIndex(i) - p.GetByIndex(i)) * inv_d; float t2 = (UpperBound.GetByIndex(i) - p.GetByIndex(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 = Vector2.Zero; normal.SetByIndex(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; }
/// <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 { Hit = false }; Vec2 p = input.P1; Vec2 d = input.P2 - input.P1; Vec2 absD = Common.MathB2.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.MathB2.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.MathB2.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; }
internal bool RayCast(RayCastOutput output, RayCastInput input, XForm xf, int v) { Vec2 p1 = MathB2.MulT(xf.R, input.P1 - xf.Position); Vec2 p2 = MathB2.MulT(xf.R, input.P2 - xf.Position); Vec2 d = p2 - p1; Vec2 v1 = _v1; Vec2 v2 = _v2; Vec2 e = v2 - v1; Vec2 normal = new Vec2(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 = MathB2.Dot(normal, v1 - p1); float denominator = MathB2.Dot(normal, d); if (denominator == 0.0f) { return(false); } float t = numerator / denominator; if (t < 0.0f || input.MaxFraction < t) { return(false); } Vec2 q = p1 + t * d; // q = v1 + s * r // s = dot(q - v1, r) / dot(r, r) Vec2 r = v2 - v1; float rr = MathB2.Dot(r, r); if (rr == 0.0f) { return(false); } float s = MathB2.Dot(q - v1, r) / rr; if (s < 0.0f || 1.0f < s) { return(false); } output.Fraction = t; if (numerator > 0.0f) { output.Normal = -MathB2.Mul(xf.R, normal); } else { output.Normal = MathB2.Mul(xf.R, normal); } return(true); }