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

void CollisionSolver::UpdateDiagnostics(const Simplex& simplex, 
                                        const D3DXVECTOR3& furthestPoint)
{
    if(m_engine->diagnostic()->AllowDiagnostics(Diagnostic::COLLISION))
    {
        const float radius = 0.1f;
        const float normalLength = 1.5f;
        D3DXVECTOR3 origin(0.0, 0.0, 0.0);

        m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION,
            "OriginPoint", Diagnostic::WHITE, origin, radius);

        m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION, 
            "FurthestPoint", Diagnostic::MAGENTA, furthestPoint, radius);

        const auto& borders = simplex.GetBorderEdges();
        for(unsigned int i = 0; i < borders.size(); ++i)
        {
            m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
                "BorderEdge" + StringCast(i), Diagnostic::RED, 
                simplex.GetPoint(borders[i].indices[0]), 
                simplex.GetPoint(borders[i].indices[1]));                    
        }

        const auto& faces = simplex.GetFaces();
        for(unsigned int i = 0; i < faces.size(); ++i)
        {
            if(faces[i].alive)
            {
                std::string id = StringCast(i);
                const Face& face = faces[i];

                const D3DXVECTOR3 center = simplex.GetFaceCenter(i);
                const D3DXVECTOR3& normal = face.normal * normalLength;
                const D3DXVECTOR3& pointA = simplex.GetPoint(face.indices[0]);
                const D3DXVECTOR3& pointB = simplex.GetPoint(face.indices[1]);
                const D3DXVECTOR3& pointC = simplex.GetPoint(face.indices[2]);

                m_engine->diagnostic()->UpdateSphere(Diagnostic::COLLISION, 
                    "sCenter" + id, Diagnostic::BLUE, center, radius);

                m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION, 
                    "sNormal" + id, Diagnostic::BLUE, center, center + normal);

                m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION, 
                    "sLine1" + id, Diagnostic::YELLOW, pointA, pointB);

                m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION, 
                    "sLine2" + id, Diagnostic::YELLOW, pointA, pointC);

                m_engine->diagnostic()->UpdateLine(Diagnostic::COLLISION,
                    "sLine3" + id, Diagnostic::YELLOW, pointC, pointB);
            }
        }
    }
}

bool CollisionSolver::SolveTetrahedronSimplex(Simplex& simplex, D3DXVECTOR3& direction)
{
    const D3DXVECTOR3& pointA = simplex.GetPoint(3);
    const D3DXVECTOR3& pointB = simplex.GetPoint(0);
    const D3DXVECTOR3& pointC = simplex.GetPoint(1);
    const D3DXVECTOR3& pointD = simplex.GetPoint(2);

    const D3DXVECTOR3 AB = pointB - pointA;
    const D3DXVECTOR3 AC = pointC - pointA;
    const D3DXVECTOR3 AD = pointD - pointA;
    const D3DXVECTOR3 AO = -pointA;

    // Check if within the three surrounding planes
    // The forth plane has been previously tested with the plane simplex
    // All normals will point to the center of the tetrahedron
    D3DXVECTOR3 CBnormal, BDnormal, DCnormal;
    D3DXVec3Cross(&CBnormal, &AC, &AB);
    D3DXVec3Cross(&BDnormal, &AB, &AD);
    D3DXVec3Cross(&DCnormal, &AD, &AC);

    const float CBdistance = D3DXVec3Dot(&CBnormal, &AO);
    const float BDdistance = D3DXVec3Dot(&BDnormal, &AO);
    const float DCdistance = D3DXVec3Dot(&DCnormal, &AO);

    bool originInsideSimplex = true;
    if(CBdistance < 0.0f)
    {
        // Origin is outside of the CB plane
        // D is furthest point, remove it and search towards the origin
        simplex.RemovePoint(pointD);
        direction = -CBnormal;
        originInsideSimplex = false;
    }
    else if(BDdistance < 0.0f)
    {
        // Origin is outside of the BD plane
        // C is furthest point, remove it and search towards the origin
        simplex.RemovePoint(pointC);
        direction = -BDnormal;
        originInsideSimplex = false;
    }
    else if(DCdistance < 0.0f)
    {
        // Origin is outside of the DC plane
        // C is furthest point, remove it and search towards the origin
        simplex.RemovePoint(pointB);
        direction = -DCnormal;
        originInsideSimplex = false;
    }
    return originInsideSimplex;
}

void CollisionSolver::SolvePlaneSimplex(Simplex& simplex, D3DXVECTOR3& direction)
{
    const D3DXVECTOR3& pointA = simplex.GetPoint(2);
    const D3DXVECTOR3& pointB = simplex.GetPoint(0);
    const D3DXVECTOR3& pointC = simplex.GetPoint(1);

    const D3DXVECTOR3 AB = pointB - pointA;
    const D3DXVECTOR3 AC = pointC - pointA;
    const D3DXVECTOR3 AO = -pointA;
                
    // Determine which side of the plane the origin is on
    D3DXVECTOR3 planeNormal;
    D3DXVec3Cross(&planeNormal, &AB, &AC);

    // Determine the new search direction towards the origin
    const float distanceToPlane = D3DXVec3Dot(&planeNormal, &AO);
    direction = (distanceToPlane < 0.0f) ? -planeNormal : planeNormal;
}

void CollisionSolver::SolveLineSimplex(const Simplex& simplex, D3DXVECTOR3& direction)
{
    const D3DXVECTOR3& pointA = simplex.GetPoint(1);
    const D3DXVECTOR3& pointB = simplex.GetPoint(0);
    const D3DXVECTOR3 AB = pointB - pointA;
    const D3DXVECTOR3 AO = -pointA;

    // Generate a new direction for the next point 
    // perpendicular to the line using triple product
    D3DXVECTOR3 ABcrossAO;
    D3DXVec3Cross(&ABcrossAO, &AB, &AO);
    D3DXVec3Cross(&direction, &ABcrossAO, &AB);
}

D3DXVECTOR3 CollisionSolver::GetConvexHullPenetration(const CollisionMesh& particle, 
                                                      const CollisionMesh& hull, 
                                                      Simplex& simplex)
{
    D3DXVECTOR3 furthestPoint;
    D3DXVECTOR3 penetrationDirection;
    float penetrationDistance = 0.0f;
    bool penetrationFound = false;
    const float minDistance = 0.1f;
    const int maxIterations = 10;
    int iteration = 0;

    while(!penetrationFound && iteration < maxIterations)
    {
        ++iteration;
        const Face& face = simplex.GetClosestFaceToOrigin();
        penetrationDirection = face.normal;
        penetrationDistance = face.distanceToOrigin;
        penetrationFound = penetrationDistance == 0.0f;

        if(!penetrationFound)
        {
            // Check if there are any edge points beyond the closest face
            furthestPoint = GetMinkowskiSumEdgePoint(face.normal, particle, hull);
            const D3DXVECTOR3 faceToPoint = furthestPoint - simplex.GetPoint(face.indices[0]);
            const float distance = fabs(D3DXVec3Dot(&faceToPoint, &face.normal));
            penetrationFound = distance < minDistance;

            if(!penetrationFound)
            {
                // Add the new point and extend the convex hull
                simplex.ExtendFace(furthestPoint);
            }
        }
    }

    if(!penetrationFound)
    {
        // Fallback on the initial closest face
        const Face& face = simplex.GetClosestFaceToOrigin();
        penetrationDirection = face.normal;
        penetrationDistance = face.distanceToOrigin;
    }

    if(particle.RenderSolverDiagnostics())
    {
        UpdateDiagnostics(simplex, furthestPoint);
    }

    return -(penetrationDirection * penetrationDistance);
}