include/vehicle2/PxVehicleParams.h

File members: include/vehicle2/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-2023 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