CmScaling.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 CM_SCALING_H
#define CM_SCALING_H
 
#include "foundation/PxBounds3.h"
#include "foundation/PxMat33.h"
#include "foundation/PxMathUtils.h"
#include "foundation/PxMat34.h"
#include "foundation/PxSIMDHelpers.h"
#include "geometry/PxMeshScale.h"
#include "CmUtils.h"
 
namespace physx
{
namespace Cm
{
    // PT: same as PxMeshScale::toMat33() but faster
    PX_FORCE_INLINE PxMat33 toMat33(const PxMeshScale& meshScale)
    {
        const PxMat33Padded rot(meshScale.rotation);
 
        PxMat33 trans = rot.getTranspose();
        trans.column0 *= meshScale.scale[0];
        trans.column1 *= meshScale.scale[1];
        trans.column2 *= meshScale.scale[2];
        return trans * rot;
    }
 
    // class that can perform scaling fast.  Relatively large size, generated from PxMeshScale on demand.
    // CS: I've removed most usages of this class, because most of the time only one-way transform is needed.
    // If you only need a temporary FastVertex2ShapeScaling, setup your transform as PxMat34Legacy and use
    // normal matrix multiplication or a transform() overload to convert points and bounds between spaces.
    class FastVertex2ShapeScaling
    {
    public:
        PX_INLINE FastVertex2ShapeScaling()
        {
            //no scaling by default:
            vertex2ShapeSkew = PxMat33(PxIdentity);
            shape2VertexSkew = PxMat33(PxIdentity);
            mFlipNormal = false;
        }
 
        PX_INLINE explicit FastVertex2ShapeScaling(const PxMeshScale& scale)
        {
            init(scale);
        }
 
        PX_INLINE FastVertex2ShapeScaling(const PxVec3& scale, const PxQuat& rotation)
        {
            init(scale, rotation);
        }
 
        PX_INLINE void init(const PxMeshScale& scale)
        {
            init(scale.scale, scale.rotation);
        }
 
        PX_INLINE void setIdentity()
        {
            vertex2ShapeSkew = PxMat33(PxIdentity);
            shape2VertexSkew = PxMat33(PxIdentity);
            mFlipNormal = false;
        }
 
        PX_INLINE void init(const PxVec3& scale, const PxQuat& rotation)
        {
            // TODO: may want to optimize this for cases where we have uniform or axis aligned scaling!
            // That would introduce branches and it's unclear to me whether that's faster than just doing the math.
            // Lazy computation would be another option, at the cost of introducing even more branches.
 
            const PxMat33Padded R(rotation);
            vertex2ShapeSkew = R.getTranspose();
            const PxMat33 diagonal = PxMat33::createDiagonal(scale);
            vertex2ShapeSkew = vertex2ShapeSkew * diagonal;
            vertex2ShapeSkew = vertex2ShapeSkew * R;
 
            /*
            The inverse, is, explicitly:
            shape2VertexSkew.setTransposed(R);
            shape2VertexSkew.multiplyDiagonal(PxVec3(1.0f/scale.x, 1.0f/scale.y, 1.0f/scale.z));
            shape2VertexSkew *= R;
 
            It may be competitive to compute the inverse -- though this has a branch in it:
            */
            
            shape2VertexSkew = vertex2ShapeSkew.getInverse();           
 
            mFlipNormal = ((scale.x * scale.y * scale.z) < 0.0f);
        }
 
        PX_FORCE_INLINE void flipNormal(PxVec3& v1, PxVec3& v2) const
        {
            if (mFlipNormal)
            {
                PxVec3 tmp = v1; v1 = v2; v2 = tmp;
            }
        }
 
        PX_FORCE_INLINE PxVec3 operator*  (const PxVec3& src) const
        {
            return vertex2ShapeSkew * src;
        }
 
        PX_FORCE_INLINE PxVec3 operator%  (const PxVec3& src) const
        {
            return shape2VertexSkew * src;
        }
 
        PX_FORCE_INLINE const PxMat33& getVertex2ShapeSkew() const
        {
            return vertex2ShapeSkew;
        }
 
        PX_FORCE_INLINE const PxMat33& getShape2VertexSkew() const
        {
            return shape2VertexSkew;
        }
 
        PX_INLINE PxMat34 getVertex2WorldSkew(const PxMat34& shape2world) const
        {
            const PxMat34 vertex2worldSkew = shape2world * getVertex2ShapeSkew();
            //vertex2worldSkew = shape2world * [vertex2shapeSkew, 0]
            //[aR at] * [bR bt] = [aR * bR      aR * bt + at]  NOTE: order of operations important so it works when this ?= left ?= right.
            return vertex2worldSkew;
        }
 
        PX_INLINE PxMat34 getWorld2VertexSkew(const PxMat34& shape2world) const
        {
            //world2vertexSkew = shape2vertex * invPQ(shape2world)
            //[aR 0] * [bR' -bR'*bt] = [aR * bR'        -aR * bR' * bt + 0]
 
            const PxMat33 rotate( shape2world[0], shape2world[1], shape2world[2] );
            const PxMat33 M = getShape2VertexSkew() * rotate.getTranspose();
            return PxMat34(M[0], M[1], M[2], -M * shape2world[3]);          
        }
 
        void transformQueryBounds(PxVec3& center, PxVec3& extents, PxMat33& basis)  const
        {
            basis.column0 = shape2VertexSkew * (basis.column0 * extents.x);
            basis.column1 = shape2VertexSkew * (basis.column1 * extents.y);
            basis.column2 = shape2VertexSkew * (basis.column2 * extents.z);
 
            center = shape2VertexSkew * center;
            extents = PxOptimizeBoundingBox(basis);
        }
 
        void transformPlaneToShapeSpace(const PxVec3& nIn, const PxReal dIn, PxVec3& nOut, PxReal& dOut)    const
        {
            const PxVec3 tmp = shape2VertexSkew.transformTranspose(nIn);
            const PxReal denom = 1.0f / tmp.magnitude();
            nOut = tmp * denom;
            dOut = dIn * denom;
        }
 
        PX_FORCE_INLINE bool flipsNormal() const { return mFlipNormal; }
 
    private:
        PxMat33 vertex2ShapeSkew;
        PxMat33 shape2VertexSkew;
        bool mFlipNormal;
    };
 
    PX_FORCE_INLINE void getScaledVertices(PxVec3* v, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2, bool idtMeshScale, const Cm::FastVertex2ShapeScaling& scaling)
    {
        if(idtMeshScale)
        {
            v[0] = v0;
            v[1] = v1;
            v[2] = v2;
        }
        else
        {
            const PxI32 winding = scaling.flipsNormal() ? 1 : 0;
            v[0] = scaling * v0;
            v[1+winding] = scaling * v1;
            v[2-winding] = scaling * v2;
        }
    }
 
} // namespace Cm
 
 
PX_INLINE PxMat34 operator*(const PxTransform& transform, const PxMeshScale& scale) 
{
    const PxMat33Padded tmp(transform.q);
 
    return PxMat34(tmp * Cm::toMat33(scale), transform.p);
}
 
PX_INLINE PxMat34 operator*(const PxMeshScale& scale, const PxTransform& transform) 
{
    const PxMat33 scaleMat = Cm::toMat33(scale);
    const PxMat33Padded t(transform.q);
    const PxMat33 r = scaleMat * t;
    const PxVec3 p = scaleMat * transform.p;
    return PxMat34(r, p);
}
 
PX_INLINE PxMat34 operator*(const PxMat34& transform, const PxMeshScale& scale) 
{
    return PxMat34(transform.m * Cm::toMat33(scale), transform.p);
}
 
PX_INLINE PxMat34 operator*(const PxMeshScale& scale, const PxMat34& transform) 
{
    const PxMat33 scaleMat = Cm::toMat33(scale);
    return PxMat34(scaleMat * transform.m, scaleMat * transform.p);
}
 
}
 
#endif