Пример #1
0
        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);
        }
Пример #2
0
        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);
        }
Пример #3
0
        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);
        }
Пример #4
0
 /// <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));
 }
Пример #5
0
        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);
        }
Пример #6
0
        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);
        }
Пример #7
0
        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);
        }
Пример #8
0
 public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);