Created
June 10, 2022 20:33
-
-
Save jrouwe/5494733c7aecb7ce6fe68dfada3e5789 to your computer and use it in GitHub Desktop.
Fixed joint stress test for Bullet and PhysX
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "ConstraintDemo.h" | |
#include "btBulletDynamicsCommon.h" | |
#include "LinearMath/btIDebugDraw.h" | |
#include "../CommonInterfaces/CommonRigidBodyBase.h" | |
#ifndef M_PI | |
#define M_PI 3.14159265358979323846f | |
#endif | |
class AllConstraintDemo : public CommonRigidBodyBase | |
{ | |
public: | |
AllConstraintDemo(struct GUIHelperInterface* helper); | |
virtual ~AllConstraintDemo(); | |
virtual void initPhysics(); | |
virtual void exitPhysics(); | |
virtual void resetCamera() | |
{ | |
float dist = 40; | |
float pitch = 0; | |
float yaw = 0; | |
float targetPos[3] = {0, 15, 10}; | |
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); | |
} | |
virtual bool keyboardCallback(int key, int state); | |
}; | |
btFixedConstraint* createFixedConstraint(btRigidBody* body1, btRigidBody* body2) | |
{ | |
btTransform globalFrame = btTransform::getIdentity(); | |
float im1 = body1->getInvMass(); | |
float im2 = body2->getInvMass(); | |
globalFrame.setOrigin((im1 * body1->getWorldTransform().getOrigin() + im2 * body2->getWorldTransform().getOrigin()) / (im1 + im2)); | |
btTransform frameInA = body1->getWorldTransform().inverse() * globalFrame; | |
btTransform frameInB = body2->getWorldTransform().inverse() * globalFrame; | |
return new btFixedConstraint(*body1, *body2, frameInA, frameInB); | |
} | |
void AllConstraintDemo::initPhysics() | |
{ | |
m_guiHelper->setUpAxis(1); | |
m_collisionConfiguration = new btDefaultCollisionConfiguration(); | |
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); | |
m_broadphase = new btDbvtBroadphase(); | |
m_solver = new btSequentialImpulseConstraintSolver(); | |
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); | |
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); | |
btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 1, 0), 40); | |
m_collisionShapes.push_back(groundShape); | |
btTransform groundTransform; | |
groundTransform.setIdentity(); | |
groundTransform.setOrigin(btVector3(0, -56, 0)); | |
btRigidBody* groundBody = createRigidBody(0, groundTransform, groundShape); | |
btVector3 base_position(0, 15, 0); | |
btQuaternion base_rotation(btVector3(0, 0, 1), -0.5f * M_PI); | |
btCollisionShape* pillar = new btBoxShape(btVector3(0.1f, 1.0f, 0.1f)); | |
float pillar_mass = 1000.0f * 0.1f * 1.0f * 0.1f * 8.0f; | |
btCollisionShape* beam = new btBoxShape(btVector3(0.01f, 1.5f, 0.1f)); | |
float beam_mass = 1000.0f * 0.01f * 1.5f * 0.1f * 8.0f; | |
btRigidBody *prev_pillars[4] = { groundBody, groundBody, groundBody, groundBody }; | |
btVector3 center(0, 0, 0); | |
for (int y = 0; y < 10; ++y) | |
{ | |
// Create pillars | |
btRigidBody *pillars[4]; | |
for (int i = 0; i < 4; ++i) | |
{ | |
btQuaternion rotation = btQuaternion(btVector3(0, 1, 0), i * 0.5f * M_PI); | |
btTransform pillar_transform(base_rotation); | |
pillar_transform.setOrigin(base_position + btTransform(base_rotation) * (center + btTransform(rotation) * btVector3(1.0f, 1.0f, 1.0f))); | |
pillars[i] = createRigidBody(pillar_mass, pillar_transform, pillar); | |
} | |
for (int i = 0; i < 4; ++i) | |
{ | |
btQuaternion rotation = btQuaternion(btVector3(0, 1, 0), i * 0.5f * M_PI); | |
// Create cross beam | |
btTransform beam_transform(base_rotation * rotation * btQuaternion(btVector3(1, 0, 0), 0.3f * M_PI)); | |
beam_transform.setOrigin(base_position + btTransform(base_rotation) * (center + btTransform(rotation) * btVector3(1.105f, 1.0f, 0.0f))); | |
btRigidBody *cross = createRigidBody(beam_mass, beam_transform, beam); | |
// Attach cross beam to pillars | |
for (int j = 0; j < 2; ++j) | |
m_dynamicsWorld->addConstraint(createFixedConstraint(cross, pillars[(i + j) % 4]), true); | |
// Attach to previous pillar | |
if (prev_pillars[i] != nullptr) | |
m_dynamicsWorld->addConstraint(createFixedConstraint(prev_pillars[i], pillars[i]), true); | |
prev_pillars[i] = pillars[i]; | |
} | |
center += btVector3(0.0f, 2.0f, 0.0f); | |
} | |
// Create top | |
btTransform top_transform(base_rotation); | |
top_transform.setOrigin(base_position + btTransform(base_rotation) * (center + btVector3(0.0f, 0.1f, 0.0f))); | |
float top_mass = 1000.0f * 1.2f * 0.1f * 1.2f * 8.0f; | |
btRigidBody *top = createRigidBody(top_mass, top_transform, new btBoxShape(btVector3(1.2f, 0.1f, 1.2f))); | |
// Attach top to pillars | |
for (int i = 0; i < 4; ++i) | |
m_dynamicsWorld->addConstraint(createFixedConstraint(top, prev_pillars[i]), true); | |
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); | |
} | |
void AllConstraintDemo::exitPhysics() | |
{ | |
//delete dynamics world | |
delete m_dynamicsWorld; | |
m_dynamicsWorld = 0; | |
//delete solver | |
delete m_solver; | |
m_solver = 0; | |
//delete broadphase | |
delete m_broadphase; | |
m_broadphase = 0; | |
//delete dispatcher | |
delete m_dispatcher; | |
delete m_collisionConfiguration; | |
} | |
AllConstraintDemo::AllConstraintDemo(struct GUIHelperInterface* helper) | |
: CommonRigidBodyBase(helper) | |
{ | |
} | |
AllConstraintDemo::~AllConstraintDemo() | |
{ | |
btAssert(m_dynamicsWorld == 0); | |
} | |
bool AllConstraintDemo::keyboardCallback(int key, int state) | |
{ | |
bool handled = false; | |
switch (key) | |
{ | |
case ' ': | |
{ | |
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface(); | |
btVector3 camPos, camTarget; | |
renderer->getActiveCamera()->getCameraPosition(camPos); | |
renderer->getActiveCamera()->getCameraTargetPosition(camTarget); | |
btVector3 forward = (camTarget - camPos); | |
forward.normalize(); | |
float mass = 4.0f / 3.0f * 3.14f * 1000.0f; | |
btTransform transform = btTransform::getIdentity(); | |
transform.setOrigin(camPos); | |
btRigidBody *body = createRigidBody(mass, transform, new btSphereShape(1.0f)); | |
body->setLinearVelocity(100.0f * forward); | |
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); | |
handled = true; | |
break; | |
} | |
} | |
return handled; | |
} | |
class CommonExampleInterface* AllConstraintCreateFunc(struct CommonExampleOptions& options) | |
{ | |
return new AllConstraintDemo(options.m_guiHelper); | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <ctype.h> | |
#include "PxPhysicsAPI.h" | |
#include "../snippetcommon/SnippetPrint.h" | |
#include "../snippetcommon/SnippetPVD.h" | |
#include "../snippetutils/SnippetUtils.h" | |
using namespace physx; | |
PxDefaultAllocator gAllocator; | |
PxDefaultErrorCallback gErrorCallback; | |
PxFoundation* gFoundation = NULL; | |
PxPhysics* gPhysics = NULL; | |
PxDefaultCpuDispatcher* gDispatcher = NULL; | |
PxScene* gScene = NULL; | |
PxMaterial* gMaterial = NULL; | |
PxPvd* gPvd = NULL; | |
bool gPaused = true; | |
PxRigidDynamic *createRigidBody(const PxTransform &transform, const PxGeometry &geometry, const PxVec3& velocity=PxVec3(0)) | |
{ | |
PxRigidDynamic* body = PxCreateDynamic(*gPhysics, transform, geometry, *gMaterial, 1000.0f); | |
body->setLinearVelocity(velocity); | |
gScene->addActor(*body); | |
return body; | |
} | |
PxJoint* createFixedConstraint(PxRigidActor* body1, PxRigidActor* body2) | |
{ | |
const PxRigidDynamic* dynamic1 = body1->is<PxRigidDynamic>(); | |
const PxRigidDynamic* dynamic2 = body2->is<PxRigidDynamic>(); | |
float im1 = dynamic1 != nullptr? dynamic1->getInvMass() : 0.0f; | |
float im2 = dynamic2 != nullptr? dynamic2->getInvMass() : 0.0f; | |
PxTransform globalFrame((im1 * body1->getGlobalPose().p + im2 * body2->getGlobalPose().p) / (im1 + im2)); | |
PxTransform t0 = body1->getGlobalPose().getInverse() * globalFrame; | |
PxTransform t1 = body2->getGlobalPose().getInverse() * globalFrame; | |
return PxFixedJointCreate(*gPhysics, body1, t0, body2, t1); | |
} | |
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); | |
PxSceneDesc sceneDesc(gPhysics->getTolerancesScale()); | |
sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f); | |
gDispatcher = PxDefaultCpuDispatcherCreate(2); | |
sceneDesc.cpuDispatcher = gDispatcher; | |
sceneDesc.filterShader = PxDefaultSimulationFilterShader; | |
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); | |
} | |
gMaterial = gPhysics->createMaterial(0.5f, 0.5f, 0.6f); | |
PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0,1,0,0), *gMaterial); | |
gScene->addActor(*groundPlane); | |
PxVec3 base_position(0, 15, 0); | |
PxQuat base_rotation(-0.5f * PxPi, PxVec3(0, 0, 1)); | |
PxBoxGeometry pillar = PxBoxGeometry(PxVec3(0.1f, 1.0f, 0.1f)); | |
PxBoxGeometry beam = PxBoxGeometry(PxVec3(0.01f, 1.5f, 0.1f)); | |
PxRigidActor *prev_pillars[4] = { groundPlane, groundPlane, groundPlane, groundPlane }; | |
PxVec3 center(0, 0, 0); | |
for (int y = 0; y < 10; ++y) | |
{ | |
// Create pillars | |
PxRigidDynamic *pillars[4]; | |
for (int i = 0; i < 4; ++i) | |
{ | |
PxQuat rotation = PxQuat(i * 0.5f * PxPi, PxVec3(0, 1, 0)); | |
PxTransform pillar_transform(base_rotation); | |
pillar_transform.p = base_position + base_rotation.rotate(center + rotation.rotate(PxVec3(1.0f, 1.0f, 1.0f))); | |
pillars[i] = createRigidBody(pillar_transform, pillar); | |
} | |
for (int i = 0; i < 4; ++i) | |
{ | |
PxQuat rotation = PxQuat(i * 0.5f * PxPi, PxVec3(0, 1, 0)); | |
// Create cross beam | |
PxTransform beam_transform(base_rotation * rotation * PxQuat(0.3f * PxPi, PxVec3(1, 0, 0))); | |
beam_transform.p = base_position + base_rotation.rotate(center + rotation.rotate(PxVec3(1.105f, 1.0f, 0.0f))); | |
PxRigidDynamic *cross = createRigidBody(beam_transform, beam); | |
// Attach cross beam to pillars | |
for (int j = 0; j < 2; ++j) | |
createFixedConstraint(cross, pillars[(i + j) % 4]); | |
// Attach to previous pillar | |
if (prev_pillars[i] != nullptr) | |
createFixedConstraint(prev_pillars[i], pillars[i]); | |
prev_pillars[i] = pillars[i]; | |
} | |
center += PxVec3(0.0f, 2.0f, 0.0f); | |
} | |
// Create top | |
PxTransform top_transform(base_rotation); | |
top_transform.p = base_position + base_rotation.rotate(center + PxVec3(0.0f, 0.1f, 0.0f)); | |
PxRigidDynamic *top = createRigidBody(top_transform, PxBoxGeometry(PxVec3(1.2f, 0.1f, 1.2f))); | |
// Attach top to pillars | |
for (int i = 0; i < 4; ++i) | |
createFixedConstraint(top, prev_pillars[i]); | |
} | |
void stepPhysics(bool /*interactive*/) | |
{ | |
if (!gPaused) | |
{ | |
gScene->simulate(1.0f/60.0f); | |
gScene->fetchResults(true); | |
} | |
} | |
void cleanupPhysics(bool /*interactive*/) | |
{ | |
PX_RELEASE(gScene); | |
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 keyPress(unsigned char key, const PxTransform& camera) | |
{ | |
switch(toupper(key)) | |
{ | |
case ' ': createRigidBody(camera, PxSphereGeometry(1.0f), camera.rotate(PxVec3(0,0,-1))*100); break; | |
case 'P': gPaused = !gPaused; break; | |
} | |
} | |
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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment