Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 515 insertions, 0 deletions

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; i<mBodies.size(); i++)
+	{
+		if(mBodies[i]==&body)
+			return i;
+	}
+	PX_ASSERT(0);
+	return 0x80000000;
+}
+
+void Sc::ArticulationSim::updateCached(Cm::BitMapPinned* shapeChangedMap)
+{
+	for(PxU32 i=0; i<mBodies.size(); i++)
+		mBodies[i]->updateCached(shapeChangedMap);
+}
+
+void Sc::ArticulationSim::updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray)
+{
+	for (PxU32 i = 0; i<mBodies.size(); i++)
+		mBodies[i]->updateContactDistance(contactDistance, dt, boundsArray);
+}
+
+ArticulationLinkHandle Sc::ArticulationSim::getLinkHandle(BodySim &body) const
+{
+	return reinterpret_cast<size_t>(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)<<index;
+		link.inboundJoint = &joint->getCore().getCore();
+		mLinks[parentIndex].children |= ArticulationBitField(1)<<index;
+	}
+	else
+	{
+		currentlyAsleep = (wakeCounter == 0.0f);
+		shouldSleep = currentlyAsleep && bodyReadyForSleep;
+
+		link.parent = DY_ARTICULATION_LINK_NONE;
+		link.pathToRoot = 1;
+		link.inboundJoint = NULL;
+	}
+
+	if (currentlyAsleep && (!shouldSleep))
+	{
+		for(PxU32 i=0; i < (mBodies.size() - 1); i++)
+			mBodies[i]->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;i<mLinks.size();i++)
+	{
+		mLinks[i-1] = mLinks[i];
+		mBodies[i-1] = mBodies[i];
+		mJoints[i-1] = mJoints[i];
+		//setIslandHandle(*mBodies[i-1], i-1);
+	}
+
+	// adjust parent/child indices
+	ArticulationBitField fixedIndices = (ArticulationBitField(1)<<index)-1;
+	ArticulationBitField shiftIndices = ~(fixedIndices|(ArticulationBitField(1)<<index));
+
+	for(PxU32 i=0;i<mLinks.size();i++)
+	{
+		ArticulationLink &link = mLinks[i];
+
+		if(link.parent != DY_ARTICULATION_LINK_NONE && link.parent>index)
+			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<FsData *>(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;i<mLinks.size();i++)
+		{
+			PxReal timer = mBodies[i]->getBodyCore().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;i<mLinks.size();i++)
+	{
+		PxReal timer = mBodies[i]->updateWakeCounter(dt, sleepThreshold, reinterpret_cast<Cm::SpatialVector&>(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;i<mLinks.size();i++)
+				mBodies[i]->getBodyCore().setWakeCounterFromSim(PxMax(1e-6f, mBodies[i]->getBodyCore().getWakeCounter()));
+		}
+		return;
+	}
+
+	for(PxU32 i=0;i<mLinks.size();i++)
+	{
+		mBodies[i]->notifyReadyForSleeping();
+		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;i<mLinks.size();i++)
+			mBodies[i]->internalWakeUpArticulationLink(wakeCounter);
+	}
+}
+
+void Sc::ArticulationSim::setActive(const bool b, const PxU32 infoFlag)
+{
+	for(PxU32 i=0;i<mBodies.size();i++)
+	{
+		if (i+1 < mBodies.size())
+		{
+			Ps::prefetchLine(mBodies[i+1],0);
+			Ps::prefetchLine(mBodies[i+1],128);
+		}
+		mBodies[i]->setActive(b, infoFlag);
+	}
+}
+
+void Sc::ArticulationSim::updateForces(PxReal dt, bool simUsesAdaptiveForce)
+{
+	PxU32 count = 0;
+	for(PxU32 i=0;i<mBodies.size();i++)
+	{
+		if (i+1 < mBodies.size())
+		{
+			Ps::prefetchLine(mBodies[i+1],128);
+			Ps::prefetchLine(mBodies[i+1],256);
+		}
+		mBodies[i]->updateForces(dt, NULL, NULL, count, &mAcceleration[i], simUsesAdaptiveForce);
+	}
+}
+
+void Sc::ArticulationSim::saveLastCCDTransform()
+{
+	for(PxU32 i=0;i<mBodies.size();i++)
+	{
+		if (i+1 < mBodies.size())
+		{
+			Ps::prefetchLine(mBodies[i+1],128);
+			Ps::prefetchLine(mBodies[i+1],256);
+		}
+		mBodies[i]->getLowLevelBody().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<FsData*>(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;i<mLinks.size();i++)
+	{
+		Sc::BodyCore& body = mBodies[i]->getBodyCore();
+		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
+}