PxVehiclePhysXConstraintStates.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/PxAssert.h"
#include "foundation/PxTransform.h"
#include "extensions/PxConstraintExt.h"
 
#include "vehicle2/PxVehicleLimits.h"
#include "vehicle2/tire/PxVehicleTireStates.h"
 
#include "PxConstraint.h"
#include "PxConstraintDesc.h"
 
#if !PX_DOXYGEN
namespace physx
{
 
class PxConstraint;
 
namespace vehicle2
{
#endif
 
 
struct PxVehiclePhysXConstraintLimits
{
    enum Enum
    {
        eNB_DOFS_PER_PXCONSTRAINT = 12,
        eNB_DOFS_PER_WHEEL = 3,
        eNB_WHEELS_PER_PXCONSTRAINT = eNB_DOFS_PER_PXCONSTRAINT / eNB_DOFS_PER_WHEEL,
        eNB_CONSTRAINTS_PER_VEHICLE = (PxVehicleLimits::eMAX_NB_WHEELS + (eNB_WHEELS_PER_PXCONSTRAINT - 1)) / (eNB_WHEELS_PER_PXCONSTRAINT)
    };
};
 
struct PxVehiclePhysXConstraintState
{
    bool tireActiveStatus[PxVehicleTireDirectionModes::eMAX_NB_PLANAR_DIRECTIONS];
    PxVec3 tireLinears[PxVehicleTireDirectionModes::eMAX_NB_PLANAR_DIRECTIONS];
    PxVec3 tireAngulars[PxVehicleTireDirectionModes::eMAX_NB_PLANAR_DIRECTIONS];
    PxReal tireDamping[PxVehicleTireDirectionModes::eMAX_NB_PLANAR_DIRECTIONS];
 
 
    bool suspActiveStatus;
    PxVec3 suspLinear;
    PxVec3 suspAngular;
    PxReal suspGeometricError;
    PxReal restitution;
 
    PX_FORCE_INLINE void setToDefault()
    {
        PxMemZero(this, sizeof(PxVehiclePhysXConstraintState));
    }
};
 
//TAG:solverprepshader
PX_FORCE_INLINE PxU32 vehicleConstraintSolverPrep
(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 PxVehiclePhysXConstraintState* data = static_cast<const PxVehiclePhysXConstraintState*>(constantBlock);
    PxU32 numActive = 0;
 
    //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;
    // note: this is only needed for PxSolverType::eTGS and even then it should not have an effect as
    //       long as a constraint raises Px1DConstraintFlag::eANGULAR_CONSTRAINT
 
    //Susp limit constraints.
    for (PxU32 i = 0; i < PxVehiclePhysXConstraintLimits::eNB_WHEELS_PER_PXCONSTRAINT; i++)
    {
        if (data[i].suspActiveStatus)
        {
            // Going beyond max suspension compression should be treated similar to rigid body contacts.
            // Thus setting up constraints that try to emulate such contacts.
            //
            // linear l = contact normal = n
            // angular a = suspension force application offset x contact normal = cross(r, n)
            //
            // velocity at contact:
            // vl: part from linear vehicle velocity v
            // vl = dot(n, v) = dot(l, v)
            //
            // va: part from angular vehicle velocity w
            // va = dot(n, cross(w, r)) = dot(w, cross(r, n)) = dot(w, a)
            //
            // ve: part from excess suspension compression
            // ve = (geomError / dt)    (note: geomError is expected to be negative here)
            //
            // velocity target vt = vl + va + ve
            // => should become 0 by applying positive impulse along l. If vt is positive,
            //    nothing will happen as a negative impulse would have to be applied (but
            //    min impulse is set to 0). If vt is negative, a positive impulse will get
            //    applied to push vt towards 0.
            //
 
            Px1DConstraint& p = constraints[numActive];
            p.linear0 = data[i].suspLinear;
            p.angular0 = data[i].suspAngular;
            p.geometricError = data[i].suspGeometricError;
            p.linear1 = PxVec3(0);
            p.angular1 = PxVec3(0);
            p.minImpulse = 0;
            p.maxImpulse = FLT_MAX;
            p.velocityTarget = 0;
            p.solveHint = PxConstraintSolveHint::eINEQUALITY;
 
            // note: this is only needed for PxSolverType::eTGS to not have the angular part
            //       be modified based on the linear part during substeps. Basically, it will
            //       disable the constraint re-projection etc. to emulate PxSolverType::ePGS.
            p.flags |= Px1DConstraintFlag::eANGULAR_CONSTRAINT;
 
            if (data[i].restitution > 0.0f)
            {
                p.flags |= Px1DConstraintFlag::eRESTITUTION;
                p.mods.bounce.restitution = data[i].restitution;
                p.mods.bounce.velocityThreshold = -FLT_MAX;
            }
            numActive++;
        }
    }
 
    //Sticky tire friction constraints.
    for (PxU32 i = 0; i < PxVehiclePhysXConstraintLimits::eNB_WHEELS_PER_PXCONSTRAINT; i++)
    {
        if (data[i].tireActiveStatus[PxVehicleTireDirectionModes::eLONGITUDINAL])
        {
            Px1DConstraint& p = constraints[numActive];
            p.linear0 = data[i].tireLinears[PxVehicleTireDirectionModes::eLONGITUDINAL];
            p.angular0 = data[i].tireAngulars[PxVehicleTireDirectionModes::eLONGITUDINAL];
            p.geometricError = 0.0f;
            p.linear1 = PxVec3(0);
            p.angular1 = PxVec3(0);
            p.minImpulse = -FLT_MAX;
            p.maxImpulse = FLT_MAX;
            p.velocityTarget = 0.0f;
            p.mods.spring.damping = data[i].tireDamping[PxVehicleTireDirectionModes::eLONGITUDINAL];
            // note: no stiffness specified as this will have no effect with geometricError=0
            p.flags = Px1DConstraintFlag::eSPRING | Px1DConstraintFlag::eACCELERATION_SPRING;
            p.flags |= Px1DConstraintFlag::eANGULAR_CONSTRAINT;  // see explanation of same flag usage further above
            numActive++;
        }
    }
 
    //Sticky tire friction constraints.
    for (PxU32 i = 0; i < PxVehiclePhysXConstraintLimits::eNB_WHEELS_PER_PXCONSTRAINT; i++)
    {
        if (data[i].tireActiveStatus[PxVehicleTireDirectionModes::eLATERAL])
        {
            Px1DConstraint& p = constraints[numActive];
            p.linear0 = data[i].tireLinears[PxVehicleTireDirectionModes::eLATERAL];
            p.angular0 = data[i].tireAngulars[PxVehicleTireDirectionModes::eLATERAL];
            p.geometricError = 0.0f;
            p.linear1 = PxVec3(0);
            p.angular1 = PxVec3(0);
            p.minImpulse = -FLT_MAX;
            p.maxImpulse = FLT_MAX;
            p.velocityTarget = 0.0f;
            p.mods.spring.damping = data[i].tireDamping[PxVehicleTireDirectionModes::eLATERAL];
            // note: no stiffness specified as this will have no effect with geometricError=0
            p.flags = Px1DConstraintFlag::eSPRING | Px1DConstraintFlag::eACCELERATION_SPRING;
            p.flags |= Px1DConstraintFlag::eANGULAR_CONSTRAINT;  // see explanation of same flag usage further above
            numActive++;
        }
    }
 
    return numActive;
}
 
PX_FORCE_INLINE void visualiseVehicleConstraint
(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());
}
 
class PxVehicleConstraintConnector : public PxConstraintConnector
{
public:
 
    PxVehicleConstraintConnector() : mVehicleConstraintState(NULL) {}
    PxVehicleConstraintConnector(PxVehiclePhysXConstraintState* vehicleConstraintState) : mVehicleConstraintState(vehicleConstraintState) {}
    ~PxVehicleConstraintConnector() {}
 
    void setConstraintState(PxVehiclePhysXConstraintState* constraintState) { mVehicleConstraintState = constraintState; }
 
    virtual void* prepareData() { return mVehicleConstraintState; }
    virtual const void* getConstantBlock() const { return mVehicleConstraintState; }
    virtual PxConstraintSolverPrep getPrep() const { return vehicleConstraintSolverPrep; }
 
    //Is this necessary if physx no longer supports double-buffering?
    virtual void onConstraintRelease() { }
 
    //Can be empty functions.
    virtual bool updatePvdProperties(physx::pvdsdk::PvdDataStream& pvdConnection, const PxConstraint* c, PxPvdUpdateType::Enum updateType) const
    {
        PX_UNUSED(pvdConnection);
        PX_UNUSED(c);
        PX_UNUSED(updateType);
        return true;
    }
    virtual void updateOmniPvdProperties() const { }
    virtual void onComShift(PxU32 actor) { PX_UNUSED(actor); }
    virtual void onOriginShift(const PxVec3& shift) { PX_UNUSED(shift); }
    virtual void*   getExternalReference(PxU32& typeID) { typeID = PxConstraintExtIDs::eVEHICLE_JOINT; return this; }
    virtual PxBase* getSerializable() { return NULL; }
 
private:
 
    PxVehiclePhysXConstraintState* mVehicleConstraintState;
};
 
struct PxVehiclePhysXConstraints
{
    PxVehiclePhysXConstraintState constraintStates[PxVehicleLimits::eMAX_NB_WHEELS];
 
    PxConstraint* constraints[PxVehiclePhysXConstraintLimits::eNB_CONSTRAINTS_PER_VEHICLE];
 
    PxVehicleConstraintConnector* constraintConnectors[PxVehiclePhysXConstraintLimits::eNB_CONSTRAINTS_PER_VEHICLE];
 
    PX_FORCE_INLINE void setToDefault()
    {
        for (PxU32 i = 0; i < PxVehicleLimits::eMAX_NB_WHEELS; i++)
        {
            constraintStates[i].setToDefault();
        }
        for(PxU32 i = 0; i < PxVehiclePhysXConstraintLimits::eNB_CONSTRAINTS_PER_VEHICLE; i++)
        {
            constraints[i] = NULL;
            constraintConnectors[i] = NULL;
        }
    }
};
 
#if !PX_DOXYGEN
} // namespace vehicle2
} // namespace physx
#endif