예제 #1
0
        internal unsafe void SetAsTriangle(float3 v0, float3 v1, float3 v2)
        {
            m_Header.Type     = ColliderType.Triangle;
            m_Header.Version += 1;

            ConvexHull.VerticesBlob.Length          = 3;
            ConvexHull.FaceVertexIndicesBlob.Length = 6;

            fixed(PolygonCollider *collider = &this)
            {
                float3 *vertices = (float3 *)(&collider->m_Vertices[0]);

                vertices[0] = v0;
                vertices[1] = v1;
                vertices[2] = v2;

                ConvexHull.Face *faces = (ConvexHull.Face *)(&collider->m_Faces[0]);
                faces[0] = new ConvexHull.Face {
                    FirstIndex = 0, NumVertices = 3, MinHalfAngleCompressed = 0xff
                };
                faces[1] = new ConvexHull.Face {
                    FirstIndex = 3, NumVertices = 3, MinHalfAngleCompressed = 0xff
                };

                byte *index = &collider->m_FaceVertexIndices[0];

                *index++ = 0; *index++ = 1; *index++ = 2;
                *index++ = 2; *index++ = 1; *index++ = 0;
            }

            SetPlanes();
        }
예제 #2
0
        // Tries to generate a manifold between a face and an edge.  It can fail for the same reasons as FaceFace().
        // In those cases, FaceEdge() returns false and the caller should generate a contact from the closest points on the shapes.
        private static unsafe bool FaceEdge(
            ref ConvexHull faceConvexA, ref ConvexHull edgeConvexB, int faceIndexA, MTransform worldFromA, MTransform aFromB,
            float3 normal, float distance, ref Manifold manifold)
        {
            // Check if the face is nearly perpendicular to the normal
            const float cosMaxAngle = 0.05f;
            Plane       planeA      = faceConvexA.Planes[faceIndexA];
            float       dotA        = math.dot(planeA.Normal, normal);

            if (math.abs(dotA) < cosMaxAngle)
            {
                return(false);
            }

            // Check if the manifold gets a point roughly as close as the closest
            distance += closestDistanceTolerance;
            bool foundClosestPoint = false;

            // Get the supporting face on A
            ConvexHull.Face faceA    = faceConvexA.Faces[faceIndexA];
            byte *          indicesA = faceConvexA.FaceVertexIndicesPtr + faceA.FirstIndex;

            // Get edge in B
            float3 vertexB0 = Math.Mul(aFromB, edgeConvexB.Vertices[0]);
            float3 edgeB    = math.mul(aFromB.Rotation, edgeConvexB.Vertices[1] - edgeConvexB.Vertices[0]);

            // For each edge of A
            float3 *verticesA  = faceConvexA.VerticesPtr;
            float   fracEnterB = 0.0f;
            float   fracExitB  = 1.0f;

            for (EdgeIterator edgeA = EdgeIterator.Begin(verticesA, indicesA, -normal, faceA.NumVertices); edgeA.Valid(); edgeA.Advance())
            {
                // Cast edge B against plane A
                castRayPlane(vertexB0, edgeB, edgeA.Perp, edgeA.Offset, ref fracEnterB, ref fracExitB);
            }

            // If edge B hits A, add a contact points
            if (fracEnterB < fracExitB)
            {
                float invDotA       = math.rcp(dotA);
                float sumRadii      = faceConvexA.ConvexRadius + edgeConvexB.ConvexRadius;
                float distance0     = (math.dot(vertexB0, planeA.Normal) + planeA.Distance) * -invDotA;
                float deltaDistance = math.dot(edgeB, planeA.Normal) * -invDotA;
                foundClosestPoint |= AddEdgeContact(vertexB0, edgeB, distance0, deltaDistance, fracEnterB, normal, edgeConvexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
                foundClosestPoint |= AddEdgeContact(vertexB0, edgeB, distance0, deltaDistance, fracExitB, normal, edgeConvexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
            }

            return(foundClosestPoint);
        }
예제 #3
0
        // Tries to generate a manifold between a pair of faces.  It can fail in some cases due to numerical accuracy:
        // 1) both faces are nearly perpendicular to the normal
        // 2) the closest features on the shapes are vertices, so that the intersection of the projection of the faces to the plane perpendicular to the normal contains only one point
        // In those cases, FaceFace() returns false and the caller should generate a contact from the closest points on the shapes.
        private static unsafe bool FaceFace(
            ref ConvexHull convexA, ref ConvexHull convexB, int faceIndexA, int faceIndexB, MTransform worldFromA, MTransform aFromB,
            float3 normal, float distance, ref Manifold manifold)
        {
            // Get the plane of each face
            Plane planeA = convexA.Planes[faceIndexA];
            Plane planeB = TransformPlane(aFromB, convexB.Planes[faceIndexB]);

            // Handle cases where one of the faces is nearly perpendicular to the contact normal
            // This gets around divide by zero / numerical problems from dividing collider planes which often contain some error by a very small number, amplifying that error
            const float cosMaxAngle = 0.05f;
            float       dotA        = math.dot(planeA.Normal, normal);
            float       dotB        = math.dot(planeB.Normal, normal);
            bool        acceptB     = true; // true if vertices of B projected onto the face of A are accepted

            if (dotA > -cosMaxAngle)
            {
                // Handle cases where both faces are nearly perpendicular to the contact normal.
                if (dotB < cosMaxAngle)
                {
                    // Both faces are nearly perpendicular to the contact normal, let the caller generate a single contact
                    return(false);
                }

                // Face of A is nearly perpendicular to the contact normal, don't try to project vertices onto it
                acceptB = false;
            }
            else if (dotB < cosMaxAngle)
            {
                // Face of B is nearly perpendicular to the normal, so we need to clip the edges of B against face A instead
                MTransform bFromA     = Inverse(aFromB);
                float3     normalInB  = math.mul(bFromA.Rotation, -normal);
                MTransform worldFromB = Mul(worldFromA, aFromB);
                bool       result     = FaceFace(ref convexB, ref convexA, faceIndexB, faceIndexA, worldFromB, bFromA, normalInB, distance, ref manifold);
                manifold.Normal = -manifold.Normal;
                manifold.Flip();
                return(result);
            }

            // Check if the manifold gets a point roughly as close as the closest
            distance += closestDistanceTolerance;
            bool foundClosestPoint = false;

            // Transform vertices of B into A-space
            // Initialize validB, which is true for each vertex of B that is inside face A
            ConvexHull.Face faceA        = convexA.Faces[faceIndexA];
            ConvexHull.Face faceB        = convexB.Faces[faceIndexB];
            bool *          validB       = stackalloc bool[faceB.NumVertices];
            float3 *        verticesBinA = stackalloc float3[faceB.NumVertices];
            {
                byte *  indicesB  = convexB.FaceVertexIndicesPtr + faceB.FirstIndex;
                float3 *verticesB = convexB.VerticesPtr;
                for (int i = 0; i < faceB.NumVertices; i++)
                {
                    validB[i]       = acceptB;
                    verticesBinA[i] = Mul(aFromB, verticesB[indicesB[i]]);
                }
            }

            // For each edge of A
            float   invDotB   = math.rcp(dotB);
            float   sumRadii  = convexA.ConvexRadius + convexB.ConvexRadius;
            byte *  indicesA  = convexA.FaceVertexIndicesPtr + faceA.FirstIndex;
            float3 *verticesA = convexA.VerticesPtr;

            for (EdgeIterator edgeA = EdgeIterator.Begin(verticesA, indicesA, -normal, faceA.NumVertices); edgeA.Valid(); edgeA.Advance())
            {
                float fracEnterA = 0.0f;
                float fracExitA  = 1.0f;

                // For each edge of B
                for (EdgeIterator edgeB = EdgeIterator.Begin(verticesBinA, null, normal, faceB.NumVertices); edgeB.Valid(); edgeB.Advance())
                {
                    // Cast edge A against plane B and test if vertex B is inside plane A
                    castRayPlane(edgeA.Vertex0, edgeA.Edge, edgeB.Perp, edgeB.Offset, ref fracEnterA, ref fracExitA);
                    validB[edgeB.Index] &= (math.dot(edgeB.Vertex1, edgeA.Perp) < edgeA.Offset);
                }

                // If edge A hits B, add a contact points
                if (fracEnterA < fracExitA)
                {
                    float  distance0     = (math.dot(edgeA.Vertex0, planeB.Normal) + planeB.Distance) * invDotB;
                    float  deltaDistance = math.dot(edgeA.Edge, planeB.Normal) * invDotB;
                    float3 vertexAOnB    = edgeA.Vertex0 - normal * distance0;
                    float3 edgeAOnB      = edgeA.Edge - normal * deltaDistance;
                    foundClosestPoint |= AddEdgeContact(vertexAOnB, edgeAOnB, distance0, deltaDistance, fracEnterA, normal, convexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
                    if (fracExitA < 1.0f) // If the exit fraction is 1, then the next edge has the same contact point with enter fraction 0
                    {
                        foundClosestPoint |= AddEdgeContact(vertexAOnB, edgeAOnB, distance0, deltaDistance, fracExitA, normal, convexB.ConvexRadius, sumRadii, worldFromA, distance, ref manifold);
                    }
                }
            }

            // For each vertex of B
            float invDotA = math.rcp(dotA);

            for (int i = 0; i < faceB.NumVertices; i++)
            {
                if (validB[i] && manifold.NumContacts < Manifold.k_MaxNumContacts)
                {
                    float3 vertexB   = verticesBinA[i];
                    float  distanceB = (math.dot(vertexB, planeA.Normal) + planeA.Distance) * -invDotA;
                    manifold[manifold.NumContacts++] = new ContactPoint
                    {
                        Position = Mul(worldFromA, vertexB) + manifold.Normal * convexB.ConvexRadius,
                        Distance = distanceB - sumRadii
                    };
                    foundClosestPoint |= distanceB <= distance;
                }
            }

            return(foundClosestPoint);
        }