Init
This commit is contained in:
231
physx/snippets/snippetcustomjoint/PulleyJoint.cpp
Normal file
231
physx/snippets/snippetcustomjoint/PulleyJoint.cpp
Normal file
@ -0,0 +1,231 @@
|
||||
//
|
||||
// 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-2021 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 "PulleyJoint.h"
|
||||
#include <assert.h>
|
||||
#include "PxConstraint.h"
|
||||
|
||||
using namespace physx;
|
||||
|
||||
|
||||
PxConstraintShaderTable PulleyJoint::sShaderTable = { &PulleyJoint::solverPrep, &PulleyJoint::project, &PulleyJoint::visualize, PxConstraintFlag::Enum(0) };
|
||||
|
||||
PulleyJoint::PulleyJoint(PxPhysics& physics, PxRigidBody& body0, const PxTransform& localFrame0, const PxVec3& attachment0,
|
||||
PxRigidBody& body1, const PxTransform& localFrame1, const PxVec3& attachment1)
|
||||
{
|
||||
mConstraint = physics.createConstraint(&body0, &body1, *this, PulleyJoint::sShaderTable, sizeof(PulleyJointData));
|
||||
|
||||
mBody[0] = &body0;
|
||||
mBody[1] = &body1;
|
||||
|
||||
// keep these around in case the CoM gets relocated
|
||||
mLocalPose[0] = localFrame0.getNormalized();
|
||||
mLocalPose[1] = localFrame1.getNormalized();
|
||||
|
||||
// the data which will be fed to the joint solver and projection shaders
|
||||
mData.attachment0 = attachment0;
|
||||
mData.attachment1 = attachment1;
|
||||
mData.distance = 1.0f;
|
||||
mData.ratio = 1.0f;
|
||||
mData.c2b[0] = body0.getCMassLocalPose().transformInv(mLocalPose[0]);
|
||||
mData.c2b[1] = body1.getCMassLocalPose().transformInv(mLocalPose[1]);
|
||||
}
|
||||
|
||||
void PulleyJoint::release()
|
||||
{
|
||||
mConstraint->release();
|
||||
}
|
||||
|
||||
///////////////////////////////////////////// attribute accessors and mutators
|
||||
|
||||
|
||||
void PulleyJoint::setAttachment0(const PxVec3& pos)
|
||||
{
|
||||
mData.attachment0 = pos;
|
||||
mConstraint->markDirty();
|
||||
}
|
||||
|
||||
PxVec3 PulleyJoint::getAttachment0() const
|
||||
{
|
||||
return mData.attachment0;
|
||||
}
|
||||
|
||||
void PulleyJoint::setAttachment1(const PxVec3& pos)
|
||||
{
|
||||
mData.attachment1 = pos;
|
||||
mConstraint->markDirty();
|
||||
}
|
||||
|
||||
PxVec3 PulleyJoint::getAttachment1() const
|
||||
{
|
||||
return mData.attachment1;
|
||||
}
|
||||
|
||||
void PulleyJoint::setDistance(float totalDistance)
|
||||
{
|
||||
mData.distance = totalDistance;
|
||||
mConstraint->markDirty();
|
||||
}
|
||||
|
||||
float PulleyJoint::getDistance() const
|
||||
{
|
||||
return mData.distance;
|
||||
}
|
||||
|
||||
void PulleyJoint::setRatio(float ratio)
|
||||
{
|
||||
mData.ratio = ratio;
|
||||
mConstraint->markDirty();
|
||||
}
|
||||
|
||||
float PulleyJoint::getRatio() const
|
||||
{
|
||||
return mData.ratio;
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////////// PxConstraintConnector methods
|
||||
|
||||
void* PulleyJoint::prepareData()
|
||||
{
|
||||
return &mData;
|
||||
}
|
||||
|
||||
void PulleyJoint::onConstraintRelease()
|
||||
{
|
||||
delete this;
|
||||
}
|
||||
|
||||
void PulleyJoint::onComShift(PxU32 actor)
|
||||
{
|
||||
mData.c2b[actor] = mBody[actor]->getCMassLocalPose().transformInv(mLocalPose[actor]);
|
||||
mConstraint->markDirty();
|
||||
}
|
||||
|
||||
void PulleyJoint::onOriginShift(const PxVec3& shift)
|
||||
{
|
||||
mData.attachment0 -= shift;
|
||||
mData.attachment1 -= shift;
|
||||
}
|
||||
|
||||
void* PulleyJoint::getExternalReference(PxU32& typeID)
|
||||
{
|
||||
typeID = TYPE_ID;
|
||||
return this;
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////////// work functions
|
||||
|
||||
|
||||
PxU32 PulleyJoint::solverPrep(Px1DConstraint* constraints,
|
||||
PxVec3& body0WorldOffset,
|
||||
PxU32 maxConstraints,
|
||||
PxConstraintInvMassScale&,
|
||||
const void* constantBlock,
|
||||
const PxTransform& bA2w,
|
||||
const PxTransform& bB2w,
|
||||
bool /*useExtendedLimits*/,
|
||||
PxVec3& cA2wOut, PxVec3& cB2wOut)
|
||||
{
|
||||
PX_UNUSED(maxConstraints);
|
||||
|
||||
const PulleyJointData& data = *reinterpret_cast<const PulleyJointData*>(constantBlock);
|
||||
|
||||
PxTransform cA2w = bA2w.transform(data.c2b[0]);
|
||||
PxTransform cB2w = bB2w.transform(data.c2b[1]);
|
||||
|
||||
cA2wOut = cA2w.p;
|
||||
cB2wOut = cB2w.p;
|
||||
|
||||
body0WorldOffset = cB2w.p - bA2w.p;
|
||||
|
||||
PxVec3 directionA = data.attachment0 - cA2w.p;
|
||||
PxReal distanceA = directionA.normalize();
|
||||
|
||||
PxVec3 directionB = data.attachment1 - cB2w.p;
|
||||
PxReal distanceB = directionB.normalize();
|
||||
|
||||
directionB *= data.ratio;
|
||||
|
||||
PxReal totalDistance = distanceA + distanceB;
|
||||
|
||||
// compute geometric error:
|
||||
PxReal geometricError = (data.distance - totalDistance);
|
||||
|
||||
Px1DConstraint *c = constraints;
|
||||
// constraint is breakable, so we need to output forces
|
||||
c->flags = Px1DConstraintFlag::eOUTPUT_FORCE;
|
||||
|
||||
if (geometricError < 0.0f)
|
||||
{
|
||||
c->maxImpulse = PX_MAX_F32;
|
||||
c->minImpulse = 0;
|
||||
c->geometricError = geometricError;
|
||||
}
|
||||
else if(geometricError > 0.0f)
|
||||
{
|
||||
c->maxImpulse = 0;
|
||||
c->minImpulse = -PX_MAX_F32;
|
||||
c->geometricError = geometricError;
|
||||
}
|
||||
|
||||
c->linear0 = directionA; c->angular0 = (cA2w.p - bA2w.p).cross(c->linear0);
|
||||
c->linear1 = -directionB; c->angular1 = (cB2w.p - bB2w.p).cross(c->linear1);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
void PulleyJoint::project(const void* constantBlock,
|
||||
PxTransform& bodyAToWorld,
|
||||
PxTransform& bodyBToWorld,
|
||||
bool projectToA)
|
||||
{
|
||||
PX_UNUSED(constantBlock);
|
||||
PX_UNUSED(bodyAToWorld);
|
||||
PX_UNUSED(bodyBToWorld);
|
||||
PX_UNUSED(projectToA);
|
||||
}
|
||||
|
||||
void PulleyJoint::visualize(PxConstraintVisualizer& viz,
|
||||
const void* constantBlock,
|
||||
const PxTransform& body0Transform,
|
||||
const PxTransform& body1Transform,
|
||||
PxU32 flags)
|
||||
{
|
||||
PX_UNUSED(flags);
|
||||
const PulleyJointData& data = *reinterpret_cast<const PulleyJointData*>(constantBlock);
|
||||
|
||||
PxTransform cA2w = body0Transform * data.c2b[0];
|
||||
PxTransform cB2w = body1Transform * data.c2b[1];
|
||||
viz.visualizeJointFrames(cA2w, cB2w);
|
||||
viz.visualizeJointFrames(PxTransform(data.attachment0), PxTransform(data.attachment1));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user