C++ Simplex::getSimplex方法代码示例

// Compute the penetration depth with the EPA algorithm.
/// This method computes the penetration depth and contact points between two
/// enlarged objects (with margin) where the original objects (without margin)
/// intersect. An initial simplex that contains origin has been computed with
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
/// the correct penetration depth
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
                                                           ProxyShape* collisionShape1,
                                                           const Transform& transform1,
                                                           ProxyShape* collisionShape2,
                                                           const Transform& transform2,
                                                           Vector3& v, ContactPointInfo*& contactInfo) {

    Vector3 suppPointsA[MAX_SUPPORT_POINTS];  // Support points of object A in local coordinates
    Vector3 suppPointsB[MAX_SUPPORT_POINTS];  // Support points of object B in local coordinates
    Vector3 points[MAX_SUPPORT_POINTS];       // Current points
    TrianglesStore triangleStore;             // Store the triangles
    TriangleEPA* triangleHeap[MAX_FACETS];    // Heap that contains the face
                                              // candidate of the EPA algorithm

    // Transform a point from local space of body 2 to local
    // space of body 1 (the GJK algorithm is done in local space of body 1)
    Transform body2Tobody1 = transform1.getInverse() * transform2;

    // Matrix that transform a direction from local
    // space of body 1 into local space of body 2
    Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
                              transform1.getOrientation();

    // Get the simplex computed previously by the GJK algorithm
    unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);

    // Compute the tolerance
    decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();

    // Number of triangles in the polytope
    unsigned int nbTriangles = 0;

    // Clear the storing of triangles
    triangleStore.clear();

    // Select an action according to the number of points in the simplex
    // computed with GJK algorithm in order to obtain an initial polytope for
    // The EPA algorithm.
    switch(nbVertices) {
        case 1:
            // Only one point in the simplex (which should be the origin).
            // We have a touching contact with zero penetration depth.
            // We drop that kind of contact. Therefore, we return false
            return false;

        case 2: {
            // The simplex returned by GJK is a line segment d containing the origin.
            // We add two additional support points to construct a hexahedron (two tetrahedron
            // glued together with triangle faces. The idea is to compute three different vectors
            // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively
            // rotated of 120 degree around the d segment. The the three new points to
            // construct the polytope are the three support points in those three directions
            // v1, v2 and v3.

            // Direction of the segment
            Vector3 d = (points[1] - points[0]).getUnit();

            // Choose the coordinate axis from the minimal absolute component of the vector d
            int minAxis = d.getAbsoluteVector().getMinAxis();

            // Compute sin(60)
            const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5);

            // Create a rotation quaternion to rotate the vector v1 to get the vectors
            // v2 and v3
            Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5);

            // Compute the vector v1, v2, v3
            Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
            Vector3 v2 = rotationQuat * v1;
            Vector3 v3 = rotationQuat * v2;

            // Compute the support point in the direction of v1
            suppPointsA[2] = collisionShape1->getLocalSupportPointWithMargin(v1);
            suppPointsB[2] = body2Tobody1 *
                       collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1));
            points[2] = suppPointsA[2] - suppPointsB[2];

            // Compute the support point in the direction of v2
            suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(v2);
            suppPointsB[3] = body2Tobody1 *
                     collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2));
            points[3] = suppPointsA[3] - suppPointsB[3];

            // Compute the support point in the direction of v3
            suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(v3);
            suppPointsB[4] = body2Tobody1 *
                            collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3));
            points[4] = suppPointsA[4] - suppPointsB[4];

            // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
            // tetrahedron that contains the origin in order that the initial polytope of the
            // EPA algorithm is a tetrahedron, which is simpler to deal with.

//.........这里部分代码省略.........