// Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions // are met: // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // * Neither the name of NVIDIA CORPORATION nor the names of its // contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR // PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, // EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY // OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // // Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved. // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved. // Copyright (c) 2001-2004 NovodeX AG. All rights reserved. #include "foundation/PxMathUtils.h" #include "CmConeLimitHelper.h" #include "DySolverConstraint1D.h" #include "DyFeatherstoneArticulation.h" #include "PxsRigidBody.h" #include "PxcConstraintBlockStream.h" #include "DyArticulationContactPrep.h" #include "DyDynamics.h" #include "DyArticulationPImpl.h" #include "DyFeatherstoneArticulationLink.h" #include "DyFeatherstoneArticulationJointData.h" #include "DySolverConstraint1DStep.h" #include "DyTGSDynamics.h" #include "DyConstraintPrep.h" #include "common/PxProfileZone.h" #include "PxsContactManager.h" #include "DyContactPrep.h" #include "DySolverContext.h" #include "DyTGSContactPrep.h" #include "DyCpuGpuArticulation.h" #include "DyArticulationUtils.h" #ifndef FEATURESTONE_DEBUG #define FEATURESTONE_DEBUG 0 #endif // we encode articulation link handles in the lower bits of the pointer, so the // articulation has to be aligned, which in an aligned pool means we need to size it // appropriately namespace physx { namespace Dy { ArticulationData::~ArticulationData() { PX_FREE(mLinksData); PX_FREE(mJointData); PX_FREE(mPathToRootElements); } void ArticulationData::resizeLinkData(const PxU32 linkCount) { const PxU32 oldSize = mMotionVelocities.size(); mMotionVelocities.reserve(linkCount); mMotionVelocities.forceSize_Unsafe(linkCount); mSolverLinkSpatialDeltaVels.reserve(linkCount); mSolverLinkSpatialDeltaVels.forceSize_Unsafe(linkCount); mSolverLinkSpatialImpulses.reserve(linkCount); mSolverLinkSpatialImpulses.forceSize_Unsafe(linkCount); mMotionAccelerations.reserve(linkCount); mMotionAccelerations.forceSize_Unsafe(linkCount); mLinkIncomingJointForces.reserve(linkCount); mLinkIncomingJointForces.forceSize_Unsafe(linkCount); mMotionAccelerationsInternal.reserve(linkCount); mMotionAccelerationsInternal.forceSize_Unsafe(linkCount); mCorioliseVectors.reserve(linkCount); mCorioliseVectors.forceSize_Unsafe(linkCount); mZAForces.reserve(linkCount); mZAForces.forceSize_Unsafe(linkCount); mZAInternalForces.reserve(linkCount); mZAInternalForces.forceSize_Unsafe(linkCount); mNbStatic1DConstraints.reserve(linkCount); mNbStatic1DConstraints.forceSize_Unsafe(linkCount); mStatic1DConstraintStartIndex.reserve(linkCount); mStatic1DConstraintStartIndex.forceSize_Unsafe(linkCount); mNbStaticContactConstraints.reserve(linkCount); mNbStaticContactConstraints.forceSize_Unsafe(linkCount); mStaticContactConstraintStartIndex.reserve(linkCount); mStaticContactConstraintStartIndex.forceSize_Unsafe(linkCount); mDeltaMotionVector.reserve(linkCount); mDeltaMotionVector.forceSize_Unsafe(linkCount); mPreTransform.reserve(linkCount); mPreTransform.forceSize_Unsafe(linkCount); mResponseMatrixW.reserve(linkCount); mResponseMatrixW.forceSize_Unsafe(linkCount); mWorldSpatialArticulatedInertia.reserve(linkCount); mWorldSpatialArticulatedInertia.forceSize_Unsafe(linkCount); mWorldIsolatedSpatialArticulatedInertia.reserve(linkCount); mWorldIsolatedSpatialArticulatedInertia.forceSize_Unsafe(linkCount); mMasses.reserve(linkCount); mMasses.forceSize_Unsafe(linkCount); mInvStIs.reserve(linkCount); mInvStIs.forceSize_Unsafe(linkCount); /*mMotionMatrix.resize(linkCount); mWorldMotionMatrix.reserve(linkCount); mWorldMotionMatrix.forceSize_Unsafe(linkCount);*/ mAccumulatedPoses.reserve(linkCount); mAccumulatedPoses.forceSize_Unsafe(linkCount); mDeltaQ.reserve(linkCount); mDeltaQ.forceSize_Unsafe(linkCount); mPosIterMotionVelocities.reserve(linkCount); mPosIterMotionVelocities.forceSize_Unsafe(linkCount); mJointTransmittedForce.reserve(linkCount); mJointTransmittedForce.forceSize_Unsafe(linkCount); mRw.reserve(linkCount); mRw.forceSize_Unsafe(linkCount); mRelativeQuat.resize(linkCount); if (oldSize < linkCount) { ArticulationLinkData* oldLinks = mLinksData; ArticulationJointCoreData* oldJoints = mJointData; mLinksData = PX_ALLOCATE(ArticulationLinkData, linkCount, "ArticulationLinkData"); mJointData = PX_ALLOCATE(ArticulationJointCoreData, linkCount, "ArticulationJointCoreData"); PxMemCopy(mLinksData, oldLinks, sizeof(ArticulationLinkData)*oldSize); PxMemCopy(mJointData, oldJoints, sizeof(ArticulationJointCoreData)*oldSize); PX_FREE(oldLinks); PX_FREE(oldJoints); const PxU32 newElems = (linkCount - oldSize); PxMemZero(mLinksData + oldSize, sizeof(ArticulationLinkData) * newElems); PxMemZero(mJointData + oldSize, sizeof(ArticulationJointCoreData) * newElems); for (PxU32 linkID = oldSize; linkID < linkCount; ++linkID) { PX_PLACEMENT_NEW(mLinksData + linkID, ArticulationLinkData)(); PX_PLACEMENT_NEW(mJointData + linkID, ArticulationJointCoreData)(); } } } void ArticulationData::resizeJointData(const PxU32 dofs) { mJointAcceleration.reserve(dofs); mJointAcceleration.forceSize_Unsafe(dofs); mJointInternalAcceleration.reserve(dofs); mJointInternalAcceleration.forceSize_Unsafe(dofs); mJointVelocity.reserve(dofs); mJointVelocity.forceSize_Unsafe(dofs); mJointNewVelocity.reserve(dofs+3); mJointNewVelocity.forceSize_Unsafe(dofs+3); mJointPosition.reserve(dofs); mJointPosition.forceSize_Unsafe(dofs); mJointForce.reserve(dofs); mJointForce.forceSize_Unsafe(dofs); mJointTargetPositions.reserve(dofs); mJointTargetPositions.forceSize_Unsafe(dofs); mJointTargetVelocities.reserve(dofs); mJointTargetVelocities.forceSize_Unsafe(dofs); mMotionMatrix.resize(dofs); mWorldMotionMatrix.reserve(dofs); mWorldMotionMatrix.forceSize_Unsafe(dofs); mJointAxis.reserve(dofs); mJointAxis.forceSize_Unsafe(dofs); mIsW.reserve(dofs); mIsW.forceSize_Unsafe(dofs); mDeferredQstZ.reserve(dofs); mDeferredQstZ.forceSize_Unsafe(dofs); qstZIc.reserve(dofs); qstZIc.forceSize_Unsafe(dofs); qstZIntIc.reserve(dofs); qstZIntIc.forceSize_Unsafe(dofs); mISInvStIS.reserve(dofs); mISInvStIS.forceSize_Unsafe(dofs); mPosIterJointVelocities.reserve(dofs); mPosIterJointVelocities.forceSize_Unsafe(dofs); PxMemZero(mJointAcceleration.begin(), sizeof(PxReal) * dofs); PxMemZero(mJointVelocity.begin(), sizeof(PxReal) * dofs); PxMemZero(mJointPosition.begin(), sizeof(PxReal) * dofs); PxMemZero(mJointForce.begin(), sizeof(PxReal) * dofs); PxMemZero(mJointTargetPositions.begin(), sizeof(PxReal) * dofs); PxMemZero(mJointTargetVelocities.begin(), sizeof(PxReal) * dofs); } ArticulationLinkData& ArticulationData::getLinkData(PxU32 index) const { PX_ASSERT(index < mLinkCount); return mLinksData[index]; } FeatherstoneArticulation::FeatherstoneArticulation(void* userData) : mUserData(userData), mContext(NULL), mUpdateSolverData(true), mMaxDepth(0), mJcalcDirty(true) { mGPUDirtyFlags = 0; mInternalErrorAccumulatorVelIter.reset(); mContactErrorAccumulatorVelIter.reset(); mInternalErrorAccumulatorPosIter.reset(); mContactErrorAccumulatorPosIter.reset(); } FeatherstoneArticulation::~FeatherstoneArticulation() { } void FeatherstoneArticulation::copyJointData(const ArticulationData& data, PxReal* toJointData, const PxReal* fromJointData) { const PxU32 dofCount = data.getDofs(); PxMemCopy(toJointData, fromJointData, sizeof(PxReal)*dofCount); } PxU32 FeatherstoneArticulation::countDofs() { const PxU32 linkCount = mArticulationData.getLinkCount(); PxU32 totalDofs = 0; for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = mArticulationData.getLink(linkID); ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID); const PxU8 dof = jointDatum.countJointDofs(link.inboundJoint); totalDofs += dof; } return totalDofs; } void FeatherstoneArticulation::configureDofs() { const PxU32 linkCount = mArticulationData.getLinkCount(); PxU32 totalDof = 0; for(PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = mArticulationData.getLink(linkID); ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID); const PxU8 dof = jointDatum.configureJointDofs(link.inboundJoint, mArticulationData.mJointAxis.begin() + totalDof); jointDatum.jointOffset = totalDof; link.inboundJoint->jointOffset = totalDof; totalDof += dof; } } bool FeatherstoneArticulation::resize(const PxU32 linkCount) { if (mUpdateSolverData) { if (linkCount != mSolverDesc.linkCount) { mSolverDesc.acceleration = mAcceleration.begin(); mSolverDesc.articulation = this; } mUpdateSolverData = false; if (linkCount != mSolverDesc.linkCount) mArticulationData.resizeLinkData(linkCount); return true; } return false; } void FeatherstoneArticulation::getDataSizes(PxU32 /*linkCount*/, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize) { solverDataSize = 0; totalSize = 0; scratchSize = 0; } void FeatherstoneArticulation::setupLinks(PxU32 nbLinks, Dy::ArticulationLink* links) { //if this is needed, we need to re-allocated the link data resize(nbLinks); mSolverDesc.links = links; mSolverDesc.linkCount = PxTo8(nbLinks); mArticulationData.mLinks = links; mArticulationData.mLinkCount = PxTo8(nbLinks); mArticulationData.mFlags = mSolverDesc.core ? &mSolverDesc.core->flags : mSolverDesc.flags; // PT: PX-1399 mArticulationData.mExternalAcceleration = mSolverDesc.acceleration; mArticulationData.mArticulation = this; //allocate memory for articulation data setupDofs(); } void FeatherstoneArticulation::setupDofs() { PxU32 totalDofs = countDofs(); const PxU32 existedTotalDofs = mArticulationData.getDofs(); if(totalDofs != existedTotalDofs) { mArticulationData.resizeJointData(totalDofs + 1); mArticulationData.setDofs(totalDofs); } configureDofs(); } void FeatherstoneArticulation::allocatePathToRootElements(const PxU32 totalPathToRootElements) { if (mArticulationData.mNumPathToRootElements < totalPathToRootElements) { mArticulationData.mPathToRootElements = PX_ALLOCATE(PxU32, totalPathToRootElements, "PxU32"); mArticulationData.mNumPathToRootElements = totalPathToRootElements; } } void FeatherstoneArticulation::initPathToRoot() { Dy::ArticulationLink* links = mArticulationData.getLinks(); const PxU32 linkCount = mArticulationData.getLinkCount(); links[0].mPathToRootCount = 0; links[0].mPathToRootStartIndex = 0; PxU32 totalPathToRootCount = 1; //add on root for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { Dy::ArticulationLink& link = links[linkID]; PxU32 parent = link.parent; PxU32 pathToRootCount = 1; // add myself to the path while (parent != 0) // don't add the root { parent = links[parent].parent; pathToRootCount++; } link.mPathToRootStartIndex = totalPathToRootCount; link.mPathToRootCount = PxU16(pathToRootCount); totalPathToRootCount += pathToRootCount; } allocatePathToRootElements(totalPathToRootCount); PxU32* pathToRootElements = mArticulationData.getPathToRootElements(); pathToRootElements[0] = 0; //add on root index for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { Dy::ArticulationLink& link = links[linkID]; PxU32* pathToRoot = &pathToRootElements[link.mPathToRootStartIndex]; PxU32 numElements = link.mPathToRootCount; pathToRoot[--numElements] = linkID; PxU32 parent = links[linkID].parent; while (parent != 0) { pathToRoot[--numElements] = parent; parent = links[parent].parent; } } } void FeatherstoneArticulation::assignTendons(const PxU32 nbTendons, Dy::ArticulationSpatialTendon** tendons) { mArticulationData.mSpatialTendons = tendons; mArticulationData.mNumSpatialTendons = nbTendons; } void FeatherstoneArticulation::assignTendons(const PxU32 nbTendons, Dy::ArticulationFixedTendon** tendons) { mArticulationData.mFixedTendons = tendons; mArticulationData.mNumFixedTendons = nbTendons; } void FeatherstoneArticulation::assignMimicJoints(const PxU32 nbMimicJoints, Dy::ArticulationMimicJointCore** mimicJoints) { mArticulationData.mMimicJoints = mimicJoints; mArticulationData.mNbMimicJoints = nbMimicJoints; } PxU32 FeatherstoneArticulation::getDofs() const { return mArticulationData.getDofs(); } PxU32 FeatherstoneArticulation::getDof(const PxU32 linkID) { const ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID); return jointDatum.nbDof; } bool FeatherstoneArticulation::applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, bool& shouldWake) { return applyCacheToDest(mArticulationData, cache, mArticulationData.getJointVelocities(), mArticulationData.getJointPositions(), mArticulationData.getJointForces(), mArticulationData.getJointTargetPositions(), mArticulationData.getJointTargetVelocities(), flag, shouldWake); } void FeatherstoneArticulation::copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, const bool isGpuSimEnabled) { if (flag & PxArticulationCacheFlag::eVELOCITY) { copyJointData(mArticulationData, cache.jointVelocity, mArticulationData.getJointVelocities()); } if (flag & PxArticulationCacheFlag::eACCELERATION) { copyJointData(mArticulationData, cache.jointAcceleration, mArticulationData.getJointAccelerations()); } if (flag & PxArticulationCacheFlag::ePOSITION) { copyJointData(mArticulationData, cache.jointPosition, mArticulationData.getJointPositions()); } if (flag & PxArticulationCacheFlag::eFORCE) { copyJointData(mArticulationData, cache.jointForce, mArticulationData.getJointForces()); } if (flag & PxArticulationCacheFlag::eJOINT_TARGET_POSITIONS) { copyJointData(mArticulationData, cache.jointTargetPositions, mArticulationData.getJointTargetPositions()); } if (flag & PxArticulationCacheFlag::eJOINT_TARGET_VELOCITIES) { copyJointData(mArticulationData, cache.jointTargetVelocities, mArticulationData.getJointTargetVelocities()); } if (flag & PxArticulationCacheFlag::eLINK_VELOCITY) { const Cm::SpatialVectorF* vels = mArticulationData.getMotionVelocities(); const PxU32 numLinks = mArticulationData.getLinkCount(); for (PxU32 i = 0; i < numLinks; ++i) { const Cm::SpatialVectorF& vel = vels[i]; cache.linkVelocity[i].linear = vel.bottom; cache.linkVelocity[i].angular = vel.top; } } if (flag & PxArticulationCacheFlag::eLINK_ACCELERATION) { const PxU32 linkCount = mArticulationData.getLinkCount(); if(mArticulationData.getDt() == 0.0f) { PxMemZero(cache.linkAcceleration, sizeof(PxSpatialVelocity)*linkCount); } else if(isGpuSimEnabled) { const Cm::SpatialVectorF* linkMotionAccelerationsW = mArticulationData.mMotionAccelerations.begin(); //Iterate over all links and compute the acceleration for each link. for (PxU32 i = 0; i < linkCount; ++i) { cache.linkAcceleration[i].linear = linkMotionAccelerationsW[i].bottom; cache.linkAcceleration[i].angular = linkMotionAccelerationsW[i].top; } } else { const PxReal invDt = 1.0f/mArticulationData.getDt(); const Cm::SpatialVectorF* linkMotionAccelerationsW = mArticulationData.mMotionAccelerations.begin(); const Cm::SpatialVectorF* linkSpatialDeltaVelsW = mArticulationData.mSolverLinkSpatialDeltaVels.begin(); //Iterate over all links and compute the acceleration for each link. for (PxU32 i = 0; i < linkCount; ++i) { cache.linkAcceleration[i].linear = linkMotionAccelerationsW[i].bottom + linkSpatialDeltaVelsW[i].bottom*invDt; cache.linkAcceleration[i].angular = linkMotionAccelerationsW[i].top + linkSpatialDeltaVelsW[i].top*invDt; } } } if(flag & PxArticulationCacheFlag::eLINK_INCOMING_JOINT_FORCE) { const PxU32 linkCount = mArticulationData.getLinkCount(); if (mArticulationData.getDt() == 0.0f) { PxMemZero(cache.linkIncomingJointForce, sizeof(PxSpatialForce)*linkCount); } else if(isGpuSimEnabled) { //Calculation already completed on gpu. const Cm::SpatialVectorF* incomingJointForces = mArticulationData.getLinkIncomingJointForces(); //Root links have no incoming joint. cache.linkIncomingJointForce[0].force = PxVec3(PxZero); cache.linkIncomingJointForce[0].torque = PxVec3(PxZero); //Iterate over all links and get the incoming joint force for each link. for (PxU32 i = 1; i < linkCount; ++i) { cache.linkIncomingJointForce[i].force = incomingJointForces[i].top; cache.linkIncomingJointForce[i].torque = incomingJointForces[i].bottom; } } else { const PxReal invDt = 1.0f/mArticulationData.getDt(); //Get everything we need. const Cm::SpatialVectorF* linkZAForcesExtW = mArticulationData.mZAForces.begin(); const Cm::SpatialVectorF* linkZAForcesIntW = mArticulationData.mZAInternalForces.begin(); const Cm::SpatialVectorF* linkMotionAccelerationsW = mArticulationData.mMotionAccelerations.begin(); const SpatialMatrix* linkSpatialInertiasW = mArticulationData.mWorldSpatialArticulatedInertia.begin(); const Cm::SpatialVectorF* linkSpatialDeltaVelsW = mArticulationData.mSolverLinkSpatialDeltaVels.begin(); const Cm::SpatialVectorF* linkSpatialImpulsesW = mArticulationData.mSolverLinkSpatialImpulses.begin(); //Root links have no incoming joint. cache.linkIncomingJointForce[0].force = PxVec3(PxZero); cache.linkIncomingJointForce[0].torque = PxVec3(PxZero); //Iterate over all links and compute the incoming joint force for each link. for (PxU32 i = 1; i < linkCount; ++i) { const ArticulationLink& link = mArticulationData.getLink(i); const ArticulationJointCore* joint = link.inboundJoint; const PxTransform Gc = link.bodyCore->body2World; const PxTransform Lc = joint->childPose; const PxTransform GcLc = Gc*Lc; const PxVec3 dW = Gc.rotate(Lc.p); //Compute the force measured at the link. const Cm::SpatialVectorF incomingJointForceAtLinkW = linkSpatialInertiasW[i]*(linkMotionAccelerationsW[i] + linkSpatialDeltaVelsW[i]*invDt) + (linkZAForcesExtW[i] + linkZAForcesIntW[i] + linkSpatialImpulsesW[i]*invDt); //Compute the equivalent force measured at the joint. const Cm::SpatialVectorF incomingJointForceAtJointW = FeatherstoneArticulation::translateSpatialVector(-dW, incomingJointForceAtLinkW); //Transform the force to the child joint frame. cache.linkIncomingJointForce[i].force = GcLc.rotateInv(incomingJointForceAtJointW.top); cache.linkIncomingJointForce[i].torque = GcLc.rotateInv(incomingJointForceAtJointW.bottom); } } } if (flag & PxArticulationCacheFlag::eROOT_TRANSFORM) { const ArticulationLink& rLink = mArticulationData.getLink(0); const PxsBodyCore& rBodyCore = *rLink.bodyCore; const PxTransform& body2World = rBodyCore.body2World; // PT:: tag: scalar transform*transform cache.rootLinkData->transform = body2World * rBodyCore.getBody2Actor().getInverse(); } if (flag & PxArticulationCacheFlag::eROOT_VELOCITIES) { const Cm::SpatialVectorF& vel = mArticulationData.getMotionVelocity(0); cache.rootLinkData->worldLinVel = vel.bottom; cache.rootLinkData->worldAngVel = vel.top; const Cm::SpatialVectorF& accel = mArticulationData.getMotionAcceleration(0); cache.rootLinkData->worldLinAccel = accel.bottom; cache.rootLinkData->worldAngAccel = accel.top; } } PxU32 FeatherstoneArticulation::getCacheDataSize(PxU32 totalDofs, PxU32 linkCount) { const PxU32 totalSize = sizeof(PxSpatialForce) * linkCount // external force + sizeof(PxReal) * (6 + totalDofs) * (linkCount * 6) // Free floating base dofs = 6 + totalDofs, and each link (incl. base) velocity has 6 elements // == offset to end of dense jacobian (assuming free floating base) + sizeof(PxReal) * (totalDofs + 6) * (totalDofs + 6) // mass matrix + sizeof(PxReal) * (totalDofs + 6) // Coriolis and centrifugal compensation force + sizeof(PxReal) * (totalDofs + 6) // gravity compensation force + sizeof(PxReal) * 6 * (totalDofs + 6) // centroidal momentum matrix + sizeof(PxReal) * 6 // centroidal momentum bias + sizeof(PxReal) * totalDofs * 6 // jointVelocity (PxArticulationCacheFlag::eVELOCITY) // jointAcceleration (PxArticulationCacheFlag::eACCELERATION) // jointPosition (PxArticulationCacheFlag::ePOSITION) // joint force (PxArticulationCacheFlag::eFORCE) // joint target positions (PxArticulationCacheFlag::eJOINT_TARGET_POSITIONS) // joint target velocities (PxArticulationCacheFlag::eJOINT_TARGET_VELOCITIES) + sizeof(PxSpatialVelocity) * linkCount * 2 // link velocity, (PxArticulationCacheFlag::eLINK_VELOCITY) // link acceleration (PxArticulationCacheFlag::eLINK_ACCELERATION) + sizeof(PxSpatialForce) * linkCount // link incoming joint forces (PxArticulationCacheFlag::eLINK_INCOMING_JOINT_FORCE) + sizeof(PxVec3) * linkCount * 2 // link force (PxArticulationCacheFlag::eLINK_FORCE) // link torque (PxArticulationCacheFlag::eLINK_TORQUE) + sizeof(PxArticulationRootLinkData); // root link data (PxArticulationCacheFlag::eROOT_TRANSFORM, PxArticulationCacheFlag::eROOT_VELOCITIES) return totalSize; } // AD: attention - some of the types here have 16B alignment requirements. // But the size of PxArticulationCache is not necessarily a multiple of 16B. PX_COMPILE_TIME_ASSERT(sizeof(Cm::SpatialVector)==sizeof(PxSpatialForce)); PxArticulationCache* FeatherstoneArticulation::createCache(PxU32 totalDofs, PxU32 linkCount) { const PxU32 pxArticulationCacheSize16BAligned = (sizeof(PxArticulationCache) + 15) & ~15; const PxU32 totalSize = getCacheDataSize(totalDofs, linkCount) + pxArticulationCacheSize16BAligned; PxU8* tCache = reinterpret_cast(PX_ALLOC(totalSize, "Articulation cache")); PX_ASSERT(((size_t)tCache & 15) == 0); PxMemZero(tCache, totalSize); PxArticulationCache* cache = reinterpret_cast(tCache); PxU32 offset = pxArticulationCacheSize16BAligned; // the following code assumes that the size of PxSpatialForce and PxSpatialVelocity are multiples of 16B PX_COMPILE_TIME_ASSERT((sizeof(PxSpatialForce) & 15) == 0); PX_COMPILE_TIME_ASSERT((sizeof(PxSpatialVelocity) & 15) == 0); // PT: filled in FeatherstoneArticulation::getGeneralizedExternalForce, size = mArticulationData.getLinkCount() // 16B aligned PX_ASSERT((offset & 15) == 0); cache->externalForces = reinterpret_cast(tCache + offset); offset += sizeof(PxSpatialForce) * linkCount; // PT: PxArticulationCacheFlag::eLINK_VELOCITY // 16B aligned PX_ASSERT((offset & 15) == 0); cache->linkVelocity = reinterpret_cast(tCache + offset); offset += sizeof(PxSpatialVelocity) * linkCount; // PT: PxArticulationCacheFlag::eLINK_ACCELERATION // 16B aligned PX_ASSERT((offset & 15) == 0); cache->linkAcceleration = reinterpret_cast(tCache + offset); offset += sizeof(PxSpatialVelocity) * linkCount; // PT: PxArticulationCacheFlag::eLINK_INCOMING_JOINT_FORCE // 16B aligned PX_ASSERT((offset & 15) == 0); cache->linkIncomingJointForce = reinterpret_cast(tCache + offset); offset += sizeof(PxSpatialForce) * linkCount; // PxArticulationCacheFlag::eLINK_FORCE PX_ASSERT((offset & 15) == 0); cache->linkForce = reinterpret_cast(tCache + offset); offset += sizeof(PxVec3) * linkCount; // PxArticulationCacheFlag::eLINK_TORQUE cache->linkTorque = reinterpret_cast(tCache + offset); offset += sizeof(PxVec3) * linkCount; cache->denseJacobian = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * (6 + totalDofs) * (linkCount * 6); //size of dense jacobian assuming free floating base link. cache->massMatrix = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * (totalDofs + 6) * (totalDofs + 6); cache->coriolisForce = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * (totalDofs + 6); cache->gravityCompensationForce = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * (totalDofs + 6); cache->centroidalMomentumMatrix = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * 6 * (totalDofs + 6); cache->centroidalMomentumBias = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * 6; // PT: PxArticulationCacheFlag::eVELOCITY cache->jointVelocity = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * totalDofs; // PT: PxArticulationCacheFlag::eACCELERATION cache->jointAcceleration = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * totalDofs; // PT: PxArticulationCacheFlag::ePOSITION cache->jointPosition = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * totalDofs; // PT: PxArticulationCacheFlag::eFORCE cache->jointForce = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * totalDofs; // PxArticulationCacheFlag::eJOINT_TARGET_POSITIONS cache->jointTargetPositions = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * totalDofs; // PxArticulationCacheFlag::eJOINT_TARGET_VELOCITIES cache->jointTargetVelocities = reinterpret_cast(tCache + offset); offset += sizeof(PxReal) * totalDofs; // PT: PxArticulationCacheFlag::eROOT_TRANSFORM, PxArticulationCacheFlag::eROOT_VELOCITIES cache->rootLinkData = reinterpret_cast(tCache + offset); PX_ASSERT((offset + sizeof(PxArticulationRootLinkData)) == totalSize); cache->coefficientMatrix = NULL; cache->lambda = NULL; PxU32 scratchMemorySize = sizeof(Cm::SpatialVectorF) * linkCount * 5 //motionVelocity, motionAccelerations, coriolisVectors, spatialZAVectors, externalAccels; + sizeof(Dy::SpatialMatrix) * linkCount //compositeSpatialInertias; + sizeof(PxVec3) * linkCount * 2 //linkForce, linkTorque + sizeof(PxReal) * totalDofs * 7; //jointVelocity, jointAcceleration, jointForces, jointPositions, jointFrictionForces, jointTargetPositions, jointTargetVelocities scratchMemorySize = (scratchMemorySize+15)&~15; void* scratchMemory = PX_ALLOC(scratchMemorySize, "Cache scratch memory"); cache->scratchMemory = scratchMemory; PxcScratchAllocator* sa = PX_NEW(PxcScratchAllocator); sa->setBlock(scratchMemory, scratchMemorySize); cache->scratchAllocator = sa; return cache; } static PX_FORCE_INLINE Mat33V loadPxMat33(const PxMat33& m) { return Mat33V(Vec3V_From_Vec4V(V4LoadU(&m.column0.x)), Vec3V_From_Vec4V(V4LoadU(&m.column1.x)), V3LoadU(&m.column2.x)); } static PX_FORCE_INLINE void storePxMat33(const Mat33V& src, PxMat33& dst) { V3StoreU(src.col0, dst.column0); V3StoreU(src.col1, dst.column1); V3StoreU(src.col2, dst.column2); } void FeatherstoneArticulation::transformInertia(const SpatialTransform& sTod, SpatialMatrix& spatialInertia) { #if 1 const SpatialTransform dTos = sTod.getTranspose(); Mat33V tL = loadPxMat33(spatialInertia.topLeft); Mat33V tR = loadPxMat33(spatialInertia.topRight); Mat33V bL = loadPxMat33(spatialInertia.bottomLeft); Mat33V R = loadPxMat33(sTod.R); Mat33V T = loadPxMat33(sTod.T); Mat33V tl = M33MulM33(R, tL); Mat33V tr = M33MulM33(R, tR); Mat33V bl = M33Add(M33MulM33(T, tL), M33MulM33(R, bL)); Mat33V br = M33Add(M33MulM33(T, tR), M33MulM33(R, M33Trnsps(tL))); Mat33V dR = loadPxMat33(dTos.R); Mat33V dT = loadPxMat33(dTos.T); tL = M33Add(M33MulM33(tl, dR), M33MulM33(tr, dT)); tR = M33MulM33(tr, dR); bL = M33Add(M33MulM33(bl, dR), M33MulM33(br, dT)); bL = M33Scale(M33Add(bL, M33Trnsps(bL)), FHalf()); storePxMat33(tL, spatialInertia.topLeft); storePxMat33(tR, spatialInertia.topRight); storePxMat33(bL, spatialInertia.bottomLeft); #else const SpatialTransform dTos = sTod.getTranspose(); PxMat33 tl = sTod.R * spatialInertia.topLeft; PxMat33 tr = sTod.R * spatialInertia.topRight; PxMat33 bl = sTod.T * spatialInertia.topLeft + sTod.R * spatialInertia.bottomLeft; PxMat33 br = sTod.T * spatialInertia.topRight + sTod.R * spatialInertia.getBottomRight(); spatialInertia.topLeft = tl * dTos.R + tr * dTos.T; spatialInertia.topRight = tr * dTos.R; spatialInertia.bottomLeft = bl * dTos.R + br * dTos.T; //aligned inertia spatialInertia.bottomLeft = (spatialInertia.bottomLeft + spatialInertia.bottomLeft.getTranspose()) * 0.5f; #endif } void FeatherstoneArticulation::translateInertia(const PxMat33& sTod, SpatialMatrix& inertia) { #if 1 Mat33V sTodV = loadPxMat33(sTod); Mat33V dTos = M33Trnsps(sTodV); const Mat33V tL = loadPxMat33(inertia.topLeft); const Mat33V tR = loadPxMat33(inertia.topRight); const Mat33V bL = loadPxMat33(inertia.bottomLeft); const Mat33V bl = M33Add(M33MulM33(sTodV, tL), bL); const Mat33V br = M33Add(M33MulM33(sTodV, tR), M33Trnsps(tL)); const Mat33V bottomLeft = M33Add(bl, M33MulM33(br, dTos)); storePxMat33(M33Add(tL, M33MulM33(tR, dTos)), inertia.topLeft); storePxMat33(M33Scale(M33Add(bottomLeft, M33Trnsps(bottomLeft)), FHalf()), inertia.bottomLeft); #else const PxMat33 dTos = sTod.getTranspose(); PxMat33 bl = sTod * inertia.topLeft + inertia.bottomLeft; PxMat33 br = sTod * inertia.topRight + inertia.getBottomRight(); inertia.topLeft = inertia.topLeft + inertia.topRight * dTos; inertia.bottomLeft = bl + br * dTos; //aligned inertia - make it symmetrical! OPTIONAL!!!! inertia.bottomLeft = (inertia.bottomLeft + inertia.bottomLeft.getTranspose()) * 0.5f; #endif } void FeatherstoneArticulation::getImpulseResponse( PxU32 linkID, const Cm::SpatialVector& impulse, Cm::SpatialVector& deltaVV) const { PX_ASSERT(impulse.pad0 == 0.f && impulse.pad1 == 0.f); //impulse lin is contact normal, and ang is raxn. R is body2World, R(t) is world2Body //| R(t), 0 | //| R(t)*r, R(t)| //r is the vector from center of mass to contact point //p(impluse) = |n| // |0| Cm::SpatialVectorF deltaV = mArticulationData.getImpulseResponseMatrixWorld()[linkID].getLinkDeltaVImpulseResponse(reinterpret_cast(impulse)); deltaVV.linear = deltaV.bottom; deltaVV.angular = deltaV.top; } void FeatherstoneArticulation::getImpulseResponse( PxU32 linkID, const Cm::SpatialVectorV& impulse, Cm::SpatialVectorV& deltaVV) const { #if 0 const PxTransform& body2World = mArticulationData.getPreTransform(linkID); QuatV rot = QuatVLoadU(&body2World.q.x); Cm::SpatialVectorV impl(QuatRotateInv(rot, impulse.linear), QuatRotateInv(rot, impulse.angular)); //transform p(impluse) from world space to the local space of linkId //Cm::SpatialVectorF impl(impulse.linear, impulse.angular); Cm::SpatialVectorV deltaV = mArticulationData.getImpulseResponseMatrix()[linkID].getResponse(impl); deltaVV.linear = QuatRotate(rot, deltaV.angular); deltaVV.angular = QuatRotate(rot, deltaV.linear); #else Cm::SpatialVectorV deltaV = mArticulationData.getImpulseResponseMatrixWorld()[linkID].getLinkDeltaVImpulseResponse(impulse); deltaVV.linear = deltaV.angular; deltaVV.angular = deltaV.linear; #endif } //This will return world space SpatialVectorV Cm::SpatialVectorV FeatherstoneArticulation::getLinkVelocity(const PxU32 linkID) const { //This is in the world space const Cm::SpatialVectorF& motionVelocity = mArticulationData.getMotionVelocity(linkID); Cm::SpatialVectorV velocity; velocity.linear = V3LoadA(motionVelocity.bottom); velocity.angular = V3LoadA(motionVelocity.top); return velocity; } Cm::SpatialVector FeatherstoneArticulation::getLinkScalarVelocity(const PxU32 linkID) const { //This is in the world space const Cm::SpatialVectorF& motionVelocity = mArticulationData.getMotionVelocity(linkID); return Cm::SpatialVector(motionVelocity.bottom, motionVelocity.top); } Cm::SpatialVectorV FeatherstoneArticulation::getLinkMotionVector(const PxU32 linkID) const { const Cm::SpatialVectorF& motionVector = mArticulationData.getDeltaMotionVector(linkID); Cm::SpatialVectorV velocity; velocity.linear = V3LoadU(motionVector.bottom); velocity.angular = V3LoadU(motionVector.top); return velocity; } //this is called by island gen to determine whether the articulation should be awake or sleep Cm::SpatialVector FeatherstoneArticulation::getMotionVelocity(const PxU32 linkID) const { //This is in the world space const Cm::SpatialVectorF& motionVelocity = mArticulationData.getPosIterMotionVelocities()[linkID]; return Cm::SpatialVector(motionVelocity.bottom, motionVelocity.top); } Cm::SpatialVector FeatherstoneArticulation::getMotionAcceleration(const PxU32 linkID, const bool isGpuSimEnabled) const { Cm::SpatialVector a = Cm::SpatialVector::zero(); if(mArticulationData.getDt() > 0.0f) { if(isGpuSimEnabled) { const Cm::SpatialVectorF& linkAccel = mArticulationData.mMotionAccelerations[linkID]; a = Cm::SpatialVector(linkAccel.bottom, linkAccel.top); } else { const PxReal invDt = 1.0f/mArticulationData.getDt(); const Cm::SpatialVectorF linkAccel = mArticulationData.mMotionAccelerations[linkID] + mArticulationData.mSolverLinkSpatialDeltaVels[linkID]*invDt; a = Cm::SpatialVector(linkAccel.bottom, linkAccel.top); } } return a; } PxReal FeatherstoneArticulation::getLinkMaxPenBias(const PxU32 linkID) const { return mArticulationData.getLinkData(linkID).maxPenBias; } PxReal FeatherstoneArticulation::getCfm(const PxU32 linkID) const { return mArticulationData.getLink(linkID).cfm; } void PxcFsFlushVelocity(FeatherstoneArticulation& articulation, Cm::SpatialVectorF* deltaV) { PX_ASSERT(deltaV); ArticulationData& data = articulation.mArticulationData; const bool fixBase = data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE; Cm::SpatialVectorF* motionVelocities = data.getMotionVelocities(); //Cm::SpatialVectorF* deferredZ = data.getSpatialZAVectors(); ArticulationLink* links = data.getLinks(); ArticulationJointCoreData* jointData = data.getJointData(); //PxTransform* poses = data.getAccumulatedPoses(); //const PxTransform* poses = data.getPreTransform(); //This will be zero at the beginning of the frame PxReal* jointNewVelocities = data.getJointNewVelocities(); if (fixBase) { deltaV[0] = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f)); } else { //ArticulationLink& link = links[0]; deltaV[0] = data.getBaseInvSpatialArticulatedInertiaW() * -data.getRootDeferredZ(); motionVelocities[0] += deltaV[0]; data.incrementSolverSpatialDeltaVel(0, deltaV[0]); PX_ASSERT(motionVelocities[0].isFinite()); } const PxU32 linkCount = data.getLinkCount(); for (PxU32 i = 1; i < linkCount; i++) { const ArticulationLink& tLink = links[i]; const ArticulationJointCoreData& tJointDatum = jointData[i]; const Cm::SpatialVectorF dV = propagateAccelerationW( data.getRw(i), deltaV[tLink.parent], data.getInvStIs(i), &data.getWorldMotionMatrix(tJointDatum.jointOffset), &data.getIsW(tJointDatum.jointOffset), &data.getDeferredQstZ()[tJointDatum.jointOffset], tJointDatum.nbDof, &jointNewVelocities[tJointDatum.jointOffset]); deltaV[i] = dV; motionVelocities[i] += dV; data.incrementSolverSpatialDeltaVel(i, dV); PX_ASSERT(motionVelocities[i].isFinite()); } //PxMemZero(deferredZ, sizeof(Cm::SpatialVectorF)*linkCount); PxMemZero(data.getDeferredQstZ(), sizeof(PxReal) * data.getDofs()); data.getRootDeferredZ() = Cm::SpatialVectorF::Zero(); } // Apply substep external forces and joint actuation, propagate from leaves to root // Gravity and external force/acceleration fullstep application happens in FeatherstoneArticulation::computeLinkStates // Joint force fullstep application happens in FeatherstoneArticulation::computePropagateSpatialInertia_ZA_ZIc void FeatherstoneArticulation::applyTgsSubstepForces(const ArticulationSolverDesc& desc, const PxReal stepDt, Cm::SpatialVectorF* scratchExtForcesArticulatedYW) { FeatherstoneArticulation* articulation = desc.articulation; ArticulationData& data = articulation->mArticulationData; // inputs const ArticulationLink* links = data.getLinks(); const PxU32 linkCount = data.getLinkCount(); const Cm::UnAlignedSpatialVector* jointDofMotionMatrixW = data.getWorldMotionMatrix(); const Cm::SpatialVectorF* jointDofISInvStISW = data.getISInvStIS(); const PxVec3* childToParentRW = data.getRw(); const ArticulationJointCoreData* jointData = data.getJointData(); const bool fixBase = data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE; const PxReal* jointForces = data.getJointForces(); const Cm::SpatialVector* linkExternalAcceleration = data.mExternalAcceleration; // we have converted to spatial forces here already, see FeatherstoneArticulation::computeLinkStates // outputs PxReal* jointDofQStY = data.getDeferredQstZ(); Cm::SpatialVectorF& rootDeferredZ = data.getRootDeferredZ(); data.mJointDirty = true; // Scratch only temporarily needed until this function returns, passed in from outside to avoid allocating here. // We set it to zero because it might be in a dirty state. PX_ASSERT(scratchExtForcesArticulatedYW); Cm::SpatialVectorF* extForcesArticulatedYW = scratchExtForcesArticulatedYW; for(PxU32 linkID = 0; linkID < linkCount; linkID++) { extForcesArticulatedYW[linkID] = Cm::SpatialVectorF::Zero(); } // Iterate from leaves to root, accumulating the substep forces in articulatedY up the tree // This is like FeatherstoneArticulation::propagateImpulseW but with impulses on all links const PxI32 startIndex = linkCount - 1; for (PxI32 linkID = startIndex; linkID > 0; --linkID) { const ArticulationLink& link = links[linkID]; const Cm::SpatialVector& extForceStoredAsMotionVec = linkExternalAcceleration[linkID]; const Cm::SpatialVectorF isolatedYW(extForceStoredAsMotionVec.linear, extForceStoredAsMotionVec.angular); // order defined in FeatherstoneArticulation::computeLinkStates const Cm::SpatialVectorF articulatedYW = isolatedYW * stepDt + extForcesArticulatedYW[linkID]; // could avoid adding this for leaf links const ArticulationJointCoreData& jointDatum = jointData[linkID]; const PxU32 jointOffset = jointDatum.jointOffset; const PxU8 dofCount = jointDatum.nbDof; Cm::SpatialVectorF parentYW = articulatedYW; for(PxU8 jointDof = 0; jointDof < dofCount; jointDof++) { const PxU32 jointIdx = jointOffset + jointDof; const Cm::UnAlignedSpatialVector& sa = jointDofMotionMatrixW[jointIdx]; const Cm::SpatialVectorF& ISInvStISW = jointDofISInvStISW[jointIdx]; const PxReal jointForce = jointForces[jointIdx]; const PxReal stZY = jointForce * stepDt - sa.innerProduct(articulatedYW); PX_ASSERT(PxIsFinite(stZY)); parentYW += ISInvStISW * stZY; jointDofQStY[jointIdx] += stZY; // store as deferred impulse in joint space } const PxU32 parentLinkID = link.parent; if(parentLinkID == 0 && fixBase) continue; // no need to propagate to a fixed base parentYW = FeatherstoneArticulation::translateSpatialVector(childToParentRW[linkID], parentYW); extForcesArticulatedYW[parentLinkID] += parentYW; // add rather than assign because other children may have already contributed } // process the root if it's not fixed if(!fixBase) { const Cm::SpatialVector& extForceStoredAsMotionVec = linkExternalAcceleration[0]; const Cm::SpatialVectorF isolatedYW(extForceStoredAsMotionVec.linear, extForceStoredAsMotionVec.angular); // order defined in FeatherstoneArticulation::computeLinkStates const Cm::SpatialVectorF articulatedYW = isolatedYW * stepDt + extForcesArticulatedYW[0]; rootDeferredZ += articulatedYW; } } void FeatherstoneArticulation::recordDeltaMotion(const ArticulationSolverDesc& desc, const PxReal dt, Cm::SpatialVectorF* deltaV, const PxReal /*totalInvDt*/) { PX_ASSERT(deltaV); FeatherstoneArticulation* articulation = static_cast(desc.articulation); ArticulationData& data = articulation->mArticulationData; const PxU32 linkCount = data.getLinkCount(); const PxU32 flags = data.getArticulationFlags(); if (data.mJointDirty) { PxcFsFlushVelocity(*articulation, deltaV); } Cm::SpatialVectorF* deltaMotion = data.getDeltaMotionVector(); Cm::SpatialVectorF* posMotionVelocities = data.getPosIterMotionVelocities(); Cm::SpatialVectorF* motionVelocities = data.getMotionVelocities(); PxReal* jointPosition = data.getJointPositions(); PxReal* jointNewVelocities = data.getJointNewVelocities(); //data.mAccumulatedDt += dt; data.setDt(dt); const bool fixBase = flags & PxArticulationFlag::eFIX_BASE; if (!fixBase) { Cm::SpatialVectorF& motionVelocity = motionVelocities[0]; PX_ASSERT(motionVelocity.top.isFinite()); PX_ASSERT(motionVelocity.bottom.isFinite()); const PxTransform preTrans = data.mAccumulatedPoses[0]; const PxVec3 lin = motionVelocity.bottom; const PxVec3 ang = motionVelocity.top; const PxVec3 newP = preTrans.p + lin * dt; const PxTransform newPose = PxTransform(newP, PxExp(ang*dt) * preTrans.q); //PxVec3 lin, ang; /*calculateNewVelocity(newPose, data.mPreTransform[0], 1.f, lin, ang); */ data.mAccumulatedPoses[0] = newPose; PxQuat dq = newPose.q * data.mPreTransform[0].q.getConjugate(); if (dq.w < 0.f) dq = -dq; data.mDeltaQ[0] = dq; Cm::SpatialVectorF delta = motionVelocity * dt; deltaMotion[0] += delta; posMotionVelocities[0] += delta; } for (PxU32 linkID = 1; linkID < linkCount; linkID++) { ArticulationJointCoreData& jointDatum = data.getJointData(linkID); const PxTransform newPose = articulation->propagateTransform(linkID, data.getLinks(), jointDatum, data.getMotionVelocities(), dt, data.mAccumulatedPoses[data.getLink(linkID).parent], data.mAccumulatedPoses[linkID], jointNewVelocities, jointPosition, &data.getMotionMatrix(jointDatum.jointOffset), &data.getWorldMotionMatrix(jointDatum.jointOffset)); //data.mDeltaQ[linkID] = data.mPreTransform[linkID].q.getConjugate() * newPose.q; PxQuat dq = newPose.q * data.mPreTransform[linkID].q.getConjugate(); if(dq.w < 0.f) dq = -dq; data.mDeltaQ[linkID] = dq; /*PxVec3 lin, ang; calculateNewVelocity(newPose, data.mPreTransform[linkID], 1.f, lin, ang);*/ PxVec3 lin = (newPose.p - data.mPreTransform[linkID].p); Cm::SpatialVectorF delta = motionVelocities[linkID] * dt; //deltaMotion[linkID].top = ang;// motionVeloties[linkID].top * dt; deltaMotion[linkID].top += delta.top; deltaMotion[linkID].bottom = lin;// motionVeloties[linkID].top * dt; posMotionVelocities[linkID] += delta; //Record the new current pose data.mAccumulatedPoses[linkID] = newPose; } } void FeatherstoneArticulation::deltaMotionToMotionVelocity(const ArticulationSolverDesc& desc, PxReal invDt) { FeatherstoneArticulation* articulation = static_cast(desc.articulation); ArticulationData& data = articulation->mArticulationData; const PxU32 linkCount = data.getLinkCount(); const Cm::SpatialVectorF* deltaMotion = data.getDeltaMotionVector(); for (PxU32 linkID = 0; linkID(delta); } } Cm::SpatialVectorV FeatherstoneArticulation::pxcFsGetVelocity(const PxU32 linkID, PxReal* jointDofSpeeds) const { const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE; ArticulationLink* links = mArticulationData.getLinks(); Cm::SpatialVectorF deltaV(PxVec3(0.f), PxVec3(0.f)); if (!fixBase) { //deltaV = mArticulationData.mBaseInvSpatialArticulatedInertia * (-deferredZ[0]); //DeferredZ now in world space! deltaV = mArticulationData.mBaseInvSpatialArticulatedInertiaW * -mArticulationData.getRootDeferredZ(); } const PxU32 startIndex = links[linkID].mPathToRootStartIndex; const PxU32 elementCount = links[linkID].mPathToRootCount; //Take care of the case where elementCount is 0. const PxU32 elementCountMinusOne = (0 == elementCount) ? 0 : elementCount -1; const PxU32* pathToRootElements = &mArticulationData.mPathToRootElements[startIndex]; //We don't need to compute the deltaJointDofSpeed for these links and joints. for (PxU32 i = 0; i < elementCountMinusOne; ++i) { const PxU32 index = pathToRootElements[i]; PX_ASSERT(links[index].parent < index); const PxU32 jointOffset = mArticulationData.getJointData(index).jointOffset; const PxU32 dofCount = mArticulationData.getJointData(index).nbDof; deltaV = propagateAccelerationW( mArticulationData.getRw(index), deltaV, mArticulationData.mInvStIs[index], &mArticulationData.mWorldMotionMatrix[jointOffset], &mArticulationData.mIsW[jointOffset], &mArticulationData.mDeferredQstZ[jointOffset], dofCount, NULL); } //The last link might need to compute the deltaJointDofSpeed. PxReal deltaJointDofSpeeds[3] = {0, 0, 0}; PxReal* optionalDeltaJointDofSpeeds = jointDofSpeeds ? deltaJointDofSpeeds : NULL; for (PxU32 i = elementCountMinusOne; i < elementCount; ++i) { const PxU32 index = pathToRootElements[i]; PX_ASSERT(links[index].parent < index); const PxU32 jointOffset = mArticulationData.getJointData(index).jointOffset; const PxU32 dofCount = mArticulationData.getJointData(index).nbDof; deltaV = propagateAccelerationW( mArticulationData.getRw(index), deltaV, mArticulationData.mInvStIs[index], &mArticulationData.mWorldMotionMatrix[jointOffset], &mArticulationData.mIsW[jointOffset], &mArticulationData.mDeferredQstZ[jointOffset], dofCount, optionalDeltaJointDofSpeeds); } if(jointDofSpeeds) { const PxU32 jointOffset = mArticulationData.getJointData(linkID).jointOffset; const PxU32 dofCount = mArticulationData.getJointData(linkID).nbDof; for(PxU32 i = 0; i < dofCount; i++) { jointDofSpeeds[i] = mArticulationData.mJointNewVelocity[jointOffset + i] + deltaJointDofSpeeds[i]; } } const Cm::SpatialVectorF vel = mArticulationData.getMotionVelocity(linkID) + deltaV; return Cm::SpatialVector(vel.bottom, vel.top); } void FeatherstoneArticulation::pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1) const { { const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE; ArticulationLink* links = mArticulationData.getLinks(); Cm::SpatialVectorF deltaV(PxVec3(0.f), PxVec3(0.f)); if (!fixBase) { //deltaV = mArticulationData.mBaseInvSpatialArticulatedInertia * (-deferredZ[0]); deltaV = mArticulationData.mBaseInvSpatialArticulatedInertiaW * (-mArticulationData.mRootDeferredZ); } const PxU32* pathToRootElements = mArticulationData.mPathToRootElements; Dy::ArticulationLink& link0 = links[linkID]; Dy::ArticulationLink& link1 = links[linkID1]; const PxU32* pathToRoot0 = &pathToRootElements[link0.mPathToRootStartIndex]; const PxU32* pathToRoot1 = &pathToRootElements[link1.mPathToRootStartIndex]; const PxU32 numElems0 = link0.mPathToRootCount; const PxU32 numElems1 = link1.mPathToRootCount; PxU32 offset = 0; while (pathToRoot0[offset] == pathToRoot1[offset]) { const PxU32 index = pathToRoot0[offset++]; PX_ASSERT(links[index].parent < index); if (offset >= numElems0 || offset >= numElems1) break; const PxU32 jointOffset = mArticulationData.getJointData(index).jointOffset; const PxU32 dofCount = mArticulationData.getJointData(index).nbDof; deltaV = propagateAccelerationW( mArticulationData.getRw(index), deltaV, mArticulationData.mInvStIs[index], &mArticulationData.mWorldMotionMatrix[jointOffset], &mArticulationData.mIsW[jointOffset], &mArticulationData.mDeferredQstZ[jointOffset], dofCount, NULL); } Cm::SpatialVectorF deltaV1 = deltaV; for (PxU32 idx = offset; idx < numElems0; ++idx) { const PxU32 index = pathToRoot0[idx]; PX_ASSERT(links[index].parent < index); const PxU32 jointOffset = mArticulationData.getJointData(index).jointOffset; const PxU32 dofCount = mArticulationData.getJointData(index).nbDof; deltaV = propagateAccelerationW( mArticulationData.getRw(index), deltaV, mArticulationData.mInvStIs[index], &mArticulationData.mWorldMotionMatrix[jointOffset], &mArticulationData.mIsW[jointOffset], &mArticulationData.mDeferredQstZ[jointOffset], dofCount, NULL); } for (PxU32 idx = offset; idx < numElems1; ++idx) { const PxU32 index = pathToRoot1[idx]; PX_ASSERT(links[index].parent < index); const PxU32 jointOffset = mArticulationData.getJointData(index).jointOffset; const PxU32 dofCount = mArticulationData.getJointData(index).nbDof; deltaV1 = propagateAccelerationW( mArticulationData.getRw(index), deltaV1, mArticulationData.mInvStIs[index], &mArticulationData.mWorldMotionMatrix[jointOffset], &mArticulationData.mIsW[jointOffset], &mArticulationData.mDeferredQstZ[jointOffset], dofCount, NULL); } Cm::SpatialVectorF vel = mArticulationData.getMotionVelocity(linkID) + deltaV; v0 = Cm::SpatialVector(vel.bottom, vel.top); Cm::SpatialVectorF vel1 = mArticulationData.getMotionVelocity(linkID1) + deltaV1; v1 = Cm::SpatialVector(vel1.bottom, vel1.top); } } /*Cm::SpatialVectorV FeatherstoneArticulation::pxcFsGetVelocity(PxU32 linkID) { Cm::SpatialVectorF& vel = mArticulationData.getMotionVelocity(linkID); return Cm::SpatialVector(vel.bottom, vel.top); }*/ Cm::SpatialVectorV FeatherstoneArticulation::pxcFsGetVelocityTGS(PxU32 linkID) { return getLinkVelocity(linkID); } //This is used in the solveExt1D, solveExtContact void FeatherstoneArticulation::pxcFsApplyImpulse(PxU32 linkID, aos::Vec3V linkImpulseLinear, aos::Vec3V linkImpulseAngular, const PxReal* jointImpulse) { const ArticulationSolverDesc* desc = &mSolverDesc; const ArticulationLink* links = desc->links; ArticulationData& data = mArticulationData; data.mJointDirty = true; //impulse is in world space Cm::SpatialVector impulse; V4StoreA(Vec4V_From_Vec3V(linkImpulseAngular), &impulse.angular.x); V4StoreA(Vec4V_From_Vec3V(linkImpulseLinear), &impulse.linear.x); Cm::SpatialVectorF Z0(-impulse.linear, -impulse.angular); for (PxU32 i = linkID; i; i = links[i].parent) { const PxU32 jointOffset = data.getJointData(i).jointOffset; const PxU8 dofCount = data.getJointData(i).nbDof; data.mSolverLinkSpatialImpulses[i] += Z0; //Only apply the joint impulse to the inbound joint of linkID. const PxReal* jointImpulseToApply = (linkID == i) ? jointImpulse : NULL; Z0 = propagateImpulseW( data.getRw(i), Z0, jointImpulseToApply, &data.mISInvStIS[jointOffset], &data.mWorldMotionMatrix[jointOffset], dofCount, &data.mDeferredQstZ[jointOffset]); } data.mRootDeferredZ += Z0; } void FeatherstoneArticulation::pxcFsApplyImpulses( PxU32 linkID1, const aos::Vec3V& linear1, const aos::Vec3V& angular1, const PxReal* jointImpulse1, PxU32 linkID2, const aos::Vec3V& linear2, const aos::Vec3V& angular2, const PxReal* jointImpulse2) { if (0) { pxcFsApplyImpulse(linkID1, linear1, angular1, jointImpulse1); pxcFsApplyImpulse(linkID2, linear2, angular2, jointImpulse2); } else { const ArticulationSolverDesc* desc = &mSolverDesc; ArticulationData& data = mArticulationData; data.mJointDirty = true; ArticulationLink* links = static_cast(desc->links); //impulse is in world space Cm::SpatialVector impulse0; V3StoreU(angular1, impulse0.angular); V3StoreU(linear1, impulse0.linear); Cm::SpatialVector impulse1; V3StoreU(angular2, impulse1.angular); V3StoreU(linear2, impulse1.linear); Cm::SpatialVectorF Z1(-impulse0.linear, -impulse0.angular); Cm::SpatialVectorF Z2(-impulse1.linear, -impulse1.angular); ArticulationLink& link1 = links[linkID1]; ArticulationLink& link2 = links[linkID2]; const PxU32* pathToRoot1 = &mArticulationData.mPathToRootElements[link1.mPathToRootStartIndex]; const PxU32* pathToRoot2 = &mArticulationData.mPathToRootElements[link2.mPathToRootStartIndex]; const PxU32 numElems1 = link1.mPathToRootCount; const PxU32 numElems2 = link2.mPathToRootCount; //find the common link, work from one to that common, then the other to that common, then go from there upwards... PxU32 offset = 0; PxU32 commonLink = 0; while (pathToRoot1[offset] == pathToRoot2[offset]) { commonLink = pathToRoot1[offset++]; PX_ASSERT(links[commonLink].parent < commonLink); if (offset >= numElems1 || offset >= numElems2) break; } //The common link will either be linkID2, or its ancestors. //The common link cannot be an index before either linkID2 or linkID for (PxU32 i = linkID2; i != commonLink; i = links[i].parent) { const PxU32 jointOffset = mArticulationData.getJointData(i).jointOffset; const PxU8 dofCount = mArticulationData.getJointData(i).nbDof; //Note: linkID2 might be the common link. If this is the case, we will only apply //jointImpulse2 when we propagate from the common link to the root. //Watch out for that when we propagate from the common link. const PxReal* jointImpulseToApply = (linkID2 == i) ? jointImpulse2 : NULL; data.mSolverLinkSpatialImpulses[i] += Z2; Z2 = propagateImpulseW( mArticulationData.getRw(i), Z2, jointImpulseToApply, &data.mISInvStIS[jointOffset], &data.mWorldMotionMatrix[jointOffset], dofCount, &data.mDeferredQstZ[jointOffset]); } for (PxU32 i = linkID1; i != commonLink; i = links[i].parent) { const PxU32 jointOffset = mArticulationData.getJointData(i).jointOffset; const PxU8 dofCount = mArticulationData.getJointData(i).nbDof; //Note: linkID1 might be the common link. If this is the case, we will only apply //jointImpulse1 when we propagate from the common link to the root. //Watch out for that when we propagate from the common link. const PxReal* jointImpulseToApply = (linkID1 == i) ? jointImpulse1 : NULL; data.mSolverLinkSpatialImpulses[i] += Z1; Z1 = propagateImpulseW( mArticulationData.getRw(i), Z1, jointImpulseToApply, &data.mISInvStIS[jointOffset], &data.mWorldMotionMatrix[jointOffset],dofCount, &data.mDeferredQstZ[jointOffset]); } //If linkID1 (or linkID2) is the common link then we will not yet have applied //jointImpulse1 (or jointImpulse2) to the inbound joint of the link. //Work out how much joint impulse to apply to the inbound joint of the common link. PxReal jointImpulseToApplyAtCommonLink[3] = {0, 0, 0}; if(((linkID1 == commonLink) && jointImpulse1) || ((linkID2 == commonLink) && jointImpulse2)) { const PxU32 linkIndices[2] = {linkID1, linkID2}; const PxReal* jointImpulses[2]= {jointImpulse1, jointImpulse2}; const PxU32 dofCountAtCommonLink = mArticulationData.getJointData(commonLink).nbDof; for(PxU32 k = 0; k < 2; k++) { const PxU32 linkId = linkIndices[k]; const PxReal* jointImpulse = jointImpulses[k]; if((linkId == commonLink) && jointImpulse) { for(PxU32 i = 0; i < dofCountAtCommonLink; i++) { jointImpulseToApplyAtCommonLink[i] += jointImpulse[i]; } } } } Cm::SpatialVectorF ZCommon = Z1 + Z2; for (PxU32 i = commonLink; i; i = links[i].parent) { const PxU32 jointOffset = mArticulationData.getJointData(i).jointOffset; const PxU8 dofCount = mArticulationData.getJointData(i).nbDof; //Only apply a joint impulse to the inbound joint of commonLink. //The joint impulse to apply to the inbound joint will only be non-zero if the common link //is either linkID1 or linkID2. const PxReal* jointImpulseToApply = (commonLink == i) ? jointImpulseToApplyAtCommonLink : NULL; data.mSolverLinkSpatialImpulses[i] += ZCommon; ZCommon = propagateImpulseW( mArticulationData.getRw(i), ZCommon, jointImpulseToApply, &data.mISInvStIS[jointOffset], &data.mWorldMotionMatrix[jointOffset], dofCount, &data.mDeferredQstZ[jointOffset]); } data.mRootDeferredZ += ZCommon; } } //This version uses in updateBodies PxQuat computeSphericalJointPositions(const PxQuat& relativeQuat, const PxQuat& newRot, const PxQuat& pBody2WorldRot, PxReal* jPositions, const Cm::UnAlignedSpatialVector* motionMatrix, const PxU32 dofs); PxQuat computeSphericalJointPositions(const PxQuat& relativeQuat, const PxQuat& newRot, const PxQuat& pBody2WorldRot); PxTransform FeatherstoneArticulation::propagateTransform(const PxU32 linkID, ArticulationLink* links, ArticulationJointCoreData& jointDatum, Cm::SpatialVectorF* motionVelocities, const PxReal dt, const PxTransform& pBody2World, const PxTransform& currentTransform, PxReal* jointVelocities, PxReal* jointPositions, const Cm::UnAlignedSpatialVector* motionMatrix, const Cm::UnAlignedSpatialVector* /*worldMotionMatrix*/) { ArticulationLink& link = links[linkID]; const PxQuat relativeQuat = mArticulationData.mRelativeQuat[linkID]; ArticulationJointCore* joint = link.inboundJoint; PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset]; PxReal* jPosition = &jointPositions[jointDatum.jointOffset]; PxQuat newParentToChild; PxQuat newWorldQ; PxVec3 r; const PxVec3 childOffset = -joint->childPose.p; const PxVec3 parentOffset = joint->parentPose.p; switch (joint->jointType) { case PxArticulationJointType::ePRISMATIC: { PxReal tJointPosition = jPosition[0] + (jVelocity[0]) * dt; const PxU32 dofId = link.inboundJoint->dofIds[0]; if (link.inboundJoint->motion[dofId] == PxArticulationMotion::eLIMITED) { if (tJointPosition < (link.inboundJoint->limits[dofId].low)) tJointPosition = link.inboundJoint->limits[dofId].low; if (tJointPosition >(link.inboundJoint->limits[dofId].high)) tJointPosition = link.inboundJoint->limits[dofId].high; } jPosition[0] = tJointPosition; newParentToChild = relativeQuat; const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d + motionMatrix[0].bottom * tJointPosition; break; } case PxArticulationJointType::eREVOLUTE: case PxArticulationJointType::eREVOLUTE_UNWRAPPED: { PxReal tJointPosition = jPosition[0] + (jVelocity[0]) * dt; /*PxU8 dofId = link.inboundJoint->dofIds[0]; if (link.inboundJoint->motion[dofId] == PxArticulationMotion::eLIMITED) { if (tJointPosition < (link.inboundJoint->limits[dofId].low)) tJointPosition = link.inboundJoint->limits[dofId].low; if (tJointPosition >(link.inboundJoint->limits[dofId].high)) tJointPosition = link.inboundJoint->limits[dofId].high; }*/ jPosition[0] = tJointPosition; const PxVec3& u = motionMatrix[0].top; PxQuat jointRotation = PxQuat(-tJointPosition, u); if (jointRotation.w < 0) //shortest angle. jointRotation = -jointRotation; newParentToChild = (jointRotation * relativeQuat).getNormalized(); const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; PX_ASSERT(r.isFinite()); break; } case PxArticulationJointType::eSPHERICAL: { Cm::SpatialVectorF worldVel = motionVelocities[linkID]; const PxTransform oldTransform = currentTransform; PxVec3 worldAngVel = worldVel.top; //PxVec3 worldAngVel = motionVelocities[linkID].top; PxReal dist = worldAngVel.normalize() * dt; if (dist > 1e-6f) newWorldQ = PxQuat(dist, worldAngVel) * oldTransform.q; else newWorldQ = oldTransform.q; //newWorldQ = Ps::exp(worldAngVel*dt) * oldTransform.q; //PxVec3 axis; newParentToChild = computeSphericalJointPositions(mArticulationData.mRelativeQuat[linkID], newWorldQ, pBody2World.q); PxQuat jointRotation = newParentToChild * relativeQuat.getConjugate(); if(jointRotation.w < 0.0f) jointRotation = -jointRotation; /*PxVec3 axis = jointRotation.getImaginaryPart(); for (PxU32 i = 0; i < jointDatum.dof; ++i) { PxVec3 sa = mArticulationData.getMotionMatrix(jointDatum.jointOffset + i).top; PxReal angle = -compAng(sa.dot(axis), jointRotation.w); jPosition[i] = angle; }*/ PxVec3 axis; PxReal angle; jointRotation.toRadiansAndUnitAxis(angle, axis); axis *= angle; for (PxU32 i = 0; i < jointDatum.nbDof; ++i) { PxVec3 sa = mArticulationData.getMotionMatrix(jointDatum.jointOffset + i).top; PxReal ang = -sa.dot(axis); jPosition[i] = ang; } const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; PX_ASSERT(r.isFinite()); break; } case PxArticulationJointType::eFIX: { //this is fix joint so joint don't have velocity newParentToChild = relativeQuat; const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; break; } default: break; } PxTransform cBody2World; cBody2World.q = (pBody2World.q * newParentToChild.getConjugate()).getNormalized(); cBody2World.p = pBody2World.p + cBody2World.q.rotate(r); PX_ASSERT(cBody2World.isSane()); return cBody2World; } const PxQuat& FeatherstoneArticulation::getDeltaQ(PxU32 linkID) const { return mArticulationData.mDeltaQ[linkID]; } Cm::SpatialVectorF FeatherstoneArticulation::propagateVelocityW( const PxVec3& parentToChild, const Cm::SpatialVectorF& parentLinkDeltaV, const Dy::SpatialMatrix& spatialInertia, const Cm::SpatialVectorF& Z, const PxReal* jointDofImpulses, const InvStIs& invStIs, const Cm::UnAlignedSpatialVector* motionMatrixW, const PxU32 dofCount, PxReal* jointVelocity) { const Cm::SpatialVectorF pDeltaV = translateSpatialVector(-parentToChild, parentLinkDeltaV); //parent velocity change // -s_i^T * [I_i^A * translated(vParent) + Z] const Cm::SpatialVectorF temp = spatialInertia * pDeltaV + Z; PxReal tJointDelta[6]; for (PxU32 ind = 0; ind < dofCount; ++ind) { const Cm::UnAlignedSpatialVector& sa = motionMatrixW[ind]; const PxReal jointDofImpulse = jointDofImpulses ? jointDofImpulses[ind] : 0.0f; tJointDelta[ind] = jointDofImpulse - sa.innerProduct(temp); } //qdot = [1/ s_i^T * i_i^A * s_i] * { -[s_i^T * I_i^A * translated(vParent) + Z]} //deltaV = qdot * s^T_i Cm::SpatialVectorF jointSpatialDeltaV(PxVec3(0.f), PxVec3(0.f)); for (PxU32 ind = 0; ind < dofCount; ++ind) { PxReal jDelta = 0.f; for (PxU32 ind2 = 0; ind2 < dofCount; ++ind2) { jDelta += invStIs.invStIs[ind2][ind] * tJointDelta[ind2]; } if(jointVelocity) jointVelocity[ind] += jDelta; jointSpatialDeltaV.top += motionMatrixW[ind].top * jDelta; jointSpatialDeltaV.bottom += motionMatrixW[ind].bottom * jDelta; } //deltaV = translated(parentLinkDeltaV) + qdot * s^T_i return pDeltaV + jointSpatialDeltaV; } Cm::SpatialVectorF FeatherstoneArticulation::getDeltaVWithDeltaJV(const bool fixBase, const PxU32 linkID, const ArticulationData& data, Cm::SpatialVectorF* Z, PxReal* jointVelocities) { Cm::SpatialVectorF deltaV = Cm::SpatialVectorF::Zero(); if (!fixBase) { //velocity change //SpatialMatrix inverseArticulatedInertia = hLinkDatum.spatialArticulatedInertia.getInverse(); const SpatialMatrix& inverseArticulatedInertia = data.mBaseInvSpatialArticulatedInertiaW; deltaV = inverseArticulatedInertia * (-Z[0]); } ArticulationLink* links = data.getLinks(); const ArticulationLink& link = links[linkID]; const PxU32* pathToRoot = &data.mPathToRootElements[link.mPathToRootStartIndex]; const PxU32 numElems = link.mPathToRootCount; for (PxU32 i = 0; i < numElems; ++i) { const PxU32 index = pathToRoot[i]; PX_ASSERT(links[index].parent < index); ArticulationJointCoreData& tJointDatum = data.getJointData(index); PxReal* jVelocity = &jointVelocities[tJointDatum.jointOffset]; deltaV = FeatherstoneArticulation::propagateVelocityW( data.getRw(index), deltaV, data.mWorldSpatialArticulatedInertia[index], Z[index], NULL, data.mInvStIs[index], &data.mWorldMotionMatrix[tJointDatum.jointOffset], tJointDatum.nbDof, jVelocity); } return deltaV; } void FeatherstoneArticulation::getZ(const PxU32 linkID, const ArticulationData& data, Cm::SpatialVectorF* Z, const Cm::SpatialVectorF& impulse) { ArticulationLink* links = data.getLinks(); //impulse need to be in linkID space!!! Z[linkID] = -impulse; for (PxU32 i = linkID; i; i = links[i].parent) { ArticulationLink& tLink = links[i]; const PxU32 jointOffset = data.getJointData(i).jointOffset; const PxU8 dofCount = data.getJointData(i).nbDof; Z[tLink.parent] = propagateImpulseW( data.getRw(i), Z[i], NULL, &data.mISInvStIS[jointOffset], &data.mWorldMotionMatrix[jointOffset], dofCount, NULL); } } Cm::SpatialVectorF FeatherstoneArticulation::getImpulseResponseW( const PxU32 linkID, const ArticulationData& data, const Cm::SpatialVectorF& impulse) { return data.getImpulseResponseMatrixWorld()[linkID].getLinkDeltaVImpulseResponse(impulse); } //This method use in impulse self response. The input impulse is in the link space Cm::SpatialVectorF FeatherstoneArticulation::getImpulseResponseWithJ( const PxU32 linkID, const bool fixBase, const ArticulationData& data, Cm::SpatialVectorF* Z, const Cm::SpatialVectorF& impulse, PxReal* jointVelocites) { getZ(linkID, data, Z, impulse); return getDeltaVWithDeltaJV(fixBase, linkID, data, Z, jointVelocites); } void FeatherstoneArticulation::saveVelocity(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* deltaV) { ArticulationData& data = articulation->mArticulationData; //update all links' motion velocity, joint delta velocity if there are contacts/constraints if (data.mJointDirty) { PxcFsFlushVelocity(*articulation, deltaV); } const PxU32 linkCount = data.getLinkCount(); //copy motion velocites Cm::SpatialVectorF* vels = data.getMotionVelocities(); Cm::SpatialVectorF* posVels = data.getPosIterMotionVelocities(); PxMemCopy(posVels, vels, sizeof(Cm::SpatialVectorF) * linkCount); //copy joint velocities const PxU32 dofs = data.getDofs(); PxReal* jPosVels = data.getPosIterJointVelocities(); const PxReal* jNewVels = data.getJointNewVelocities(); PxMemCopy(jPosVels, jNewVels, sizeof(PxReal) * dofs); articulation->concludeInternalConstraints(false); } void FeatherstoneArticulation::saveVelocityTGS(FeatherstoneArticulation* articulation, PxReal invDtF32) { ArticulationData& data = articulation->mArticulationData; //KS - we should not need to flush velocity because we will have already stepped the articulation with TGS const PxU32 linkCount = data.getLinkCount(); Cm::SpatialVectorF* posVels = data.getPosIterMotionVelocities(); for (PxU32 i = 0; i < linkCount; ++i) { posVels[i] = posVels[i] * invDtF32; } } void FeatherstoneArticulation::getImpulseSelfResponse( PxU32 linkID0, PxU32 linkID1, const Cm::SpatialVector& impulse0, const Cm::SpatialVector& impulse1, Cm::SpatialVector& deltaV0, Cm::SpatialVector& deltaV1) const { FeatherstoneArticulation::getImpulseSelfResponse(mArticulationData.getLinks(), const_cast(mArticulationData), linkID0, reinterpret_cast(impulse0), reinterpret_cast(deltaV0), linkID1, reinterpret_cast(impulse1), reinterpret_cast(deltaV1)); } void FeatherstoneArticulation::getImpulseResponseSlow(Dy::ArticulationLink* links, ArticulationData& data, PxU32 linkID0_, const Cm::SpatialVector& impulse0, Cm::SpatialVector& deltaV0, PxU32 linkID1_, const Cm::SpatialVector& impulse1, Cm::SpatialVector& deltaV1) { const PxU32 linkCount = data.getLinkCount(); PX_ALLOCA(_stack, PxU32, linkCount); PxU32* stack = _stack; PxU32 i0, i1;//, ic; PxU32 linkID0 = linkID0_; PxU32 linkID1 = linkID1_; for (i0 = linkID0, i1 = linkID1; i0 != i1;) // find common path { if (i0 i0;) { //Dy::ArticulationLinkData& tLinkDatum = data.getLinkData(stack[index]); const PxU32 id = stack[index]; const PxU32 jointOffset = data.getJointData(id).jointOffset; const PxU32 dofCount = data.getJointData(id).nbDof; dv1 = propagateAccelerationW( data.getRw(id), dv1, data.mInvStIs[id], &data.mWorldMotionMatrix[jointOffset], &data.mIsW[jointOffset], &qstZ[jointOffset], dofCount, NULL); } Cm::SpatialVectorF dv0= v; for (PxU32 index = i0; (index--) > 0;) { const PxU32 id = stack[index]; const PxU32 jointOffset = data.getJointData(id).jointOffset; const PxU32 dofCount = data.getJointData(id).nbDof; dv0 = propagateAccelerationW( data.getRw(id), dv0, data.mInvStIs[id], &data.mWorldMotionMatrix[jointOffset], &data.mIsW[jointOffset], &qstZ[jointOffset], dofCount, NULL); } deltaV0.linear = dv0.bottom; deltaV0.angular = dv0.top; deltaV1.linear = dv1.bottom; deltaV1.angular = dv1.top; } void FeatherstoneArticulation::getImpulseSelfResponse(ArticulationLink* links, ArticulationData& data, PxU32 linkID0, const Cm::SpatialVectorV& impulse0, Cm::SpatialVectorV& deltaV0, PxU32 linkID1, const Cm::SpatialVectorV& impulse1, Cm::SpatialVectorV& deltaV1) { ArticulationLink& link = links[linkID1]; if (link.parent == linkID0) { PX_ASSERT(linkID0 == link.parent); PX_ASSERT(linkID0 < linkID1); //impulse is in world space Cm::SpatialVectorF imp1; V4StoreA(Vec4V_From_Vec3V(impulse1.angular), &imp1.bottom.x); V4StoreA(Vec4V_From_Vec3V(impulse1.linear), &imp1.top.x); Cm::SpatialVectorF imp0; V4StoreA(Vec4V_From_Vec3V(impulse0.angular), &imp0.bottom.x); V4StoreA(Vec4V_From_Vec3V(impulse0.linear), &imp0.top.x); Cm::SpatialVectorF Z1W(-imp1.top, -imp1.bottom); const PxU32 jointOffset1 = data.getJointData(linkID1).jointOffset; const PxU8 dofCount1 = data.getJointData(linkID1).nbDof; PxReal qstZ[3] = { 0.f, 0.f, 0.f }; const Cm::SpatialVectorF Z0W = propagateImpulseW( data.getRw(linkID1), Z1W, NULL, &data.mISInvStIS[jointOffset1], &data.mWorldMotionMatrix[jointOffset1], dofCount1, qstZ); //in parent space const Cm::SpatialVectorF impulseDifW = imp0 - Z0W; //calculate velocity change start from the parent link to the root const Cm::SpatialVectorF delV0W = FeatherstoneArticulation::getImpulseResponseW(linkID0, data, impulseDifW); const Cm::SpatialVectorF delV1W = propagateAccelerationW( data.getRw(linkID1), delV0W, data.mInvStIs[linkID1], &data.mWorldMotionMatrix[jointOffset1], &data.mIsW[jointOffset1], qstZ, dofCount1, NULL); deltaV0.linear = Vec3V_From_Vec4V(V4LoadA(&delV0W.bottom.x)); deltaV0.angular = Vec3V_From_Vec4V(V4LoadA(&delV0W.top.x)); deltaV1.linear = Vec3V_From_Vec4V(V4LoadA(&delV1W.bottom.x)); deltaV1.angular = Vec3V_From_Vec4V(V4LoadA(&delV1W.top.x)); } else { getImpulseResponseSlow(links, data, linkID0, reinterpret_cast(impulse0), reinterpret_cast(deltaV0), linkID1, reinterpret_cast(impulse1), reinterpret_cast(deltaV1)); } } struct ArticulationStaticConstraintSortPredicate { bool operator()(const PxSolverConstraintDesc& left, const PxSolverConstraintDesc& right) const { PxU32 linkIndexA = left.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? left.linkIndexA : left.linkIndexB; PxU32 linkIndexB = right.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? right.linkIndexA : right.linkIndexB; return linkIndexA < linkIndexB; } }; bool createFinalizeSolverContactsStep(PxTGSSolverContactDesc& contactDesc, PxsContactManagerOutput& output, ThreadContext& threadContext, const PxReal invDtF32, const PxReal invTotalDt, const PxReal totalDt, const PxReal stepDt, const PxReal bounceThresholdF32, const PxReal frictionOffsetThreshold, const PxReal correlationDistance, const PxReal biasCoefficient, PxConstraintAllocator& constraintAllocator); void FeatherstoneArticulation::prepareStaticConstraintsTGS(const PxReal stepDt, const PxReal totalDt, const PxReal invStepDt, const PxReal invTotalDt, PxsContactManagerOutputIterator& outputs, Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold, PxTGSSolverBodyData* solverBodyData, PxTGSSolverBodyTxInertia* txInertia, PxsConstraintBlockManager& blockManager, Dy::ConstraintWriteback* constraintWritebackPool, const PxReal biasCoefficient, const PxReal lengthScale) { BlockAllocator blockAllocator(blockManager, threadContext.mConstraintBlockStream, threadContext.mFrictionPatchStreamPair, threadContext.mConstraintSize); const PxTransform id(PxIdentity); PxSort(mStatic1DConstraints.begin(), mStatic1DConstraints.size(), ArticulationStaticConstraintSortPredicate()); PxSort(mStaticContactConstraints.begin(), mStaticContactConstraints.size(), ArticulationStaticConstraintSortPredicate()); for (PxU32 i = 0; i < mStatic1DConstraints.size(); ++i) { PxSolverConstraintDesc& desc = mStatic1DConstraints[i]; PxU32 linkIndex = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? desc.linkIndexA : desc.linkIndexB; PX_ASSERT(desc.constraintType == DY_SC_TYPE_RB_1D); const Constraint* constraint = reinterpret_cast(desc.constraint); SolverConstraintShaderPrepDesc shaderPrepDesc; PxTGSSolverConstraintPrepDesc prepDesc; const PxConstraintSolverPrep solverPrep = constraint->solverPrep; const void* constantBlock = constraint->constantBlock; const PxU32 constantBlockByteSize = constraint->constantBlockSize; const PxTransform& pose0 = (constraint->body0 ? constraint->body0->getPose() : id); const PxTransform& pose1 = (constraint->body1 ? constraint->body1->getPose() : id); const PxTGSSolverBodyVel* sbody0 = desc.tgsBodyA; const PxTGSSolverBodyVel* sbody1 = desc.tgsBodyB; PxTGSSolverBodyData* sbodyData0 = &solverBodyData[desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? 0 : desc.bodyADataIndex]; PxTGSSolverBodyData* sbodyData1 = &solverBodyData[desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY ? 0 : desc.bodyBDataIndex]; PxTGSSolverBodyTxInertia& txI0 = txInertia[desc.bodyADataIndex]; PxTGSSolverBodyTxInertia& txI1 = txInertia[desc.bodyBDataIndex]; shaderPrepDesc.constantBlock = constantBlock; shaderPrepDesc.constantBlockByteSize = constantBlockByteSize; shaderPrepDesc.constraint = constraint; shaderPrepDesc.solverPrep = solverPrep; prepDesc.desc = static_cast(&desc); prepDesc.bodyFrame0 = pose0; prepDesc.bodyFrame1 = pose1; prepDesc.body0 = sbody0; prepDesc.body1 = sbody1; prepDesc.body0TxI = &txI0; prepDesc.body1TxI = &txI1; prepDesc.bodyData0 = sbodyData0; prepDesc.bodyData1 = sbodyData1; prepDesc.linBreakForce = constraint->linBreakForce; prepDesc.angBreakForce = constraint->angBreakForce; prepDesc.writeback = &constraintWritebackPool[constraint->index]; setupConstraintFlags(prepDesc, constraint->flags); prepDesc.minResponseThreshold = constraint->minResponseThreshold; prepDesc.bodyState0 = desc.linkIndexA == PxSolverConstraintDesc::RIGID_BODY ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eARTICULATION; prepDesc.bodyState1 = desc.linkIndexB == PxSolverConstraintDesc::RIGID_BODY ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eARTICULATION; SetupSolverConstraintStep(shaderPrepDesc, prepDesc, blockAllocator, stepDt, totalDt, invStepDt, invTotalDt, lengthScale, biasCoefficient); if (desc.constraint) { if (mArticulationData.mNbStatic1DConstraints[linkIndex] == 0) mArticulationData.mStatic1DConstraintStartIndex[linkIndex] = i; mArticulationData.mNbStatic1DConstraints[linkIndex]++; } else { //Shuffle down mStatic1DConstraints.remove(i); i--; } } for (PxU32 i = 0; i < mStaticContactConstraints.size(); ++i) { PxSolverConstraintDesc& desc = mStaticContactConstraints[i]; PxU32 linkIndex = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? desc.linkIndexA : desc.linkIndexB; PX_ASSERT(desc.constraintType == DY_SC_TYPE_RB_CONTACT); PxTGSSolverContactDesc blockDesc; PxsContactManager* cm = reinterpret_cast(desc.constraint); PxcNpWorkUnit& unit = cm->getWorkUnit(); PxsContactManagerOutput* cmOutput = &outputs.getContactManagerOutput(unit.mNpIndex); PxTGSSolverBodyVel& b0 = *desc.tgsBodyA; PxTGSSolverBodyVel& b1 = *desc.tgsBodyB; PxTGSSolverBodyData& data0 = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? solverBodyData[0] : solverBodyData[desc.bodyADataIndex]; PxTGSSolverBodyData& data1 = desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY ? solverBodyData[0] : solverBodyData[desc.bodyBDataIndex]; PxTGSSolverBodyTxInertia& txI0 = txInertia[desc.bodyADataIndex]; PxTGSSolverBodyTxInertia& txI1 = txInertia[desc.bodyBDataIndex]; blockDesc.bodyFrame0 = unit.mRigidCore0->body2World; blockDesc.bodyFrame1 = unit.mRigidCore1->body2World; blockDesc.shapeInteraction = cm->getShapeInteraction(); blockDesc.contactForces = cmOutput->contactForces; blockDesc.desc = static_cast(&desc); blockDesc.body0 = &b0; blockDesc.body1 = &b1; blockDesc.body0TxI = &txI0; blockDesc.body1TxI = &txI1; blockDesc.bodyData0 = &data0; blockDesc.bodyData1 = &data1; blockDesc.hasForceThresholds = !!(unit.mFlags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD); blockDesc.disableStrongFriction = !!(unit.mFlags & PxcNpWorkUnitFlag::eDISABLE_STRONG_FRICTION); blockDesc.bodyState0 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? PxSolverContactDesc::eARTICULATION : PxSolverContactDesc::eDYNAMIC_BODY; blockDesc.bodyState1 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) ? PxSolverContactDesc::eARTICULATION : (unit.mFlags & PxcNpWorkUnitFlag::eHAS_KINEMATIC_ACTOR) ? PxSolverContactDesc::eKINEMATIC_BODY : ((unit.mFlags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eSTATIC_BODY); //blockDesc.flags = unit.flags; const PxReal maxImpulse0 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? static_cast(unit.mRigidCore0)->maxContactImpulse : data0.maxContactImpulse; const PxReal maxImpulse1 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) ? static_cast(unit.mRigidCore1)->maxContactImpulse : data1.maxContactImpulse; unit.setInvMassScaleFromDominance(blockDesc.invMassScales); blockDesc.restDistance = unit.mRestDistance; blockDesc.frictionPtr = unit.mFrictionDataPtr; blockDesc.frictionCount = unit.mFrictionPatchCount; blockDesc.maxCCDSeparation = PX_MAX_F32; blockDesc.maxImpulse = PxMin(maxImpulse0, maxImpulse1); blockDesc.torsionalPatchRadius = unit.mTorsionalPatchRadius; blockDesc.minTorsionalPatchRadius = unit.mMinTorsionalPatchRadius; blockDesc.offsetSlop = unit.mOffsetSlop; createFinalizeSolverContactsStep(blockDesc, *cmOutput, threadContext, invStepDt, invTotalDt, totalDt, stepDt, bounceThreshold, frictionOffsetThreshold, correlationDist, biasCoefficient, blockAllocator); getContactManagerConstraintDesc(*cmOutput, *cm, desc); updateFrictionAnchorCountAndPosition(desc, *cmOutput, blockDesc); unit.mFrictionDataPtr = blockDesc.frictionPtr; unit.mFrictionPatchCount = blockDesc.frictionCount; //KS - Don't track this for now! //axisConstraintCount += blockDesc.axisConstraintCount; if (desc.constraint) { if (mArticulationData.mNbStaticContactConstraints[linkIndex] == 0) mArticulationData.mStaticContactConstraintStartIndex[linkIndex] = i; mArticulationData.mNbStaticContactConstraints[linkIndex]++; } else { //Shuffle down mStaticContactConstraints.remove(i); i--; } } } void FeatherstoneArticulation::prepareStaticConstraints(const PxReal dt, const PxReal invDt, PxsContactManagerOutputIterator& outputs, Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold, PxReal ccdMaxSeparation, PxSolverBodyData* solverBodyData, PxsConstraintBlockManager& blockManager, Dy::ConstraintWriteback* constraintWritebackPool) { BlockAllocator blockAllocator(blockManager, threadContext.mConstraintBlockStream, threadContext.mFrictionPatchStreamPair, threadContext.mConstraintSize); const PxTransform id(PxIdentity); Cm::SpatialVectorF* Z = threadContext.mZVector.begin(); PxSort(mStatic1DConstraints.begin(), mStatic1DConstraints.size(), ArticulationStaticConstraintSortPredicate()); PxSort(mStaticContactConstraints.begin(), mStaticContactConstraints.size(), ArticulationStaticConstraintSortPredicate()); for (PxU32 i = 0; i < mStatic1DConstraints.size(); ++i) { PxSolverConstraintDesc& desc = mStatic1DConstraints[i]; PxU32 linkIndex = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? desc.linkIndexA : desc.linkIndexB; PX_ASSERT(desc.constraintType == DY_SC_TYPE_RB_1D); const Constraint* constraint = reinterpret_cast(desc.constraint); SolverConstraintShaderPrepDesc shaderPrepDesc; PxSolverConstraintPrepDesc prepDesc; const PxConstraintSolverPrep solverPrep = constraint->solverPrep; const void* constantBlock = constraint->constantBlock; const PxU32 constantBlockByteSize = constraint->constantBlockSize; const PxTransform& pose0 = (constraint->body0 ? constraint->body0->getPose() : id); const PxTransform& pose1 = (constraint->body1 ? constraint->body1->getPose() : id); const PxSolverBody* sbody0 = desc.bodyA; const PxSolverBody* sbody1 = desc.bodyB; PxSolverBodyData* sbodyData0 = &solverBodyData[desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? 0 : desc.bodyADataIndex]; PxSolverBodyData* sbodyData1 = &solverBodyData[desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY ? 0 : desc.bodyBDataIndex]; shaderPrepDesc.constantBlock = constantBlock; shaderPrepDesc.constantBlockByteSize = constantBlockByteSize; shaderPrepDesc.constraint = constraint; shaderPrepDesc.solverPrep = solverPrep; prepDesc.desc = &desc; prepDesc.bodyFrame0 = pose0; prepDesc.bodyFrame1 = pose1; prepDesc.data0 = sbodyData0; prepDesc.data1 = sbodyData1; prepDesc.body0 = sbody0; prepDesc.body1 = sbody1; prepDesc.linBreakForce = constraint->linBreakForce; prepDesc.angBreakForce = constraint->angBreakForce; prepDesc.writeback = &constraintWritebackPool[constraint->index]; setupConstraintFlags(prepDesc, constraint->flags); prepDesc.minResponseThreshold = constraint->minResponseThreshold; SetupSolverConstraint(shaderPrepDesc, prepDesc, blockAllocator, dt, invDt); if (desc.constraint) { if (mArticulationData.mNbStatic1DConstraints[linkIndex] == 0) mArticulationData.mStatic1DConstraintStartIndex[linkIndex] = i; mArticulationData.mNbStatic1DConstraints[linkIndex]++; } else { //Shuffle down mStatic1DConstraints.remove(i); i--; } } for (PxU32 i = 0; i < mStaticContactConstraints.size(); ++i) { PxSolverConstraintDesc& desc = mStaticContactConstraints[i]; PxU32 linkIndex = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? desc.linkIndexA : desc.linkIndexB; PX_ASSERT(desc.constraintType == DY_SC_TYPE_RB_CONTACT); PxSolverContactDesc blockDesc; PxsContactManager* cm = reinterpret_cast(desc.constraint); PxcNpWorkUnit& unit = cm->getWorkUnit(); PxsContactManagerOutput* cmOutput = &outputs.getContactManagerOutput(unit.mNpIndex); PxSolverBodyData& data0 = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? solverBodyData[0] : solverBodyData[desc.bodyADataIndex]; PxSolverBodyData& data1 = desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY ? solverBodyData[0] : solverBodyData[desc.bodyBDataIndex]; blockDesc.data0 = &data0; blockDesc.data1 = &data1; PxU8 flags = unit.mRigidCore0->mFlags; if (unit.mRigidCore1) flags |= PxU8(unit.mRigidCore1->mFlags); blockDesc.bodyFrame0 = unit.mRigidCore0->body2World; blockDesc.bodyFrame1 = unit.mRigidCore1 ? unit.mRigidCore1->body2World : id; blockDesc.shapeInteraction = cm->getShapeInteraction(); blockDesc.contactForces = cmOutput->contactForces; blockDesc.desc = &desc; blockDesc.body0 = desc.bodyA; blockDesc.body1 = desc.bodyB; blockDesc.hasForceThresholds = !!(unit.mFlags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD); blockDesc.disableStrongFriction = !!(unit.mFlags & PxcNpWorkUnitFlag::eDISABLE_STRONG_FRICTION); blockDesc.bodyState0 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? PxSolverContactDesc::eARTICULATION : PxSolverContactDesc::eDYNAMIC_BODY; blockDesc.bodyState1 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) ? PxSolverContactDesc::eARTICULATION : (unit.mFlags & PxcNpWorkUnitFlag::eHAS_KINEMATIC_ACTOR) ? PxSolverContactDesc::eKINEMATIC_BODY : ((unit.mFlags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eSTATIC_BODY); //blockDesc.flags = unit.flags; unit.setInvMassScaleFromDominance(blockDesc.invMassScales); blockDesc.restDistance = unit.mRestDistance; blockDesc.frictionPtr = unit.mFrictionDataPtr; blockDesc.frictionCount = unit.mFrictionPatchCount; blockDesc.maxCCDSeparation = (flags & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD) ? ccdMaxSeparation : PX_MAX_F32; blockDesc.offsetSlop = unit.mOffsetSlop; createFinalizeSolverContacts(blockDesc, *cmOutput, threadContext, invDt, dt, bounceThreshold, frictionOffsetThreshold, correlationDist, blockAllocator, Z); getContactManagerConstraintDesc(*cmOutput, *cm, desc); updateFrictionAnchorCountAndPosition(desc, *cmOutput, blockDesc); unit.mFrictionDataPtr = blockDesc.frictionPtr; unit.mFrictionPatchCount = blockDesc.frictionCount; //KS - Don't track this for now! //axisConstraintCount += blockDesc.axisConstraintCount; if (desc.constraint) { if (mArticulationData.mNbStaticContactConstraints[linkIndex] == 0) mArticulationData.mStaticContactConstraintStartIndex[linkIndex] = i; mArticulationData.mNbStaticContactConstraints[linkIndex]++; } else { mStaticContactConstraints.remove(i); i--; } } } void setupComplexLimit(ArticulationLink* links, ArticulationData& data, const PxU32 linkID, const PxReal angle, const PxReal lowLimit, const PxReal highLimit, const PxVec3& axis, const PxReal cfm, ArticulationInternalConstraint& complexConstraint, ArticulationInternalLimit& limit) { Cm::SpatialVectorV deltaVA, deltaVB; FeatherstoneArticulation::getImpulseSelfResponse(links, data, links[linkID].parent, Cm::SpatialVector(PxVec3(0), axis), deltaVA, linkID, Cm::SpatialVector(PxVec3(0), -axis), deltaVB); const Cm::SpatialVector& deltaV0 = unsimdRef(deltaVA); const Cm::SpatialVector& deltaV1 = unsimdRef(deltaVB); const PxReal r0 = deltaV0.angular.dot(axis); const PxReal r1 = deltaV1.angular.dot(axis); const PxReal unitResponse = r0 - r1; const PxReal recipResponse = unitResponse > DY_ARTICULATION_MIN_RESPONSE ? 1.0f / (cfm + unitResponse) : 0.0f; complexConstraint.row0 = Cm::UnAlignedSpatialVector(PxVec3(0), axis); complexConstraint.row1 = Cm::UnAlignedSpatialVector(PxVec3(0), axis); complexConstraint.deltaVA.top = unsimdRef(deltaVA).angular; complexConstraint.deltaVA.bottom = unsimdRef(deltaVA).linear; complexConstraint.deltaVB.top = unsimdRef(deltaVB).angular; complexConstraint.deltaVB.bottom = unsimdRef(deltaVB).linear; complexConstraint.recipResponse = recipResponse; complexConstraint.response = unitResponse; complexConstraint.isLinearConstraint = true; limit.errorLow = angle - lowLimit; limit.errorHigh = highLimit - angle; limit.lowImpulse = 0.f; limit.highImpulse = 0.f; } void FeatherstoneArticulation::setupInternalConstraintsRecursive( ArticulationLink* links, const PxU32 linkCount, const bool fixBase, ArticulationData& data, const PxReal stepDt, const PxReal dt, const PxReal invDt, const bool isTGSSolver, const PxU32 linkID, const PxReal maxForceScale ) { const ArticulationLink& link = links[linkID]; ArticulationJointCoreData& jointDatum = data.getJointData(linkID); const ArticulationLink& pLink = links[link.parent]; const ArticulationJointCore& j = *link.inboundJoint; //const bool jointDrive = (j.driveType != PxArticulationJointDriveType::eNONE); bool hasFriction = j.frictionCoefficient > 0.f; const PxReal fCoefficient = j.frictionCoefficient * stepDt; const PxReal transmissionForce = data.getTransmittedForce(linkID).magnitude() * fCoefficient; // PT:: tag: scalar transform*transform const PxTransform cA2w = pLink.bodyCore->body2World.transform(j.parentPose); const PxTransform cB2w = link.bodyCore->body2World.transform(j.childPose); const PxU32 parent = link.parent; const PxReal cfm = PxMax(link.cfm, pLink.cfm); //Linear, then angular... PxVec3 driveError(0.f); PxVec3 angles(0.f); PxVec3 row[3]; if (j.jointType == PxArticulationJointType::eSPHERICAL && jointDatum.nbDof > 1) { //It's a spherical joint. We can't directly work on joint positions with spherical joints, so we instead need to compute the quaternion //and from that compute the joint error projected onto the DOFs. This will yield a rotation that is singularity-free, where the joint //angles match the target joint angles provided provided the angles are within +/- Pi around each axis. Spherical joints do not support //quaternion double cover cases/wide angles. PxVec3 driveAxis(0.f); bool hasAngularDrives = false; PxU32 tmpDofId = 0; for (PxU32 i = 0; i < PxArticulationAxis::eX; ++i) { if (j.motion[i] != PxArticulationMotion::eLOCKED) { const bool hasDrive = (j.motion[i] != PxArticulationMotion::eLOCKED && j.drives[i].driveType != PxArticulationDriveType::eNONE); if (hasDrive) { const PxVec3 axis = data.mMotionMatrix[jointDatum.jointOffset + tmpDofId].top; PxReal target = data.mJointTargetPositions[jointDatum.jointOffset + tmpDofId]; driveAxis += axis * target; hasAngularDrives = true; } tmpDofId++; } } { PxQuat qB2qA = cA2w.q.getConjugate() * cB2w.q; { //Spherical joint drive calculation using 3x child-space Euler angles if (hasAngularDrives) { PxReal angle = driveAxis.normalize(); if (angle < 1e-12f) { driveAxis = PxVec3(1.f, 0.f, 0.f); angle = 0.f; } PxQuat targetQ = PxQuat(angle, driveAxis); if (targetQ.dot(qB2qA) < 0.f) targetQ = -targetQ; driveError = -2.f * (targetQ.getConjugate() * qB2qA).getImaginaryPart(); } for (PxU32 i = 0, tmpDof = 0; i < PxArticulationAxis::eX; ++i) { if (j.motion[i] != PxArticulationMotion::eLOCKED) { angles[i] = data.mJointPosition[j.jointOffset + tmpDof]; row[i] = data.mWorldMotionMatrix[jointDatum.jointOffset + tmpDof].top; tmpDof++; } } } } } else { for (PxU32 i = 0; i < PxArticulationAxis::eX; ++i) { if (j.motion[i] != PxArticulationMotion::eLOCKED) { driveError[i] = data.mJointTargetPositions[j.jointOffset] - data.mJointPosition[j.jointOffset]; angles[i] = data.mJointPosition[j.jointOffset]; row[i] = data.mWorldMotionMatrix[jointDatum.jointOffset].top; } } } PxU32 dofId = 0; for (PxU32 i = 0; i < PxArticulationAxis::eX; ++i) { if (j.motion[i] != PxArticulationMotion::eLOCKED) { //Impulse response vector and axes are common for all constraints on this axis besides locked axis!!! const PxVec3 axis = row[i]; Cm::SpatialVectorV deltaVA, deltaVB; FeatherstoneArticulation::getImpulseSelfResponse(links, data, parent, Cm::SpatialVector(PxVec3(0), axis), deltaVA, linkID, Cm::SpatialVector(PxVec3(0), -axis), deltaVB); const Cm::SpatialVector& deltaV0 = unsimdRef(deltaVA); const Cm::SpatialVector& deltaV1 = unsimdRef(deltaVB); const PxReal r0 = deltaV0.angular.dot(axis); const PxReal r1 = deltaV1.angular.dot(axis); const PxReal unitResponse = r0 - r1; const PxReal recipResponse = unitResponse <= 0.f ? 0.f : 1.0f / (unitResponse+cfm); const PxU32 count = data.mInternalConstraints.size(); data.mInternalConstraints.forceSize_Unsafe(count + 1); ArticulationInternalConstraint* constraints = &data.mInternalConstraints[count]; constraints->recipResponse = recipResponse; constraints->response = unitResponse; constraints->row0 = Cm::SpatialVectorF(PxVec3(0), axis); constraints->row1 = Cm::SpatialVectorF(PxVec3(0), axis); constraints->deltaVA.top = unsimdRef(deltaVA).angular; constraints->deltaVA.bottom = unsimdRef(deltaVA).linear; constraints->deltaVB.top = unsimdRef(deltaVB).angular; constraints->deltaVB.bottom = unsimdRef(deltaVB).linear; constraints->isLinearConstraint = false; constraints->maxJointVelocity = j.maxJointVelocity[i]; constraints->accumulatedFrictionImpulse = 0.0f; constraints->frictionMaxForce = hasFriction ? transmissionForce : 0.f; constraints->dynamicFrictionEffort = j.frictionParams[i].dynamicFrictionEffort; constraints->staticFrictionEffort = j.frictionParams[i].staticFrictionEffort; constraints->viscousFrictionCoefficient = j.frictionParams[i].viscousFrictionCoefficient; const bool hasDrive = (j.motion[i] != PxArticulationMotion::eLOCKED && j.drives[i].driveType != PxArticulationDriveType::eNONE); constraints->driveImpulse = 0.0f; constraints->driveMaxImpulse = j.drives[i].maxForce * maxForceScale; constraints->envelope = j.drives[i].envelope; constraints->externalJointForce = data.getJointForces()[jointDatum.jointOffset + dofId]; if(hasDrive) { constraints->setImplicitDriveDesc( computeImplicitDriveParams( j.drives[i].driveType, j.drives[i].stiffness, j.drives[i].damping, isTGSSolver ? stepDt : dt, dt, unitResponse, recipResponse, driveError[i], data.mJointTargetVelocities[j.jointOffset + dofId], isTGSSolver)); } else { constraints->setImplicitDriveDesc(ArticulationImplicitDriveDesc(PxZero)); } if (j.motion[i] == PxArticulationMotion::eLIMITED) { const PxU32 limitCount = data.mInternalLimits.size(); data.mInternalLimits.forceSize_Unsafe(limitCount + 1); ArticulationInternalLimit* limits = &data.mInternalLimits[limitCount]; const PxReal jPos = angles[i]; limits->errorHigh = j.limits[i].high - jPos; limits->errorLow = jPos - j.limits[i].low; limits->lowImpulse = 0.f; limits->highImpulse = 0.f; } dofId++; } } for (PxU32 i = PxArticulationAxis::eX; i < PxArticulationAxis::eCOUNT; ++i) { if (j.motion[i] != PxArticulationMotion::eLOCKED) { //Impulse response vector and axes are common for all constraints on this axis besides locked axis!!! const PxVec3 axis = data.mWorldMotionMatrix[jointDatum.jointOffset + dofId].bottom; const PxVec3 ang0 = (cA2w.p - pLink.bodyCore->body2World.p).cross(axis); const PxVec3 ang1 = (cB2w.p - link.bodyCore->body2World.p).cross(axis); Cm::SpatialVectorV deltaVA, deltaVB; FeatherstoneArticulation::getImpulseSelfResponse(links, data, links[linkID].parent, Cm::SpatialVector(axis, ang0), deltaVA, linkID, Cm::SpatialVector(-axis, -ang1), deltaVB); const Cm::SpatialVector& deltaV0 = unsimdRef(deltaVA); const Cm::SpatialVector& deltaV1 = unsimdRef(deltaVB); const PxReal r0 = deltaV0.linear.dot(axis) + deltaV0.angular.dot(ang0); const PxReal r1 = deltaV1.linear.dot(axis) + deltaV1.angular.dot(ang1); const PxReal unitResponse = r0 - r1; //const PxReal recipResponse = unitResponse > DY_ARTICULATION_MIN_RESPONSE ? 1.0f / (unitResponse+cfm) : 0.0f; const PxReal recipResponse = 1.0f / (unitResponse + cfm); const PxU32 count = data.mInternalConstraints.size(); data.mInternalConstraints.forceSize_Unsafe(count + 1); ArticulationInternalConstraint* constraints = &data.mInternalConstraints[count]; constraints->response = unitResponse; constraints->recipResponse = recipResponse; constraints->row0 = Cm::SpatialVectorF(axis, ang0); constraints->row1 = Cm::SpatialVectorF(axis, ang1); constraints->deltaVA.top = unsimdRef(deltaVA).angular; constraints->deltaVA.bottom = unsimdRef(deltaVA).linear; constraints->deltaVB.top = unsimdRef(deltaVB).angular; constraints->deltaVB.bottom = unsimdRef(deltaVB).linear; constraints->isLinearConstraint = true; constraints->maxJointVelocity = j.maxJointVelocity[i]; constraints->accumulatedFrictionImpulse = 0.f; constraints->frictionMaxForce = hasFriction ? transmissionForce : 0.f; constraints->dynamicFrictionEffort = j.frictionParams[i].dynamicFrictionEffort; constraints->staticFrictionEffort = j.frictionParams[i].staticFrictionEffort; constraints->viscousFrictionCoefficient = j.frictionParams[i].viscousFrictionCoefficient; const bool hasDrive = (j.motion[i] != PxArticulationMotion::eLOCKED && (j.drives[i].envelope.maxEffort > 0.0f || j.drives[i].maxForce > 0.0f) && (j.drives[i].stiffness > 0.f || j.drives[i].damping > 0.f)); constraints->driveImpulse = 0.0f; constraints->envelope = j.drives[i].envelope; constraints->driveMaxImpulse = j.drives[i].maxForce * maxForceScale; constraints->externalJointForce = data.getJointForces()[jointDatum.jointOffset + dofId]; if(hasDrive) { constraints->setImplicitDriveDesc( computeImplicitDriveParams( j.drives[i].driveType, j.drives[i].stiffness, j.drives[i].damping, isTGSSolver ? stepDt : dt, dt, unitResponse, recipResponse, data.mJointTargetPositions[j.jointOffset + dofId] - data.mJointPosition[j.jointOffset + dofId], data.mJointTargetVelocities[j.jointOffset + dofId], isTGSSolver)); } else { constraints->setImplicitDriveDesc(ArticulationImplicitDriveDesc(PxZero)); } if (j.motion[i] == PxArticulationMotion::eLIMITED) { const PxU32 limitCount = data.mInternalLimits.size(); data.mInternalLimits.forceSize_Unsafe(limitCount + 1); ArticulationInternalLimit* limits = &data.mInternalLimits[limitCount]; const PxReal jPos = data.mJointPosition[j.jointOffset + dofId]; limits->errorHigh = j.limits[i].high - jPos; limits->errorLow = jPos - j.limits[i].low; limits->lowImpulse = 0.f; limits->highImpulse = 0.f; } dofId++; } } if (jointDatum.dofLimitMask) { for (PxU32 dof = 0; dof < jointDatum.nbDof; ++dof) { PxU32 i = j.dofIds[dof]; if (j.motion[i] == PxArticulationMotion::eLOCKED) { const PxU32 count = data.mInternalConstraints.size(); data.mInternalConstraints.forceSize_Unsafe(count + 1); ArticulationInternalConstraint* constraints = &data.mInternalConstraints[count]; const PxU32 limitCount = data.mInternalLimits.size(); data.mInternalLimits.forceSize_Unsafe(limitCount + 1); ArticulationInternalLimit* limits = &data.mInternalLimits[limitCount]; const PxVec3 axis = row[i]; PxReal angle = angles[i]; //A locked axis is equivalent to a limit of 0 PxReal low = 0.f; PxReal high = 0.f; setupComplexLimit(links, data, linkID, angle, low, high, axis, cfm, *constraints, *limits++); } } } const PxU32 numChildren = link.mNumChildren; const PxU32 offset = link.mChildrenStartIndex; for (PxU32 i = 0; i < numChildren; ++i) { const PxU32 child = offset + i; setupInternalConstraintsRecursive(links, linkCount, fixBase, data, stepDt, dt, invDt, isTGSSolver, child, maxForceScale); } } void FeatherstoneArticulation::setupInternalSpatialTendonConstraintsRecursive( ArticulationLink* links, ArticulationAttachment* attachments, const PxU32 attachmentCount, const PxVec3& pAttachPoint, const bool fixBase, ArticulationData& data, const PxReal stepDt, const bool isTGSSolver, const PxU32 attachmentID, const PxReal stiffness, const PxReal damping, const PxReal limitStiffness, const PxReal accumLength, const PxU32 startLink, const PxVec3& startAxis, const PxVec3& startRaXn) { ArticulationAttachment& attachment = attachments[attachmentID]; ArticulationLink& cLink = links[attachment.linkInd]; const PxTransform cBody2World = cLink.bodyCore->body2World; const PxVec3 rb = cBody2World.q.rotate(attachment.relativeOffset); const PxVec3 cAttachPoint = cBody2World.p + rb; const PxVec3 dif = pAttachPoint - cAttachPoint; const PxReal distanceSq = dif.magnitudeSquared(); const PxReal distance = PxSqrt(distanceSq); const PxReal u = distance * attachment.coefficient + accumLength; const PxU32 childCount = attachment.childCount; if (childCount) { for (ArticulationBitField children = attachment.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); setupInternalSpatialTendonConstraintsRecursive(links, attachments, attachmentCount, cAttachPoint, fixBase, data, stepDt, isTGSSolver, child, stiffness, damping, limitStiffness, u, startLink, startAxis, startRaXn); } } else { const PxVec3 axis = distance > 0.001f ? dif / distance : PxVec3(0.f); const PxVec3 rbXn = rb.cross(axis); Cm::SpatialVectorV deltaVA, deltaVB; FeatherstoneArticulation::getImpulseSelfResponse(links, data, startLink, Cm::SpatialVector(startAxis, startRaXn), deltaVA, attachment.linkInd, Cm::SpatialVector(-axis, -rbXn), deltaVB); const Cm::SpatialVector& deltaV0 = unsimdRef(deltaVA); const Cm::SpatialVector& deltaV1 = unsimdRef(deltaVB); const PxReal r0 = deltaV0.linear.dot(startAxis) + deltaV0.angular.dot(startRaXn); const PxReal r1 = deltaV1.linear.dot(axis) + deltaV1.angular.dot(rbXn); const PxReal unitResponse = (r0 - r1); //const PxReal recipResponse = unitResponse > DY_ARTICULATION_MIN_RESPONSE ? 1.0f / (unitResponse + cfm) : 0.0f; //const PxReal recipResponse = unitResponse > DY_ARTICULATION_MIN_RESPONSE ? 1.0f / (unitResponse) : 0.0f; const PxReal recipResponse = 1.0f / (unitResponse + cLink.cfm); const PxU32 count = data.mInternalSpatialTendonConstraints.size(); data.mInternalSpatialTendonConstraints.forceSize_Unsafe(count + 1); ArticulationInternalTendonConstraint* constraint = &data.mInternalSpatialTendonConstraints[count]; attachment.mConstraintInd = PxU16(count); constraint->row0 = Cm::SpatialVectorF(startAxis, startRaXn); constraint->row1 = Cm::SpatialVectorF(axis, rbXn); constraint->linkID0 = startLink; constraint->linkID1 = attachment.linkInd; constraint->recipResponse = recipResponse; const PxReal a = stepDt * (stepDt*stiffness + damping); const PxReal a2 = stepDt * (stepDt*limitStiffness + damping); const PxReal x = unitResponse > 0.f ? 1.0f / (1.0f + a * unitResponse) : 0.f; const PxReal x2 = unitResponse > 0.f ? 1.0f / (1.0f + a2 * unitResponse) : 0.f; constraint->velMultiplier = -x * a;// * unitResponse; //constraint->velMultiplier = -x * damping*stepDt; constraint->impulseMultiplier = isTGSSolver ? 1.f : 1.f - x; constraint->biasCoefficient = (-stiffness * x * stepDt);//*unitResponse; constraint->appliedForce = 0.f; constraint->accumulatedLength = u;// + u*0.2f; constraint->restDistance = attachment.restLength; constraint->lowLimit = attachment.lowLimit; constraint->highLimit = attachment.highLimit; constraint->limitBiasCoefficient = (-limitStiffness * x2 * stepDt);//*unitResponse; constraint->limitImpulseMultiplier = isTGSSolver ? 1.f : 1.f - x2; constraint->limitAppliedForce = 0.f; } } void FeatherstoneArticulation::updateSpatialTendonConstraintsRecursive(ArticulationAttachment* attachments, ArticulationData& data, const PxU32 attachmentID, PxReal accumLength, const PxVec3& pAttachPoint) { ArticulationAttachment& attachment = attachments[attachmentID]; //const PxReal restDist = attachment.restDistance; const PxTransform& cBody2World = data.getAccumulatedPoses()[attachment.linkInd]; const PxVec3 rb = cBody2World.q.rotate(attachment.relativeOffset); const PxVec3 cAttachPoint = cBody2World.p + rb; const PxVec3 dif = pAttachPoint - cAttachPoint; const PxReal distanceSq = dif.magnitudeSquared(); const PxReal distance = PxSqrt(distanceSq); const PxReal u = distance * attachment.coefficient + accumLength; const PxU32 childCount = attachment.childCount; if (childCount) { for (ArticulationBitField children = attachment.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); updateSpatialTendonConstraintsRecursive(attachments, data, child, u, cAttachPoint); } } else { PxU32 index = attachment.mConstraintInd; ArticulationInternalTendonConstraint& constraint = data.mInternalSpatialTendonConstraints[index]; constraint.accumulatedLength = u; } } void FeatherstoneArticulation::setupInternalFixedTendonConstraintsRecursive( ArticulationLink* links, ArticulationTendonJoint* tendonJoints, const bool fixBase, ArticulationData& data, const PxReal stepDt, const bool isTGSSolver, const PxU32 tendonJointID, const PxReal stiffness, const PxReal damping, const PxReal limitStiffness, const PxU32 startLink, const PxVec3& startAxis, const PxVec3& startRaXn) { ArticulationTendonJoint& tendonJoint = tendonJoints[tendonJointID]; ArticulationLink& cLink = links[tendonJoint.linkInd]; const PxTransform& cBody2World = cLink.bodyCore->body2World; const PxReal cfm = PxMax(cLink.cfm, links[startLink].cfm); ArticulationJointCoreData& jointDatum = data.getJointData(tendonJoint.linkInd); ArticulationJointCore& joint = *cLink.inboundJoint; PxU16 tendonJointAxis = tendonJoint.axis; PX_ASSERT(joint.motion[tendonJointAxis] != PxArticulationMotion::eLOCKED); //Cm::SpatialVector cImpulse; PxU32 dofIndex = joint.invDofIds[tendonJointAxis]; tendonJoint.startJointOffset = PxTo16(jointDatum.jointOffset + dofIndex); ///*PxReal jointPose = jointPositions[jointDatum.jointOffset + dofIndex] * tendonJoint.coefficient; //jointPose += accumulatedJointPose;*/ { PxVec3 axis, rbXn; if (tendonJointAxis < PxArticulationAxis::eX) { const PxVec3 tAxis = data.mWorldMotionMatrix[jointDatum.jointOffset + dofIndex].top; axis = PxVec3(0.f); rbXn = tAxis; } else { // PT:: tag: scalar transform*transform const PxTransform cB2w = cBody2World.transform(joint.childPose); const PxVec3 tAxis = data.mWorldMotionMatrix[jointDatum.jointOffset + dofIndex].bottom; axis = tAxis; rbXn = (cB2w.p - cBody2World.p).cross(axis); } Cm::SpatialVectorV deltaVA, deltaVB; FeatherstoneArticulation::getImpulseSelfResponse(links, data, startLink, Cm::SpatialVector(startAxis, startRaXn), deltaVA, tendonJoint.linkInd, Cm::SpatialVector(-axis, -rbXn), deltaVB); const Cm::SpatialVector& deltaV0 = unsimdRef(deltaVA); const Cm::SpatialVector& deltaV1 = unsimdRef(deltaVB); /*const PxU32 pLinkInd = cLink.parent; printf("(%i, %i) deltaV1(%f, %f, %f, %f, %f, %f)\n", pLinkInd, tendonJoint.linkInd, deltaV1.linear.x, deltaV1.linear.y, deltaV1.linear.z, deltaV1.angular.x, deltaV1.angular.y, deltaV1.angular.z);*/ const PxReal r0 = deltaV0.linear.dot(startAxis) + deltaV0.angular.dot(startRaXn); const PxReal r1 = deltaV1.linear.dot(axis) + deltaV1.angular.dot(rbXn); const PxReal unitResponse = r0 - r1; const PxReal recipResponse = 1.0f / (unitResponse + cfm); const PxU32 count = data.mInternalFixedTendonConstraints.size(); data.mInternalFixedTendonConstraints.forceSize_Unsafe(count + 1); ArticulationInternalTendonConstraint* constraint = &data.mInternalFixedTendonConstraints[count]; tendonJoint.mConstraintInd = PxU16(count); constraint->row0 = Cm::UnAlignedSpatialVector(startAxis, startRaXn); constraint->row1 = Cm::UnAlignedSpatialVector(axis, rbXn); constraint->deltaVA = r0; //We only need to record the change in velocity projected onto the dof for this! constraint->deltaVB = Cm::UnAlignedSpatialVector(deltaV1.angular, deltaV1.linear); constraint->linkID0 = startLink; constraint->linkID1 = tendonJoint.linkInd; constraint->recipResponse = recipResponse; const PxReal a = stepDt * (stepDt*stiffness + damping); const PxReal a2 = stepDt * (stepDt*limitStiffness + damping); PxReal x = unitResponse > 0.f ? 1.0f / (1.0f + a * unitResponse) : 0.f; PxReal x2 = unitResponse > 0.f ? 1.0f / (1.0f + a2* unitResponse) : 0.f; constraint->velMultiplier = -x * a;// * unitResponse; constraint->impulseMultiplier = isTGSSolver ? 1.f : 1.f - x; constraint->biasCoefficient = (-stiffness * x * stepDt);//*unitResponse; constraint->appliedForce = 0.f; //constraint->accumulatedLength = jointPose; constraint->limitImpulseMultiplier = isTGSSolver ? 1.f : 1.f - x2; constraint->limitBiasCoefficient = (-limitStiffness * x2 * stepDt);//*unitResponse; constraint->limitAppliedForce = 0.f; /*printf("(%i, %i) r0 %f, r1 %f cmf %f unitResponse %f recipResponse %f a %f x %f\n", pLinkInd, tendonJoint.linkInd, r0, r1, cLink.cfm, unitResponse, recipResponse, a, x);*/ } const PxU32 childCount = tendonJoint.childCount; if (childCount) { for (ArticulationBitField children = tendonJoint.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); setupInternalFixedTendonConstraintsRecursive(links, tendonJoints, fixBase, data, stepDt, isTGSSolver, child, stiffness, damping, limitStiffness, startLink, startAxis, startRaXn); } } } void FeatherstoneArticulation::setupInternalConstraints( ArticulationLink* links, const PxU32 linkCount, const bool fixBase, ArticulationData& data, PxReal stepDt, PxReal dt, PxReal invDt, bool isTGSSolver) { data.mInternalConstraints.forceSize_Unsafe(0); data.mInternalConstraints.reserve(data.getDofs()); data.mInternalLimits.forceSize_Unsafe(0); data.mInternalLimits.reserve(data.getDofs()); const PxReal maxForceScale = data.getArticulationFlags() & PxArticulationFlag::eDRIVE_LIMITS_ARE_FORCES ? dt : 1.f; const PxU32 numChildren = links[0].mNumChildren; const PxU32 offset = links[0].mChildrenStartIndex; for (PxU32 i = 0; i < numChildren; ++i) { const PxU32 child = offset + i; setupInternalConstraintsRecursive(links, linkCount, fixBase, data, stepDt, dt, invDt, isTGSSolver, child, maxForceScale); } PxU32 totalNumAttachments = 0; for (PxU32 i = 0; i < data.mNumSpatialTendons; ++i) { Dy::ArticulationSpatialTendon* tendon = data.mSpatialTendons[i]; totalNumAttachments += tendon->getNumAttachments(); } data.mInternalSpatialTendonConstraints.forceSize_Unsafe(0); data.mInternalSpatialTendonConstraints.reserve(totalNumAttachments); for (PxU32 i = 0; i < data.mNumSpatialTendons; ++i) { Dy::ArticulationSpatialTendon* tendon = data.mSpatialTendons[i]; Dy::ArticulationAttachment* attachments = tendon->getAttachments(); ArticulationAttachment& pAttachment = attachments[0]; //const PxU32 childCount = pAttachment.childCount; //PxReal scale = 1.f/PxReal(childCount); const PxReal coefficient = pAttachment.coefficient; const PxU32 startLink = pAttachment.linkInd; ArticulationLink& pLink = links[startLink]; const PxTransform pBody2World = pLink.bodyCore->body2World; const PxVec3 ra = pBody2World.q.rotate(pAttachment.relativeOffset); const PxVec3 pAttachPoint = pBody2World.p + ra; for (ArticulationAttachmentBitField children = pAttachment.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); ArticulationAttachment& attachment = attachments[child]; ArticulationLink& cLink = links[attachment.linkInd]; const PxTransform cBody2World = cLink.bodyCore->body2World; const PxVec3 rb = cBody2World.q.rotate(attachment.relativeOffset); const PxVec3 cAttachPoint = cBody2World.p + rb; const PxVec3 axis = (pAttachPoint - cAttachPoint).getNormalized(); const PxVec3 raXn = ra.cross(axis); setupInternalSpatialTendonConstraintsRecursive(links, attachments, tendon->getNumAttachments(), pAttachPoint, fixBase, data, stepDt, isTGSSolver, child, tendon->mStiffness, tendon->mDamping, tendon->mLimitStiffness, tendon->mOffset*coefficient, startLink, axis, raXn); } } PxU32 totalNumTendonJoints = 0; for (PxU32 i = 0; i < data.mNumFixedTendons; ++i) { Dy::ArticulationFixedTendon* tendon = data.mFixedTendons[i]; totalNumTendonJoints += tendon->getNumJoints(); } data.mInternalFixedTendonConstraints.forceSize_Unsafe(0); data.mInternalFixedTendonConstraints.reserve(totalNumTendonJoints); for (PxU32 i = 0; i < data.mNumFixedTendons; ++i) { ArticulationFixedTendon* tendon = data.mFixedTendons[i]; ArticulationTendonJoint* tendonJoints = tendon->getTendonJoints(); ArticulationTendonJoint& pTendonJoint = tendonJoints[0]; const PxU32 startLinkInd = pTendonJoint.linkInd; ArticulationLink& pLink = links[startLinkInd]; const PxTransform& pBody2World = pLink.bodyCore->body2World; for (ArticulationAttachmentBitField children = pTendonJoint.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); ArticulationTendonJoint& cTendonJoint = tendonJoints[child]; ArticulationLink& cLink = links[cTendonJoint.linkInd]; ArticulationJointCore* joint = cLink.inboundJoint; PX_ASSERT(joint != NULL); ArticulationJointCoreData* jointDatum = &data.getJointData(cTendonJoint.linkInd); PxU16 tendonJointAxis = cTendonJoint.axis; PX_ASSERT(joint->motion[tendonJointAxis] != PxArticulationMotion::eLOCKED); PxVec3 startAxis, raXn; PxU32 dofIndex = joint->invDofIds[tendonJointAxis]; if (tendonJointAxis < PxArticulationAxis::eX) { const PxVec3 axis = data.mWorldMotionMatrix[jointDatum->jointOffset + dofIndex].top; startAxis = PxVec3(0.f); raXn = axis; } else { // PT:: tag: scalar transform*transform const PxTransform cA2w = pBody2World.transform(joint->parentPose); const PxVec3 axis = data.mWorldMotionMatrix[jointDatum->jointOffset + dofIndex].bottom; const PxVec3 ang0 = (cA2w.p - pBody2World.p).cross(axis); startAxis = axis; raXn = ang0; } setupInternalFixedTendonConstraintsRecursive(links, tendonJoints, fixBase, data, stepDt, isTGSSolver, child, tendon->mStiffness, tendon->mDamping, tendon->mLimitStiffness, startLinkInd, startAxis, raXn); } } //Now set up the internal mimic joint constraints. setupInternalMimicJointConstraints(); } PxU32 FeatherstoneArticulation::setupSolverConstraints( ArticulationLink* links, const PxU32 linkCount, const bool fixBase, ArticulationData& data, PxU32& acCount) { acCount = 0; setupInternalConstraints(links, linkCount, fixBase, data, data.getDt(), data.getDt(), 1.f / data.getDt(), false); return 0; } PxU32 FeatherstoneArticulation::setupSolverConstraintsTGS(const ArticulationSolverDesc& articDesc, PxReal dt, PxReal invDt, PxReal totalDt) { FeatherstoneArticulation* thisArtic = articDesc.articulation; ArticulationLink* links = thisArtic->mArticulationData.getLinks(); const PxU32 linkCount = thisArtic->mArticulationData.getLinkCount(); const bool fixBase = thisArtic->mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE; thisArtic->setupInternalConstraints(links, linkCount, fixBase, thisArtic->mArticulationData, dt, totalDt, invDt, true); return 0; } void FeatherstoneArticulation::teleportLinks(ArticulationData& data) { ArticulationLink* links = mArticulationData.getLinks(); ArticulationJointCoreData* jointData = mArticulationData.getJointData(); const PxReal* jointPositions = data.getJointPositions(); const PxU32 linkCount = mArticulationData.getLinkCount(); for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = links[linkID]; const ArticulationJointCoreData& jointDatum = jointData[linkID]; const ArticulationLink& pLink = links[link.parent]; const PxTransform pBody2World = pLink.bodyCore->body2World; const ArticulationJointCore* joint = link.inboundJoint; const PxReal* jPosition = &jointPositions[jointDatum.jointOffset]; PxQuat newParentToChild; PxQuat newWorldQ; PxVec3 r; const PxVec3 childOffset = -joint->childPose.p; const PxVec3 parentOffset = joint->parentPose.p; const PxQuat relativeQuat = mArticulationData.mRelativeQuat[linkID]; switch (joint->jointType) { case PxArticulationJointType::ePRISMATIC: { newParentToChild = relativeQuat; const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; const PxVec3& u = data.mMotionMatrix[jointDatum.jointOffset].bottom; r = e + d + u * jPosition[0]; break; } case PxArticulationJointType::eREVOLUTE: case PxArticulationJointType::eREVOLUTE_UNWRAPPED: { const PxVec3& u = data.mMotionMatrix[jointDatum.jointOffset].top; PxQuat jointRotation = PxQuat(-jPosition[0], u); if (jointRotation.w < 0) //shortest angle. jointRotation = -jointRotation; newParentToChild = (jointRotation * relativeQuat).getNormalized(); const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; PX_ASSERT(r.isFinite()); break; } case PxArticulationJointType::eSPHERICAL: { PxQuat jointRotation(PxIdentity); { PxVec3 axis(0.f); for (PxU32 d = 0; d < jointDatum.nbDof; ++d) { axis += data.mMotionMatrix[jointDatum.jointOffset + d].top * -jPosition[d]; } PxReal angle = axis.normalize(); jointRotation = angle < 1e-10f ? PxQuat(PxIdentity) : PxQuat(angle, axis); if(jointRotation.w < 0.f) jointRotation = -jointRotation; } newParentToChild = (jointRotation * relativeQuat).getNormalized(); const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; break; } case PxArticulationJointType::eFIX: { //this is fix joint so joint don't have velocity newParentToChild = relativeQuat; const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; break; } default: break; } PxTransform& body2World = link.bodyCore->body2World; body2World.q = (pBody2World.q * newParentToChild.getConjugate()).getNormalized(); body2World.p = pBody2World.p + body2World.q.rotate(r); PX_ASSERT(body2World.isSane()); } } void FeatherstoneArticulation::computeLinkVelocities(ArticulationData& data) { ArticulationLink* links = data.getLinks(); const PxU32 linkCount = data.getLinkCount(); Cm::SpatialVectorF* motionVelocities = data.getMotionVelocities(); const PxReal* jointVelocities = data.mJointVelocity.begin(); // sync root motion vel: const PxsBodyCore& rootBodyCore = *links[0].bodyCore; motionVelocities[0].top = rootBodyCore.angularVelocity; motionVelocities[0].bottom = rootBodyCore.linearVelocity; for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = links[linkID]; PxsBodyCore& bodyCore = *link.bodyCore; PxTransform body2World = bodyCore.body2World; ArticulationLink& plink = links[link.parent]; const PxsBodyCore& pbodyCore = *plink.bodyCore; Cm::SpatialVectorF parentVel(pbodyCore.angularVelocity, pbodyCore.linearVelocity); PxTransform pBody2World = pbodyCore.body2World; const PxVec3 rw = body2World.p - pBody2World.p; Cm::SpatialVectorF vel = FeatherstoneArticulation::translateSpatialVector(-rw,parentVel); if (jointVelocities) { ArticulationJointCoreData& jointDatum = data.getJointData(linkID); const PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset]; Cm::UnAlignedSpatialVector deltaV = Cm::UnAlignedSpatialVector::Zero(); for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind) { deltaV += data.mMotionMatrix[jointDatum.jointOffset + ind] * jVelocity[ind]; } vel.top += body2World.q.rotate(deltaV.top); vel.bottom += body2World.q.rotate(deltaV.bottom); } bodyCore.linearVelocity = vel.bottom; bodyCore.angularVelocity = vel.top; motionVelocities[linkID] = vel; } } // AD: needed because we define the templated function in a CPP. template void FeatherstoneArticulation::jcalc(ArticulationData& data); template void FeatherstoneArticulation::jcalc(ArticulationData& data); template void FeatherstoneArticulation::jcalc(ArticulationData& data) { const ArticulationLink* links = data.getLinks(); ArticulationJointCoreData* jointData = data.getJointData(); const PxU32 linkCount = data.getLinkCount(); PxU32 totalDof = 0; for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = links[linkID]; ArticulationJointCore* joint = link.inboundJoint; ArticulationJointCoreData& jointDatum = jointData[linkID]; PX_CHECK_AND_RETURN(joint->jointType != PxArticulationJointType::eUNDEFINED, "FeatherstoneArticulation::jcalc application need to define valid joint type and motion"); // AD: only used in immediate mode because we don't have the llArticulation there to write directly. if (immediateMode) { // compute joint dof const PxU32 dof = jointDatum.configureJointDofs(joint, data.mJointAxis.begin() + totalDof); PxReal* PX_RESTRICT jointTargetPositions = data.getJointTargetPositions(); PxReal* PX_RESTRICT jointTargetVelocities = data.getJointTargetVelocities(); for(PxU32 dofId = 0; dofId < dof; ++dofId) { PxU32 id = totalDof + dofId; PxU32 llDofId = joint->dofIds[dofId]; jointTargetPositions[id] = joint->targetP[llDofId]; jointTargetVelocities[id] = joint->targetV[llDofId]; } jointDatum.jointOffset = totalDof; joint->jointOffset = totalDof; totalDof += dof; } joint->setJointFrame(&data.mMotionMatrix[jointDatum.jointOffset], &data.mJointAxis[jointDatum.jointOffset], mArticulationData.mRelativeQuat[linkID], jointDatum.nbDof); } if(immediateMode) { if(totalDof != mArticulationData.getDofs()) { mArticulationData.resizeJointData(totalDof); } mArticulationData.setDofs(totalDof); } } //compute link's spatial inertia tensor void FeatherstoneArticulation::computeSpatialInertia(ArticulationData& data) { for (PxU32 linkID = 0; linkID < data.getLinkCount(); ++linkID) { const ArticulationLink& link = data.getLink(linkID); //ArticulationLinkData& linkDatum = data.getLinkData(linkID); const PxsBodyCore& core = *link.bodyCore; const PxVec3& ii = core.inverseInertia; const PxReal m = core.inverseMass == 0.f ? 0.f : 1.0f / core.inverseMass; SpatialMatrix& worldArticulatedInertia = data.mWorldSpatialArticulatedInertia[linkID]; //construct inertia matrix const PxVec3 inertiaTensor = PxVec3(ii.x == 0.f ? 0.f : (1.f / ii.x), ii.y == 0.f ? 0.f : (1.f / ii.y), ii.z == 0.f ? 0.f : (1.f / ii.z)); PxMat33 rot(data.getLink(linkID).bodyCore->body2World.q); worldArticulatedInertia.topLeft = PxMat33(PxZero); worldArticulatedInertia.topRight = PxMat33::createDiagonal(PxVec3(m)); Cm::transformInertiaTensor(inertiaTensor, rot, worldArticulatedInertia.bottomLeft); data.mWorldIsolatedSpatialArticulatedInertia[linkID] = worldArticulatedInertia.bottomLeft; data.mMasses[linkID] = m; } } void FeatherstoneArticulation::computeZ(const ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData) { const Cm::SpatialVectorF* motionVelocities = scratchData.motionVelocities; Cm::SpatialVectorF* spatialZAForces = scratchData.spatialZAVectors; const Cm::SpatialVector* externalAccels = scratchData.externalAccels; for (PxU32 linkID = 0; linkID < data.getLinkCount(); ++linkID) { ArticulationLink& link = data.getLink(linkID); const PxsBodyCore& core = *link.bodyCore; //const PxTransform& body2World = core.body2World; const PxMat33& I = data.mWorldSpatialArticulatedInertia[linkID].bottomLeft; //construct spatial zero acceleration Cm::SpatialVectorF& z = spatialZAForces[linkID]; //Cm::SpatialVectorF v; //v.top = motionVelocities[linkID].top; //v.bottom = motionVelocities[linkID].bottom; //KS - limit the magnitude of the angular velocity that contributes to the geometric term. This is a //v^2 term and can become unstable if it is too large! Cm::SpatialVectorF v = motionVelocities[linkID]; PxVec3 vA = v.top; PxVec3 gravLinAccel(0.f); if(!core.disableGravity) gravLinAccel = -gravity; PX_ASSERT(core.inverseMass != 0.f); const PxReal m = 1.0f / core.inverseMass; Cm::SpatialVectorF zTmp; zTmp.top = (gravLinAccel * m); zTmp.bottom = vA.cross(I * vA); PX_ASSERT(zTmp.top.isFinite()); PX_ASSERT(zTmp.bottom.isFinite()); if (externalAccels) { const Cm::SpatialVector& externalAccel = externalAccels[linkID]; const PxVec3 exLinAccel = -externalAccel.linear; const PxVec3 exAngAccel = -externalAccel.angular; zTmp.top += (exLinAccel * m); zTmp.bottom += I * exAngAccel; } z = zTmp; } } void FeatherstoneArticulation::computeZD(const ArticulationData& data, const PxVec3& gravity, ScratchData& scratchData) { const Cm::SpatialVectorF* motionVelocities = scratchData.motionVelocities; Cm::SpatialVectorF* spatialZAForces = scratchData.spatialZAVectors; const Cm::SpatialVector* externalAccels = scratchData.externalAccels; const PxReal dt = data.getDt(); const PxReal invDt = dt < 1e-6f ? PX_MAX_F32 : 1.f / data.mDt; for (PxU32 linkID = 0; linkID < data.getLinkCount(); ++linkID) { ArticulationLink& link = data.getLink(linkID); const PxsBodyCore& core = *link.bodyCore; //const PxTransform& body2World = core.body2World; const PxMat33& I = data.mWorldSpatialArticulatedInertia[linkID].bottomLeft; //construct spatial zero acceleration Cm::SpatialVectorF& z = spatialZAForces[linkID]; //Cm::SpatialVectorF v; //v.top = motionVelocities[linkID].top; //v.bottom = motionVelocities[linkID].bottom; //KS - limit the magnitude of the angular velocity that contributes to the geometric term. This is a //v^2 term and can become unstable if it is too large! Cm::SpatialVectorF v = motionVelocities[linkID]; PxVec3 vA = v.top; PxVec3 gravLinAccel(0.f); if (!core.disableGravity) gravLinAccel = -gravity; PX_ASSERT(core.inverseMass != 0.f); const PxReal m = 1.0f / core.inverseMass; Cm::SpatialVectorF zTmp; zTmp.top = (gravLinAccel * m); zTmp.bottom = vA.cross(I * vA); PX_ASSERT(zTmp.top.isFinite()); PX_ASSERT(zTmp.bottom.isFinite()); if (externalAccels) { const Cm::SpatialVector& externalAccel = externalAccels[linkID]; const PxVec3 exLinAccel = -externalAccel.linear; const PxVec3 exAngAccel = -externalAccel.angular; zTmp.top += (exLinAccel * m); zTmp.bottom += I * exAngAccel; } if (core.linearDamping > 0.f || core.angularDamping > 0.f) { const PxReal linDamp = PxMin(core.linearDamping, invDt); const PxReal angDamp = PxMin(core.angularDamping, invDt); zTmp.top += (v.bottom * linDamp*m) - zTmp.top * linDamp*dt; zTmp.bottom += I * (v.top* angDamp) - zTmp.bottom * angDamp*dt; } const PxReal maxAng = core.maxAngularVelocitySq; const PxReal maxLin = core.maxLinearVelocitySq; const PxReal angMag = v.top.magnitudeSquared(); const PxReal linMag = v.bottom.magnitudeSquared(); if (angMag > maxAng || linMag > maxLin) { if (angMag > maxAng) { const PxReal scale = 1.f - PxSqrt(maxAng) / PxSqrt(angMag); const PxVec3 tmpaccelerationAng = (I * v.top)*scale; zTmp.bottom += tmpaccelerationAng*invDt; } if (linMag > maxLin) { const PxReal scale = 1.f - (PxSqrt(maxLin) / PxSqrt(linMag)); const PxVec3 tmpaccelerationLin = (v.bottom*m*scale); PX_UNUSED(tmpaccelerationLin); zTmp.top += tmpaccelerationLin*invDt; } } z = zTmp; } } //compute coriolis and centrifugal term void FeatherstoneArticulation::computeC(ArticulationData& data, ScratchData& scratchData) { const PxReal* jointVelocities = scratchData.jointVelocities; Cm::SpatialVectorF* coriolisVectors = scratchData.coriolisVectors; const PxU32 linkCount = data.getLinkCount(); coriolisVectors[0] = Cm::SpatialVectorF::Zero(); for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = data.getLink(linkID); const ArticulationJointCoreData& jointDatum = data.getJointData(linkID); const PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset]; Cm::SpatialVectorF& coriolis = coriolisVectors[linkID]; //const PxTransform& body2World = link.bodyCore->body2World; //transform parent link's angular velocity into current link's body space const PxVec3 pAngular = scratchData.motionVelocities[link.parent].top; PxVec3 torque = pAngular.cross(pAngular.cross(data.getRw(linkID))); //PX_ASSERT(parentAngular.magnitude() < 100.f); PxVec3 force(0.f); if (jointDatum.nbDof > 0) { Cm::SpatialVectorF relVel(PxVec3(0.f), PxVec3(0.f)); for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind) { //Clamp joint velocity used in coriolis terms to reduce chances of unstable feed-back loops const PxReal jV = jVelocity[ind]; relVel.top += data.mWorldMotionMatrix[jointDatum.jointOffset + ind].top * jV; relVel.bottom += data.mWorldMotionMatrix[jointDatum.jointOffset + ind].bottom * jV; } const PxVec3 aVec = relVel.top; force = pAngular.cross(aVec); //compute linear part const PxVec3 lVel = relVel.bottom; const PxVec3 temp1 = 2.f * pAngular.cross(lVel); const PxVec3 temp2 = aVec.cross(lVel); torque += temp1 + temp2; } PX_ASSERT(force.isFinite()); PX_ASSERT(torque.isFinite()); coriolis = Cm::SpatialVectorF(force, torque);//.rotateInv(body2World); } } void FeatherstoneArticulation::computeRelativeTransformC2P( const ArticulationLink* links, const PxU32 linkCount, const ArticulationJointCoreData* jointCoreDatas, const Cm::UnAlignedSpatialVector* jonitDofMotionMatrices, PxTransform* linkAccumulatedPoses, PxVec3* linkRws, Cm::UnAlignedSpatialVector* jointDofMotionMatricesW) { linkAccumulatedPoses[0] = links[0].bodyCore->body2World; for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = links[linkID]; const PxsBodyCore& bodyCore = *link.bodyCore; const PxU32 jointOffset = jointCoreDatas[linkID].jointOffset; const PxU32 dofCount = jointCoreDatas[linkID].nbDof; const PxTransform& body2World = bodyCore.body2World; const ArticulationLink& pLink = links[link.parent]; const PxsBodyCore& pBodyCore = *pLink.bodyCore; const PxTransform& pBody2World = pBodyCore.body2World; //const PxTransform tC2P = pBody2World.transformInv(body2World).getNormalized(); linkRws[linkID] = body2World.p - pBody2World.p; const Cm::UnAlignedSpatialVector* motionMatrix = &jonitDofMotionMatrices[jointOffset]; Cm::UnAlignedSpatialVector* worldMotionMatrix = &jointDofMotionMatricesW[jointOffset]; for (PxU32 i = 0; i < dofCount; ++i) { const Cm::UnAlignedSpatialVector worldRow = motionMatrix[i].rotate(body2World); worldMotionMatrix[i] = worldRow; } linkAccumulatedPoses[linkID] = body2World; #if FEATURESTONE_DEBUG { //debug PxMat33 pToC = c2p.getTranspose(); //parentToChild -rR PxMat33 T2 = skewMatrixPR * pToC; PX_ASSERT(SpatialMatrix::isTranspose(linkDatum.childToParent.T, T2)); } #endif } } void FeatherstoneArticulation::computeRelativeTransformC2B(ArticulationData& data) { ArticulationLink* links = data.getLinks(); ArticulationLinkData* linkData = data.getLinkData(); const PxU32 linkCount = data.getLinkCount(); const ArticulationLink& bLink = links[0]; const PxTransform& bBody2World = bLink.bodyCore->body2World; for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = links[linkID]; ArticulationLinkData& linkDatum = linkData[linkID]; const PxsBodyCore& bodyCore = *link.bodyCore; const PxTransform& body2World = bodyCore.body2World; const PxVec3 rw = body2World.p - bBody2World.p;//body space of link i //rotation matrix cToP's inverse is rotation matrix pToC linkDatum.childToBase = rw; } } void FeatherstoneArticulation::getDenseJacobian(PxArticulationCache& cache, PxU32 & nRows, PxU32 & nCols) { //make sure motionMatrix has been set //jcalc(mArticulationData); initializeCommonData(); const PxU32 linkCount = mArticulationData.getLinkCount(); ArticulationLink* links = mArticulationData.getLinks(); #if 0 //Enable this if you want to compare results of this function with results of computeLinkVelocities(). if (mArticulationData.getDataDirty()) { PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "Articulation::getDenseJacobian(): commonInit need to be called first to initialize data!"); return; } PxcScratchAllocator* allocator = reinterpret_cast(cache.scratchAllocator); ScratchData scratchData; PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData); scratchData.jointVelocities = mArticulationData.getJointVelocities(); computeLinkVelocities(mArticulationData, scratchData); const ArticulationLink& baseLink = links[0]; PxsBodyCore& core0 = *baseLink.bodyCore; #endif ArticulationLinkData* linkData = mArticulationData.getLinkData(); const PxU32 totalDofs = getDofs(); const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE; //matrix dims is nCols = (fixBase ? 0 : 6) + totalDofs; nRows = (fixBase ? 0 : 6) + (linkCount - 1) * 6; // auto jacobian = [&](PxU32 row, PxU32 col) -> PxReal & { return cache.denseJacobian[nCols * row + col]; } ; #define jacobian(row, col) cache.denseJacobian[nCols * (row) + (col)] PxU32 destRow = 0; PxU32 destCol = 0; if (!fixBase) { jacobian(0, 0) = 1.0f; jacobian(0, 1) = 0.0f; jacobian(0, 2) = 0.0f; jacobian(0, 3) = 0.0f; jacobian(0, 4) = 0.0f; jacobian(0, 5) = 0.0f; jacobian(1, 0) = 0.0f; jacobian(1, 1) = 1.0f; jacobian(1, 2) = 0.0f; jacobian(1, 3) = 0.0f; jacobian(1, 4) = 0.0f; jacobian(1, 5) = 0.0f; jacobian(2, 0) = 0.0f; jacobian(2, 1) = 0.0f; jacobian(2, 2) = 1.0f; jacobian(2, 3) = 0.0f; jacobian(2, 4) = 0.0f; jacobian(2, 5) = 0.0f; jacobian(3, 0) = 0.0f; jacobian(3, 1) = 0.0f; jacobian(3, 2) = 0.0f; jacobian(3, 3) = 1.0f; jacobian(3, 4) = 0.0f; jacobian(3, 5) = 0.0f; jacobian(4, 0) = 0.0f; jacobian(4, 1) = 0.0f; jacobian(4, 2) = 0.0f; jacobian(4, 3) = 0.0f; jacobian(4, 4) = 1.0f; jacobian(4, 5) = 0.0f; jacobian(5, 0) = 0.0f; jacobian(5, 1) = 0.0f; jacobian(5, 2) = 0.0f; jacobian(5, 3) = 0.0f; jacobian(5, 4) = 0.0f; jacobian(5, 5) = 1.0f; destRow += 6; destCol += 6; } for (PxU32 linkID = 1; linkID < linkCount; ++linkID)//each iteration of this writes 6 rows in the matrix { const ArticulationLink& link = links[linkID]; ArticulationLinkData& linkDatum = linkData[linkID]; const PxsBodyCore& bodyCore = *link.bodyCore; linkDatum.maxPenBias = bodyCore.maxPenBias; const PxTransform& body2World = bodyCore.body2World; const ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID); const PxU32 parentLinkID = link.parent; if (parentLinkID || !fixBase) { // VR: mArticulationData.getJointData(0) isn't initialized. parentLinkID can be 0 if fixBase is false. //const ArticulationJointCoreData& parentJointDatum = mArticulationData.getJointData(parentLinkID); //const PxU32 parentsFirstDestCol = parentJointDatum.jointOffset + (fixBase ? 0 : 6); //const PxU32 parentsLastDestCol = parentsFirstDestCol + parentJointDatum.dof; const PxU32 parentsLastDestCol = parentLinkID ? mArticulationData.getJointData(parentLinkID).jointOffset + (fixBase ? 0 : 6) + mArticulationData.getJointData(parentLinkID).nbDof : 6; // VR: With parentLinkID == 0 this expression has two unsigned integer overflows, but the result is still correct. const PxU32 parentsDestRow = (fixBase ? 0 : 6) + (parentLinkID - 1) * 6; for (PxU32 col = 0; col < parentsLastDestCol; col++) { //copy downward the 6 cols from parent const PxVec3 parentAng( jacobian(parentsDestRow + 3, col), jacobian(parentsDestRow + 4, col), jacobian(parentsDestRow + 5, col) ); const PxVec3 parentAngxRw = parentAng.cross(mArticulationData.getRw(linkID)); jacobian(destRow + 0, col) = jacobian(parentsDestRow + 0, col) + parentAngxRw.x; jacobian(destRow + 1, col) = jacobian(parentsDestRow + 1, col) + parentAngxRw.y; jacobian(destRow + 2, col) = jacobian(parentsDestRow + 2, col) + parentAngxRw.z; jacobian(destRow + 3, col) = parentAng.x; jacobian(destRow + 4, col) = parentAng.y; jacobian(destRow + 5, col) = parentAng.z; } for (PxU32 col = parentsLastDestCol; col < destCol; col++) { //fill with zeros. jacobian(destRow + 0, col) = 0.0f; jacobian(destRow + 1, col) = 0.0f; jacobian(destRow + 2, col) = 0.0f; jacobian(destRow + 3, col) = 0.0f; jacobian(destRow + 4, col) = 0.0f; jacobian(destRow + 5, col) = 0.0f; } } //diagonal block: for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind) { const Cm::UnAlignedSpatialVector& v = mArticulationData.mMotionMatrix[jointDatum.jointOffset + ind]; const PxVec3 ang = body2World.rotate(v.top); const PxVec3 lin = body2World.rotate(v.bottom); jacobian(destRow + 0, destCol) = lin.x; jacobian(destRow + 1, destCol) = lin.y; jacobian(destRow + 2, destCol) = lin.z; jacobian(destRow + 3, destCol) = ang.x; jacobian(destRow + 4, destCol) = ang.y; jacobian(destRow + 5, destCol) = ang.z; destCol++; } //above diagonal block: always zero for (PxU32 col = destCol; col < nCols; col++) { jacobian(destRow + 0, col) = 0.0f; jacobian(destRow + 1, col) = 0.0f; jacobian(destRow + 2, col) = 0.0f; jacobian(destRow + 3, col) = 0.0f; jacobian(destRow + 4, col) = 0.0f; jacobian(destRow + 5, col) = 0.0f; } destRow += 6; } #undef jacobian #if 0 //Enable this if you want to compare results of this function with results of computeLinkVelocities(). PxReal * jointVels = mArticulationData.getJointVelocities();//size is totalDofs PxReal * jointSpaceVelsVector = new PxReal[nCols]; PxReal * worldSpaceVelsVector = new PxReal[nRows]; PxU32 offset = 0; //stack input: if (!fixBase) { jointSpaceVelsVector[0] = core0.linearVelocity[0]; jointSpaceVelsVector[1] = core0.linearVelocity[1]; jointSpaceVelsVector[2] = core0.linearVelocity[2]; jointSpaceVelsVector[3] = core0.angularVelocity[0]; jointSpaceVelsVector[4] = core0.angularVelocity[1]; jointSpaceVelsVector[5] = core0.angularVelocity[2]; offset = 6; } for (PxU32 i = 0; i < totalDofs; i++) jointSpaceVelsVector[i + offset] = jointVels[i]; //multiply: for (PxU32 row = 0; row < nRows; row++) { worldSpaceVelsVector[row] = 0.0f; for (PxU32 col = 0; col < nCols; col++) { worldSpaceVelsVector[row] += jacobian(row, col)*jointSpaceVelsVector[col]; } } //worldSpaceVelsVector should now contain the same result as scratchData.motionVelocities (except for swapped linear/angular vec3 order). delete[] jointSpaceVelsVector; delete[] worldSpaceVelsVector; allocator->free(tempMemory); #endif } void FeatherstoneArticulation::computeLinkStates( const PxF32 dt, const PxReal invLengthScale, const PxVec3& gravity, const bool fixBase, const PxU32 linkCount, const PxTransform* linkAccumulatedPosesW, Cm::SpatialVector* linkExternalAccelsW, const PxVec3* linkRsW, const Cm::UnAlignedSpatialVector* jointDofMotionMatricesW, const Dy::ArticulationJointCoreData* jointCoreData, bool externalForcesEveryTgsIterationEnabled, Dy::ArticulationLinkData *linkData, Dy::ArticulationLink* links, Cm::SpatialVectorF* linkMotionAccelerationsW, Cm::SpatialVectorF* linkMotionVelocitiesW, Cm::SpatialVectorF* linkZAExtForcesW, Cm::SpatialVectorF* linkZAIntForcesW, Cm::SpatialVectorF* linkCoriolisVectorsW, PxMat33* linkIsolatedSpatialArticulatedInertiasW, PxF32* linkMasses, Dy::SpatialMatrix* linkSpatialArticulatedInertiasW, PxReal* jointDofVelocities, Cm::SpatialVectorF& rootPreMotionVelocityW, PxVec3& comW, PxF32& invSumMass) { const PxReal invDt = dt < 1e-6f ? PX_MAX_F32 : 1.f / dt; //Initialise motion velocity, motion acceleration and coriolis vector of root link. Cm::SpatialVectorF rootLinkVel; { const Dy::ArticulationLink& baseLink = links[0]; const PxsBodyCore& core0 = *baseLink.bodyCore; rootLinkVel = fixBase ? Cm::SpatialVectorF::Zero() : Cm::SpatialVectorF(core0.angularVelocity, core0.linearVelocity); linkMotionVelocitiesW[0] = rootLinkVel; linkMotionAccelerationsW[0] = fixBase ? Cm::SpatialVectorF::Zero() : linkMotionAccelerationsW[0]; linkCoriolisVectorsW[0] = Cm::SpatialVectorF::Zero(); rootPreMotionVelocityW = rootLinkVel; } //Is it really necessary? It is already resolved as an internal cosntraint. PxReal ratio = 1.f; if (jointDofVelocities) { for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = links[linkID]; const ArticulationJointCoreData& jointDatum = jointCoreData[linkID]; const PxReal* jVelocity = &jointDofVelocities[jointDatum.jointOffset]; for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind) { const PxReal maxJVelocity = link.inboundJoint->maxJointVelocity[ind]; PxReal jVel = jVelocity[ind]; ratio = (jVel != 0.0f) ? PxMin(ratio, maxJVelocity / PxAbs(jVel)) : ratio; } } } PxReal sumMass = 0.f; PxVec3 COM(0.f); for (PxU32 linkID = 0; linkID < linkCount; ++linkID) { ArticulationLink& link = links[linkID]; ArticulationLinkData& linkDatum = linkData[linkID]; const PxsBodyCore& bodyCore = *link.bodyCore; //Set the maxPemBias and cfm values from bodyCore linkDatum.maxPenBias = bodyCore.maxPenBias; link.cfm = (fixBase && linkID == 0) ? 0.f : bodyCore.cfmScale * invLengthScale; //Read the inertia and mass from bodyCore const PxVec3& ii = bodyCore.inverseInertia; const PxVec3 inertiaTensor = PxVec3(ii.x == 0.f ? 0.f : (1.f / ii.x), ii.y == 0.f ? 0.f : (1.f / ii.y), ii.z == 0.f ? 0.f : (1.f / ii.z)); const PxReal invMass = bodyCore.inverseMass; const PxReal m = invMass == 0.f ? 0.f : 1.0f / invMass; //Compute the inertia matrix Iw = R * I * Rtranspose //Compute the articulated inertia. PxMat33 Iw; //R * I * Rtranspose SpatialMatrix worldArticulatedInertia; { PxMat33 rot(linkAccumulatedPosesW[linkID].q); Cm::transformInertiaTensor(inertiaTensor, rot, Iw); worldArticulatedInertia.topLeft = PxMat33(PxZero); worldArticulatedInertia.topRight = PxMat33::createDiagonal(PxVec3(m)); worldArticulatedInertia.bottomLeft = Iw; } //Set the articulated inertia, inertia and mass of the link. linkSpatialArticulatedInertiasW[linkID] = worldArticulatedInertia; linkIsolatedSpatialArticulatedInertiasW[linkID] = Iw; linkMasses[linkID] = m; //Accumulate the centre of mass. sumMass += m; COM += linkAccumulatedPosesW[linkID].p * m; Cm::SpatialVectorF vel; if (linkID != 0) { //Propagate spatial velocity of link parent to link's spatial velocity. const Cm::SpatialVectorF pVel = linkMotionVelocitiesW[link.parent]; vel = FeatherstoneArticulation::translateSpatialVector(-linkRsW[linkID], pVel); //Propagate joint dof velocities to the link's spatial velocity vector. //Accumulate spatial forces that the joint applies to the link. if (jointDofVelocities) { //The coriolis vector depends on the type of joint and the joint motion matrix. //However, some terms in the coriolis vector are common to all joint types. //Write down the term that is independent of the joint. Cm::SpatialVectorF coriolisVector(PxVec3(PxZero), pVel.top.cross(pVel.top.cross(linkRsW[linkID]))); const ArticulationJointCoreData& jointDatum = jointCoreData[linkID]; if (jointDatum.nbDof) { //Compute the effect of the joint velocities on the link. PxReal* jVelocity = &jointDofVelocities[jointDatum.jointOffset]; Cm::UnAlignedSpatialVector deltaV = Cm::UnAlignedSpatialVector::Zero(); for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind) { PxReal jVel = jVelocity[ind] * ratio; deltaV += jointDofMotionMatricesW[jointDatum.jointOffset + ind] * jVel; jVelocity[ind] = jVel; } //Add the effect of the joint velocities to the link. vel.top += deltaV.top; vel.bottom += deltaV.bottom; //Compute the coriolis vector. //mu(i) is the velocity arising from the joint motion: mu(i) = motionMatrix*qdot //where qdot is the joint velocity. //mu(i) is already computed and cached in deltaV. //For the revolute joint we have: //deltaV = {mu(i), mu(i) X d(i)} //coriolis vector += {omega(i-1) X mu(i), 2*omega(i-1) X (mu(i) X d(i)) + mu(i) X (mu(i) X d(i))} //For the prismatic joint we have: //deltaV = {0, mu(i)} //coriolis vector += {0, 2*omega(i-1) X mu(i)} coriolisVector += Cm::SpatialVectorF(pVel.top.cross(deltaV.top), 2.0f*pVel.top.cross(deltaV.bottom) + deltaV.top.cross(deltaV.bottom)); } //TODOGY - if jointVelocities is null we do not appear to set coriolisVectors[linkId] but we do set coriolisVectors[0] linkCoriolisVectorsW[linkID] = coriolisVector; } //PX_ASSERT(vel.top.isFinite() && PxAbs(vel.top.x) < 10000.f && PxAbs(vel.top.y) < 10000.f && PxAbs(vel.top.z) < 10000.f); //PX_ASSERT(vel.bottom.isFinite() && PxAbs(vel.bottom.x) < 10000.f && PxAbs(vel.bottom.y) < 10000.f && PxAbs(vel.bottom.z) < 10000.f); linkMotionVelocitiesW[linkID] = vel; } else { vel = rootLinkVel; //Note: we have already set motionVelocities[0] and coriolisVectors[0] so no need to set them again. } //Account for force arising from external accelerations //Account for damping force arising from the velocity and from the velocity that will accumulate from the acceleration terms. //Account for scaling force that will bring velocity back to the maximum allowed velocity if velocity exceed the maximum allowed. //Example for the linear term: //acceleration force = m*(g + extLinAccel) //linVelNew = linVel + (g + extLinAccel)*dt //damping force = -m*linVelNew*linDamp = -m*linVel*linDamp - m*(g + extLinAccel)*linDamp*dt //scaling force = -m*linVel*linScale/dt with linScale = (1.0f - maxLinVel/linVel) //Put it all together // m*(g + extLinAccel)*(1 - linDamp*dt) - m*linVel*(linDamp + linScale/dt) //Bit more reordering: //m*[g + extLinAccel)*(1 - linDamp*dt) - linVel*(linDamp + linScale/dt)] //Zero acceleration means we need to work against change: //-m*[g + extLinAccel)*(1 - linDamp*dt) - linVel*(linDamp + linScale/dt)] Cm::SpatialVectorF zExtForces; Cm::SpatialVectorF zDamping; { const PxVec3 g = bodyCore.disableGravity ? PxVec3(PxZero) : gravity; const PxVec3 extLinAccel = linkExternalAccelsW ? linkExternalAccelsW[linkID].linear : PxVec3(PxZero); const PxF32 lindamp = bodyCore.linearDamping > 0.f ? PxMin(bodyCore.linearDamping, invDt) : 0.0f; const PxF32 linscale = (vel.bottom.magnitudeSquared() > bodyCore.maxLinearVelocitySq) ? (1.0f - (PxSqrt(bodyCore.maxLinearVelocitySq)/PxSqrt(vel.bottom.magnitudeSquared()))): 0.0f; zExtForces.top = (m * (lindamp * dt - 1.0f)) * (g + extLinAccel); zDamping.top = (m * (lindamp + linscale * invDt)) * vel.bottom; } { const PxVec3 extAngAccel = linkExternalAccelsW ? linkExternalAccelsW[linkID].angular : PxVec3(PxZero); const PxF32 angdamp = bodyCore.angularDamping > 0.f ? PxMin(bodyCore.angularDamping, invDt) : 0.0f; const PxF32 angscale = (vel.top.magnitudeSquared() > bodyCore.maxAngularVelocitySq) ? (1.0f - (PxSqrt(bodyCore.maxAngularVelocitySq)/PxSqrt(vel.top.magnitudeSquared()))) : 0.0f; zExtForces.bottom = Iw * (extAngAccel * (angdamp * dt - 1.0f)); zDamping.bottom = Iw * (vel.top * (angdamp + angscale * invDt)); } if(externalForcesEveryTgsIterationEnabled) { // We precompute the isolated z vector for gravity and ext forces here so we don't need to load inertias in the per-substep application. // The ugly thing is that we start storing spatial forces into the spatial motion vector linkExternalAccelsW. // Reusing linkExternalAccelsW is ok because at the end of the sim step they are anyways cleared or refreshed from velMod in Sc::ArticulationSim::clearAcceleration // and inverse dynamics does not use it. linkZAExtForcesW[linkID] = zDamping; // only apply damping once for the full step linkExternalAccelsW[linkID].linear = zExtForces.top; // must use the same order (linear<=>top) again when picking this up in the substep application linkExternalAccelsW[linkID].angular = zExtForces.bottom; } else { linkZAExtForcesW[linkID] = zExtForces + zDamping; } //Account for forces arising from internal accelerations. //Note: Mirtich thesis introduces a single spatial zero acceleration force that contains an external [mass*gravity] term and the internal [omega X (Iw *omega)] term. //In Mirtich it looks like this: // [-m_i * g ] // [omega_i * (I_i * omega_i) ] //We split the spatial zero acceleration force into external (above) and internal (below). //The sum of the two (external and internal) corresponds to Z_i in the Mirtich formulation. // In this function, we're only computing the isolated zero acceleration Z_i but already store // it in the Z_i^A (articulated) fields as they will be accumulated later. const Cm::SpatialVectorF zInternal(PxVec3(0.f), vel.top.cross(Iw*vel.top)); linkZAIntForcesW[linkID] = zInternal; } PxReal invMass = 1.f / sumMass; comW = COM * invMass; invSumMass = invMass; } //compute all links velocities void FeatherstoneArticulation::computeLinkVelocities(ArticulationData& data, ScratchData& scratchData) { Cm::SpatialVectorF* coriolisVectors = scratchData.coriolisVectors; ArticulationLink* links = data.getLinks(); ArticulationLinkData* linkData = data.getLinkData(); const PxU32 linkCount = data.getLinkCount(); const bool fixBase = data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE; //motion velocities has to be in world space to avoid numerical errors caused by space Cm::SpatialVectorF* motionVelocities = scratchData.motionVelocities; Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations; PxReal* jointVelocities = scratchData.jointVelocities; const ArticulationLink& baseLink = links[0]; ArticulationLinkData& baseLinkDatum = linkData[0]; PxsBodyCore& core0 = *baseLink.bodyCore; baseLinkDatum.maxPenBias = core0.maxPenBias; if (fixBase) { motionVelocities[0] = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f)); motionAccelerations[0] = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f)); } else { motionVelocities[0] = Cm::SpatialVectorF(core0.angularVelocity, core0.linearVelocity); //PX_ASSERT(core0.angularVelocity.isFinite() && PxAbs(core0.angularVelocity.x) < 10000.f && PxAbs(core0.angularVelocity.y) < 10000.f && PxAbs(core0.angularVelocity.z) < 10000.f); //PX_ASSERT(core0.linearVelocity.isFinite() && PxAbs(core0.linearVelocity.x) < 10000.f && PxAbs(core0.linearVelocity.y) < 10000.f && PxAbs(core0.linearVelocity.z) < 10000.f); } coriolisVectors[0] = Cm::SpatialVectorF::Zero(); data.mRootPreMotionVelocity = motionVelocities[0]; //Is it really necessary? It is already resolved as an internal cosntraint. //const PxU32 dofCount = data.mDofs; PxReal ratio = 1.f; if (jointVelocities) { for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = links[linkID]; ArticulationJointCoreData& jointDatum = data.getJointData(linkID); PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset]; for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind) { const PxReal maxJVelocity = link.inboundJoint->maxJointVelocity[ind]; PxReal absJvel = PxAbs(jVelocity[ind]); ratio = ratio * absJvel > maxJVelocity ? (maxJVelocity / absJvel) : ratio; } } } for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { const ArticulationLink& link = links[linkID]; ArticulationLinkData& linkDatum = linkData[linkID]; const PxsBodyCore& bodyCore = *link.bodyCore; linkDatum.maxPenBias = bodyCore.maxPenBias; const Cm::SpatialVectorF pVel = motionVelocities[link.parent]; Cm::SpatialVectorF vel = FeatherstoneArticulation::translateSpatialVector(-mArticulationData.getRw(linkID), pVel); const PxTransform& body2World = bodyCore.body2World; if (jointVelocities) { PxVec3 torque = pVel.top.cross(pVel.top.cross(data.getRw(linkID))); ArticulationJointCoreData& jointDatum = data.getJointData(linkID); PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset]; PxVec3 force(0.f); if (jointDatum.nbDof) { Cm::UnAlignedSpatialVector deltaV = Cm::UnAlignedSpatialVector::Zero(); for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind) { PxReal jVel = jVelocity[ind] * ratio; //deltaV += data.mWorldMotionMatrix[jointDatum.jointOffset + ind] * jVel; deltaV += data.mMotionMatrix[jointDatum.jointOffset+ind].rotate(body2World) * jVel; jVelocity[ind] = jVel; } vel.top += deltaV.top; vel.bottom += deltaV.bottom; const PxVec3 aVec = deltaV.top; force = pVel.top.cross(aVec); //compute linear part const PxVec3 lVel = deltaV.bottom; const PxVec3 temp1 = 2.f * pVel.top.cross(lVel); const PxVec3 temp2 = aVec.cross(lVel); torque += temp1 + temp2; } coriolisVectors[linkID] = Cm::SpatialVectorF(force, torque); } //PX_ASSERT(vel.top.isFinite() && PxAbs(vel.top.x) < 10000.f && PxAbs(vel.top.y) < 10000.f && PxAbs(vel.top.z) < 10000.f); //PX_ASSERT(vel.bottom.isFinite() && PxAbs(vel.bottom.x) < 10000.f && PxAbs(vel.bottom.y) < 10000.f && PxAbs(vel.bottom.z) < 10000.f); motionVelocities[linkID] = vel; } } void solveExtContact(const PxSolverConstraintDesc& desc, Vec3V& linVel0, Vec3V& linVel1, Vec3V& angVel0, Vec3V& angVel1, Vec3V& linImpulse0, Vec3V& linImpulse1, Vec3V& angImpulse0, Vec3V& angImpulse1, bool doFriction, Dy::ErrorAccumulator* contactErrorAccumulator); void solveExt1D(const PxSolverConstraintDesc& desc, Vec3V& linVel0, Vec3V& linVel1, Vec3V& angVel0, Vec3V& angVel1, Vec3V& li0, Vec3V& li1, Vec3V& ai0, Vec3V& ai1, bool isPositionIteration); void solveExt1D(const PxSolverConstraintDesc& desc, Vec3V& linVel0, Vec3V& linVel1, Vec3V& angVel0, Vec3V& angVel1, const Vec3V& linMotion0, const Vec3V& linMotion1, const Vec3V& angMotion0, const Vec3V& angMotion1, const QuatV& rotA, const QuatV& rotB, const PxReal elapsedTimeF32, Vec3V& linImpulse0, Vec3V& linImpulse1, Vec3V& angImpulse0, Vec3V& angImpulse1, bool isPositionIteration); void solveExtContactStep(const PxSolverConstraintDesc& desc, Vec3V& linVel0, Vec3V& linVel1, Vec3V& angVel0, Vec3V& angVel1, Vec3V& linDelta0, Vec3V& linDelta1, Vec3V& angDelta0, Vec3V& angDelta1, Vec3V& linImpulse0, Vec3V& linImpulse1, Vec3V& angImpulse0, Vec3V& angImpulse1, bool doFriction, const PxReal minPenetration, const PxReal elapsedTimeF32, Dy::ErrorAccumulator* contactErrorAccumulator); void solveStaticConstraint(const PxSolverConstraintDesc& desc, Cm::SpatialVectorF& linkV, Cm::SpatialVectorF& impulse, Cm::SpatialVectorF& deltaV, const Cm::SpatialVectorF& motion, const PxQuat& rot, bool isTGS, PxReal elapsedTime, const PxReal minPenetration, Dy::ErrorAccumulator* contactErrorAccumulator, bool isPositionIteration) { Vec3V linVel = V3LoadA(linkV.bottom); Vec3V angVel = V3LoadA(linkV.top); Vec3V linVel0, linVel1, angVel0, angVel1; Vec3V li0 = V3Zero(), li1 = V3Zero(), ai0 = V3Zero(), ai1 = V3Zero(); if (isTGS) { PxQuat idt(PxIdentity); Vec3V linMotion0, angMotion0, linMotion1, angMotion1; QuatV rotA, rotB; if (desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY) { linVel0 = linVel; angVel0 = angVel; linMotion0 = V3LoadA(motion.bottom); angMotion0 = V3LoadA(motion.top); rotA = QuatVLoadU(&rot.x); rotB = QuatVLoadU(&idt.x); linVel1 = angVel1 = linMotion1 = angMotion1 = V3Zero(); } else { linVel1 = linVel; angVel1 = angVel; linMotion1 = V3LoadA(motion.bottom); angMotion1 = V3LoadA(motion.top); rotB = QuatVLoadU(&rot.x); rotA = QuatVLoadU(&idt.x); linVel0 = angVel0 = linMotion0 = angMotion0 = V3Zero(); } if (*desc.constraint == DY_SC_TYPE_EXT_CONTACT) { Dy::solveExtContactStep(desc, linVel0, linVel1, angVel0, angVel1, linMotion0, linMotion1, angMotion0, angMotion1, li0, li1, ai0, ai1, true, minPenetration, elapsedTime, contactErrorAccumulator); } else { Dy::solveExt1D(desc, linVel0, linVel1, angVel0, angVel1, linMotion0, linMotion1, angMotion0, angMotion1, rotA, rotB, elapsedTime, li0, li1, ai0, ai1, isPositionIteration); } } else { if (desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY) { linVel0 = linVel; angVel0 = angVel; linVel1 = angVel1 = V3Zero(); } else { linVel1 = linVel; angVel1 = angVel; linVel0 = angVel0 = V3Zero(); } if (*desc.constraint == DY_SC_TYPE_EXT_CONTACT) { Dy::solveExtContact(desc, linVel0, linVel1, angVel0, angVel1, li0, li1, ai0, ai1, true, contactErrorAccumulator); } else { Dy::solveExt1D(desc, linVel0, linVel1, angVel0, angVel1, li0, li1, ai0, ai1, isPositionIteration); } } Cm::SpatialVectorF newVel, newImp; if (desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY) { V4StoreA(Vec4V_From_Vec3V(linVel0), &newVel.bottom.x); V4StoreA(Vec4V_From_Vec3V(angVel0), &newVel.top.x); V4StoreA(Vec4V_From_Vec3V(li0), &newImp.top.x); V4StoreA(Vec4V_From_Vec3V(ai0), &newImp.bottom.x); } else { V4StoreA(Vec4V_From_Vec3V(linVel1), &newVel.bottom.x); V4StoreA(Vec4V_From_Vec3V(angVel1), &newVel.top.x); V4StoreA(Vec4V_From_Vec3V(li1), &newImp.top.x); V4StoreA(Vec4V_From_Vec3V(ai1), &newImp.bottom.x); } deltaV.top += (newVel.top - linkV.top); deltaV.bottom += (newVel.bottom - linkV.bottom); linkV.top = newVel.top; linkV.bottom = newVel.bottom; impulse -= newImp; } // Forward declarations void writeBackContact(const PxSolverConstraintDesc& desc, SolverContext& cache, PxSolverBodyData& bd0, PxSolverBodyData& bd1); void writeBackContact(const PxSolverConstraintDesc& desc, SolverContext* cache); void writeBack1D(const PxSolverConstraintDesc& desc); void writeBack1DStep(const PxSolverConstraintDesc& desc); void FeatherstoneArticulation::writebackInternalConstraints(bool isTGS) { SolverContext context; PxSolverBodyData data; for (PxU32 i = 0; i < mStatic1DConstraints.size(); ++i) { PxSolverConstraintDesc& desc = mStatic1DConstraints[i]; PX_ASSERT(*desc.constraint == DY_SC_TYPE_EXT_1D); if (isTGS) { writeBack1DStep(desc); } else { writeBack1D(desc); } } for (PxU32 i = 0; i < mStaticContactConstraints.size(); ++i) { PxSolverConstraintDesc& desc = mStaticContactConstraints[i]; PX_ASSERT(*desc.constraint == DY_SC_TYPE_EXT_CONTACT); if (isTGS) { writeBackContact(static_cast(desc), NULL); } else { writeBackContact(desc, context, data, data); } } } // Forward declarations void concludeContact(const PxSolverConstraintDesc& desc); void concludeContactStep(const PxSolverConstraintDesc& desc); void conclude1D(const PxSolverConstraintDesc& desc); void conclude1DStep(const PxSolverConstraintDesc& desc); void FeatherstoneArticulation::concludeInternalConstraints(bool isTGS) { for (PxU32 i = 0; i < mStatic1DConstraints.size(); ++i) { PxSolverConstraintDesc& desc = mStatic1DConstraints[i]; PX_ASSERT(*desc.constraint == DY_SC_TYPE_EXT_1D); if (isTGS) { conclude1DStep(desc); } else { conclude1D(desc); } } for (PxU32 i = 0; i < mStaticContactConstraints.size(); ++i) { PxSolverConstraintDesc& desc = mStaticContactConstraints[i]; PX_ASSERT(*desc.constraint == DY_SC_TYPE_EXT_CONTACT); if (isTGS) { concludeContactStep(desc); } else { concludeContact(desc); } } } struct InternalConstraintSolverData { PxReal dt; PxReal stepDt; PxReal invStepDt; PxReal elapsedTime; PxReal erp; bool isTGS; bool isVelIter; bool isResidualReportingActive; bool isExternalForceEveryStep; InternalConstraintSolverData(const PxReal dt_, const PxReal stepDt_, const PxReal invStepDt_, const PxReal elapsedTime_, const PxReal erp_, const bool isTGS_, const bool isVelIter_, const bool isResidualReportingActive_, const bool isExternalForcesEveryTgsIterationEnabled_) : dt(dt_) , stepDt(stepDt_) , invStepDt(invStepDt_) , elapsedTime(elapsedTime_) , erp(erp_) , isTGS(isTGS_) , isVelIter(isVelIter_) , isResidualReportingActive(isResidualReportingActive_) , isExternalForceEveryStep(isExternalForcesEveryTgsIterationEnabled_) { } PX_NOCOPY(InternalConstraintSolverData) }; Cm::SpatialVectorF FeatherstoneArticulation::solveInternalJointConstraintRecursive(const InternalConstraintSolverData& data, const PxU32 linkID, const Cm::SpatialVectorF& parentDeltaV, PxU32& dofId, PxU32& limitId) { //PxU32 linkID = stack[stackSize]; const ArticulationLink* links = mArticulationData.mLinks; const ArticulationLink& link = links[linkID]; const ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID); Cm::SpatialVectorF i1(PxVec3(0.f), PxVec3(0.f)); //We know the absolute parentDeltaV from the call to this function so no need to modify it. Cm::SpatialVectorF parentV = parentDeltaV + mArticulationData.mMotionVelocities[link.parent]; Cm::SpatialVectorF parentVelContrib = propagateAccelerationW( mArticulationData.getRw(linkID), parentDeltaV, mArticulationData.mInvStIs[linkID], &mArticulationData.mWorldMotionMatrix[jointDatum.jointOffset], &mArticulationData.mIsW[jointDatum.jointOffset], &mArticulationData.mDeferredQstZ[jointDatum.jointOffset], jointDatum.nbDof, NULL); Cm::SpatialVectorF childV = mArticulationData.mMotionVelocities[linkID] + parentVelContrib; Cm::UnAlignedSpatialVector i0(PxVec3(0.f), PxVec3(0.f)); Cm::SpatialVectorF dv1 = parentVelContrib; //Process internal constraints (parent/child limits/locks/drives) for (PxU32 dof = 0; dof < jointDatum.nbDof; ++dof) { if (mArticulationData.mInternalConstraints.size() <= dofId) continue; ArticulationInternalConstraint& constraint = mArticulationData.mInternalConstraints[dofId++]; const PxReal jointPDelta = constraint.row1.innerProduct(mArticulationData.mDeltaMotionVector[linkID]) - constraint.row0.innerProduct(mArticulationData.mDeltaMotionVector[link.parent]); // This jointV is just used to compute velocity-dependent forces (friction, limits, ...). It is not stored to the articulation joint velocities. PxReal jointV = constraint.row1.innerProduct(childV) - constraint.row0.innerProduct(parentV); PxReal frictionDeltaF = 0.0f; bool newFrictionModel = constraint.staticFrictionEffort != 0.0f || constraint.viscousFrictionCoefficient != 0.0f; const bool isPerStep = (data.isTGS && data.isExternalForceEveryStep && !(data.isVelIter)); const PxReal effectiveTimestep = isPerStep ? data.stepDt : data.dt; // deprecated friction model if (!newFrictionModel) { // Friction force is accumulated through all position iterations only for PGS const PxReal appliedFriction = data.isTGS ? 0.0f : constraint.accumulatedFrictionImpulse; const PxReal frictionForce = PxClamp(-jointV * constraint.recipResponse + appliedFriction, -constraint.frictionMaxForce, constraint.frictionMaxForce); constraint.accumulatedFrictionImpulse = frictionForce; // This is not used for TGS frictionDeltaF = frictionForce - appliedFriction; jointV += frictionDeltaF * constraint.response; } PxReal driveDeltaF = 0.0f; if (constraint.envelope.maxEffort > 0.0f) { const PxReal appliedDriveImpulse = isPerStep ? 0.0f : constraint.driveImpulse; const PxReal maxImpulse = constraint.envelope.maxEffort * effectiveTimestep; const PxReal speedImpulseGradient = constraint.envelope.speedEffortGradient / effectiveTimestep; const PxReal velocityDependentResistance = constraint.envelope.velocityDependentResistance * effectiveTimestep; const PxReal externalJointImpulse = constraint.externalJointForce * effectiveTimestep; const PxReal unclampedImpulse = (data.isTGS && data.isVelIter) ? appliedDriveImpulse : computeDriveImpulse(appliedDriveImpulse, jointV, jointPDelta, data.elapsedTime, constraint.getImplicitDriveDesc()); const PxReal clampedImpulse = clampDriveImpulse( jointV, appliedDriveImpulse + externalJointImpulse, unclampedImpulse + externalJointImpulse, constraint.response, constraint.envelope.maxActuatorVelocity, maxImpulse, speedImpulseGradient, velocityDependentResistance ) - externalJointImpulse; driveDeltaF = clampedImpulse - appliedDriveImpulse; constraint.driveImpulse += driveDeltaF; } else { const PxReal unclampedImpulse = (data.isTGS && data.isVelIter) ? constraint.driveImpulse : computeDriveImpulse(constraint.driveImpulse, jointV, jointPDelta, data.elapsedTime, constraint.getImplicitDriveDesc()); const PxReal clampedImpulse = PxClamp(unclampedImpulse, -constraint.driveMaxImpulse, constraint.driveMaxImpulse); driveDeltaF = (clampedImpulse - constraint.driveImpulse); constraint.driveImpulse = clampedImpulse; } jointV += driveDeltaF * constraint.response; if (newFrictionModel) { const PxReal staticFrictionImpulse = constraint.staticFrictionEffort * effectiveTimestep; const PxReal dynamicFrictionImpulse = constraint.dynamicFrictionEffort * effectiveTimestep; const PxReal viscousFrictionCoefficient = constraint.viscousFrictionCoefficient * effectiveTimestep; const PxReal accumulatedFrictionImpulse = isPerStep ? 0.0f : constraint.accumulatedFrictionImpulse; const PxReal totalFrictionImpulse = computeFrictionImpulse( accumulatedFrictionImpulse - jointV * constraint.recipResponse, staticFrictionImpulse, dynamicFrictionImpulse, viscousFrictionCoefficient, jointV); frictionDeltaF = totalFrictionImpulse - accumulatedFrictionImpulse; constraint.accumulatedFrictionImpulse += frictionDeltaF; // to keep track of accumulated impulse for velIter in TGS with isExternalForceEveryStep jointV += frictionDeltaF * constraint.response; } //Where we will be next frame - we use this to compute error bias terms to correct limits and drives... //printf("LinkID %i driveDeltaV = %f, jointV = %f\n", linkID, driveDeltaF, jointV); PxReal posLimitDeltaF = 0.0f; if (jointDatum.dofLimitMask & (1 << dof)) { ArticulationInternalLimit& limit = mArticulationData.mInternalLimits[limitId++]; posLimitDeltaF = computeLimitImpulse( data.stepDt, data.invStepDt, data.isVelIter, constraint.response, constraint.recipResponse, data.erp, limit.errorLow, limit.errorHigh, jointPDelta, limit.lowImpulse, limit.highImpulse, jointV); } PxReal velLimitDeltaF = 0.0f; const PxReal maxJointVel =constraint.maxJointVelocity; if (PxAbs(jointV) > maxJointVel) { PxReal newJointV = PxClamp(jointV, -maxJointVel, maxJointVel); velLimitDeltaF = (newJointV - jointV) * constraint.recipResponse; jointV = newJointV; } const PxReal deltaF = frictionDeltaF + driveDeltaF + posLimitDeltaF + velLimitDeltaF; //Accumulate error even if it is zero because the increment of the counter affects the RMS value if (data.isResidualReportingActive) (data.isVelIter ? mInternalErrorAccumulatorVelIter : mInternalErrorAccumulatorPosIter).accumulateErrorLocal(deltaF, constraint.recipResponse); if (deltaF != 0.f) { //impulse = true; i0 += constraint.row0 * deltaF; i1.top -= constraint.row1.top * deltaF; i1.bottom -= constraint.row1.bottom * deltaF; const Cm::UnAlignedSpatialVector deltaVP = constraint.deltaVA * (-deltaF); const Cm::UnAlignedSpatialVector deltaVC = constraint.deltaVB * (-deltaF); parentV += Cm::SpatialVectorF(deltaVP.top, deltaVP.bottom); childV += Cm::SpatialVectorF(deltaVC.top, deltaVC.bottom); dv1.top += deltaVC.top; dv1.bottom += deltaVC.bottom; } } //Cache the impulse arising from internal constraints. //We'll subtract this from the total impulse applied later in this function. const Cm::SpatialVectorF i1Internal = i1; const Cm::SpatialVectorF& deltaMotion = mArticulationData.getDeltaMotionVector(linkID); const PxQuat& deltaQ = getDeltaQ(linkID); const PxU32 nbStatic1DConstraints = mArticulationData.mNbStatic1DConstraints[linkID]; PxU32 start1DIdx = mArticulationData.mStatic1DConstraintStartIndex[linkID]; for (PxU32 i = 0; i < nbStatic1DConstraints; ++i) { PxSolverConstraintDesc& desc = mStatic1DConstraints[start1DIdx++]; solveStaticConstraint( desc, childV, i1, dv1, deltaMotion, deltaQ, data.isTGS, data.elapsedTime, data.isVelIter ? 0.f : -PX_MAX_F32, data.isVelIter ? &mContactErrorAccumulatorVelIter : &mContactErrorAccumulatorPosIter, !data.isVelIter); } const PxU32 nbStaticContactConstraints = mArticulationData.mNbStaticContactConstraints[linkID]; PxU32 startContactIdx = mArticulationData.mStaticContactConstraintStartIndex[linkID]; for (PxU32 i = 0; i < nbStaticContactConstraints; ++i) { PxSolverConstraintDesc& desc = mStaticContactConstraints[startContactIdx++]; solveStaticConstraint( desc, childV, i1, dv1, deltaMotion, deltaQ, data.isTGS, data.elapsedTime, data.isVelIter ? 0.f : -PX_MAX_F32, data.isVelIter ? &mContactErrorAccumulatorVelIter : &mContactErrorAccumulatorPosIter, !data.isVelIter); } PxU32 numChildren = link.mNumChildren; PxU32 offset = link.mChildrenStartIndex; for(PxU32 i = 0; i < numChildren; ++i) { const PxU32 child = offset+i; Cm::SpatialVectorF childImp = solveInternalJointConstraintRecursive(data, child, dv1, dofId, limitId); i1 += childImp; if ((numChildren-i) > 1) { //Propagate the childImp to my dv1 so that the next constraint gets to see an updated velocity state based //on the propagation of the child velocities Cm::SpatialVectorF deltaV = mArticulationData.mResponseMatrixW[linkID].getLinkDeltaVImpulseResponse(-childImp); dv1 += deltaV; childV += deltaV; } } Cm::SpatialVectorF propagatedImpulseAtParentW; { //const inputs const PxVec3& r = mArticulationData.getRw(linkID); const Cm::SpatialVectorF* jointDofISInvStISW = &mArticulationData.mISInvStIS[jointDatum.jointOffset]; const Cm::UnAlignedSpatialVector* jointDofMotionMatrixW = &mArticulationData.mWorldMotionMatrix[jointDatum.jointOffset]; const PxU8 nbDofs = jointDatum.nbDof; //output PxReal* jointDofDeferredQstZ = &mArticulationData.mDeferredQstZ[jointDatum.jointOffset]; propagatedImpulseAtParentW = propagateImpulseW( r, i1, NULL, jointDofISInvStISW, jointDofMotionMatrixW, nbDofs, jointDofDeferredQstZ); } //Accumulate the propagated impulse at the link. //Don't forget to subtract the impulse arising from internal constraints. //This can be used to compute the link's incoming joint force. mArticulationData.mSolverLinkSpatialImpulses[linkID] += (i1 - i1Internal); return Cm::SpatialVectorF(i0.top, i0.bottom) + propagatedImpulseAtParentW; } void FeatherstoneArticulation::solveInternalJointConstraints(const PxReal dt, const PxReal stepDt, const PxReal invStepDt, bool isVelIter, bool isTGS, const PxReal elapsedTime, const PxReal biasCoefficient, bool residualReportingActive, bool isExternalForcesEveryTgsIterationEnabled) { //const PxU32 count = mArticulationData.getLinkCount(); //const PxReal erp = isTGS ? 0.5f*biasCoefficient : biasCoefficient; const PxReal erp = biasCoefficient; const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE; PxU32* static1DConstraintCounts = mArticulationData.mNbStatic1DConstraints.begin(); PxU32* static1DConstraintStarts = mArticulationData.mStatic1DConstraintStartIndex.begin(); PxU32* staticContactConstraintCounts = mArticulationData.mNbStaticContactConstraints.begin(); PxU32* staticContactConstraintStarts = mArticulationData.mStaticContactConstraintStartIndex.begin(); ArticulationLink* links = mArticulationData.getLinks(); Cm::SpatialVectorF* baseVelocities = mArticulationData.getMotionVelocities(); //PxTransform* transforms = mArticulationData.mPreTransform.begin(); //Cm::SpatialVectorF* deferredZ = mArticulationData.getSpatialZAVectors(); const PxReal minPenetration = isVelIter ? 0.f : -PX_MAX_F32; Cm::SpatialVectorF rootLinkV; { Cm::SpatialVectorF rootLinkDeltaV(PxVec3(0.f), PxVec3(0.f)); if (!fixBase) { //Cm::SpatialVectorF temp = mArticulationData.getBaseInvSpatialArticulatedInertia() * (-deferredZ[0]); //const PxTransform& body2World0 = transforms[0]; //rootLinkDeltaV = temp.rotate(body2World0); //temp is now in world space! rootLinkDeltaV = mArticulationData.getBaseInvSpatialArticulatedInertiaW() * -mArticulationData.mRootDeferredZ; } rootLinkV = rootLinkDeltaV + baseVelocities[0]; Cm::SpatialVectorF im0 = Cm::SpatialVectorF::Zero(); { const PxU32 nbStatic1DConstraints = static1DConstraintCounts[0]; if (nbStatic1DConstraints) { const Cm::SpatialVectorF& deltaMotion = mArticulationData.getDeltaMotionVector(0); const PxQuat& deltaQ = getDeltaQ(0); PxU32 startIdx = static1DConstraintStarts[0]; for (PxU32 i = 0; i < nbStatic1DConstraints; ++i) { PxSolverConstraintDesc& desc = mStatic1DConstraints[startIdx++]; solveStaticConstraint(desc, rootLinkV, im0, rootLinkDeltaV, deltaMotion, deltaQ, isTGS, elapsedTime, minPenetration, isVelIter ? &mContactErrorAccumulatorVelIter : &mContactErrorAccumulatorPosIter, !isVelIter); } //Impulses and deferredZ are now in world space, not link space! /*im0.top = transforms[0].rotateInv(im0.top); im0.bottom = transforms[0].rotateInv(im0.bottom);*/ } const PxU32 nbStaticContactConstraints = staticContactConstraintCounts[0]; if (nbStaticContactConstraints) { const Cm::SpatialVectorF& deltaMotion = mArticulationData.getDeltaMotionVector(0); const PxQuat& deltaQ = getDeltaQ(0); PxU32 startIdx = staticContactConstraintStarts[0]; for (PxU32 i = 0; i < nbStaticContactConstraints; ++i) { PxSolverConstraintDesc& desc = mStaticContactConstraints[startIdx++]; solveStaticConstraint(desc, rootLinkV, im0, rootLinkDeltaV, deltaMotion, deltaQ, isTGS, elapsedTime, minPenetration, isVelIter ? &mContactErrorAccumulatorVelIter : &mContactErrorAccumulatorPosIter, !isVelIter); } //Impulses and deferredZ are now in world space, not link space! /*im0.top = transforms[0].rotateInv(im0.top); im0.bottom = transforms[0].rotateInv(im0.bottom);*/ } } //Store the constant that will be used by every dof and limit encountered. const InternalConstraintSolverData data(dt, stepDt, invStepDt, elapsedTime, erp, isTGS, isVelIter, residualReportingActive, isExternalForcesEveryTgsIterationEnabled); //Increment dofId and limitId as each is encountered to make sure we stay in sync with the ordering of //ArticulationData::mInternalConstraints and ArticulationData::mInternalLimits. PxU32 dofId = 0; PxU32 limitId = 0; const PxU32 numChildren = links[0].mNumChildren; const PxU32 offset = links[0].mChildrenStartIndex; for (PxU32 i = 0; i < numChildren; ++i) { const PxU32 child = offset + i; Cm::SpatialVectorF imp = solveInternalJointConstraintRecursive(data, child, rootLinkDeltaV, dofId, limitId); im0 += imp; //There's an impulse, we have to work out how it impacts our velocity (only if required (we have more children to traverse))! if (!fixBase && (numChildren - 1) != 0) { //Impulses and deltaVs are all now in world space rootLinkDeltaV += mArticulationData.getBaseInvSpatialArticulatedInertiaW() * (-imp); } } mArticulationData.mRootDeferredZ += im0; mArticulationData.mJointDirty = true; } } void FeatherstoneArticulation::solveInternalSpatialTendonConstraints(bool isTGS) { if (mArticulationData.mInternalSpatialTendonConstraints.size() == 0) return; if (isTGS) { //Update the error terms in the tendons recursively... const PxU32 nbTendons = mArticulationData.mNumSpatialTendons; for (PxU32 i = 0; i < nbTendons; ++i) { Dy::ArticulationSpatialTendon* tendon = mArticulationData.mSpatialTendons[i]; Dy::ArticulationAttachment& attachment = tendon->getAttachment(0); //const PxU32 childCount = attachment.childCount; //PxReal scale = 1.f / PxReal(childCount); PxReal coefficient = attachment.coefficient; const PxU32 startLink = tendon->getAttachment(0).linkInd; const PxTransform pBody2World = mArticulationData.getAccumulatedPoses()[startLink]; const PxVec3 pAttachPoint = pBody2World.transform(tendon->getAttachment(0).relativeOffset); Dy::ArticulationAttachment* attachments = tendon->getAttachments(); for (ArticulationAttachmentBitField children = attachments[0].children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); updateSpatialTendonConstraintsRecursive(attachments, mArticulationData, child, tendon->mOffset*coefficient, pAttachPoint); } } } for (PxU32 i = 0; i < mArticulationData.mInternalSpatialTendonConstraints.size(); ++i) { ArticulationInternalTendonConstraint& constraint = mArticulationData.mInternalSpatialTendonConstraints[i]; const PxU32 parentID = constraint.linkID0; const PxU32 linkID = constraint.linkID1; //Cm::SpatialVectorF childDeltaP = mArticulationData.mDeltaMotionVector[linkID]; //Cm::SpatialVectorF parentDeltaP = mArticulationData.mDeltaMotionVector[parentID]; //PxReal deltaP = constraint.row1.innerProduct(childDeltaP) - constraint.row0.innerProduct(parentDeltaP);// + deltaErr; Cm::SpatialVectorV childVel = pxcFsGetVelocity(linkID); Cm::SpatialVectorV parentVel = pxcFsGetVelocity(parentID); Cm::UnAlignedSpatialVector childV; V3StoreU(childVel.angular, childV.top); V3StoreU(childVel.linear, childV.bottom); Cm::UnAlignedSpatialVector parentV; V3StoreU(parentVel.angular, parentV.top); V3StoreU(parentVel.linear, parentV.bottom); PxReal error = constraint.restDistance - constraint.accumulatedLength;// + deltaP; PxReal error2 = 0.f; if (constraint.accumulatedLength > constraint.highLimit) error2 = constraint.highLimit - constraint.accumulatedLength; if (constraint.accumulatedLength < constraint.lowLimit) error2 = constraint.lowLimit - constraint.accumulatedLength; PxReal jointV = constraint.row1.innerProduct(childV) - constraint.row0.innerProduct(parentV); PX_ASSERT(PxIsFinite(jointV)); PxReal unclampedForce = (jointV * constraint.velMultiplier + error * constraint.biasCoefficient) /** constraint.recipResponse*/ + constraint.appliedForce * constraint.impulseMultiplier; PxReal unclampedForce2 = (error2 * constraint.limitBiasCoefficient) + constraint.limitAppliedForce * constraint.limitImpulseMultiplier; const PxReal deltaF = (unclampedForce - constraint.appliedForce) + (unclampedForce2 - constraint.limitAppliedForce); constraint.appliedForce = unclampedForce; constraint.limitAppliedForce = unclampedForce2; //Accumulate error even if it is zero because the increment of the counter affects the RMS value //Ignore tendons for now //(isVelIter ? mInternalErrorAccumulatorVelIter : mInternalErrorAccumulatorPosIter).accumulateErrorLocal(deltaF, constraint.recipResponse); if (deltaF != 0.f) { Cm::UnAlignedSpatialVector i0 = constraint.row0 * -deltaF; Cm::UnAlignedSpatialVector i1 = constraint.row1 * deltaF; pxcFsApplyImpulses( parentID, V3LoadU(i0.top), V3LoadU(i0.bottom), NULL, linkID, V3LoadU(i1.top), V3LoadU(i1.bottom), NULL); } } } PxVec3 FeatherstoneArticulation::calculateFixedTendonVelocityAndPositionRecursive(FixedTendonSolveData& solveData, const Cm::SpatialVectorF& parentV, const Cm::SpatialVectorF& parentDeltaV, const PxU32 tendonJointID) { ArticulationTendonJoint& tendonJoint = solveData.tendonJoints[tendonJointID]; ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(tendonJoint.linkInd); Cm::SpatialVectorF deltaV = propagateAccelerationW( mArticulationData.getRw(tendonJoint.linkInd), parentDeltaV, mArticulationData.mInvStIs[tendonJoint.linkInd], &mArticulationData.mWorldMotionMatrix[jointDatum.jointOffset], &mArticulationData.mIsW[jointDatum.jointOffset], &mArticulationData.mDeferredQstZ[jointDatum.jointOffset], jointDatum.nbDof, NULL); Cm::SpatialVectorF childV = mArticulationData.mMotionVelocities[tendonJoint.linkInd] + deltaV; PxU32 index = tendonJoint.mConstraintInd; ArticulationInternalTendonConstraint& constraint = mArticulationData.mInternalFixedTendonConstraints[index]; const PxU32 childCount = tendonJoint.childCount; PxVec3 jointVError; const PxReal jointV = constraint.row1.innerProduct(childV) - constraint.row0.innerProduct(parentV); const PxReal jointP = mArticulationData.mJointPosition[tendonJoint.startJointOffset]; jointVError.x = jointV * tendonJoint.coefficient; jointVError.y = jointP * tendonJoint.coefficient; //printf("%i: jointPose = %f, jointV = %f, coefficient = %f\n", tendonJoint.linkInd, jointP, jointV, tendonJoint.coefficient); jointVError.z = 1.f; if (childCount) { for (ArticulationBitField children = tendonJoint.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); jointVError += calculateFixedTendonVelocityAndPositionRecursive(solveData, childV, deltaV, child); } } return jointVError; } Cm::SpatialVectorF FeatherstoneArticulation::solveFixedTendonConstraintsRecursive(FixedTendonSolveData& solveData, const PxU32 tendonJointID) { ArticulationTendonJoint& tendonJoint = solveData.tendonJoints[tendonJointID]; ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(tendonJoint.linkInd); PxU32 index = tendonJoint.mConstraintInd; ArticulationInternalTendonConstraint& constraint = mArticulationData.mInternalFixedTendonConstraints[index]; const PxU32 childCount = tendonJoint.childCount; PxReal jointV = solveData.rootVel; // calculate current accumulated tendon length from parent accumulated length const PxReal lengthError = solveData.error; PxReal limitError = solveData.limitError; // the constraint bias coefficients need to flip signs together with the tendon joint's coefficient // in order for the constraint force to point into the correct direction: const PxReal coefficientSign = tendonJoint.recipCoefficient;// PxSign(tendonJoint.coefficient); const PxReal biasCoefficient = constraint.biasCoefficient; const PxReal limitBiasCoefficient = constraint.limitBiasCoefficient; PxReal unclampedForce = ((jointV * constraint.velMultiplier + lengthError * biasCoefficient)*coefficientSign) + constraint.appliedForce * constraint.impulseMultiplier; PxReal unclampedForce2 = (limitError * limitBiasCoefficient * coefficientSign) + constraint.limitAppliedForce * constraint.limitImpulseMultiplier; const PxReal deltaF = ((unclampedForce - constraint.appliedForce) + (unclampedForce2 - constraint.limitAppliedForce)); //Ignore tendons for now //(isVelIter ? mInternalErrorAccumulatorVelIter : mInternalErrorAccumulatorPosIter).accumulateErrorLocal(deltaF, constraint.recipResponse); constraint.appliedForce = unclampedForce; constraint.limitAppliedForce = unclampedForce2; solveData.rootImp += deltaF; Cm::SpatialVectorF impulse(constraint.row1.top * -deltaF, constraint.row1.bottom * -deltaF); const Cm::SpatialVectorF YInt = impulse; if (childCount) { for (ArticulationBitField children = tendonJoint.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); Cm::SpatialVectorF propagatedImpulse = solveFixedTendonConstraintsRecursive(solveData, child); impulse.top += propagatedImpulse.top; impulse.bottom += propagatedImpulse.bottom; } } mArticulationData.mSolverLinkSpatialImpulses[tendonJoint.linkInd] += impulse - YInt; return propagateImpulseW( mArticulationData.mRw[tendonJoint.linkInd], impulse, NULL, &mArticulationData.mISInvStIS[jointDatum.jointOffset], &mArticulationData.mWorldMotionMatrix[jointDatum.jointOffset], jointDatum.nbDof, &mArticulationData.mDeferredQstZ[jointDatum.jointOffset]); } void FeatherstoneArticulation::solveInternalFixedTendonConstraints(bool isTGS) { PX_UNUSED(isTGS); if (mArticulationData.mInternalFixedTendonConstraints.size() == 0) return; { //Update the error terms in the tendons recursively... const PxU32 nbTendons = mArticulationData.mNumFixedTendons; ArticulationLink* links = mArticulationData.getLinks(); for (PxU32 i = 0; i < nbTendons; ++i) { Dy::ArticulationFixedTendon* tendon = mArticulationData.mFixedTendons[i]; ArticulationTendonJoint* tendonJoints = tendon->getTendonJoints(); Dy::ArticulationTendonJoint& pTendonJoint = tendonJoints[0]; //const PxU32 childCount = pTendonJoint.childCount; const PxU32 startLink = pTendonJoint.linkInd; Cm::SpatialVectorV parentVel = pxcFsGetVelocity(startLink); Cm::SpatialVectorF parentV; V3StoreU(parentVel.angular, parentV.top); V3StoreU(parentVel.linear, parentV.bottom); Cm::SpatialVectorF Z(PxVec3(0.f), PxVec3(0.f)); Cm::SpatialVectorF parentDeltaV = parentV - mArticulationData.mMotionVelocities[startLink]; PxVec3 velError(0.f); for (ArticulationAttachmentBitField children = pTendonJoint.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); FixedTendonSolveData solveData; solveData.links = links; solveData.erp = 1.f; solveData.rootImp = 0.f; solveData.error = tendon->mError; solveData.tendonJoints = tendonJoints; velError += calculateFixedTendonVelocityAndPositionRecursive(solveData, parentV, parentDeltaV, child); } const PxReal recipScale = velError.z == 0.f ? 0.f : 1.f / velError.z; for (ArticulationAttachmentBitField children = pTendonJoint.children; children != 0; children &= (children - 1)) { //index of child of link h on path to link linkID const PxU32 child = PxLowestSetBit(children); ArticulationTendonJoint& tendonJoint = tendonJoints[child]; ArticulationInternalTendonConstraint& constraint = mArticulationData.mInternalFixedTendonConstraints[tendonJoint.mConstraintInd]; const PxReal length = (velError.y + tendon->mOffset); FixedTendonSolveData solveData; solveData.links = links; solveData.erp = 1.f; solveData.rootImp = 0.f; solveData.error = (length - tendon->mRestLength) * recipScale; solveData.rootVel = velError.x*recipScale; PxReal limitError = 0.f; if (length < tendon->mLowLimit) limitError = length - tendon->mLowLimit; else if (length > tendon->mHighLimit) limitError = length - tendon->mHighLimit; solveData.limitError = limitError * recipScale; solveData.tendonJoints = tendonJoints; //KS - TODO - hook up offsets Cm::SpatialVectorF propagatedImpulse = solveFixedTendonConstraintsRecursive(solveData, child); propagatedImpulse.top += constraint.row0.top * solveData.rootImp; propagatedImpulse.bottom += constraint.row0.bottom * solveData.rootImp; Z += propagatedImpulse; } for (PxU32 linkID = pTendonJoint.linkInd; linkID; linkID = links[linkID].parent) { const PxU32 jointOffset = mArticulationData.getJointData(linkID).jointOffset; const PxU8 dofCount = mArticulationData.getJointData(linkID).nbDof; Z = propagateImpulseW( mArticulationData.getRw(linkID), Z, NULL, &mArticulationData.mISInvStIS[jointOffset], &mArticulationData.mWorldMotionMatrix[jointOffset], dofCount, &mArticulationData.mDeferredQstZ[jointOffset]); } mArticulationData.mRootDeferredZ += Z; mArticulationData.mJointDirty = true; } } } void FeatherstoneArticulation::solveInternalConstraints(const PxReal dt, const PxReal stepDt, const PxReal invStepDt, bool velocityIteration, bool isTGS, const PxReal elapsedTime, const PxReal biasCoefficient, bool residualReportingActive, bool isExternalForcesEveryTgsIterationEnabled) { if (velocityIteration) { mInternalErrorAccumulatorVelIter.reset(); mContactErrorAccumulatorVelIter.reset(); } else { mInternalErrorAccumulatorPosIter.reset(); mContactErrorAccumulatorPosIter.reset(); } solveInternalSpatialTendonConstraints(isTGS); solveInternalFixedTendonConstraints(isTGS); solveInternalMimicJointConstraints(stepDt, invStepDt, velocityIteration, isTGS, biasCoefficient); solveInternalJointConstraints(dt, stepDt, invStepDt, velocityIteration, isTGS, elapsedTime, biasCoefficient, residualReportingActive, isExternalForcesEveryTgsIterationEnabled); } bool FeatherstoneArticulation::storeStaticConstraint(const PxSolverConstraintDesc& desc) { if (DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER) { if (desc.constraintType == DY_SC_TYPE_RB_CONTACT) mStaticContactConstraints.pushBack(desc); else mStatic1DConstraints.pushBack(desc); } return DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER; } void FeatherstoneArticulation::setRootLinearVelocity(const PxVec3& velocity) { ArticulationLink& rLink = mArticulationData.getLink(0); rLink.bodyCore->linearVelocity = velocity; mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_ROOT_VELOCITIES; computeLinkVelocities(mArticulationData); } void FeatherstoneArticulation::setRootAngularVelocity(const PxVec3& velocity) { ArticulationLink& rLink = mArticulationData.getLink(0); rLink.bodyCore->angularVelocity = velocity; mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_ROOT_VELOCITIES; computeLinkVelocities(mArticulationData); } //This method is for user update the root link transform so we need to //fix up other link's position. In this case, we should assume all joint //velocity/pose is to be zero void FeatherstoneArticulation::teleportRootLink() { //make sure motionMatrix has been set //jcalc(mArticulationData); const PxU32 linkCount = mArticulationData.getLinkCount(); ArticulationLink* links = mArticulationData.getLinks(); PxReal* jointPositions = mArticulationData.getJointPositions(); Cm::SpatialVectorF* motionVelocities = mArticulationData.getMotionVelocities(); for (PxU32 linkID = 1; linkID < linkCount; ++linkID) { ArticulationLink& link = links[linkID]; const PxTransform oldTransform = link.bodyCore->body2World; ArticulationLink& pLink = links[link.parent]; const PxTransform pBody2World = pLink.bodyCore->body2World; ArticulationJointCore* joint = link.inboundJoint; ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID); PxReal* jPosition = &jointPositions[jointDatum.jointOffset]; PxQuat newParentToChild; PxQuat newWorldQ; PxVec3 r; const PxVec3 childOffset = -joint->childPose.p; const PxVec3 parentOffset = joint->parentPose.p; const PxQuat relativeQuat = mArticulationData.mRelativeQuat[linkID]; switch (joint->jointType) { case PxArticulationJointType::ePRISMATIC: { newParentToChild = relativeQuat; const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; const PxVec3& u = mArticulationData.mMotionMatrix[jointDatum.jointOffset].bottom; r = e + d + u * jPosition[0]; break; } case PxArticulationJointType::eREVOLUTE: case PxArticulationJointType::eREVOLUTE_UNWRAPPED: { const PxVec3& u = mArticulationData.mMotionMatrix[jointDatum.jointOffset].top; PxQuat jointRotation = PxQuat(-jPosition[0], u); if (jointRotation.w < 0) //shortest angle. jointRotation = -jointRotation; newParentToChild = (jointRotation * relativeQuat).getNormalized(); const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; /*PxVec3 worldAngVel = oldTransform.rotate(link.motionVelocity.top); newWorldQ = PxExp(worldAngVel*dt) * oldTransform.q; PxQuat newParentToChild2 = (newWorldQ.getConjugate() * joint->relativeQuat * pBody2World.q).getNormalized(); const PxVec3 e2 = newParentToChild2.rotate(parentOffset); const PxVec3 d2 = childOffset; r = e2 + d2;*/ PX_ASSERT(r.isFinite()); break; } case PxArticulationJointType::eSPHERICAL: { //PxVec3 angVel(joint->jointVelocity[0], joint->jointVelocity[1], joint->jointVelocity[2]); //PxVec3 worldAngVel = pLink.bodyCore->angularVelocity + oldTransform.rotate(angVel); PxVec3 worldAngVel = motionVelocities[linkID].top; /*const PxReal eps = 0.001f; const PxVec3 dif = worldAngVel - worldAngVel2; PX_ASSERT(PxAbs(dif.x) < eps && PxAbs(dif.y) < eps && PxAbs(dif.z) < eps);*/ newWorldQ = PxExp(worldAngVel) * oldTransform.q; newParentToChild = (newWorldQ.getConjugate() * relativeQuat * pBody2World.q).getNormalized(); const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; PX_ASSERT(r.isFinite()); break; } case PxArticulationJointType::eFIX: { //this is fix joint so joint don't have velocity newParentToChild = relativeQuat; const PxVec3 e = newParentToChild.rotate(parentOffset); const PxVec3 d = childOffset; r = e + d; break; } default: break; } PxTransform& body2World = link.bodyCore->body2World; body2World.q = (pBody2World.q * newParentToChild.getConjugate()).getNormalized(); body2World.p = pBody2World.p + body2World.q.rotate(r); PX_ASSERT(body2World.isSane()); } } PxU8* FeatherstoneArticulation::allocateScratchSpatialData(PxcScratchAllocator* allocator, const PxU32 linkCount, ScratchData& scratchData, bool fallBackToHeap) { const PxU32 size = sizeof(Cm::SpatialVectorF) * linkCount; const PxU32 totalSize = size * 4 + sizeof(Dy::SpatialMatrix) * linkCount; PxU8* tempMemory = reinterpret_cast(allocator->alloc(totalSize, fallBackToHeap)); scratchData.motionVelocities = reinterpret_cast(tempMemory); PxU32 offset = size; scratchData.motionAccelerations = reinterpret_cast(tempMemory + offset); offset += size; scratchData.coriolisVectors = reinterpret_cast(tempMemory + offset); offset += size; scratchData.spatialZAVectors = reinterpret_cast(tempMemory + offset); offset += size; scratchData.compositeSpatialInertias = reinterpret_cast(tempMemory + offset); return tempMemory; } /* void FeatherstoneArticulation::allocateScratchSpatialData(DyScratchAllocator& allocator, const PxU32 linkCount, ScratchData& scratchData) { const PxU32 size = sizeof(Cm::SpatialVectorF) * linkCount; const PxU32 totalSize = size * 5 + sizeof(Dy::SpatialMatrix) * linkCount; PxU8* tempMemory = allocator.alloc(totalSize); scratchData.motionVelocities = reinterpret_cast(tempMemory); PxU32 offset = size; scratchData.motionAccelerations = reinterpret_cast(tempMemory + offset); offset += size; scratchData.coriolisVectors = reinterpret_cast(tempMemory + offset); offset += size; scratchData.spatialZAVectors = reinterpret_cast(tempMemory + offset); offset += size; scratchData.externalAccels = reinterpret_cast(tempMemory + offset); offset += size; scratchData.compositeSpatialInertias = reinterpret_cast(tempMemory + offset); }*/ }//namespace Dy }