PxVehicleParams.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.  
 
#pragma once
#include "foundation/PxFoundation.h"
#include "foundation/PxAssert.h"
#include "foundation/PxMemory.h"
#include "foundation/PxVec3.h"
#include "foundation/PxMat33.h"
 
#include "PxVehicleLimits.h"
 
class OmniPvdWriter;
 
#if !PX_DOXYGEN
namespace physx
{
class PxConvexMesh;
class PxScene;
namespace vehicle2
{
#endif
 
struct PxVehicleAxleDescription
{
    PX_FORCE_INLINE void setToDefault()
    {
        PxMemZero(this, sizeof(PxVehicleAxleDescription));
    }
 
    void addAxle(const PxU32 nbWheelsOnAxle, const PxU32* const wheelIdsOnAxle)
    {
        PX_ASSERT((nbWheels + nbWheelsOnAxle) < PxVehicleLimits::eMAX_NB_WHEELS);
        PX_ASSERT(nbAxles < PxVehicleLimits::eMAX_NB_AXLES);
        nbWheelsPerAxle[nbAxles] = nbWheelsOnAxle;
        axleToWheelIds[nbAxles] = nbWheels;
        for (PxU32 i = 0; i < nbWheelsOnAxle; i++)
        {
            wheelIdsInAxleOrder[nbWheels + i] = wheelIdsOnAxle[i];
        }
        nbWheels += nbWheelsOnAxle;
        nbAxles++;
    }
 
    PX_FORCE_INLINE PxU32 getNbAxles() const 
    {
        return nbAxles;
    }
 
    PX_FORCE_INLINE PxU32 getNbWheelsOnAxle(const PxU32 i) const 
    {
        return nbWheelsPerAxle[i];
    }
 
    PX_FORCE_INLINE PxU32 getWheelOnAxle(const PxU32 j, const PxU32 i) const
    {
        return wheelIdsInAxleOrder[axleToWheelIds[i] + j];
    }
 
    PX_FORCE_INLINE PxU32 getNbWheels() const
    {
        return nbWheels;
    }
 
    PX_FORCE_INLINE PxU32 getAxle(const PxU32 wheelId) const
    {
        for (PxU32 i = 0; i < getNbAxles(); i++)
        {
            for (PxU32 j = 0; j < getNbWheelsOnAxle(i); j++)
            {
                if (getWheelOnAxle(j, i) == wheelId)
                    return i;
            }
        }
        return 0xffffffff;
    }
 
    PX_FORCE_INLINE bool isValid() const
    {
        PX_CHECK_AND_RETURN_VAL(nbAxles > 0, "PxVehicleAxleDescription.nbAxles must be greater than zero", false);
        PX_CHECK_AND_RETURN_VAL(nbWheels > 0, "PxVehicleAxleDescription.nbWheels must be greater than zero", false);
        return true;
    }
 
    PxU32 nbAxles;                                                  
    PxU32 nbWheelsPerAxle[PxVehicleLimits::eMAX_NB_AXLES];          
    PxU32 axleToWheelIds[PxVehicleLimits::eMAX_NB_AXLES];           
    
    PxU32 wheelIdsInAxleOrder[PxVehicleLimits::eMAX_NB_WHEELS];     
    PxU32 nbWheels;                                                 
 
 
    PX_COMPILE_TIME_ASSERT(PxVehicleLimits::eMAX_NB_AXLES == PxVehicleLimits::eMAX_NB_WHEELS);
    // It should be possible to support cases where each wheel is controlled individually and thus
    // having a wheel per axle for up to the max wheel count.
};
 
 
struct PxVehicleAxes
{
    enum Enum
    {
        ePosX = 0,  
        eNegX,      
        ePosY,      
        eNegY,      
        ePosZ,      
        eNegZ,      
        eMAX_NB_AXES
    };
};
 
struct PxVehicleFrame
{
    PxVehicleAxes::Enum lngAxis;  
    PxVehicleAxes::Enum latAxis;  
    PxVehicleAxes::Enum vrtAxis;  
 
    PX_FORCE_INLINE void setToDefault()
    {
        lngAxis = PxVehicleAxes::ePosX;
        latAxis = PxVehicleAxes::ePosY;
        vrtAxis = PxVehicleAxes::ePosZ;
    }
 
    PX_FORCE_INLINE PxMat33 getFrame() const
    {
        const PxVec3 basisDirs[6] = { PxVec3(1,0,0), PxVec3(-1,0,0), PxVec3(0,1,0), PxVec3(0,-1,0), PxVec3(0,0,1), PxVec3(0,0,-1) };
        const PxMat33 mat33(basisDirs[lngAxis], basisDirs[latAxis], basisDirs[vrtAxis]);
        return mat33;
    }
 
    PX_FORCE_INLINE PxVec3 getLngAxis() const 
    {
        const PxVec3 basisDirs[6] = { PxVec3(1,0,0), PxVec3(-1,0,0), PxVec3(0,1,0), PxVec3(0,-1,0), PxVec3(0,0,1), PxVec3(0,0,-1) };
        return basisDirs[lngAxis];
    }
 
    PX_FORCE_INLINE PxVec3 getLatAxis() const
    {
        const PxVec3 basisDirs[6] = { PxVec3(1,0,0), PxVec3(-1,0,0), PxVec3(0,1,0), PxVec3(0,-1,0), PxVec3(0,0,1), PxVec3(0,0,-1) };
        return basisDirs[latAxis];
    }
 
    PX_FORCE_INLINE PxVec3 getVrtAxis() const
    {
        const PxVec3 basisDirs[6] = { PxVec3(1,0,0), PxVec3(-1,0,0), PxVec3(0,1,0), PxVec3(0,-1,0), PxVec3(0,0,1), PxVec3(0,0,-1) };
        return basisDirs[vrtAxis];
    }
 
    PX_FORCE_INLINE bool isValid() const
    {
        PX_CHECK_AND_RETURN_VAL(lngAxis < PxVehicleAxes::eMAX_NB_AXES, "PxVehicleFrame.lngAxis is invalid", false);
        PX_CHECK_AND_RETURN_VAL(latAxis < PxVehicleAxes::eMAX_NB_AXES, "PxVehicleFrame.latAxis is invalid", false);
        PX_CHECK_AND_RETURN_VAL(vrtAxis < PxVehicleAxes::eMAX_NB_AXES, "PxVehicleFrame.vrtAxis is invalid", false);
        const PxMat33 frame = getFrame();
        const PxQuat quat(frame);
        PX_CHECK_AND_RETURN_VAL(quat.isFinite() && quat.isUnit() && quat.isSane(), "PxVehicleFrame is not a legal frame", false);
        return true;
    }
};
 
struct PxVehicleScale
{
    PxReal scale;  
 
    PX_FORCE_INLINE void setToDefault()
    {
        scale = 1.0f;
    }
 
    PX_FORCE_INLINE bool isValid() const
    {
        PX_CHECK_AND_RETURN_VAL(scale > 0.0f, "PxVehicleScale.scale must be greater than zero", false);
        return true;
    }
};
 
template<typename T>
struct PxVehicleArrayData
{
    enum DataFormat
    {
        eARRAY_OF_STRUCTS = 0,  
        eARRAY_OF_POINTERS      
    };
 
    PX_FORCE_INLINE void setData(T* data)
    {
        arrayOfStructs = data;
        dataFormat = eARRAY_OF_STRUCTS;
    }
 
    PX_FORCE_INLINE void setData(T*const* data)
    {
        arrayOfPointers= data;
        dataFormat = eARRAY_OF_POINTERS;
    }
 
    PX_FORCE_INLINE PxVehicleArrayData()
    {
    }
 
    PX_FORCE_INLINE explicit PxVehicleArrayData(T* data)
    {
        setData(data);
    }
 
    PX_FORCE_INLINE explicit PxVehicleArrayData(T*const* data)
    {
        setData(data);
    }
 
    PX_FORCE_INLINE T& getData(PxU32 index)
    {
        if (dataFormat == eARRAY_OF_STRUCTS)
            return arrayOfStructs[index];
        else
            return *arrayOfPointers[index];
    }
 
    PX_FORCE_INLINE T& operator[](PxU32 index)
    {
        return getData(index);
    }
 
    PX_FORCE_INLINE const T& getData(PxU32 index) const
    {
        if (dataFormat == eARRAY_OF_STRUCTS)
            return arrayOfStructs[index];
        else
            return *arrayOfPointers[index];
    }
 
    PX_FORCE_INLINE const T& operator[](PxU32 index) const
    {
        return getData(index);
    }
 
    PX_FORCE_INLINE void setEmpty()
    {
        arrayOfStructs = NULL;
    }
 
    PX_FORCE_INLINE bool isEmpty() const
    {
        return (arrayOfStructs == NULL);
    }
 
    PX_FORCE_INLINE const PxVehicleArrayData<const T>& getConst() const
    {
        return reinterpret_cast<const PxVehicleArrayData<const T>&>(*this);
    }
 
 
    union
    {
        T* arrayOfStructs;         
        T*const* arrayOfPointers;  
    };
    PxU8 dataFormat;
};
 
template<typename T>
struct PxVehicleSizedArrayData : public PxVehicleArrayData<T>
{
    PX_FORCE_INLINE void setDataAndCount(T* data, const PxU32 count)
    {
        PxVehicleArrayData<T>::setData(data);
        size = count;
    }
 
    PX_FORCE_INLINE void setDataAndCount(T*const* data, const PxU32 count)
    {
        PxVehicleArrayData<T>::setData(data);
        size = count;
    }
 
    PX_FORCE_INLINE void setEmpty()
    {
        PxVehicleArrayData<T>::setEmpty();
        size = 0;
    }
 
    PX_FORCE_INLINE bool isEmpty() const
    {
        return ((size == 0) || PxVehicleArrayData<T>::isEmpty());
    }
 
    PxU32 size;
};
 
struct PxVehiclePhysXActorUpdateMode
{
    enum Enum
    {
        eAPPLY_VELOCITY = 0,
        eAPPLY_ACCELERATION
    };
};
 
struct PxVehicleTireSlipParams
{
    PxReal minLatSlipDenominator;
 
    PxReal minPassiveLongSlipDenominator;
 
    PxReal minActiveLongSlipDenominator;
 
    PX_FORCE_INLINE void setToDefault()
    {
        minLatSlipDenominator = 1.0f;
        minActiveLongSlipDenominator = 0.1f;
        minPassiveLongSlipDenominator = 4.0f;
    }
 
    PX_FORCE_INLINE PxVehicleTireSlipParams transformAndScale(
        const PxVehicleFrame& srcFrame, const PxVehicleFrame& trgFrame, const PxVehicleScale& srcScale, const PxVehicleScale& trgScale) const
    {
        PX_UNUSED(srcFrame);
        PX_UNUSED(trgFrame);
        PxVehicleTireSlipParams p = *this;
        const PxReal scaleRatio = trgScale.scale / srcScale.scale;
        p.minLatSlipDenominator *= scaleRatio;
        p.minPassiveLongSlipDenominator *= scaleRatio;
        p.minActiveLongSlipDenominator *= scaleRatio;
        return p;
    }
 
    PX_FORCE_INLINE bool isValid() const
    {
        PX_CHECK_AND_RETURN_VAL(minLatSlipDenominator > 0.0f, "PxVehicleTireSlipParams.minLatSlipDenominator must be greater than zero", false);
        PX_CHECK_AND_RETURN_VAL(minPassiveLongSlipDenominator > 0.0f, "PxVehicleTireSlipParams.minPassiveLongSlipDenominator must be greater than zero", false);
        PX_CHECK_AND_RETURN_VAL(minActiveLongSlipDenominator > 0.0f, "PxVehicleTireSlipParams.minActiveLongSlipDenominator must be greater than zero", false);
        return true;
    }
};
 
struct PxVehicleTireDirectionModes
{
    enum Enum
    {
        eLONGITUDINAL = 0,
        eLATERAL,
        eMAX_NB_PLANAR_DIRECTIONS
    };
};
 
struct PxVehicleTireAxisStickyParams
{
    PxReal thresholdSpeed;
 
    PxReal thresholdTime;
 
    PxReal damping;
 
 
    PX_FORCE_INLINE PxVehicleTireAxisStickyParams transformAndScale( 
        const PxVehicleFrame& srcFrame, const PxVehicleFrame& trgFrame, const PxVehicleScale& srcScale, const PxVehicleScale& trgScale) const
    {
        PX_UNUSED(srcFrame);
        PX_UNUSED(trgFrame);
        PxVehicleTireAxisStickyParams p = *this;
        const PxReal scaleRatio = trgScale.scale / srcScale.scale;
        p.thresholdSpeed *= scaleRatio;
        return p;
    }
 
    PX_FORCE_INLINE bool isValid() const
    {
        PX_CHECK_AND_RETURN_VAL(thresholdSpeed >= 0.0f, "PxVehicleTireAxisStickyParams.thresholdSpeed must be greater than or equal to zero", false);
        PX_CHECK_AND_RETURN_VAL(thresholdTime >= 0.0f, "PxVehicleTireAxisStickyParams.thresholdTime must be greater than or equal to zero", false);
        PX_CHECK_AND_RETURN_VAL(damping >= 0.0f, "PxVehicleTireAxisStickyParams.damping must be greater than or equal to zero", false);
        return true;
    }
};
 
struct PxVehicleTireStickyParams
{
    PxVehicleTireAxisStickyParams stickyParams[PxVehicleTireDirectionModes::eMAX_NB_PLANAR_DIRECTIONS];
 
    PX_FORCE_INLINE void setToDefault()
    {
        stickyParams[PxVehicleTireDirectionModes::eLONGITUDINAL].thresholdSpeed = 0.2f;
        stickyParams[PxVehicleTireDirectionModes::eLONGITUDINAL].thresholdTime = 1.0f;
        stickyParams[PxVehicleTireDirectionModes::eLONGITUDINAL].damping = 1.0f; 
        stickyParams[PxVehicleTireDirectionModes::eLATERAL].thresholdSpeed = 0.2f;
        stickyParams[PxVehicleTireDirectionModes::eLATERAL].thresholdTime = 1.0f;
        stickyParams[PxVehicleTireDirectionModes::eLATERAL].damping = 0.1f;
    }
 
    PX_FORCE_INLINE PxVehicleTireStickyParams transformAndScale(
        const PxVehicleFrame& srcFrame, const PxVehicleFrame& trgFrame, const PxVehicleScale& srcScale, const PxVehicleScale& trgScale) const
    {
        PxVehicleTireStickyParams p = *this;
        p.stickyParams[PxVehicleTireDirectionModes::eLONGITUDINAL] = 
            stickyParams[PxVehicleTireDirectionModes::eLONGITUDINAL].transformAndScale(srcFrame, trgFrame, srcScale, trgScale);
        p.stickyParams[PxVehicleTireDirectionModes::eLATERAL] = 
            stickyParams[PxVehicleTireDirectionModes::eLATERAL].transformAndScale(srcFrame, trgFrame, srcScale, trgScale);
        return p;
    }
 
    PX_FORCE_INLINE bool isValid() const
    {
        if (!stickyParams[PxVehicleTireDirectionModes::eLONGITUDINAL].isValid())
            return false;
        if (!stickyParams[PxVehicleTireDirectionModes::eLATERAL].isValid())
            return false;
        return true;
    }
};
 
struct PxVehicleSimulationContextType
{
    enum Enum
    {
        eDEFAULT,    
        ePHYSX       
    };
};
 
struct PxVehiclePvdContext
{
public:
    PX_FORCE_INLINE void setToDefault()
    {
        attributeHandles = NULL;
        writer = NULL;
    }
 
 
    const struct PxVehiclePvdAttributeHandles* attributeHandles;
 
    OmniPvdWriter* writer;
};
 
struct PxVehicleSimulationContext
{
    PxVehicleSimulationContext()
        : type(PxVehicleSimulationContextType::eDEFAULT)
    {}
 
    PxVec3 gravity;
 
    PxVehicleFrame frame;
    PxVehicleScale scale;
 
    //Tire 
    PxVehicleTireSlipParams tireSlipParams;
    PxVehicleTireStickyParams tireStickyParams;
 
    PxReal thresholdForwardSpeedForWheelAngleIntegration;
 
    PxVehiclePvdContext pvdContext;
 
protected:
    PxVehicleSimulationContextType::Enum type;
 
 
public:
    PX_FORCE_INLINE PxVehicleSimulationContextType::Enum getType() const { return type; }
 
    PX_FORCE_INLINE void setToDefault()
    {
        frame.setToDefault();
        scale.setToDefault();
 
        gravity = frame.getVrtAxis() * (-9.81f * scale.scale);
 
        tireSlipParams.setToDefault();
        tireStickyParams.setToDefault();
 
        thresholdForwardSpeedForWheelAngleIntegration = 5.0f * scale.scale;
 
        pvdContext.setToDefault();
    }
 
    PX_FORCE_INLINE PxVehicleSimulationContext transformAndScale(
        const PxVehicleFrame& srcFrame, const PxVehicleFrame& trgFrame, const PxVehicleScale& srcScale, const PxVehicleScale& trgScale) const
    {
        PxVehicleSimulationContext c = *this;
        const PxReal scaleRatio = trgScale.scale / srcScale.scale;
        c.gravity = trgFrame.getFrame()*srcFrame.getFrame().getTranspose()*c.gravity;
        c.gravity *= scaleRatio;
        c.tireSlipParams = tireSlipParams.transformAndScale(srcFrame, trgFrame, srcScale, trgScale);
        c.tireStickyParams = tireStickyParams.transformAndScale(srcFrame, trgFrame, srcScale, trgScale);
        c.thresholdForwardSpeedForWheelAngleIntegration *= scaleRatio;
        c.frame = trgFrame;
        c.scale = trgScale;
        return c;
    }
};
 
struct PxVehiclePhysXSimulationContext : public PxVehicleSimulationContext
{
    PxVehiclePhysXSimulationContext()
        : PxVehicleSimulationContext()
    {
        type = PxVehicleSimulationContextType::ePHYSX;
    }
 
    //Road geometry queries to find the plane under the wheel.
    const PxConvexMesh* physxUnitCylinderSweepMesh;
    const PxScene* physxScene;
 
    //PhysX actor update
    PxVehiclePhysXActorUpdateMode::Enum physxActorUpdateMode;
 
    PxReal physxActorWakeCounterResetValue;
 
    PxReal physxActorWakeCounterThreshold;
 
 
    PX_FORCE_INLINE void setToDefault()
    {
        PxVehicleSimulationContext::setToDefault();
 
        physxUnitCylinderSweepMesh = NULL;
        physxScene = NULL;
 
        physxActorUpdateMode = PxVehiclePhysXActorUpdateMode::eAPPLY_VELOCITY;
 
        physxActorWakeCounterResetValue = 20.0f * 0.02f;  // 20 timesteps of size 0.02
        physxActorWakeCounterThreshold = 0.5f * physxActorWakeCounterResetValue;
    }
 
    PX_FORCE_INLINE PxVehiclePhysXSimulationContext transformAndScale(
        const PxVehicleFrame& srcFrame, const PxVehicleFrame& trgFrame, const PxVehicleScale& srcScale, const PxVehicleScale& trgScale) const
    {
        PxVehiclePhysXSimulationContext r = *this;
        static_cast<PxVehicleSimulationContext&>(r) = PxVehicleSimulationContext::transformAndScale(srcFrame, trgFrame, srcScale, trgScale);
        return r;
    }
};
 
template <class T, unsigned int NB_ELEMENTS>
class PxVehicleFixedSizeLookupTable
{
public:
 
    PxVehicleFixedSizeLookupTable()
        : nbDataPairs(0)
    {
    }
 
    PxVehicleFixedSizeLookupTable(const PxVehicleFixedSizeLookupTable& src)
    {
        PxMemCopy(xVals, src.xVals, sizeof(PxReal)* src.nbDataPairs);
        PxMemCopy(yVals, src.yVals, sizeof(T)*src.nbDataPairs);
        nbDataPairs = src.nbDataPairs;
    }
 
    ~PxVehicleFixedSizeLookupTable()
    {
    }
 
    PxVehicleFixedSizeLookupTable& operator=(const PxVehicleFixedSizeLookupTable& src)
    {
        PxMemCopy(xVals, src.xVals, sizeof(PxReal)*src.nbDataPairs);
        PxMemCopy(yVals, src.yVals, sizeof(T)*src.nbDataPairs);
        nbDataPairs = src.nbDataPairs;
        return *this;
    }
 
    PX_FORCE_INLINE bool addPair(const PxReal x, const T y)
    {
        PX_CHECK_AND_RETURN_VAL(nbDataPairs < NB_ELEMENTS, "PxVehicleFixedSizeLookupTable::addPair() exceeded fixed size capacity", false);
        xVals[nbDataPairs] = x;
        yVals[nbDataPairs] = y;
        nbDataPairs++;
        return true;
    }
 
    PX_FORCE_INLINE T interpolate(const PxReal x) const
    {
        if (0 == nbDataPairs)
        {
            return T(0);
        }
 
        if (1 == nbDataPairs || x < xVals[0])
        {
            return yVals[0];
        }
 
        PxReal x0 = xVals[0];
        T y0 = yVals[0];
 
        for (PxU32 i = 1; i < nbDataPairs; i++)
        {
            const PxReal x1 = xVals[i];
            const T y1 = yVals[i];
 
            if ((x >= x0) && (x < x1))
            {
                return (y0 + (y1 - y0) * (x - x0) / (x1 - x0));
            }
 
            x0 = x1;
            y0 = y1;
        }
 
        PX_ASSERT(x >= xVals[nbDataPairs - 1]);
        return yVals[nbDataPairs - 1];
    }
 
    void clear()
    {
        PxMemSet(xVals, 0, NB_ELEMENTS * sizeof(PxReal));
        PxMemSet(yVals, 0, NB_ELEMENTS * sizeof(T));
        nbDataPairs = 0;
    }
 
    PxReal xVals[NB_ELEMENTS];
    T yVals[NB_ELEMENTS];
    PxU32 nbDataPairs;
 
    PX_FORCE_INLINE bool isValid() const
    {
        for (PxU32 i = 1; i < nbDataPairs; i++)
        {
            PX_CHECK_AND_RETURN_VAL(xVals[i] > xVals[i - 1], "PxVehicleFixedSizeLookupTable:: xVals[i+1] must be greater than xVals[i]", false);
        }
        return true;
    }
};
 
#if !PX_DOXYGEN
} // namespace vehicle2
} // namespace physx
#endif