main.cpp

#ifdef RENDER_SNIPPET

#include <vector>

#include "PxPhysicsAPI.h"

#include "../snippetrender/SnippetRender.h"
#include "../snippetrender/SnippetCamera.h"

using namespace physx;

extern void initPhysics(bool interactive);
extern void stepPhysics(bool interactive);	
extern void cleanupPhysics(bool interactive);
extern void keyPress(unsigned char key, const PxTransform& camera);
extern void renderText();

namespace
{
Snippets::Camera* sCamera;

void renderCallback()
{
	stepPhysics(true);

	if(0)
	{
		PxVec3 camPos = sCamera->getEye();
		PxVec3 camDir = sCamera->getDir();
		printf("camPos: (%ff, %ff, %ff)\n", camPos.x, camPos.y, camPos.z);
		printf("camDir: (%ff, %ff, %ff)\n", camDir.x, camDir.y, camDir.z);
	}

	Snippets::startRender(sCamera);

	const PxVec3 dynColor(1.0f, 0.5f, 0.25f);

	PxScene* scene;
	PxGetPhysics().getScenes(&scene,1);
	PxU32 nbActors = scene->getNbActors(PxActorTypeFlag::eRIGID_DYNAMIC | PxActorTypeFlag::eRIGID_STATIC);
	if(nbActors)
	{
		std::vector<PxRigidActor*> actors(nbActors);
		scene->getActors(PxActorTypeFlag::eRIGID_DYNAMIC | PxActorTypeFlag::eRIGID_STATIC, reinterpret_cast<PxActor**>(&actors[0]), nbActors);
		Snippets::renderActors(&actors[0], static_cast<PxU32>(actors.size()), true, dynColor);
	}

	PxU32 nbArticulations = scene->getNbArticulations();
	for(PxU32 i=0;i<nbArticulations;i++)
	{
		PxArticulationReducedCoordinate* articulation;
		scene->getArticulations(&articulation, 1, i);

		const PxU32 nbLinks = articulation->getNbLinks();
		std::vector<PxArticulationLink*> links(nbLinks);
		articulation->getLinks(&links[0], nbLinks);

		Snippets::renderActors(reinterpret_cast<PxRigidActor**>(&links[0]), static_cast<PxU32>(links.size()), true, dynColor);
	}

	renderText();

	Snippets::finishRender();
}

void exitCallback(void)
{
	delete sCamera;
	cleanupPhysics(true);
}
}

void renderLoop()
{
	sCamera = new Snippets::Camera(PxVec3(37.595165f, 61.040634f, 13.437684f), PxVec3(-0.801959f, -0.343857f, -0.488492f));

	//sCamera = new Snippets::Camera(PxVec3(34.613838f, 27.653027f, 9.363596f), PxVec3(-0.754040f, -0.401930f, -0.519496f));

	Snippets::setupDefault("PhysX Snippet Joint", sCamera, keyPress, renderCallback, exitCallback);

	initPhysics(true);
	glutMainLoop();
}
#endif

scene.cpp

#include <ctype.h>
#include <chrono>
#include <iostream>
#include "PxPhysicsAPI.h"
#include "../snippetcommon/SnippetPrint.h"
#include "../snippetcommon/SnippetPVD.h"
#include "../snippetutils/SnippetUtils.h"
#ifdef RENDER_SNIPPET
	#include "../snippetrender/SnippetRender.h"
#endif

using namespace physx;

static PxDefaultAllocator	gAllocator;
static PxDefaultErrorCallback	gErrorCallback;
static PxFoundation*		gFoundation = NULL;
static PxPhysics*		gPhysics    = NULL;
static PxDefaultCpuDispatcher*	gDispatcher = NULL;
static PxScene*			gScene	    = NULL;
static PxMaterial*		gMaterial   = NULL;
static PxPvd*			gPvd        = NULL;

static bool			gPause	    = false;
static bool			gOneFrame   = false;
static bool			gUseDefaultSettings = true;

enum Scenario
{
	WOBBLY_CONSTRAINTS_DEFAULT,
	WOBBLY_CONSTRAINTS_IMPROVED,
	WOBBLY_CONSTRAINTS_ARTICULATION,
};

static PxU32 gScenarioCount = 3;
static PxU32 gScenario = WOBBLY_CONSTRAINTS_DEFAULT;

static PxRigidDynamic* createDynamic(const PxTransform& t, const PxGeometry& geometry, const PxVec3& velocity=PxVec3(0))
{
	PxRigidDynamic* dynamic = PxCreateDynamic(*gPhysics, t, geometry, *gMaterial, 10.0f);
	dynamic->setAngularDamping(0.5f);
	dynamic->setLinearVelocity(velocity);
	gScene->addActor(*dynamic);

	if(gScenario==WOBBLY_CONSTRAINTS_IMPROVED)
		dynamic->setSolverIterationCounts(64, 1);

	return dynamic;
}

PxRigidDynamic* createBox(const PxTransform& t, float x, float y = 0, float z = 0, bool disableGravity = true)
{
	if (y == 0)
	{
		y = x;
	}

	if (z == 0)
	{
		z = y;
	}

	auto actor = createDynamic(t, PxBoxGeometry(x, y, z));
	
	if (disableGravity)
	{
		actor->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, true);
	}
	
	return actor;
}

PxFixedJoint* createFixed(PxRigidActor* a0, const PxTransform& t0, PxRigidActor* a1, const PxTransform& t1)
{
	return PxFixedJointCreate(*gPhysics, a0, t0, a1, t1);
}

PxJoint* push(PxRigidActor* a, PxRigidActor* b, PxD6Axis::Enum axis, PxD6Drive::Enum driveAx, PxVec3 velocity)
{
	auto ax = a->getGlobalPose();
	auto bx = b->getGlobalPose();

	//TODO: Fix rotation
	auto t = PxTransform(ax.p - bx.p);

	auto joint = PxD6JointCreate(*gPhysics, a, PxTransform(PxVec3(0)), b, t);
	joint->setMotion(axis, PxD6Motion::eFREE);
	
	PxD6JointDrive drive(PX_MAX_F32/1000, PX_MAX_F32/1000, PX_MAX_F32, true);
	joint->setDrive(driveAx, drive);
	joint->setDrivePosition(PxTransform(PxIdentity));
	joint->setDriveVelocity(velocity, PxVec3(0));

	return joint;
}

void fix(PxRigidActor* a, PxRigidActor* b)
{
	auto ax = a->getGlobalPose();
	auto bx = b->getGlobalPose();

	//TODO: Fix rotation
	auto t = PxTransform(ax.p - bx.p);

	//auto joint = 
		createFixed(a, PxTransform(PxVec3(0)), b, t);

	//joint->setProjectionLinearTolerance(0.001f);
	//joint->setProjectionAngularTolerance(0.001f);
}

// spherical joint limited to an angle of at most pi/4 radians (45 degrees)
/*static*/ PxJoint* createLimitedSpherical(PxRigidActor* a0, const PxTransform& t0, PxRigidActor* a1, const PxTransform& t1)
{
	PxSphericalJoint* j = PxSphericalJointCreate(*gPhysics, a0, t0, a1, t1);
	j->setLimitCone(PxJointLimitCone(PxPi/4, PxPi/4, 0.05f));
	j->setSphericalJointFlag(PxSphericalJointFlag::eLIMIT_ENABLED, true);
	return j;
}

// revolute joint limited to an angle of at most pi/4 radians (45 degrees)

// fixed, breakable joint
/*static*/ PxJoint* createBreakableFixed(PxRigidActor* a0, const PxTransform& t0, PxRigidActor* a1, const PxTransform& t1)
{
	PxFixedJoint* j = PxFixedJointCreate(*gPhysics, a0, t0, a1, t1);
	j->setBreakForce(1000, 100000);	
	j->setConstraintFlag(PxConstraintFlag::eDRIVE_LIMITS_ARE_FORCES, true);
	j->setConstraintFlag(PxConstraintFlag::eDISABLE_PREPROCESSING, true);
	return j;
}

// D6 joint with a spring maintaining its position
/*static*/ PxJoint* createDampedD6(PxRigidActor* a0, const PxTransform& t0, PxRigidActor* a1, const PxTransform& t1)
{
	PxD6Joint* j = PxD6JointCreate(*gPhysics, a0, t0, a1, t1);
	j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
	j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
	j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
	j->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(0, 1000, FLT_MAX, true));
	return j;
}

typedef PxJoint* (*JointCreateFunction)(PxRigidActor* a0, const PxTransform& t0, PxRigidActor* a1, const PxTransform& t1);

// create a chain rooted at the origin and extending along the x-axis, all transformed by the argument t.

/*static*/ void createChain(const PxTransform& t, PxU32 length, const PxGeometry& g, PxReal separation, JointCreateFunction createJoint)
{
	PxVec3 offset(separation/2, 0, 0);
	PxTransform localTm(offset);
	PxRigidDynamic* prev = NULL;

	for(PxU32 i=0;i<length;i++)
	{
		PxRigidDynamic* current = PxCreateDynamic(*gPhysics, t*localTm, g, *gMaterial, 1.0f);
		(*createJoint)(prev, prev ? PxTransform(offset) : t, current, PxTransform(-offset));
		gScene->addActor(*current);
		prev = current;
		localTm.p.x += separation;
	}
}

static PxArticulationLink* createLink(PxArticulationReducedCoordinate* articulation, PxArticulationLink* parent, PxArticulationJointType::Enum joint, const PxVec3& pos, const PxVec3& size, float density)
{
	PxArticulationLink*	link = articulation->createLink(parent, PxTransform(pos));
	PxRigidActorExt::createExclusiveShape(*link, PxBoxGeometry(size), *gMaterial);
	PxRigidBodyExt::updateMassAndInertia(*link, density);
	link->getInboundJoint()->setJointType(joint);
	return link;
}

static void initScene()
{
	PxSceneDesc sceneDesc(gPhysics->getTolerancesScale());
	sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);
	gDispatcher = PxDefaultCpuDispatcherCreate(2);
	sceneDesc.cpuDispatcher	= gDispatcher;
	sceneDesc.filterShader	= PxDefaultSimulationFilterShader;
	if(gScenario==WOBBLY_CONSTRAINTS_IMPROVED)
		sceneDesc.solverType = PxSolverType::eTGS;
	gScene = gPhysics->createScene(sceneDesc);

	PxPvdSceneClient* pvdClient = gScene->getScenePvdClient();
	if(pvdClient)
	{
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONSTRAINTS, true);
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONTACTS, true);
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_SCENEQUERIES, true);
	}

	PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0,1,0,0), *gMaterial);
	gScene->addActor(*groundPlane);

//	createChain(PxTransform(PxVec3(0.0f, 20.0f, 0.0f)), 5, PxBoxGeometry(2.0f, 0.5f, 0.5f), 4.0f, createLimitedSpherical);
//	createChain(PxTransform(PxVec3(0.0f, 20.0f, -10.0f)), 5, PxBoxGeometry(2.0f, 0.5f, 0.5f), 4.0f, createBreakableFixed);
//	createChain(PxTransform(PxVec3(0.0f, 20.0f, -20.0f)), 5, PxBoxGeometry(2.0f, 0.5f, 0.5f), 4.0f, createDampedD6);

	//Wobbly constraints
	if(1)
	{
		const float l = 10;
		if(gScenario==WOBBLY_CONSTRAINTS_ARTICULATION)
		{
			const float density = 10.0f;

			PxArticulationReducedCoordinate* articulation = gPhysics->createArticulationReducedCoordinate();

			articulation->setArticulationFlag(PxArticulationFlag::eFIX_BASE, true);

			const PxVec3 rootPos(0.0f, 40.0f, 0.0f);
			PxArticulationLink*	root = articulation->createLink(NULL, PxTransform(rootPos));
			PxRigidActorExt::createExclusiveShape(*root, PxBoxGeometry(1.0f, 1.0f, 1.0f), *gMaterial);
			PxRigidBodyExt::updateMassAndInertia(*root, density);

			const PxVec3 side1Pos(PxVec3(l - 1, 42.5f, 0.0f));
			PxArticulationLink*	side1 = createLink(articulation, root, PxArticulationJointType::eREVOLUTE, side1Pos - rootPos, PxVec3(l, 1, 1), density);
			PxArticulationJointReducedCoordinate* j = side1->getInboundJoint();
			j->setMotion(PxArticulationAxis::eSWING1, PxArticulationMotion::eFREE);
			j->setChildPose(PxTransform(PxVec3(-l + 1.0f, 0.0f, 0.0f)));
			j->setParentPose(PxTransform(PxVec3(0.0f, side1Pos.y - rootPos.y, 0.0f)));

			const PxVec3 mid1Pos(PxVec3((l - 1)*2, 45.0f, 0.0f));
			PxArticulationLink*	mid1 = createLink(articulation, side1, PxArticulationJointType::eFIX, mid1Pos - side1Pos, PxVec3(1, 1, 1), density);

			const PxVec3 side2Pos(PxVec3(l - 1, 47.5f, 0.0f));
			PxArticulationLink*	side2 = createLink(articulation, mid1, PxArticulationJointType::eFIX, side2Pos - mid1Pos, PxVec3(l, 1, 1), density);

			const PxVec3 midPos(PxVec3(0.0f, 50.0f, 0.0f));
			PxArticulationLink*	mid = createLink(articulation, side2, PxArticulationJointType::eFIX, midPos - side2Pos, PxVec3(1, 1, 1), density);

			const PxVec3 side3Pos(PxVec3(l - 1, 52.5f, 0.0f));
			/*PxArticulationLink* side3 =*/ createLink(articulation, mid, PxArticulationJointType::eFIX, side3Pos - midPos, PxVec3(l, 1, 1), density);

			gScene->addArticulation(*articulation);
		}
		else
		{
			auto root = PxCreateStatic(*gPhysics, PxTransform(PxVec3(0.0f, 40.0f, 0.0f)), PxBoxGeometry(1, 1, 1), *gMaterial, PxTransform(PxVec3(0)));
			gScene->addActor(*root);

			auto side1 = createBox(PxTransform(PxVec3(l - 1,     42.5f, 0.0f)), l, 1, 1, false);
			auto mid1  = createBox(PxTransform(PxVec3((l - 1)*2, 45,    0.0f)), 1, 1, 1, false);
			auto side2 = createBox(PxTransform(PxVec3(l - 1,     47.5f, 0.0f)), l, 1, 1, false);
			auto mid2  = createBox(PxTransform(PxVec3(    0,     50,    0.0f)), 1, 1, 1, false);
			auto side3 = createBox(PxTransform(PxVec3(l - 1,     52.5f, 0.0f)), l, 1, 1, false);
	
			auto rotorJoint = PxD6JointCreate(*gPhysics, root, PxTransform(PxVec3(0)), side1, PxTransform(root->getGlobalPose().p - side1->getGlobalPose().p));
			rotorJoint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);

			fix(side1, mid1);
			fix(mid1, side2);
			fix(side2, mid2);
			fix(mid2, side3);
		}

		auto impactor = createBox(PxTransform(PxVec3((l - 1) * 2, 42.5f, 10)), 1, 1, 1, true);
		impactor->setLinearVelocity(PxVec3(0, 0, -50));
	}

	//Phantom force
	if(0)
	{
		float l2 = 4.0f;
		auto root  = createBox(PxTransform(PxVec3( 0.0f,   40.0f - 20, 0.0f)), 1, 1, 1);
		auto push1 = createBox(PxTransform(PxVec3( l2-1,   40.0f - 20, 0.0f)), 1, 1, 1);
		auto push2 = createBox(PxTransform(PxVec3(-(l2-1), 40.0f - 20, 0.0f)), 1, 1, 1);
		auto side1 = createBox(PxTransform(PxVec3( 0.0f,   44.0f - 20, 0.0f)), l2, 1, 1);
		auto side2 = createBox(PxTransform(PxVec3( 0.0f,   36.0f - 20, 0.0f)), l2, 1, 1);
		auto pad1  = createBox(PxTransform(PxVec3( l2-1,   38.0f - 20, 0.0f)), 1, 0.3, 1);
		auto pad2  = createBox(PxTransform(PxVec3(-(l2-1),  42.0f - 20, 0.0f)), 1, 0.3, 1);

		fix(root, side1);
		fix(root, side2);
		fix(pad1, side1);
		fix(pad2, side2);
		push(side1, push1, PxD6Axis::Enum::eY, PxD6Drive::eY, PxVec3(0, -10, 0));
		push(side2, push2, PxD6Axis::Enum::eY, PxD6Drive::eY, PxVec3(0, 10, 0));

		//root->setLinearVelocity(PxVec3(0, -20, 0));
	}

	//Jiggle grid
	if(0)
	{
		auto root =  createBox(PxTransform(PxVec3(  0.0f + 50,   40.0f, 0.0f)),  1,  10,  1);
		auto wing1 = createBox(PxTransform(PxVec3( 12.0f + 50, 35.0f, 0.0f)), 10, 4.5f, 1);
		auto wing2 = createBox(PxTransform(PxVec3( 12.0f + 50, 45.0f, 0.0f)), 10, 4.5f, 1);
		auto wing3 = createBox(PxTransform(PxVec3(-12.0f + 50, 35.0f, 0.0f)), 10, 4.5f, 1);
		auto wing4 = createBox(PxTransform(PxVec3(-12.0f + 50, 45.0f, 0.0f)), 10, 4.5f, 1);
	
		//This make the simulation better but still not good
		//root->setMass(1000000);
		//root->setMassSpaceInertiaTensor(PxVec3(10000));

		fix(root, wing1);
		fix(root, wing2);
		fix(root, wing3);
		fix(root, wing4);
	
		wing1->addForce(PxVec3(10000));
	}

	if(0)
	{
		//Large grid
		for(int i = 0; i < 20; i++)
		{
			auto grid = createBox(PxTransform(PxVec3(0.0f, 40.0f + i* 100, -100.0f)), 1, 1, 1, false);

			for (int x = -5; x < 5; x++)
			for (int y = -5; y < 5; y++)
			for (int z = -5; z < 5; z++)
			{
				if (x == 0 && y == 0 && z == 0)
					continue;
			
				auto shape = gPhysics->createShape(PxBoxGeometry(0.5f, 0.5f, 0.5f), *gMaterial, true);
				shape->setLocalPose(PxTransform(PxVec3((float)x, (float)y, (float)z)));
				grid->attachShape(*shape);
			}
			PxRigidBodyExt::updateMassAndInertia(*grid, 10.0f);
		}
	}
}

void initPhysics(bool /*interactive*/)
{
	gFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, gAllocator, gErrorCallback);
	gPvd = PxCreatePvd(*gFoundation);
	PxPvdTransport* transport = PxDefaultPvdSocketTransportCreate(PVD_HOST, 5425, 10);
	gPvd->connect(*transport,PxPvdInstrumentationFlag::eALL);

	gPhysics = PxCreatePhysics(PX_PHYSICS_VERSION, *gFoundation, PxTolerancesScale(),true, gPvd);
	PxInitExtensions(*gPhysics, gPvd);

	gMaterial = gPhysics->createMaterial(0.5f, 0.5f, 0.6f);

	initScene();
}

void stepPhysics(bool /*interactive*/)
{
	if (gPause && !gOneFrame)
		return;
	gOneFrame = false;

	gScene->simulate(1.0f/60.0f);
	gScene->fetchResults(true);
}
	
static void releaseScene()
{
	PX_RELEASE(gScene);
}

void cleanupPhysics(bool /*interactive*/)
{
	releaseScene();
	PX_RELEASE(gDispatcher);
	PxCloseExtensions();
	PX_RELEASE(gPhysics);
	if(gPvd)
	{
		PxPvdTransport* transport = gPvd->getTransport();
		gPvd->release();	gPvd = NULL;
		PX_RELEASE(transport);
	}
	PX_RELEASE(gFoundation);
	
	printf("SnippetJoint done.\n");
}

void renderText()
{
#ifdef RENDER_SNIPPET
	Snippets::print("Current scenario:");
	if(gScenario==WOBBLY_CONSTRAINTS_DEFAULT)
		Snippets::print("Wobbly constraints, regular joints, default settings (PGS, 4 iterations)");
	else if(gScenario==WOBBLY_CONSTRAINTS_IMPROVED)
		Snippets::print("Wobbly constraints, regular joints, improved settings (TGS, 64 iterations)");
	else if(gScenario==WOBBLY_CONSTRAINTS_ARTICULATION)
		Snippets::print("Wobbly constraints, articulation, default settings (PGS, 4 iterations)");
#endif
}

void keyPress(unsigned char key, const PxTransform& /*camera*/)
{
	if(key == 'p' || key == 'P')
		gPause = !gPause;

	if(key == 'o' || key == 'O')
	{
		gPause = true;
		gOneFrame = true;
	}

	if(gScene)
	{
		if(key >= 1 && key <= gScenarioCount)
		{
			gScenario = key - 1;
			releaseScene();
			initScene();
		}

		if(key == 'r' || key == 'R')
		{
			releaseScene();
			initScene();
		}
	}
}

int snippetMain(int, const char*const*)
{
#ifdef RENDER_SNIPPET
	extern void renderLoop();
	renderLoop();
#else
	static const PxU32 frameCount = 100;
	initPhysics(false);
	for(PxU32 i=0; i<frameCount; i++)
		stepPhysics(false);
	cleanupPhysics(false);
#endif

	return 0;
}