public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { // Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius output = new RayCastOutput(); TSVector2 position = transform.p + MathUtils.Mul(transform.q, Position); TSVector2 s = input.Point1 - position; FP b = TSVector2.Dot(s, s) - _2radius; // Solve quadratic equation. TSVector2 r = input.Point2 - input.Point1; FP c = TSVector2.Dot(s, r); FP rr = TSVector2.Dot(r, r); FP sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < Settings.Epsilon) { return(false); } // Find the point of intersection of the line with the circle. FP a = -(c + FP.Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= input.MaxFraction * rr) { a /= rr; output.Fraction = a; //TODO: Check results here output.Normal = s + a * r; output.Normal.Normalize(); return(true); } return(false); }
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { output = default(RayCastOutput); TSVector2 value = transform.p + MathUtils.Mul(transform.q, this.Position); TSVector2 tSVector = input.Point1 - value; FP y = TSVector2.Dot(tSVector, tSVector) - this._2radius; TSVector2 tSVector2 = input.Point2 - input.Point1; FP fP = TSVector2.Dot(tSVector, tSVector2); FP fP2 = TSVector2.Dot(tSVector2, tSVector2); FP x = fP * fP - fP2 * y; bool flag = x < 0f || fP2 < Settings.Epsilon; bool result; if (flag) { result = false; } else { FP fP3 = -(fP + FP.Sqrt(x)); bool flag2 = 0f <= fP3 && fP3 <= input.MaxFraction * fP2; if (flag2) { fP3 /= fP2; output.Fraction = fP3; output.Normal = tSVector + fP3 * tSVector2; output.Normal.Normalize(); result = true; } else { result = false; } } return(result); }
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { output = default(RayCastOutput); TSVector2 value = MathUtils.MulT(transform.q, input.Point1 - transform.p); TSVector2 value2 = MathUtils.MulT(transform.q, input.Point2 - transform.p); TSVector2 value3 = value2 - value; FP fP = 0f; FP x = input.MaxFraction; int num = -1; int i = 0; bool result; while (i < this.Vertices.Count) { FP x2 = TSVector2.Dot(this.Normals[i], this.Vertices[i] - value); FP fP2 = TSVector2.Dot(this.Normals[i], value3); bool flag = fP2 == 0f; if (flag) { bool flag2 = x2 < 0f; if (flag2) { result = false; return(result); } } else { bool flag3 = fP2 < 0f && x2 < fP * fP2; if (flag3) { fP = x2 / fP2; num = i; } else { bool flag4 = fP2 > 0f && x2 < x * fP2; if (flag4) { x = x2 / fP2; } } } bool flag5 = x < fP; if (!flag5) { i++; continue; } result = false; return(result); } Debug.Assert(0f <= fP && fP <= input.MaxFraction); bool flag6 = num >= 0; if (flag6) { output.Fraction = fP; output.Normal = MathUtils.Mul(transform.q, this.Normals[num]); result = true; return(result); } result = false; return(result); }
/// <summary> /// Cast a ray against this Shape. /// </summary> /// <param name="output">The ray-cast results.</param> /// <param name="input">The ray-cast input parameters.</param> /// <param name="childIndex">Index of the child.</param> /// <returns></returns> public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex) { return(Shape.RayCast(out output, ref input, ref Body._xf, childIndex)); }
public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true) { output = default(RayCastOutput); FP fP = -Settings.MaxFP; FP fP2 = Settings.MaxFP; TSVector2 point = input.Point1; TSVector2 tSVector = input.Point2 - input.Point1; TSVector2 tSVector2 = MathUtils.Abs(tSVector); TSVector2 zero = TSVector2.zero; bool result; for (int i = 0; i < 2; i++) { FP x = (i == 0) ? tSVector2.x : tSVector2.y; FP fP3 = (i == 0) ? this.LowerBound.x : this.LowerBound.y; FP x2 = (i == 0) ? this.UpperBound.x : this.UpperBound.y; FP fP4 = (i == 0) ? point.x : point.y; bool flag = x < Settings.Epsilon; if (flag) { bool flag2 = fP4 < fP3 || x2 < fP4; if (flag2) { result = false; return(result); } } else { FP y = (i == 0) ? tSVector.x : tSVector.y; FP y2 = 1f / y; FP fP5 = (fP3 - fP4) * y2; FP fP6 = (x2 - fP4) * y2; FP fP7 = -1f; bool flag3 = fP5 > fP6; if (flag3) { MathUtils.Swap <FP>(ref fP5, ref fP6); fP7 = 1f; } bool flag4 = fP5 > fP; if (flag4) { bool flag5 = i == 0; if (flag5) { zero.x = fP7; } else { zero.y = fP7; } fP = fP5; } fP2 = TSMath.Min(fP2, fP6); bool flag6 = fP > fP2; if (flag6) { result = false; return(result); } } } bool flag7 = doInteriorCheck && (fP < 0f || input.MaxFraction < fP); if (flag7) { result = false; return(result); } output.Fraction = fP; output.Normal = zero; result = true; return(result); }
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { output = new RayCastOutput(); // Put the ray into the polygon's frame of reference. TSVector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p); TSVector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p); TSVector2 d = p2 - p1; FP lower = 0.0f, upper = input.MaxFraction; int index = -1; for (int i = 0; i < Vertices.Count; ++i) { // p = p1 + a * d // dot(normal, p - v) = 0 // dot(normal, p1 - v) + a * dot(normal, d) = 0 FP numerator = TSVector2.Dot(Normals[i], Vertices[i] - p1); FP denominator = TSVector2.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; } } // The use of epsilon here causes the assert on lower to trip // in some cases. Apparently the use of epsilon was to make edge // shapes work, but now those are handled separately. //if (upper < lower - b2_epsilon) if (upper < lower) { return(false); } } Debug.Assert(0.0f <= lower && lower <= input.MaxFraction); if (index >= 0) { output.Fraction = lower; output.Normal = MathUtils.Mul(transform.q, Normals[index]); return(true); } return(false); }
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { output = default(RayCastOutput); TSVector2 tSVector = MathUtils.MulT(transform.q, input.Point1 - transform.p); TSVector2 value = MathUtils.MulT(transform.q, input.Point2 - transform.p); TSVector2 tSVector2 = value - tSVector; TSVector2 vertex = this._vertex1; TSVector2 vertex2 = this._vertex2; TSVector2 tSVector3 = vertex2 - vertex; TSVector2 tSVector4 = new TSVector2(tSVector3.y, -tSVector3.x); tSVector4.Normalize(); FP x = TSVector2.Dot(tSVector4, vertex - tSVector); FP fP = TSVector2.Dot(tSVector4, tSVector2); bool flag = fP == 0f; bool result; if (flag) { result = false; } else { FP fP2 = x / fP; bool flag2 = fP2 < 0f || input.MaxFraction < fP2; if (flag2) { result = false; } else { TSVector2 value2 = tSVector + fP2 * tSVector2; TSVector2 tSVector5 = vertex2 - vertex; FP fP3 = TSVector2.Dot(tSVector5, tSVector5); bool flag3 = fP3 == 0f; if (flag3) { result = false; } else { FP fP4 = TSVector2.Dot(value2 - vertex, tSVector5) / fP3; bool flag4 = fP4 < 0f || 1f < fP4; if (flag4) { result = false; } else { output.Fraction = fP2; bool flag5 = x > 0f; if (flag5) { output.Normal = -tSVector4; } else { output.Normal = tSVector4; } result = true; } } } } return(result); }
public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);