PxVehicleLinearMath.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 PX_VEHICLE_LINEAR_MATH_H
#define PX_VEHICLE_LINEAR_MATH_H
#include "vehicle/PxVehicleSDK.h"
 
#if !PX_DOXYGEN
namespace physx
{
#endif
 
#define MAX_VECTORN_SIZE (PX_MAX_NB_WHEELS+3)
 
class VectorN
{
public:
 
    VectorN(const PxU32 size)
        : mSize(size)
    {
        PX_ASSERT(mSize <= MAX_VECTORN_SIZE);
    }
    ~VectorN()
    {
    }
 
    VectorN(const VectorN& src)
    {
        for(PxU32 i = 0; i < src.mSize; i++)
        {
            mValues[i] = src.mValues[i];
        }
        mSize = src.mSize;
    }
 
    PX_FORCE_INLINE VectorN& operator=(const VectorN& src)
    {
        for(PxU32 i = 0; i < src.mSize; i++)
        {
            mValues[i] = src.mValues[i];
        }
        mSize = src.mSize;
        return *this;
    }
 
    PX_FORCE_INLINE PxF32& operator[] (const PxU32 i)
    {
        PX_ASSERT(i < mSize);
        return (mValues[i]);
    }
 
    PX_FORCE_INLINE const PxF32& operator[] (const PxU32 i) const
    {
        PX_ASSERT(i < mSize);
        return (mValues[i]);
    }
 
    PX_FORCE_INLINE PxU32 getSize() const {return mSize;}
 
private:
 
    PxF32 mValues[MAX_VECTORN_SIZE];
    PxU32 mSize;
};
 
class MatrixNN
{
public:
 
    MatrixNN()
        : mSize(0)
    {
    }
    MatrixNN(const PxU32 size)
        : mSize(size)
    {
        PX_ASSERT(mSize <= MAX_VECTORN_SIZE);
    }
    MatrixNN(const MatrixNN& src)
    {
        for(PxU32 i = 0; i < src.mSize; i++)
        {
            for(PxU32 j = 0; j < src.mSize; j++)
            {
                mValues[i][j] = src.mValues[i][j];
            }
        }
        mSize=src.mSize;
    }
    ~MatrixNN()
    {
    }
 
    PX_FORCE_INLINE MatrixNN& operator=(const MatrixNN& src)
    {
        for(PxU32 i = 0;i < src.mSize; i++)
        {
            for(PxU32 j = 0;j < src.mSize; j++)
            {
                mValues[i][j] = src.mValues[i][j];
            }
        }
        mSize = src.mSize;
        return *this;
    }
 
    PX_FORCE_INLINE PxF32 get(const PxU32 i, const PxU32 j) const
    {
        PX_ASSERT(i < mSize);
        PX_ASSERT(j < mSize);
        return mValues[i][j];
    }
    PX_FORCE_INLINE void set(const PxU32 i, const PxU32 j, const PxF32 val)
    {
        PX_ASSERT(i < mSize);
        PX_ASSERT(j < mSize);
        mValues[i][j] = val;
    }
 
    PX_FORCE_INLINE PxU32 getSize() const {return mSize;}
 
    PX_FORCE_INLINE void setSize(const PxU32 size)
    {
        PX_ASSERT(size <= MAX_VECTORN_SIZE);
        mSize = size;
    }
 
public:
 
    PxF32 mValues[MAX_VECTORN_SIZE][MAX_VECTORN_SIZE];
    PxU32 mSize;
};
 
 
/*
    LUPQ decomposition
 
    Based upon "Outer Product LU with Complete Pivoting," from Matrix Computations (4th Edition), Golub and Van Loan
 
    Solve A*x = b using:
 
        MatrixNNLUSolver solver;
        solver.decomposeLU(A);
        solver.solve(b, x);
*/
class MatrixNNLUSolver
{
private:
 
    MatrixNN mLU;
    PxU32   mP[MAX_VECTORN_SIZE-1]; // Row permutation
    PxU32   mQ[MAX_VECTORN_SIZE-1]; // Column permutation
    PxF32   mdetM;
 
public:
 
    MatrixNNLUSolver(){}
    ~MatrixNNLUSolver(){}
 
    PxF32 getDet() const {return mdetM;}
 
    void decomposeLU(const MatrixNN& A)
    {
        const PxU32 D = A.mSize;
 
        mLU = A;
 
        mdetM = 1.0f;
 
        for (PxU32 k = 0; k < D-1; ++k)
        {
            PxU32 pivot_row = k;
            PxU32 pivot_col = k;
            float abs_pivot_elem = 0.0f;
            for (PxU32 c = k; c < D; ++c)
            {
                for (PxU32 r = k; r < D; ++r)
                {
                    const PxF32 abs_elem = PxAbs(mLU.get(r,c));
                    if (abs_elem > abs_pivot_elem)
                    {
                        abs_pivot_elem = abs_elem;
                        pivot_row = r;
                        pivot_col = c;
                    }
                }
            }
 
            mP[k] = pivot_row;
            if (pivot_row != k)
            {
                mdetM = -mdetM;
                for (PxU32 c = 0; c < D; ++c) 
                {
                    //swap(m_LU(k,c), m_LU(pivot_row,c));
                    const PxF32 pivotrowc = mLU.get(pivot_row, c);
                    mLU.set(pivot_row, c, mLU.get(k, c));
                    mLU.set(k, c, pivotrowc);
                }
            }
 
            mQ[k] = pivot_col;
            if (pivot_col != k)
            {
                mdetM = -mdetM;
                for (PxU32 r = 0; r < D; ++r) 
                {
                    //swap(m_LU(r,k), m_LU(r,pivot_col));
                    const PxF32 rpivotcol = mLU.get(r, pivot_col);
                    mLU.set(r,pivot_col, mLU.get(r,k));
                    mLU.set(r, k, rpivotcol);
                }
            }
 
            mdetM *= mLU.get(k,k);
 
            if (mLU.get(k,k) != 0.0f)
            {
                for (PxU32 r = k+1; r < D; ++r)
                {
                    mLU.set(r, k, mLU.get(r,k) / mLU.get(k,k));
                    for (PxU32 c = k+1; c < D; ++c) 
                    {
                        //m_LU(r,c) -= m_LU(r,k)*m_LU(k,c);
                        const PxF32 rc = mLU.get(r, c);
                        const PxF32 rk = mLU.get(r, k);
                        const PxF32 kc = mLU.get(k, c);
                        mLU.set(r, c, rc - rk*kc);
                    }
                }
            }
        }
 
        mdetM *= mLU.get(D-1,D-1);
    }
 
    //Given a matrix A and a vector b find x that satisfies Ax = b, where the matrix A is the matrix that was passed to decomposeLU.
    //Returns true if the lu decomposition indicates that the matrix has an inverse and x was successfully computed.
    //Returns false if the lu decomposition resulted in zero determinant ie the matrix has no inverse and no solution exists for x.
    //Returns false if the size of either b or x doesn't match the size of the matrix passed to decomposeLU.
    //If false is returned then each relevant element of x is set to zero. 
    bool solve(const VectorN& b, VectorN& x) const
    {
        const PxU32 D = x.getSize();
 
        if((b.getSize() != x.getSize()) || (b.getSize() != mLU.getSize()) || (0.0f == mdetM))
        {
            for(PxU32 i = 0; i < D; i++)
            {
                x[i] = 0.0f;
            }
            return false;
        }
 
        x = b;
 
        // Perform row permutation to get Pb
        for(PxU32 i = 0; i < D-1; ++i) 
        {
            //swap(x(i), x(m_P[i]));                                                        
            const PxF32 xp = x[mP[i]];
            x[mP[i]] = x[i];
            x[i] = xp;
        }
 
        // Forward substitute to get (L^-1)Pb
        for (PxU32 r = 1; r < D; ++r)
        {
            for (PxU32 i = 0; i < r; ++i) 
            {
                x[r] -= mLU.get(r,i)*x[i];                              
            }
        }
 
        // Back substitute to get (U^-1)(L^-1)Pb
        for (PxU32 r = D; r-- > 0;) 
        { 
            for (PxU32 i = r+1; i < D; ++i) 
            {
                x[r] -= mLU.get(r,i)*x[i]; 
            } 
            x[r] /= mLU.get(r,r);           
        }
 
        // Perform column permutation to get the solution (Q^T)(U^-1)(L^-1)Pb   
        for (PxU32 i = D-1; i-- > 0;) 
        {
            //swap(x(i), x(m_Q[i]));                                                    
            const PxF32 xq = x[mQ[i]];
            x[mQ[i]] = x[i];
            x[i] = xq;
        }
 
        return true;
    }
 
};
 
 
class MatrixNGaussSeidelSolver
{
public:
 
    void solve(const PxU32 maxIterations, const PxF32 tolerance, const MatrixNN& A, const VectorN& b, VectorN& result) const
    {
        const PxU32 N = A.getSize();
 
        VectorN DInv(N);
        PxF32 bLength2 = 0.0f;
        for(PxU32 i = 0; i < N; i++)
        {
            DInv[i] = 1.0f/A.get(i,i);
            bLength2 += (b[i] * b[i]);
        }
 
        PxU32 iteration = 0;
        PxF32 error = PX_MAX_F32;
        while(iteration < maxIterations && tolerance < error)
        {
            for(PxU32 i = 0; i < N; i++)
            {
                PxF32 l = 0.0f;
                for(PxU32 j = 0; j < i; j++)
                {
                    l += A.get(i,j) * result[j];
                }
 
                PxF32 u = 0.0f;
                for(PxU32 j = i + 1; j < N; j++)
                {
                    u += A.get(i,j) * result[j];
                }
 
                result[i] = DInv[i] * (b[i] - l - u);
            }
 
            //Compute the error.
            PxF32 rLength2 = 0;
            for(PxU32 i = 0; i < N; i++)
            {
                PxF32 e = -b[i];
                for(PxU32 j = 0; j < N; j++)
                {
                    e += A.get(i,j) * result[j];
                }
                rLength2 += e * e;
            }
            error = (rLength2 / (bLength2 + 1e-10f));
 
            iteration++;
        }
    }
};
 
class Matrix33Solver
{
public:
 
    bool solve(const MatrixNN& A_, const VectorN& b_, VectorN& result) const
    {
        const PxF32 a = A_.get(0,0);
        const PxF32 b = A_.get(0,1);
        const PxF32 c = A_.get(0,2);
 
        const PxF32 d = A_.get(1,0);
        const PxF32 e = A_.get(1,1);
        const PxF32 f = A_.get(1,2);
 
        const PxF32 g = A_.get(2,0);
        const PxF32 h = A_.get(2,1);
        const PxF32 k = A_.get(2,2);
 
        const PxF32 detA = a*(e*k - f*h) - b*(k*d - f*g) + c*(d*h - e*g);
        if(0.0f == detA)
        {
            return false;
        }
        const PxF32 detAInv = 1.0f/detA;
 
        const PxF32 A = (e*k - f*h);
        const PxF32 D = -(b*k - c*h);
        const PxF32 G = (b*f - c*e);
        const PxF32 B = -(d*k - f*g);
        const PxF32 E = (a*k - c*g);
        const PxF32 H = -(a*f - c*d);
        const PxF32 C = (d*h - e*g);
        const PxF32 F = -(a*h - b*g);
        const PxF32 K = (a*e - b*d); 
 
        result[0] = detAInv*(A*b_[0] + D*b_[1] + G*b_[2]);
        result[1] = detAInv*(B*b_[0] + E*b_[1] + H*b_[2]);
        result[2] = detAInv*(C*b_[0] + F*b_[1] + K*b_[2]);
 
        return true;
    }
};
 
#if !PX_DOXYGEN
} // namespace physx
#endif
 
#endif