PxVehicleSuspLimitConstraintShader.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_SUSP_LIMIT_CONSTRAINT_SHADER_H
#define PX_VEHICLE_SUSP_LIMIT_CONSTRAINT_SHADER_H
#include "foundation/PxTransform.h"
#include "extensions/PxConstraintExt.h"
#include "PxConstraintDesc.h"
#include "PxConstraint.h"
#include "vehicle/PxVehicleWheels.h"
 
#if !PX_DOXYGEN
namespace physx
{
#endif
 
class PxVehicleConstraintShader : public PxConstraintConnector
{
//= ATTENTION! =====================================================================================
// Changing the data layout of this class breaks the binary serialization format.  See comments for 
// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
// accordingly.
//==================================================================================================
public:
 
    friend class PxVehicleWheels;
 
    PxVehicleConstraintShader(PxVehicleWheels* vehicle, PxConstraint* constraint = NULL)
        : mConstraint(constraint),
          mVehicle(vehicle)
    {
    }
    PxVehicleConstraintShader(){}
    ~PxVehicleConstraintShader()
    {
    }
 
    static void getBinaryMetaData(PxOutputStream& stream);
 
    void release()
    {
        if(mConstraint)
        {
            mConstraint->release();
        }
    }
 
    virtual void            onComShift(PxU32 actor) { PX_UNUSED(actor); }
 
    virtual void            onOriginShift(const PxVec3& shift) { PX_UNUSED(shift); }
 
    virtual void*           prepareData()   
    {
        return &mData;
    }
 
    virtual bool            updatePvdProperties(pvdsdk::PvdDataStream& pvdConnection,
        const PxConstraint* c,
        PxPvdUpdateType::Enum updateType) const  { PX_UNUSED(c); PX_UNUSED(updateType); PX_UNUSED(&pvdConnection); return true;}
 
    virtual void            updateOmniPvdProperties() const { }
 
    virtual void            onConstraintRelease()
    {
        mVehicle->onConstraintRelease();
    }
 
    virtual void* getExternalReference(PxU32& typeID) { typeID = PxConstraintExtIDs::eVEHICLE_SUSP_LIMIT_DEPRECATED; return this; }
    virtual PxBase* getSerializable() { return NULL; }
 
    //TAG:solverprepshader
    static PxU32 vehicleSuspLimitConstraintSolverPrep(
        Px1DConstraint* constraints,
        PxVec3p& body0WorldOffset,
        PxU32 maxConstraints,
        PxConstraintInvMassScale&,
        const void* constantBlock,
        const PxTransform& bodyAToWorld,
        const PxTransform& bodyBToWorld,
        bool,
        PxVec3p& cA2w, PxVec3p& cB2w
        )
    {
        PX_UNUSED(maxConstraints);
        PX_UNUSED(body0WorldOffset);
        PX_UNUSED(bodyBToWorld);
        PX_ASSERT(bodyAToWorld.isValid()); PX_ASSERT(bodyBToWorld.isValid());
 
        const VehicleConstraintData* data = static_cast<const VehicleConstraintData*>(constantBlock);
        PxU32 numActive=0;
        const PxQuat bodyRotation = bodyAToWorld.q * data->mCMassRotation.getConjugate();
 
        //KS - the TGS solver will use raXn to try to add to the angular part of the linear constraints.
        //We overcome this by setting the ra and rb offsets to be 0.
        cA2w = bodyAToWorld.p;
        cB2w = bodyBToWorld.p;
 
        //Susp limit constraints.
        for(PxU32 i=0;i<4;i++)
        {
            if(data->mSuspLimitData.mActiveFlags[i])
            {
                Px1DConstraint& p=constraints[numActive];
                p.linear0 = bodyRotation.rotate(data->mSuspLimitData.mDirs[i]);
                p.angular0 = bodyRotation.rotate(data->mSuspLimitData.mCMOffsets[i].cross(data->mSuspLimitData.mDirs[i]));
                p.geometricError=data->mSuspLimitData.mErrors[i];
                p.linear1=PxVec3(0);
                p.angular1=PxVec3(0);
                p.minImpulse=-FLT_MAX;
                p.maxImpulse=0;
                p.velocityTarget=0;     
                p.flags |= Px1DConstraintFlag::eANGULAR_CONSTRAINT;
                numActive++;
            }
        }
 
        //Sticky tire friction constraints.
        for(PxU32 i=0;i<4;i++)
        {
            if(data->mStickyTireForwardData.mActiveFlags[i])
            {
                Px1DConstraint& p=constraints[numActive];
                p.linear0=data->mStickyTireForwardData.mDirs[i];
                p.angular0=data->mStickyTireForwardData.mCMOffsets[i].cross(data->mStickyTireForwardData.mDirs[i]);
                p.geometricError=0.0f;
                p.linear1=PxVec3(0);
                p.angular1=PxVec3(0);
                p.minImpulse=-FLT_MAX;
                p.maxImpulse=FLT_MAX;
                p.velocityTarget=data->mStickyTireForwardData.mTargetSpeeds[i]; 
                p.mods.spring.damping = 1000.0f;
                p.flags = Px1DConstraintFlag::eSPRING | Px1DConstraintFlag::eACCELERATION_SPRING;
                p.flags |= Px1DConstraintFlag::eANGULAR_CONSTRAINT;
                numActive++;
            }
        }
 
        //Sticky tire friction constraints.
        for(PxU32 i=0;i<4;i++)
        {
            if(data->mStickyTireSideData.mActiveFlags[i])
            {
                Px1DConstraint& p=constraints[numActive];
                p.linear0=data->mStickyTireSideData.mDirs[i];
                p.angular0=data->mStickyTireSideData.mCMOffsets[i].cross(data->mStickyTireSideData.mDirs[i]);
                p.geometricError=0.0f;
                p.linear1=PxVec3(0);
                p.angular1=PxVec3(0);
                p.minImpulse=-FLT_MAX;
                p.maxImpulse=FLT_MAX;
                p.velocityTarget=data->mStickyTireSideData.mTargetSpeeds[i];    
                p.mods.spring.damping = 1000.0f;
                p.flags = Px1DConstraintFlag::eSPRING | Px1DConstraintFlag::eACCELERATION_SPRING;
                p.flags |= Px1DConstraintFlag::eANGULAR_CONSTRAINT;
                numActive++;
            }
        }
 
 
        return numActive;
    }
 
    static void visualiseConstraint(PxConstraintVisualizer &viz,
        const void* constantBlock,
        const PxTransform& body0Transform,
        const PxTransform& body1Transform,
        PxU32 flags){ PX_UNUSED(&viz); PX_UNUSED(constantBlock); PX_UNUSED(body0Transform); 
                      PX_UNUSED(body1Transform); PX_UNUSED(flags); 
                      PX_ASSERT(body0Transform.isValid()); PX_ASSERT(body1Transform.isValid()); }
 
public:
 
    struct SuspLimitConstraintData
    {
        PxVec3 mCMOffsets[4];
        PxVec3 mDirs[4];
        PxReal mErrors[4];
        bool mActiveFlags[4];
    };
    struct StickyTireConstraintData
    {
        PxVec3 mCMOffsets[4];
        PxVec3 mDirs[4];
        PxReal mTargetSpeeds[4];
        bool mActiveFlags[4];
    };
 
    struct VehicleConstraintData
    {
        SuspLimitConstraintData mSuspLimitData;
        StickyTireConstraintData mStickyTireForwardData;
        StickyTireConstraintData mStickyTireSideData;
        PxQuat mCMassRotation;
    };
    VehicleConstraintData mData;
 
    PxConstraint* mConstraint;
    
    PX_INLINE void setPxConstraint(PxConstraint* pxConstraint)
    {
        mConstraint = pxConstraint;
    }
 
    PX_INLINE PxConstraint* getPxConstraint()
    {
        return mConstraint;
    }
 
    PxConstraintConnector* getConnector()
    {
        return this;
    }
 
    virtual PxConstraintSolverPrep getPrep() const { return vehicleSuspLimitConstraintSolverPrep; }
    virtual const void* getConstantBlock() const { return &mData; }
 
private:
 
    PxVehicleWheels* mVehicle;
 
#if !PX_P64_FAMILY
    PxU32 mPad[2];
#else
    PxU32 mPad[1];
#endif
};
PX_COMPILE_TIME_ASSERT(0==(sizeof(PxVehicleConstraintShader)& 0x0f));
 
 
void PxVehicleComputeTireForceDefault
 (const void* shaderData, 
 const PxF32 tireFriction,
 const PxF32 longSlip, const PxF32 latSlip, const PxF32 camber,
 const PxF32 wheelOmega, const PxF32 wheelRadius, const PxF32 recipWheelRadius,
 const PxF32 restTireLoad, const PxF32 normalisedTireLoad, const PxF32 tireLoad,
 const PxF32 gravity, const PxF32 recipGravity,
 PxF32& wheelTorque, PxF32& tireLongForceMag, PxF32& tireLatForceMag, PxF32& tireAlignMoment);
 
 
class PxVehicleTireForceCalculator
{
public:
 
    PxVehicleTireForceCalculator()
        : mShader(PxVehicleComputeTireForceDefault)
    {
    }
 
    const void** mShaderData;
 
    PxVehicleComputeTireForce mShader;
 
#if !PX_P64_FAMILY
    PxU32 mPad[2];
#endif
};
 
PX_COMPILE_TIME_ASSERT(0==(sizeof(PxVehicleTireForceCalculator) & 15));
 
 
#if !PX_DOXYGEN
} // namespace physx
#endif
 
 
#endif