This problem has me stumped and I don't understand what could have caused it. Maybe something to do with pointers? Any help would be appreciated. Also is the way I am specifing shapes sane or way or is there a generally accepted pattern to doing these things effectivley?
Code: Select all
/*
Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans
Motor Demo
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btBulletDynamicsCommon.h"
#include "GlutStuff.h"
#include "GL_ShapeDrawer.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
#include "MotorDemo.h"
#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
#ifndef M_PI_8
#define M_PI_8 0.5 * M_PI_4
#endif
// LOCAL FUNCTIONS
void vertex(btVector3 &v)
{
glVertex3d(v.getX(), v.getY(), v.getZ());
}
void drawFrame(btTransform &tr)
{
const float fSize = 1.f;
glBegin(GL_LINES);
// x
glColor3f(255.f,0,0);
btVector3 vX = tr*btVector3(fSize,0,0);
vertex(tr.getOrigin()); vertex(vX);
// y
glColor3f(0,255.f,0);
btVector3 vY = tr*btVector3(0,fSize,0);
vertex(tr.getOrigin()); vertex(vY);
// z
glColor3f(0,0,255.f);
btVector3 vZ = tr*btVector3(0,0,fSize);
vertex(tr.getOrigin()); vertex(vZ);
glEnd();
}
// /LOCAL FUNCTIONS
#define NUM_FINGERS 3 + 1
#define BODYPART_COUNT 3 * NUM_FINGERS + 1
#define JOINT_COUNT BODYPART_COUNT
class TestRig
{
btDynamicsWorld* m_ownerWorld;
btCollisionShape* m_shapes[BODYPART_COUNT];
btRigidBody* m_bodies[BODYPART_COUNT];
btTypedConstraint* m_joints[JOINT_COUNT];
btVector3* m_fingers[NUM_FINGERS-1];
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;
}
btTransform localAsTransform(btVector3 v)
{
btTransform localTransform;
localTransform.setIdentity();
localTransform.setOrigin(v);
return localTransform;
}
public:
TestRig (btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
: m_ownerWorld (ownerWorld)
{
btVector3 vUp(0, 1, 0);
//
// Setup geometry
//
//Ok this is going to be a bit more complicated,
//but hopefully also organized
//1)Set up the parameters
float fForearm_Len = 1.5f;
float fForearm_Radius = 3.5f;
float fPalm_Len = 4.5f;
float fPalm_Width = 5.0f;
float fPalm_Dim = 1.5f;
float fFinger_Width = 1.0f;
//Finger Segment lenghts
float Finger_Diff_Len[2];
Finger_Diff_Len[0] = 2.0f;
Finger_Diff_Len[1] = 2.0f;
Finger_Diff_Len[2] = 1.0f;
//2) Vector Positions and Sizes, positions relative to hand base
//So the hand base would be at (0,0,0)
btVector3 Forearm_Pos(0,fForearm_Len,0);
btVector3 Palm_Pos(0, fForearm_Len*2 + fPalm_Len, 0 );
btVector3 Forearm_Size(fForearm_Radius, fForearm_Len, fForearm_Radius);
btVector3 Palm_Size(fPalm_Width, fPalm_Len, fPalm_Dim);
//Finger Base Positions
float fFinger_Offset_Y = fForearm_Len*2 + fPalm_Len*2;
m_fingers[0] = new btVector3(fFinger_Width, fForearm_Len*2 + fFinger_Width, -(fPalm_Dim + fFinger_Width));
m_fingers[1] = new btVector3(fPalm_Width-(1.0*fFinger_Width)-1.0,fFinger_Offset_Y, 0.5);
m_fingers[2] = new btVector3(fPalm_Width-(4.0*fFinger_Width)-1.0,fFinger_Offset_Y, 0.5);
m_fingers[3] = new btVector3(fPalm_Width-(7.0*fFinger_Width)-1.0,fFinger_Offset_Y, 0.5);
//Finger Position, relative to finger bases
btVector3* Finger_Diff_Pos[2];
Finger_Diff_Pos[0] = new btVector3(0,Finger_Diff_Len[0], 0);
Finger_Diff_Pos[1] = new btVector3(0,Finger_Diff_Len[0]*2+Finger_Diff_Len[1], 0);
Finger_Diff_Pos[2] = new btVector3(0,Finger_Diff_Len[0]*2+Finger_Diff_Len[1]*2+Finger_Diff_Len[2], 0);
//Finger Joint Positions, relative to finger bases
btVector3* Joint_Diff_Pos[2];
Joint_Diff_Pos[0] = new btVector3(0,0,0);
Joint_Diff_Pos[1] = new btVector3(0,Finger_Diff_Len[0]*2, 0);
Joint_Diff_Pos[2] = new btVector3(0,Finger_Diff_Len[0]*2+Finger_Diff_Len[1]*2, 0);
//
// Setup rigid bodies
//
//Set up Forearm and Palm
btTransform offset;
offset.setIdentity();
offset.setOrigin(positionOffset);
//
btTransform tForearm_Pos = localAsTransform(Forearm_Pos);
btTransform tPalm_Pos = localAsTransform(Palm_Pos);
m_shapes[0] = new btBoxShape(Forearm_Size);
m_shapes[1] = new btBoxShape(Palm_Size);
if (bFixed)
{
m_bodies[0] = localCreateRigidBody(btScalar(0.), offset*tForearm_Pos, m_shapes[0]);
m_bodies[1] = localCreateRigidBody(btScalar(0.), offset*tPalm_Pos, m_shapes[1]);
} else
{
m_bodies[0] = localCreateRigidBody(btScalar(1.), offset*tForearm_Pos, m_shapes[0]);
m_bodies[1] = localCreateRigidBody(btScalar(1.), offset*tPalm_Pos, m_shapes[1]);
}
// Fingers
int PALM=1;
int i;
for ( i=0; i<NUM_FINGERS; i++)
{
//Center of finger segments for body creation
btVector3 localV;
btTransform segment_Trans,joint_Trans,localA, localB;
int n, index, BASE;
for (n=0;n<3;n++)
{
index = (2+3*i)+n;
localV = *m_fingers[i] + *Finger_Diff_Pos[n];
segment_Trans = offset*localAsTransform(localV);
//printf("%f->",localV.getY());
//printf("%f\n",segment_Trans.getOrigin().getY());
localV = *m_fingers[i] + *Joint_Diff_Pos[n];
joint_Trans = offset*localAsTransform(localV);
printf("Added Segment %i under index %i::y=%f\n",n,index,segment_Trans.getOrigin().getY());
m_shapes[index] = new btCapsuleShape(fFinger_Width,Finger_Diff_Len[n]);
m_bodies[index] = localCreateRigidBody(btScalar(1.0),segment_Trans, m_shapes[index]);
//
// Setup the constraints
//
btHingeConstraint* hingeC;
// hip joints
if (n == 0)
{
BASE = PALM;
}
else
{
BASE=index-1;
}
printf("Attaching %i to %i\n",index,BASE);
localA.setIdentity();
localB.setIdentity();
localA = (m_bodies[BASE]->getWorldTransform().inverse() * joint_Trans);
localB =(m_bodies[index]->getWorldTransform().inverse() * joint_Trans);
printf("SgmT:%f,%f,%f\n",
segment_Trans.getOrigin().getX(),
segment_Trans.getOrigin().getY(),
segment_Trans.getOrigin().getZ());
//XXX - good result w/o this block, bad result w
printf("JntT:%f,%f,%f\n",
localA.getOrigin().getX(),
localA.getOrigin().getY(),
localA.getOrigin().getZ());
//XXX
/*
hingeC = new btHingeConstraint(*m_bodies[BASE], *m_bodies[index],localA,localB);
hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
//m_joints[index] = hingeC;
//m_ownerWorld->addConstraint(m_joints[index], false);
*/
printf("\n");
}
}
// Setup some damping on the m_bodies
for (i = 0; i < BODYPART_COUNT; ++i)
{
m_bodies[i]->setDamping(0.05, 0.85);
m_bodies[i]->setDeactivationTime(0.8);
//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
}
}
virtual ~TestRig ()
{
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(m_bodies[i]);
delete m_bodies[i]->getMotionState();
delete m_bodies[i]; m_bodies[i] = 0;
delete m_shapes[i]; m_shapes[i] = 0;
}
}
btTypedConstraint** GetJoints() {return &m_joints[0];}
};
void motorPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
{
MotorDemo* motorDemo = (MotorDemo*)world->getWorldUserInfo();
motorDemo->setMotorTargets(timeStep);
}
void MotorDemo::initPhysics()
{
setTexturing(true);
setShadows(true);
// Setup the basic world
m_Time = 0;
m_fCyclePeriod = 2000.f; // in milliseconds
// m_fMuscleStrength = 0.05f;
// new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
// should be (numberOfsolverIterations * oldLimits)
// currently solver uses 10 iterations, so:
m_fMuscleStrength = 0.5f;
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->setInternalTickCallback(motorPreTickCallback,this,true);
// 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,-30,0));
localCreateRigidBody(btScalar(0.),groundTransform,groundShape);
}
// Spawn one ragdoll
btVector3 startOffset(10,-10,-30);
//spawnTestRig(startOffset, false);
startOffset.setValue(-2,-10,30);
spawnTestRig(startOffset, false);
clientResetScene();
}
void MotorDemo::spawnTestRig(const btVector3& startOffset, bool bFixed)
{
TestRig* rig = new TestRig(m_dynamicsWorld, startOffset, bFixed);
m_rigs.push_back(rig);
}
void PreStep()
{
}
void MotorDemo::setMotorTargets(btScalar deltaTime)
{
float ms = deltaTime*1000000.;
float minFPS = 1000000.f/60.f;
if (ms > minFPS)
ms = minFPS;
m_Time += ms;
//
// set per-frame sinusoidal position targets using angular motor (hacky?)
//
/*
for (int r=0; r<m_rigs.size(); r++)
{
for (int i=0; i<2*NUM_LEGS; i++)
{
btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_rigs[r]->GetJoints()[i]);
btScalar fCurAngle = hingeC->getHingeAngle();
btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
btScalar fTargetAngle = 0.5 * (1 + sin(2 * M_PI * fTargetPercent));
btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
btScalar fAngleError = fTargetLimitAngle - fCurAngle;
btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms;
hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
}
}
*/
}
void MotorDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//simple dynamics world doesn't handle fixed-time-stepping
float deltaTime = getDeltaTimeMicroseconds()/1000000.f;
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(deltaTime);
m_dynamicsWorld->debugDrawWorld();
}
renderme();
for (int i=2; i>=0 ;i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
drawFrame(body->getWorldTransform());
}
glFlush();
glutSwapBuffers();
}
void MotorDemo::displayCallback()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
if (m_dynamicsWorld)
m_dynamicsWorld->debugDrawWorld();
renderme();
glFlush();
glutSwapBuffers();
}
void MotorDemo::keyboardCallback(unsigned char key, int x, int y)
{
switch (key)
{
case '+': case '=':
m_fCyclePeriod /= 1.1f;
if (m_fCyclePeriod < 1.f)
m_fCyclePeriod = 1.f;
break;
case '-': case '_':
m_fCyclePeriod *= 1.1f;
break;
case '[':
m_fMuscleStrength /= 1.1f;
break;
case ']':
m_fMuscleStrength *= 1.1f;
break;
default:
DemoApplication::keyboardCallback(key, x, y);
}
}
void MotorDemo::exitPhysics()
{
int i;
for (i=0;i<m_rigs.size();i++)
{
TestRig* rig = m_rigs[i];
delete rig;
}
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
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;
}
Code: Select all
Added Segment 0 under index 2::y=-4.000000
Attaching 2 to 1
SgmT:-1.000000,-4.000000,27.500000
Added Segment 1 under index 3::y=0.000000
Attaching 3 to 2
SgmT:-1.000000,0.000000,27.500000
Added Segment 2 under index 4::y=3.000000
Attaching 4 to 3
SgmT:-1.000000,3.000000,27.500000
Added Segment 0 under index 5::y=4.000000
Attaching 5 to 1
SgmT:1.000000,4.000000,30.500000
Added Segment 1 under index 6::y=8.000000
Attaching 6 to 5
SgmT:1.000000,8.000000,30.500000
Added Segment 2 under index 7::y=11.000000
Attaching 7 to 6
SgmT:1.000000,11.000000,30.500000
Added Segment 0 under index 8::y=4.000000
Attaching 8 to 1
SgmT:-2.000000,4.000000,30.500000
Added Segment 1 under index 9::y=8.000000
Attaching 9 to 8
SgmT:-2.000000,8.000000,30.500000
Added Segment 2 under index 10::y=11.000000
Attaching 10 to 9
SgmT:-2.000000,11.000000,30.500000
Added Segment 0 under index 11::y=4.000000
Attaching 11 to 1
SgmT:-5.000000,4.000000,30.500000
Added Segment 1 under index 12::y=8.000000
Attaching 12 to 11
SgmT:-5.000000,8.000000,30.500000
Added Segment 2 under index 13::y=11.000000
Attaching 13 to 12
SgmT:-5.000000,11.000000,30.500000
generating font at resolution 640,480
Code: Select all
Added Segment 0 under index 2::y=2.000000
Attaching 2 to 1
SgmT:-1.000000,2.000000,27.500000
JntT:1.000000,-3.500000,-2.500000
Added Segment 1 under index 3::y=0.000000
Attaching 3 to 2
SgmT:-1.000000,0.000000,27.500000
JntT:0.000000,-4.000000,0.000000
Added Segment 2 under index 4::y=3.000000
Attaching 4 to 3
SgmT:-1.000000,3.000000,27.500000
JntT:0.000000,2.000000,0.000000
Added Segment 0 under index 5::y=10.000000
Attaching 5 to 1
SgmT:1.000000,10.000000,30.500000
JntT:3.000000,4.500000,0.500000
Added Segment 1 under index 6::y=8.000000
Attaching 6 to 5
SgmT:1.000000,8.000000,30.500000
JntT:0.000000,-4.000000,0.000000
Added Segment 2 under index 7::y=11.000000
Attaching 7 to 6
SgmT:1.000000,11.000000,30.500000
JntT:0.000000,2.000000,0.000000
Added Segment 0 under index 8::y=10.000000
Attaching 8 to 1
SgmT:-2.000000,10.000000,30.500000
JntT:0.000000,4.500000,0.500000
Added Segment 1 under index 9::y=8.000000
Attaching 9 to 8
SgmT:-2.000000,8.000000,30.500000
JntT:0.000000,-4.000000,0.000000
Added Segment 2 under index 10::y=11.000000
Attaching 10 to 9
SgmT:-2.000000,11.000000,30.500000
JntT:0.000000,2.000000,0.000000
Added Segment 0 under index 11::y=10.000000
Attaching 11 to 1
SgmT:-5.000000,10.000000,30.500000
JntT:-3.000000,4.500000,0.500000
Added Segment 1 under index 12::y=8.000000
Attaching 12 to 11
SgmT:-5.000000,8.000000,30.500000
JntT:0.000000,-4.000000,0.000000
Added Segment 2 under index 13::y=11.000000
Attaching 13 to 12
SgmT:-5.000000,11.000000,30.500000
JntT:0.000000,2.000000,0.000000
generating font at resolution 640,480