251 lines
9.2 KiB
C++
251 lines
9.2 KiB
C++
// 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-2025 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#include "foundation/PxMathUtils.h"
|
|
#include "extensions/PxD6JointCreate.h"
|
|
#include "extensions/PxD6Joint.h"
|
|
#include "extensions/PxFixedJoint.h"
|
|
#include "extensions/PxRevoluteJoint.h"
|
|
#include "extensions/PxSphericalJoint.h"
|
|
#include "extensions/PxPrismaticJoint.h"
|
|
#include "extensions/PxDistanceJoint.h"
|
|
#include "PxPhysics.h"
|
|
|
|
using namespace physx;
|
|
|
|
static const PxVec3 gX(1.0f, 0.0f, 0.0f);
|
|
|
|
PxJoint* physx::PxD6JointCreate_Fixed(PxPhysics& physics, PxRigidActor* actor0, const PxVec3& localPos0, PxRigidActor* actor1, const PxVec3& localPos1, bool useD6)
|
|
{
|
|
const PxTransform jointFrame0(localPos0);
|
|
const PxTransform jointFrame1(localPos1);
|
|
if(useD6)
|
|
// PT: by default all D6 axes are locked, i.e. it is a fixed joint.
|
|
return PxD6JointCreate(physics, actor0, jointFrame0, actor1, jointFrame1);
|
|
else
|
|
return PxFixedJointCreate(physics, actor0, jointFrame0, actor1, jointFrame1);
|
|
}
|
|
|
|
PxJoint* physx::PxD6JointCreate_Distance(PxPhysics& physics, PxRigidActor* actor0, const PxVec3& localPos0, PxRigidActor* actor1, const PxVec3& localPos1, float maxDist, bool useD6)
|
|
{
|
|
const PxTransform localFrame0(localPos0);
|
|
const PxTransform localFrame1(localPos1);
|
|
|
|
if(useD6)
|
|
{
|
|
PxD6Joint* j = PxD6JointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
|
|
j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
|
|
j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
|
|
|
|
j->setMotion(PxD6Axis::eX, PxD6Motion::eLIMITED);
|
|
j->setMotion(PxD6Axis::eY, PxD6Motion::eLIMITED);
|
|
j->setMotion(PxD6Axis::eZ, PxD6Motion::eLIMITED);
|
|
j->setDistanceLimit(PxJointLinearLimit(maxDist));
|
|
return j;
|
|
}
|
|
else
|
|
{
|
|
PxDistanceJoint* j = PxDistanceJointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
j->setDistanceJointFlag(PxDistanceJointFlag::eMAX_DISTANCE_ENABLED, true);
|
|
j->setMaxDistance(maxDist);
|
|
return j;
|
|
}
|
|
}
|
|
|
|
PxJoint* physx::PxD6JointCreate_Prismatic(PxPhysics& physics, PxRigidActor* actor0, const PxVec3& localPos0, PxRigidActor* actor1, const PxVec3& localPos1, const PxVec3& axis, float minLimit, float maxLimit, bool useD6)
|
|
{
|
|
const PxQuat q = PxShortestRotation(gX, axis);
|
|
const PxTransform localFrame0(localPos0, q);
|
|
const PxTransform localFrame1(localPos1, q);
|
|
|
|
const PxJointLinearLimitPair limit(PxTolerancesScale(), minLimit, maxLimit);
|
|
|
|
if(useD6)
|
|
{
|
|
PxD6Joint* j = PxD6JointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
j->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
|
|
if(minLimit==maxLimit)
|
|
j->setMotion(PxD6Axis::eX, PxD6Motion::eLOCKED);
|
|
else if(minLimit>maxLimit)
|
|
j->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
|
|
else// if(minLimit<maxLimit)
|
|
{
|
|
j->setMotion(PxD6Axis::eX, PxD6Motion::eLIMITED);
|
|
j->setLinearLimit(PxD6Axis::eX, limit);
|
|
}
|
|
return j;
|
|
}
|
|
else
|
|
{
|
|
PxPrismaticJoint* j = PxPrismaticJointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
if(minLimit<maxLimit)
|
|
{
|
|
j->setPrismaticJointFlag(PxPrismaticJointFlag::eLIMIT_ENABLED, true);
|
|
j->setLimit(limit);
|
|
}
|
|
return j;
|
|
}
|
|
}
|
|
|
|
PxJoint* physx::PxD6JointCreate_Revolute(PxPhysics& physics, PxRigidActor* actor0, const PxVec3& localPos0, PxRigidActor* actor1, const PxVec3& localPos1, const PxVec3& axis, float minLimit, float maxLimit, bool useD6)
|
|
{
|
|
const PxQuat q = PxShortestRotation(gX, axis);
|
|
const PxTransform localFrame0(localPos0, q);
|
|
const PxTransform localFrame1(localPos1, q);
|
|
|
|
const PxJointAngularLimitPair limit(minLimit, maxLimit);
|
|
|
|
if(useD6)
|
|
{
|
|
PxD6Joint* j = PxD6JointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
if(minLimit==maxLimit)
|
|
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eLOCKED);
|
|
else if(minLimit>maxLimit)
|
|
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
|
|
else// if(minLimit<maxLimit)
|
|
{
|
|
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eLIMITED);
|
|
j->setTwistLimit(limit);
|
|
}
|
|
return j;
|
|
}
|
|
else
|
|
{
|
|
PxRevoluteJoint* j = PxRevoluteJointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
if(minLimit<maxLimit)
|
|
{
|
|
j->setRevoluteJointFlag(PxRevoluteJointFlag::eLIMIT_ENABLED, true);
|
|
j->setLimit(limit);
|
|
}
|
|
return j;
|
|
}
|
|
}
|
|
|
|
PxJoint* physx::PxD6JointCreate_Spherical(PxPhysics& physics, PxRigidActor* actor0, const PxVec3& localPos0, PxRigidActor* actor1, const PxVec3& localPos1, const PxVec3& axis, float limit1, float limit2, bool useD6)
|
|
{
|
|
const PxQuat q = PxShortestRotation(gX, axis);
|
|
const PxTransform localFrame0(localPos0, q);
|
|
const PxTransform localFrame1(localPos1, q);
|
|
|
|
const PxJointLimitCone limit(limit1, limit2);
|
|
|
|
if(useD6)
|
|
{
|
|
PxD6Joint* j = PxD6JointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
|
|
if(limit1>0.0f && limit2>0.0f)
|
|
{
|
|
j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLIMITED);
|
|
j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLIMITED);
|
|
j->setSwingLimit(limit);
|
|
}
|
|
else
|
|
{
|
|
j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
|
|
j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
|
|
}
|
|
return j;
|
|
}
|
|
else
|
|
{
|
|
PxSphericalJoint* j = PxSphericalJointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
if(limit1>0.0f && limit2>0.0f)
|
|
{
|
|
j->setSphericalJointFlag(PxSphericalJointFlag::eLIMIT_ENABLED, true);
|
|
j->setLimitCone(limit);
|
|
}
|
|
return j;
|
|
}
|
|
}
|
|
|
|
PxJoint* physx::PxD6JointCreate_GenericCone(float& apiroty, float& apirotz, PxPhysics& physics, PxRigidActor* actor0, const PxVec3& localPos0, PxRigidActor* actor1, const PxVec3& localPos1, float minLimit1, float maxLimit1, float minLimit2, float maxLimit2, bool useD6)
|
|
{
|
|
const float DesiredMinSwingY = minLimit1;
|
|
const float DesiredMaxSwingY = maxLimit1;
|
|
const float DesiredMinSwingZ = minLimit2;
|
|
const float DesiredMaxSwingZ = maxLimit2;
|
|
const float APIMaxY = (DesiredMaxSwingY - DesiredMinSwingY)*0.5f;
|
|
const float APIMaxZ = (DesiredMaxSwingZ - DesiredMinSwingZ)*0.5f;
|
|
const float APIRotY = (DesiredMaxSwingY + DesiredMinSwingY)*0.5f;
|
|
const float APIRotZ = (DesiredMaxSwingZ + DesiredMinSwingZ)*0.5f;
|
|
apiroty = APIRotY;
|
|
apirotz = APIRotZ;
|
|
|
|
const PxQuat RotY = PxGetRotYQuat(APIRotY);
|
|
const PxQuat RotZ = PxGetRotZQuat(APIRotZ);
|
|
const PxQuat Rot = RotY * RotZ;
|
|
|
|
const PxTransform localFrame0(localPos0, Rot);
|
|
const PxTransform localFrame1(localPos1);
|
|
|
|
const PxJointLimitCone limit(APIMaxY, APIMaxZ);
|
|
|
|
if(useD6)
|
|
{
|
|
PxD6Joint* j = PxD6JointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
|
|
j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLIMITED);
|
|
j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLIMITED);
|
|
j->setSwingLimit(limit);
|
|
return j;
|
|
}
|
|
else
|
|
{
|
|
PxSphericalJoint* j = PxSphericalJointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
j->setSphericalJointFlag(PxSphericalJointFlag::eLIMIT_ENABLED, true);
|
|
j->setLimitCone(limit);
|
|
return j;
|
|
}
|
|
}
|
|
|
|
PxJoint* physx::PxD6JointCreate_Pyramid(PxPhysics& physics, PxRigidActor* actor0, const PxVec3& localPos0, PxRigidActor* actor1, const PxVec3& localPos1, const PxVec3& axis,
|
|
float minLimit1, float maxLimit1, float minLimit2, float maxLimit2)
|
|
{
|
|
const PxQuat q = PxShortestRotation(gX, axis);
|
|
const PxTransform localFrame0(localPos0, q);
|
|
const PxTransform localFrame1(localPos1, q);
|
|
|
|
const PxJointLimitPyramid limit(minLimit1, maxLimit1, minLimit2, maxLimit2);
|
|
|
|
PxD6Joint* j = PxD6JointCreate(physics, actor0, localFrame0, actor1, localFrame1);
|
|
j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
|
|
if(limit.isValid())
|
|
{
|
|
j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLIMITED);
|
|
j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLIMITED);
|
|
j->setPyramidSwingLimit(limit);
|
|
}
|
|
else
|
|
{
|
|
j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
|
|
j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
|
|
}
|
|
return j;
|
|
}
|