1088 lines
37 KiB
C++
1088 lines
37 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-2019 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 "PxPhysics.h"
|
||
|
|
|
||
|
|
#include "ExtD6Joint.h"
|
||
|
|
#include "ExtConstraintHelper.h"
|
||
|
|
#include "CmConeLimitHelper.h"
|
||
|
|
|
||
|
|
using namespace physx;
|
||
|
|
using namespace Ext;
|
||
|
|
using namespace shdfnd;
|
||
|
|
|
||
|
|
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");
|
||
|
|
|
||
|
|
D6Joint* j;
|
||
|
|
PX_NEW_SERIALIZED(j, D6Joint)(physics.getTolerancesScale(), actor0, localFrame0, actor1, localFrame1);
|
||
|
|
if(j->attach(physics, actor0, actor1))
|
||
|
|
return j;
|
||
|
|
|
||
|
|
PX_DELETE(j);
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
D6Joint::D6Joint(const PxTolerancesScale& scale, PxRigidActor* actor0, const PxTransform& localFrame0, PxRigidActor* actor1, const PxTransform& localFrame1) :
|
||
|
|
D6JointT(PxJointConcreteType::eD6, PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE, actor0, localFrame0, actor1, localFrame1, sizeof(D6JointData), "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(scale, 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<PxD6Drive::eCOUNT;i++)
|
||
|
|
data->drive[i] = PxD6JointDrive();
|
||
|
|
|
||
|
|
data->drivePosition = PxTransform(PxIdentity);
|
||
|
|
data->driveLinearVelocity = PxVec3(0.0f);
|
||
|
|
data->driveAngularVelocity = PxVec3(0.0f);
|
||
|
|
|
||
|
|
data->projectionLinearTolerance = 1e10f;
|
||
|
|
data->projectionAngularTolerance = PxPi;
|
||
|
|
|
||
|
|
data->mUseDistanceLimit = false;
|
||
|
|
data->mUseNewLinearLimits = false;
|
||
|
|
data->mUseConeLimit = false;
|
||
|
|
data->mUsePyramidLimits = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
PxReal D6Joint::getTwistAngle() const
|
||
|
|
{
|
||
|
|
return getTwistAngle_Internal();
|
||
|
|
}
|
||
|
|
|
||
|
|
PxReal D6Joint::getSwingYAngle() const
|
||
|
|
{
|
||
|
|
return getSwingYAngle_Internal();
|
||
|
|
}
|
||
|
|
|
||
|
|
PxReal D6Joint::getSwingZAngle() const
|
||
|
|
{
|
||
|
|
return getSwingZAngle_Internal();
|
||
|
|
}
|
||
|
|
|
||
|
|
PxD6JointDrive D6Joint::getDrive(PxD6Drive::Enum index) const
|
||
|
|
{
|
||
|
|
return data().drive[index];
|
||
|
|
}
|
||
|
|
|
||
|
|
void D6Joint::setDrive(PxD6Drive::Enum index, const PxD6JointDrive& d)
|
||
|
|
{
|
||
|
|
PX_CHECK_AND_RETURN(d.isValid(), "PxD6Joint::setDrive: drive is invalid");
|
||
|
|
|
||
|
|
data().drive[index] = d;
|
||
|
|
mRecomputeMotion = true;
|
||
|
|
markDirty();
|
||
|
|
}
|
||
|
|
|
||
|
|
void D6Joint::setDistanceLimit(const PxJointLinearLimit& l)
|
||
|
|
{
|
||
|
|
PX_CHECK_AND_RETURN(l.isValid(), "PxD6Joint::setDistanceLimit: limit invalid");
|
||
|
|
data().distanceLimit = l;
|
||
|
|
data().mUseDistanceLimit = true;
|
||
|
|
markDirty();
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
void D6Joint::setProjectionAngularTolerance(PxReal tolerance)
|
||
|
|
{
|
||
|
|
PX_CHECK_AND_RETURN(PxIsFinite(tolerance) && tolerance >=0 && tolerance <= PxPi, "PxD6Joint::setProjectionAngularTolerance: tolerance invalid");
|
||
|
|
data().projectionAngularTolerance = tolerance;
|
||
|
|
markDirty();
|
||
|
|
}
|
||
|
|
|
||
|
|
PxReal D6Joint::getProjectionAngularTolerance() const
|
||
|
|
{
|
||
|
|
return data().projectionAngularTolerance;
|
||
|
|
}
|
||
|
|
|
||
|
|
void D6Joint::setProjectionLinearTolerance(PxReal tolerance)
|
||
|
|
{
|
||
|
|
PX_CHECK_AND_RETURN(PxIsFinite(tolerance) && tolerance >=0, "PxD6Joint::setProjectionLinearTolerance: invalid parameter");
|
||
|
|
data().projectionLinearTolerance = tolerance;
|
||
|
|
markDirty();
|
||
|
|
}
|
||
|
|
|
||
|
|
PxReal D6Joint::getProjectionLinearTolerance() const
|
||
|
|
{
|
||
|
|
return data().projectionLinearTolerance;
|
||
|
|
}
|
||
|
|
|
||
|
|
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(active(PxD6Drive::eX) && d.motion[PxD6Axis::eX]!=PxD6Motion::eLOCKED) d.driving |= 1<< PxD6Drive::eX;
|
||
|
|
if(active(PxD6Drive::eY) && d.motion[PxD6Axis::eY]!=PxD6Motion::eLOCKED) d.driving |= 1<< PxD6Drive::eY;
|
||
|
|
if(active(PxD6Drive::eZ) && d.motion[PxD6Axis::eZ]!=PxD6Motion::eLOCKED) d.driving |= 1<< PxD6Drive::eZ;
|
||
|
|
|
||
|
|
// SLERP drive requires all angular dofs unlocked, and inhibits swing/twist
|
||
|
|
|
||
|
|
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(active(PxD6Drive::eSLERP) && !swing1Locked && !swing2Locked && !twistLocked)
|
||
|
|
d.driving |= 1<<PxD6Drive::eSLERP;
|
||
|
|
else
|
||
|
|
{
|
||
|
|
if(active(PxD6Drive::eTWIST) && !twistLocked)
|
||
|
|
d.driving |= 1<<PxD6Drive::eTWIST;
|
||
|
|
if(active(PxD6Drive::eSWING) && (!swing1Locked || !swing2Locked))
|
||
|
|
d.driving |= 1<< PxD6Drive::eSWING;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
this->D6JointT::prepareData();
|
||
|
|
|
||
|
|
return mData;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool D6Joint::attach(PxPhysics &physics, PxRigidActor* actor0, PxRigidActor* actor1)
|
||
|
|
{
|
||
|
|
mPxConstraint = physics.createConstraint(actor0, actor1, *this, sShaders, sizeof(D6JointData));
|
||
|
|
return mPxConstraint!=NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
void D6Joint::exportExtraData(PxSerializationContext& stream)
|
||
|
|
{
|
||
|
|
if(mData)
|
||
|
|
{
|
||
|
|
stream.alignData(PX_SERIAL_ALIGN);
|
||
|
|
stream.writeData(mData, sizeof(D6JointData));
|
||
|
|
}
|
||
|
|
stream.writeName(mName);
|
||
|
|
}
|
||
|
|
|
||
|
|
void D6Joint::importExtraData(PxDeserializationContext& context)
|
||
|
|
{
|
||
|
|
if(mData)
|
||
|
|
mData = context.readExtraData<D6JointData, PX_SERIAL_ALIGN>();
|
||
|
|
|
||
|
|
context.readName(mName);
|
||
|
|
}
|
||
|
|
|
||
|
|
void D6Joint::resolveReferences(PxDeserializationContext& context)
|
||
|
|
{
|
||
|
|
setPxConstraint(resolveConstraintPtr(context, getPxConstraint(), getConnector(), sShaders));
|
||
|
|
}
|
||
|
|
|
||
|
|
D6Joint* D6Joint::createObject(PxU8*& address, PxDeserializationContext& context)
|
||
|
|
{
|
||
|
|
D6Joint* obj = new (address) D6Joint(PxBaseFlag::eIS_RELEASABLE);
|
||
|
|
address += sizeof(D6Joint);
|
||
|
|
obj->importExtraData(context);
|
||
|
|
obj->resolveReferences(context);
|
||
|
|
return obj;
|
||
|
|
}
|
||
|
|
|
||
|
|
// global function to share the joint shaders with API capture
|
||
|
|
const PxConstraintShaderTable* Ext::GetD6JointShaderTable()
|
||
|
|
{
|
||
|
|
return &D6Joint::getConstraintShaderTable();
|
||
|
|
}
|
||
|
|
|
||
|
|
//~PX_SERIALIZATION
|
||
|
|
|
||
|
|
// Notes:
|
||
|
|
/*
|
||
|
|
|
||
|
|
This used to be in the linear drive model:
|
||
|
|
|
||
|
|
if(motion[PxD6Axis::eX+i] == PxD6Motion::eLIMITED)
|
||
|
|
{
|
||
|
|
if(data.driveLinearVelocity[i] < 0.0f && cB2cA.p[i] < -mLimits[PxD6Limit::eLINEAR].mValue ||
|
||
|
|
data.driveLinearVelocity[i] > 0.0f && cB2cA.p[i] > mLimits[PxD6Limit::eLINEAR].mValue)
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
it doesn't seem like a good idea though, because it turns off drive altogether, despite the fact that positional
|
||
|
|
drive might pull us back in towards the limit. Might be better to make the drive unilateral so it can only pull
|
||
|
|
us in from the limit
|
||
|
|
|
||
|
|
This used to be in angular locked:
|
||
|
|
|
||
|
|
// Angular locked
|
||
|
|
//TODO fix this properly.
|
||
|
|
if(PxAbs(cB2cA.q.x) < 0.0001f) cB2cA.q.x = 0;
|
||
|
|
if(PxAbs(cB2cA.q.y) < 0.0001f) cB2cA.q.y = 0;
|
||
|
|
if(PxAbs(cB2cA.q.z) < 0.0001f) cB2cA.q.z = 0;
|
||
|
|
if(PxAbs(cB2cA.q.w) < 0.0001f) cB2cA.q.w = 0;
|
||
|
|
*/
|
||
|
|
|
||
|
|
static PxQuat truncate(const PxQuat& qIn, PxReal minCosHalfTol, bool& truncated)
|
||
|
|
{
|
||
|
|
const PxQuat q = qIn.w >= 0.0f ? qIn : -qIn;
|
||
|
|
truncated = q.w < minCosHalfTol;
|
||
|
|
if(!truncated)
|
||
|
|
return q;
|
||
|
|
const PxVec3 v = q.getImaginaryPart().getNormalized() * PxSqrt(1.0f - minCosHalfTol * minCosHalfTol);
|
||
|
|
return PxQuat(v.x, v.y, v.z, minCosHalfTol);
|
||
|
|
}
|
||
|
|
|
||
|
|
// we decompose the quaternion as q1 * q2, where q1 is a rotation orthogonal to the unit axis, and q2 a rotation around it.
|
||
|
|
// (so for example if 'axis' is the twist axis, this is separateSwingTwist).
|
||
|
|
static PxQuat project(const PxQuat& q, const PxVec3& axis, PxReal cosHalfTol, bool& truncated)
|
||
|
|
{
|
||
|
|
const PxReal a = q.getImaginaryPart().dot(axis);
|
||
|
|
const PxQuat q2 = PxAbs(a) >= 1e-6f ? PxQuat(a*axis.x, a*axis.y, a*axis.z, q.w).getNormalized() : PxQuat(PxIdentity);
|
||
|
|
const PxQuat q1 = q * q2.getConjugate();
|
||
|
|
|
||
|
|
PX_ASSERT(PxAbs(q1.getImaginaryPart().dot(q2.getImaginaryPart())) < 1e-6f);
|
||
|
|
|
||
|
|
return truncate(q1, cosHalfTol, truncated) * q2;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Here's how the angular part works:
|
||
|
|
// * if no DOFs are locked, there's nothing to do.
|
||
|
|
// * if all DOFs are locked, we just truncate the rotation
|
||
|
|
// * if two DOFs are locked
|
||
|
|
// * we decompose the rotation into swing * twist, where twist is a rotation around the free DOF and swing is a rotation around an axis orthogonal to the free DOF
|
||
|
|
// * then we truncate swing
|
||
|
|
// The case of one locked DOF is currently unimplemented, but one option would be:
|
||
|
|
// * if one DOF is locked (the tricky case), we define the 'free' axis as follows (as the velocity solver prep function does)
|
||
|
|
// TWIST: cB[0]
|
||
|
|
// SWING1: cB[0].cross(cA[2])
|
||
|
|
// SWING2: cB[0].cross(cA[1])
|
||
|
|
// then, as above, we decompose into swing * free, and truncate the free rotation
|
||
|
|
|
||
|
|
//export this in the physx namespace so we can unit test it
|
||
|
|
namespace physx
|
||
|
|
{
|
||
|
|
PxQuat angularProject(PxU32 lockedDofs, const PxQuat& q, PxReal cosHalfTol, bool& truncated)
|
||
|
|
{
|
||
|
|
PX_ASSERT(lockedDofs <= 7);
|
||
|
|
truncated = false;
|
||
|
|
|
||
|
|
switch(lockedDofs)
|
||
|
|
{
|
||
|
|
case 0: return q;
|
||
|
|
case 1: return q; // currently unimplemented
|
||
|
|
case 2: return q; // currently unimplemented
|
||
|
|
case 3: return project(q, PxVec3(0.0f, 0.0f, 1.0f), cosHalfTol, truncated);
|
||
|
|
case 4: return q; // currently unimplemented
|
||
|
|
case 5: return project(q, PxVec3(0.0f, 1.0f, 0.0f), cosHalfTol, truncated);
|
||
|
|
case 6: return project(q, PxVec3(1.0f, 0.0f, 0.0f), cosHalfTol, truncated);
|
||
|
|
case 7: return truncate(q, cosHalfTol, truncated);
|
||
|
|
default: return PxQuat(PxIdentity);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void D6JointProject(const void* constantBlock, PxTransform& bodyAToWorld, PxTransform& bodyBToWorld, bool projectToA)
|
||
|
|
{
|
||
|
|
const D6JointData& data = *reinterpret_cast<const D6JointData*>(constantBlock);
|
||
|
|
|
||
|
|
PxTransform cA2w, cB2w, cB2cA, projected;
|
||
|
|
joint::computeDerived(data, bodyAToWorld, bodyBToWorld, cA2w, cB2w, cB2cA, false);
|
||
|
|
|
||
|
|
const PxVec3 v(data.locked & 1 ? cB2cA.p.x : 0.0f,
|
||
|
|
data.locked & 2 ? cB2cA.p.y : 0.0f,
|
||
|
|
data.locked & 4 ? cB2cA.p.z : 0.0f);
|
||
|
|
|
||
|
|
bool linearTrunc, angularTrunc = false;
|
||
|
|
projected.p = joint::truncateLinear(v, data.projectionLinearTolerance, linearTrunc) + (cB2cA.p - v);
|
||
|
|
|
||
|
|
projected.q = angularProject(data.locked >> 3, cB2cA.q, PxCos(data.projectionAngularTolerance / 2), angularTrunc);
|
||
|
|
|
||
|
|
if(linearTrunc || angularTrunc)
|
||
|
|
joint::projectTransforms(bodyAToWorld, bodyBToWorld, cA2w, cB2w, projected, data, projectToA);
|
||
|
|
}
|
||
|
|
|
||
|
|
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 D6JointData& data, const PxTransform& t, float swingYZ, float swingW, float swingLimitYZ)
|
||
|
|
{
|
||
|
|
bool active = PxAbs(computeSwingAngle(swingYZ, swingW)) > swingLimitYZ - data.swingLimit.contactDistance;
|
||
|
|
viz.visualizeAngularLimit(t, -swingLimitYZ, swingLimitYZ, active);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void visualizeDoubleCone(PxConstraintVisualizer& viz, const D6JointData& data, const PxTransform& t, float sin, float swingLimitYZ)
|
||
|
|
{
|
||
|
|
const PxReal angle = PxAsin(sin);
|
||
|
|
const PxReal pad = data.swingLimit.contactDistance;
|
||
|
|
const PxReal low = -swingLimitYZ;
|
||
|
|
const PxReal high = swingLimitYZ;
|
||
|
|
|
||
|
|
const bool active = isLimitActive(data.swingLimit, pad, angle, low, high);
|
||
|
|
viz.visualizeDoubleCone(t, swingLimitYZ, active);
|
||
|
|
}
|
||
|
|
|
||
|
|
// PT: TODO: refactor with spherical joint code
|
||
|
|
static void visualizeCone(PxConstraintVisualizer& viz, const D6JointData& data, const PxQuat& swing, const PxTransform& cA2w)
|
||
|
|
{
|
||
|
|
const PxVec3 swingAngle(0.0f, computeSwingAngle(swing.y, swing.w), computeSwingAngle(swing.z, swing.w));
|
||
|
|
const PxReal pad = data.swingLimit.isSoft() ? 0.0f : data.swingLimit.contactDistance;
|
||
|
|
Cm::ConeLimitHelperTanLess coneHelper(data.swingLimit.yAngle, data.swingLimit.zAngle, pad);
|
||
|
|
viz.visualizeLimitCone(cA2w, PxTan(data.swingLimit.zAngle / 4), PxTan(data.swingLimit.yAngle / 4), !coneHelper.contains(swingAngle));
|
||
|
|
}
|
||
|
|
|
||
|
|
static PX_FORCE_INLINE bool isLinearLimitActive(const PxJointLinearLimitPair& limit, float ordinate)
|
||
|
|
{
|
||
|
|
const PxReal pad = limit.isSoft() ? 0.0f : limit.contactDistance;
|
||
|
|
return (ordinate < limit.lower + pad) || (ordinate > limit.upper - pad);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void visualizeLine(PxConstraintVisualizer& viz, const PxVec3& origin, const PxVec3& axis, const PxJointLinearLimitPair& limit, float ordinate)
|
||
|
|
{
|
||
|
|
const bool active = isLinearLimitActive(limit, ordinate);
|
||
|
|
const PxVec3 p0 = origin + axis * limit.lower;
|
||
|
|
const PxVec3 p1 = origin + axis * limit.upper;
|
||
|
|
viz.visualizeLine(p0, p1, active ? 0xff0000u : 0xffffffu);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void visualizeQuad(PxConstraintVisualizer& viz, const PxVec3& origin, const PxVec3& axis0, const PxJointLinearLimitPair& limit0, float ordinate0,
|
||
|
|
const PxVec3& axis1, const PxJointLinearLimitPair& limit1, float ordinate1)
|
||
|
|
{
|
||
|
|
const bool active0 = isLinearLimitActive(limit0, ordinate0);
|
||
|
|
const bool active1 = isLinearLimitActive(limit1, ordinate1);
|
||
|
|
const PxU32 color = (active0 || active1) ? 0xff0000u : 0xffffffu;
|
||
|
|
|
||
|
|
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, float ordinate0,
|
||
|
|
const PxVec3& axis1, const PxJointLinearLimitPair& limit1, float ordinate1,
|
||
|
|
const PxVec3& axis2, const PxJointLinearLimitPair& limit2, float ordinate2)
|
||
|
|
{
|
||
|
|
const bool active0 = isLinearLimitActive(limit0, ordinate0);
|
||
|
|
const bool active1 = isLinearLimitActive(limit1, ordinate1);
|
||
|
|
const bool active2 = isLinearLimitActive(limit2, ordinate2);
|
||
|
|
const PxU32 color = (active0 || active1 || active2) ? 0xff0000u : 0xffffffu;
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
void _setRotY(PxMat33& m, PxReal angle)
|
||
|
|
{
|
||
|
|
m = PxMat33(PxIdentity);
|
||
|
|
|
||
|
|
const PxReal cos = cosf(angle);
|
||
|
|
const PxReal sin = sinf(angle);
|
||
|
|
|
||
|
|
m[0][0] = m[2][2] = cos;
|
||
|
|
m[0][2] = -sin;
|
||
|
|
m[2][0] = sin;
|
||
|
|
}
|
||
|
|
|
||
|
|
void _setRotZ(PxMat33& m, PxReal angle)
|
||
|
|
{
|
||
|
|
m = PxMat33(PxIdentity);
|
||
|
|
|
||
|
|
const PxReal cos = cosf(angle);
|
||
|
|
const PxReal sin = sinf(angle);
|
||
|
|
|
||
|
|
m[0][0] = m[1][1] = cos;
|
||
|
|
m[0][1] = sin;
|
||
|
|
m[1][0] = -sin;
|
||
|
|
}
|
||
|
|
|
||
|
|
PxQuat _getRotYQuat(float angle)
|
||
|
|
{
|
||
|
|
PxMat33 m;
|
||
|
|
_setRotY(m, angle);
|
||
|
|
return PxQuat(m);
|
||
|
|
}
|
||
|
|
|
||
|
|
PxQuat _getRotZQuat(float angle)
|
||
|
|
{
|
||
|
|
PxMat33 m;
|
||
|
|
_setRotZ(m, angle);
|
||
|
|
return PxQuat(m);
|
||
|
|
}
|
||
|
|
|
||
|
|
void _setRotX(PxMat33& m, PxReal angle)
|
||
|
|
{
|
||
|
|
m = PxMat33(PxIdentity);
|
||
|
|
|
||
|
|
const PxReal cos = cosf(angle);
|
||
|
|
const PxReal sin = sinf(angle);
|
||
|
|
|
||
|
|
m[1][1] = m[2][2] = cos;
|
||
|
|
m[1][2] = sin;
|
||
|
|
m[2][1] = -sin;
|
||
|
|
}
|
||
|
|
|
||
|
|
PxQuat _getRotXQuat(float angle)
|
||
|
|
{
|
||
|
|
PxMat33 m;
|
||
|
|
_setRotX(m, angle);
|
||
|
|
return PxQuat(m);
|
||
|
|
}
|
||
|
|
|
||
|
|
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; _setRotZ(my, z);
|
||
|
|
PxMat33 mz; _setRotY(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 bool activeY = useY ? isLimitActive(l, l.contactDistance, computeSwingAngle(swing.y, swing.w), l.yAngleMin, l.yAngleMax) : false;
|
||
|
|
const bool activeZ = useZ ? isLimitActive(l, l.contactDistance, computeSwingAngle(swing.z, swing.w), l.zAngleMin, l.zAngleMax) : false;
|
||
|
|
const PxU32 color = (activeY||activeZ) ? PxDebugColor::eARGB_RED : PxDebugColor::eARGB_GREY;
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
PxTransform cA2w, cB2w;
|
||
|
|
joint::computeJointFrames(cA2w, cB2w, data, body0Transform, body1Transform);
|
||
|
|
if(flags & PxConstraintVisualizationFlag::eLOCAL_FRAMES)
|
||
|
|
viz.visualizeJointFrames(cA2w, cB2w);
|
||
|
|
|
||
|
|
if(flags & PxConstraintVisualizationFlag::eLIMITS)
|
||
|
|
{
|
||
|
|
// PT: it is a mistake to use the neighborhood operator since it
|
||
|
|
// prevents us from using the quat's double-cover feature.
|
||
|
|
// if(cA2w.q.dot(cB2w.q)<0.0f)
|
||
|
|
// cB2w.q = -cB2w.q;
|
||
|
|
|
||
|
|
const PxTransform cB2cA = cA2w.transformInv(cB2w);
|
||
|
|
const PxMat33 cA2w_m(cA2w.q), cB2w_m(cB2w.q);
|
||
|
|
|
||
|
|
if(data.mUseNewLinearLimits)
|
||
|
|
{
|
||
|
|
switch(data.limited)
|
||
|
|
{
|
||
|
|
case 1<<PxD6Axis::eX:
|
||
|
|
visualizeLine(viz, cA2w.p, cA2w_m.column0, data.linearLimitX, cB2cA.p.x);
|
||
|
|
break;
|
||
|
|
case 1<<PxD6Axis::eY:
|
||
|
|
visualizeLine(viz, cA2w.p, cA2w_m.column1, data.linearLimitY, cB2cA.p.y);
|
||
|
|
break;
|
||
|
|
case 1<<PxD6Axis::eZ:
|
||
|
|
visualizeLine(viz, cA2w.p, cA2w_m.column2, data.linearLimitZ, cB2cA.p.z);
|
||
|
|
break;
|
||
|
|
case 1<<PxD6Axis::eX|1<<PxD6Axis::eY:
|
||
|
|
visualizeQuad(viz, cA2w.p, cA2w_m.column0, data.linearLimitX, cB2cA.p.x, cA2w_m.column1, data.linearLimitY, cB2cA.p.y);
|
||
|
|
break;
|
||
|
|
case 1<<PxD6Axis::eX|1<<PxD6Axis::eZ:
|
||
|
|
visualizeQuad(viz, cA2w.p, cA2w_m.column0, data.linearLimitX, cB2cA.p.x, cA2w_m.column2, data.linearLimitZ, cB2cA.p.z);
|
||
|
|
break;
|
||
|
|
case 1<<PxD6Axis::eY|1<<PxD6Axis::eZ:
|
||
|
|
visualizeQuad(viz, cA2w.p, cA2w_m.column1, data.linearLimitY, cB2cA.p.y, cA2w_m.column2, data.linearLimitZ, cB2cA.p.z);
|
||
|
|
break;
|
||
|
|
case 1<<PxD6Axis::eX|1<<PxD6Axis::eY|1<<PxD6Axis::eZ:
|
||
|
|
visualizeBox(viz, cA2w.p, cA2w_m.column0, data.linearLimitX, cB2cA.p.x, cA2w_m.column1, data.linearLimitY, cB2cA.p.y, cA2w_m.column2, data.linearLimitZ, cB2cA.p.z);
|
||
|
|
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;
|
||
|
|
Ps::separateSwingTwist(cB2cA.q, swing, twist);
|
||
|
|
|
||
|
|
const PxVec3& bX = cB2w_m.column0;
|
||
|
|
const PxVec3& aY = cA2w_m.column1;
|
||
|
|
const PxVec3& aZ = cA2w_m.column2;
|
||
|
|
|
||
|
|
if(data.limited&TWIST_FLAG)
|
||
|
|
{
|
||
|
|
const PxReal angle = computePhi(twist);
|
||
|
|
const PxReal pad = data.twistLimit.contactDistance;
|
||
|
|
const PxReal low = data.twistLimit.lower;
|
||
|
|
const PxReal high = data.twistLimit.upper;
|
||
|
|
|
||
|
|
const bool active = isLimitActive(data.twistLimit, pad, angle, low, high);
|
||
|
|
viz.visualizeAngularLimit(cA2w, data.twistLimit.lower, data.twistLimit.upper, active);
|
||
|
|
}
|
||
|
|
|
||
|
|
const bool swing1Limited = (data.limited & SWING1_FLAG)!=0, swing2Limited = (data.limited & SWING2_FLAG)!=0;
|
||
|
|
|
||
|
|
if(swing1Limited && swing2Limited)
|
||
|
|
{
|
||
|
|
if(data.mUseConeLimit)
|
||
|
|
visualizeCone(viz, data, swing, 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
|
||
|
|
visualizeAngularLimit(viz, data, cA2w * yToX, swing.y, swing.w, data.swingLimit.yAngle); // PT: swing Y limited, swing Z locked
|
||
|
|
}
|
||
|
|
else
|
||
|
|
if(!data.mUsePyramidLimits)
|
||
|
|
visualizeDoubleCone(viz, data, cA2w * zToX, aZ.dot(bX), 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
|
||
|
|
visualizeAngularLimit(viz, data, cA2w * zToX, swing.z, swing.w, data.swingLimit.zAngle); // PT: swing Z limited, swing Y locked
|
||
|
|
}
|
||
|
|
else
|
||
|
|
if(!data.mUsePyramidLimits)
|
||
|
|
visualizeDoubleCone(viz, data, cA2w * yToX, aY.dot(bX), 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, data.swingLimit.contactDistance, 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, data.swingLimit.contactDistance, axis.getNormalized(), data.swingLimit);
|
||
|
|
}
|
||
|
|
|
||
|
|
// PT: TODO: refactor with spherical joint code
|
||
|
|
static void setupConeSwingLimits(joint::ConstraintHelper& ch, const D6JointData& data, const PxQuat& swing, const PxTransform& cA2w)
|
||
|
|
{
|
||
|
|
PxVec3 axis;
|
||
|
|
PxReal error;
|
||
|
|
const PxReal pad = data.swingLimit.isSoft() ? 0.0f : data.swingLimit.contactDistance;
|
||
|
|
const Cm::ConeLimitHelperTanLess coneHelper(data.swingLimit.yAngle, data.swingLimit.zAngle, pad);
|
||
|
|
bool active = coneHelper.getLimit(swing, axis, error);
|
||
|
|
if(active)
|
||
|
|
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, l.contactDistance, q.getBasisVector1(), l);
|
||
|
|
if(useZ)
|
||
|
|
ch.anglePair(computeSwingAngle(swing.z, swing.w), l.zAngleMin, l.zAngleMax, l.contactDistance, 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);
|
||
|
|
}
|
||
|
|
|
||
|
|
static PxU32 D6JointSolverPrep(Px1DConstraint* constraints,
|
||
|
|
PxVec3& body0WorldOffset,
|
||
|
|
PxU32 /*maxConstraints*/,
|
||
|
|
PxConstraintInvMassScale& invMassScale,
|
||
|
|
const void* constantBlock,
|
||
|
|
const PxTransform& bA2w,
|
||
|
|
const PxTransform& bB2w,
|
||
|
|
bool useExtendedLimits,
|
||
|
|
PxVec3& cA2wOut, PxVec3& cB2wOut)
|
||
|
|
{
|
||
|
|
const D6JointData& data = *reinterpret_cast<const D6JointData*>(constantBlock);
|
||
|
|
|
||
|
|
PxTransform 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 && cA2w.q.dot(cB2w.q)<0.0f) // minimum dist quat (equiv to flipping cB2bB.q, which we don't use anywhere)
|
||
|
|
cB2w.q = -cB2w.q;
|
||
|
|
|
||
|
|
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 PxMat33 cA2w_m(cA2w.q);
|
||
|
|
const PxMat33 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[PxD6Drive::eX+i]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if(driving & ((1<<PxD6Drive::eSLERP)|(1<<PxD6Drive::eSWING)|(1<<PxD6Drive::eTWIST)))
|
||
|
|
{
|
||
|
|
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[PxD6Drive::eSLERP].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[PxD6Drive::eSLERP], PxConstraintSolveHint::eSLERP_SPRING);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
if(driving & (1<<PxD6Drive::eTWIST))
|
||
|
|
ch.angular(bX, v.x, -2.0f * delta.x, drives[PxD6Drive::eTWIST]);
|
||
|
|
|
||
|
|
if(driving & (1<<PxD6Drive::eSWING))
|
||
|
|
{
|
||
|
|
const PxVec3 err = delta.rotate(PxVec3(1.0f, 0.0f, 0.0f));
|
||
|
|
|
||
|
|
if(!(locked & SWING1_FLAG))
|
||
|
|
ch.angular(cB2w_m[1], v.y, err.z, drives[PxD6Drive::eSWING]);
|
||
|
|
|
||
|
|
if(!(locked & SWING2_FLAG))
|
||
|
|
ch.angular(cB2w_m[2], v.z, -err.y, drives[PxD6Drive::eSWING]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if(limited & ANGULAR_MASK)
|
||
|
|
{
|
||
|
|
PxQuat swing, twist;
|
||
|
|
Ps::separateSwingTwist(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 == Ps::tanHalf, 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
|
||
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "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
|
||
|
|
Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "D6JointSolverPrep: invalid joint setup. Double pyramid mode not supported.");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if(limited & TWIST_FLAG)
|
||
|
|
{
|
||
|
|
ch.anglePair(computePhi(twist), data.twistLimit.lower, data.twistLimit.upper, data.twistLimit.contactDistance, 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));
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
|
||
|
|
PxConstraintShaderTable Ext::D6Joint::sShaders = { D6JointSolverPrep, D6JointProject, D6JointVisualize, /*PxConstraintFlag::Enum(0)*/PxConstraintFlag::eGPU_COMPATIBLE };
|