/// <summary>
 /// Initializes a new instance of the <see cref="RayConvexAlgorithm"/> class.
 /// </summary>
 /// <param name="collisionDetection">The collision detection service.</param>
 public RayConvexAlgorithm(CollisionDetection collisionDetection)
     : base(collisionDetection)
 {
     _gjk = new Gjk(CollisionDetection);
 }
Esempio n. 2
0
        private Vector3 DoMpr(CollisionQueryType type, ContactSet contactSet, Vector3 v0)
        {
            int       iterationCount = 0;
            const int iterationLimit = 100;

            CollisionObject  collisionObjectA = contactSet.ObjectA;
            IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
            ConvexShape      shapeA           = (ConvexShape)geometricObjectA.Shape;
            Vector3          scaleA           = geometricObjectA.Scale;
            Pose             poseA            = geometricObjectA.Pose;

            CollisionObject  collisionObjectB = contactSet.ObjectB;
            IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
            ConvexShape      shapeB           = (ConvexShape)geometricObjectB.Shape;
            Vector3          scaleB           = geometricObjectB.Scale;
            Pose             poseB            = geometricObjectB.Pose;

            // Cache inverted rotations.
            var orientationAInverse = poseA.Orientation.Transposed;
            var orientationBInverse = poseB.Orientation.Transposed;

            Vector3 n   = -v0; // Shoot from v0 to the origin.
            Vector3 v1A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
            Vector3 v1B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
            Vector3 v1  = v1B - v1A;

            // Separating axis test:
            if (Vector3.Dot(v1, n) < 0)
            {
                // TODO: We could cache the separating axis n in ContactSet for future collision checks.
                //       Also in the separating axis tests below.
                return(Vector3.Zero);
            }

            // Second support direction = perpendicular to plane of origin, v0 and v1.
            n = Vector3.Cross(v1, v0);

            // If n is a zero vector, then origin, v0 and v1 are on a line with the origin inside the support plane.
            if (n.IsNumericallyZero)
            {
                // Contact found.
                contactSet.HaveContact = true;
                if (type == CollisionQueryType.Boolean)
                {
                    return(Vector3.Zero);
                }

                // Compute contact information.
                // (v0 is an inner point. v1 is a support point on the CSO. => The contact normal is -v1.
                // However, v1 could be close to the origin. To avoid numerical
                // problems we use v0 - v1, which is the same direction.)
                Vector3 normal = v0 - v1;
                if (!normal.TryNormalize())
                {
                    // This happens for Point vs. flat object when they are on the same position.
                    // Maybe we could even find a better normal.
                    normal = Vector3.UnitY;
                }

                Vector3 position         = (v1A + v1B) / 2;
                float   penetrationDepth = v1.Length;
                Contact contact          = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
                ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);

                return(Vector3.Zero);
            }

            Vector3 v2A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
            Vector3 v2B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
            Vector3 v2  = v2B - v2A;

            // Separating axis test:
            if (Vector3.Dot(v2, n) < 0)
            {
                return(Vector3.Zero);
            }

            // Third support direction = perpendicular to plane of v0, v1 and v2.
            n = Vector3.Cross(v1 - v0, v2 - v0);

            // If the origin is on the negative side of the plane, then reverse the plane direction.
            // n must point into the origin direction and not away...
            if (Vector3.Dot(n, v0) > 0)
            {
                MathHelper.Swap(ref v1, ref v2);
                MathHelper.Swap(ref v1A, ref v2A);
                MathHelper.Swap(ref v1B, ref v2B);
                n = -n;
            }

            if (n.IsNumericallyZero)
            {
                // Degenerate case:
                // Interpretation (HelmutG): v2 is on the line with v0 and v1. I think this can only happen
                // if the CSO is flat and in the plane of (origin, v0, v1).
                // This happens for example in Point vs. Line Segment, or triangle vs. triangle when both
                // triangles are in the same plane.
                // Simply ignore this case (Infinite small/flat objects do not touch).
                return(Vector3.Zero);
            }

            // Search for a valid portal.
            Vector3 v3, v3A, v3B;

            while (true)
            {
                iterationCount++;

                // Abort if we cannot find a valid portal.
                if (iterationCount > iterationLimit)
                {
                    return(Vector3.Zero);
                }

                // Get next support point.
                //v3A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
                //v3B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
                //v3 = v3B - v3A;

                // ----- Optimized version:
                Vector3 supportDirectionA;
                supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z);
                supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z);
                supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z);
                Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
                v3A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X;
                v3A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y;
                v3A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z;
                Vector3 supportDirectionB;
                supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z;
                supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z;
                supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z;
                Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
                v3B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X;
                v3B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y;
                v3B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z;
                v3    = v3B - v3A;

                // Separating axis test:
                //if (Vector3.Dot(v3, n) < 0)
                if (v3.X * n.X + v3.Y * n.Y + v3.Z * n.Z < 0)
                {
                    return(Vector3.Zero);
                }

                // v0, v1, v2, v3 form a tetrahedron.
                // v0 is an inner point of the CSO and v1, v2, v3 are support points.
                // v1, v2, v3 should form a valid portal.

                // If origin is outside the plane of v0, v1, v3 then the portal is invalid and we choose a new n.
                //if (Vector3.Dot(Vector3.Cross(v1, v3), v0) < 0) // ORIENT3D test, see Ericson: "Real-Time Collision Detection"
                if ((v1.Y * v3.Z - v1.Z * v3.Y) * v0.X
                    + (v1.Z * v3.X - v1.X * v3.Z) * v0.Y
                    + (v1.X * v3.Y - v1.Y * v3.X) * v0.Z < 0)
                {
                    v2  = v3; // Get rid of v2. A new v3 will be chosen in the next iteration.
                    v2A = v3A;
                    v2B = v3B;
                    //n = Vector3.Cross(v1 - v0, v3 - v0);
                    // ----- Optimized version:
                    Vector3 v1MinusV0;
                    v1MinusV0.X = v1.X - v0.X;
                    v1MinusV0.Y = v1.Y - v0.Y;
                    v1MinusV0.Z = v1.Z - v0.Z;
                    Vector3 v3MinusV0;
                    v3MinusV0.X = v3.X - v0.X;
                    v3MinusV0.Y = v3.Y - v0.Y;
                    v3MinusV0.Z = v3.Z - v0.Z;
                    n.X         = v1MinusV0.Y * v3MinusV0.Z - v1MinusV0.Z * v3MinusV0.Y;
                    n.Y         = v1MinusV0.Z * v3MinusV0.X - v1MinusV0.X * v3MinusV0.Z;
                    n.Z         = v1MinusV0.X * v3MinusV0.Y - v1MinusV0.Y * v3MinusV0.X;
                    continue;
                }

                // If origin is outside the plane of v0, v2, v3 then the portal is invalid and we choose a new n.
                //if (Vector3.Dot(Vector3.Cross(v3, v2), v0) < 0)
                if ((v3.Y * v2.Z - v3.Z * v2.Y) * v0.X
                    + (v3.Z * v2.X - v3.X * v2.Z) * v0.Y
                    + (v3.X * v2.Y - v3.Y * v2.X) * v0.Z < 0)
                {
                    v1  = v3; // Get rid of v1. A new v3 will be chosen in the next iteration.
                    v1A = v3A;
                    v1B = v3B;
                    //n = Vector3.Cross(v3 - v0, v2 - v0);
                    // ----- Optimized version:
                    Vector3 v3MinusV0;
                    v3MinusV0.X = v3.X - v0.X;
                    v3MinusV0.Y = v3.Y - v0.Y;
                    v3MinusV0.Z = v3.Z - v0.Z;
                    Vector3 v2MinusV0;
                    v2MinusV0.X = v2.X - v0.X;
                    v2MinusV0.Y = v2.Y - v0.Y;
                    v2MinusV0.Z = v2.Z - v0.Z;
                    n.X         = v3MinusV0.Y * v2MinusV0.Z - v3MinusV0.Z * v2MinusV0.Y;
                    n.Y         = v3MinusV0.Z * v2MinusV0.X - v3MinusV0.X * v2MinusV0.Z;
                    n.Z         = v3MinusV0.X * v2MinusV0.Y - v3MinusV0.Y * v2MinusV0.X;
                    continue;
                }

                // If come to here, then we have found a valid portal to begin with.
                // (We have a tetrahedron that contains the ray (v0 to origin)).
                break;
            }

            // Refine the portal
            while (true)
            {
                iterationCount++;

                // Store old n. Numerical inaccuracy can lead to endless loops where n is constant.
                Vector3 oldN = n;

                // Compute outward pointing normal of the portal
                //n = Vector3.Cross(v2 - v1, v3 - v1);
                Vector3 v2MinusV1;
                v2MinusV1.X = v2.X - v1.X;
                v2MinusV1.Y = v2.Y - v1.Y;
                v2MinusV1.Z = v2.Z - v1.Z;
                Vector3 v3MinusV1;
                v3MinusV1.X = v3.X - v1.X;
                v3MinusV1.Y = v3.Y - v1.Y;
                v3MinusV1.Z = v3.Z - v1.Z;
                n.X         = v2MinusV1.Y * v3MinusV1.Z - v2MinusV1.Z * v3MinusV1.Y;
                n.Y         = v2MinusV1.Z * v3MinusV1.X - v2MinusV1.X * v3MinusV1.Z;
                n.Z         = v2MinusV1.X * v3MinusV1.Y - v2MinusV1.Y * v3MinusV1.X;

                //if (!n.TryNormalize())
                // ----- Optimized version:
                float nLengthSquared = n.LengthSquared();
                if (nLengthSquared < Numeric.EpsilonFSquared)
                {
                    // The portal is degenerate (some vertices of v1, v2, v3 are identical).
                    // This can happen for coplanar shapes, e.g. long thin triangles in the
                    // same plane. The portal (v1, v2, v3) is a line segment.
                    // This might be a contact or not. We use the GJK as a fallback to check this case.

                    if (_gjk == null)
                    {
                        _gjk = new Gjk(CollisionDetection);
                    }

                    _gjk.ComputeCollision(contactSet, CollisionQueryType.Boolean);
                    if (contactSet.HaveContact == false)
                    {
                        return(Vector3.Zero);
                    }

                    // GJK reports a contact - but it cannot compute contact positions.
                    // We use the best point on the current portal as the contact point.

                    // Find the point closest to the origin.
                    float u, v, w;
                    GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w);
                    Vector3 vClosest = u * v1 + v * v2 + w * v3;

                    // We have not found a separating axis so far. --> Contact.
                    contactSet.HaveContact = true;
                    if (type == CollisionQueryType.Boolean)
                    {
                        return(Vector3.Zero);
                    }

                    // The points on the objects have the same barycentric coordinates.
                    Vector3 pointOnA = u * v1A + v * v2A + w * v3A;
                    Vector3 pointOnB = u * v1B + v * v2B + w * v3B;

                    Vector3 normal = pointOnA - pointOnB;
                    if (!normal.TryNormalize())
                    {
                        if (contactSet.IsPreferredNormalAvailable)
                        {
                            normal = contactSet.PreferredNormal;
                        }
                        else
                        {
                            normal = Vector3.UnitY;
                        }
                    }

                    Vector3 position         = (pointOnA + pointOnB) / 2;
                    float   penetrationDepth = vClosest.Length;
                    Contact contact          = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
                    ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);

                    return(Vector3.Zero);
                }

                // ----- Optimized version: Rest of n.TryNormalize():
                float nLength = (float)Math.Sqrt(nLengthSquared);
                float scale   = 1.0f / nLength;
                n.X *= scale;
                n.Y *= scale;
                n.Z *= scale;

                // Separating axis test:
                // Testing > instead of >= is important otherwise coplanar triangles may report false contacts
                // because the portal is in the same plane as the origin.
                if (!contactSet.HaveContact &&
                    v1.X * n.X + v1.Y * n.Y + v1.Z * n.Z > 0) // Optimized version of && Vector3.Dot(v1, n) > 0)
                {
                    // Portal points aways from origin --> Origin is in the tetrahedron.
                    contactSet.HaveContact = true;
                    if (type == CollisionQueryType.Boolean)
                    {
                        return(Vector3.Zero);
                    }
                }

                // Find new support point.
                //Vector3 v4A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
                //Vector3 v4B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
                //Vector3 v4 = v4B - v4A;

                // ----- Optimized version:
                Vector3 supportDirectionA;
                supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z);
                supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z);
                supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z);
                Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
                Vector3 v4A;
                v4A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X;
                v4A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y;
                v4A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z;
                Vector3 supportDirectionB;
                supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z;
                supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z;
                supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z;
                Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
                Vector3 v4B;
                v4B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X;
                v4B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y;
                v4B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z;
                Vector3 v4 = v4B - v4A;

                // Separating axis test:
                if (!contactSet.HaveContact &&                // <--- New (see below).
                    v4.X * n.X + v4.Y * n.Y + v4.Z * n.Z < 0) // Optimized version of && Vector3.Dot(v4, n) < 0)
                {
                    // Following assert can fail. For example if the above dot product returns -0.000000001
                    // for nearly perfectly touching objects. Therefore I have added the condition
                    // hit == false to the condition.
                    return(Vector3.Zero);
                }

                // Test if we have refined more than the collision epsilon.
                // Condition 1: Project the point difference v4-v3 onto normal n and check whether we have
                // improved in this direction.
                // Condition 2: If n has not changed, then we couldn't improve anymore. This is caused
                // by numerical problems, e.g. when a large object (>10000) is checked.
                //if (Vector3.Dot(v4 - v3, n) <= CollisionDetection.Epsilon
                // ----- Optimized version:
                if ((v4.X - v3.X) * n.X + (v4.Y - v3.Y) * n.Y + (v4.Z - v3.Z) * n.Z <= CollisionDetection.Epsilon ||
                    Vector3.AreNumericallyEqual(n, oldN) ||
                    iterationCount >= iterationLimit)
                {
                    // We have the final portal.
                    if (!contactSet.HaveContact)
                    {
                        return(Vector3.Zero);
                    }

                    if (type == CollisionQueryType.Boolean)
                    {
                        return(Vector3.Zero);
                    }

                    // Find the point closest to the origin.
                    float u, v, w;
                    GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w);

                    // Note: If u, v or w is 0 or 1, then the point was probably outside portal triangle.
                    // We can use the returned data, but re-running MPR will give us a better contact.

                    Vector3 closest = u * v1 + v * v2 + w * v3;

                    // The points on the objects have the same barycentric coordinates.
                    Vector3 pointOnA = u * v1A + v * v2A + w * v3A;
                    Vector3 pointOnB = u * v1B + v * v2B + w * v3B;

                    // Use difference between points as normal direction, only if it can be normalized.
                    Vector3 normal = pointOnA - pointOnB;
                    if (!normal.TryNormalize())
                    {
                        normal = -n; // Else use the inverted normal of the portal.
                    }
                    Vector3 position         = (pointOnA + pointOnB) / 2;
                    float   penetrationDepth = closest.Length;
                    Contact contact          = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
                    ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);

                    // If real closest point is outside the portal triangle, then one of u, v, w will
                    // be exactly 0 or 1. In this case we should run a new MPR with the portal ray n.
                    if (u == 0 || v == 0 || w == 0 || u == 1 || v == 1 || w == 1)
                    {
                        return(n);
                    }

                    return(Vector3.Zero);
                }

                // Now we have a new point v4 and have to make a new portal by eliminating v1, v2 or v3.
                // The possible new tetrahedron faces are: (v0, v1, v4), (v0, v4, v2), (v0, v4, v3)
                // We don't know the orientation yet.
                // Test with the ORIENT3D test.
                //Vector3 cross = Vector3.Cross(v4, v0);
                // ----- Optimized version:
                Vector3 cross;
                cross.X = v4.Y * v0.Z - v4.Z * v0.Y;
                cross.Y = v4.Z * v0.X - v4.X * v0.Z;
                cross.Z = v4.X * v0.Y - v4.Y * v0.X;

                //if (Vector3.Dot(v1, cross) > 0)
                if (v1.X * cross.X + v1.Y * cross.Y + v1.Z * cross.Z > 0)
                {
                    // Eliminate v3 or v1.
                    //if (Vector3.Dot(v2, cross) > 0)
                    if (v2.X * cross.X + v2.Y * cross.Y + v2.Z * cross.Z > 0)
                    {
                        v1  = v4;
                        v1A = v4A;
                        v1B = v4B;
                    }
                    else
                    {
                        v3  = v4;
                        v3A = v4A;
                        v3B = v4B;
                    }
                }
                else
                {
                    // Eliminate v1 or v2.
                    //if (Vector3.Dot(v3, cross) > 0)
                    if (v3.X * cross.X + v3.Y * cross.Y + v3.Z * cross.Z > 0)
                    {
                        v2  = v4;
                        v2A = v4A;
                        v2B = v4B;
                    }
                    else
                    {
                        v1  = v4;
                        v1A = v4A;
                        v1B = v4B;
                    }
                }
            }
        }
 /// <summary>
 /// Initializes a new instance of the <see cref="RayTriangleAlgorithm"/> class.
 /// </summary>
 /// <param name="collisionDetection">The collision detection service.</param>
 public RayTriangleAlgorithm(CollisionDetection collisionDetection)
     : base(collisionDetection)
 {
     _gjk = new Gjk(collisionDetection);
 }