I can connect two leg components together, and they lock in position but they are rigidly fixed at their starting position at 90deg. Also it seems like the x and z cood for the hinge are switched from the world frame?
Here is my code, and help would be great.
RagdollDemo.h
Code: Select all
/*
#ifndef RAGDOLLDEMO_H
#define RAGDOLLDEMO_H
#include "GlutDemoApplication.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
class btBroadphaseInterface;
class btCollisionShape;
class btOverlappingPairCache;
class btCollisionDispatcher;
class btConstraintSolver;
struct btCollisionAlgorithmCreateFunc;
class btDefaultCollisionConfiguration;
class RagdollDemo : public GlutDemoApplication
{
btAlignedObjectArray<class RagDoll*> m_ragdolls;
//keep the collision shapes, for deletion/cleanup
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
btBroadphaseInterface* m_broadphase;
btCollisionDispatcher* m_dispatcher;
btConstraintSolver* m_solver;
btDefaultCollisionConfiguration* m_collisionConfiguration;
btRigidBody* body[9]; // one main body, 4x2 leg segments
btCollisionShape* geom[9];
btHingeConstraint* joints[8];
bool pause = false;
bool oneStep;
double M_PI = 3.14159265358979323846;
double M_PI_2 = 1.57079632679489661923;
double M_PI_4 = 0.785398163397448309616;
public:
void initPhysics();
void exitPhysics();
virtual ~RagdollDemo()
{
exitPhysics();
}
void spawnRagdoll(const btVector3& startOffset);
virtual void clientMoveAndDisplay();
virtual void displayCallback();
virtual void keyboardCallback(unsigned char key, int x, int y);
static DemoApplication* Create()
{
RagdollDemo* demo = new RagdollDemo();
demo->myinit();
demo->initPhysics();
return demo;
}
void CreateBox(int index, double x, double y, double z, double length, double width, double height)
{
geom[index] = new btBoxShape(btVector3(width, height, length));
//btDefaultMotionState* motionstate = new btDefaultMotionState();
btDefaultMotionState* motionstate = new btDefaultMotionState(btTransform(
btQuaternion(0, 0, 0, 1),
btVector3(x, y, z)
));
btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
1.0, // mass, in kg. 0 -> Static object, will never move.
motionstate,
geom[index], // collision shape of body
btVector3(0, 0, 0) // local inertia
);
body[index] = new btRigidBody(rigidBodyCI);
m_dynamicsWorld->addRigidBody(body[index]);
}
void CreateCylinder(int index, double x, double y, double z, double diameter, double sideLength, double angle)
{
geom[index] = new btCylinderShape(btVector3(diameter, sideLength, diameter));
//btDefaultMotionState* motionstate = new btDefaultMotionState();
btDefaultMotionState* motionstate = new btDefaultMotionState(btTransform(
btQuaternion(btVector3(0, 0, 1), angle),
btVector3(x, y, z)
));
btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
1.0, // mass, in kg. 0 -> Static object, will never move.
motionstate,
geom[index], // collision shape of body
btVector3(0, 0, 0.5) // local inertia
);
// glRotatef(90, 1, 0, 0);
body[index] = new btRigidBody(rigidBodyCI);
m_dynamicsWorld->addRigidBody(body[index]);
}
void CreateCylinder2(int index, double x, double y, double z, double diameter, double sideLength, double angle)
{
geom[index] = new btCylinderShape(btVector3(diameter, sideLength, diameter));
//btDefaultMotionState* motionstate = new btDefaultMotionState();
btDefaultMotionState* motionstate = new btDefaultMotionState(btTransform(
btQuaternion(btVector3(1, 0, 0), angle),
btVector3(x, y, z)
));
btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
1.0, // mass, in kg. 0 -> Static object, will never move.
motionstate,
geom[index], // collision shape of body
btVector3(0.5, 0, 0) // local inertia
);
//glRotatef(90, 1, 0, 0);
body[index] = new btRigidBody(rigidBodyCI);
m_dynamicsWorld->addRigidBody(body[index]);
}
// takes a point p in world coordinates, and returns the coordinates of this point relative to the body stored in body[index].
btVector3 PointWorldToLocal(int index, btVector3 &p) {
btTransform local1 = body[index]->getCenterOfMassTransform().inverse();
return local1 * p;
}
//takes an axis a in world coordinates and returns the coordinates of this point relative to the body stored in body[index].
btVector3 AxisWorldToLocal(int index, btVector3 &a) {
btTransform local1 = body[index]->getCenterOfMassTransform().inverse();
btVector3 zero(0, 0, 0);
local1.setOrigin(zero);
return local1 * a;
}
btVector3 flipZY(btVector3 &input) {
btScalar temp;
temp = input[1];
input[1] = input[2];
input[2] = temp;
return input;
}
void CreateHinge(int index, int body1, int body2,
double x, double y, double z, double ax, double ay, double az) {
btVector3 p(x, y, z);
btVector3 a(ax, ay, az);
btVector3 p1 = PointWorldToLocal(body1, p);
btVector3 p2 = PointWorldToLocal(body2, p);
btVector3 a1 = AxisWorldToLocal(body1, a);
btVector3 a2 = AxisWorldToLocal(body2, a);
//btVector3 p1 = PointWorldToLocal(body1, flipZY(p));
////btVector3 p2 = PointWorldToLocal(body2, flipZY(p));
//btVector3 a1 = AxisWorldToLocal(body1, flipZY(a));
//btVector3 a2 = AxisWorldToLocal(body2, flipZY(a));
//
joints[index] = new btHingeConstraint(*body[body1], *body[body2],
p1, p2,
a1, a2, false);
//joints[index]->setLimit(btScalar(-M_PI_2), btScalar(M_PI_2));
m_dynamicsWorld->addConstraint(joints[index], true);
}
void DestroyHinge(int index) {
m_dynamicsWorld->removeConstraint(joints[index]);
}
void DeleteObject(int index) {
m_dynamicsWorld->removeRigidBody(body[index]);
delete body[index]->getMotionState();
delete body[index];
delete geom[index];
}
};
#endif
Code: Select all
/*
#define CONSTRAINT_DEBUG_SIZE 0.2f
#include "btBulletDynamicsCommon.h"
#include "GlutStuff.h"
#include "GL_ShapeDrawer.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
#include "RagdollDemo.h"
// Enrico: Shouldn't these three variables be real constants and not defines?
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923
#endif
#ifndef M_PI_4
#define M_PI_4 0.785398163397448309616
#endif
class RagDoll
{
enum
{
BODYPART_COUNT = 10
};
enum
{
JOINT_PELVIS_SPINE = 0,
JOINT_SPINE_HEAD,
JOINT_LEFT_HIP,
JOINT_LEFT_KNEE,
JOINT_RIGHT_HIP,
JOINT_RIGHT_KNEE,
JOINT_LEFT_SHOULDER,
JOINT_LEFT_ELBOW,
JOINT_RIGHT_SHOULDER,
JOINT_RIGHT_ELBOW,
JOINT_COUNT
};
btDynamicsWorld* m_ownerWorld;
btCollisionShape* geom[9]; //m_shapes[BODYPART_COUNT];
btRigidBody* body[9]; //m_bodies[BODYPART_COUNT];
btTypedConstraint* m_joints[JOINT_COUNT];
btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
{
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
shape->calculateLocalInertia(mass,localInertia);
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_ownerWorld->addRigidBody(body);
return body;
}
public:
RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset)
: m_ownerWorld (ownerWorld)
{
//
// Setup some damping on the m_bodies
for (int i = 0; i < BODYPART_COUNT; ++i)
{
body[i]->setDamping(0.05, 0.85);
body[i]->setDeactivationTime(0.8);
body[i]->setSleepingThresholds(1.6, 2.5);
}
// Now setup the constraints
/*btHingeConstraint* hingeC;
btConeTwistConstraint* coneC;
btTransform localA, localB;*/
}
virtual ~RagDoll ()
{
int i;
// Remove all constraints
for ( i = 0; i < JOINT_COUNT; ++i)
{
m_ownerWorld->removeConstraint(m_joints[i]);
delete m_joints[i]; m_joints[i] = 0;
}
// Remove all bodies and shapes
for ( i = 0; i < BODYPART_COUNT; ++i)
{
m_ownerWorld->removeRigidBody(body[i]);
delete body[i]->getMotionState();
delete body[i]; body[i] = 0;
delete geom[i]; geom[i] = 0;
}
}
};
void RagdollDemo::initPhysics()
{
// Setup the basic world
setTexturing(true);
setShadows(true);
setCameraDistance(btScalar(5.));
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btVector3 worldAabbMin(-10000,-10000,-10000);
btVector3 worldAabbMax(10000,10000,10000);
m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax);
m_solver = new btSequentialImpulseConstraintSolver;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
//m_dynamicsWorld->getDispatchInfo().m_useConvexConservativeDistanceUtil = true;
//m_dynamicsWorld->getDispatchInfo().m_convexConservativeDistanceThreshold = 0.01f;
// Setup a big ground box
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-10,0));
#define CREATE_GROUND_COLLISION_OBJECT 1
#ifdef CREATE_GROUND_COLLISION_OBJECT
btCollisionObject* fixedGround = new btCollisionObject();
fixedGround->setCollisionShape(groundShape);
fixedGround->setWorldTransform(groundTransform);
m_dynamicsWorld->addCollisionObject(fixedGround);
#else
localCreateRigidBody(btScalar(0.),groundTransform,groundShape);
#endif //CREATE_GROUND_COLLISION_OBJECT
}
// Spawn one ragdoll
btVector3 startOffset(1,0.5,0);
//spawnRagdoll(startOffset);
startOffset.setValue(-1,0.5,0);
//spawnRagdoll(startOffset);
CreateBox(0, 0, 2, 0, 1, 1, 0.2); // Create the box
CreateCylinder(1, 2, 2, 0, 0.2, 1, -1.57); // Create Leg 1
CreateCylinder(2, -2., 2., 0., 0.2, 1., -1.57); // Create Leg 2
CreateCylinder2(3, 0, 2., 2., 0.2, 1., -1.57); // Create Leg 3
CreateCylinder2(4, 0, 2., -2., 0.2, 1., -1.57); // Create Leg 4
CreateCylinder(5, 3., 1, 0, 0.2, 1, 0); // Create Leg 5
CreateCylinder(6, -3., 1, 0., 0.2, 1., 0); // Create Leg 6
CreateCylinder2(7, 0, 1, 3., 0.2, 1., 0); // Create Leg 7
CreateCylinder2(8, 0, 1, -3., 0.2, 1., 0); // Create Leg 8
CreateHinge(0, 4, 8, -1.5, 1.0, 0, 0, 0, 1);
clientResetScene();
}
void RagdollDemo::spawnRagdoll(const btVector3& startOffset)
{
RagDoll* ragDoll = new RagDoll (m_dynamicsWorld, startOffset);
m_ragdolls.push_back(ragDoll);
}
void RagdollDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//simple dynamics world doesn't handle fixed-time-stepping
float ms = getDeltaTimeMicroseconds();
float minFPS = 1000000.f/60.f;
if (ms > minFPS)
ms = minFPS;
if (m_dynamicsWorld)
{
if (!pause) {
m_dynamicsWorld->stepSimulation(ms / 1000000.f);
}
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
}
renderme();
glFlush();
glutSwapBuffers();
}
void RagdollDemo::displayCallback()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
renderme();
//optional but useful: debug drawing
if (m_dynamicsWorld)
m_dynamicsWorld->debugDrawWorld();
glFlush();
glutSwapBuffers();
}
void RagdollDemo::keyboardCallback(unsigned char key, int x, int y)
{
switch (key)
{
case 'e':
{
btVector3 startOffset(0,2,0);
spawnRagdoll(startOffset);
break;
}
default:
DemoApplication::keyboardCallback(key, x, y);
}
switch (key)
{
case 'p':
{
if (pause == false) {
pause = true;
}
else {
pause = false;
}
}
default:
DemoApplication::keyboardCallback(key, x, y);
}
}
void RagdollDemo::exitPhysics()
{
int i;
for (i=0;i<m_ragdolls.size();i++)
{
RagDoll* doll = m_ragdolls[i];
delete doll;
}
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
DeleteObject(0);
DeleteObject(1);
DeleteObject(2);
DeleteObject(3);
DeleteObject(4);
DeleteObject(5);
DeleteObject(6);
DeleteObject(7);
DeleteObject(8);
DestroyHinge(0);
for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject( obj );
delete obj;
}
//delete collision shapes
for (int j=0;j<m_collisionShapes.size();j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
//delete dynamics world
delete m_dynamicsWorld;
//delete solver
delete m_solver;
//delete broadphase
delete m_broadphase;
//delete dispatcher
delete m_dispatcher;
delete m_collisionConfiguration;
}