RagdollDemo.cpp

/*
RagdollDemo.cpp

*/

#include <sstream>
#include <iostream>
#include <string>
#include <math.h>
#include <time.h>
#include <fstream>
#include <cstdio>
#include <cmath>
#include <float.h>

#define CONSTRAINT_DEBUG_SIZE 0.2f
#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

#include "btBulletDynamicsCommon.h"
#include "GlutStuff.h"
#include "GL_ShapeDrawer.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
#include "RagdollDemo.h"
#include "RagDollclass.h"

using namespace std;


void ConsoleCollideIDs(int Acid, int Bcid);

static RagdollDemo * ragdollDemo;
static RagdollDemo * ragdollptrs[12];

#include "ContactCallbacks.h" // ..

bool myContactProcessedCallback(btManifoldPoint& cp,
                                void* body0, void* body1)
{
    int groundID = 9;
    int conID1,conID2;
    int * ID1;
    int * ID2;

    btCollisionObject * o1 = static_cast<btCollisionObject*>(body0);
    btCollisionObject * o2 = static_cast<btCollisionObject*>(body1);

    ID1 = static_cast<int*>(o1->getUserPointer());
    ID2 = static_cast<int*>(o2->getUserPointer());

    conID1 = (*ID1);
    conID2 = (*ID2);

    if((conID1<10)&&(conID2<10) ){
        if( (conID1 == groundID) || (conID2 == groundID ) ){
            ragdollDemo->touches[conID1]=1;
            ragdollDemo->touches[conID2]=1;
            ragdollDemo->touchPoints[conID1] = cp.m_positionWorldOnB;
            ragdollDemo->touchPoints[conID2] = cp.m_positionWorldOnB;
        }
    }
    return false;
}



void RagdollDemo::initPhysics()
{
    int i,  nocans;
    //.

    // Setup the basic world
    setTexturing(true);
    setShadows(true);
    setCameraDistance(btScalar(5.));
    btVector3 worldAabbMin(-10000,-10000,-10000);
    btVector3 worldAabbMax(10000,10000,10000);

    m_collisionConfiguration    = new btDefaultCollisionConfiguration();
    m_dispatcher                = new btCollisionDispatcher(m_collisionConfiguration);
    m_broadphase                = new btAxisSweep3 (worldAabbMin, worldAabbMax);
    m_solver                    = new btSequentialImpulseConstraintSolver;
    m_dynamicsWorld             = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);


    srand( (unsigned)(time(NULL)) );
    pause = true;
    oneStep = false;
    position_sane = true;
    globaljointA = 0.001;
    simtick = 0;
    timeStep = 0;
    i=0;
    while( i<45 ) {
        IDs[i] = i;
        i++;
    }

    // Setup a big ground box
    {
        btCollisionShape * groundShape = new btBoxShape(btVector3(btScalar(300.),btScalar(10.),btScalar(300.)));
        m_collisionShapes.push_back(groundShape);
        btTransform groundTransform;
        groundTransform.setIdentity();
        groundTransform.setOrigin(btVector3(0,-10,0));

        fixedGround = new btCollisionObject();
        fixedGround->setCollisionShape(groundShape);
        fixedGround->setWorldTransform(groundTransform);
        m_dynamicsWorld->addCollisionObject(fixedGround);
    }

    //cout << "CreateBox(0...)" << endl;
    CreateBox(0,
              0., 2., 0.,
              1., 1., 0.2 ); // Create the robot body

    CreateCylinder(1,
              2.02 , 2. , 0.0 ,
              0.2 , 0.2 , 0.8 ,
              M_PI_2 , 0. , 0. ); // West thigh

    CreateCylinder(2,
              -2.02 , 2. , 0.0 ,
              0.2 , 0.2 , 0.8 ,
              M_PI_2 , 0. , 0. ); // East thigh

    CreateCylinder(3,
              0. , 2. , -2.02 ,
              0.2 , 0.2 , 0.8 ,
              0 , 0. , M_PI_2 ); // South thigh

    CreateCylinder(4,
              0. , 2. , 2.02  ,
              0.2 , 0.2 , 0.8 ,
              0 , 0. , M_PI_2 ); // North thigh

    CreateCylinder(5,
              3.10 , 0.89 , 0.0 ,
              0.2 ,  0.2 , 0.89 ,
              0 , 0. , 0. ); // West foot

    CreateCylinder(6,
              -3.10 , 0.89 , 0.0 ,
              0.2 ,   0.2 , 0.89 ,
              0 , 0. , 0. ); // East foot

    CreateCylinder(7,
              0.00 , 0.89 , 3.10 ,
              0.2 ,  0.2 , 0.89 ,
              0 , 0. , 0. ); // North foot

    CreateCylinder(8,
              0.00 , 0.89 , -3.10 ,
              0.2 ,   0.2 , 0.89 ,
              0 , 0. , 0. ); // South foot


    if( !OPTloadbestweights ) {
        //cout << "BuildWallofCans()" << endl;
        BuildWallofCans();   // wall of cans
    }

    CreateBox(9,
              0., 2.01, 105.,
              4., 2., 2. ); // Create a north marker

    CreateSphere(10,
                -3.00 , 2. , 0.0 ,
                0.27 );    // East kneeball

    CreateSphere(11,
                3.00 , 2. , 0.0 ,
                0.27 );    // West kneeball

    CreateSphere(12,
                0.0 , 2. , 3.00 ,
                0.27 );    // North kneeball

    CreateSphere(13,
                0.0 , 2. , -3.00 ,
                0.27 );    // South kneeball



    CreateWeld(0, 2, 10 ); // Weld east kneeball(10) to east thigh(2)
    CreateWeld(1, 1, 11 ); // Weld west kneeball(11) to west thigh(1)
    CreateWeld(2, 4, 12 ); // Weld north kneeball(12) to north thigh(4)
    CreateWeld(3, 3, 13 ); // Weld south kneeball(13) to south thigh(3)


    if( OPTloadbestweights ) {
        nocans = 15;
    } else {
        nocans = 42;
    }

    i=0;
    while( i < nocans ) {
        if(i==9) {
            fixedGround->setUserPointer( &(IDs[i]) );
            (body[i])->setUserPointer(  &(IDs[14]) );
        } else {
            if( i < 14 ) {
                (body[i])->setUserPointer(  &(IDs[i] ));
            }

            if( i > 14 ) {
                (body[i])->setUserPointer(  &(IDs[i] ));
            }
        }
        i++;
    }

    //cout << "CreateHinge(0.." << endl;

    // JOINT = 0
    // East thigh(2) connects East foot(6)
    CreateHinge(0 , 2 , 6 ,
        -3.10 , 2.00 , 0.0,
         0.00 , 0.00 , -1.00 );

    // JOINT = 1
    // West thigh(1) connects West foot(5)
    CreateHinge(1 , 1 , 5,
         3.10 , 2.00 , 0.0,
         0.00 , 0.00 , -1.00 );

    // JOINT = 2
    // South thigh(3) connects South foot(8)
    CreateHinge(2 , 8 , 3,
          0.00 ,  2.00 , -3.10,
         -1.00 , 0.00 , 0.00 );

    // JOINT = 3
    // North thigh(4) connects North foot(7)
    CreateHinge(3 , 7 , 4,
          0.00 ,  2.00 , 3.10,
         -1.00 , 0.00 ,  0.00 );


    // JOINT = 4
    // body connects East thigh(2)
    CreateHinge(4 , 0 , 2,
          -1.02 , 2.00 , 0.00,
           0.00 , 0.00 , -1.00 );

    // JOINT = 5
    // body connects West thigh(1)
    CreateHinge(5 , 0 , 1,
           1.02 , 2.00 , 0.00,
           0.00 , 0.00 , -1.00 );

    // JOINT = 6
    // body connects North thigh(4)
    CreateHinge(6 , 0 , 4,
            0.00 , 2.00 , 1.02,
           -1.00 , 0.00 , 0.00 );

    // JOINT = 7
    // body connects South thigh(3)
    CreateHinge(7 , 0 , 3,
            0.00 , 2.00 , -1.02,
           -1.00 , 0.00 , 0.00 );
    i=0;
    while(i<8){
        (joints[i])->enableMotor(true);
        i++;
    }

    if( threadID < 0 ) {
        //cout << "gContactProcessedCallback" << endl;
        ragdollDemo = this;
        gContactProcessedCallback = myContactProcessedCallback;
    } else {
        ragdollptrs[threadID] = this;
        AssignToCallback();
    }

    //cout << "clientResetScene()" << endl;
    pause = false;
    clientResetScene();
}


// * //

void RagdollDemo::clientMoveAndDisplay()
{
    long int rollover_escape;
    int nn, m;
    bool sS;
    bool air;
    double deltaT;
    double motorCommand;
    //.

    if( OPTdrawgraphics ) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    }

    //simple dynamics world doesn't handle fixed-time-stepping
    float ms = getDeltaTimeMicroseconds();
    float minFPS = 90000.f/60.f;
    if (ms > minFPS) ms = minFPS;
    deltaT = ms / 90000.f;

    if (m_dynamicsWorld)
    {
        sS = false;
        if(!pause){
            sS=true;
        }
        if( pause && oneStep ) {
            oneStep = false;
            sS = true;
        }

        if(sS) {
            m=0;
            while( m<15 ) {
                touches[m] = 0;
                m++;
            }
            air = true; // because of the above.


            rollover_escape = 1;
            while ( air && (rollover_escape < 1510) )
            {
                    m_dynamicsWorld->stepSimulation( btScalar(deltaT) );

                    if( !SanityCheck( 500.0 ) ) {
                        // The physics simulation is crashing?
                        rollover_escape = 9999;
                        cout << "Sanity check failed at timeStep = ";
                        cout << timeStep;
                        cout <<endl;
                    }

                    air = airborne();
                    rollover_escape++;
            }

            if( rollover_escape > 1505 ) {
                // The robot is upside down, or it fell off the earth.
                if( OPTdrawgraphics ) {
                    pause = true;  // Trial runs pause.
                } else {
                    timeStep = 987123123;  // Fitness tests should terminate.
                }
            }



            if( simtick == 0 ) {
                m=0;
                while(m<8) {
                    nn=0;
                    while( nn < 4 ) {
                        // We need the touches from 5,6,7, and 8
                        touchsensor[nn] = (double)touches[nn+5];
                        nn++;
                    }

                    motorCommand = 0.0;
                    nn=0;
                    while( nn < 4 ) {
                        motorCommand = motorCommand +
                                       (touchsensor[nn] * weights[nn][m]);
                        nn++;
                    }

                    motorCommand = tanh(motorCommand);
                    motorCommand = motorCommand * 45.0;
                    ActuateJoint2(m , motorCommand , -1.  ,  deltaT );
                    m++;
                }
            }

            simtick = (simtick+1)%23;
        }

        if(OPTdrawgraphics) {
            //optional but useful: debug drawing
            m_dynamicsWorld->debugDrawWorld();
        }

        timeStep++;
    }

    if(OPTdrawgraphics) {
        renderme();
        glFlush();
        glutSwapBuffers();
    }
}


// * //

void    RagdollDemo::exitPhysics()
{
    int i, nocans;

    i=8;
    while(i>0){
        i--;
        (joints[i])->enableMotor(false);
    }

    i=8;
    while(i>0) {
        i--;
        DestroyHinge(i);
    }

    i=4;
    while(i>0) {
        i--;
        DestroyWeld(i);
    }


    if( OPTloadbestweights ){nocans = 14;}
    else                    {nocans = 42;}

    i=nocans;
    while(i>0) {
        i--;
        if( i != 14 ){  DeleteObject(i); }
    }



    //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 m_dynamicsWorld;
    delete m_solver;
    delete m_broadphase;
    delete m_dispatcher;
    delete m_collisionConfiguration;
}


double RagdollDemo
::Fitness(void)
{
    if( position_sane ) {
        btRigidBody *  spbody   = body[0];
        btTransform btT         = spbody->getWorldTransform();
        btVector3 mbodypos      = btT.getOrigin();
        return( (double)mbodypos.getZ() );
    } else {
        return( -99.88888 );
    }
}



bool RagdollDemo
::SanityCheck(double maxbound)
{
    double xrobo, yrobo, zrobo;
    btRigidBody *  spbody   ;
    btTransform btT         ;
    btVector3 mbodypos      ;
    //.

    spbody      = body[0];
    btT         = spbody->getWorldTransform();
    mbodypos    = btT.getOrigin();
    zrobo       = (double)mbodypos.getZ();
    xrobo       = (double)mbodypos.getX();
    yrobo       = (double)mbodypos.getY();

    if( _isnan( zrobo ) ) {
        cout << "Z coord is not a number." << endl;
        position_sane = false;
    }

    if( _isnan( xrobo ) ) {
        cout << "X coord is not a number." << endl;
        position_sane = false;
    }

    if( _isnan( yrobo ) ) {
        cout << "Y coord is not a number." << endl;
        position_sane = false;
    }

    zrobo = fabs(zrobo);
    xrobo = fabs(xrobo);
    yrobo = fabs(yrobo);

    if( zrobo > maxbound ) position_sane = false;
    if( xrobo > maxbound ) position_sane = false;
    if( yrobo > maxbound ) position_sane = false;

    return position_sane;
}


void RagdollDemo
::WeightFileHandler(void)
{
    int succget;
    long int fpwL;
    char fullpathw[256];
    //.

    strcpy(fullpathw, "C:\\Project_Integration\\");
    strcat(fullpathw, OPTbestweightsfile );
    fpwL = strlen(fullpathw);
    cout << "fullpathw[] has length ";
    cout << fpwL << endl;
    succget = GetFileWeights( fullpathw );
    if( succget == 1 ) {
        cout << "Loaded weights from file ";
    }else {
        cout << "!ERROR. Failed to read file ";
    }
    cout << fullpathw;
    cout << endl;
}



void RagdollDemo
::BuildWallofCans(void)
{
    int can;
    double Xct, Yct, pang, sweep, R;
    const double raddeg = 0.0174532925199433L;
    //.

    R = 10.0;
    sweep = 60.0 * raddeg;
    pang  =  6.0 * raddeg;

    can=0;
    while(can<10) {
        Xct = -1.0 * R * cos(sweep);
        Yct = R * sin(sweep);
        CreateCylinder(15+can,
              Xct , 0.4901 , Yct ,
              0.4 ,   0.4 , 0.49 ,
              0 , 0. , 0. );

        sweep += pang ;
        can   += 1 ;
    }

    sweep = 63.0 * raddeg;
    pang =   6.0 * raddeg;
    can=0;
    while(can<9) {
        Xct = -1.0 * R * cos(sweep);
        Yct = R * sin(sweep);
        CreateCylinder(25+can,
              Xct , 3.0 * 0.4901 , Yct ,
              0.4 ,   0.4 , 0.49 ,
              0 , 0. , 0. );

        sweep += pang ;
        can   += 1 ;
    }

    sweep = 66.0 * raddeg;
    pang =   6.0 * raddeg;
    can=0;
    while(can<8) {
        Xct = -1.0 * R * cos(sweep);
        Yct = R * sin(sweep);
        CreateCylinder(34+can,
              Xct , 5.0 * 0.4901 , Yct ,
              0.4 ,   0.4 , 0.49 ,
              0 , 0. , 0. );

        sweep += pang ;
        can   += 1 ;
    }
}

void RagdollDemo
::CreateBox(
        int index,
        double x ,      double y ,      double z ,
        double length , double width , double height)
{
    btCollisionShape * btCSptr;
    btDefaultMotionState * myMotionState;
    btTransform offset;
    btTransform transform;
    btTransform startTransform;
    btVector3 localInertia(0,0,0);
    double mass;
    //.

    mass = 1.000;

    // `offset` is the location of the entire robot in world.
    offset.setIdentity();
    offset.setOrigin(  btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.))    );

    // `transform` is the  local body coordinates of appendages.
    transform.setIdentity();
    transform.setOrigin(
        btVector3(
            btScalar(x) ,
            btScalar(y) ,
            btScalar(z) )
    );

    geom[index] = new btBoxShape(
            btVector3(
                btScalar(length) ,
                btScalar(height) ,
                btScalar(width ) )
        );
    btCSptr = geom[index];

    btCSptr->calculateLocalInertia( btScalar(mass) , localInertia );

    myMotionState = new btDefaultMotionState(offset*transform);

    btRigidBody::btRigidBodyConstructionInfo rbInfo(
        btScalar(mass),myMotionState,btCSptr,localInertia
        );

    body[index] = new btRigidBody(rbInfo);

    m_dynamicsWorld->addRigidBody( body[index] );
}



void RagdollDemo
::CreateCylinder(
        int index,
        double x,       double y,       double z,
        double length,  double width,   double height,
        double xrot ,   double yrot ,   double zrot )
{
    btCollisionShape * btCSptr;
    btDefaultMotionState * myMotionState;
    btTransform offset;
    btTransform transform;
    btTransform startTransform;
    btVector3 localInertia(0,0,0);
    double mass;
    //.

    mass = 1.000;

    // `offset` is the location of the entire robot in world.
    offset.setIdentity();
    offset.setOrigin(  btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.))    );

    // `transform` is the  local body coordinates of appendages.
    transform.setIdentity();
    transform.setOrigin(
        btVector3(
            btScalar(x) ,
            btScalar(y) ,
            btScalar(z) )
    );
    transform.getBasis().setEulerZYX(
        btScalar(zrot) ,
        btScalar(yrot),
        btScalar(xrot) );

    geom[index] = new btCylinderShape(
            btVector3(
                btScalar(length) ,
                btScalar(height) ,
                btScalar(width ) )
        );
    btCSptr = geom[index];

    btCSptr->calculateLocalInertia( btScalar(mass) , localInertia );

    myMotionState = new btDefaultMotionState(offset*transform);

    btRigidBody::btRigidBodyConstructionInfo rbInfo(
        btScalar(mass),myMotionState,btCSptr,localInertia);

    body[index] = new btRigidBody(rbInfo);

    m_dynamicsWorld->addRigidBody( body[index] );
}


void RagdollDemo
::CreateSphere(
        int index,
        double x,       double y,       double z,
        double radius )
{
    btCollisionShape * btCSptr;
    btDefaultMotionState * myMotionState;
    btTransform offset;
    btTransform transform;
    btTransform startTransform;
    btVector3 localInertia(0,0,0);
    double mass;
    //.

    mass = 0.30;

    // `offset` is the location of the entire robot in world.
    offset.setIdentity();
    offset.setOrigin(  btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.))    );

    // `transform` is the  local body coordinates of appendages.
    transform.setIdentity();
    transform.setOrigin(
        btVector3(
            btScalar(x) ,
            btScalar(y) ,
            btScalar(z) )
    );

    geom[index] = new btSphereShape(  btScalar(radius)  );
    btCSptr = geom[index];

    btCSptr->calculateLocalInertia( btScalar(mass) , localInertia );

    myMotionState = new btDefaultMotionState(offset*transform);

    btRigidBody::btRigidBodyConstructionInfo rbInfo(
        btScalar(mass) , myMotionState,btCSptr,localInertia);

    body[index] = new btRigidBody(rbInfo);

    m_dynamicsWorld->addRigidBody( body[index] );
}




void RagdollDemo
::CreateHinge(int index,
                     int body1, int body2,
                     double x, double y, double z,
                     double ax, double ay, double az)
{
    double convr, uplim, lowlim, defangle;
    btVector3 p;
    btVector3 a;
    //.

    p.setX( btScalar(x) );
    p.setY( btScalar(y) );
    p.setZ( btScalar(z) );

    a.setX( btScalar(ax) );
    a.setY( btScalar(ay) );
    a.setZ( btScalar(az) );

    btVector3 p1 = PointWorldToLocal(body1, p);
    btVector3 p2 = PointWorldToLocal(body2, p);

    btVector3 a1 = AxisWorldToLocal(body1, a);
    btVector3 a2 = AxisWorldToLocal(body2, a);

    joints[index] =
        new btHingeConstraint(*body[body1], *body[body2],
                               p1, p2,
                               a1, a2, false);

    switch(index) {

    // Knees
    case 0:   // East
        lowlim = -60.0;   // Swing out   ( Actuator -45 deg)
        uplim  =  25.0;    // Pull in    ( Actuator +45 deg)
        defangle = 270.0;
        break;
    case 1:   // West
        lowlim =  -25.0;   // pull in    ( Actuator +45 deg)
        uplim  =  60.0;    // Swing out   ( Actuator -45 deg)
        defangle = 270.0;
        break;
    case 2:   // South
        lowlim = -60.0;    // swing out   ( Actuator -45 deg)
        uplim  =  25.0;     // pull in   ( Actuator +45 deg)
        defangle = 90.0;
        break;
    case 3:   // North
        lowlim = -25.0;    // pull in   ( Actuator +45 deg)
        uplim  =  60.0;   // swing out    ( Actuator -45 deg)
        defangle = 90.0;
        break;

    // Body joints
    case 4:    // East
        lowlim =     -35.0;  // Leg above body    ( Actuator -45 deg)
        uplim  =      45.0;  // Leg goes below  ( Actuator +45 deg)
        defangle = 90.0;
        break;
    case 5:  // West  *@*
        lowlim =  -45.0;   // Leg goes below body  ( Actuator +45 deg)
        uplim  =   35.0;   // Leg goes above body   ( Actuator -45 deg)
        defangle = 90.0;
        break;
    case 6:   // North
        lowlim =   -35.0;  // Leg above body          ( Actuator -45 deg)
        uplim  =    45.0;   // Leg goes below      ( Actuator +45 deg)
        defangle = 90.0;
        break;
    case 7:   // South *@*
        lowlim =  -45.0;   // Leg goes below body    ( Actuator +45 deg)
        uplim  =   35.0;   // Let goes up above body   ( Actuator -45 deg)
        defangle = 90.0;
        break;
    }

    convr  = (double)0.0174532925199433;

    (joints[index])->setLimit(
        btScalar((lowlim+defangle)*convr ) ,
        btScalar((uplim +defangle)*convr )
        );

    m_dynamicsWorld->addConstraint(joints[index]);
}



void RagdollDemo
::CreateWeld(int index,
             int body1, int body2 )
{
    btRigidBody * rbA;
    btRigidBody * rbB;
    btTransform trA,trB;
    btTransform globalFrame;
    //.


    rbA = body[body1];
    rbB = body[body2];
    trA.setIdentity();
    trB.setIdentity();
    globalFrame.setIdentity();
    globalFrame.setOrigin(btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.)));

    trA = (  ((rbA)->getWorldTransform()).inverse() ) *  (globalFrame);
    trB = (  ((rbB)->getWorldTransform()).inverse() ) *  (globalFrame);

    welds[index] = new btFixedConstraint( *rbA , *rbB , trA , trB );

    (welds[index])->setOverrideNumSolverIterations(100);

    bool disableCollisionsBetweenLinkedBodies=true;

    m_dynamicsWorld->addConstraint(welds[index], disableCollisionsBetweenLinkedBodies );
}





void RagdollDemo
::DeleteObject( int index )
{
    m_dynamicsWorld->removeRigidBody( body[index] );
    delete body[index];
    delete geom[index];
}


void RagdollDemo
::DestroyHinge( int index )
{
    m_dynamicsWorld->removeConstraint( joints[index] );
    delete joints[index];
}

void RagdollDemo
::DestroyWeld( int index )
{
    m_dynamicsWorld->removeConstraint( welds[index] );
    delete welds[index];
}




btVector3 RagdollDemo
::PointWorldToLocal(int index, btVector3 &p)
{
  btVector3 btvloc;
  btTransform local1  = body[index]->getCenterOfMassTransform().inverse();
  //.
  btvloc = (local1)*(p);
  return (btvloc);
}

btVector3 RagdollDemo
::AxisWorldToLocal(int index, btVector3 &a) {
  btTransform local1 = body[index]->getCenterOfMassTransform().inverse();
  btVector3 zero(0,0,0);
  local1.setOrigin(zero);
  return (
      local1 * a
    );
}


double RagdollDemo
::RandomJoint(void)
{
    unsigned lu, hu;
    unsigned long int HI, LO, J;
    double rJ, rD;
    //.

    lu = rand();
    hu = rand();
    HI = ((unsigned long int)hu) &(0x00006FFF);
    LO = ((unsigned long int)lu) &(0x0000FFFF);
    J  = (HI<<16) | (LO);
    rJ = (double)J;
    rD = (double)(0x6FFFFFFF);
    return( (90.00L*(rJ/rD)) - (45.00L) );
}


double RagdollDemo
::RandomWeight(void)
{
    unsigned lu, hu;
    unsigned long int HI, LO, J;
    double rJ, rD;
    //.

    lu = rand();
    hu = rand();
    HI = ((unsigned long int)hu) &(0x00006FFF);
    LO = ((unsigned long int)lu) &(0x0000FFFF);
    J  = (HI<<16) | (LO);
    rJ = (double)J;
    rD = (double)(0x6FFFFFFF);
    return( (2.00L*(rJ/rD)) - (1.00L) );
}



// * //
// ActuateJoint
void RagdollDemo
::ActuateJoint(
    int jointIndex,
    double desiredAngle,
    double jointOffset,
    double timeStep)
{
    double udA;
    btHingeConstraint * lHC;
    //.

    lHC = joints[jointIndex];
    udA = ConvertActuatorToJoint( jointIndex , desiredAngle );
    udA *= (double)0.0174532925199433L;

    lHC->setMaxMotorImpulse( 0.01 );
    lHC->setMotorTarget( btScalar(udA) , btScalar(timeStep) );
}


// * //
// * // ActuateJoint2  II
void RagdollDemo
::ActuateJoint2(
    int jointIndex,
    double desiredAngle,
    double jointOffset,
    double timeStep)
{
    double udA, cjA, diff, mmi;
    btHingeConstraint * lHC;
    //.

    lHC = joints[jointIndex];
    cjA = lHC->getHingeAngle();
    udA = ConvertActuatorToJoint( jointIndex , desiredAngle );
    udA *= (double)0.0174532925199433L;
    diff = udA - cjA;
    mmi = 1.45;
    lHC->setMaxMotorImpulse( btScalar(mmi) );
    lHC->enableAngularMotor(true, btScalar(5.0*diff), btScalar(1.0) );
    lHC->setMaxMotorImpulse( btScalar(mmi) );
}



double RagdollDemo
::ConvertActuatorToJoint( int index , double actang )
{
    double usang, ja, station, retang;

    usang = actang / 45.0;

    switch(index) {
        // Knees
        case 0:   // East
            if( usang < 0.0 )   { ja = -60.0; }
            else                { ja =  25.0; }
            station = -90.0;
            break;
        case 1:   // West
            if( usang < 0.0 )   { ja =  60.0; }
            else                { ja = -25.0; }
            station = -90.0;
            break;
        case 2:   // South
            if( usang < 0.0 )   { ja = -60.0; }
            else                { ja =  25.0; }
            station = 90.0;
            break;
        case 3:   // North
            if( usang < 0.0 )   { ja =  60.0; }
            else                { ja = -25.0; }
            station = 90.0;
            break;

        // Body joints
        case 4:    // East
            if( usang < 0.0 )   { ja = -35.0; }
            else                { ja =  45.0; }
            station = 90.0;
            break;

        case 5:  // West  *@*

            if( usang < 0.0 )   { ja =  35.0; }
            else                { ja = -45.0; }
            station = 90.0;
            break;

        case 6:   // North

            if( usang < 0.0 )   { ja =  -35.0; }
            else                { ja =  45.0; }
            station = 90.0;
            break;


        case 7:   // South *@*
            if( usang < 0.0 )   { ja =  35.0; }
            else                { ja = -45.0; }
            station = 90.0;
            break;
    }

    retang = station +  ( (ja) * (fabs(usang)) );

    return (retang);
}






void RagdollDemo::displayCallback()
{
    if(OPTdrawgraphics) {
        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 'p':
        {
        pause = !pause;
        break;
        }
    case 'o':
        {
        oneStep = true;
        break;
        }
    case '1':
        {
        globaljointA = 45.0;
        break;
        }
    case '2':
        {
        globaljointA = 0.001;
        break;
        }
    case '3':
        {
        globaljointA = -45.0;
        break;
        }
    case 'n':
        {
            int sen;
            int mot;
            mot=0;
            while(mot<8) {
                sen=0;
                while(sen<4) {
                    weights[sen][mot] = RandomWeight();
                    sen++;
                }
                mot++;
            }
        }
        break;
    default:
        DemoApplication::keyboardCallback(key, x, y);
    }


}




int RagdollDemo
::GetFileWeights(char * GFWname )
{
    int wread;
    int sen, mot;
    double Wij;
    char Wijtxt[80];
    string line;
    ifstream myfile;
    //.

    myfile.open (GFWname, ios::in);
    if (myfile.is_open())
    {
        sen=0;
        mot=0;
        wread = 0;
        while ( getline (myfile,line) )
        {
            if( wread < 32 ) {
                strcpy(Wijtxt, (char *)(line.c_str()) );
                sscanf(Wijtxt, "%Lf,", &Wij );
                weights[sen][mot] = Wij;
                sen++;
                if( sen >= 4 ) {
                    sen = 0;
                    mot++;
                }
            }
            wread++;
        }
        myfile.close();
        if( wread > 31 ) {
            return 1;
        } else {
            cout << "Failed to read all 32 weights from disk.";
            pause= true;
            return -3;
        }
    }else {
        cout << "Unable to open ";
        cout << GFWname;
        cout << endl;
        pause = true;
        return -1;
    }

    return 1;
}


int RagdollDemo
::SavePosition( btRigidBody * spbody, char * SPname )
{
    int k;
    double zd;
    char positxt[80];
    ofstream fitfile;
    btTransform btT;
    btVector3 mbodypos;
    //.

    btT = spbody->getWorldTransform();
    mbodypos = btT.getOrigin();
    zd = (double)(mbodypos.getZ());
    sprintf(positxt, "%.11Lf\r\n", zd );
    /*
    cout << "z = ";
    ConsoleFloat(zd);
    k = ConsoleWait();  */

    fitfile.open(SPname, ios::out );
    if (fitfile.is_open()) {
        k=0;
        while( k < 12 ) {
            fitfile << positxt;
            k++;
        }
        fitfile.close();
        return 1;
    } else {
        cout << "Unable to open output fitness file." << endl;
        k = ConsoleWait();
        return -1;
    }
    return 1;
}



void RagdollDemo
::ConsoleFloat(double cf)
{
    std::stringstream os;
    std::string cjstr;
    //.
    os.flush();
    os.precision(7);
    os << std::fixed << cf;
    cjstr = os.str();
    std::cout << cjstr;
    std::cout << std::endl;
}


void RagdollDemo
::ConsoleTouches(void)
{
    int t;
    std::stringstream os;
    std::string cjstr;
    //.
    os.flush();
    t=0;
    while(t<9) {
        if( t == 5 ) {
            os << " ";
        }
        os << touches[t];
        t++;
    }
    cjstr = os.str();
    cjstr.append("\r\n");
    std::cout << cjstr;
}


void ConsoleCollideIDs(int Acid, int Bcid)
{
    std::stringstream os;
    std::string cjstr;
    //.
    os.flush();
    os << "ID1 = " << Acid << " , ID2 = " << Bcid << "\r\n";
    cjstr = os.str();
    std::cout << cjstr;
}


int ConsoleWait(void)
{
    int ink, lastgc;

    lastgc = ' ';
    while((ink = getchar()) != EOF) {
        if (ink == '\n') {
            break;
        } else {
            lastgc = ink;
        }
    }

    return lastgc;
}