GuWindingNumberT.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_WINDING_NUMBER_T_H
#define GU_WINDING_NUMBER_T_H
 
#include "GuTriangle.h"
#include "foundation/PxArray.h"
#include "foundation/PxHashMap.h"
#include "foundation/PxVec3.h"
#include "GuBVH.h"
#include "GuAABBTreeQuery.h"
#include "GuAABBTreeNode.h"
#include "GuWindingNumberCluster.h"
 
namespace physx
{
namespace Gu
{
    using Triangle = Gu::IndexedTriangleT<PxI32>;
    
    template<typename R, typename V3>
    struct SecondOrderClusterApproximationT : public ClusterApproximationT<R, V3>
    {
        PxMat33 WeightedOuterProductSum;
 
        PX_FORCE_INLINE SecondOrderClusterApproximationT() {}
 
        PX_FORCE_INLINE SecondOrderClusterApproximationT(R radius, R areaSum, const V3& weightedCentroid, const V3& weightedNormalSum, const PxMat33& weightedOuterProductSum) :
            ClusterApproximationT<R, V3>(radius, areaSum, weightedCentroid, weightedNormalSum), WeightedOuterProductSum(weightedOuterProductSum)
        { }
    };
       
    //Evaluates a first order winding number approximation for a given cluster (cluster = bunch of triangles)
    template<typename R, typename V3>
    PX_FORCE_INLINE R firstOrderClusterApproximation(const V3& weightedCentroid, const V3& weightedNormalSum,
        const V3& evaluationPoint)
    {
        const V3 dir = weightedCentroid - evaluationPoint;
        const R l = dir.magnitude();
        return (R(0.25 / 3.141592653589793238462643383) / (l * l * l)) * weightedNormalSum.dot(dir);
    }
    template<typename R, typename V3>
    PX_FORCE_INLINE R clusterApproximation(const ClusterApproximationT<R, V3>& c, const V3& evaluationPoint)
    {
        return firstOrderClusterApproximation(c.WeightedCentroid, c.WeightedNormalSum, evaluationPoint);
    }
 
    //Evaluates a second order winding number approximation for a given cluster (cluster = bunch of triangles)
    template<typename R, typename V3>
    PX_FORCE_INLINE R secondOrderClusterApproximation(const V3& weightedCentroid, const V3& weightedNormalSum,
        const PxMat33& weightedOuterProductSum, const V3& evaluationPoint)
    {
        const V3 dir = weightedCentroid - evaluationPoint;
        const R l = dir.magnitude();
        const R l2 = l * l;
        const R scaling = R(0.25 / 3.141592653589793238462643383) / (l2 * l);
        const R firstOrder = scaling * weightedNormalSum.dot(dir);
 
        const R scaling2 = -R(3.0) * scaling / l2;
        const R m11 = scaling + scaling2 * dir.x * dir.x, m12 = scaling2 * dir.x * dir.y, m13 = scaling2 * dir.x * dir.z;
        const R m21 = scaling2 * dir.y * dir.x, m22 = scaling + scaling2 * dir.y * dir.y, m23 = scaling2 * dir.y * dir.z;
        const R m31 = scaling2 * dir.z * dir.x, m32 = scaling2 * dir.z * dir.y, m33 = scaling + scaling2 * dir.z * dir.z;
 
        return firstOrder + (weightedOuterProductSum.column0.x * m11 + weightedOuterProductSum.column1.x * m12 + weightedOuterProductSum.column2.x * m13 +
            weightedOuterProductSum.column0.y * m21 + weightedOuterProductSum.column1.y * m22 + weightedOuterProductSum.column2.y * m23 +
            weightedOuterProductSum.column0.z * m31 + weightedOuterProductSum.column1.z * m32 + weightedOuterProductSum.column2.z * m33);
    }
    template<typename R, typename V3>
    PX_FORCE_INLINE R clusterApproximation(const SecondOrderClusterApproximationT<R, V3>& c, const V3& evaluationPoint)
    {
        return secondOrderClusterApproximation(c.WeightedCentroid, c.WeightedNormalSum, c.WeightedOuterProductSum, evaluationPoint);
    }
 
    //Computes parameters to approximately represent a cluster (cluster = bunch of triangles) to be used to compute a winding number approximation
    template<typename R, typename V3>
    void approximateCluster(const PxArray<PxI32>& triangleSet, PxU32 start, PxU32 end, const PxU32* triangles, const V3* points,
        const PxArray<R>& triangleAreas, const PxArray<V3>& triangleNormalsTimesTriangleArea, const PxArray<V3>& triangleCentroids, ClusterApproximationT<R, V3>& cluster)
    {
        V3 weightedCentroid(0., 0., 0.);
        R areaSum = 0;
        V3 weightedNormalSum(0., 0., 0.);
 
        for (PxU32 i = start; i < end; ++i)
        {
            PxI32 triId = triangleSet[i];
            areaSum += triangleAreas[triId];
            weightedCentroid += triangleCentroids[triId] * triangleAreas[triId];
            weightedNormalSum += triangleNormalsTimesTriangleArea[triId];
        }
        weightedCentroid = weightedCentroid / areaSum;
 
        R radiusSquared = 0;
        for (PxU32 i = start; i < end; ++i)
        {
            PxI32 triId = triangleSet[i];
            const PxU32* tri = &triangles[3 * triId];
            R d2 = (weightedCentroid - points[tri[0]]).magnitudeSquared();
            if (d2 > radiusSquared) radiusSquared = d2;
            d2 = (weightedCentroid - points[tri[1]]).magnitudeSquared();
            if (d2 > radiusSquared) radiusSquared = d2;
            d2 = (weightedCentroid - points[tri[2]]).magnitudeSquared();
            if (d2 > radiusSquared) radiusSquared = d2;
        }
        cluster = ClusterApproximationT<R, V3>(PxSqrt(radiusSquared), areaSum, weightedCentroid, weightedNormalSum/*, weightedOuterProductSum*/);
    }
 
    //Computes parameters to approximately represent a cluster (cluster = bunch of triangles) to be used to compute a winding number approximation
    template<typename R, typename V3>
    void approximateCluster(const PxArray<PxI32>& triangleSet, PxU32 start, PxU32 end, const PxU32* triangles, const V3* points,
        const PxArray<R>& triangleAreas, const PxArray<V3>& triangleNormalsTimesTriangleArea, const PxArray<V3>& triangleCentroids, SecondOrderClusterApproximationT<R, V3>& cluster)
    {
        V3 weightedCentroid(0., 0., 0.);
        R areaSum = 0;
        V3 weightedNormalSum(0., 0., 0.);
 
        for (PxU32 i = start; i < end; ++i)
        {
            PxI32 triId = triangleSet[i];
            areaSum += triangleAreas[triId];
            weightedCentroid += triangleCentroids[triId] * triangleAreas[triId];
            weightedNormalSum += triangleNormalsTimesTriangleArea[triId];
        }
        weightedCentroid = weightedCentroid / areaSum;
 
        R radiusSquared = 0;
        PxMat33 weightedOuterProductSum(PxZERO::PxZero);
        for (PxU32 i = start; i < end; ++i)
        {
            PxI32 triId = triangleSet[i];
            const PxU32* tri = &triangles[3 * triId];
            R d2 = (weightedCentroid - points[tri[0]]).magnitudeSquared();
            if (d2 > radiusSquared) radiusSquared = d2;
            d2 = (weightedCentroid - points[tri[1]]).magnitudeSquared();
            if (d2 > radiusSquared) radiusSquared = d2;
            d2 = (weightedCentroid - points[tri[2]]).magnitudeSquared();
            if (d2 > radiusSquared) radiusSquared = d2;
 
            weightedOuterProductSum = weightedOuterProductSum + PxMat33::outer(triangleCentroids[triId] - weightedCentroid, triangleNormalsTimesTriangleArea[triId]);
        }
        cluster = SecondOrderClusterApproximationT<R, V3>(PxSqrt(radiusSquared), areaSum, weightedCentroid, weightedNormalSum, weightedOuterProductSum);
    }
 
    //Exact winding number evaluation, needs to be called for every triangle close to the winding number query point
    template<typename R, typename V3>
    PX_FORCE_INLINE R evaluateExact(V3 a, V3 b, V3 c, const V3& p)
    {
        const R twoOver4PI = R(0.5 / 3.141592653589793238462643383);
 
        a -= p;
        b -= p;
        c -= p;
 
        const R la = a.magnitude(),
            lb = b.magnitude(),
            lc = c.magnitude();
 
        const R y = a.x * b.y * c.z - a.x * b.z * c.y - a.y * b.x * c.z + a.y * b.z * c.x + a.z * b.x * c.y - a.z * b.y * c.x;
        const R x = (la * lb * lc + (a.x * b.x + a.y * b.y + a.z * b.z) * lc +
            (b.x * c.x + b.y * c.y + b.z * c.z) * la + (c.x * a.x + c.y * a.y + c.z * a.z) * lb);
        return twoOver4PI * PxAtan2(y, x);
    }
 
    struct Section
    {
        PxI32 start;
        PxI32 end;
 
        Section(PxI32 s, PxI32 e) : start(s), end(e)
        {}
    };
 
    //Helper method that recursively traverses the given BVH tree and computes a cluster approximation for every node and links it to the node
    template<typename R, typename V3>
    void precomputeClusterInformation(PxI32 nodeId, const BVHNode* tree, const PxU32* triangles, const PxU32 numTriangles,
        const V3* points, PxHashMap<PxU32, ClusterApproximationT<R, V3>>& infos, const PxArray<R> triangleAreas,
        const PxArray<V3>& triangleNormalsTimesTriangleArea, const PxArray<V3>& triangleCentroids)
    {
        PxArray<PxI32> stack;
        stack.pushBack(nodeId);
        PxArray<Section> returnStack;
 
        PxArray<PxI32> triIndices;
        triIndices.reserve(numTriangles);
        infos.reserve(PxU32(1.2f*numTriangles));
 
        while (stack.size() > 0)
        {
            nodeId = stack.popBack();
 
            if (nodeId >= 0)
            {
                const BVHNode& node = tree[nodeId];
                if (node.isLeaf())
                {
                    triIndices.pushBack(node.getPrimitiveIndex());
                    returnStack.pushBack(Section(triIndices.size() - 1, triIndices.size()));
                    continue;
                }
 
                stack.pushBack(-nodeId - 1); //Marker for return index
                stack.pushBack(node.getPosIndex());
                stack.pushBack(node.getPosIndex() + 1);
            }
            else
            {
                Section trianglesA = returnStack.popBack();
                Section trianglesB = returnStack.popBack();
                Section sum(trianglesB.start, trianglesA.end);
 
                nodeId = -nodeId - 1;
                ClusterApproximationT<R, V3> c;
                approximateCluster<R, V3>(triIndices, sum.start, sum.end, triangles, points, triangleAreas, triangleNormalsTimesTriangleArea, triangleCentroids, c);
                infos.insert(PxU32(nodeId), c);
 
                returnStack.pushBack(sum);
            }
        }
    }
 
    //Precomputes a cluster approximation for every node in the BVH tree
    template<typename R, typename V3>
    void precomputeClusterInformation(const BVHNode* tree, const PxU32* triangles, const PxU32 numTriangles,
        const V3* points, PxHashMap<PxU32, ClusterApproximationT<R, V3>>& result, PxI32 rootNodeIndex)
    {
        PxArray<R> triangleAreas;
        triangleAreas.resize(numTriangles);
        PxArray<V3> triangleNormalsTimesTriangleArea;
        triangleNormalsTimesTriangleArea.resize(numTriangles);
        PxArray<V3> triangleCentroids;
        triangleCentroids.resize(numTriangles);
 
        for (PxU32 i = 0; i < numTriangles; ++i)
        {
            const PxU32* tri = &triangles[3 * i];
            const V3& a = points[tri[0]];
            const V3& b = points[tri[1]];
            const V3& c = points[tri[2]];
            triangleNormalsTimesTriangleArea[i] = (b - a).cross(c - a) * R(0.5);
            triangleAreas[i] = triangleNormalsTimesTriangleArea[i].magnitude();
            triangleCentroids[i] = (a + b + c) * R(1.0 / 3.0);
        }
 
        result.clear();
        precomputeClusterInformation(rootNodeIndex, tree, triangles, numTriangles, points, result, triangleAreas, triangleNormalsTimesTriangleArea, triangleCentroids);
    }
 
    template<typename R, typename V3>
    class WindingNumberTraversalController
    {
    public:
        R mWindingNumber = 0;
    private:
        const PxU32* mTriangles;
        const V3* mPoints;
        const PxHashMap<PxU32, ClusterApproximationT<R, V3>>& mClusters;
        V3 mQueryPoint;
        R mDistanceThresholdBeta;
 
    public:
        PX_FORCE_INLINE WindingNumberTraversalController(const PxU32* triangles, const V3* points,
            const PxHashMap<PxU32, ClusterApproximationT<R, V3>>& clusters, const V3& queryPoint, R distanceThresholdBeta = 2)
            : mTriangles(triangles), mPoints(points), mClusters(clusters), mQueryPoint(queryPoint), mDistanceThresholdBeta(distanceThresholdBeta)
        { }
 
        PX_FORCE_INLINE Gu::TraversalControl::Enum analyze(const BVHNode& node, PxI32 nodeIndex)
        {
            if (node.isLeaf())
            {
                PX_ASSERT(node.getNbPrimitives() == 1);
                const PxU32* tri = &mTriangles[3 * node.getPrimitiveIndex()];
                mWindingNumber += evaluateExact<R, V3>(mPoints[tri[0]], mPoints[tri[1]], mPoints[tri[2]], mQueryPoint);
                return Gu::TraversalControl::eDontGoDeeper;
            }
            const ClusterApproximationT<R, V3>& cluster = mClusters.find(nodeIndex)->second;
            const R distSquared = (mQueryPoint - cluster.WeightedCentroid).magnitudeSquared();
            const R threshold = mDistanceThresholdBeta * cluster.Radius;
            if (distSquared > threshold * threshold)
            {
                //mWindingNumber += secondOrderClusterApproximation(cluster.WeightedCentroid, cluster.WeightedNormalSum, cluster.WeightedOuterProductSum, mQueryPoint);
                mWindingNumber += firstOrderClusterApproximation<R, V3>(cluster.WeightedCentroid, cluster.WeightedNormalSum, mQueryPoint); // secondOrderClusterApproximation(cluster.WeightedCentroid, cluster.WeightedNormalSum, cluster.WeightedOuterProductSum, mQueryPoint);
                return Gu::TraversalControl::eDontGoDeeper;
            }
            return Gu::TraversalControl::eGoDeeper;
        }
 
    private:
        PX_NOCOPY(WindingNumberTraversalController)
    };
 
    template<typename R, typename V3>
    R computeWindingNumber(const BVHNode* tree, const V3& q, R beta, const PxHashMap<PxU32, ClusterApproximationT<R, V3>>& clusters,
        const PxU32* triangles, const V3* points)
    {
        WindingNumberTraversalController<R, V3> c(triangles, points, clusters, q, beta);
        traverseBVH<WindingNumberTraversalController<R, V3>>(tree, c);
        return c.mWindingNumber;
    }
}
}
 
#endif