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-2024 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