btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btConvexShape* 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);
btVector3 color(1,0,0);
btVector3 scaling(1,1,1);
btShapeHull* hull = new btShapeHull(shape);
hull->buildHull(0.01);
{
int strideInBytes = 9*sizeof(float);
int numVertices = hull->numVertices();
int numIndices =hull->numIndices();
btAlignedObjectArray<GraphicsVertex> gvertices;
for (int i=0;i<numVertices;i++)
{
GraphicsVertex vtx;
btVector3 pos =hull->getVertexPointer()[i];
vtx.pos[0] = pos.x();
vtx.pos[1] = pos.y();
vtx.pos[2] = pos.z();
vtx.pos[3] = 1.f;
pos.normalize();
vtx.normal[0] =pos.x();
vtx.normal[1] =pos.y();
vtx.normal[2] =pos.z();
vtx.texcoord[0] = 0.5f;
vtx.texcoord[1] = 0.5f;
gvertices.push_back(vtx);
}
btAlignedObjectArray<int> indices;
for (int i=0;i<numIndices;i++)
indices.push_back(hull->getIndexPointer()[i]);
int shapeId = m_app->m_instancingRenderer->registerShape(&gvertices[0].pos[0],numVertices,&indices[0],numIndices);
m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,body->getWorldTransform().getOrigin(),body->getWorldTransform().getRotation(),color,scaling);
}
delete hull;
return body;
}
BulletDebug::BulletDebug(Ogre::SceneManager & scm, btDynamicsWorld & world) : _dd(&scm, 0.5), _world(&world), enabled(false)
{
world.setDebugDrawer(this);
_dd.setEnabled(true);
setDebugMode(btIDebugDraw::DBG_DrawWireframe);
}
void AddBody(const Json::Value& bodyDesc) {
std::string shapeName = bodyDesc["shape"].asString();
float mass = bodyDesc["mass"].asFloat();
float friction = bodyDesc["friction"].asFloat();
Json::Value transform = bodyDesc["transform"];
btCollisionShape* shape = boxShape;
if (shapes.count(shapeName) == 0) {
NaClAMPrintf("Could not find shape %s defaulting to unit cube.", shapeName.c_str());
} else {
shape = shapes[shapeName];
}
btTransform T;
T.setIdentity();
{
float m[16];
for (int i = 0; i < transform.size(); i++) {
m[i] = transform[i].asFloat();
}
T.setFromOpenGLMatrix(&m[0]);
}
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
shape->calculateLocalInertia(mass,localInertia);
btDefaultMotionState* myMotionState = new btDefaultMotionState(T);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(friction);
dynamicsWorld->addRigidBody(body);
}
virtual ~RigidObject()
{
if(body->getMotionState())
delete body->getMotionState();
world->removeRigidBody(body);
delete body;
}
void addPickingConstraint(const btVector3& rayFrom, const btVector3& rayTo) {
if (!dynamicsWorld) {
return;
}
removePickingConstraint();
if (pickedObjectIndex <= 0 || pickedObjectIndex >= dynamicsWorld->getNumCollisionObjects()) {
return;
}
pickedBody = btRigidBody::upcast(dynamicsWorld->getCollisionObjectArray()[pickedObjectIndex]);
btVector3 pickPos = rayTo;
btVector3 localPivot = pickedBody->getCenterOfMassTransform().inverse() * pickPos;
pickConstraint = new btPoint2PointConstraint(*pickedBody,localPivot);
pickedBody->setActivationState(DISABLE_DEACTIVATION);
dynamicsWorld->addConstraint(pickConstraint,true);
pickingDistance = (rayFrom-rayTo).length();
pickConstraint->m_setting.m_impulseClamp = 3.0f;
pickConstraint->m_setting.m_tau = 0.001f;
}
void EmptyScene() {
if (dynamicsWorld) {
int i;
for (i=dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--) {
btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState()) {
delete body->getMotionState();
}
dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
removePickingConstraint();
}
if (dynamicsWorld) {
delete dynamicsWorld;
dynamicsWorld = NULL;
}
if (solver) {
delete solver;
solver = NULL;
}
if (broadphase) {
delete broadphase;
broadphase = NULL;
}
if (dispatcher) {
delete dispatcher;
dispatcher = NULL;
}
if (collisionConfiguration) {
delete collisionConfiguration;
collisionConfiguration = NULL;
}
// Delete shapes
std::map<std::string, btCollisionShape*>::iterator it = shapes.begin();
while (it != shapes.end()) {
delete (*it).second;
it++;
}
shapes.clear();
// Clear name table
objectNames.clear();
}
void AddBox(const btTransform& T, float mass) {
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
boxShape->calculateLocalInertia(mass,localInertia);
btDefaultMotionState* myMotionState = new btDefaultMotionState(T);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,boxShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
dynamicsWorld->addRigidBody(body);
}
void removePickingConstraint() {
if (pickConstraint && dynamicsWorld)
{
dynamicsWorld->removeConstraint(pickConstraint);
delete pickConstraint;
pickedBody->forceActivationState(ACTIVE_TAG);
pickedBody->setDeactivationTime( 0.f );
}
pickConstraint = NULL;
pickedBody = NULL;
}
virtual ~RigidObject()
{
if(body)
{
if(body->getMotionState() != NULL)
delete body->getMotionState();
// if(body->getCollisionShape() != NULL)
// delete body->getCollisionShape();
world->removeRigidBody(body);
delete body;
}
}
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(m_bodies[i]);
delete m_bodies[i]->getMotionState();
delete m_bodies[i]; m_bodies[i] = 0;
delete m_shapes[i]; m_shapes[i] = 0;
}
}
RigidBody(Object3D* parent, Float mass, btCollisionShape* bShape, btDynamicsWorld& bWorld): Object3D{parent}, _bWorld(bWorld) {
/* Calculate inertia so the object reacts as it should with
rotation and everything */
btVector3 bInertia(0.0f, 0.0f, 0.0f);
if(!Math::TypeTraits<Float>::equals(mass, 0.0f))
bShape->calculateLocalInertia(mass, bInertia);
/* Bullet rigid body setup */
auto* motionState = new BulletIntegration::MotionState{*this};
_bRigidBody.emplace(btRigidBody::btRigidBodyConstructionInfo{
mass, &motionState->btMotionState(), bShape, bInertia});
_bRigidBody->forceActivationState(DISABLE_DEACTIVATION);
bWorld.addRigidBody(_bRigidBody.get());
}
void AddGroundPlane() {
btTransform groundTransform;
groundTransform.setIdentity();
btScalar mass = 0.0f;
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
dynamicsWorld->addRigidBody(body);
}
void FractureBody::clear(btDynamicsWorld& world)
{
for (int i = 0; i < m_connections.size(); ++i)
{
btRigidBody* cb = m_connections[i].m_body;
btAssert(!cb->getCollisionShape()->isCompound());
if (cb->isInWorld())
{
world.removeRigidBody(cb);
}
delete cb->getCollisionShape();
delete cb;
}
m_connections.clear();
}
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;
}
void* castRay(const btVector3 &from, const btVector3 &to, btVehicleRaycasterResult &result)
{
ClosestNotMeRayResultCallback rayCallback( _vehicle->collision->getBulletBody(), from, to );
const void *res = 0;
_world->rayTest(from, to, rayCallback);
if( rayCallback.hasHit() ) {
const btRigidBody* body = btRigidBody::upcast( rayCallback.m_collisionObject );
if( body && body->hasContactResponse() ) {
result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
result.m_hitNormalInWorld.normalize();
result.m_distFraction = rayCallback.m_closestHitFraction;
res = body;
}
}
return (void* )res;
}
void Step() {
if (dynamicsWorld)
dynamicsWorld->stepSimulation(1.0/60.0);
}
RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset)
: m_ownerWorld (ownerWorld)
{
// Setup the geometry
m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15), btScalar(0.20));
m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15), btScalar(0.28));
m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.40), btScalar(0.20));
m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45));
m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37));
m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45));
m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37));
m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33));
m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25));
m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33));
m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25));
// Setup all the rigid bodies
btTransform offset; offset.setIdentity();
offset.setOrigin(positionOffset);
btTransform transform;
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(1.), btScalar(0.)));
m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(1.2), btScalar(0.)));
m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(1.6), btScalar(0.)));
m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.65), btScalar(0.)));
m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.2), btScalar(0.)));
m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.65), btScalar(0.)));
m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.)));
m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.35), btScalar(1.45), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,M_PI_2);
m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.7), btScalar(1.45), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,M_PI_2);
m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.35), btScalar(1.45), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,-M_PI_2);
m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.7), btScalar(1.45), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,-M_PI_2);
m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]);
// Setup some damping on the m_bodies
for (int 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);
}
// Now setup the constraints
btHingeConstraint* hingeC;
btConeTwistConstraint* coneC;
btTransform localA, localB;
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB);
hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2));
m_joints[JOINT_PELVIS_SPINE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, M_PI_2);
m_joints[JOINT_SPINE_HEAD] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, 0);
m_joints[JOINT_LEFT_HIP] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB);
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_LEFT_KNEE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, 0);
m_joints[JOINT_RIGHT_HIP] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB);
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_RIGHT_KNEE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB);
coneC->setLimit(M_PI_2, M_PI_2, 0);
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_joints[JOINT_LEFT_SHOULDER] = coneC;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB);
// hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_LEFT_ELBOW] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB);
coneC->setLimit(M_PI_2, M_PI_2, 0);
m_joints[JOINT_RIGHT_SHOULDER] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB);
// hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_RIGHT_ELBOW] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
}
TestRig (btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
: m_ownerWorld (ownerWorld)
{
btVector3 vUp(0, 1, 0);
//
// Setup geometry
//
float fBodySize = 0.25f;
float fLegLength = 0.45f;
float fForeLegLength = 0.75f;
m_shapes[0] = new btCapsuleShape(btScalar(fBodySize), btScalar(0.10));
int i;
for ( i=0; i<NUM_LEGS; i++)
{
m_shapes[1 + 2*i] = new btCapsuleShape(btScalar(0.10), btScalar(fLegLength));
m_shapes[2 + 2*i] = new btCapsuleShape(btScalar(0.08), btScalar(fForeLegLength));
}
//
// Setup rigid bodies
//
float fHeight = 0.5;
btTransform offset; offset.setIdentity();
offset.setOrigin(positionOffset);
// root
btVector3 vRoot = btVector3(btScalar(0.), btScalar(fHeight), btScalar(0.));
btTransform transform;
transform.setIdentity();
transform.setOrigin(vRoot);
if (bFixed)
{
m_bodies[0] = localCreateRigidBody(btScalar(0.), offset*transform, m_shapes[0]);
} else
{
m_bodies[0] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[0]);
}
// legs
for ( i=0; i<NUM_LEGS; i++)
{
float fAngle = 2 * M_PI * i / NUM_LEGS;
float fSin = sin(fAngle);
float fCos = cos(fAngle);
transform.setIdentity();
btVector3 vBoneOrigin = btVector3(btScalar(fCos*(fBodySize+0.5*fLegLength)), btScalar(fHeight), btScalar(fSin*(fBodySize+0.5*fLegLength)));
transform.setOrigin(vBoneOrigin);
// thigh
btVector3 vToBone = (vBoneOrigin - vRoot).normalize();
btVector3 vAxis = vToBone.cross(vUp);
transform.setRotation(btQuaternion(vAxis, M_PI_2));
m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[1+2*i]);
// shin
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(fHeight-0.5*fForeLegLength), btScalar(fSin*(fBodySize+fLegLength))));
m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[2+2*i]);
}
// 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);
}
//
// Setup the constraints
//
btHingeConstraint* hingeC;
//btConeTwistConstraint* coneC;
btTransform localA, localB, localC;
for ( i=0; i<NUM_LEGS; i++)
{
float fAngle = 2 * M_PI * i / NUM_LEGS;
float fSin = sin(fAngle);
float fCos = cos(fAngle);
// hip joints
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,-fAngle,0); localA.setOrigin(btVector3(btScalar(fCos*fBodySize), btScalar(0.), btScalar(fSin*fBodySize)));
localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
m_joints[2*i] = hingeC;
m_ownerWorld->addConstraint(m_joints[2*i], true);
// knee joints
localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
localA.getBasis().setEulerZYX(0,-fAngle,0); localA.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(0.), btScalar(fSin*(fBodySize+fLegLength))));
localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
localC = m_bodies[2+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
//hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2));
m_joints[1+2*i] = hingeC;
m_ownerWorld->addConstraint(m_joints[1+2*i], true);
}
}
void HapticDevice::feedback(btDynamicsWorld &dynamic)
{
for(unsigned int i=0;i<m_nbDevices;i++)
{
bool ground_collide = false;
// free move
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 )// (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_2))
{
m_hss[i].m_free.m_done = true;
//m_hss[i].m_free.m_force = hduVector3Dd(0,0,0);
}
if(m_constraints[i] != NULL)
{
btRigidBody * myBody = &m_constraints[i]->getRigidBodyB();
btTransform myTrans = myBody->getWorldTransform();
m_effRenderPos = myTrans.getOrigin();
//Check collision
if(m_constraints[i]->getUserConstraintPtr() != NULL)
{
//std::cout<< " se cas la " <<std::endl;
m_hss[i].m_free.m_nbCollision = 1;
//if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_1) == 0)
btCollisionObject * object = static_cast<btCollisionObject *>(m_constraints[i]->getUserConstraintPtr());
if(object->getInternalType()== btCollisionObject::CO_RIGID_BODY)
{
btRigidBody * collideBody = static_cast<btRigidBody *>(object);
// collide with ground
if(collideBody == m_ground)
{
m_hss[i].m_free.m_nbCollision = 2;
ground_collide = true;
}
// collide with other object
if(collideBody->getInvMass()!=0 && collideBody != m_ground)
{
if(m_itsConstraints[i] == NULL )
{
// catch it if colide with it
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) == 0 )
{
//create constraint
btTransform bodyTrans = collideBody->getWorldTransform();
m_itsConstraints[i] = createConstraint(*myBody,*collideBody);
dynamic.addConstraint(m_itsConstraints[i],true);
m_newConstraint(m_ptr,collideBody,i);
m_caught = collideBody;
m_coll = true;
m_devine = false;
showTarget(collideBody);
deactivateMove();
}
}else
// realise it when button 1 pressed
if(m_freeT || (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0)
{
//remove constraint
dynamic.removeConstraint(m_itsConstraints[i]);
delete m_itsConstraints[i];
m_itsConstraints[i]=NULL;
m_deleteConstraint(m_ptr,collideBody,i);
//m_hss[i].setThrown(NULL);
m_hss[i].m_free.m_done = true;
showTarget(collideBody);
m_variator = 0;
m_coll = false;
deactivateMove();
}
}
}
}
else
{
m_hss[i].m_free.m_nbCollision = 0;
//if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_1) == 0)
if(m_freeT || (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0)
{
if(m_itsConstraints[i] != NULL )
{
//remove constraint
m_deleteConstraint(m_ptr,&m_itsConstraints[i]->getRigidBodyB(),i);
dynamic.removeConstraint(m_itsConstraints[i]);
delete m_itsConstraints[i];
m_itsConstraints[i]=NULL;
m_hss[i].m_free.m_done = true;
if(m_caught != NULL)
showTarget(m_caught);
m_coll = false;
deactivateMove();
}
}
}
if(m_itsConstraints[i]!=NULL )
m_hss[i].m_free.m_nbCollision = 1;
else
// launch an other target
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_2) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_2) == 0)
{
//m_canLaunch = true;
}
m_oldButtons[i]=m_hss[i].m_free.m_buttons;
//put back cursor world position into device referencial <<<<< ------------------------------
btTransform offset(btMatrix3x3::getIdentity(),btVector3(0,0,-0.5));
m_effectors[i].setOrigin(myTrans.getOrigin());
m_effectors[i].mult(m_effectors[i],offset);
btVector3 pos = m_cameraViews[i]->inverse()(myTrans.getOrigin());
pos*=SCALE_WORLD_TO_DEVICE;
m_hss[i].m_free.m_realPosition.set(pos.getX(),pos.getY(),pos.getZ()+OFFSET_TO_CAMERA);
}
btTransform camInv = m_cameraViews[i]->inverse();
// compute feed back for ground
if(ground_collide)
{
m_hss[i].m_free.m_force = groundForce(true,&(m_hss[i].m_free.m_position),&camInv);
}else
m_hss[i].m_free.m_force = hduVector3Dd(0,0,0);
// detecte the direction
HDdouble deplacement = betweenTwoPoints(m_hss[i].m_free.m_atThrowPos,m_hss[i].m_free.m_position);
HDdouble distanceMax = Distance_max;
hduVector3Dd move = m_hss[i].m_free.m_oldPosition - m_hss[i].m_free.m_position;
HDdouble selectedDistance = 0;
if( m_devine && m_thrownRigids != NULL && m_thrownRigids->size()>0 && deplacement > distanceMax && m_sible == 0 /* && !m_targetChoosen */){
HDdouble distance = Quick_Distance_max;
//unsigned int targ = 0;
for(unsigned int j = 0; j<m_thrownRigids->size(); j++){
HDdouble d = distanceToTrajectory(m_hss[i].m_free.m_position,j,&camInv);
if((d)<distance){
if(m_Thrown != (*m_thrownRigids)[j]){
m_hss[i].m_free.m_currentThrown = (*m_thrownRigids)[j];
distance = d;
m_Thrown = (*m_thrownRigids)[j];
m_impactPos = m_possibleImpact[j];
m_targetChoosen = true;
if(m_Feedback){
activateMove();
}
selectedDistance = d;
m_index = j;
m_sible = Nbr_frame_wait;
}
}
}
if(m_targetChoosen){
//showTarget(m_Thrown);
addPrevious(m_Thrown);
}
else
m_sible = 0;
//m_time = Time;
}
if(m_canLaunch){
//m_time = Time;
m_hss[i].m_free.m_done = true;
m_variator = 0.001;
m_sible = 0;
m_devine = true;
deactivateMove();
cleanHistory();
m_hss[i].m_free.m_atThrowPos = m_hss[i].m_free.m_position;
}
m_hss[i].m_free.m_oldPosition = m_hss[i].m_free.m_position ;
if(m_hss[i].m_free.m_currentThrown != NULL && m_posSet && m_Feedback ){
hduVector3Dd impact = invertTransform(m_impactPos, &camInv);
hduVector3Dd pos(m_hss[i].m_free.m_position);
btVector3 balltest = ((*m_thrownRigids)[0])->getWorldTransform().getOrigin();
hduVector3Dd test = invertTransform(&balltest, &camInv);
m_velocity = m_hss[i].m_free.m_velocity.magnitude();
if(pos[2]-5 >= test[2]){
if(!m_hss[i].m_free.m_done){
//hduVector3Dd helpForce = ForceToImpact(&pos,&impact);
hduVector3Dd helpForce = magneticForce(&pos, &impact, &camInv);
//hduVector3Dd helpForce = atomeForce(&pos, &impact, &camInv);
if(!helpForce.isZero(EPSILON)){
hduVector3Dd force = 1.0 * helpForce;
m_hss[i].m_free.m_force = force;
m_Force = force;
}else
{
//m_hss[i].m_free.m_force = m_Force;
}
}
}
}
hdScheduleSynchronous(sScheduleIn, &m_hss, HD_DEFAULT_SCHEDULER_PRIORITY);
m_selectedDistance = selectedDistance;
if(m_sible > 0)
m_sible--;
}
}
RagDoll2 (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,SimpleOpenGL3App* app)
: m_ownerWorld (ownerWorld),
m_app(app)
{
// Setup the geometry
m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15), btScalar(0.20));
m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15), btScalar(0.28));
m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.10), btScalar(0.05));
m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45));
m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37));
m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45));
m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37));
m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33));
m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25));
m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33));
m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25));
// Setup all the rigid bodies
btTransform offset; offset.setIdentity();
offset.setOrigin(positionOffset);
btTransform transform;
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(1.), btScalar(0.)));
//m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS], btVector3(0,1,0));
// btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep);
int n_links = 0;
float mass = 1.f;
btVector3 localInertia;
m_shapes[BODYPART_PELVIS]->calculateLocalInertia(mass,localInertia);
bool isFixedBase = true;
bool canSleep = true;
bool isMultiDof = false;
btMultiBody * bod = new btMultiBody(n_links, mass, localInertia, isFixedBase, canSleep, isMultiDof);
btTransform tr;
tr = offset*transform;
bod->setBasePos(tr.getOrigin());
bod->setWorldToBaseRot(tr.getRotation());
btVector3 vel(0,0,0);
bod->setBaseVel(vel);
#if 0
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(1.2), btScalar(0.)));
m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE],btVector3(0,0,1));
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.), btScalar(1.6), btScalar(0.)));
m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.65), btScalar(0.)));
m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.2), btScalar(0.)));
m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.65), btScalar(0.)));
m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.)));
m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.35), btScalar(1.45), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,M_PI_2);
m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(-0.7), btScalar(1.45), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,M_PI_2);
m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.35), btScalar(1.45), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,-M_PI_2);
m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]);
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0.7), btScalar(1.45), btScalar(0.)));
transform.getBasis().setEulerZYX(0,0,-M_PI_2);
m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]);
// Setup some damping on the m_bodies
for (int 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);
}
#endif
#if 0
// Now setup the constraints
btHingeConstraint* hingeC;
btConeTwistConstraint* coneC;
btTransform localA, localB;
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB);
hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2));
m_joints[JOINT_PELVIS_SPINE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, M_PI_2);
m_joints[JOINT_SPINE_HEAD] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, 0);
m_joints[JOINT_LEFT_HIP] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB);
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_LEFT_KNEE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, 0);
m_joints[JOINT_RIGHT_HIP] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB);
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_RIGHT_KNEE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB);
coneC->setLimit(M_PI_2, M_PI_2, 0);
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_joints[JOINT_LEFT_SHOULDER] = coneC;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB);
// hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_LEFT_ELBOW] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.)));
localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB);
coneC->setLimit(M_PI_2, M_PI_2, 0);
m_joints[JOINT_RIGHT_SHOULDER] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
localA.setIdentity(); localB.setIdentity();
localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.)));
localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB);
// hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_RIGHT_ELBOW] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
#endif
}