1366 lines
54 KiB
C++
1366 lines
54 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 "common/PxRenderBuffer.h"
|
|
|
|
#include "ExtD6Joint.h"
|
|
#include "ExtConstraintHelper.h"
|
|
#include "CmConeLimitHelper.h"
|
|
|
|
#include "omnipvd/ExtOmniPvdSetData.h"
|
|
|
|
using namespace physx;
|
|
using namespace Ext;
|
|
|
|
D6Joint::D6Joint(const PxTolerancesScale& scale, PxRigidActor* actor0, const PxTransform& localFrame0, PxRigidActor* actor1, const PxTransform& localFrame1) :
|
|
D6JointT (PxJointConcreteType::eD6, actor0, localFrame0, actor1, localFrame1, "D6JointData"),
|
|
mRecomputeMotion(true)
|
|
{
|
|
D6JointData* data = static_cast<D6JointData*>(mData);
|
|
|
|
for(PxU32 i=0;i<6;i++)
|
|
data->motion[i] = PxD6Motion::eLOCKED;
|
|
|
|
data->twistLimit = PxJointAngularLimitPair(-PxPi/2, PxPi/2);
|
|
data->swingLimit = PxJointLimitCone(PxPi/2, PxPi/2);
|
|
data->pyramidSwingLimit = PxJointLimitPyramid(-PxPi/2, PxPi/2, -PxPi/2, PxPi/2);
|
|
data->distanceLimit = PxJointLinearLimit(PX_MAX_F32);
|
|
data->distanceMinDist = 1e-6f*scale.length;
|
|
|
|
data->linearLimitX = PxJointLinearLimitPair(scale);
|
|
data->linearLimitY = PxJointLinearLimitPair(scale);
|
|
data->linearLimitZ = PxJointLinearLimitPair(scale);
|
|
|
|
for(PxU32 i=0;i<D6JointData::sDriveEntryCapacity;i++)
|
|
data->drive[i] = PxD6JointDrive();
|
|
|
|
data->drivePosition = PxTransform(PxIdentity);
|
|
data->driveLinearVelocity = PxVec3(0.0f);
|
|
data->driveAngularVelocity = PxVec3(0.0f);
|
|
|
|
data->mUseDistanceLimit = false;
|
|
data->mUseNewLinearLimits = false;
|
|
data->mUseConeLimit = false;
|
|
data->mUsePyramidLimits = false;
|
|
|
|
data->angularDriveConfig = PxD6AngularDriveConfig::eLEGACY;
|
|
}
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
|
|
PX_FORCE_INLINE static const PxD6JointDrive* omniPvdCreateDriveObjectHandle(PxD6Drive::Enum type, const D6JointData& jData)
|
|
{
|
|
// creating fake object handles to work around an omni PVD issue where references to objects that
|
|
// get deleted/created (using the same memory address) are not working properly (the references point
|
|
// to old objects). In general, it is not important what the handles are, they just need to be unique.
|
|
|
|
PX_COMPILE_TIME_ASSERT(sizeof(jData.drive) >= PxD6Drive::eCOUNT);
|
|
// using the base address for joint drive data and adding the type value to get a unique ID for all
|
|
// drive types (note that we do not allocate memory for each drive type because some can not be active
|
|
// at the same time).
|
|
|
|
return reinterpret_cast<const PxD6JointDrive*>(reinterpret_cast<size_t>(jData.drive) + type);
|
|
}
|
|
|
|
#define EXT_OMNI_PVD_DESTROY_DRIVE_DATA(driveType, jointData) \
|
|
{ \
|
|
const PxD6JointDrive* objectHandle = omniPvdCreateDriveObjectHandle(driveType, jointData); \
|
|
OMNI_PVD_DESTROY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6JointDrive, *objectHandle) \
|
|
}
|
|
|
|
D6Joint::~D6Joint()
|
|
{
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const D6JointData& jointData = data();
|
|
|
|
EXT_OMNI_PVD_DESTROY_DRIVE_DATA(PxD6Drive::eX, jointData)
|
|
EXT_OMNI_PVD_DESTROY_DRIVE_DATA(PxD6Drive::eY, jointData)
|
|
EXT_OMNI_PVD_DESTROY_DRIVE_DATA(PxD6Drive::eZ, jointData)
|
|
EXT_OMNI_PVD_DESTROY_DRIVE_DATA(PxD6Drive::eSWING, jointData)
|
|
EXT_OMNI_PVD_DESTROY_DRIVE_DATA(PxD6Drive::eTWIST, jointData)
|
|
EXT_OMNI_PVD_DESTROY_DRIVE_DATA(PxD6Drive::eSLERP, jointData)
|
|
EXT_OMNI_PVD_DESTROY_DRIVE_DATA(PxD6Drive::eSWING1, jointData)
|
|
EXT_OMNI_PVD_DESTROY_DRIVE_DATA(PxD6Drive::eSWING2, jointData)
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
}
|
|
|
|
#endif
|
|
|
|
PxD6Motion::Enum D6Joint::getMotion(PxD6Axis::Enum index) const
|
|
{
|
|
return data().motion[index];
|
|
}
|
|
|
|
void D6Joint::setMotion(PxD6Axis::Enum index, PxD6Motion::Enum t)
|
|
{
|
|
data().motion[index] = t;
|
|
mRecomputeMotion = true;
|
|
markDirty();
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
PxD6Motion::Enum motions[PxD6Axis::eCOUNT];
|
|
for (PxU32 i = 0; i < PxD6Axis::eCOUNT; ++i)
|
|
motions[i] = getMotion(PxD6Axis::Enum(i));
|
|
OMNI_PVD_SET_ARRAY(OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, motions, static_cast<PxD6Joint&>(*this), motions, PxD6Axis::eCOUNT)
|
|
#endif
|
|
}
|
|
|
|
PxReal D6Joint::getTwistAngle() const
|
|
{
|
|
return getTwistAngle_Internal();
|
|
}
|
|
|
|
PxReal D6Joint::getSwingYAngle() const
|
|
{
|
|
return getSwingYAngle_Internal();
|
|
}
|
|
|
|
PxReal D6Joint::getSwingZAngle() const
|
|
{
|
|
return getSwingZAngle_Internal();
|
|
}
|
|
|
|
#if PX_CHECKED
|
|
static bool isDriveTypeAllowed(PxD6Drive::Enum driveType, PxD6AngularDriveConfig::Enum driveConfig, const char* apiName)
|
|
{
|
|
if (driveConfig == PxD6AngularDriveConfig::eSWING_TWIST)
|
|
{
|
|
if ((driveType <= PxD6Drive::eZ) || (driveType == PxD6Drive::eSWING1) || (driveType == PxD6Drive::eSWING2) || (driveType == PxD6Drive::eTWIST))
|
|
return true;
|
|
else
|
|
{
|
|
PxGetFoundation().error(physx::PxErrorCode::eINVALID_PARAMETER, PX_FL, "%s: with angular drive configuration PxD6AngularDriveConfig::eSWING_TWIST, "
|
|
"drive parameters for PxD6Drive::eSWING and ::eSLERP are not accessible.", apiName);
|
|
}
|
|
}
|
|
else if (driveConfig == PxD6AngularDriveConfig::eSLERP)
|
|
{
|
|
if ((driveType <= PxD6Drive::eZ) || (driveType == PxD6Drive::eSLERP))
|
|
return true;
|
|
else
|
|
{
|
|
PxGetFoundation().error(physx::PxErrorCode::eINVALID_PARAMETER, PX_FL, "%s: with angular drive configuration PxD6AngularDriveConfig::eSLERP, "
|
|
"drive parameters for PxD6Drive::eSWING, ::eTWIST, ::eSWING1 and ::eSWING2 are not accessible.", apiName);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
PX_ASSERT(driveConfig == PxD6AngularDriveConfig::eLEGACY);
|
|
|
|
if ((driveType <= PxD6Drive::eZ) || (driveType == PxD6Drive::eSWING) || (driveType == PxD6Drive::eTWIST) || (driveType == PxD6Drive::eSLERP))
|
|
return true;
|
|
else
|
|
{
|
|
PxGetFoundation().error(physx::PxErrorCode::eINVALID_PARAMETER, PX_FL, "%s: with angular drive configuration PxD6AngularDriveConfig::eLEGACY, "
|
|
"drive parameters for PxD6Drive::eSWING1 and ::eSWING2 are not accessible.", apiName);
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
#endif
|
|
|
|
static constexpr PxU32 gDriveXDataIndex = 0;
|
|
PX_COMPILE_TIME_ASSERT(gDriveXDataIndex < D6JointData::sDriveEntryCapacity);
|
|
|
|
static constexpr PxU32 gDriveYDataIndex = 1;
|
|
PX_COMPILE_TIME_ASSERT(gDriveYDataIndex < D6JointData::sDriveEntryCapacity);
|
|
|
|
static constexpr PxU32 gDriveZDataIndex = 2;
|
|
PX_COMPILE_TIME_ASSERT(gDriveZDataIndex < D6JointData::sDriveEntryCapacity);
|
|
|
|
static constexpr PxU32 gDriveSwingDataIndex = 3;
|
|
PX_COMPILE_TIME_ASSERT(gDriveSwingDataIndex < D6JointData::sDriveEntryCapacity);
|
|
|
|
static constexpr PxU32 gDriveTwistDataIndex = 4;
|
|
PX_COMPILE_TIME_ASSERT(gDriveTwistDataIndex < D6JointData::sDriveEntryCapacity);
|
|
|
|
static constexpr PxU32 gDriveSlerpDataIndex = 5;
|
|
PX_COMPILE_TIME_ASSERT(gDriveSlerpDataIndex < D6JointData::sDriveEntryCapacity);
|
|
|
|
static constexpr PxU32 gDriveSwing1DataIndex = gDriveSwingDataIndex; // PxD6Drive::eSWING1 maps to the data entry of PxD6Drive::eSWING
|
|
PX_COMPILE_TIME_ASSERT(gDriveSwing1DataIndex < D6JointData::sDriveEntryCapacity);
|
|
|
|
static constexpr PxU32 gDriveSwing2DataIndex = gDriveSlerpDataIndex; // PxD6Drive::eSWING2 maps to the data entry of PxD6Drive::eSLERP
|
|
PX_COMPILE_TIME_ASSERT(gDriveSwing2DataIndex < D6JointData::sDriveEntryCapacity);
|
|
|
|
// internally, PxD6Drive::eSWING1 and PxD6Drive::eSWING2 are mapped to indices
|
|
// of drive parameters that are not used for PxD6AngularDriveConfig::eSWING_TWIST
|
|
// such that the size of D6JointData does not have to be increased for no good
|
|
// reason
|
|
static constexpr PxU32 gDriveTypeToIndexMap[] = {
|
|
gDriveXDataIndex,
|
|
gDriveYDataIndex,
|
|
gDriveZDataIndex,
|
|
gDriveSwingDataIndex,
|
|
gDriveTwistDataIndex,
|
|
gDriveSlerpDataIndex,
|
|
gDriveSwing1DataIndex,
|
|
gDriveSwing2DataIndex
|
|
};
|
|
PX_COMPILE_TIME_ASSERT(gDriveTypeToIndexMap[PxD6Drive::eX] == gDriveXDataIndex);
|
|
PX_COMPILE_TIME_ASSERT(gDriveTypeToIndexMap[PxD6Drive::eY] == gDriveYDataIndex);
|
|
PX_COMPILE_TIME_ASSERT(gDriveTypeToIndexMap[PxD6Drive::eZ] == gDriveZDataIndex);
|
|
PX_COMPILE_TIME_ASSERT(gDriveTypeToIndexMap[PxD6Drive::eSWING] == gDriveSwingDataIndex);
|
|
PX_COMPILE_TIME_ASSERT(gDriveTypeToIndexMap[PxD6Drive::eTWIST] == gDriveTwistDataIndex);
|
|
PX_COMPILE_TIME_ASSERT(gDriveTypeToIndexMap[PxD6Drive::eSLERP] == gDriveSlerpDataIndex);
|
|
PX_COMPILE_TIME_ASSERT(gDriveTypeToIndexMap[PxD6Drive::eSWING1] == gDriveSwing1DataIndex);
|
|
PX_COMPILE_TIME_ASSERT(gDriveTypeToIndexMap[PxD6Drive::eSWING2] == gDriveSwing2DataIndex);
|
|
|
|
PX_FORCE_INLINE static PxU32 getDriveDataIndex(PxD6Drive::Enum driveType)
|
|
{
|
|
return gDriveTypeToIndexMap[driveType];
|
|
}
|
|
|
|
PxD6JointDrive D6Joint::getDrive(PxD6Drive::Enum driveType) const
|
|
{
|
|
#if PX_CHECKED
|
|
if (!isDriveTypeAllowed(driveType, static_cast<PxD6AngularDriveConfig::Enum>(data().angularDriveConfig), "PxD6Joint::getDrive"))
|
|
return PxD6JointDrive();
|
|
#endif
|
|
|
|
const PxU32 dataIndex = getDriveDataIndex(driveType);
|
|
return data().drive[dataIndex];
|
|
}
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
|
|
PX_FORCE_INLINE static void omniPvdSetDriveData(const PxD6JointDrive& driveData, const PxD6JointDrive& objectHandle,
|
|
OmniPvdWriter* pvdWriter, const OmniPvdPxExtensionsRegistrationData* pvdRegData)
|
|
{
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6JointDrive, stiffness, objectHandle, driveData.stiffness)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6JointDrive, damping, objectHandle, driveData.damping)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6JointDrive, forceLimit, objectHandle, driveData.forceLimit)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6JointDrive, flags, objectHandle, driveData.flags)
|
|
}
|
|
|
|
#endif
|
|
|
|
void D6Joint::setDrive(PxD6Drive::Enum driveType, const PxD6JointDrive& d)
|
|
{
|
|
PX_CHECK_AND_RETURN(d.isValid(), "PxD6Joint::setDrive: drive is invalid");
|
|
|
|
D6JointData& jData = data();
|
|
|
|
#if PX_CHECKED
|
|
if (!isDriveTypeAllowed(driveType, static_cast<PxD6AngularDriveConfig::Enum>(jData.angularDriveConfig), "PxD6Joint::setDrive"))
|
|
return;
|
|
#endif
|
|
|
|
const PxU32 dataIndex = getDriveDataIndex(driveType);
|
|
|
|
jData.drive[dataIndex] = d;
|
|
mRecomputeMotion = true;
|
|
markDirty();
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const PxD6JointDrive* objectHandle = omniPvdCreateDriveObjectHandle(driveType, jData);
|
|
omniPvdSetDriveData(jData.drive[dataIndex], *objectHandle, pvdWriter, pvdRegData);
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
|
|
void D6Joint::setDistanceLimit(const PxJointLinearLimit& l)
|
|
{
|
|
PX_CHECK_AND_RETURN(l.isValid(), "PxD6Joint::setDistanceLimit: limit invalid");
|
|
data().distanceLimit = l;
|
|
data().mUseDistanceLimit = true;
|
|
markDirty();
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const PxD6Joint& j = static_cast<const PxD6Joint&>(*this);
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, distanceLimitValue, j, l.value)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, distanceLimitRestitution, j, l.restitution)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, distanceLimitBounceThreshold, j, l.bounceThreshold)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, distanceLimitStiffness, j, l.stiffness)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, distanceLimitDamping, j, l.damping)
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
|
|
PxJointLinearLimit D6Joint::getDistanceLimit() const
|
|
{
|
|
return data().distanceLimit;
|
|
}
|
|
|
|
void D6Joint::setLinearLimit(PxD6Axis::Enum axis, const PxJointLinearLimitPair& limit)
|
|
{
|
|
PX_CHECK_AND_RETURN(axis>=PxD6Axis::eX && axis<=PxD6Axis::eZ, "PxD6Joint::setLinearLimit: invalid axis value");
|
|
PX_CHECK_AND_RETURN(limit.isValid(), "PxD6Joint::setLinearLimit: limit invalid");
|
|
D6JointData& d = data();
|
|
if(axis==PxD6Axis::eX)
|
|
d.linearLimitX = limit;
|
|
else if(axis==PxD6Axis::eY)
|
|
d.linearLimitY = limit;
|
|
else if(axis==PxD6Axis::eZ)
|
|
d.linearLimitZ = limit;
|
|
else
|
|
return;
|
|
d.mUseNewLinearLimits = true;
|
|
markDirty();
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const PxD6Joint& j = static_cast<const PxD6Joint&>(*this);
|
|
const PxU32 valueCount = 3;
|
|
PxReal values[valueCount];
|
|
for (PxU32 i = 0; i < valueCount; ++i)
|
|
values[i] = getLinearLimit(PxD6Axis::Enum(i)).lower;
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, linearLimitLower, j, values, valueCount)
|
|
for (PxU32 i = 0; i < valueCount; ++i)
|
|
values[i] = getLinearLimit(PxD6Axis::Enum(i)).upper;
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, linearLimitUpper, j, values, valueCount)
|
|
for (PxU32 i = 0; i < valueCount; ++i)
|
|
values[i] = getLinearLimit(PxD6Axis::Enum(i)).restitution;
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, linearLimitRestitution, j, values, valueCount)
|
|
for (PxU32 i = 0; i < valueCount; ++i)
|
|
values[i] = getLinearLimit(PxD6Axis::Enum(i)).bounceThreshold;
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, linearLimitBounceThreshold, j, values, valueCount)
|
|
for (PxU32 i = 0; i < valueCount; ++i)
|
|
values[i] = getLinearLimit(PxD6Axis::Enum(i)).stiffness;
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, linearLimitStiffness, j, values, valueCount)
|
|
for (PxU32 i = 0; i < valueCount; ++i)
|
|
values[i] = getLinearLimit(PxD6Axis::Enum(i)).damping;
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, linearLimitDamping, j, values, valueCount)
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
|
|
PxJointLinearLimitPair D6Joint::getLinearLimit(PxD6Axis::Enum axis) const
|
|
{
|
|
PX_CHECK_AND_RETURN_VAL(axis>=PxD6Axis::eX && axis<=PxD6Axis::eZ, "PxD6Joint::getLinearLimit: invalid axis value", PxJointLinearLimitPair(PxTolerancesScale(), 0.0f, 0.0f));
|
|
const D6JointData& d = data();
|
|
if(axis==PxD6Axis::eX)
|
|
return d.linearLimitX;
|
|
else if(axis==PxD6Axis::eY)
|
|
return d.linearLimitY;
|
|
else if(axis==PxD6Axis::eZ)
|
|
return d.linearLimitZ;
|
|
return PxJointLinearLimitPair(PxTolerancesScale(), 0.0f, 0.0f);
|
|
}
|
|
|
|
PxJointAngularLimitPair D6Joint::getTwistLimit() const
|
|
{
|
|
return data().twistLimit;
|
|
}
|
|
|
|
void D6Joint::setTwistLimit(const PxJointAngularLimitPair& l)
|
|
{
|
|
PX_CHECK_AND_RETURN(l.isValid(), "PxD6Joint::setTwistLimit: limit invalid");
|
|
// PT: the tangent version is not compatible with the double-cover feature, since the potential limit extent in that case is 4*PI.
|
|
// i.e. we'd potentially take the tangent of something equal to PI/2. So the tangent stuff makes the limits less accurate, and it
|
|
// also reduces the available angular range for the joint. All that for questionable performance gains.
|
|
PX_CHECK_AND_RETURN(l.lower>-PxTwoPi && l.upper<PxTwoPi , "PxD6Joint::twist limit must be strictly between -2*PI and 2*PI");
|
|
|
|
data().twistLimit = l;
|
|
markDirty();
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const PxD6Joint& j = static_cast<const PxD6Joint&>(*this);
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, twistLimitLower, j, l.lower)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, twistLimitUpper, j, l.upper)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, twistLimitRestitution, j, l.restitution)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, twistLimitBounceThreshold, j, l.bounceThreshold)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, twistLimitStiffness, j, l.stiffness)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, twistLimitDamping, j, l.damping)
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
|
|
PxJointLimitPyramid D6Joint::getPyramidSwingLimit() const
|
|
{
|
|
return data().pyramidSwingLimit;
|
|
}
|
|
|
|
void D6Joint::setPyramidSwingLimit(const PxJointLimitPyramid& l)
|
|
{
|
|
PX_CHECK_AND_RETURN(l.isValid(), "PxD6Joint::setPyramidSwingLimit: limit invalid");
|
|
|
|
data().pyramidSwingLimit = l;
|
|
data().mUsePyramidLimits = true;
|
|
markDirty();
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const PxD6Joint& j = static_cast<const PxD6Joint&>(*this);
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, pyramidSwingLimitYAngleMin, j, l.yAngleMin)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, pyramidSwingLimitYAngleMax, j, l.yAngleMax)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, pyramidSwingLimitZAngleMin, j, l.zAngleMin)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, pyramidSwingLimitZAngleMax, j, l.zAngleMax)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, pyramidSwingLimitRestitution, j, l.restitution)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, pyramidSwingLimitBounceThreshold, j, l.bounceThreshold)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, pyramidSwingLimitStiffness, j, l.stiffness)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, pyramidSwingLimitDamping, j, l.damping)
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
|
|
PxJointLimitCone D6Joint::getSwingLimit() const
|
|
{
|
|
return data().swingLimit;
|
|
}
|
|
|
|
void D6Joint::setSwingLimit(const PxJointLimitCone& l)
|
|
{
|
|
PX_CHECK_AND_RETURN(l.isValid(), "PxD6Joint::setSwingLimit: limit invalid");
|
|
|
|
data().swingLimit = l;
|
|
data().mUseConeLimit = true;
|
|
markDirty();
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const PxD6Joint& j = static_cast<const PxD6Joint&>(*this);
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingLimitYAngle, j, l.yAngle)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingLimitZAngle, j, l.zAngle)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingLimitRestitution, j, l.restitution)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingLimitBounceThreshold, j, l.bounceThreshold)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingLimitStiffness, j, l.stiffness)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingLimitDamping, j, l.damping)
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
|
|
PxTransform D6Joint::getDrivePosition() const
|
|
{
|
|
return data().drivePosition;
|
|
}
|
|
|
|
void D6Joint::setDrivePosition(const PxTransform& pose, bool autowake)
|
|
{
|
|
PX_CHECK_AND_RETURN(pose.isSane(), "PxD6Joint::setDrivePosition: pose invalid");
|
|
data().drivePosition = pose.getNormalized();
|
|
if(autowake)
|
|
wakeUpActors();
|
|
markDirty();
|
|
|
|
OMNI_PVD_SET(OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, drivePosition, static_cast<PxD6Joint&>(*this), pose)
|
|
}
|
|
|
|
void D6Joint::getDriveVelocity(PxVec3& linear, PxVec3& angular) const
|
|
{
|
|
linear = data().driveLinearVelocity;
|
|
angular = data().driveAngularVelocity;
|
|
}
|
|
|
|
void D6Joint::setDriveVelocity(const PxVec3& linear, const PxVec3& angular, bool autowake)
|
|
{
|
|
PX_CHECK_AND_RETURN(linear.isFinite() && angular.isFinite(), "PxD6Joint::setDriveVelocity: velocity invalid");
|
|
data().driveLinearVelocity = linear;
|
|
data().driveAngularVelocity = angular;
|
|
if(autowake)
|
|
wakeUpActors();
|
|
markDirty();
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const PxD6Joint& j = static_cast<const PxD6Joint&>(*this);
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveLinVelocity, j, linear)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveAngVelocity, j, angular)
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
|
|
PxD6JointGPUIndex D6Joint::getGPUIndex() const
|
|
{
|
|
PX_COMPILE_TIME_ASSERT(sizeof(PxD6JointGPUIndex) == sizeof(PxConstraintGPUIndex));
|
|
PX_COMPILE_TIME_ASSERT(PX_INVALID_D6_JOINT_GPU_INDEX == PX_INVALID_CONSTRAINT_GPU_INDEX);
|
|
|
|
return getConstraint()->getGPUIndex();
|
|
}
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
|
|
#define EXT_OMNI_PVD_SET_AND_LINK_DRIVE_DATA(joint, attrName, driveData, driveType, jointData) \
|
|
{ \
|
|
const PxD6JointDrive* objectHandle = omniPvdCreateDriveObjectHandle(driveType, jointData); \
|
|
omniPvdSetDriveData(driveData, *objectHandle, pvdWriter, pvdRegData); \
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, attrName, joint, objectHandle) \
|
|
}
|
|
|
|
PX_FORCE_INLINE static void omniPvdClearDriveData(const PxD6Joint& joint, PxD6AngularDriveConfig::Enum angularDriveConfig,
|
|
OmniPvdWriter* pvdWriter, const OmniPvdPxExtensionsRegistrationData* pvdRegData)
|
|
{
|
|
if (angularDriveConfig == PxD6AngularDriveConfig::eSWING_TWIST)
|
|
{
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing1, joint, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing2, joint, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveTwist, joint, OMNI_PVD_INVALID_HANDLE)
|
|
}
|
|
else if (angularDriveConfig == PxD6AngularDriveConfig::eSLERP)
|
|
{
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSlerp, joint, OMNI_PVD_INVALID_HANDLE)
|
|
}
|
|
else
|
|
{
|
|
PX_ASSERT(angularDriveConfig == PxD6AngularDriveConfig::eLEGACY);
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing, joint, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveTwist, joint, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSlerp, joint, OMNI_PVD_INVALID_HANDLE)
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
void D6Joint::setAngularDriveConfig(PxD6AngularDriveConfig::Enum config)
|
|
{
|
|
D6JointData& d = data();
|
|
|
|
if (config != d.angularDriveConfig)
|
|
{
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
const PxD6Joint& pxD6Joint = static_cast<const PxD6Joint&>(*this);
|
|
|
|
{
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
omniPvdClearDriveData(pxD6Joint, static_cast<PxD6AngularDriveConfig::Enum>(d.angularDriveConfig),
|
|
pvdWriter, pvdRegData);
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, angularDriveConfig, pxD6Joint, config)
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
}
|
|
#endif
|
|
|
|
if (config == PxD6AngularDriveConfig::eSWING_TWIST)
|
|
{
|
|
d.drive[gDriveSwing1DataIndex] = PxD6JointDrive();
|
|
d.drive[gDriveSwing2DataIndex] = PxD6JointDrive();
|
|
d.drive[gDriveTwistDataIndex] = PxD6JointDrive();
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
EXT_OMNI_PVD_SET_AND_LINK_DRIVE_DATA(pxD6Joint, driveSwing1, d.drive[gDriveSwing1DataIndex], PxD6Drive::eSWING1, d)
|
|
EXT_OMNI_PVD_SET_AND_LINK_DRIVE_DATA(pxD6Joint, driveSwing2, d.drive[gDriveSwing2DataIndex], PxD6Drive::eSWING2, d)
|
|
EXT_OMNI_PVD_SET_AND_LINK_DRIVE_DATA(pxD6Joint, driveTwist, d.drive[gDriveTwistDataIndex], PxD6Drive::eTWIST, d)
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
else if (config == PxD6AngularDriveConfig::eSLERP)
|
|
{
|
|
d.drive[gDriveSlerpDataIndex] = PxD6JointDrive();
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
EXT_OMNI_PVD_SET_AND_LINK_DRIVE_DATA(pxD6Joint, driveSlerp, d.drive[gDriveSlerpDataIndex], PxD6Drive::eSLERP, d)
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
PX_ASSERT(config == PxD6AngularDriveConfig::eLEGACY);
|
|
|
|
d.drive[gDriveSwingDataIndex] = PxD6JointDrive();
|
|
d.drive[gDriveTwistDataIndex] = PxD6JointDrive();
|
|
d.drive[gDriveSlerpDataIndex] = PxD6JointDrive();
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
EXT_OMNI_PVD_SET_AND_LINK_DRIVE_DATA(pxD6Joint, driveSwing, d.drive[gDriveSwingDataIndex], PxD6Drive::eSWING, d)
|
|
EXT_OMNI_PVD_SET_AND_LINK_DRIVE_DATA(pxD6Joint, driveTwist, d.drive[gDriveTwistDataIndex], PxD6Drive::eTWIST, d)
|
|
EXT_OMNI_PVD_SET_AND_LINK_DRIVE_DATA(pxD6Joint, driveSlerp, d.drive[gDriveSlerpDataIndex], PxD6Drive::eSLERP, d)
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
#endif
|
|
}
|
|
|
|
d.angularDriveConfig = static_cast<PxU8>(config);
|
|
|
|
mRecomputeMotion = true;
|
|
markDirty();
|
|
}
|
|
}
|
|
|
|
PxD6AngularDriveConfig::Enum D6Joint::getAngularDriveConfig() const
|
|
{
|
|
const D6JointData& d = data();
|
|
|
|
return static_cast<PxD6AngularDriveConfig::Enum>(d.angularDriveConfig);
|
|
}
|
|
|
|
void* D6Joint::prepareData()
|
|
{
|
|
D6JointData& d = data();
|
|
|
|
if(mRecomputeMotion)
|
|
{
|
|
mRecomputeMotion = false;
|
|
|
|
d.driving = 0;
|
|
d.limited = 0;
|
|
d.locked = 0;
|
|
|
|
for(PxU32 i=0;i<PxD6Axis::eCOUNT;i++)
|
|
{
|
|
if(d.motion[i] == PxD6Motion::eLIMITED)
|
|
d.limited |= 1<<i;
|
|
else if(d.motion[i] == PxD6Motion::eLOCKED)
|
|
d.locked |= 1<<i;
|
|
}
|
|
|
|
// a linear direction isn't driven if it's locked
|
|
if (isDriveActive(gDriveXDataIndex) && d.motion[PxD6Axis::eX]!=PxD6Motion::eLOCKED)
|
|
d.driving |= 1 << PxD6Drive::eX;
|
|
|
|
if (isDriveActive(gDriveYDataIndex) && d.motion[PxD6Axis::eY]!=PxD6Motion::eLOCKED)
|
|
d.driving |= 1 << PxD6Drive::eY;
|
|
|
|
if (isDriveActive(gDriveZDataIndex) && d.motion[PxD6Axis::eZ]!=PxD6Motion::eLOCKED)
|
|
d.driving |= 1 << PxD6Drive::eZ;
|
|
|
|
const bool swing1Locked = d.motion[PxD6Axis::eSWING1] == PxD6Motion::eLOCKED;
|
|
const bool swing2Locked = d.motion[PxD6Axis::eSWING2] == PxD6Motion::eLOCKED;
|
|
const bool twistLocked = d.motion[PxD6Axis::eTWIST] == PxD6Motion::eLOCKED;
|
|
|
|
if (d.angularDriveConfig == PxD6AngularDriveConfig::eSWING_TWIST)
|
|
{
|
|
if (isDriveActive(gDriveTwistDataIndex) && !twistLocked)
|
|
d.driving |= 1 << PxD6Drive::eTWIST;
|
|
|
|
if (isDriveActive(gDriveSwing1DataIndex) && !swing1Locked)
|
|
d.driving |= 1 << PxD6Drive::eSWING1;
|
|
|
|
if (isDriveActive(gDriveSwing2DataIndex) && !swing2Locked)
|
|
d.driving |= 1 << PxD6Drive::eSWING2;
|
|
}
|
|
else if (d.angularDriveConfig == PxD6AngularDriveConfig::eSLERP)
|
|
{
|
|
// SLERP drive requires all angular dofs unlocked
|
|
|
|
if (isDriveActive(gDriveSlerpDataIndex) && !swing1Locked && !swing2Locked && !twistLocked)
|
|
{
|
|
d.driving |= 1 << PxD6Drive::eSLERP;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// SLERP drive requires all angular dofs unlocked, and inhibits swing/twist
|
|
|
|
if (isDriveActive(gDriveSlerpDataIndex) && !swing1Locked && !swing2Locked && !twistLocked)
|
|
{
|
|
d.driving |= 1 << PxD6Drive::eSLERP;
|
|
}
|
|
else
|
|
{
|
|
if (isDriveActive(gDriveTwistDataIndex) && !twistLocked)
|
|
d.driving |= 1 << PxD6Drive::eTWIST;
|
|
|
|
if (isDriveActive(gDriveSwingDataIndex) && (!swing1Locked || !swing2Locked))
|
|
d.driving |= 1 << PxD6Drive::eSWING;
|
|
}
|
|
}
|
|
}
|
|
|
|
this->D6JointT::prepareData();
|
|
|
|
return mData;
|
|
}
|
|
|
|
static PX_FORCE_INLINE PxReal computePhi(const PxQuat& q)
|
|
{
|
|
PxQuat twist = q;
|
|
twist.normalize();
|
|
|
|
PxReal angle = twist.getAngle();
|
|
if(twist.x<0.0f)
|
|
angle = -angle;
|
|
return angle;
|
|
}
|
|
|
|
static void visualizeAngularLimit(PxConstraintVisualizer& viz, const PxTransform& t, float swingLimitYZ)
|
|
{
|
|
viz.visualizeAngularLimit(t, -swingLimitYZ, swingLimitYZ);
|
|
}
|
|
|
|
static void visualizeDoubleCone(PxConstraintVisualizer& viz, const PxTransform& t, float swingLimitYZ)
|
|
{
|
|
viz.visualizeDoubleCone(t, swingLimitYZ);
|
|
}
|
|
|
|
static void visualizeCone(PxConstraintVisualizer& viz, const D6JointData& data, const PxTransform& cA2w)
|
|
{
|
|
viz.visualizeLimitCone(cA2w, PxTan(data.swingLimit.zAngle / 4), PxTan(data.swingLimit.yAngle / 4));
|
|
}
|
|
|
|
static void visualizeLine(PxConstraintVisualizer& viz, const PxVec3& origin, const PxVec3& axis, const PxJointLinearLimitPair& limit)
|
|
{
|
|
const PxVec3 p0 = origin + axis * limit.lower;
|
|
const PxVec3 p1 = origin + axis * limit.upper;
|
|
viz.visualizeLine(p0, p1, PxU32(PxDebugColor::eARGB_YELLOW));
|
|
}
|
|
|
|
static void visualizeQuad(PxConstraintVisualizer& viz, const PxVec3& origin, const PxVec3& axis0, const PxJointLinearLimitPair& limit0,
|
|
const PxVec3& axis1, const PxJointLinearLimitPair& limit1)
|
|
{
|
|
const PxU32 color = PxU32(PxDebugColor::eARGB_YELLOW);
|
|
|
|
const PxVec3 l0 = axis0 * limit0.lower;
|
|
const PxVec3 u0 = axis0 * limit0.upper;
|
|
const PxVec3 l1 = axis1 * limit1.lower;
|
|
const PxVec3 u1 = axis1 * limit1.upper;
|
|
|
|
const PxVec3 p0 = origin + l0 + l1;
|
|
const PxVec3 p1 = origin + u0 + l1;
|
|
const PxVec3 p2 = origin + u0 + u1;
|
|
const PxVec3 p3 = origin + l0 + u1;
|
|
|
|
viz.visualizeLine(p0, p1, color);
|
|
viz.visualizeLine(p1, p2, color);
|
|
viz.visualizeLine(p2, p3, color);
|
|
viz.visualizeLine(p3, p0, color);
|
|
}
|
|
|
|
static void visualizeBox(PxConstraintVisualizer& viz, const PxVec3& origin, const PxVec3& axis0, const PxJointLinearLimitPair& limit0,
|
|
const PxVec3& axis1, const PxJointLinearLimitPair& limit1,
|
|
const PxVec3& axis2, const PxJointLinearLimitPair& limit2)
|
|
{
|
|
const PxU32 color = PxU32(PxDebugColor::eARGB_YELLOW);
|
|
|
|
const PxVec3 l0 = axis0 * limit0.lower;
|
|
const PxVec3 u0 = axis0 * limit0.upper;
|
|
const PxVec3 l1 = axis1 * limit1.lower;
|
|
const PxVec3 u1 = axis1 * limit1.upper;
|
|
const PxVec3 l2 = axis2 * limit2.lower;
|
|
const PxVec3 u2 = axis2 * limit2.upper;
|
|
|
|
const PxVec3 p0 = origin + l0 + l1 + l2;
|
|
const PxVec3 p1 = origin + u0 + l1 + l2;
|
|
const PxVec3 p2 = origin + u0 + u1 + l2;
|
|
const PxVec3 p3 = origin + l0 + u1 + l2;
|
|
const PxVec3 p0b = origin + l0 + l1 + u2;
|
|
const PxVec3 p1b = origin + u0 + l1 + u2;
|
|
const PxVec3 p2b = origin + u0 + u1 + u2;
|
|
const PxVec3 p3b = origin + l0 + u1 + u2;
|
|
|
|
viz.visualizeLine(p0, p1, color);
|
|
viz.visualizeLine(p1, p2, color);
|
|
viz.visualizeLine(p2, p3, color);
|
|
viz.visualizeLine(p3, p0, color);
|
|
viz.visualizeLine(p0b, p1b, color);
|
|
viz.visualizeLine(p1b, p2b, color);
|
|
viz.visualizeLine(p2b, p3b, color);
|
|
viz.visualizeLine(p3b, p0b, color);
|
|
viz.visualizeLine(p0, p0b, color);
|
|
viz.visualizeLine(p1, p1b, color);
|
|
viz.visualizeLine(p2, p2b, color);
|
|
viz.visualizeLine(p3, p3b, color);
|
|
}
|
|
|
|
static float computeLimitedDistance(const D6JointData& data, const PxTransform& cB2cA, const PxMat33& cA2w_m, PxVec3& _limitDir)
|
|
{
|
|
PxVec3 limitDir(0.0f);
|
|
|
|
for(PxU32 i = 0; i<3; i++)
|
|
{
|
|
if(data.limited & (1 << (PxD6Axis::eX + i)))
|
|
limitDir += cA2w_m[i] * cB2cA.p[i];
|
|
}
|
|
|
|
_limitDir = limitDir;
|
|
return limitDir.magnitude();
|
|
}
|
|
|
|
static void drawPyramid(PxConstraintVisualizer& viz, const D6JointData& data, const PxTransform& cA2w, const PxQuat& /*swing*/, bool /*useY*/, bool /*useZ*/)
|
|
{
|
|
struct Local
|
|
{
|
|
static void drawArc(PxConstraintVisualizer& _viz, const PxTransform& _cA2w, float ymin, float ymax, float zmin, float zmax, PxU32 color)
|
|
{
|
|
// PT: we use 32 segments for the cone case, so that's 32/4 segments per arc in the pyramid case
|
|
const PxU32 nb = 32/4;
|
|
PxVec3 prev(0.0f);
|
|
for(PxU32 i=0;i<nb;i++)
|
|
{
|
|
const float coeff = float(i)/float(nb-1);
|
|
const float y = coeff*ymax + (1.0f-coeff)*ymin;
|
|
const float z = coeff*zmax + (1.0f-coeff)*zmin;
|
|
|
|
const float r = 1.0f;
|
|
PxMat33 my; PxSetRotZ(my, z);
|
|
PxMat33 mz; PxSetRotY(mz, y);
|
|
const PxVec3 p0 = (my*mz).transform(PxVec3(r, 0.0f, 0.0f));
|
|
const PxVec3 p0w = _cA2w.transform(p0);
|
|
_viz.visualizeLine(_cA2w.p, p0w, color);
|
|
if(i)
|
|
_viz.visualizeLine(prev, p0w, color);
|
|
prev = p0w;
|
|
}
|
|
}
|
|
};
|
|
|
|
const PxJointLimitPyramid& l = data.pyramidSwingLimit;
|
|
const PxU32 color = PxU32(PxDebugColor::eARGB_YELLOW);
|
|
|
|
Local::drawArc(viz, cA2w, l.yAngleMin, l.yAngleMin, l.zAngleMin, l.zAngleMax, color);
|
|
Local::drawArc(viz, cA2w, l.yAngleMax, l.yAngleMax, l.zAngleMin, l.zAngleMax, color);
|
|
Local::drawArc(viz, cA2w, l.yAngleMin, l.yAngleMax, l.zAngleMin, l.zAngleMin, color);
|
|
Local::drawArc(viz, cA2w, l.yAngleMin, l.yAngleMax, l.zAngleMax, l.zAngleMax, color);
|
|
}
|
|
|
|
static void D6JointVisualize(PxConstraintVisualizer& viz, const void* constantBlock, const PxTransform& body0Transform, const PxTransform& body1Transform, PxU32 flags)
|
|
{
|
|
const PxU32 SWING1_FLAG = 1<<PxD6Axis::eSWING1,
|
|
SWING2_FLAG = 1<<PxD6Axis::eSWING2,
|
|
TWIST_FLAG = 1<<PxD6Axis::eTWIST;
|
|
|
|
const PxU32 ANGULAR_MASK = SWING1_FLAG | SWING2_FLAG | TWIST_FLAG;
|
|
const PxU32 LINEAR_MASK = 1<<PxD6Axis::eX | 1<<PxD6Axis::eY | 1<<PxD6Axis::eZ;
|
|
|
|
PX_UNUSED(ANGULAR_MASK);
|
|
PX_UNUSED(LINEAR_MASK);
|
|
|
|
const D6JointData& data = *reinterpret_cast<const D6JointData*>(constantBlock);
|
|
|
|
PxTransform32 cA2w, cB2w;
|
|
joint::computeJointFrames(cA2w, cB2w, data, body0Transform, body1Transform);
|
|
if(flags & PxConstraintVisualizationFlag::eLOCAL_FRAMES)
|
|
viz.visualizeJointFrames(cA2w, cB2w);
|
|
|
|
if(flags & PxConstraintVisualizationFlag::eLIMITS)
|
|
{
|
|
const PxTransform cB2cA = cA2w.transformInv(cB2w);
|
|
const PxMat33Padded cA2w_m(cA2w.q);
|
|
const PxMat33Padded cB2w_m(cB2w.q);
|
|
|
|
if(data.mUseNewLinearLimits)
|
|
{
|
|
switch(data.limited)
|
|
{
|
|
case 1<<PxD6Axis::eX:
|
|
visualizeLine(viz, cA2w.p, cA2w_m.column0, data.linearLimitX);
|
|
break;
|
|
case 1<<PxD6Axis::eY:
|
|
visualizeLine(viz, cA2w.p, cA2w_m.column1, data.linearLimitY);
|
|
break;
|
|
case 1<<PxD6Axis::eZ:
|
|
visualizeLine(viz, cA2w.p, cA2w_m.column2, data.linearLimitZ);
|
|
break;
|
|
case 1<<PxD6Axis::eX|1<<PxD6Axis::eY:
|
|
visualizeQuad(viz, cA2w.p, cA2w_m.column0, data.linearLimitX, cA2w_m.column1, data.linearLimitY);
|
|
break;
|
|
case 1<<PxD6Axis::eX|1<<PxD6Axis::eZ:
|
|
visualizeQuad(viz, cA2w.p, cA2w_m.column0, data.linearLimitX, cA2w_m.column2, data.linearLimitZ);
|
|
break;
|
|
case 1<<PxD6Axis::eY|1<<PxD6Axis::eZ:
|
|
visualizeQuad(viz, cA2w.p, cA2w_m.column1, data.linearLimitY, cA2w_m.column2, data.linearLimitZ);
|
|
break;
|
|
case 1<<PxD6Axis::eX|1<<PxD6Axis::eY|1<<PxD6Axis::eZ:
|
|
visualizeBox(viz, cA2w.p, cA2w_m.column0, data.linearLimitX, cA2w_m.column1, data.linearLimitY, cA2w_m.column2, data.linearLimitZ);
|
|
break;
|
|
}
|
|
}
|
|
|
|
if(data.mUseDistanceLimit) // PT: old linear/distance limit
|
|
{
|
|
PxVec3 limitDir;
|
|
|
|
const float distance = computeLimitedDistance(data, cB2cA, cA2w_m, limitDir);
|
|
|
|
// visualise only if some of the axis is limited
|
|
if(distance > data.distanceMinDist)
|
|
{
|
|
PxU32 color = 0x00ff00;
|
|
if(distance>data.distanceLimit.value)
|
|
color = 0xff0000;
|
|
|
|
viz.visualizeLine(cA2w.p, cB2w.p, color);
|
|
}
|
|
}
|
|
|
|
PxQuat swing, twist;
|
|
PxSeparateSwingTwist(cB2cA.q, swing, twist);
|
|
|
|
if(data.limited&TWIST_FLAG)
|
|
viz.visualizeAngularLimit(cA2w, data.twistLimit.lower, data.twistLimit.upper);
|
|
|
|
const bool swing1Limited = (data.limited & SWING1_FLAG)!=0, swing2Limited = (data.limited & SWING2_FLAG)!=0;
|
|
|
|
if(swing1Limited && swing2Limited)
|
|
{
|
|
if(data.mUseConeLimit)
|
|
visualizeCone(viz, data, cA2w);
|
|
|
|
if(data.mUsePyramidLimits)
|
|
drawPyramid(viz, data, cA2w, swing, true, true);
|
|
}
|
|
else if(swing1Limited ^ swing2Limited)
|
|
{
|
|
const PxTransform yToX(PxVec3(0.0f), PxQuat(-PxPi/2.0f, PxVec3(0.0f, 0.0f, 1.0f)));
|
|
const PxTransform zToX(PxVec3(0.0f), PxQuat(PxPi/2.0f, PxVec3(0.0f, 1.0f, 0.0f)));
|
|
|
|
if(swing1Limited)
|
|
{
|
|
if(data.locked & SWING2_FLAG)
|
|
{
|
|
if(data.mUsePyramidLimits)
|
|
drawPyramid(viz, data, cA2w, swing, true, false);
|
|
else
|
|
// PT:: tag: scalar transform*transform
|
|
visualizeAngularLimit(viz, cA2w * yToX, data.swingLimit.yAngle); // PT: swing Y limited, swing Z locked
|
|
}
|
|
else
|
|
if(!data.mUsePyramidLimits)
|
|
// PT:: tag: scalar transform*transform
|
|
visualizeDoubleCone(viz, cA2w * zToX, data.swingLimit.yAngle); // PT: swing Y limited, swing Z free
|
|
}
|
|
else
|
|
{
|
|
if(data.locked & SWING1_FLAG)
|
|
{
|
|
if(data.mUsePyramidLimits)
|
|
drawPyramid(viz, data, cA2w, swing, false, true);
|
|
else
|
|
// PT:: tag: scalar transform*transform
|
|
visualizeAngularLimit(viz, cA2w * zToX, data.swingLimit.zAngle); // PT: swing Z limited, swing Y locked
|
|
}
|
|
else
|
|
if(!data.mUsePyramidLimits)
|
|
// PT:: tag: scalar transform*transform
|
|
visualizeDoubleCone(viz, cA2w * yToX, data.swingLimit.zAngle); // PT: swing Z limited, swing Y free
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static PX_FORCE_INLINE void setupSingleSwingLimit(joint::ConstraintHelper& ch, const D6JointData& data, const PxVec3& axis, float swingYZ, float swingW, float swingLimitYZ)
|
|
{
|
|
ch.anglePair(computeSwingAngle(swingYZ, swingW), -swingLimitYZ, swingLimitYZ, axis, data.swingLimit);
|
|
}
|
|
|
|
static PX_FORCE_INLINE void setupDualConeSwingLimits(joint::ConstraintHelper& ch, const D6JointData& data, const PxVec3& axis, float sin, float swingLimitYZ)
|
|
{
|
|
ch.anglePair(PxAsin(sin), -swingLimitYZ, swingLimitYZ, axis.getNormalized(), data.swingLimit);
|
|
}
|
|
|
|
static void setupConeSwingLimits(joint::ConstraintHelper& ch, const D6JointData& data, const PxQuat& swing, const PxTransform& cA2w)
|
|
{
|
|
PxVec3 axis;
|
|
PxReal error;
|
|
const Cm::ConeLimitHelperTanLess coneHelper(data.swingLimit.yAngle, data.swingLimit.zAngle);
|
|
coneHelper.getLimit(swing, axis, error);
|
|
ch.angularLimit(cA2w.rotate(axis), error, data.swingLimit);
|
|
}
|
|
|
|
static void setupPyramidSwingLimits(joint::ConstraintHelper& ch, const D6JointData& data, const PxQuat& swing, const PxTransform& cA2w, bool useY, bool useZ)
|
|
{
|
|
const PxQuat q = cA2w.q * swing;
|
|
const PxJointLimitPyramid& l = data.pyramidSwingLimit;
|
|
if(useY)
|
|
ch.anglePair(computeSwingAngle(swing.y, swing.w), l.yAngleMin, l.yAngleMax, q.getBasisVector1(), l);
|
|
if(useZ)
|
|
ch.anglePair(computeSwingAngle(swing.z, swing.w), l.zAngleMin, l.zAngleMax, q.getBasisVector2(), l);
|
|
}
|
|
|
|
static void setupLinearLimit(joint::ConstraintHelper& ch, const PxJointLinearLimitPair& limit, const float origin, const PxVec3& axis)
|
|
{
|
|
ch.linearLimit(axis, origin, limit.upper, limit);
|
|
ch.linearLimit(-axis, -origin, -limit.lower, limit);
|
|
}
|
|
|
|
//TAG:solverprepshader
|
|
static PxU32 D6JointSolverPrep(Px1DConstraint* constraints,
|
|
PxVec3p& body0WorldOffset,
|
|
PxU32 /*maxConstraints*/,
|
|
PxConstraintInvMassScale& invMassScale,
|
|
const void* constantBlock,
|
|
const PxTransform& bA2w,
|
|
const PxTransform& bB2w,
|
|
bool useExtendedLimits,
|
|
PxVec3p& cA2wOut, PxVec3p& cB2wOut)
|
|
{
|
|
//bA2w is the pose of the centre of mass of body a expressed in the world frame.
|
|
//bB2w is the pose of the centre of mass of body b expressed in the world frame.
|
|
|
|
const D6JointData& data = *reinterpret_cast<const D6JointData*>(constantBlock);
|
|
|
|
//cA2w is Ga*Ja where Ga is the global pose of actor a and Ja is the joint frame associated with actor a.
|
|
//cB2w is Gb*Jb where Gb is the global pose of actor b and Jb is the joint frame associated with actor b.
|
|
//ch caches cA2w and cB2w as well as ra = ca2w.p - bA2w.p and rb = cb2w.p - bB2w.p
|
|
PxTransform32 cA2w, cB2w;
|
|
joint::ConstraintHelper ch(constraints, invMassScale, cA2w, cB2w, body0WorldOffset, data, bA2w, bB2w);
|
|
|
|
const PxU32 SWING1_FLAG = 1<<PxD6Axis::eSWING1;
|
|
const PxU32 SWING2_FLAG = 1<<PxD6Axis::eSWING2;
|
|
const PxU32 TWIST_FLAG = 1<<PxD6Axis::eTWIST;
|
|
|
|
const PxU32 ANGULAR_MASK = SWING1_FLAG | SWING2_FLAG | TWIST_FLAG;
|
|
const PxU32 LINEAR_MASK = 1<<PxD6Axis::eX | 1<<PxD6Axis::eY | 1<<PxD6Axis::eZ;
|
|
|
|
const PxD6JointDrive* drives = data.drive;
|
|
PxU32 locked = data.locked;
|
|
const PxU32 limited = data.limited;
|
|
const PxU32 driving = data.driving;
|
|
|
|
// PT: it is a mistake to use the neighborhood operator since it
|
|
// prevents us from using the quat's double-cover feature.
|
|
if(!useExtendedLimits)
|
|
joint::applyNeighborhoodOperator(cA2w, cB2w);
|
|
|
|
//cB2cA = cA2w^-1 * cB2w
|
|
//This allows us to compute the constraint error in joint frame associated with body A.
|
|
//We want to compute cA2w.rotateInv(cA2w.p - cB2w.p) to be consistent with the specification we have for the Jacobian.
|
|
//But (cA2w^-1 * cB2w).p = cA2w.rotateInv(cB2w.p - cA2w.p)
|
|
//The geometric error in the joint frame associated with body A is therefore -cB2cA.p.
|
|
//This is useful to know when calling prepareLockedAxes().
|
|
const PxTransform cB2cA = cA2w.transformInv(cB2w);
|
|
|
|
PX_ASSERT(data.c2b[0].isValid());
|
|
PX_ASSERT(data.c2b[1].isValid());
|
|
PX_ASSERT(cA2w.isValid());
|
|
PX_ASSERT(cB2w.isValid());
|
|
PX_ASSERT(cB2cA.isValid());
|
|
|
|
const PxMat33Padded cA2w_m(cA2w.q);
|
|
const PxMat33Padded cB2w_m(cB2w.q);
|
|
|
|
// handy for swing computation
|
|
const PxVec3& bX = cB2w_m.column0;
|
|
const PxVec3& aY = cA2w_m.column1;
|
|
const PxVec3& aZ = cA2w_m.column2;
|
|
|
|
if(driving & ((1<<PxD6Drive::eX)|(1<<PxD6Drive::eY)|(1<<PxD6Drive::eZ)))
|
|
{
|
|
// TODO: make drive unilateral if we are outside the limit
|
|
const PxVec3 posErr = data.drivePosition.p - cB2cA.p;
|
|
for(PxU32 i=0; i<3; i++)
|
|
{
|
|
// -driveVelocity because velTarget is child (body1) - parent (body0) and Jacobian is 1 for body0 and -1 for parent
|
|
if(driving & (1<<(PxD6Drive::eX+i)))
|
|
ch.linear(cA2w_m[i], -data.driveLinearVelocity[i], posErr[i], drives[gDriveXDataIndex + i]);
|
|
|
|
PX_COMPILE_TIME_ASSERT(gDriveYDataIndex == (gDriveXDataIndex + 1));
|
|
PX_COMPILE_TIME_ASSERT(gDriveZDataIndex == (gDriveYDataIndex + 1));
|
|
|
|
PX_COMPILE_TIME_ASSERT(PxD6Drive::eY == (PxD6Drive::eX + 1));
|
|
PX_COMPILE_TIME_ASSERT(PxD6Drive::eZ == (PxD6Drive::eY + 1));
|
|
}
|
|
}
|
|
|
|
if (driving & ((1 << PxD6Drive::eSLERP) | (1 << PxD6Drive::eSWING) | (1 << PxD6Drive::eTWIST) | (1 << PxD6Drive::eSWING1) | (1 << PxD6Drive::eSWING2)))
|
|
{
|
|
const PxQuat d2cA_q = cB2cA.q.dot(data.drivePosition.q)>0.0f ? data.drivePosition.q : -data.drivePosition.q;
|
|
|
|
const PxVec3& v = data.driveAngularVelocity;
|
|
const PxQuat delta = d2cA_q.getConjugate() * cB2cA.q;
|
|
|
|
if(driving & (1<<PxD6Drive::eSLERP))
|
|
{
|
|
const PxVec3 velTarget = -cA2w.rotate(data.driveAngularVelocity);
|
|
|
|
PxVec3 axis[3] = { PxVec3(1.0f, 0.0f, 0.0f), PxVec3(0.0f, 1.0f, 0.0f), PxVec3(0.0f, 0.0f, 1.0f) };
|
|
|
|
if (drives[gDriveSlerpDataIndex].stiffness != 0.0f)
|
|
joint::computeJacobianAxes(axis, cA2w.q * d2cA_q, cB2w.q); // converges faster if there is only velocity drive
|
|
|
|
for(PxU32 i=0; i<3; i++)
|
|
ch.angular(axis[i], axis[i].dot(velTarget), -delta.getImaginaryPart()[i], drives[gDriveSlerpDataIndex], PxConstraintSolveHint::eSLERP_SPRING);
|
|
}
|
|
else
|
|
{
|
|
if(driving & (1<<PxD6Drive::eTWIST))
|
|
ch.angular(cA2w_m.column0, v.x, -2.0f * delta.x, drives[gDriveTwistDataIndex]);
|
|
|
|
if(driving & (1<<PxD6Drive::eSWING))
|
|
{
|
|
const PxVec3 err = delta.getBasisVector0();
|
|
|
|
if(!(locked & SWING1_FLAG))
|
|
ch.angular(aY, v.y, err.z, drives[gDriveSwingDataIndex]);
|
|
|
|
if(!(locked & SWING2_FLAG))
|
|
ch.angular(aZ, v.z, -err.y, drives[gDriveSwingDataIndex]);
|
|
}
|
|
else if (driving & ((1 << PxD6Drive::eSWING1) | (1 << PxD6Drive::eSWING2)))
|
|
{
|
|
const PxVec3 err = delta.getBasisVector0();
|
|
|
|
if (driving & (1 << PxD6Drive::eSWING1))
|
|
ch.angular(aY, v.y, err.z, drives[gDriveSwing1DataIndex]);
|
|
|
|
if (driving & (1 << PxD6Drive::eSWING2))
|
|
ch.angular(aZ, v.z, -err.y, drives[gDriveSwing2DataIndex]);
|
|
}
|
|
}
|
|
}
|
|
|
|
if(limited & ANGULAR_MASK)
|
|
{
|
|
PxQuat swing, twist;
|
|
PxSeparateSwingTwist(cB2cA.q, swing, twist);
|
|
|
|
// swing limits: if just one is limited: if the other is free, we support
|
|
// (-pi/2, +pi/2) limit, using tan of the half-angle as the error measure parameter.
|
|
// If the other is locked, we support (-pi, +pi) limits using the tan of the quarter-angle
|
|
// Notation: th == PxTanHalf, tq = tanQuarter
|
|
|
|
if(limited & SWING1_FLAG && limited & SWING2_FLAG)
|
|
{
|
|
if(data.mUseConeLimit)
|
|
setupConeSwingLimits(ch, data, swing, cA2w);
|
|
// PT: no else here by design, we want to allow creating both the cone & the pyramid at the same time,
|
|
// which can be useful to make the cone more robust against large velocities.
|
|
if(data.mUsePyramidLimits)
|
|
setupPyramidSwingLimits(ch, data, swing, cA2w, true, true);
|
|
}
|
|
else
|
|
{
|
|
if(limited & SWING1_FLAG)
|
|
{
|
|
if(locked & SWING2_FLAG)
|
|
{
|
|
if(data.mUsePyramidLimits)
|
|
setupPyramidSwingLimits(ch, data, swing, cA2w, true, false);
|
|
else
|
|
setupSingleSwingLimit(ch, data, aY, swing.y, swing.w, data.swingLimit.yAngle); // PT: swing Y limited, swing Z locked
|
|
}
|
|
else
|
|
{
|
|
if(!data.mUsePyramidLimits)
|
|
setupDualConeSwingLimits(ch, data, aZ.cross(bX), -aZ.dot(bX), data.swingLimit.yAngle); // PT: swing Y limited, swing Z free
|
|
else
|
|
PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "D6JointSolverPrep: invalid joint setup. Double pyramid mode not supported.");
|
|
}
|
|
}
|
|
if(limited & SWING2_FLAG)
|
|
{
|
|
if(locked & SWING1_FLAG)
|
|
{
|
|
if(data.mUsePyramidLimits)
|
|
setupPyramidSwingLimits(ch, data, swing, cA2w, false, true);
|
|
else
|
|
setupSingleSwingLimit(ch, data, aZ, swing.z, swing.w, data.swingLimit.zAngle); // PT: swing Z limited, swing Y locked
|
|
}
|
|
else
|
|
if(!data.mUsePyramidLimits)
|
|
setupDualConeSwingLimits(ch, data, -aY.cross(bX), aY.dot(bX), data.swingLimit.zAngle); // PT: swing Z limited, swing Y free
|
|
else
|
|
PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "D6JointSolverPrep: invalid joint setup. Double pyramid mode not supported.");
|
|
}
|
|
}
|
|
|
|
if(limited & TWIST_FLAG)
|
|
ch.anglePair(computePhi(twist), data.twistLimit.lower, data.twistLimit.upper, cB2w_m.column0, data.twistLimit);
|
|
}
|
|
|
|
if(limited & LINEAR_MASK)
|
|
{
|
|
if(data.mUseDistanceLimit) // PT: old linear/distance limit
|
|
{
|
|
PxVec3 limitDir;
|
|
|
|
const float distance = computeLimitedDistance(data, cB2cA, cA2w_m, limitDir);
|
|
if(distance > data.distanceMinDist)
|
|
ch.linearLimit(limitDir * (1.0f/distance), distance, data.distanceLimit.value, data.distanceLimit);
|
|
}
|
|
|
|
if(data.mUseNewLinearLimits) // PT: new asymmetric linear limits
|
|
{
|
|
const PxVec3& bOriginInA = cB2cA.p;
|
|
|
|
// PT: TODO: we check that the DOFs are not "locked" to be consistent with the prismatic joint, but it
|
|
// doesn't look like this case is possible, since it would be caught by the "isValid" check when setting
|
|
// the limits. And in fact the "distance" linear limit above doesn't do this check.
|
|
if((limited & (1<<PxD6Axis::eX)) && data.linearLimitX.lower <= data.linearLimitX.upper)
|
|
setupLinearLimit(ch, data.linearLimitX, bOriginInA.x, cA2w_m.column0);
|
|
|
|
if((limited & (1<<PxD6Axis::eY)) && data.linearLimitY.lower <= data.linearLimitY.upper)
|
|
setupLinearLimit(ch, data.linearLimitY, bOriginInA.y, cA2w_m.column1);
|
|
|
|
if((limited & (1<<PxD6Axis::eZ)) && data.linearLimitZ.lower <= data.linearLimitZ.upper)
|
|
setupLinearLimit(ch, data.linearLimitZ, bOriginInA.z, cA2w_m.column2);
|
|
}
|
|
}
|
|
|
|
// we handle specially the case of just one swing dof locked
|
|
|
|
const PxU32 angularLocked = locked & ANGULAR_MASK;
|
|
|
|
if(angularLocked == SWING1_FLAG)
|
|
{
|
|
ch.angularHard(bX.cross(aZ), -bX.dot(aZ));
|
|
locked &= ~SWING1_FLAG;
|
|
}
|
|
else if(angularLocked == SWING2_FLAG)
|
|
{
|
|
locked &= ~SWING2_FLAG;
|
|
ch.angularHard(bX.cross(aY), -bX.dot(aY));
|
|
}
|
|
|
|
// PT: TODO: cA2w_m has already been computed above, no need to recompute it within prepareLockedAxes
|
|
PxVec3 ra, rb;
|
|
ch.prepareLockedAxes(cA2w.q, cB2w.q, cB2cA.p, locked&7, locked>>3, ra, rb);
|
|
|
|
cA2wOut = ra + bA2w.p;
|
|
cB2wOut = rb + bB2w.p;
|
|
|
|
/*cA2wOut = cA2w.p;
|
|
cB2wOut = cB2w.p;*/
|
|
|
|
// PT: TODO: check the number cannot be too high now
|
|
return ch.getCount();
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
static PxConstraintShaderTable gD6JointShaders = { D6JointSolverPrep, D6JointVisualize, PxConstraintFlag::eGPU_COMPATIBLE };
|
|
|
|
PxConstraintSolverPrep D6Joint::getPrep() const { return gD6JointShaders.solverPrep; }
|
|
|
|
PxD6Joint* physx::PxD6JointCreate(PxPhysics& physics, PxRigidActor* actor0, const PxTransform& localFrame0, PxRigidActor* actor1, const PxTransform& localFrame1)
|
|
{
|
|
PX_CHECK_AND_RETURN_NULL(localFrame0.isSane(), "PxD6JointCreate: local frame 0 is not a valid transform");
|
|
PX_CHECK_AND_RETURN_NULL(localFrame1.isSane(), "PxD6JointCreate: local frame 1 is not a valid transform");
|
|
PX_CHECK_AND_RETURN_NULL(actor0 != actor1, "PxD6JointCreate: actors must be different");
|
|
PX_CHECK_AND_RETURN_NULL((actor0 && actor0->is<PxRigidBody>()) || (actor1 && actor1->is<PxRigidBody>()), "PxD6JointCreate: at least one actor must be dynamic");
|
|
|
|
return createJointT<D6Joint, D6JointData>(physics, actor0, localFrame0, actor1, localFrame1, gD6JointShaders);
|
|
}
|
|
|
|
// PX_SERIALIZATION
|
|
void D6Joint::resolveReferences(PxDeserializationContext& context)
|
|
{
|
|
mPxConstraint = resolveConstraintPtr(context, mPxConstraint, this, gD6JointShaders);
|
|
}
|
|
//~PX_SERIALIZATION
|
|
|
|
#if PX_SUPPORT_OMNI_PVD
|
|
|
|
void D6Joint::updateOmniPvdProperties() const
|
|
{
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const PxD6Joint& j = static_cast<const PxD6Joint&>(*this);
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, twistAngle, j, getTwistAngle())
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingYAngle, j, getSwingYAngle())
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingZAngle, j, getSwingZAngle())
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
}
|
|
|
|
#define EXT_OMNI_PVD_CREATE_DRIVE_DATA(objectHandle, joint, attrName, driveData, driveType, jointData) \
|
|
const PxD6JointDrive* objectHandle = omniPvdCreateDriveObjectHandle(driveType, jointData); \
|
|
OMNI_PVD_CREATE_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6JointDrive, *objectHandle) \
|
|
omniPvdSetDriveData(driveData, *objectHandle, pvdWriter, pvdRegData); \
|
|
|
|
template<>
|
|
void physx::Ext::omniPvdInitJoint<D6Joint>(D6Joint& joint)
|
|
{
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
|
|
|
const D6JointData& jData = joint.data();
|
|
|
|
const PxD6Joint& j = static_cast<const PxD6Joint&>(joint);
|
|
OMNI_PVD_CREATE_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, j);
|
|
omniPvdSetBaseJointParams(static_cast<const PxJoint&>(joint), PxJointConcreteType::eD6);
|
|
|
|
PxD6Motion::Enum motions[PxD6Axis::eCOUNT];
|
|
for (PxU32 i = 0; i < PxD6Axis::eCOUNT; ++i)
|
|
motions[i] = jData.motion[i];
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, motions, j, motions, PxD6Axis::eCOUNT)
|
|
|
|
//
|
|
// note: the assumption is that the drive data memory is persistent during the lifetime of a D6 joint
|
|
//
|
|
|
|
EXT_OMNI_PVD_CREATE_DRIVE_DATA(driveXHandle, j, driveX, jData.drive[gDriveXDataIndex], PxD6Drive::eX, jData)
|
|
EXT_OMNI_PVD_CREATE_DRIVE_DATA(driveYHandle, j, driveY, jData.drive[gDriveYDataIndex], PxD6Drive::eY, jData)
|
|
EXT_OMNI_PVD_CREATE_DRIVE_DATA(driveZHandle, j, driveZ, jData.drive[gDriveZDataIndex], PxD6Drive::eZ, jData)
|
|
|
|
EXT_OMNI_PVD_CREATE_DRIVE_DATA(driveSwingHandle, j, driveSwing, jData.drive[gDriveSwingDataIndex], PxD6Drive::eSWING, jData)
|
|
EXT_OMNI_PVD_CREATE_DRIVE_DATA(driveTwistHandle, j, driveTwist, jData.drive[gDriveTwistDataIndex], PxD6Drive::eTWIST, jData)
|
|
EXT_OMNI_PVD_CREATE_DRIVE_DATA(driveSlerpHandle, j, driveSlerp, jData.drive[gDriveSlerpDataIndex], PxD6Drive::eSLERP, jData)
|
|
EXT_OMNI_PVD_CREATE_DRIVE_DATA(driveSwing1Handle, j, driveSwing1, jData.drive[gDriveSwing1DataIndex], PxD6Drive::eSWING1, jData)
|
|
EXT_OMNI_PVD_CREATE_DRIVE_DATA(driveSwing2Handle, j, driveSwing2, jData.drive[gDriveSwing2DataIndex], PxD6Drive::eSWING2, jData)
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveX, j, driveXHandle)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveY, j, driveYHandle)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveZ, j, driveZHandle)
|
|
|
|
if (jData.angularDriveConfig == PxD6AngularDriveConfig::eSWING_TWIST)
|
|
{
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing1, j, driveSwing1Handle)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing2, j, driveSwing2Handle)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveTwist, j, driveTwistHandle)
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing, j, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSlerp, j, OMNI_PVD_INVALID_HANDLE)
|
|
}
|
|
else if (jData.angularDriveConfig == PxD6AngularDriveConfig::eSLERP)
|
|
{
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSlerp, j, driveSlerpHandle)
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing, j, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing1, j, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing2, j, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveTwist, j, OMNI_PVD_INVALID_HANDLE)
|
|
}
|
|
else
|
|
{
|
|
PX_ASSERT(jData.angularDriveConfig == PxD6AngularDriveConfig::eLEGACY);
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing, j, driveSwingHandle)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveTwist, j, driveTwistHandle)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSlerp, j, driveSlerpHandle)
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing1, j, OMNI_PVD_INVALID_HANDLE)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveSwing2, j, OMNI_PVD_INVALID_HANDLE)
|
|
}
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, drivePosition, j, jData.drivePosition)
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveLinVelocity, j, jData.driveLinearVelocity)
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, driveAngVelocity, j, jData.driveAngularVelocity)
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, twistAngle, j, joint.getTwistAngle_Internal())
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingYAngle, j, joint.getSwingYAngle_Internal())
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, swingZAngle, j, joint.getSwingZAngle_Internal())
|
|
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxD6Joint, angularDriveConfig, j, static_cast<PxD6AngularDriveConfig::Enum>(jData.angularDriveConfig))
|
|
|
|
OMNI_PVD_WRITE_SCOPE_END
|
|
}
|
|
|
|
#endif
|
|
|