335 lines
11 KiB
C++
335 lines
11 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 "ExtDistanceJoint.h"
|
|
#include "ExtConstraintHelper.h"
|
|
#include "PxPhysics.h"
|
|
|
|
using namespace physx;
|
|
using namespace Ext;
|
|
|
|
PxDistanceJoint* physx::PxDistanceJointCreate(PxPhysics& physics, PxRigidActor* actor0, const PxTransform& localFrame0, PxRigidActor* actor1, const PxTransform& localFrame1)
|
|
{
|
|
PX_CHECK_AND_RETURN_NULL(localFrame0.isSane(), "PxDistanceJointCreate: local frame 0 is not a valid transform");
|
|
PX_CHECK_AND_RETURN_NULL(localFrame1.isSane(), "PxDistanceJointCreate: local frame 1 is not a valid transform");
|
|
PX_CHECK_AND_RETURN_NULL(actor0 != actor1, "PxDistanceJointCreate: 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");
|
|
|
|
DistanceJoint* j;
|
|
PX_NEW_SERIALIZED(j, DistanceJoint)(physics.getTolerancesScale(), actor0, localFrame0, actor1, localFrame1);
|
|
if(j->attach(physics, actor0, actor1))
|
|
return j;
|
|
|
|
PX_DELETE(j);
|
|
return NULL;
|
|
}
|
|
|
|
PxReal DistanceJoint::getDistance() const
|
|
{
|
|
return getRelativeTransform().p.magnitude();
|
|
}
|
|
|
|
void DistanceJoint::setMinDistance(PxReal distance)
|
|
{
|
|
PX_CHECK_AND_RETURN(PxIsFinite(distance), "PxDistanceJoint::setMinDistance: invalid parameter");
|
|
data().minDistance = distance;
|
|
markDirty();
|
|
}
|
|
|
|
PxReal DistanceJoint::getMinDistance() const
|
|
{
|
|
return data().minDistance;
|
|
}
|
|
|
|
void DistanceJoint::setMaxDistance(PxReal distance)
|
|
{
|
|
PX_CHECK_AND_RETURN(PxIsFinite(distance), "PxDistanceJoint::setMaxDistance: invalid parameter");
|
|
data().maxDistance = distance;
|
|
markDirty();
|
|
}
|
|
|
|
PxReal DistanceJoint::getMaxDistance() const
|
|
{
|
|
return data().maxDistance;
|
|
}
|
|
|
|
void DistanceJoint::setTolerance(PxReal tolerance)
|
|
{
|
|
PX_CHECK_AND_RETURN(PxIsFinite(tolerance), "PxDistanceJoint::setTolerance: invalid parameter");
|
|
data().tolerance = tolerance;
|
|
markDirty();
|
|
}
|
|
|
|
PxReal DistanceJoint::getTolerance() const
|
|
{
|
|
return data().tolerance;
|
|
}
|
|
|
|
void DistanceJoint::setStiffness(PxReal stiffness)
|
|
{
|
|
PX_CHECK_AND_RETURN(PxIsFinite(stiffness), "PxDistanceJoint::setStiffness: invalid parameter");
|
|
data().stiffness = stiffness;
|
|
markDirty();
|
|
}
|
|
|
|
PxReal DistanceJoint::getStiffness() const
|
|
{
|
|
return data().stiffness;
|
|
}
|
|
|
|
void DistanceJoint::setDamping(PxReal damping)
|
|
{
|
|
PX_CHECK_AND_RETURN(PxIsFinite(damping), "PxDistanceJoint::setDamping: invalid parameter");
|
|
data().damping = damping;
|
|
markDirty();
|
|
}
|
|
|
|
PxReal DistanceJoint::getDamping() const
|
|
{
|
|
return data().damping;
|
|
}
|
|
|
|
PxDistanceJointFlags DistanceJoint::getDistanceJointFlags(void) const
|
|
{
|
|
return data().jointFlags;
|
|
}
|
|
|
|
void DistanceJoint::setDistanceJointFlags(PxDistanceJointFlags flags)
|
|
{
|
|
data().jointFlags = flags;
|
|
markDirty();
|
|
}
|
|
|
|
void DistanceJoint::setDistanceJointFlag(PxDistanceJointFlag::Enum flag, bool value)
|
|
{
|
|
if(value)
|
|
data().jointFlags |= flag;
|
|
else
|
|
data().jointFlags &= ~flag;
|
|
markDirty();
|
|
}
|
|
|
|
bool DistanceJoint::attach(PxPhysics &physics, PxRigidActor* actor0, PxRigidActor* actor1)
|
|
{
|
|
mPxConstraint = physics.createConstraint(actor0, actor1, *this, sShaders, sizeof(DistanceJointData));
|
|
return mPxConstraint!=NULL;
|
|
}
|
|
|
|
void DistanceJoint::exportExtraData(PxSerializationContext& stream)
|
|
{
|
|
if(mData)
|
|
{
|
|
stream.alignData(PX_SERIAL_ALIGN);
|
|
stream.writeData(mData, sizeof(DistanceJointData));
|
|
}
|
|
stream.writeName(mName);
|
|
}
|
|
|
|
void DistanceJoint::importExtraData(PxDeserializationContext& context)
|
|
{
|
|
if(mData)
|
|
mData = context.readExtraData<DistanceJointData, PX_SERIAL_ALIGN>();
|
|
|
|
context.readName(mName);
|
|
}
|
|
|
|
void DistanceJoint::resolveReferences(PxDeserializationContext& context)
|
|
{
|
|
setPxConstraint(resolveConstraintPtr(context, getPxConstraint(), getConnector(), sShaders));
|
|
}
|
|
|
|
DistanceJoint* DistanceJoint::createObject(PxU8*& address, PxDeserializationContext& context)
|
|
{
|
|
DistanceJoint* obj = new (address) DistanceJoint(PxBaseFlag::eIS_RELEASABLE);
|
|
address += sizeof(DistanceJoint);
|
|
obj->importExtraData(context);
|
|
obj->resolveReferences(context);
|
|
return obj;
|
|
}
|
|
|
|
// global function to share the joint shaders with API capture
|
|
const PxConstraintShaderTable* Ext::GetDistanceJointShaderTable()
|
|
{
|
|
return &DistanceJoint::getConstraintShaderTable();
|
|
}
|
|
|
|
//~PX_SERIALIZATION
|
|
|
|
static void DistanceJointProject(const void* /*constantBlock*/, PxTransform& /*bodyAToWorld*/, PxTransform& /*bodyBToWorld*/, bool /*projectToA*/)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
static void DistanceJointVisualize(PxConstraintVisualizer& viz, const void* constantBlock, const PxTransform& body0Transform, const PxTransform& body1Transform, PxU32 flags)
|
|
{
|
|
const DistanceJointData& data = *reinterpret_cast<const DistanceJointData*>(constantBlock);
|
|
|
|
PxTransform cA2w, cB2w;
|
|
joint::computeJointFrames(cA2w, cB2w, data, body0Transform, body1Transform);
|
|
if(flags & PxConstraintVisualizationFlag::eLOCAL_FRAMES)
|
|
viz.visualizeJointFrames(cA2w, cB2w);
|
|
|
|
// PT: we consider the following is part of the joint's "limits" since that's the only available flag we have
|
|
if(flags & PxConstraintVisualizationFlag::eLIMITS)
|
|
{
|
|
const bool enforceMax = (data.jointFlags & PxDistanceJointFlag::eMAX_DISTANCE_ENABLED);
|
|
const bool enforceMin = (data.jointFlags & PxDistanceJointFlag::eMIN_DISTANCE_ENABLED);
|
|
if(!enforceMin && !enforceMax)
|
|
return;
|
|
|
|
PxVec3 dir = cB2w.p - cA2w.p;
|
|
const float currentDist = dir.normalize();
|
|
|
|
PxU32 color = 0x00ff00;
|
|
if(enforceMax && currentDist>data.maxDistance)
|
|
color = 0xff0000;
|
|
if(enforceMin && currentDist<data.minDistance)
|
|
color = 0x0000ff;
|
|
|
|
viz.visualizeLine(cA2w.p, cB2w.p, color);
|
|
}
|
|
}
|
|
|
|
PX_FORCE_INLINE void setupContraint(Px1DConstraint& c, const PxVec3& direction, const PxVec3& angular0, const PxVec3& angular1, const DistanceJointData& data)
|
|
{
|
|
// constraint is breakable, so we need to output forces
|
|
|
|
c.flags = Px1DConstraintFlag::eOUTPUT_FORCE;
|
|
|
|
c.linear0 = direction; c.angular0 = angular0;
|
|
c.linear1 = direction; c.angular1 = angular1;
|
|
|
|
if(data.jointFlags & PxDistanceJointFlag::eSPRING_ENABLED)
|
|
{
|
|
c.flags |= Px1DConstraintFlag::eSPRING;
|
|
c.mods.spring.stiffness= data.stiffness;
|
|
c.mods.spring.damping = data.damping;
|
|
}
|
|
}
|
|
|
|
static PxU32 DistanceJointSolverPrep(Px1DConstraint* constraints,
|
|
PxVec3& body0WorldOffset,
|
|
PxU32 /*maxConstraints*/,
|
|
PxConstraintInvMassScale& invMassScale,
|
|
const void* constantBlock,
|
|
const PxTransform& bA2w,
|
|
const PxTransform& bB2w,
|
|
bool /*useExtendedLimits*/,
|
|
PxVec3& cA2wOut, PxVec3& cB2wOut)
|
|
{
|
|
const DistanceJointData& data = *reinterpret_cast<const DistanceJointData*>(constantBlock);
|
|
|
|
PxTransform cA2w, cB2w;
|
|
joint::ConstraintHelper ch(constraints, invMassScale, cA2w, cB2w, body0WorldOffset, data, bA2w, bB2w);
|
|
|
|
cA2wOut = cB2w.p;
|
|
cB2wOut = cB2w.p;
|
|
|
|
PxVec3 direction = cA2w.p - cB2w.p;
|
|
const PxReal distance = direction.normalize();
|
|
|
|
const bool enforceMax = (data.jointFlags & PxDistanceJointFlag::eMAX_DISTANCE_ENABLED);
|
|
const bool enforceMin = (data.jointFlags & PxDistanceJointFlag::eMIN_DISTANCE_ENABLED);
|
|
|
|
#define EPS_REAL 1.192092896e-07F
|
|
|
|
if(distance < EPS_REAL)
|
|
direction = PxVec3(1.0f, 0.0f, 0.0f);
|
|
|
|
Px1DConstraint* c = constraints;
|
|
|
|
const PxVec3 angular0 = ch.getRa().cross(direction);
|
|
const PxVec3 angular1 = ch.getRb().cross(direction);
|
|
|
|
setupContraint(*c, direction, angular0, angular1, data);
|
|
|
|
//add tolerance so we don't have contact-style jitter problem.
|
|
|
|
if(data.minDistance == data.maxDistance && enforceMin && enforceMax)
|
|
{
|
|
const PxReal error = distance - data.maxDistance;
|
|
c->geometricError = error > data.tolerance ? error - data.tolerance :
|
|
error < -data.tolerance ? error + data.tolerance : 0.0f;
|
|
}
|
|
else if(enforceMax && distance > data.maxDistance)
|
|
{
|
|
c->geometricError = distance - data.maxDistance - data.tolerance;
|
|
c->maxImpulse = 0.0f;
|
|
}
|
|
else if(enforceMin && distance < data.minDistance)
|
|
{
|
|
c->geometricError = distance - data.minDistance + data.tolerance;
|
|
c->minImpulse = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
if(enforceMin && enforceMax)
|
|
{
|
|
// since we dont know the current rigid velocity, we need to insert row for both limits
|
|
Px1DConstraint* minConstraint = constraints;
|
|
minConstraint->geometricError = distance - data.minDistance;
|
|
minConstraint->minImpulse = 0.0f;
|
|
minConstraint->maxImpulse = FLT_MAX;
|
|
minConstraint->flags |= Px1DConstraintFlag::eKEEPBIAS;
|
|
|
|
Px1DConstraint* maxConstraint = constraints;
|
|
maxConstraint++;
|
|
|
|
setupContraint(*maxConstraint, direction, angular0, angular1, data);
|
|
|
|
maxConstraint->geometricError = distance - data.maxDistance;
|
|
maxConstraint->minImpulse = -FLT_MAX;
|
|
maxConstraint->maxImpulse = 0.0f;
|
|
maxConstraint->flags |= Px1DConstraintFlag::eKEEPBIAS;
|
|
|
|
return 2;
|
|
}
|
|
else if(enforceMax)
|
|
{
|
|
c->geometricError = distance - data.maxDistance;
|
|
c->minImpulse = -FLT_MAX;
|
|
c->maxImpulse = 0.0f;
|
|
c->flags |= Px1DConstraintFlag::eKEEPBIAS;
|
|
return 0;
|
|
}
|
|
else if(enforceMin)
|
|
{
|
|
c->geometricError = distance - data.minDistance;
|
|
c->minImpulse = 0.0f;
|
|
c->maxImpulse = FLT_MAX;
|
|
c->flags |= Px1DConstraintFlag::eKEEPBIAS;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
PxConstraintShaderTable Ext::DistanceJoint::sShaders = { DistanceJointSolverPrep, DistanceJointProject, DistanceJointVisualize, PxConstraintFlag::Enum(0) };
|