include/vehicle2/PxVehicleFunctions.h

File members: include/vehicle2/PxVehicleFunctions.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/PxTransform.h"
#include "foundation/PxMat33.h"
#include "foundation/PxSimpleTypes.h"
#include "PxRigidBody.h"
#include "PxVehicleParams.h"
#include "roadGeometry/PxVehicleRoadGeometryState.h"
#include "rigidBody/PxVehicleRigidBodyStates.h"
#include "physxRoadGeometry/PxVehiclePhysXRoadGeometryState.h"
#include "physxActor/PxVehiclePhysXActorStates.h"

#if !PX_DOXYGEN
namespace physx
{
namespace vehicle2
{
#endif
PX_FORCE_INLINE PxVec3 PxVehicleTransformFrameToFrame
(const PxVehicleFrame& srcFrame, const PxVehicleFrame& trgFrame, const PxVec3& v)
{
    PxVec3 result = v;
    if ((srcFrame.lngAxis != trgFrame.lngAxis) || (srcFrame.latAxis != trgFrame.latAxis) || (srcFrame.vrtAxis != trgFrame.vrtAxis))
    {
        const PxMat33 a = srcFrame.getFrame();
        const PxMat33 r = trgFrame.getFrame();
        result = (r * a.getTranspose() * v);
    }
    return result;
}

PX_FORCE_INLINE PxVec3 PxVehicleTransformFrameToFrame
(const PxVehicleFrame& srcFrame, const PxVehicleFrame& trgFrame,
 const PxVehicleScale& srcScale, const PxVehicleScale& trgScale,
 const PxVec3& v)
{
    PxVec3 result = PxVehicleTransformFrameToFrame(srcFrame, trgFrame, v);
    if((srcScale.scale != trgScale.scale))
        result *= (trgScale.scale / srcScale.scale);
    return result;
}

PX_FORCE_INLINE PxTransform PxVehicleTransformFrameToFrame
(const PxVehicleFrame& srcFrame, const PxVehicleFrame& trgFrame,
 const PxVehicleScale& srcScale, const PxVehicleScale& trgScale,
 const PxTransform& v)
{
    PxTransform result(PxVehicleTransformFrameToFrame(srcFrame, trgFrame, srcScale, trgScale, v.p), v.q);
    if ((srcFrame.lngAxis != trgFrame.lngAxis) || (srcFrame.latAxis != trgFrame.latAxis) || (srcFrame.vrtAxis != trgFrame.vrtAxis))
    {
        PxF32 angle;
        PxVec3 axis;
        v.q.toRadiansAndUnitAxis(angle, axis);
        result.q = PxQuat(angle, PxVehicleTransformFrameToFrame(srcFrame, trgFrame, axis));
    }
    return result;
}

PX_FORCE_INLINE PxVec3 PxVehicleComputeTranslation(const PxVehicleFrame& frame, const PxReal lng, const PxReal lat, const PxReal vrt)
{
    const PxVec3 v = frame.getFrame()*PxVec3(lng, lat, vrt);
    return v;
}

PX_FORCE_INLINE PxQuat PxVehicleComputeRotation(const PxVehicleFrame& frame, const PxReal roll, const PxReal pitch, const PxReal yaw)
{
    const PxMat33 m = frame.getFrame();
    const PxVec3& lngAxis = m.column0;
    const PxVec3& latAxis = m.column1;
    const PxVec3& vrtAxis = m.column2;
    const PxQuat quatPitch(pitch, latAxis);
    const PxQuat quatRoll(roll, lngAxis);
    const PxQuat quatYaw(yaw, vrtAxis);
    const PxQuat result = quatYaw * quatRoll * quatPitch;
    return result;
}

PX_FORCE_INLINE PxF32 PxVehicleComputeSign(const PxReal f)
{
    return physx::intrinsics::fsel(f, physx::intrinsics::fsel(-f, 0.0f, 1.0f), -1.0f);
}

PX_FORCE_INLINE void PxVehicleShiftOrigin
(const PxVehicleAxleDescription& axleDesc, const PxVec3& shift,
 PxVehicleRigidBodyState& rigidBodyState, PxVehicleRoadGeometryState* roadGeometryStates,
 PxVehiclePhysXActor* physxActor = NULL, PxVehiclePhysXRoadGeometryQueryState* physxQueryStates = NULL)
{
    //Adjust the vehicle's internal pose.
    rigidBodyState.pose.p -= shift;

    //Optionally adjust the PxRigidActor pose.
    if (physxActor && !physxActor->rigidBody->getScene())
    {
        const PxTransform oldPose = physxActor->rigidBody->getGlobalPose();
        const PxTransform newPose(oldPose.p - shift, oldPose.q);
        physxActor->rigidBody->setGlobalPose(newPose);
    }

    for (PxU32 i = 0; i < axleDesc.nbWheels; i++)
    {
        const PxU32 wheelId = axleDesc.wheelIdsInAxleOrder[i];

        //Optionally adjust the hit position.
        if (physxQueryStates && physxQueryStates[wheelId].actor)
            physxQueryStates[wheelId].hitPosition -= shift;

        //Adjust the hit plane.
        if (roadGeometryStates[wheelId].hitState)
        {
            const PxPlane plane = roadGeometryStates[wheelId].plane;
            PxU32 largestNormalComponentAxis = 0;
            PxReal largestNormalComponent = 0.0f;
            const PxF32 normalComponents[3] = { plane.n.x, plane.n.y, plane.n.z };
            for (PxU32 k = 0; k < 3; k++)
            {
                if (PxAbs(normalComponents[k]) > largestNormalComponent)
                {
                    largestNormalComponent = PxAbs(normalComponents[k]);
                    largestNormalComponentAxis = k;
                }
            }
            PxVec3 pointInPlane(PxZero);
            switch (largestNormalComponentAxis)
            {
            case 0:
                pointInPlane.x = -plane.d / plane.n.x;
                break;
            case 1:
                pointInPlane.y = -plane.d / plane.n.y;
                break;
            case 2:
                pointInPlane.z = -plane.d / plane.n.z;
                break;
            default:
                break;
            }
            roadGeometryStates[wheelId].plane.d = -plane.n.dot(pointInPlane - shift);
        }
    }
}

#if !PX_DOXYGEN
} // namespace vehicle2
} // namespace physx
#endif