2241 lines
68 KiB
C++
2241 lines
68 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-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.
|
|
|
|
// ****************************************************************************
|
|
// This snippet demonstrates the use of immediate articulations.
|
|
// ****************************************************************************
|
|
|
|
#include "PxPhysicsAPI.h"
|
|
#include "PxImmediateMode.h"
|
|
#include "PsArray.h"
|
|
#include "PsHashSet.h"
|
|
#include "PsHashMap.h"
|
|
#include "ExtConstraintHelper.h"
|
|
|
|
#include "../snippetutils/SnippetUtils.h"
|
|
#include "../snippetcommon/SnippetPrint.h"
|
|
#include "../snippetcommon/SnippetPVD.h"
|
|
|
|
#define USE_TGS 0
|
|
|
|
//#define PRINT_TIMINGS
|
|
#define TEST_IMMEDIATE_JOINTS
|
|
|
|
//Enables whether we want persistent state caching (contact cache, friction caching) or not. Avoiding persistency results in one-shot collision detection and zero friction
|
|
//correlation but simplifies code by not longer needing to cache persistent pairs.
|
|
#define WITH_PERSISTENCY 1
|
|
|
|
//Toggles whether we batch constraints or not. Constraint batching is an optional process which can improve performance by grouping together independent constraints. These independent constraints
|
|
//can be solved in parallel by using multiple lanes of SIMD registers.
|
|
#define BATCH_CONTACTS 1
|
|
|
|
using namespace physx;
|
|
using namespace shdfnd;
|
|
using namespace immediate;
|
|
|
|
static const PxVec3 gGravity(0.0f, -9.81f, 0.0f);
|
|
static const float gContactDistance = 0.1f;
|
|
static const float gMeshContactMargin = 0.01f;
|
|
static const float gToleranceLength = 1.0f;
|
|
static const float gBounceThreshold = -2.0f;
|
|
static const float gFrictionOffsetThreshold = 0.04f;
|
|
static const float gCorrelationDistance = 0.025f;
|
|
static const float gBoundsInflation = 0.02f;
|
|
static const float gStaticFriction = 0.5f;
|
|
static const float gDynamicFriction = 0.5f;
|
|
static const float gRestitution = 0.0f;
|
|
static const float gMaxDepenetrationVelocity = 10.0f;
|
|
static const float gMaxContactImpulse = FLT_MAX;
|
|
static const float gLinearDamping = 0.1f;
|
|
static const float gAngularDamping = 0.05f;
|
|
static const float gMaxLinearVelocity = 100.0f;
|
|
static const float gMaxAngularVelocity = 100.0f;
|
|
static const float gJointFrictionCoefficient = 0.05f;
|
|
static const PxU32 gNbIterPos = 4;
|
|
static const PxU32 gNbIterVel = 1;
|
|
|
|
static bool gPause = false;
|
|
static bool gOneFrame = false;
|
|
static bool gDrawBounds = false;
|
|
static PxU32 gSceneIndex = 2;
|
|
|
|
#if WITH_PERSISTENCY
|
|
struct PersistentContactPair
|
|
{
|
|
PersistentContactPair()
|
|
{
|
|
reset();
|
|
}
|
|
|
|
PxCache cache;
|
|
PxU8* frictions;
|
|
PxU32 nbFrictions;
|
|
|
|
PX_FORCE_INLINE void reset()
|
|
{
|
|
cache = PxCache();
|
|
frictions = NULL;
|
|
nbFrictions = 0;
|
|
}
|
|
};
|
|
#endif
|
|
|
|
class BlockBasedAllocator
|
|
{
|
|
struct AllocationPage
|
|
{
|
|
static const PxU32 PageSize = 32 * 1024;
|
|
PxU8 mPage[PageSize];
|
|
|
|
PxU32 currentIndex;
|
|
|
|
AllocationPage() : currentIndex(0){}
|
|
|
|
PxU8* allocate(const PxU32 size)
|
|
{
|
|
PxU32 alignedSize = (size + 15)&(~15);
|
|
if ((currentIndex + alignedSize) < PageSize)
|
|
{
|
|
PxU8* ret = &mPage[currentIndex];
|
|
currentIndex += alignedSize;
|
|
return ret;
|
|
}
|
|
return NULL;
|
|
}
|
|
};
|
|
|
|
AllocationPage* currentPage;
|
|
|
|
Array<AllocationPage*> mAllocatedBlocks;
|
|
PxU32 mCurrentIndex;
|
|
|
|
public:
|
|
BlockBasedAllocator() : currentPage(NULL), mCurrentIndex(0)
|
|
{
|
|
}
|
|
|
|
virtual PxU8* allocate(const PxU32 byteSize)
|
|
{
|
|
if (currentPage)
|
|
{
|
|
PxU8* data = currentPage->allocate(byteSize);
|
|
if (data)
|
|
return data;
|
|
}
|
|
|
|
if (mCurrentIndex < mAllocatedBlocks.size())
|
|
{
|
|
currentPage = mAllocatedBlocks[mCurrentIndex++];
|
|
currentPage->currentIndex = 0;
|
|
return currentPage->allocate(byteSize);
|
|
}
|
|
currentPage = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(AllocationPage), PX_DEBUG_EXP("AllocationPage")), AllocationPage)();
|
|
mAllocatedBlocks.pushBack(currentPage);
|
|
mCurrentIndex = mAllocatedBlocks.size();
|
|
|
|
return currentPage->allocate(byteSize);
|
|
}
|
|
|
|
void release() { for (PxU32 a = 0; a < mAllocatedBlocks.size(); ++a) PX_FREE(mAllocatedBlocks[a]); mAllocatedBlocks.clear(); currentPage = NULL; mCurrentIndex = 0; }
|
|
|
|
void reset() { currentPage = NULL; mCurrentIndex = 0; }
|
|
|
|
virtual ~BlockBasedAllocator()
|
|
{
|
|
release();
|
|
}
|
|
};
|
|
|
|
class TestCacheAllocator : public PxCacheAllocator
|
|
{
|
|
BlockBasedAllocator mAllocator[2];
|
|
PxU32 currIdx;
|
|
|
|
public:
|
|
|
|
TestCacheAllocator() : currIdx(0)
|
|
{
|
|
}
|
|
|
|
virtual PxU8* allocateCacheData(const PxU32 byteSize)
|
|
{
|
|
return mAllocator[currIdx].allocate(byteSize);
|
|
}
|
|
|
|
void release() { currIdx = 1 - currIdx; mAllocator[currIdx].release(); }
|
|
|
|
void reset() { currIdx = 1 - currIdx; mAllocator[currIdx].reset(); }
|
|
|
|
virtual ~TestCacheAllocator(){}
|
|
};
|
|
|
|
class TestConstraintAllocator : public PxConstraintAllocator
|
|
{
|
|
BlockBasedAllocator mConstraintAllocator;
|
|
BlockBasedAllocator mFrictionAllocator[2];
|
|
|
|
PxU32 currIdx;
|
|
public:
|
|
|
|
TestConstraintAllocator() : currIdx(0)
|
|
{
|
|
}
|
|
virtual PxU8* reserveConstraintData(const PxU32 byteSize){ return mConstraintAllocator.allocate(byteSize); }
|
|
virtual PxU8* reserveFrictionData(const PxU32 byteSize){ return mFrictionAllocator[currIdx].allocate(byteSize); }
|
|
|
|
void release() { currIdx = 1 - currIdx; mConstraintAllocator.release(); mFrictionAllocator[currIdx].release(); }
|
|
|
|
virtual ~TestConstraintAllocator() {}
|
|
};
|
|
|
|
struct IDS
|
|
{
|
|
PX_FORCE_INLINE IDS(PxU32 id0, PxU32 id1) : mID0(id0), mID1(id1) {}
|
|
PxU32 mID0;
|
|
PxU32 mID1;
|
|
|
|
PX_FORCE_INLINE bool operator == (const IDS& other) const
|
|
{
|
|
return mID0 == other.mID0 && mID1 == other.mID1;
|
|
}
|
|
};
|
|
|
|
PX_FORCE_INLINE uint32_t hash(const IDS& p)
|
|
{
|
|
return shdfnd::hash(uint64_t(p.mID0)|(uint64_t(p.mID1)<<32));
|
|
}
|
|
|
|
struct MassProps
|
|
{
|
|
PxVec3 mInvInertia;
|
|
float mInvMass;
|
|
};
|
|
|
|
static void computeMassProps(MassProps& props, const PxGeometry& geometry, float mass)
|
|
{
|
|
if(mass!=0.0f)
|
|
{
|
|
PxMassProperties inertia(geometry);
|
|
inertia = inertia * (mass/inertia.mass);
|
|
|
|
PxQuat orient;
|
|
const PxVec3 diagInertia = PxMassProperties::getMassSpaceInertia(inertia.inertiaTensor, orient);
|
|
props.mInvMass = 1.0f/inertia.mass;
|
|
props.mInvInertia.x = diagInertia.x == 0.0f ? 0.0f : 1.0f/diagInertia.x;
|
|
props.mInvInertia.y = diagInertia.y == 0.0f ? 0.0f : 1.0f/diagInertia.y;
|
|
props.mInvInertia.z = diagInertia.z == 0.0f ? 0.0f : 1.0f/diagInertia.z;
|
|
}
|
|
else
|
|
{
|
|
props.mInvMass = 0.0f;
|
|
props.mInvInertia = PxVec3(0.0f);
|
|
}
|
|
}
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
struct MyJointData : Ext::JointData
|
|
{
|
|
PxU32 mActors[2];
|
|
PxTransform mLocalFrames[2];
|
|
|
|
void initInvMassScale()
|
|
{
|
|
invMassScale.linear0 = 1.0f;
|
|
invMassScale.angular0 = 1.0f;
|
|
invMassScale.linear1 = 1.0f;
|
|
invMassScale.angular1 = 1.0f;
|
|
}
|
|
};
|
|
#endif
|
|
|
|
class ImmediateScene
|
|
{
|
|
PX_NOCOPY(ImmediateScene)
|
|
public:
|
|
ImmediateScene();
|
|
~ImmediateScene();
|
|
|
|
void reset();
|
|
|
|
PxU32 createActor(const PxGeometry& geometry, const PxTransform& pose, const MassProps* massProps=NULL, Dy::ArticulationLinkHandle link=0);
|
|
|
|
void createGroundPlane()
|
|
{
|
|
createActor(PxPlaneGeometry(), PxTransformFromPlaneEquation(PxPlane(0.0f, 1.0f, 0.0f, 0.0f)));
|
|
}
|
|
|
|
void createScene();
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
void createSphericalJoint(PxU32 id0, PxU32 id1, const PxTransform& localFrame0, const PxTransform& localFrame1, const PxTransform* pose0=NULL, const PxTransform* pose1=NULL);
|
|
#endif
|
|
void updateArticulations(float dt);
|
|
void updateBounds();
|
|
void broadPhase();
|
|
void narrowPhase();
|
|
void buildSolverBodyData(float dt);
|
|
void buildSolverConstraintDesc();
|
|
void createContactConstraints(float dt, float invDt, float lengthScale, PxU32 nbPositionIterations);
|
|
void solveAndIntegrate(float dt);
|
|
|
|
TestCacheAllocator* mCacheAllocator;
|
|
TestConstraintAllocator* mConstraintAllocator;
|
|
|
|
// PT: TODO: revisit this basic design once everything works
|
|
Array<PxGeometryHolder> mGeoms;
|
|
Array<PxTransform> mPoses;
|
|
Array<PxBounds3> mBounds;
|
|
|
|
class ImmediateActor
|
|
{
|
|
public:
|
|
ImmediateActor() {}
|
|
~ImmediateActor() {}
|
|
|
|
enum Type
|
|
{
|
|
eSTATIC,
|
|
eDYNAMIC,
|
|
eLINK,
|
|
};
|
|
Type mType;
|
|
PxU32 mCollisionGroup;
|
|
MassProps mMassProps;
|
|
PxVec3 mLinearVelocity;
|
|
PxVec3 mAngularVelocity;
|
|
Dy::ArticulationLinkHandle mLink;
|
|
};
|
|
Array<ImmediateActor> mActors;
|
|
#if USE_TGS
|
|
Array<PxTGSSolverBodyData> mSolverBodyData;
|
|
Array<PxTGSSolverBodyVel> mSolverBodies;
|
|
Array<PxTGSSolverBodyTxInertia> mSolverBodyTxInertias;
|
|
#else
|
|
Array<PxSolverBodyData> mSolverBodyData;
|
|
Array<PxSolverBody> mSolverBodies;
|
|
#endif
|
|
Array<Dy::ArticulationV*> mArticulations;
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
Array<MyJointData> mJointData;
|
|
#endif
|
|
Array<IDS> mBroadphasePairs;
|
|
HashSet<IDS> mFilteredPairs;
|
|
|
|
struct ContactPair
|
|
{
|
|
PxU32 mID0;
|
|
PxU32 mID1;
|
|
|
|
PxU32 mNbContacts;
|
|
PxU32 mStartContactIndex;
|
|
};
|
|
// PT: we use separate arrays here because the immediate mode API expects an array of Gu::ContactPoint
|
|
Array<ContactPair> mContactPairs;
|
|
Array<Gu::ContactPoint> mContactPoints;
|
|
#if WITH_PERSISTENCY
|
|
HashMap<IDS, PersistentContactPair> mPersistentPairs;
|
|
#endif
|
|
Array<PxSolverConstraintDesc> mSolverConstraintDesc;
|
|
#if BATCH_CONTACTS
|
|
Array<PxSolverConstraintDesc> mOrderedSolverConstraintDesc;
|
|
#endif
|
|
Array<PxConstraintBatchHeader> mHeaders;
|
|
Array<PxReal> mContactForces;
|
|
|
|
Array<PxVec3> mMotionLinearVelocity; // Persistent to avoid runtime allocations but could be managed on the stack
|
|
Array<PxVec3> mMotionAngularVelocity; // Persistent to avoid runtime allocations but could be managed on the stack
|
|
|
|
PxU32 mNbStaticActors;
|
|
PxU32 mNbArticulationLinks;
|
|
|
|
PX_FORCE_INLINE void disableCollision(PxU32 i, PxU32 j)
|
|
{
|
|
if(i>j)
|
|
swap(i, j);
|
|
mFilteredPairs.insert(IDS(i, j));
|
|
}
|
|
|
|
PX_FORCE_INLINE bool isCollisionDisabled(PxU32 i, PxU32 j) const
|
|
{
|
|
if(i>j)
|
|
swap(i, j);
|
|
return mFilteredPairs.contains(IDS(i, j));
|
|
}
|
|
|
|
Dy::ArticulationLinkHandle mMotorLink;
|
|
};
|
|
|
|
ImmediateScene::ImmediateScene() :
|
|
mNbStaticActors (0),
|
|
mNbArticulationLinks(0),
|
|
mMotorLink (0)
|
|
{
|
|
mCacheAllocator = new TestCacheAllocator;
|
|
mConstraintAllocator = new TestConstraintAllocator;
|
|
}
|
|
|
|
ImmediateScene::~ImmediateScene()
|
|
{
|
|
reset();
|
|
|
|
PX_DELETE_AND_RESET(mConstraintAllocator);
|
|
PX_DELETE_AND_RESET(mCacheAllocator);
|
|
}
|
|
|
|
void ImmediateScene::reset()
|
|
{
|
|
mGeoms.clear();
|
|
mPoses.clear();
|
|
mBounds.clear();
|
|
mActors.clear();
|
|
mSolverBodyData.clear();
|
|
mSolverBodies.clear();
|
|
#if USE_TGS
|
|
mSolverBodyTxInertias.clear();
|
|
#endif
|
|
mBroadphasePairs.clear();
|
|
mFilteredPairs.clear();
|
|
mContactPairs.clear();
|
|
mContactPoints.clear();
|
|
mSolverConstraintDesc.clear();
|
|
#if BATCH_CONTACTS
|
|
mOrderedSolverConstraintDesc.clear();
|
|
#endif
|
|
mHeaders.clear();
|
|
mContactForces.clear();
|
|
mMotionLinearVelocity.clear();
|
|
mMotionAngularVelocity.clear();
|
|
|
|
const PxU32 size = mArticulations.size();
|
|
for(PxU32 i=0;i<size;i++)
|
|
PxReleaseArticulation(mArticulations[i]);
|
|
|
|
mArticulations.clear();
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
mJointData.clear();
|
|
#endif
|
|
#if WITH_PERSISTENCY
|
|
mPersistentPairs.clear();
|
|
#endif
|
|
mNbStaticActors = mNbArticulationLinks = 0;
|
|
mMotorLink = 0;
|
|
}
|
|
|
|
PxU32 ImmediateScene::createActor(const PxGeometry& geometry, const PxTransform& pose, const MassProps* massProps, Dy::ArticulationLinkHandle link)
|
|
{
|
|
const PxU32 id = mActors.size();
|
|
// PT: we don't support compounds in this simple snippet. 1 actor = 1 shape/geom.
|
|
PX_ASSERT(mGeoms.size()==id);
|
|
PX_ASSERT(mPoses.size()==id);
|
|
PX_ASSERT(mBounds.size()==id);
|
|
|
|
const bool isStaticActor = !massProps;
|
|
if(isStaticActor)
|
|
{
|
|
PX_ASSERT(!link);
|
|
mNbStaticActors++;
|
|
}
|
|
else
|
|
{
|
|
// PT: make sure we don't create dynamic actors after static ones. We could reorganize the array but
|
|
// in this simple snippet we just enforce the order in which actors are created.
|
|
PX_ASSERT(!mNbStaticActors);
|
|
if(link)
|
|
mNbArticulationLinks++;
|
|
}
|
|
|
|
ImmediateActor actor;
|
|
if(isStaticActor)
|
|
actor.mType = ImmediateActor::eSTATIC;
|
|
else if(link)
|
|
actor.mType = ImmediateActor::eLINK;
|
|
else
|
|
actor.mType = ImmediateActor::eDYNAMIC;
|
|
actor.mCollisionGroup = 0;
|
|
actor.mLinearVelocity = PxVec3(0.0f);
|
|
actor.mAngularVelocity = PxVec3(0.0f);
|
|
actor.mLink = link;
|
|
|
|
if(massProps)
|
|
actor.mMassProps = *massProps;
|
|
else
|
|
{
|
|
actor.mMassProps.mInvMass = 0.0f;
|
|
actor.mMassProps.mInvInertia = PxVec3(0.0f);
|
|
}
|
|
|
|
mActors.pushBack(actor);
|
|
|
|
#if USE_TGS
|
|
mSolverBodyData.pushBack(PxTGSSolverBodyData());
|
|
mSolverBodies.pushBack(PxTGSSolverBodyVel());
|
|
mSolverBodyTxInertias.pushBack(PxTGSSolverBodyTxInertia());
|
|
#else
|
|
mSolverBodyData.pushBack(PxSolverBodyData());
|
|
mSolverBodies.pushBack(PxSolverBody());
|
|
#endif
|
|
|
|
mGeoms.pushBack(geometry);
|
|
mPoses.pushBack(pose);
|
|
mBounds.pushBack(PxBounds3());
|
|
|
|
return id;
|
|
}
|
|
|
|
static Dy::ArticulationV* createImmediateArticulation(bool fixBase, Array<Dy::ArticulationV*>& articulations)
|
|
{
|
|
PxFeatherstoneArticulationData data;
|
|
data.flags = fixBase ? PxArticulationFlag::eFIX_BASE : PxArticulationFlag::Enum(0);
|
|
Dy::ArticulationV* immArt = PxCreateFeatherstoneArticulation(data);
|
|
articulations.pushBack(immArt);
|
|
return immArt;
|
|
}
|
|
|
|
static void setupCommonLinkData(PxFeatherstoneArticulationLinkData& data, Dy::ArticulationLinkHandle parent, const PxTransform& pose, const MassProps& massProps)
|
|
{
|
|
data.parent = parent;
|
|
data.pose = pose;
|
|
data.inverseMass = massProps.mInvMass;
|
|
data.inverseInertia = massProps.mInvInertia;
|
|
data.linearDamping = gLinearDamping;
|
|
data.angularDamping = gAngularDamping;
|
|
data.maxLinearVelocitySq = gMaxLinearVelocity * gMaxLinearVelocity;
|
|
data.maxAngularVelocitySq = gMaxAngularVelocity * gMaxAngularVelocity;
|
|
data.inboundJoint.frictionCoefficient = gJointFrictionCoefficient;
|
|
}
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
void ImmediateScene::createSphericalJoint(PxU32 id0, PxU32 id1, const PxTransform& localFrame0, const PxTransform& localFrame1, const PxTransform* pose0, const PxTransform* pose1)
|
|
{
|
|
const bool isStatic0 = mActors[id0].mType == ImmediateActor::eSTATIC;
|
|
const bool isStatic1 = mActors[id1].mType == ImmediateActor::eSTATIC;
|
|
|
|
MyJointData jointData;
|
|
jointData.mActors[0] = id0;
|
|
jointData.mActors[1] = id1;
|
|
jointData.mLocalFrames[0] = localFrame0;
|
|
jointData.mLocalFrames[1] = localFrame1;
|
|
if(isStatic0)
|
|
jointData.c2b[0] = pose0->getInverse().transformInv(localFrame0);
|
|
else
|
|
jointData.c2b[0] = localFrame0;
|
|
if(isStatic1)
|
|
jointData.c2b[1] = pose1->getInverse().transformInv(localFrame1);
|
|
else
|
|
jointData.c2b[1] = localFrame1;
|
|
|
|
jointData.initInvMassScale();
|
|
|
|
mJointData.pushBack(jointData);
|
|
disableCollision(id0, id1);
|
|
}
|
|
#endif
|
|
|
|
void ImmediateScene::createScene()
|
|
{
|
|
mMotorLink = 0;
|
|
|
|
const PxU32 index = gSceneIndex;
|
|
if(index==0)
|
|
{
|
|
// Box stack
|
|
{
|
|
const PxVec3 extents(0.5f, 0.5f, 0.5f);
|
|
const PxBoxGeometry boxGeom(extents);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
// for(PxU32 i=0;i<8;i++)
|
|
// createBox(extents, PxTransform(PxVec3(0.0f, extents.y + float(i)*extents.y*2.0f, 0.0f)), 1.0f);
|
|
|
|
PxU32 size = 8;
|
|
// PxU32 size = 2;
|
|
// PxU32 size = 1;
|
|
float y = extents.y;
|
|
float x = 0.0f;
|
|
while(size)
|
|
{
|
|
for(PxU32 i=0;i<size;i++)
|
|
createActor(boxGeom, PxTransform(PxVec3(x+float(i)*extents.x*2.0f, y, 0.0f)), &massProps);
|
|
|
|
x += extents.x;
|
|
y += extents.y*2.0f;
|
|
size--;
|
|
}
|
|
}
|
|
|
|
createGroundPlane();
|
|
}
|
|
else if(index==1)
|
|
{
|
|
// Simple scene with regular spherical joint
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
const float boxSize = 1.0f;
|
|
const PxVec3 extents(boxSize, boxSize, boxSize);
|
|
const PxBoxGeometry boxGeom(extents);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxVec3 staticPos(0.0f, 6.0f, 0.0f);
|
|
const PxVec3 dynamicPos = staticPos - extents*2.0f;
|
|
|
|
const PxTransform dynPose(dynamicPos);
|
|
const PxTransform staticPose(staticPos);
|
|
|
|
const PxU32 dynamicObject = createActor(boxGeom, dynPose, &massProps);
|
|
const PxU32 staticObject = createActor(boxGeom, staticPose);
|
|
|
|
createSphericalJoint(staticObject, dynamicObject, PxTransform(-extents), PxTransform(extents), &staticPose, &dynPose);
|
|
#endif
|
|
}
|
|
else if(index==2)
|
|
{
|
|
// RC articulation with contacts
|
|
|
|
if(1)
|
|
{
|
|
const PxBoxGeometry boxGeom(PxVec3(1.0f));
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 0.5f);
|
|
|
|
createActor(boxGeom, PxTransform(PxVec3(0.0f, 1.0f, 0.0f)), &massProps);
|
|
}
|
|
|
|
const PxU32 nbLinks = 6;
|
|
const float Altitude = 6.0f;
|
|
const PxTransform basePose(PxVec3(0.f, Altitude, 0.f));
|
|
const PxVec3 boxExtents(0.5f, 0.1f, 0.5f);
|
|
const PxBoxGeometry boxGeom(boxExtents);
|
|
const float s = boxExtents.x*1.1f;
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
Dy::ArticulationV* immArt = createImmediateArticulation(true, mArticulations);
|
|
|
|
Dy::ArticulationLinkHandle base;
|
|
PxU32 baseID;
|
|
{
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, 0, basePose, massProps);
|
|
base = PxAddArticulationLink(immArt, linkData);
|
|
baseID = createActor(boxGeom, basePose, &massProps, base);
|
|
}
|
|
|
|
Dy::ArticulationLinkHandle parent = base;
|
|
PxU32 parentID = baseID;
|
|
PxTransform linkPose = basePose;
|
|
for(PxU32 i=0;i<nbLinks;i++)
|
|
{
|
|
linkPose.p.z += s*2.0f;
|
|
|
|
Dy::ArticulationLinkHandle link;
|
|
PxU32 linkID;
|
|
{
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, parent, linkPose, massProps);
|
|
//
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(PxVec3(0.0f, 0.0f, s));
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.0f, 0.0f, -s));
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eFREE;
|
|
|
|
link = PxAddArticulationLink(immArt, linkData, i==nbLinks-1);
|
|
linkID = createActor(boxGeom, linkPose, &massProps, link);
|
|
|
|
disableCollision(parentID, linkID);
|
|
}
|
|
|
|
parent = link;
|
|
parentID = linkID;
|
|
}
|
|
|
|
createGroundPlane();
|
|
}
|
|
else if(index==3)
|
|
{
|
|
// RC articulation with limits
|
|
|
|
const PxU32 nbLinks = 4;
|
|
const float Altitude = 6.0f;
|
|
const PxTransform basePose(PxVec3(0.f, Altitude, 0.f));
|
|
const PxVec3 boxExtents(0.5f, 0.1f, 0.5f);
|
|
const PxBoxGeometry boxGeom(boxExtents);
|
|
const float s = boxExtents.x*1.1f;
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
Dy::ArticulationV* immArt = createImmediateArticulation(true, mArticulations);
|
|
|
|
Dy::ArticulationLinkHandle base;
|
|
PxU32 baseID;
|
|
{
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, 0, basePose, massProps);
|
|
base = PxAddArticulationLink(immArt, linkData);
|
|
baseID = createActor(boxGeom, basePose, &massProps, base);
|
|
}
|
|
|
|
Dy::ArticulationLinkHandle parent = base;
|
|
PxU32 parentID = baseID;
|
|
PxTransform linkPose = basePose;
|
|
for(PxU32 i=0;i<nbLinks;i++)
|
|
{
|
|
linkPose.p.z += s*2.0f;
|
|
|
|
Dy::ArticulationLinkHandle link;
|
|
PxU32 linkID;
|
|
{
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, parent, linkPose, massProps);
|
|
//
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(PxVec3(0.0f, 0.0f, s));
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.0f, 0.0f, -s));
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eLIMITED;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].low = -PxPi/8.0f;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].high = PxPi/8.0f;
|
|
|
|
link = PxAddArticulationLink(immArt, linkData, i==nbLinks-1);
|
|
linkID = createActor(boxGeom, linkPose, &massProps, link);
|
|
|
|
disableCollision(parentID, linkID);
|
|
}
|
|
|
|
parent = link;
|
|
parentID = linkID;
|
|
}
|
|
}
|
|
else if(index==4)
|
|
{
|
|
if(0)
|
|
{
|
|
const float Altitude = 6.0f;
|
|
const PxTransform basePose(PxVec3(0.f, Altitude, 0.f));
|
|
const PxVec3 boxExtents(0.5f, 0.1f, 0.5f);
|
|
const PxBoxGeometry boxGeom(boxExtents);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
Dy::ArticulationV* immArt = createImmediateArticulation(false, mArticulations);
|
|
|
|
Dy::ArticulationLinkHandle base;
|
|
PxU32 baseID;
|
|
{
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, 0, basePose, massProps);
|
|
|
|
base = PxAddArticulationLink(immArt, linkData, true);
|
|
baseID = createActor(boxGeom, basePose, &massProps, base);
|
|
PX_UNUSED(baseID);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
// RC articulation with drive
|
|
|
|
const PxU32 nbLinks = 1;
|
|
const float Altitude = 6.0f;
|
|
const PxTransform basePose(PxVec3(0.f, Altitude, 0.f));
|
|
const PxVec3 boxExtents(0.5f, 0.1f, 0.5f);
|
|
const PxBoxGeometry boxGeom(boxExtents);
|
|
const float s = boxExtents.x*1.1f;
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
Dy::ArticulationV* immArt = createImmediateArticulation(true, mArticulations);
|
|
|
|
Dy::ArticulationLinkHandle base;
|
|
PxU32 baseID;
|
|
{
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, 0, basePose, massProps);
|
|
base = PxAddArticulationLink(immArt, linkData);
|
|
baseID = createActor(boxGeom, basePose, &massProps, base);
|
|
}
|
|
|
|
Dy::ArticulationLinkHandle parent = base;
|
|
PxU32 parentID = baseID;
|
|
PxTransform linkPose = basePose;
|
|
for(PxU32 i=0;i<nbLinks;i++)
|
|
{
|
|
linkPose.p.z += s*2.0f;
|
|
|
|
Dy::ArticulationLinkHandle link;
|
|
PxU32 linkID;
|
|
{
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, parent, linkPose, massProps);
|
|
//
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(PxVec3(0.0f, 0.0f, s));
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.0f, 0.0f, -s));
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eFREE;
|
|
linkData.inboundJoint.drives[PxArticulationAxis::eTWIST].stiffness = 0.0f;
|
|
linkData.inboundJoint.drives[PxArticulationAxis::eTWIST].damping = 1000.0f;
|
|
linkData.inboundJoint.drives[PxArticulationAxis::eTWIST].maxForce = FLT_MAX;
|
|
linkData.inboundJoint.drives[PxArticulationAxis::eTWIST].driveType = PxArticulationDriveType::eFORCE;
|
|
linkData.inboundJoint.targetVel[PxArticulationAxis::eTWIST] = 4.0f;
|
|
|
|
link = PxAddArticulationLink(immArt, linkData, i==nbLinks-1);
|
|
linkID = createActor(boxGeom, linkPose, &massProps, link);
|
|
|
|
disableCollision(parentID, linkID);
|
|
|
|
mMotorLink = link;
|
|
}
|
|
|
|
parent = link;
|
|
parentID = linkID;
|
|
}
|
|
}
|
|
else if(index==5)
|
|
{
|
|
// Scissor lift
|
|
const PxReal runnerLength = 2.f;
|
|
const PxReal placementDistance = 1.8f;
|
|
|
|
const PxReal cosAng = (placementDistance) / (runnerLength);
|
|
|
|
const PxReal angle = PxAcos(cosAng);
|
|
const PxReal sinAng = PxSin(angle);
|
|
|
|
const PxQuat leftRot(-angle, PxVec3(1.f, 0.f, 0.f));
|
|
const PxQuat rightRot(angle, PxVec3(1.f, 0.f, 0.f));
|
|
|
|
Dy::ArticulationV* immArt = createImmediateArticulation(false, mArticulations);
|
|
|
|
//
|
|
|
|
Dy::ArticulationLinkHandle base;
|
|
PxU32 baseID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.5f, 0.25f, 1.5f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 3.0f);
|
|
|
|
const PxTransform pose(PxVec3(0.f, 0.25f, 0.f));
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, 0, pose, massProps);
|
|
base = PxAddArticulationLink(immArt, linkData);
|
|
baseID = createActor(boxGeom, pose, &massProps, base);
|
|
}
|
|
|
|
//
|
|
|
|
Dy::ArticulationLinkHandle leftRoot;
|
|
PxU32 leftRootID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.5f, 0.05f, 0.05f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(PxVec3(0.f, 0.55f, -0.9f));
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, base, pose, massProps);
|
|
|
|
linkData.inboundJoint.type = PxArticulationJointType::eFIX;
|
|
linkData.inboundJoint.parentPose = PxTransform(PxVec3(0.f, 0.25f, -0.9f));
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.f, -0.05f, 0.f));
|
|
|
|
leftRoot = PxAddArticulationLink(immArt, linkData);
|
|
leftRootID = createActor(boxGeom, pose, &massProps, leftRoot);
|
|
disableCollision(baseID, leftRootID);
|
|
}
|
|
|
|
//
|
|
|
|
Dy::ArticulationLinkHandle rightRoot;
|
|
PxU32 rightRootID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.5f, 0.05f, 0.05f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(PxVec3(0.f, 0.55f, 0.9f));
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, base, pose, massProps);
|
|
|
|
linkData.inboundJoint.type = PxArticulationJointType::ePRISMATIC;
|
|
linkData.inboundJoint.parentPose = PxTransform(PxVec3(0.f, 0.25f, 0.9f));
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.f, -0.05f, 0.f));
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eZ] = PxArticulationMotion::eLIMITED;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eZ].low = -1.4f;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eZ].high = 0.2f;
|
|
if(0)
|
|
{
|
|
linkData.inboundJoint.drives[PxArticulationAxis::eZ].stiffness = 100000.f;
|
|
linkData.inboundJoint.drives[PxArticulationAxis::eZ].damping = 0.f;
|
|
linkData.inboundJoint.drives[PxArticulationAxis::eZ].maxForce = PX_MAX_F32;
|
|
linkData.inboundJoint.drives[PxArticulationAxis::eZ].driveType = PxArticulationDriveType::eFORCE;
|
|
}
|
|
|
|
rightRoot = PxAddArticulationLink(immArt, linkData);
|
|
rightRootID = createActor(boxGeom, pose, &massProps, rightRoot);
|
|
disableCollision(baseID, rightRootID);
|
|
}
|
|
|
|
//
|
|
|
|
const PxU32 linkHeight = 3;
|
|
PxU32 currLeftID = leftRootID;
|
|
PxU32 currRightID = rightRootID;
|
|
Dy::ArticulationLinkHandle currLeft = leftRoot;
|
|
Dy::ArticulationLinkHandle currRight = rightRoot;
|
|
PxQuat rightParentRot(PxIdentity);
|
|
PxQuat leftParentRot(PxIdentity);
|
|
for(PxU32 i=0; i<linkHeight; ++i)
|
|
{
|
|
const PxVec3 pos(0.5f, 0.55f + 0.1f*(1 + i), 0.f);
|
|
|
|
Dy::ArticulationLinkHandle leftLink;
|
|
PxU32 leftLinkID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.05f, 0.05f, 1.f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(pos + PxVec3(0.f, sinAng*(2 * i + 1), 0.f), leftRot);
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, currLeft, pose, massProps);
|
|
|
|
const PxVec3 leftAnchorLocation = pos + PxVec3(0.f, sinAng*(2 * i), -0.9f);
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(mPoses[currLeftID].transformInv(leftAnchorLocation), leftParentRot);
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.f, 0.f, -1.f), rightRot);
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eLIMITED;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].low = -PxPi;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].high = angle;
|
|
|
|
leftLink = PxAddArticulationLink(immArt, linkData);
|
|
leftLinkID = createActor(boxGeom, pose, &massProps, leftLink);
|
|
disableCollision(currLeftID, leftLinkID);
|
|
mActors[leftLinkID].mCollisionGroup = 1;
|
|
}
|
|
leftParentRot = leftRot;
|
|
|
|
//
|
|
|
|
Dy::ArticulationLinkHandle rightLink;
|
|
PxU32 rightLinkID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.05f, 0.05f, 1.f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(pos + PxVec3(0.f, sinAng*(2 * i + 1), 0.f), rightRot);
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, currRight, pose, massProps);
|
|
|
|
const PxVec3 rightAnchorLocation = pos + PxVec3(0.f, sinAng*(2 * i), 0.9f);
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(mPoses[currRightID].transformInv(rightAnchorLocation), rightParentRot);
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.f, 0.f, 1.f), leftRot);
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eLIMITED;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].low = -angle;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].high = PxPi;
|
|
|
|
rightLink = PxAddArticulationLink(immArt, linkData);
|
|
rightLinkID = createActor(boxGeom, pose, &massProps, rightLink);
|
|
disableCollision(currRightID, rightLinkID);
|
|
mActors[rightLinkID].mCollisionGroup = 1;
|
|
}
|
|
rightParentRot = rightRot;
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
createSphericalJoint(leftLinkID, rightLinkID, PxTransform(PxIdentity), PxTransform(PxIdentity));
|
|
#else
|
|
disableCollision(leftLinkID, rightLinkID);
|
|
#endif
|
|
currLeftID = rightLinkID;
|
|
currRightID = leftLinkID;
|
|
currLeft = rightLink;
|
|
currRight = leftLink;
|
|
}
|
|
|
|
//
|
|
|
|
Dy::ArticulationLinkHandle leftTop;
|
|
PxU32 leftTopID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.5f, 0.05f, 0.05f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(mPoses[currLeftID].transform(PxTransform(PxVec3(-0.5f, 0.f, -1.0f), leftParentRot)));
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, currLeft, pose, massProps);
|
|
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(PxVec3(0.f, 0.f, -1.f), mPoses[currLeftID].q.getConjugate());
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.5f, 0.f, 0.f), pose.q.getConjugate());
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eFREE;
|
|
|
|
leftTop = PxAddArticulationLink(immArt, linkData);
|
|
leftTopID = createActor(boxGeom, pose, &massProps, leftTop);
|
|
disableCollision(currLeftID, leftTopID);
|
|
mActors[leftTopID].mCollisionGroup = 1;
|
|
}
|
|
|
|
//
|
|
|
|
Dy::ArticulationLinkHandle rightTop;
|
|
PxU32 rightTopID;
|
|
{
|
|
// TODO: use a capsule here
|
|
// PxRigidActorExt::createExclusiveShape(*rightTop, PxCapsuleGeometry(0.05f, 0.8f), *gMaterial);
|
|
const PxBoxGeometry boxGeom(0.5f, 0.05f, 0.05f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(mPoses[currRightID].transform(PxTransform(PxVec3(-0.5f, 0.f, 1.0f), rightParentRot)));
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, currRight, pose, massProps);
|
|
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(PxVec3(0.f, 0.f, 1.f), mPoses[currRightID].q.getConjugate());
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.5f, 0.f, 0.f), pose.q.getConjugate());
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eFREE;
|
|
|
|
rightTop = PxAddArticulationLink(immArt, linkData);
|
|
rightTopID = createActor(boxGeom, pose, &massProps, rightTop);
|
|
disableCollision(currRightID, rightTopID);
|
|
mActors[rightTopID].mCollisionGroup = 1;
|
|
}
|
|
|
|
//
|
|
|
|
currLeftID = leftRootID;
|
|
currRightID = rightRootID;
|
|
currLeft = leftRoot;
|
|
currRight = rightRoot;
|
|
rightParentRot = PxQuat(PxIdentity);
|
|
leftParentRot = PxQuat(PxIdentity);
|
|
|
|
for(PxU32 i=0; i<linkHeight; ++i)
|
|
{
|
|
const PxVec3 pos(-0.5f, 0.55f + 0.1f*(1 + i), 0.f);
|
|
|
|
Dy::ArticulationLinkHandle leftLink;
|
|
PxU32 leftLinkID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.05f, 0.05f, 1.f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(pos + PxVec3(0.f, sinAng*(2 * i + 1), 0.f), leftRot);
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, currLeft, pose, massProps);
|
|
|
|
const PxVec3 leftAnchorLocation = pos + PxVec3(0.f, sinAng*(2 * i), -0.9f);
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(mPoses[currLeftID].transformInv(leftAnchorLocation), leftParentRot);
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.f, 0.f, -1.f), rightRot);
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eLIMITED;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].low = -PxPi;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].high = angle;
|
|
|
|
leftLink = PxAddArticulationLink(immArt, linkData);
|
|
leftLinkID = createActor(boxGeom, pose, &massProps, leftLink);
|
|
disableCollision(currLeftID, leftLinkID);
|
|
mActors[leftLinkID].mCollisionGroup = 1;
|
|
}
|
|
leftParentRot = leftRot;
|
|
|
|
//
|
|
|
|
Dy::ArticulationLinkHandle rightLink;
|
|
PxU32 rightLinkID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.05f, 0.05f, 1.f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(pos + PxVec3(0.f, sinAng*(2 * i + 1), 0.f), rightRot);
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, currRight, pose, massProps);
|
|
|
|
const PxVec3 rightAnchorLocation = pos + PxVec3(0.f, sinAng*(2 * i), 0.9f);
|
|
linkData.inboundJoint.type = PxArticulationJointType::eREVOLUTE;
|
|
linkData.inboundJoint.parentPose = PxTransform(mPoses[currRightID].transformInv(rightAnchorLocation), rightParentRot);
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.f, 0.f, 1.f), leftRot);
|
|
linkData.inboundJoint.motion[PxArticulationAxis::eTWIST] = PxArticulationMotion::eLIMITED;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].low = -angle;
|
|
linkData.inboundJoint.limits[PxArticulationAxis::eTWIST].high = PxPi;
|
|
|
|
rightLink = PxAddArticulationLink(immArt, linkData);
|
|
rightLinkID = createActor(boxGeom, pose, &massProps, rightLink);
|
|
disableCollision(currRightID, rightLinkID);
|
|
mActors[rightLinkID].mCollisionGroup = 1;
|
|
}
|
|
rightParentRot = rightRot;
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
createSphericalJoint(leftLinkID, rightLinkID, PxTransform(PxIdentity), PxTransform(PxIdentity));
|
|
#else
|
|
disableCollision(leftLinkID, rightLinkID);
|
|
#endif
|
|
currLeftID = rightLinkID;
|
|
currRightID = leftLinkID;
|
|
currLeft = rightLink;
|
|
currRight = leftLink;
|
|
}
|
|
|
|
//
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
createSphericalJoint(currLeftID, leftTopID, PxTransform(PxVec3(0.f, 0.f, -1.f)), PxTransform(PxVec3(-0.5f, 0.f, 0.f)));
|
|
createSphericalJoint(currRightID, rightTopID, PxTransform(PxVec3(0.f, 0.f, 1.f)), PxTransform(PxVec3(-0.5f, 0.f, 0.f)));
|
|
#else
|
|
disableCollision(currLeftID, leftTopID);
|
|
disableCollision(currRightID, rightTopID);
|
|
#endif
|
|
//
|
|
|
|
// Create top
|
|
{
|
|
Dy::ArticulationLinkHandle top;
|
|
PxU32 topID;
|
|
{
|
|
const PxBoxGeometry boxGeom(0.5f, 0.1f, 1.5f);
|
|
|
|
MassProps massProps;
|
|
computeMassProps(massProps, boxGeom, 1.0f);
|
|
|
|
const PxTransform pose(PxVec3(0.f, mPoses[leftTopID].p.y + 0.15f, 0.f));
|
|
PxFeatherstoneArticulationLinkData linkData;
|
|
setupCommonLinkData(linkData, leftTop, pose, massProps);
|
|
|
|
linkData.inboundJoint.type = PxArticulationJointType::eFIX;
|
|
linkData.inboundJoint.parentPose = PxTransform(PxVec3(0.f, 0.0f, 0.f));
|
|
linkData.inboundJoint.childPose = PxTransform(PxVec3(0.f, -0.15f, -0.9f));
|
|
|
|
top = PxAddArticulationLink(immArt, linkData, true);
|
|
topID = createActor(boxGeom, pose, &massProps, top);
|
|
disableCollision(leftTopID, topID);
|
|
}
|
|
}
|
|
|
|
//
|
|
|
|
createGroundPlane();
|
|
}
|
|
}
|
|
|
|
void ImmediateScene::updateArticulations(float dt)
|
|
{
|
|
#if USE_TGS
|
|
const float stepDt = dt/gNbIterPos;
|
|
const float invTotalDt = 1.0f/dt;
|
|
const float stepInvDt = 1.0f/stepDt;
|
|
#endif
|
|
|
|
const PxU32 nbArticulations = mArticulations.size();
|
|
for(PxU32 i=0;i<nbArticulations;i++)
|
|
{
|
|
Dy::ArticulationV* articulation = mArticulations[i];
|
|
#if USE_TGS
|
|
PxComputeUnconstrainedVelocitiesTGS(articulation, gGravity, stepDt, dt, stepInvDt, invTotalDt);
|
|
#else
|
|
PxComputeUnconstrainedVelocities(articulation, gGravity, dt);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ImmediateScene::updateBounds()
|
|
{
|
|
// PT: in this snippet we simply recompute all bounds each frame (i.e. even static ones)
|
|
const PxU32 nbActors = mActors.size();
|
|
for(PxU32 i=0;i<nbActors;i++)
|
|
{
|
|
const PxGeometry& currentGeom = mGeoms[i].any();
|
|
const PxTransform& currentPose = mPoses[i];
|
|
|
|
const PxVec3& center = currentPose.p;
|
|
PxVec3 extents;
|
|
|
|
switch(currentGeom.getType())
|
|
{
|
|
case PxGeometryType::ePLANE:
|
|
{
|
|
extents = PxVec3(1000000.0f);
|
|
}
|
|
break;
|
|
case PxGeometryType::eBOX:
|
|
{
|
|
const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(currentGeom);
|
|
extents = boxGeom.halfExtents;
|
|
}
|
|
break;
|
|
|
|
case PxGeometryType::eSPHERE:
|
|
case PxGeometryType::eCAPSULE:
|
|
case PxGeometryType::eCONVEXMESH:
|
|
case PxGeometryType::eTRIANGLEMESH:
|
|
case PxGeometryType::eHEIGHTFIELD:
|
|
case PxGeometryType::eGEOMETRY_COUNT:
|
|
case PxGeometryType::eINVALID:
|
|
PX_ASSERT(0);
|
|
break;
|
|
}
|
|
|
|
extents += PxVec3(gBoundsInflation);
|
|
|
|
mBounds[i] = PxBounds3::basisExtent(center, PxMat33(currentPose.q), extents);
|
|
}
|
|
}
|
|
|
|
void ImmediateScene::broadPhase()
|
|
{
|
|
// PT: in this snippet we simply do a brute-force O(n^2) broadphase between all actors
|
|
mBroadphasePairs.clear();
|
|
const PxU32 nbActors = mActors.size();
|
|
for(PxU32 i=0; i<nbActors; i++)
|
|
{
|
|
const ImmediateActor::Type type0 = mActors[i].mType;
|
|
|
|
for(PxU32 j=i+1; j<nbActors; j++)
|
|
{
|
|
const ImmediateActor::Type type1 = mActors[j].mType;
|
|
|
|
// Filtering
|
|
{
|
|
if(type0==ImmediateActor::eSTATIC && type1==ImmediateActor::eSTATIC)
|
|
continue;
|
|
|
|
if(mActors[i].mCollisionGroup==1 && mActors[j].mCollisionGroup==1)
|
|
continue;
|
|
|
|
if(isCollisionDisabled(i, j))
|
|
continue;
|
|
}
|
|
|
|
if(mBounds[i].intersects(mBounds[j]))
|
|
{
|
|
mBroadphasePairs.pushBack(IDS(i, j));
|
|
}
|
|
#if WITH_PERSISTENCY
|
|
else
|
|
{
|
|
const HashMap<IDS, PersistentContactPair>::Entry* e = mPersistentPairs.find(IDS(i, j));
|
|
if(e)
|
|
{
|
|
PersistentContactPair& persistentData = const_cast<PersistentContactPair&>(e->second);
|
|
//No collision detection performed at all so clear contact cache and friction data
|
|
persistentData.reset();
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
|
|
void ImmediateScene::narrowPhase()
|
|
{
|
|
class ContactRecorder : public PxContactRecorder
|
|
{
|
|
public:
|
|
ContactRecorder(ImmediateScene* scene, PxU32 id0, PxU32 id1) : mScene(scene), mID0(id0), mID1(id1), mHasContacts(false) {}
|
|
|
|
virtual bool recordContacts(const Gu::ContactPoint* contactPoints, const PxU32 nbContacts, const PxU32 /*index*/)
|
|
{
|
|
{
|
|
ImmediateScene::ContactPair pair;
|
|
pair.mID0 = mID0;
|
|
pair.mID1 = mID1;
|
|
pair.mNbContacts = nbContacts;
|
|
pair.mStartContactIndex = mScene->mContactPoints.size();
|
|
mScene->mContactPairs.pushBack(pair);
|
|
mHasContacts = true;
|
|
}
|
|
|
|
for(PxU32 i=0; i<nbContacts; i++)
|
|
{
|
|
// Fill in solver-specific data that our contact gen does not produce...
|
|
Gu::ContactPoint point = contactPoints[i];
|
|
point.maxImpulse = PX_MAX_F32;
|
|
point.targetVel = PxVec3(0.0f);
|
|
point.staticFriction = gStaticFriction;
|
|
point.dynamicFriction = gDynamicFriction;
|
|
point.restitution = gRestitution;
|
|
point.materialFlags = 0;
|
|
mScene->mContactPoints.pushBack(point);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ImmediateScene* mScene;
|
|
PxU32 mID0;
|
|
PxU32 mID1;
|
|
bool mHasContacts;
|
|
};
|
|
|
|
mCacheAllocator->reset();
|
|
mConstraintAllocator->release();
|
|
mContactPairs.resize(0);
|
|
mContactPoints.resize(0);
|
|
|
|
const PxU32 nbPairs = mBroadphasePairs.size();
|
|
for(PxU32 i=0;i<nbPairs;i++)
|
|
{
|
|
const IDS& pair = mBroadphasePairs[i];
|
|
|
|
const PxTransform& tr0 = mPoses[pair.mID0];
|
|
const PxTransform& tr1 = mPoses[pair.mID1];
|
|
|
|
const PxGeometry* pxGeom0 = &mGeoms[pair.mID0].any();
|
|
const PxGeometry* pxGeom1 = &mGeoms[pair.mID1].any();
|
|
|
|
ContactRecorder contactRecorder(this, pair.mID0, pair.mID1);
|
|
|
|
#if WITH_PERSISTENCY
|
|
PersistentContactPair& persistentData = mPersistentPairs[IDS(pair.mID0, pair.mID1)];
|
|
|
|
PxGenerateContacts(&pxGeom0, &pxGeom1, &tr0, &tr1, &persistentData.cache, 1, contactRecorder, gContactDistance, gMeshContactMargin, gToleranceLength, *mCacheAllocator);
|
|
if(!contactRecorder.mHasContacts)
|
|
{
|
|
//Contact generation run but no touches found so clear cached friction data
|
|
persistentData.frictions = NULL;
|
|
persistentData.nbFrictions = 0;
|
|
}
|
|
#else
|
|
PxCache cache;
|
|
PxGenerateContacts(&pxGeom0, &pxGeom1, &tr0, &tr1, &cache, 1, contactRecorder, gContactDistance, gMeshContactMargin, gToleranceLength, *mCacheAllocator);
|
|
#endif
|
|
}
|
|
|
|
if(1)
|
|
printf("Narrow-phase: %d contacts \n", mContactPoints.size());
|
|
}
|
|
|
|
void ImmediateScene::buildSolverBodyData(float dt)
|
|
{
|
|
const PxU32 nbActors = mActors.size();
|
|
for(PxU32 i=0;i<nbActors;i++)
|
|
{
|
|
if(mActors[i].mType==ImmediateActor::eSTATIC)
|
|
{
|
|
#if USE_TGS
|
|
PxConstructStaticSolverBodyTGS(mPoses[i], mSolverBodies[i], mSolverBodyTxInertias[i], mSolverBodyData[i]);
|
|
#else
|
|
PxConstructStaticSolverBody(mPoses[i], mSolverBodyData[i]);
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
PxRigidBodyData data;
|
|
data.linearVelocity = mActors[i].mLinearVelocity;
|
|
data.angularVelocity = mActors[i].mAngularVelocity;
|
|
data.invMass = mActors[i].mMassProps.mInvMass;
|
|
data.invInertia = mActors[i].mMassProps.mInvInertia;
|
|
data.body2World = mPoses[i];
|
|
data.maxDepenetrationVelocity = gMaxDepenetrationVelocity;
|
|
data.maxContactImpulse = gMaxContactImpulse;
|
|
data.linearDamping = gLinearDamping;
|
|
data.angularDamping = gAngularDamping;
|
|
data.maxLinearVelocitySq = gMaxLinearVelocity*gMaxLinearVelocity;
|
|
data.maxAngularVelocitySq = gMaxAngularVelocity*gMaxAngularVelocity;
|
|
#if USE_TGS
|
|
PxConstructSolverBodiesTGS(&data, &mSolverBodies[i], &mSolverBodyTxInertias[i], &mSolverBodyData[i], 1, gGravity, dt);
|
|
#else
|
|
PxConstructSolverBodies(&data, &mSolverBodyData[i], 1, gGravity, dt);
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
|
|
#if USE_TGS
|
|
static void setupDesc(PxSolverConstraintDesc& desc, const ImmediateScene::ImmediateActor* actors, PxTGSSolverBodyVel* solverBodies, PxU32 id, bool aorb)
|
|
#else
|
|
static void setupDesc(PxSolverConstraintDesc& desc, const ImmediateScene::ImmediateActor* actors, PxSolverBody* solverBodies, PxU32 id, bool aorb)
|
|
#endif
|
|
{
|
|
if(!aorb)
|
|
desc.bodyADataIndex = PxU16(id);
|
|
else
|
|
desc.bodyBDataIndex = PxU16(id);
|
|
|
|
Dy::ArticulationLinkHandle link = actors[id].mLink;
|
|
if(link)
|
|
{
|
|
if(!aorb)
|
|
{
|
|
desc.articulationA = PxGetLinkArticulation(link);
|
|
desc.linkIndexA = PxU16(PxGetLinkIndex(link));
|
|
}
|
|
else
|
|
{
|
|
desc.articulationB = PxGetLinkArticulation(link);
|
|
desc.linkIndexB = PxU16(PxGetLinkIndex(link));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(!aorb)
|
|
{
|
|
#if USE_TGS
|
|
desc.tgsBodyA = &solverBodies[id];
|
|
#else
|
|
desc.bodyA = &solverBodies[id];
|
|
#endif
|
|
desc.linkIndexA = PxSolverConstraintDesc::NO_LINK;
|
|
}
|
|
else
|
|
{
|
|
#if USE_TGS
|
|
desc.tgsBodyB = &solverBodies[id];
|
|
#else
|
|
desc.bodyB = &solverBodies[id];
|
|
#endif
|
|
desc.linkIndexB = PxSolverConstraintDesc::NO_LINK;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ImmediateScene::buildSolverConstraintDesc()
|
|
{
|
|
const PxU32 nbContactPairs = mContactPairs.size();
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
const PxU32 nbJoints = mJointData.size();
|
|
mSolverConstraintDesc.resize(nbContactPairs+nbJoints);
|
|
#else
|
|
mSolverConstraintDesc.resize(nbContactPairs);
|
|
#endif
|
|
|
|
for(PxU32 i=0; i<nbContactPairs; i++)
|
|
{
|
|
const ContactPair& pair = mContactPairs[i];
|
|
PxSolverConstraintDesc& desc = mSolverConstraintDesc[i];
|
|
|
|
setupDesc(desc, mActors.begin(), mSolverBodies.begin(), pair.mID0, false);
|
|
setupDesc(desc, mActors.begin(), mSolverBodies.begin(), pair.mID1, true);
|
|
|
|
//Cache pointer to our contact data structure and identify which type of constraint this is. We'll need this later after batching.
|
|
//If we choose not to perform batching and instead just create a single header per-pair, then this would not be necessary because
|
|
//the constraintDescs would not have been reordered
|
|
desc.constraint = reinterpret_cast<PxU8*>(const_cast<ContactPair*>(&pair));
|
|
desc.constraintLengthOver16 = PxSolverConstraintDesc::eCONTACT_CONSTRAINT;
|
|
}
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
for(PxU32 i=0; i<nbJoints; i++)
|
|
{
|
|
const MyJointData& jointData = mJointData[i];
|
|
PxSolverConstraintDesc& desc = mSolverConstraintDesc[nbContactPairs+i];
|
|
|
|
const PxU32 id0 = jointData.mActors[0];
|
|
const PxU32 id1 = jointData.mActors[1];
|
|
|
|
setupDesc(desc, mActors.begin(), mSolverBodies.begin(), id0, false);
|
|
setupDesc(desc, mActors.begin(), mSolverBodies.begin(), id1, true);
|
|
|
|
desc.constraint = reinterpret_cast<PxU8*>(const_cast<MyJointData*>(&jointData));
|
|
desc.constraintLengthOver16 = PxSolverConstraintDesc::eJOINT_CONSTRAINT;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
|
|
// PT: this is copied from PxExtensions, it's the solver prep function for spherical joints
|
|
static PxU32 SphericalJointSolverPrep(Px1DConstraint* constraints,
|
|
PxVec3& body0WorldOffset,
|
|
PxU32 /*maxConstraints*/,
|
|
PxConstraintInvMassScale& invMassScale,
|
|
const void* constantBlock,
|
|
const PxTransform& bA2w,
|
|
const PxTransform& bB2w,
|
|
bool /*useExtendedLimits*/,
|
|
PxVec3& cA2wOut, PxVec3& cB2wOut)
|
|
{
|
|
const MyJointData& data = *reinterpret_cast<const MyJointData*>(constantBlock);
|
|
|
|
PxTransform cA2w, cB2w;
|
|
Ext::joint::ConstraintHelper ch(constraints, invMassScale, cA2w, cB2w, body0WorldOffset, data, bA2w, bB2w);
|
|
|
|
if(cB2w.q.dot(cA2w.q)<0.0f)
|
|
cB2w.q = -cB2w.q;
|
|
|
|
/* if(data.jointFlags & PxSphericalJointFlag::eLIMIT_ENABLED)
|
|
{
|
|
PxQuat swing, twist;
|
|
Ps::separateSwingTwist(cA2w.q.getConjugate() * cB2w.q, swing, twist);
|
|
PX_ASSERT(PxAbs(swing.x)<1e-6f);
|
|
|
|
// PT: TODO: refactor with D6 joint code
|
|
PxVec3 axis;
|
|
PxReal error;
|
|
const PxReal pad = data.limit.isSoft() ? 0.0f : data.limit.contactDistance;
|
|
const Cm::ConeLimitHelperTanLess coneHelper(data.limit.yAngle, data.limit.zAngle, pad);
|
|
const bool active = coneHelper.getLimit(swing, axis, error);
|
|
if(active)
|
|
ch.angularLimit(cA2w.rotate(axis), error, data.limit);
|
|
}*/
|
|
|
|
PxVec3 ra, rb;
|
|
ch.prepareLockedAxes(cA2w.q, cB2w.q, cA2w.transformInv(cB2w.p), 7, 0, ra, rb);
|
|
cA2wOut = ra + bA2w.p;
|
|
cB2wOut = rb + bB2w.p;
|
|
|
|
return ch.getCount();
|
|
}
|
|
#endif
|
|
|
|
void setupDesc(PxSolverContactDesc& contactDesc, const ImmediateScene::ImmediateActor* actors, PxSolverBodyData* solverBodyData, const PxU32 id, const bool aorb)
|
|
{
|
|
PxTransform& bodyFrame = aorb ? contactDesc.bodyFrame1 : contactDesc.bodyFrame0;
|
|
PxSolverConstraintPrepDescBase::BodyState& bodyState = aorb ? contactDesc.bodyState1 : contactDesc.bodyState0;
|
|
const PxSolverBodyData*& data = aorb ? contactDesc.data1 : contactDesc.data0;
|
|
|
|
Dy::ArticulationLinkHandle link = actors[id].mLink;
|
|
if(link)
|
|
{
|
|
PxLinkData linkData;
|
|
bool status = PxGetLinkData(link, linkData);
|
|
PX_ASSERT(status);
|
|
PX_UNUSED(status);
|
|
|
|
data = NULL;
|
|
bodyFrame = linkData.pose;
|
|
bodyState = PxSolverConstraintPrepDescBase::eARTICULATION;
|
|
}
|
|
else
|
|
{
|
|
data = &solverBodyData[id];
|
|
bodyFrame = solverBodyData[id].body2World;
|
|
bodyState = actors[id].mType == ImmediateScene::ImmediateActor::eDYNAMIC ? PxSolverConstraintPrepDescBase::eDYNAMIC_BODY : PxSolverConstraintPrepDescBase::eSTATIC_BODY;
|
|
}
|
|
}
|
|
|
|
void setupDesc(PxTGSSolverContactDesc& contactDesc, const ImmediateScene::ImmediateActor* actors, PxTGSSolverBodyTxInertia* txInertias, PxTGSSolverBodyData* solverBodyData,
|
|
PxTransform* poses, const PxU32 id, const bool aorb)
|
|
{
|
|
PxTransform& bodyFrame = aorb ? contactDesc.bodyFrame1 : contactDesc.bodyFrame0;
|
|
PxSolverConstraintPrepDescBase::BodyState& bodyState = aorb ? contactDesc.bodyState1 : contactDesc.bodyState0;
|
|
const PxTGSSolverBodyData*& data = aorb ? contactDesc.bodyData1 : contactDesc.bodyData0;
|
|
const PxTGSSolverBodyTxInertia*& txI = aorb ? contactDesc.body1TxI : contactDesc.body0TxI;
|
|
|
|
Dy::ArticulationLinkHandle link = actors[id].mLink;
|
|
if(link)
|
|
{
|
|
PxLinkData linkData;
|
|
bool status = PxGetLinkData(link, linkData);
|
|
PX_ASSERT(status);
|
|
PX_UNUSED(status);
|
|
|
|
data = NULL;
|
|
txI = NULL;
|
|
bodyFrame = linkData.pose;
|
|
bodyState = PxSolverConstraintPrepDescBase::eARTICULATION;
|
|
}
|
|
else
|
|
{
|
|
data = &solverBodyData[id];
|
|
txI = &txInertias[id];
|
|
bodyFrame = poses[id];
|
|
bodyState = actors[id].mType == ImmediateScene::ImmediateActor::eDYNAMIC ? PxSolverConstraintPrepDescBase::eDYNAMIC_BODY : PxSolverConstraintPrepDescBase::eSTATIC_BODY;
|
|
}
|
|
}
|
|
|
|
void setupJointDesc(PxSolverConstraintPrepDesc& jointDesc, const ImmediateScene::ImmediateActor* actors, PxSolverBodyData* solverBodyData, const PxU32 bodyDataIndex, const bool aorb)
|
|
{
|
|
if(!aorb)
|
|
jointDesc.data0 = &solverBodyData[bodyDataIndex];
|
|
else
|
|
jointDesc.data1 = &solverBodyData[bodyDataIndex];
|
|
|
|
PxTransform& bodyFrame = aorb ? jointDesc.bodyFrame1 : jointDesc.bodyFrame0;
|
|
PxSolverConstraintPrepDescBase::BodyState& bodyState = aorb ? jointDesc.bodyState1 : jointDesc.bodyState0;
|
|
|
|
if(actors[bodyDataIndex].mLink)
|
|
{
|
|
PxLinkData linkData;
|
|
bool status = PxGetLinkData(actors[bodyDataIndex].mLink, linkData);
|
|
PX_ASSERT(status);
|
|
PX_UNUSED(status);
|
|
|
|
bodyFrame = linkData.pose;
|
|
bodyState = PxSolverConstraintPrepDescBase::eARTICULATION;
|
|
}
|
|
else
|
|
{
|
|
//This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations.
|
|
// PT: TODO: this is a bug in the immediate mode snippet
|
|
if(actors[bodyDataIndex].mType == ImmediateScene::ImmediateActor::eSTATIC)
|
|
{
|
|
bodyFrame = PxTransform(PxIdentity);
|
|
bodyState = PxSolverConstraintPrepDescBase::eSTATIC_BODY;
|
|
}
|
|
else
|
|
{
|
|
bodyFrame = solverBodyData[bodyDataIndex].body2World;
|
|
bodyState = PxSolverConstraintPrepDescBase::eDYNAMIC_BODY;
|
|
}
|
|
}
|
|
}
|
|
|
|
void setupJointDesc(PxTGSSolverConstraintPrepDesc& jointDesc, const ImmediateScene::ImmediateActor* actors, PxTGSSolverBodyTxInertia* txInertias, PxTGSSolverBodyData* solverBodyData, PxTransform* poses, const PxU32 bodyDataIndex, const bool aorb)
|
|
{
|
|
if(!aorb)
|
|
{
|
|
jointDesc.bodyData0 = &solverBodyData[bodyDataIndex];
|
|
jointDesc.body0TxI = &txInertias[bodyDataIndex];
|
|
}
|
|
else
|
|
{
|
|
jointDesc.bodyData1 = &solverBodyData[bodyDataIndex];
|
|
jointDesc.body1TxI = &txInertias[bodyDataIndex];
|
|
}
|
|
|
|
PxTransform& bodyFrame = aorb ? jointDesc.bodyFrame1 : jointDesc.bodyFrame0;
|
|
PxSolverConstraintPrepDescBase::BodyState& bodyState = aorb ? jointDesc.bodyState1 : jointDesc.bodyState0;
|
|
|
|
if(actors[bodyDataIndex].mLink)
|
|
{
|
|
PxLinkData linkData;
|
|
bool status = PxGetLinkData(actors[bodyDataIndex].mLink, linkData);
|
|
PX_ASSERT(status);
|
|
PX_UNUSED(status);
|
|
|
|
bodyFrame = linkData.pose;
|
|
bodyState = PxSolverConstraintPrepDescBase::eARTICULATION;
|
|
}
|
|
else
|
|
{
|
|
//This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations.
|
|
// PT: TODO: this is a bug in the immediate mode snippet
|
|
if(actors[bodyDataIndex].mType == ImmediateScene::ImmediateActor::eSTATIC)
|
|
{
|
|
bodyFrame = PxTransform(PxIdentity);
|
|
bodyState = PxSolverConstraintPrepDescBase::eSTATIC_BODY;
|
|
}
|
|
else
|
|
{
|
|
bodyFrame = poses[bodyDataIndex];
|
|
bodyState = PxSolverConstraintPrepDescBase::eDYNAMIC_BODY;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ImmediateScene::createContactConstraints(float dt, float invDt, float lengthScale, const PxU32 nbPosIterations)
|
|
{
|
|
//Only referenced if using TGS solver
|
|
PX_UNUSED(lengthScale);
|
|
PX_UNUSED(nbPosIterations);
|
|
#if USE_TGS
|
|
const float stepDt = dt/float(nbPosIterations);
|
|
const float stepInvDt = invDt*float(nbPosIterations);
|
|
#endif
|
|
#if BATCH_CONTACTS
|
|
mHeaders.resize(mSolverConstraintDesc.size());
|
|
|
|
const PxU32 nbBodies = mActors.size() - mNbStaticActors;
|
|
|
|
mOrderedSolverConstraintDesc.resize(mSolverConstraintDesc.size());
|
|
Array<PxSolverConstraintDesc>& orderedDescs = mOrderedSolverConstraintDesc;
|
|
|
|
#if USE_TGS
|
|
const PxU32 nbContactHeaders = physx::immediate::PxBatchConstraintsTGS( mSolverConstraintDesc.begin(), mContactPairs.size(), mSolverBodies.begin(), nbBodies,
|
|
mHeaders.begin(), orderedDescs.begin(),
|
|
mArticulations.begin(), mArticulations.size());
|
|
|
|
//2 batch the joints...
|
|
const PxU32 nbJointHeaders = physx::immediate::PxBatchConstraintsTGS( mSolverConstraintDesc.begin() + mContactPairs.size(), mJointData.size(), mSolverBodies.begin(), nbBodies,
|
|
mHeaders.begin() + nbContactHeaders, orderedDescs.begin() + mContactPairs.size(),
|
|
mArticulations.begin(), mArticulations.size());
|
|
#else
|
|
//1 batch the contacts
|
|
const PxU32 nbContactHeaders = physx::immediate::PxBatchConstraints(mSolverConstraintDesc.begin(), mContactPairs.size(), mSolverBodies.begin(), nbBodies,
|
|
mHeaders.begin(), orderedDescs.begin(),
|
|
mArticulations.begin(), mArticulations.size());
|
|
|
|
//2 batch the joints...
|
|
const PxU32 nbJointHeaders = physx::immediate::PxBatchConstraints( mSolverConstraintDesc.begin() + mContactPairs.size(), mJointData.size(), mSolverBodies.begin(), nbBodies,
|
|
mHeaders.begin() + nbContactHeaders, orderedDescs.begin() + mContactPairs.size(),
|
|
mArticulations.begin(), mArticulations.size());
|
|
#endif
|
|
|
|
const PxU32 totalHeaders = nbContactHeaders + nbJointHeaders;
|
|
|
|
mHeaders.forceSize_Unsafe(totalHeaders);
|
|
#else
|
|
Array<PxSolverConstraintDesc>& orderedDescs = mSolverConstraintDesc;
|
|
|
|
const PxU32 nbContactHeaders = mContactPairs.size();
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
const PxU32 nbJointHeaders = mJointData.size();
|
|
PX_ASSERT(nbContactHeaders+nbJointHeaders==mSolverConstraintDesc.size());
|
|
mHeaders.resize(nbContactHeaders+nbJointHeaders);
|
|
#else
|
|
PX_ASSERT(nbContactHeaders==mSolverConstraintDesc.size());
|
|
PX_UNUSED(dt);
|
|
mHeaders.resize(nbContactHeaders);
|
|
#endif
|
|
|
|
// We are bypassing the constraint batching so we create dummy PxConstraintBatchHeaders
|
|
for(PxU32 i=0; i<nbContactHeaders; i++)
|
|
{
|
|
PxConstraintBatchHeader& hdr = mHeaders[i];
|
|
hdr.startIndex = i;
|
|
hdr.stride = 1;
|
|
hdr.constraintType = PxSolverConstraintDesc::eCONTACT_CONSTRAINT;
|
|
}
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
for(PxU32 i=0; i<nbJointHeaders; i++)
|
|
{
|
|
PxConstraintBatchHeader& hdr = mHeaders[nbContactHeaders+i];
|
|
hdr.startIndex = i;
|
|
hdr.stride = 1;
|
|
hdr.constraintType = PxSolverConstraintDesc::eJOINT_CONSTRAINT;
|
|
}
|
|
#endif
|
|
#endif
|
|
|
|
mContactForces.resize(mContactPoints.size());
|
|
|
|
for(PxU32 i=0; i<nbContactHeaders; i++)
|
|
{
|
|
PxConstraintBatchHeader& header = mHeaders[i];
|
|
PX_ASSERT(header.constraintType == PxSolverConstraintDesc::eCONTACT_CONSTRAINT);
|
|
|
|
#if USE_TGS
|
|
PxTGSSolverContactDesc contactDescs[4];
|
|
#else
|
|
PxSolverContactDesc contactDescs[4];
|
|
#endif
|
|
|
|
#if WITH_PERSISTENCY
|
|
PersistentContactPair* persistentPairs[4];
|
|
#endif
|
|
|
|
for(PxU32 a=0; a<header.stride; a++)
|
|
{
|
|
PxSolverConstraintDesc& constraintDesc = orderedDescs[header.startIndex + a];
|
|
|
|
//Extract the contact pair that we saved in this structure earlier.
|
|
const ContactPair& pair = *reinterpret_cast<const ContactPair*>(constraintDesc.constraint);
|
|
#if USE_TGS
|
|
PxTGSSolverContactDesc& contactDesc = contactDescs[a];
|
|
|
|
setupDesc(contactDesc, mActors.begin(), mSolverBodyTxInertias.begin(), mSolverBodyData.begin(), mPoses.begin(), pair.mID0, false);
|
|
setupDesc(contactDesc, mActors.begin(), mSolverBodyTxInertias.begin(), mSolverBodyData.begin(), mPoses.begin(), pair.mID1, true);
|
|
|
|
contactDesc.body0 = constraintDesc.tgsBodyA;
|
|
contactDesc.body1 = constraintDesc.tgsBodyB;
|
|
|
|
contactDesc.torsionalPatchRadius = 0.0f;
|
|
contactDesc.minTorsionalPatchRadius = 0.0f;
|
|
#else
|
|
PxSolverContactDesc& contactDesc = contactDescs[a];
|
|
|
|
setupDesc(contactDesc, mActors.begin(), mSolverBodyData.begin(), pair.mID0, false);
|
|
setupDesc(contactDesc, mActors.begin(), mSolverBodyData.begin(), pair.mID1, true);
|
|
|
|
contactDesc.body0 = constraintDesc.bodyA;
|
|
contactDesc.body1 = constraintDesc.bodyB;
|
|
#endif
|
|
contactDesc.contactForces = &mContactForces[pair.mStartContactIndex];
|
|
contactDesc.contacts = &mContactPoints[pair.mStartContactIndex];
|
|
contactDesc.numContacts = pair.mNbContacts;
|
|
|
|
#if WITH_PERSISTENCY
|
|
const HashMap<IDS, PersistentContactPair>::Entry* e = mPersistentPairs.find(IDS(pair.mID0, pair.mID1));
|
|
PX_ASSERT(e);
|
|
{
|
|
PersistentContactPair& pcp = const_cast<PersistentContactPair&>(e->second);
|
|
contactDesc.frictionPtr = pcp.frictions;
|
|
contactDesc.frictionCount = PxU8(pcp.nbFrictions);
|
|
persistentPairs[a] = &pcp;
|
|
}
|
|
#else
|
|
contactDesc.frictionPtr = NULL;
|
|
contactDesc.frictionCount = 0;
|
|
#endif
|
|
contactDesc.disableStrongFriction = false;
|
|
contactDesc.hasMaxImpulse = false;
|
|
contactDesc.hasForceThresholds = false;
|
|
contactDesc.shapeInteraction = NULL;
|
|
contactDesc.restDistance = 0.0f;
|
|
contactDesc.maxCCDSeparation = PX_MAX_F32;
|
|
|
|
contactDesc.desc = &constraintDesc;
|
|
contactDesc.invMassScales.angular0 = contactDesc.invMassScales.angular1 = contactDesc.invMassScales.linear0 = contactDesc.invMassScales.linear1 = 1.0f;
|
|
}
|
|
|
|
#if USE_TGS
|
|
PxCreateContactConstraintsTGS(&header, 1, contactDescs, *mConstraintAllocator, stepInvDt, invDt, gBounceThreshold, gFrictionOffsetThreshold, gCorrelationDistance);
|
|
#else
|
|
PxCreateContactConstraints(&header, 1, contactDescs, *mConstraintAllocator, invDt, gBounceThreshold, gFrictionOffsetThreshold, gCorrelationDistance);
|
|
#endif
|
|
|
|
#if WITH_PERSISTENCY
|
|
//Cache friction information...
|
|
for (PxU32 a = 0; a < header.stride; ++a)
|
|
{
|
|
#if USE_TGS
|
|
const PxTGSSolverContactDesc& contactDesc = contactDescs[a];
|
|
#else
|
|
const PxSolverContactDesc& contactDesc = contactDescs[a];
|
|
#endif
|
|
PersistentContactPair& pcp = *persistentPairs[a];
|
|
pcp.frictions = contactDesc.frictionPtr;
|
|
pcp.nbFrictions = contactDesc.frictionCount;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#ifdef TEST_IMMEDIATE_JOINTS
|
|
for(PxU32 i=0; i<nbJointHeaders; i++)
|
|
{
|
|
PxConstraintBatchHeader& header = mHeaders[nbContactHeaders+i];
|
|
PX_ASSERT(header.constraintType == PxSolverConstraintDesc::eJOINT_CONSTRAINT);
|
|
|
|
{
|
|
#if USE_TGS
|
|
PxTGSSolverConstraintPrepDesc jointDescs[4];
|
|
#else
|
|
PxSolverConstraintPrepDesc jointDescs[4];
|
|
#endif
|
|
PxImmediateConstraint constraints[4];
|
|
|
|
header.startIndex += mContactPairs.size();
|
|
|
|
for(PxU32 a=0; a<header.stride; a++)
|
|
{
|
|
PxSolverConstraintDesc& constraintDesc = orderedDescs[header.startIndex + a];
|
|
//Extract the contact pair that we saved in this structure earlier.
|
|
const MyJointData& jd = *reinterpret_cast<const MyJointData*>(constraintDesc.constraint);
|
|
|
|
constraints[a].prep = SphericalJointSolverPrep;
|
|
constraints[a].constantBlock = &jd;
|
|
#if USE_TGS
|
|
PxTGSSolverConstraintPrepDesc& jointDesc = jointDescs[a];
|
|
jointDesc.body0 = constraintDesc.tgsBodyA;
|
|
jointDesc.body1 = constraintDesc.tgsBodyB;
|
|
setupJointDesc(jointDesc, mActors.begin(), mSolverBodyTxInertias.begin(), mSolverBodyData.begin(), mPoses.begin(), constraintDesc.bodyADataIndex, false);
|
|
setupJointDesc(jointDesc, mActors.begin(), mSolverBodyTxInertias.begin(), mSolverBodyData.begin(), mPoses.begin(), constraintDesc.bodyBDataIndex, true);
|
|
#else
|
|
PxSolverConstraintPrepDesc& jointDesc = jointDescs[a];
|
|
jointDesc.body0 = constraintDesc.bodyA;
|
|
jointDesc.body1 = constraintDesc.bodyB;
|
|
setupJointDesc(jointDesc, mActors.begin(), mSolverBodyData.begin(), constraintDesc.bodyADataIndex, false);
|
|
setupJointDesc(jointDesc, mActors.begin(), mSolverBodyData.begin(), constraintDesc.bodyBDataIndex, true);
|
|
#endif
|
|
jointDesc.desc = &constraintDesc;
|
|
jointDesc.writeback = NULL;
|
|
jointDesc.linBreakForce = PX_MAX_F32;
|
|
jointDesc.angBreakForce = PX_MAX_F32;
|
|
jointDesc.minResponseThreshold = 0;
|
|
jointDesc.disablePreprocessing = false;
|
|
jointDesc.improvedSlerp = false;
|
|
jointDesc.driveLimitsAreForces = false;
|
|
jointDesc.invMassScales.angular0 = jointDesc.invMassScales.angular1 = jointDesc.invMassScales.linear0 = jointDesc.invMassScales.linear1 = 1.0f;
|
|
}
|
|
|
|
#if USE_TGS
|
|
immediate::PxCreateJointConstraintsWithImmediateShadersTGS(&header, 1, constraints, jointDescs, *mConstraintAllocator, stepDt, dt, stepInvDt, invDt, lengthScale);
|
|
#else
|
|
immediate::PxCreateJointConstraintsWithImmediateShaders(&header, 1, constraints, jointDescs, *mConstraintAllocator, dt, invDt);
|
|
#endif
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void ImmediateScene::solveAndIntegrate(float dt)
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time0 = __rdtsc();
|
|
#endif
|
|
const PxU32 totalNbActors = mActors.size();
|
|
const PxU32 nbDynamicActors = totalNbActors - mNbStaticActors - mNbArticulationLinks;
|
|
const PxU32 nbDynamic = nbDynamicActors + mNbArticulationLinks;
|
|
|
|
mMotionLinearVelocity.resize(nbDynamic);
|
|
mMotionAngularVelocity.resize(nbDynamic);
|
|
|
|
const PxU32 nbArticulations = mArticulations.size();
|
|
Dy::ArticulationV** articulations = mArticulations.begin();
|
|
|
|
#if USE_TGS
|
|
const float stepDt = dt/float(gNbIterPos);
|
|
|
|
immediate::PxSolveConstraintsTGS(mHeaders.begin(), mHeaders.size(),
|
|
#if BATCH_CONTACTS
|
|
mOrderedSolverConstraintDesc.begin(),
|
|
#else
|
|
mSolverConstraintDesc.begin(),
|
|
#endif
|
|
mSolverBodies.begin(), mSolverBodyTxInertias.begin(),
|
|
nbDynamic, gNbIterPos, gNbIterVel, stepDt, 1.0f / stepDt, nbArticulations, articulations);
|
|
#else
|
|
|
|
PxMemZero(mSolverBodies.begin(), mSolverBodies.size() * sizeof(PxSolverBody));
|
|
|
|
PxSolveConstraints( mHeaders.begin(), mHeaders.size(),
|
|
#if BATCH_CONTACTS
|
|
mOrderedSolverConstraintDesc.begin(),
|
|
#else
|
|
mSolverConstraintDesc.begin(),
|
|
#endif
|
|
mSolverBodies.begin(),
|
|
mMotionLinearVelocity.begin(), mMotionAngularVelocity.begin(), nbDynamic, gNbIterPos, gNbIterVel,
|
|
dt, 1.0f/dt, nbArticulations, articulations);
|
|
#endif
|
|
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time1 = __rdtsc();
|
|
#endif
|
|
|
|
#if USE_TGS
|
|
PxIntegrateSolverBodiesTGS(mSolverBodies.begin(), mSolverBodyTxInertias.begin(), mPoses.begin(), nbDynamicActors, dt);
|
|
for (PxU32 i = 0; i<nbArticulations; i++)
|
|
PxUpdateArticulationBodiesTGS(articulations[i], dt);
|
|
|
|
for (PxU32 i = 0; i<nbDynamicActors; i++)
|
|
{
|
|
PX_ASSERT(mActors[i].mType == ImmediateActor::eDYNAMIC);
|
|
const PxTGSSolverBodyVel& data = mSolverBodies[i];
|
|
mActors[i].mLinearVelocity = data.linearVelocity;
|
|
mActors[i].mAngularVelocity = data.angularVelocity;
|
|
}
|
|
|
|
#else
|
|
PxIntegrateSolverBodies(mSolverBodyData.begin(), mSolverBodies.begin(), mMotionLinearVelocity.begin(), mMotionAngularVelocity.begin(), nbDynamicActors, dt);
|
|
for (PxU32 i = 0; i<nbArticulations; i++)
|
|
PxUpdateArticulationBodies(articulations[i], dt);
|
|
|
|
for (PxU32 i = 0; i<nbDynamicActors; i++)
|
|
{
|
|
PX_ASSERT(mActors[i].mType == ImmediateActor::eDYNAMIC);
|
|
const PxSolverBodyData& data = mSolverBodyData[i];
|
|
mActors[i].mLinearVelocity = data.linearVelocity;
|
|
mActors[i].mAngularVelocity = data.angularVelocity;
|
|
mPoses[i] = data.body2World;
|
|
}
|
|
#endif
|
|
|
|
for(PxU32 i=0;i<mNbArticulationLinks;i++)
|
|
{
|
|
const PxU32 j = nbDynamicActors + i;
|
|
PX_ASSERT(mActors[j].mType==ImmediateActor::eLINK);
|
|
|
|
PxLinkData data;
|
|
bool status = PxGetLinkData(mActors[j].mLink, data);
|
|
PX_ASSERT(status);
|
|
PX_UNUSED(status);
|
|
|
|
mActors[j].mLinearVelocity = data.linearVelocity;
|
|
mActors[j].mAngularVelocity = data.angularVelocity;
|
|
mPoses[j] = data.pose;
|
|
}
|
|
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time2 = __rdtsc();
|
|
printf("solve: %d \n", (time1-time0)/1024);
|
|
printf("integrate: %d \n", (time2-time1)/1024);
|
|
#endif
|
|
}
|
|
|
|
static PxDefaultAllocator gAllocator;
|
|
static PxDefaultErrorCallback gErrorCallback;
|
|
static PxFoundation* gFoundation = NULL;
|
|
static ImmediateScene* gScene = NULL;
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
PxU32 getNbGeoms()
|
|
{
|
|
return gScene ? gScene->mGeoms.size() : 0;
|
|
}
|
|
|
|
const PxGeometryHolder* getGeoms()
|
|
{
|
|
if(!gScene || !gScene->mGeoms.size())
|
|
return NULL;
|
|
return &gScene->mGeoms[0];
|
|
}
|
|
|
|
const PxTransform* getGeomPoses()
|
|
{
|
|
if(!gScene || !gScene->mPoses.size())
|
|
return NULL;
|
|
return &gScene->mPoses[0];
|
|
}
|
|
|
|
PxU32 getNbArticulations()
|
|
{
|
|
return gScene ? gScene->mArticulations.size() : 0;
|
|
}
|
|
|
|
Dy::ArticulationV** getArticulations()
|
|
{
|
|
if(!gScene || !gScene->mArticulations.size())
|
|
return NULL;
|
|
return &gScene->mArticulations[0];
|
|
}
|
|
|
|
PxU32 getNbBounds()
|
|
{
|
|
if(!gDrawBounds)
|
|
return 0;
|
|
return gScene ? gScene->mBounds.size() : 0;
|
|
}
|
|
|
|
const PxBounds3* getBounds()
|
|
{
|
|
if(!gDrawBounds)
|
|
return NULL;
|
|
if(!gScene || !gScene->mBounds.size())
|
|
return NULL;
|
|
return &gScene->mBounds[0];
|
|
}
|
|
|
|
PxU32 getNbContacts()
|
|
{
|
|
return gScene ? gScene->mContactPoints.size() : 0;
|
|
}
|
|
|
|
const Gu::ContactPoint* getContacts()
|
|
{
|
|
if(!gScene || !gScene->mContactPoints.size())
|
|
return NULL;
|
|
return &gScene->mContactPoints[0];
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
void initPhysics(bool /*interactive*/)
|
|
{
|
|
gFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, gAllocator, gErrorCallback);
|
|
|
|
PxRegisterImmediateArticulations();
|
|
|
|
gScene = new ImmediateScene;
|
|
gScene->createScene();
|
|
}
|
|
|
|
void stepPhysics(bool /*interactive*/)
|
|
{
|
|
if(!gScene)
|
|
return;
|
|
|
|
if(gPause && !gOneFrame)
|
|
return;
|
|
gOneFrame = false;
|
|
|
|
const float dt = 1.0f/60.0f;
|
|
const float invDt = 60.0f;
|
|
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time = __rdtsc();
|
|
#endif
|
|
gScene->updateArticulations(dt);
|
|
#ifdef PRINT_TIMINGS
|
|
time = __rdtsc() - time;
|
|
printf("updateArticulations: %d \n", time/1024);
|
|
#endif
|
|
}
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time = __rdtsc();
|
|
#endif
|
|
gScene->updateBounds();
|
|
#ifdef PRINT_TIMINGS
|
|
time = __rdtsc() - time;
|
|
printf("updateBounds: %d \n", time/1024);
|
|
#endif
|
|
}
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time = __rdtsc();
|
|
#endif
|
|
gScene->broadPhase();
|
|
#ifdef PRINT_TIMINGS
|
|
time = __rdtsc() - time;
|
|
printf("broadPhase: %d \n", time/1024);
|
|
#endif
|
|
}
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time = __rdtsc();
|
|
#endif
|
|
gScene->narrowPhase();
|
|
#ifdef PRINT_TIMINGS
|
|
time = __rdtsc() - time;
|
|
printf("narrowPhase: %d \n", time/1024);
|
|
#endif
|
|
}
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time = __rdtsc();
|
|
#endif
|
|
gScene->buildSolverBodyData(dt);
|
|
#ifdef PRINT_TIMINGS
|
|
time = __rdtsc() - time;
|
|
printf("buildSolverBodyData: %d \n", time/1024);
|
|
#endif
|
|
}
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time = __rdtsc();
|
|
#endif
|
|
gScene->buildSolverConstraintDesc();
|
|
#ifdef PRINT_TIMINGS
|
|
time = __rdtsc() - time;
|
|
printf("buildSolverConstraintDesc: %d \n", time/1024);
|
|
#endif
|
|
}
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
unsigned long long time = __rdtsc();
|
|
#endif
|
|
gScene->createContactConstraints(dt, invDt, 1.f, gNbIterPos);
|
|
#ifdef PRINT_TIMINGS
|
|
time = __rdtsc() - time;
|
|
printf("createContactConstraints: %d \n", time/1024);
|
|
#endif
|
|
}
|
|
{
|
|
#ifdef PRINT_TIMINGS
|
|
// unsigned long long time = __rdtsc();
|
|
#endif
|
|
gScene->solveAndIntegrate(dt);
|
|
#ifdef PRINT_TIMINGS
|
|
// time = __rdtsc() - time;
|
|
// printf("solveAndIntegrate: %d \n", time/1024);
|
|
#endif
|
|
}
|
|
|
|
if(gScene->mMotorLink)
|
|
{
|
|
static float time = 0.0f;
|
|
time += 0.1f;
|
|
const float target = sinf(time) * 4.0f;
|
|
// printf("target: %f\n", target);
|
|
|
|
PxFeatherstoneArticulationJointData data;
|
|
bool status = PxGetJointData(gScene->mMotorLink, data);
|
|
PX_ASSERT(status);
|
|
|
|
data.targetVel[PxArticulationAxis::eTWIST] = target;
|
|
|
|
const PxVec3 boxExtents(0.5f, 0.1f, 0.5f);
|
|
const float s = boxExtents.x*1.1f + fabsf(sinf(time))*0.5f;
|
|
|
|
data.parentPose = PxTransform(PxVec3(0.0f, 0.0f, s));
|
|
data.childPose = PxTransform(PxVec3(0.0f, 0.0f, -s));
|
|
|
|
status = PxSetJointData(gScene->mMotorLink, data);
|
|
PX_ASSERT(status);
|
|
PX_UNUSED(status);
|
|
}
|
|
|
|
}
|
|
|
|
void cleanupPhysics(bool /*interactive*/)
|
|
{
|
|
PX_DELETE_AND_RESET(gScene);
|
|
PX_RELEASE(gFoundation);
|
|
|
|
printf("SnippetImmediateArticulation done.\n");
|
|
}
|
|
|
|
void keyPress(unsigned char key, const PxTransform& /*camera*/)
|
|
{
|
|
if(key=='b' || key=='B')
|
|
gDrawBounds = !gDrawBounds;
|
|
|
|
if(key=='p' || key=='P')
|
|
gPause = !gPause;
|
|
|
|
if(key=='o' || key=='O')
|
|
{
|
|
gPause = true;
|
|
gOneFrame = true;
|
|
}
|
|
|
|
if(gScene)
|
|
{
|
|
if(key>=1 && key<=6)
|
|
{
|
|
gSceneIndex = key-1;
|
|
gScene->reset();
|
|
gScene->createScene();
|
|
}
|
|
|
|
if(key=='r' || key=='R')
|
|
{
|
|
gScene->reset();
|
|
gScene->createScene();
|
|
}
|
|
}
|
|
}
|
|
|
|
int snippetMain(int, const char*const*)
|
|
{
|
|
printf("Immediate articulation snippet. Use these keys:\n");
|
|
printf(" P - enable/disable pause\n");
|
|
printf(" O - step simulation for one frame\n");
|
|
printf(" R - reset scene\n");
|
|
printf(" F1 to F6 - select scene\n");
|
|
printf("\n");
|
|
#ifdef RENDER_SNIPPET
|
|
extern void renderLoop();
|
|
renderLoop();
|
|
#else
|
|
static const PxU32 frameCount = 100;
|
|
initPhysics(false);
|
|
for(PxU32 i=0; i<frameCount; i++)
|
|
stepPhysics(false);
|
|
cleanupPhysics(false);
|
|
#endif
|
|
|
|
return 0;
|
|
}
|
|
|