Skip to content

Instantly share code, notes, and snippets.

@Pierre-Terdiman

Pierre-Terdiman/gist:eb42f474f324ef844d704aff63b5f82c

Last active November 8, 2022 19:50
Show Gist options
  • Save Pierre-Terdiman/eb42f474f324ef844d704aff63b5f82c to your computer and use it in GitHub Desktop.
Save Pierre-Terdiman/eb42f474f324ef844d704aff63b5f82c to your computer and use it in GitHub Desktop.
Download ZIP
Raw
gistfile1.txt
#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;
}
Raw
gistfile2.txt
#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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment