GuVecPlane.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_VEC_PLANE_H
#define GU_VEC_PLANE_H
#include "foundation/PxVec3.h"
#include "foundation/PxPlane.h"
#include "foundation/PxVecMath.h"
 
namespace physx
{
namespace Gu
{
    
    class PlaneV
    {
    public:
        PX_FORCE_INLINE PlaneV()
        {
        }
 
        PX_FORCE_INLINE PlaneV(const aos::FloatVArg nx, const aos::FloatVArg ny, const aos::FloatVArg nz, const aos::FloatVArg _d)
        {
            set(nx, ny, nz, _d);
        }
 
        PX_FORCE_INLINE PlaneV(const PxPlane& plane)
        {
            using namespace aos;
            const Vec3V _n = V3LoadU(plane.n); 
            const FloatV _d = FLoad(plane.d);
            nd = V4SetW(Vec4V_From_Vec3V(_n), _d);
        }
 
 
        PX_FORCE_INLINE PlaneV(const aos::Vec3VArg p0, const aos::Vec3VArg p1, const aos::Vec3VArg p2)
        {
            set(p0, p1, p2);
        }
 
        PX_FORCE_INLINE PlaneV(const aos::Vec3VArg _n, const aos::FloatVArg _d) 
        {
            nd = aos::V4SetW(aos::Vec4V_From_Vec3V(_n), _d);
        }
 
        PX_FORCE_INLINE PlaneV(const PlaneV& plane) : nd(plane.nd)
        {
        }
 
        PX_FORCE_INLINE ~PlaneV()
        {
        }
 
        PX_FORCE_INLINE PlaneV& setZero()
        {
            nd = aos::V4Zero();
            return *this;
        }
 
        PX_FORCE_INLINE PlaneV& set(const aos::FloatVArg nx, const aos::FloatVArg ny, const aos::FloatVArg nz, const aos::FloatVArg _d)
        {
 
            using namespace aos;
            const Vec3V n= V3Merge(nx, ny, nz);
            nd = V4SetW(Vec4V_From_Vec3V(n), _d);
            return *this;
        }
 
        PX_FORCE_INLINE PlaneV& set(const aos::Vec3VArg _normal, aos::FloatVArg _d)
        {
            nd = aos::V4SetW(aos::Vec4V_From_Vec3V(_normal), _d);
            return *this;
        }
 
        PlaneV& set(const aos::Vec3VArg p0, const aos::Vec3VArg p1, const aos::Vec3VArg p2)
        {
            using namespace aos;
            const Vec3V edge0 = V3Sub(p1, p0);
            const Vec3V edge1 = V3Sub(p2, p0);
 
            const Vec3V n = V3Normalize(V3Cross(edge0, edge1));
            // See comments in set() for computation of d
            const FloatV d = FNeg(V3Dot(p0, n));
            nd = V4SetW(Vec4V_From_Vec3V(n), d);
            return  *this;
        }
 
        /***
        \brief Computes distance, assuming plane is normalized
        \sa normalize
        */
        PX_FORCE_INLINE aos::FloatV distance(const aos::Vec3VArg p) const
        {
            // Valid for plane equation a*x + b*y + c*z + d = 0
            using namespace aos;
            const Vec3V n = Vec3V_From_Vec4V(nd);
            return FAdd(V3Dot(p, n), V4GetW(nd));
        }
 
        PX_FORCE_INLINE aos::BoolV belongs(const aos::Vec3VArg p) const
        {
            using namespace aos;
            const FloatV eps = FLoad(1.0e-7f);
            return FIsGrtr(eps, FAbs(distance(p)));
        }
 
        PX_FORCE_INLINE aos::Vec3V project(const aos::Vec3VArg p) const
        {
            // Pretend p is on positive side of plane, i.e. plane.distance(p)>0.
            // To project the point we have to go in a direction opposed to plane's normal, i.e.:
            using namespace aos;
            const Vec3V n = Vec3V_From_Vec4V(nd);
            return V3Sub(p, V3Scale(n, V4GetW(nd)));
 
        }
 
        PX_FORCE_INLINE aos::FloatV signedDistanceHessianNormalForm(const aos::Vec3VArg point) const
        {
            using namespace aos;
            const Vec3V n = Vec3V_From_Vec4V(nd);
            return FAdd(V3Dot(n, point), V4GetW(nd));
        }
 
        PX_FORCE_INLINE aos::Vec3V getNormal() const
        {
            return aos::Vec3V_From_Vec4V(nd);
        }
 
        PX_FORCE_INLINE aos::FloatV getSignDist() const
        {
            return aos::V4GetW(nd);
        }
 
        PX_FORCE_INLINE aos::Vec3V pointInPlane() const
        {
            // Project origin (0,0,0) to plane:
            // (0) - normal * distance(0) = - normal * ((p|(0)) + d) = -normal*d
            using namespace aos;
            const Vec3V n = Vec3V_From_Vec4V(nd);
            return V3Neg(V3Scale(n, V4GetW(nd)));
        }
 
        PX_FORCE_INLINE void normalize()
        {
            using namespace aos;
            const Vec3V n = Vec3V_From_Vec4V(nd);
            const FloatV denom = FRecip(V3Length(n));
            V4Scale(nd, denom);
        }
 
        aos::Vec4V  nd;     
    };
}
 
}
 
#endif