GuEPAFacet.h

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2022 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
 
#ifndef GU_EPA_FACET_H
#define GU_EPA_FACET_H
 
#include "foundation/PxVecMath.h"
#include "foundation/PxFPU.h"
#include "foundation/PxUtilities.h"
#include "CmIDPool.h"
 
#if (defined __GNUC__ && defined _DEBUG)
#define PX_EPA_FORCE_INLINE
#else
#define PX_EPA_FORCE_INLINE PX_FORCE_INLINE
#endif
 
#define EPA_DEBUG   0
 
namespace physx
{
#define MaxEdges 32
#define MaxFacets 64
#define MaxSupportPoints 64
 
namespace Gu
{
    const PxU32 lookUp[3] = {1, 2, 0};
    
    PX_FORCE_INLINE PxU32 incMod3(PxU32 i) { return lookUp[i]; } 
    
    class EdgeBuffer;
    class Edge;
    typedef Cm::InlineDeferredIDPool<MaxFacets> EPAFacetManager;
 
    class Facet 
    {
    public:
    
        Facet()
        {
        }
 
        PX_FORCE_INLINE Facet(const PxU32 _i0, const PxU32 _i1, const PxU32 _i2)
            : m_obsolete(false), m_inHeap(false)
        {
            m_indices[0]= PxToI8(_i0);
            m_indices[1]= PxToI8(_i1);
            m_indices[2]= PxToI8(_i2);
             
            m_adjFacets[0] = m_adjFacets[1] = m_adjFacets[2] = NULL;
            m_adjEdges[0] = m_adjEdges[1] = m_adjEdges[2] = -1;
        }
 
 
        PX_FORCE_INLINE void invalidate()
        {
            m_adjFacets[0] = m_adjFacets[1] = m_adjFacets[2] = NULL;
            m_adjEdges[0] = m_adjEdges[1] = m_adjEdges[2] = -1;
        }
 
        PX_FORCE_INLINE bool Valid()
        {
            return (m_adjFacets[0] != NULL) & (m_adjFacets[1] != NULL) & (m_adjFacets[2] != NULL);
        }
 
        PX_FORCE_INLINE PxU32 operator[](const PxU32 i) const 
        { 
            return PxU32(m_indices[i]);
        } 
 
        //create ajacency information
        bool link(const PxU32 edge0, Facet* PX_RESTRICT  facet, const PxU32 edge1);
 
        PX_FORCE_INLINE bool isObsolete() const { return m_obsolete; }
 
        //calculate the signed distance from a point to a plane
        PX_FORCE_INLINE aos::FloatV getPlaneDist(const aos::Vec3VArg p, const aos::Vec3V* PX_RESTRICT aBuf, const aos::Vec3V* PX_RESTRICT bBuf) const; 
 
        //check to see whether the triangle is a valid triangle, calculate plane normal and plane distance at the same time
        aos::BoolV isValid2(const PxU32 i0, const PxU32 i1, const PxU32 i2, const aos::Vec3V* PX_RESTRICT aBuf, const aos::Vec3V* PX_RESTRICT bBuf, 
        const aos::FloatVArg upper);
 
        //return the absolute value for the plane distance from origin 
        PX_FORCE_INLINE aos::FloatV getPlaneDist() const 
        { 
            return aos::FLoad(m_planeDist);
        }
 
        //return the plane normal
        PX_FORCE_INLINE aos::Vec3V getPlaneNormal()const
        {
            return m_planeNormal; 
        }
 
        //calculate the closest points for a shape pair
        void getClosestPoint(const aos::Vec3V* PX_RESTRICT aBuf, const aos::Vec3V* PX_RESTRICT bBuf, aos::Vec3V& closestA, 
            aos::Vec3V& closestB);
 
        //calculate the closest points for a shape pair
        //void getClosestPoint(const aos::Vec3V* PX_RESTRICT aBuf, const aos::Vec3V* PX_RESTRICT bBuf, aos::FloatV& v, aos::FloatV& w);
        
        //performs a flood fill over the boundary of the current polytope. 
        void silhouette(const aos::Vec3VArg w, const aos::Vec3V* PX_RESTRICT aBuf, const aos::Vec3V* PX_RESTRICT bBuf, EdgeBuffer& edgeBuffer, EPAFacetManager& manager);
        
 
        //m_planeDist is positive
        bool operator <(const Facet& b) const
        {
            return m_planeDist < b.m_planeDist;
        }
     
        //store all the boundary facets for the new polytope in the edgeBuffer and free indices when an old facet isn't part of the boundary anymore
        PX_FORCE_INLINE void silhouette(const PxU32 index, const aos::Vec3VArg w, const aos::Vec3V* PX_RESTRICT aBuf, const aos::Vec3V* PX_RESTRICT bBuf, EdgeBuffer& edgeBuffer, 
            EPAFacetManager& manager);
 
        aos::Vec3V m_planeNormal;                                                                                                       //16
        PxF32 m_planeDist;
#if EPA_DEBUG
        PxF32 m_lambda1;
        PxF32 m_lambda2;
#endif
        
        Facet* PX_RESTRICT m_adjFacets[3]; //the triangle adjacent to edge i in this triangle                                               //32
        PxI8 m_adjEdges[3]; //the edge connected with the corresponding triangle                                                            //35
        PxI8 m_indices[3]; //the index of vertices of the triangle                                                                          //38
        bool m_obsolete; //a flag to denote whether the triangle are still part of the bundeary of the new polytope                         //39
        bool m_inHeap;  //a flag to indicate whether the triangle is in the heap                                                            //40
        PxU8 m_FacetId;                                                                                                                     //41                                                                                                    //73
 
    };
 
    
    class Edge 
    {
    public:
        PX_FORCE_INLINE Edge() {}
        PX_FORCE_INLINE Edge(Facet * PX_RESTRICT facet, const PxU32 index) : m_facet(facet), m_index(index) {}
        PX_FORCE_INLINE Edge(const Edge& other) : m_facet(other.m_facet), m_index(other.m_index){}
 
        PX_FORCE_INLINE Edge& operator = (const Edge& other)
        {
            m_facet = other.m_facet;
            m_index = other.m_index;
            return *this;
        }
 
        PX_FORCE_INLINE Facet *getFacet() const { return m_facet; }
        PX_FORCE_INLINE PxU32 getIndex() const { return m_index; }
 
        //get out the associated start vertex index in this edge from the facet
        PX_FORCE_INLINE PxU32 getSource() const
        {
            PX_ASSERT(m_index < 3);
            return (*m_facet)[m_index];
        }
 
        //get out the associated end vertex index in this edge from the facet
        PX_FORCE_INLINE PxU32 getTarget() const
        {
            PX_ASSERT(m_index < 3);
            return (*m_facet)[incMod3(m_index)];
        }
 
        Facet* PX_RESTRICT m_facet;
        PxU32 m_index;
    };
 
 
    class EdgeBuffer
    {
    public:
        EdgeBuffer() : m_Size(0), m_OverFlow(false)
        {
        }
 
        Edge* Insert(Facet* PX_RESTRICT  facet, const PxU32 index)
        {
            if (m_Size < MaxEdges)
            {
                Edge* pEdge = &m_pEdges[m_Size++];
                pEdge->m_facet = facet;
                pEdge->m_index = index;
                return pEdge;
            }
            m_OverFlow = true;
            return NULL;
        }
 
        Edge* Get(const PxU32 index)
        {
            PX_ASSERT(index < m_Size);
            return &m_pEdges[index];
        }
 
        PxU32 Size()
        {
            return m_Size;
        }
 
        bool IsValid()
        {
            return m_Size > 0 && !m_OverFlow;
        }
 
        void MakeEmpty()
        {
            m_Size = 0;
            m_OverFlow = false;
        }
 
        Edge    m_pEdges[MaxEdges];
        PxU32   m_Size;
        bool    m_OverFlow;
    };
 
    //ML: calculate MTD points for a shape pair
    PX_FORCE_INLINE void Facet::getClosestPoint(const aos::Vec3V* PX_RESTRICT aBuf, const aos::Vec3V* PX_RESTRICT bBuf, aos::Vec3V& closestA, 
        aos::Vec3V& closestB)
    {
        using namespace aos;
 
        const Vec3V pa0(aBuf[m_indices[0]]);
        const Vec3V pa1(aBuf[m_indices[1]]);
        const Vec3V pa2(aBuf[m_indices[2]]);
 
        const Vec3V pb0(bBuf[m_indices[0]]);
        const Vec3V pb1(bBuf[m_indices[1]]);
        const Vec3V pb2(bBuf[m_indices[2]]);
 
        const Vec3V p0 = V3Sub(pa0, pb0);
        const Vec3V p1 = V3Sub(pa1, pb1);
        const Vec3V p2 = V3Sub(pa2, pb2);
 
        const Vec3V v0 = V3Sub(p1, p0);
        const Vec3V v1 = V3Sub(p2, p0);
 
        const Vec3V closestP = V3Scale(m_planeNormal, FLoad(m_planeDist));
        const Vec3V v2 = V3Sub(closestP, p0);
        //calculate barycentric coordinates
        const FloatV d00 = V3Dot(v0, v0);
        const FloatV d01 = V3Dot(v0, v1);
        const FloatV d11 = V3Dot(v1, v1);
 
        const FloatV d20 = V3Dot(v2, v0);
        const FloatV d21 = V3Dot(v2, v1);
 
        const FloatV det = FNegScaleSub(d01, d01, FMul(d00, d11));//FSub( FMul(v1dv1, v2dv2), FMul(v1dv2, v1dv2) ); // non-negative
        const FloatV recip = FSel(FIsGrtr(det, FEps()), FRecip(det), FZero());
 
        const FloatV lambda1 = FMul(FNegScaleSub(d01, d21, FMul(d11, d20)), recip);
        const FloatV lambda2 = FMul(FNegScaleSub(d01, d20, FMul(d00, d21)), recip);
 
#if EPA_DEBUG
        FStore(lambda1, &m_lambda1);
        FStore(lambda2, &m_lambda2);
#endif
 
        const FloatV u = FSub(FOne(), FAdd(lambda1, lambda2));
        closestA = V3ScaleAdd(pa0, u, V3ScaleAdd(pa1, lambda1, V3Scale(pa2, lambda2)));
        closestB = V3ScaleAdd(pb0, u, V3ScaleAdd(pb1, lambda1, V3Scale(pb2, lambda2)));
    }
 
    //ML: create adjacency informations for both facets
    PX_FORCE_INLINE bool Facet::link(const PxU32 edge0, Facet * PX_RESTRICT facet, const PxU32 edge1) 
    {
        m_adjFacets[edge0] = facet;
        m_adjEdges[edge0] = PxToI8(edge1);
        facet->m_adjFacets[edge1] = this;
        facet->m_adjEdges[edge1] = PxToI8(edge0);
 
        return (m_indices[edge0] == facet->m_indices[incMod3(edge1)]) && (m_indices[incMod3(edge0)] == facet->m_indices[edge1]);
    } 
 
}
 
}
 
#endif