From 3dfe2108cfab31ba3ee5527e217d0d8e99a51162 Mon Sep 17 00:00:00 2001 From: git perforce import user Date: Tue, 25 Oct 2016 12:29:14 -0600 Subject: Initial commit: PhysX 3.4.0 Update @ 21294896 APEX 1.4.0 Update @ 21275617 [CL 21300167] --- .../SimulationController/src/ScArticulationSim.cpp | 515 +++++++++++++++++++++ 1 file changed, 515 insertions(+) create mode 100644 PhysX_3.4/Source/SimulationController/src/ScArticulationSim.cpp (limited to 'PhysX_3.4/Source/SimulationController/src/ScArticulationSim.cpp') diff --git a/PhysX_3.4/Source/SimulationController/src/ScArticulationSim.cpp b/PhysX_3.4/Source/SimulationController/src/ScArticulationSim.cpp new file mode 100644 index 00000000..a35f4bf2 --- /dev/null +++ b/PhysX_3.4/Source/SimulationController/src/ScArticulationSim.cpp @@ -0,0 +1,515 @@ +// This code contains NVIDIA Confidential Information and is disclosed to you +// under a form of NVIDIA software license agreement provided separately to you. +// +// Notice +// NVIDIA Corporation and its licensors retain all intellectual property and +// proprietary rights in and to this software and related documentation and +// any modifications thereto. Any use, reproduction, disclosure, or +// distribution of this software and related documentation without an express +// license agreement from NVIDIA Corporation is strictly prohibited. +// +// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES +// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO +// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT, +// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE. +// +// Information and code furnished is believed to be accurate and reliable. +// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such +// information or for any infringement of patents or other rights of third parties that may +// result from its use. No license is granted by implication or otherwise under any patent +// or patent rights of NVIDIA Corporation. Details are subject to change without notice. +// This code supersedes and replaces all information previously supplied. +// NVIDIA Corporation products are not authorized for use as critical +// components in life support devices or systems without express written approval of +// NVIDIA Corporation. +// +// Copyright (c) 2008-2016 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 "ScArticulationSim.h" +#include "ScArticulationCore.h" +#include "ScArticulationJointSim.h" +#include "ScArticulationJointCore.h" +#include "ScBodySim.h" +#include "ScScene.h" + +#include "DyArticulation.h" +#include "PxsContext.h" +#include "CmSpatialVector.h" +#include "PsVecMath.h" +#include "PxsSimpleIslandManager.h" + +using namespace physx; +using namespace physx::Dy; + +Sc::ArticulationSim::ArticulationSim(ArticulationCore& core, Scene& scene, BodyCore& root) : + mLLArticulation(NULL), + mScene(scene), + mCore(core), + mLinks (PX_DEBUG_EXP("ScArticulationSim::links")), + mBodies (PX_DEBUG_EXP("ScArticulationSim::bodies")), + mJoints (PX_DEBUG_EXP("ScArticulationSim::joints")), + mInternalLoads (PX_DEBUG_EXP("ScArticulationSim::internalLoads")), + mExternalLoads (PX_DEBUG_EXP("ScArticulationSim::externalLoads")), + mPose (PX_DEBUG_EXP("ScArticulationSim::poses")), + mMotionVelocity (PX_DEBUG_EXP("ScArticulationSim::motion velocity")), + mFsDataBytes (PX_DEBUG_EXP("ScArticulationSim::fsData")), + mScratchMemory (PX_DEBUG_EXP("ScArticulationSim::scratchMemory")), + mUpdateSolverData(true) +{ + mLinks.reserve(16); + mJoints.reserve(16); + mBodies.reserve(16); + + mLLArticulation = mScene.createLLArticulation(this); + + mIslandNodeIndex = scene.getSimpleIslandManager()->addArticulation(this, mLLArticulation, false); + + if(!mLLArticulation) + { + Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Articulation: could not allocate low-level resources."); + return; + } + + PX_ASSERT(root.getSim()); + + addBody(*root.getSim(), NULL, NULL); + + + + mCore.setSim(this); + + mSolverData.core = &core.getCore(); + mSolverData.internalLoads = NULL; + mSolverData.externalLoads = NULL; + mSolverData.fsData = NULL; + mSolverData.poses = NULL; + mSolverData.motionVelocity = NULL; + mSolverData.totalDataSize = 0; + mSolverData.solverDataSize = 0; + mSolverData.linkCount = 0; + mSolverData.scratchMemory = NULL; + mSolverData.scratchMemorySize = 0; +} + + +Sc::ArticulationSim::~ArticulationSim() +{ + if (!mLLArticulation) + return; + + mScene.destroyLLArticulation(*mLLArticulation); + + mScene.getSimpleIslandManager()->removeNode(mIslandNodeIndex); + + mCore.setSim(NULL); +} + +PxU32 Sc::ArticulationSim::findBodyIndex(BodySim& body) const +{ + for(PxU32 i=0; iupdateCached(shapeChangedMap); +} + +void Sc::ArticulationSim::updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray) +{ + for (PxU32 i = 0; iupdateContactDistance(contactDistance, dt, boundsArray); +} + +ArticulationLinkHandle Sc::ArticulationSim::getLinkHandle(BodySim &body) const +{ + return reinterpret_cast(mLLArticulation) | findBodyIndex(body); +} + +void Sc::ArticulationSim::addBody(BodySim& body, + BodySim* parent, + ArticulationJointSim* joint) +{ + mBodies.pushBack(&body); + mJoints.pushBack(joint); + mAcceleration.pushBack(Cm::SpatialVector(PxVec3(0.f), PxVec3(0.f))); + + PxU32 index = mLinks.size(); + + PX_ASSERT((((index==0) && (joint == 0)) && (parent == 0)) || + (((index!=0) && joint) && (parent && (parent->getArticulation() == this)))); + + ArticulationLink &link = mLinks.insert(); + link.body = &body.getLowLevelBody(); + link.bodyCore = &body.getBodyCore().getCore(); + link.children = 0; + bool shouldSleep; + bool currentlyAsleep; + bool bodyReadyForSleep = body.checkSleepReadinessBesidesWakeCounter(); + PxReal wakeCounter = getCore().getWakeCounter(); + + if(parent) + { + currentlyAsleep = !mBodies[0]->isActive(); + shouldSleep = currentlyAsleep && bodyReadyForSleep; + + PxU32 parentIndex = findBodyIndex(*parent); + link.parent = parentIndex; + link.pathToRoot = mLinks[parentIndex].pathToRoot | ArticulationBitField(1)<getCore().getCore(); + mLinks[parentIndex].children |= ArticulationBitField(1)<internalWakeUpArticulationLink(wakeCounter); + } + + body.setArticulation(this, wakeCounter, shouldSleep, index); + + mUpdateSolverData = true; + +} + + +void Sc::ArticulationSim::removeBody(BodySim &body) +{ + PX_ASSERT(body.getArticulation() == this); + PxU32 index = findBodyIndex(body); + body.setArticulation(NULL, 0.0f, true, 0); + + ArticulationLink &link0 = mLinks[index]; + + PX_ASSERT(link0.children == 0); + PX_UNUSED(link0); + + // copy all the later links down by one + for(PxU32 i=index+1;iindex) + link.pathToRoot = (link.pathToRoot&fixedIndices) | (link.pathToRoot&shiftIndices)>>1; + link.children = (link.children&fixedIndices) | (link.children&shiftIndices)>>1; + } + + mLinks.popBack(); + + mUpdateSolverData = true; +} + + +void Sc::ArticulationSim::checkResize() const +{ + if(!mBodies.size()) + return; + + if(!mUpdateSolverData) + return; + + if(mLinks.size()!=mSolverData.linkCount) + { + PxU32 linkCount = mLinks.size(); + + mMotionVelocity.resize(linkCount, Cm::SpatialVector(PxVec3(0.0f), PxVec3(0.0f))); + mPose.resize(linkCount, PxTransform(PxIdentity)); + mExternalLoads.resize(linkCount, Ps::aos::M33Identity()); + mInternalLoads.resize(linkCount, Ps::aos::M33Identity()); + + PxU32 solverDataSize, totalSize, scratchSize; + Articulation::getDataSizes(linkCount, solverDataSize, totalSize, scratchSize); + + PX_ASSERT(mFsDataBytes.size()!=totalSize); + PX_ASSERT(!(totalSize&15) && !(solverDataSize&15)); + mFsDataBytes.resize(totalSize); + + mSolverData.motionVelocity = mMotionVelocity.begin(); + mSolverData.externalLoads = mExternalLoads.begin(); + mSolverData.internalLoads = mInternalLoads.begin(); + mSolverData.poses = mPose.begin(); + mSolverData.solverDataSize = Ps::to16(solverDataSize); + mSolverData.totalDataSize = Ps::to16(totalSize); + mSolverData.fsData = reinterpret_cast(mFsDataBytes.begin()); + mSolverData.acceleration = mAcceleration.begin(); + + mScratchMemory.resize(scratchSize); + mSolverData.scratchMemory = mScratchMemory.begin(); + mSolverData.scratchMemorySize = Ps::to16(scratchSize); + } + + + // something changed... e.g. a link deleted and one added - we need to change the warm start + + PxMemZero(mExternalLoads.begin(), sizeof(Ps::aos::Mat33V) * mExternalLoads.size()); + PxMemZero(mInternalLoads.begin(), sizeof(Ps::aos::Mat33V) * mExternalLoads.size()); + + mSolverData.links = mLinks.begin(); + mSolverData.linkCount = Ps::to8(mLinks.size()); + + mLLArticulation->setSolverDesc(mSolverData); + + mUpdateSolverData = false; +} + +PxU32 Sc::ArticulationSim::getCCDLinks(BodySim** sims) +{ + PxU32 nbCCDBodies = 0; + for (PxU32 a = 0; a < mBodies.size(); ++a) + { + if (mBodies[a]->getLowLevelBody().getCore().mFlags & PxRigidBodyFlag::eENABLE_CCD) + { + sims[nbCCDBodies++] = mBodies[a]; + } + } + return nbCCDBodies; +} + +void Sc::ArticulationSim::sleepCheck(PxReal dt) +{ + if(!mBodies.size()) + return; + +#if PX_CHECKED + { + PxReal maxTimer = 0.0f, minTimer = PX_MAX_F32; + bool allActive = true, noneActive = true; + for(PxU32 i=0;igetBodyCore().getWakeCounter(); + maxTimer = PxMax(maxTimer, timer); + minTimer = PxMin(minTimer, timer); + bool active = mBodies[i]->isActive(); + allActive &= active; + noneActive &= !active; + } + // either all links are asleep, or no links are asleep + PX_ASSERT(maxTimer==0 || minTimer!=0); + PX_ASSERT(allActive || noneActive); + } + +#endif + + if(!mBodies[0]->isActive()) + return; + + PxReal sleepThreshold = getCore().getCore().sleepThreshold; + + PxReal maxTimer = 0.0f, minTimer = PX_MAX_F32; + + for(PxU32 i=0;iupdateWakeCounter(dt, sleepThreshold, reinterpret_cast(mMotionVelocity[i]));//enableStabilization); + maxTimer = PxMax(maxTimer, timer); + minTimer = PxMin(minTimer, timer); + } + + mCore.setWakeCounterInternal(maxTimer); + + if(maxTimer != 0.0f) + { + if(minTimer == 0.0f) + { + // make sure nothing goes to sleep unless everything does + for(PxU32 i=0;igetBodyCore().setWakeCounterFromSim(PxMax(1e-6f, mBodies[i]->getBodyCore().getWakeCounter())); + } + return; + } + + for(PxU32 i=0;inotifyReadyForSleeping(); + mBodies[i]->resetSleepFilter(); + } + + mScene.getSimpleIslandManager()->deactivateNode(mIslandNodeIndex); +} + +bool Sc::ArticulationSim::isSleeping() const +{ + return (mBodies.size() > 0) ? (!mBodies[0]->isActive()) : true; +} + +void Sc::ArticulationSim::internalWakeUp(PxReal wakeCounter) +{ + if(mCore.getWakeCounter() < wakeCounter) + { + mCore.setWakeCounterInternal(wakeCounter); + for(PxU32 i=0;iinternalWakeUpArticulationLink(wakeCounter); + } +} + +void Sc::ArticulationSim::setActive(const bool b, const PxU32 infoFlag) +{ + for(PxU32 i=0;isetActive(b, infoFlag); + } +} + +void Sc::ArticulationSim::updateForces(PxReal dt, bool simUsesAdaptiveForce) +{ + PxU32 count = 0; + for(PxU32 i=0;iupdateForces(dt, NULL, NULL, count, &mAcceleration[i], simUsesAdaptiveForce); + } +} + +void Sc::ArticulationSim::saveLastCCDTransform() +{ + for(PxU32 i=0;igetLowLevelBody().saveLastCCDTransform(); + } +} + + +Sc::ArticulationDriveCache* Sc::ArticulationSim::createDriveCache(PxReal compliance, + PxU32 driveIterations) const +{ + checkResize(); + PxU32 solverDataSize, totalSize, scratchSize; + Articulation::getDataSizes(mLinks.size(), solverDataSize, totalSize, scratchSize); + + // In principle we should only need solverDataSize here. But right now prepareFsData generates the auxiliary data + // for use in potential debugging, which takes up extra space. + FsData* data = reinterpret_cast(PX_ALLOC(totalSize,"Articulation Drive Cache")); + PxvArticulationDriveCache::initialize(*data, Ps::to16(mLinks.size()), mLinks.begin(), compliance, driveIterations, mScratchMemory.begin(), mScratchMemory.size()); + return data; +} + + +void Sc::ArticulationSim::updateDriveCache(ArticulationDriveCache& cache, + PxReal compliance, + PxU32 driveIterations) const +{ + checkResize(); + PxvArticulationDriveCache::initialize(cache, Ps::to16(mLinks.size()), mLinks.begin(), compliance, driveIterations, mScratchMemory.begin(), mScratchMemory.size()); +} + + +void Sc::ArticulationSim::releaseDriveCache(Sc::ArticulationDriveCache& driveCache) const +{ + PX_FREE(&driveCache); +} + + +void Sc::ArticulationSim::applyImpulse(Sc::BodyCore& link, + const Sc::ArticulationDriveCache& driveCache, + const PxVec3& force, + const PxVec3& torque) +{ + Cm::SpatialVectorV v[DY_ARTICULATION_MAX_SIZE], z[DY_ARTICULATION_MAX_SIZE]; + PxMemZero(z, mLinks.size()*sizeof(Cm::SpatialVector)); + PxMemZero(v, mLinks.size()*sizeof(Cm::SpatialVector)); + + PxU32 bodyIndex = findBodyIndex(*link.getSim()); + z[bodyIndex].linear = Ps::aos::V3LoadU(-force); + z[bodyIndex].angular = Ps::aos::V3LoadU(-torque); + + PxvArticulationDriveCache::applyImpulses(driveCache, z, v); + for(PxU32 i=0;igetBodyCore(); + PxVec3 lv, av; + Ps::aos::V3StoreU(v[i].linear, lv); + Ps::aos::V3StoreU(v[i].angular, av); + + body.setLinearVelocity(body.getLinearVelocity()+lv); + body.setAngularVelocity(body.getAngularVelocity()+av); + } +} + +void Sc::ArticulationSim::computeImpulseResponse(Sc::BodyCore& link, + PxVec3& linearResponse, + PxVec3& angularResponse, + const Sc::ArticulationDriveCache& driveCache, + const PxVec3& force, + const PxVec3& torque) const +{ + Cm::SpatialVectorV v; + PxvArticulationDriveCache::getImpulseResponse(driveCache, findBodyIndex(*link.getSim()), Cm::SpatialVectorV(Ps::aos::V3LoadU(force), Ps::aos::V3LoadU(torque)), v); + Ps::aos::V3StoreU(v.linear, linearResponse); + Ps::aos::V3StoreU(v.angular, angularResponse); +} + +void Sc::ArticulationSim::debugCheckWakeCounterOfLinks(PxReal wakeCounter) const +{ + PX_UNUSED(wakeCounter); + +#ifdef _DEBUG + // make sure the links are in sync with the articulation + for(PxU32 i=0; i < mBodies.size(); i++) + { + PX_ASSERT(mBodies[i]->getBodyCore().getWakeCounter() == wakeCounter); + } +#endif +} + +void Sc::ArticulationSim::debugCheckSleepStateOfLinks(bool isSleeping) const +{ + PX_UNUSED(isSleeping); + +#ifdef _DEBUG + // make sure the links are in sync with the articulation + for(PxU32 i=0; i < mBodies.size(); i++) + { + if (isSleeping) + { + PX_ASSERT(!mBodies[i]->isActive()); + PX_ASSERT(mBodies[i]->getBodyCore().getWakeCounter() == 0.0f); + PX_ASSERT(mBodies[i]->checkSleepReadinessBesidesWakeCounter()); + } + else + PX_ASSERT(mBodies[i]->isActive()); + } +#endif +} -- cgit v1.2.3