API tools faq
paste
Advertisement
Guest User

RagdollDemo.cpp

a guest
Dec 27th, 2014
271
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 26.14 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. RagdollDemo.cpp
  3.  
  4. */
  5.  
  6. #include <sstream>
  7. #include <iostream>
  8. #include <string>
  9. #include <math.h>
  10. #include <time.h>
  11. #include <fstream>
  12. #include <cstdio>
  13.  
  14.  
  15. #define CONSTRAINT_DEBUG_SIZE 0.2f
  16. #ifndef M_PI
  17. #define M_PI       3.14159265358979323846
  18. #endif
  19.  
  20. #ifndef M_PI_2
  21. #define M_PI_2     1.57079632679489661923
  22. #endif
  23.  
  24. #ifndef M_PI_4
  25. #define M_PI_4     0.785398163397448309616
  26. #endif
  27.  
  28. #include "btBulletDynamicsCommon.h"
  29. #include "GlutStuff.h"
  30. #include "GL_ShapeDrawer.h"
  31. #include "LinearMath/btIDebugDraw.h"
  32. #include "GLDebugDrawer.h"
  33. #include "RagdollDemo.h"
  34. #include "RagDollclass.h"
  35.  
  36. using namespace std;
  37.  
  38.  
  39. void ConsoleCollideIDs(int Acid, int Bcid);
  40.  
  41. static RagdollDemo * ragdollDemo;
  42. static RagdollDemo * ragdollptrs[12];
  43.  
  44. #include "ContactCallbacks.h" // ..
  45.  
  46. bool myContactProcessedCallback(btManifoldPoint& cp,
  47.                                 void* body0, void* body1)
  48. {
  49.     int groundID = 9;
  50.     int conID1,conID2;
  51.     int * ID1;
  52.     int * ID2; 
  53.  
  54.     btCollisionObject * o1 = static_cast<btCollisionObject*>(body0);
  55.     btCollisionObject * o2 = static_cast<btCollisionObject*>(body1);
  56.    
  57.     ID1 = static_cast<int*>(o1->getUserPointer());
  58.     ID2 = static_cast<int*>(o2->getUserPointer());
  59.    
  60.     conID1 = (*ID1);   
  61.     conID2 = (*ID2);
  62.    
  63.     if((conID1<10)&&(conID2<10) ){
  64.         if( (conID1 == groundID) || (conID2 == groundID ) ){                       
  65.             ragdollDemo->touches[conID1]=1;
  66.             ragdollDemo->touches[conID2]=1;
  67.             ragdollDemo->touchPoints[conID1] = cp.m_positionWorldOnB;
  68.             ragdollDemo->touchPoints[conID2] = cp.m_positionWorldOnB;
  69.         }
  70.     }
  71.     return false;
  72. }
  73.  
  74.  
  75.  
  76. void RagdollDemo::initPhysics()
  77. {  
  78.     int i,  nocans;
  79.     //.
  80.  
  81.     srand( (unsigned)(time(NULL)) );
  82.     pause = true;
  83.     oneStep = false;
  84.     position_sane = true;
  85.     globaljointA = 0.001;  
  86.     simtick = 0;
  87.     timeStep = 0;
  88.     i=0;
  89.     while( i<45 ) {
  90.         IDs[i] = i;
  91.         i++;
  92.     }
  93.  
  94.     // Setup the basic world
  95.     setTexturing(true);
  96.     setShadows(true);
  97.     setCameraDistance(btScalar(5.));
  98.  
  99.     m_collisionConfiguration = new btDefaultCollisionConfiguration();
  100.     m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
  101.  
  102.     btVector3 worldAabbMin(-10000,-10000,-10000);
  103.     btVector3 worldAabbMax(10000,10000,10000);
  104.     m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax);
  105.  
  106.     m_solver = new btSequentialImpulseConstraintSolver;
  107.     m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
  108.    
  109.     // Setup a big ground box
  110.     {
  111.         btCollisionShape * groundShape = new btBoxShape(btVector3(btScalar(300.),btScalar(10.),btScalar(300.)));
  112.         m_collisionShapes.push_back(groundShape);
  113.         btTransform groundTransform;
  114.         groundTransform.setIdentity();
  115.         groundTransform.setOrigin(btVector3(0,-10,0));
  116.  
  117.         fixedGround = new btCollisionObject();
  118.         fixedGround->setCollisionShape(groundShape);
  119.         fixedGround->setWorldTransform(groundTransform);
  120.         m_dynamicsWorld->addCollisionObject(fixedGround);      
  121.     }  
  122.    
  123.     //cout << "CreateBox(0...)" << endl;
  124.     CreateBox(0,
  125.               0., 2., 0.,
  126.               1., 1., 0.2 ); // Create the robot body
  127.  
  128.     CreateCylinder(1,
  129.               2.02 , 2. , 0.0 ,
  130.               0.2 , 0.2 , 0.8 ,
  131.               M_PI_2 , 0. , 0. ); // West thigh
  132.  
  133.     CreateCylinder(2,
  134.               -2.02 , 2. , 0.0 ,
  135.               0.2 , 0.2 , 0.8 ,
  136.               M_PI_2 , 0. , 0. ); // East thigh
  137.  
  138.     CreateCylinder(3,
  139.               0. , 2. , -2.02 ,
  140.               0.2 , 0.2 , 0.8 ,
  141.               0 , 0. , M_PI_2 ); // South thigh
  142.  
  143.     CreateCylinder(4,
  144.               0. , 2. , 2.02  ,
  145.               0.2 , 0.2 , 0.8 ,
  146.               0 , 0. , M_PI_2 ); // North thigh
  147.  
  148.     CreateCylinder(5,
  149.               3.10 , 0.89 , 0.0 ,
  150.               0.2 ,  0.2 , 0.89 ,
  151.               0 , 0. , 0. ); // West foot
  152.  
  153.     CreateCylinder(6,
  154.               -3.10 , 0.89 , 0.0 ,
  155.               0.2 ,   0.2 , 0.89 ,
  156.               0 , 0. , 0. ); // East foot
  157.  
  158.     CreateCylinder(7,
  159.               0.00 , 0.89 , 3.10 ,
  160.               0.2 ,  0.2 , 0.89 ,
  161.               0 , 0. , 0. ); // North foot
  162.  
  163.     CreateCylinder(8,
  164.               0.00 , 0.89 , -3.10 ,
  165.               0.2 ,   0.2 , 0.89 ,
  166.               0 , 0. , 0. ); // South foot
  167.  
  168.  
  169.     if( !OPTloadbestweights ) {
  170.         //cout << "BuildWallofCans()" << endl;
  171.         BuildWallofCans();   // wall of cans
  172.     }
  173.  
  174.     CreateBox(9,
  175.               0., 2.01, 105.,
  176.               4., 2., 2. ); // Create a north marker
  177.    
  178.     CreateSphere(10,
  179.                 -3.00 , 2. , 0.0 ,
  180.                 0.27 );    // East kneeball
  181.  
  182.     CreateSphere(11,
  183.                 3.00 , 2. , 0.0 ,
  184.                 0.27 );    // West kneeball
  185.  
  186.     CreateSphere(12,
  187.                 0.0 , 2. , 3.00 ,
  188.                 0.27 );    // North kneeball
  189.  
  190.     CreateSphere(13,
  191.                 0.0 , 2. , -3.00 ,
  192.                 0.27 );    // South kneeball
  193.        
  194.    
  195.  
  196.     CreateWeld(0, 2, 10 ); // Weld east kneeball(10) to east thigh(2)
  197.     CreateWeld(1, 1, 11 ); // Weld west kneeball(11) to west thigh(1)
  198.     CreateWeld(2, 4, 12 ); // Weld north kneeball(12) to north thigh(4)
  199.     CreateWeld(3, 3, 13 ); // Weld south kneeball(13) to south thigh(3)
  200.        
  201.  
  202.     if( OPTloadbestweights ) {
  203.         nocans = 15;
  204.     } else {
  205.         nocans = 42;
  206.     }
  207.  
  208.     i=0;
  209.     while( i < nocans ) {
  210.         if(i==9) {
  211.             fixedGround->setUserPointer( &(IDs[i]) );
  212.             (body[i])->setUserPointer(  &(IDs[14]) );
  213.         } else {
  214.             if( i < 14 ) {
  215.                 (body[i])->setUserPointer(  &(IDs[i] ));
  216.             }
  217.  
  218.             if( i > 14 ) {
  219.                 (body[i])->setUserPointer(  &(IDs[i] ));
  220.             }
  221.         }
  222.         i++;
  223.     }
  224.  
  225.     //cout << "CreateHinge(0.." << endl;
  226.  
  227.     // JOINT = 0
  228.     // East thigh(2) connects East foot(6)
  229.     CreateHinge(0 , 2 , 6 ,
  230.         -3.10 , 2.00 , 0.0,
  231.          0.00 , 0.00 , -1.00 );
  232.      
  233.     // JOINT = 1
  234.     // West thigh(1) connects West foot(5)
  235.     CreateHinge(1 , 1 , 5,
  236.          3.10 , 2.00 , 0.0,
  237.          0.00 , 0.00 , -1.00 );
  238.  
  239.     // JOINT = 2
  240.     // South thigh(3) connects South foot(8)
  241.     CreateHinge(2 , 8 , 3,
  242.           0.00 ,  2.00 , -3.10,
  243.          -1.00 , 0.00 , 0.00 );
  244.  
  245.     // JOINT = 3
  246.     // North thigh(4) connects North foot(7)
  247.     CreateHinge(3 , 7 , 4,
  248.           0.00 ,  2.00 , 3.10,
  249.          -1.00 , 0.00 ,  0.00 );
  250.    
  251.    
  252.     // JOINT = 4
  253.     // body connects East thigh(2)
  254.     CreateHinge(4 , 0 , 2,
  255.           -1.02 , 2.00 , 0.00,
  256.            0.00 , 0.00 , -1.00 );
  257.  
  258.     // JOINT = 5
  259.     // body connects West thigh(1)
  260.     CreateHinge(5 , 0 , 1,
  261.            1.02 , 2.00 , 0.00,
  262.            0.00 , 0.00 , -1.00 );
  263.  
  264.     // JOINT = 6
  265.     // body connects North thigh(4)
  266.     CreateHinge(6 , 0 , 4,
  267.             0.00 , 2.00 , 1.02,
  268.            -1.00 , 0.00 , 0.00 );
  269.  
  270.     // JOINT = 7
  271.     // body connects South thigh(3)
  272.     CreateHinge(7 , 0 , 3,
  273.             0.00 , 2.00 , -1.02,
  274.            -1.00 , 0.00 , 0.00 );
  275.     i=0;
  276.     while(i<8){
  277.         (joints[i])->enableMotor(true);
  278.         i++;
  279.     }  
  280.  
  281.     if( threadID < 0 ) {
  282.         //cout << "gContactProcessedCallback" << endl;
  283.         ragdollDemo = this;
  284.         gContactProcessedCallback = myContactProcessedCallback;
  285.     } else {
  286.         ragdollptrs[threadID] = this;      
  287.         AssignToCallback();
  288.     }
  289.  
  290.     //cout << "clientResetScene()" << endl;
  291.     pause = false;
  292.     clientResetScene();    
  293. }
  294.  
  295.  
  296. // * //
  297.  
  298. void RagdollDemo::clientMoveAndDisplay()
  299. {
  300.     long int rollover_escape;
  301.     int nn, m;
  302.     bool sS;   
  303.     bool air;
  304.     double deltaT;
  305.     double motorCommand;   
  306.     //.
  307.  
  308.     if( OPTdrawgraphics ) {
  309.         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  310.     }
  311.  
  312.     //simple dynamics world doesn't handle fixed-time-stepping
  313.     float ms = getDeltaTimeMicroseconds();
  314.     float minFPS = 90000.f/60.f;
  315.     if (ms > minFPS) ms = minFPS;
  316.     deltaT = ms / 90000.f;
  317.  
  318.     if (m_dynamicsWorld)
  319.     {
  320.         sS = false;
  321.         if(!pause){
  322.             sS=true;
  323.         }
  324.         if( pause && oneStep ) {
  325.             oneStep = false;
  326.             sS = true;         
  327.         }
  328.  
  329.         if(sS) {           
  330.             m=0;
  331.             while( m<15 ) {
  332.                 touches[m] = 0;
  333.                 m++;
  334.             }  
  335.  
  336.             air = airborne();
  337.            
  338.             if( !air ) {
  339.                 m_dynamicsWorld->stepSimulation( btScalar(deltaT) );
  340.             } else {
  341.                 rollover_escape = 1;
  342.                 while ( air && (rollover_escape < 1510) )
  343.                 {                  
  344.                         m_dynamicsWorld->stepSimulation( btScalar(deltaT) );
  345.  
  346.                         if( !SanityCheck( 500.0 ) ) {
  347.                             // The physics simulation is crashing?
  348.                             rollover_escape = 9999;  
  349.                         }
  350.  
  351.                         air = airborne();
  352.                         rollover_escape++;
  353.                 }
  354.  
  355.                 if( rollover_escape > 1505 ) {
  356.                     // The robot is upside down, or it fell off the earth.                     
  357.                     if( OPTdrawgraphics ) {
  358.                         pause = true;  // Trial runs pause.                          
  359.                     } else {
  360.                         timeStep = 987123123;  // Fitness tests should terminate.
  361.                     }
  362.                 }
  363.             }
  364.            
  365.  
  366.             if( simtick == 0 ) {
  367.                 m=0;
  368.                 while(m<8) {               
  369.                     nn=0;
  370.                     while( nn < 4 ) {
  371.                         // We need the touches from 5,6,7, and 8
  372.                         touchsensor[nn] = (double)touches[nn+5];
  373.                         nn++;
  374.                     }
  375.  
  376.                     motorCommand = 0.0;
  377.                     nn=0;
  378.                     while( nn < 4 ) {
  379.                         motorCommand = motorCommand +
  380.                                        (touchsensor[nn] * weights[nn][m]);
  381.                         nn++;
  382.                     }
  383.  
  384.                     motorCommand = tanh(motorCommand);
  385.                     motorCommand = motorCommand * 45.0;
  386.                     ActuateJoint2(m , motorCommand , -1.  ,  deltaT );
  387.                     m++;
  388.                 }          
  389.             }
  390.  
  391.             simtick = (simtick+1)%23;
  392.         }
  393.  
  394.         if(OPTdrawgraphics) {
  395.             //optional but useful: debug drawing
  396.             m_dynamicsWorld->debugDrawWorld();
  397.         }
  398.  
  399.         timeStep++;
  400.     }
  401.  
  402.     if(OPTdrawgraphics) {
  403.         renderme();
  404.         glFlush();
  405.         glutSwapBuffers();
  406.     }
  407. }
  408.  
  409.  
  410. // * //
  411.  
  412. void    RagdollDemo::exitPhysics()
  413. {
  414.     int i, nocans;
  415.  
  416.     if( OPTloadbestweights ){nocans = 14;}
  417.     else                    {nocans = 42;}
  418.      
  419.     i=0;
  420.     while(i<nocans) {
  421.         if( i != 14 ){  DeleteObject(i); }
  422.         i++;
  423.     }
  424.      
  425.     i=0;
  426.     while(i<8) {
  427.         DestroyHinge(i);
  428.         i++;
  429.     }  
  430.      
  431.     i=0;
  432.     while(i<4) {
  433.         DestroyWeld(i);
  434.         i++;
  435.     }      
  436.      
  437.     //cleanup in the reverse order of creation/initialization
  438.     //remove the rigidbodies from the dynamics world and delete them
  439.    
  440.     for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
  441.     {
  442.         btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
  443.         btRigidBody* body = btRigidBody::upcast(obj);
  444.         if (body && body->getMotionState())
  445.         {
  446.             delete body->getMotionState();
  447.         }
  448.         m_dynamicsWorld->removeCollisionObject( obj );
  449.         delete obj;
  450.     }
  451.  
  452.      
  453.     //delete collision shapes
  454.     for (int j=0;j<m_collisionShapes.size();j++)
  455.     {
  456.         btCollisionShape* shape = m_collisionShapes[j];
  457.         delete shape;
  458.     }
  459.    
  460.     delete m_dynamicsWorld;
  461.     delete m_solver;
  462.     delete m_broadphase;
  463.     delete m_dispatcher;
  464.     delete m_collisionConfiguration;   
  465. }
  466.  
  467.  
  468.  
  469. void RagdollDemo
  470. ::WeightFileHandler(void)
  471. {
  472.     int succget;
  473.     long int fpwL;
  474.     char fullpathw[256];
  475.     //.
  476.  
  477.     strcpy(fullpathw, "C:\\Project_Integration\\");
  478.     strcat(fullpathw, OPTbestweightsfile );
  479.     fpwL = strlen(fullpathw);
  480.     cout << "fullpathw[] has length ";
  481.     cout << fpwL << endl;  
  482.     succget = GetFileWeights( fullpathw );
  483.     if( succget == 1 ) {       
  484.         cout << "Loaded weights from file ";                   
  485.     }else {
  486.         cout << "!ERROR. Failed to read file ";
  487.     }
  488.     cout << fullpathw;
  489.     cout << endl;      
  490. }
  491.  
  492.  
  493.  
  494. void RagdollDemo
  495. ::BuildWallofCans(void)
  496. {
  497.     int can;
  498.     double Xct, Yct, pang, sweep, R;
  499.     const double raddeg = 0.0174532925199433L;
  500.     //.
  501.  
  502.     R = 10.0;
  503.     sweep = 60.0 * raddeg;
  504.     pang  =  6.0 * raddeg;
  505.    
  506.     can=0;
  507.     while(can<10) {
  508.         Xct = -1.0 * R * cos(sweep);
  509.         Yct = R * sin(sweep);
  510.         CreateCylinder(15+can,
  511.               Xct , 0.4901 , Yct ,
  512.               0.4 ,   0.4 , 0.49 ,
  513.               0 , 0. , 0. );
  514.  
  515.         sweep += pang ;
  516.         can   += 1 ;
  517.     }
  518.  
  519.     sweep = 63.0 * raddeg;
  520.     pang =   6.0 * raddeg; 
  521.     can=0;
  522.     while(can<9) {
  523.         Xct = -1.0 * R * cos(sweep);
  524.         Yct = R * sin(sweep);
  525.         CreateCylinder(25+can,
  526.               Xct , 3.0 * 0.4901 , Yct ,
  527.               0.4 ,   0.4 , 0.49 ,
  528.               0 , 0. , 0. );
  529.  
  530.         sweep += pang ;
  531.         can   += 1 ;
  532.     }
  533.  
  534.     sweep = 66.0 * raddeg;
  535.     pang =   6.0 * raddeg; 
  536.     can=0;
  537.     while(can<8) {
  538.         Xct = -1.0 * R * cos(sweep);
  539.         Yct = R * sin(sweep);
  540.         CreateCylinder(34+can,
  541.               Xct , 5.0 * 0.4901 , Yct ,
  542.               0.4 ,   0.4 , 0.49 ,
  543.               0 , 0. , 0. );
  544.  
  545.         sweep += pang ;
  546.         can   += 1 ;
  547.     }
  548. }
  549.  
  550. void RagdollDemo
  551. ::CreateBox(
  552.         int index,
  553.         double x ,      double y ,      double z ,
  554.         double length , double width , double height)
  555. {
  556.     btCollisionShape * btCSptr;
  557.     btDefaultMotionState * myMotionState;
  558.     btTransform offset;
  559.     btTransform transform;
  560.     btTransform startTransform;
  561.     btVector3 localInertia(0,0,0);
  562.     double mass;
  563.     //.
  564.    
  565.     mass = 1.000;
  566.  
  567.     // `offset` is the location of the entire robot in world.
  568.     offset.setIdentity();
  569.     offset.setOrigin(  btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.))    );
  570.  
  571.     // `transform` is the  local body coordinates of appendages.  
  572.     transform.setIdentity();
  573.     transform.setOrigin(
  574.         btVector3(
  575.             btScalar(x) ,
  576.             btScalar(y) ,
  577.             btScalar(z) )
  578.     );
  579.  
  580.     geom[index] = new btBoxShape(  
  581.             btVector3(
  582.                 btScalar(length) ,
  583.                 btScalar(height) ,
  584.                 btScalar(width ) )
  585.         );
  586.     btCSptr = geom[index];
  587.  
  588.     btCSptr->calculateLocalInertia( btScalar(mass) , localInertia );  
  589.  
  590.     myMotionState = new btDefaultMotionState(offset*transform);
  591.    
  592.     btRigidBody::btRigidBodyConstructionInfo rbInfo(
  593.         btScalar(mass),myMotionState,btCSptr,localInertia
  594.         );
  595.        
  596.     body[index] = new btRigidBody(rbInfo); 
  597.  
  598.     m_dynamicsWorld->addRigidBody( body[index] );
  599. }
  600.  
  601.  
  602.  
  603. void RagdollDemo
  604. ::CreateCylinder(
  605.         int index,
  606.         double x,       double y,       double z,
  607.         double length,  double width,   double height,
  608.         double xrot ,   double yrot ,   double zrot )
  609. {
  610.     btCollisionShape * btCSptr;
  611.     btDefaultMotionState * myMotionState;
  612.     btTransform offset;
  613.     btTransform transform;
  614.     btTransform startTransform;
  615.     btVector3 localInertia(0,0,0);
  616.     double mass;
  617.     //.
  618.    
  619.     mass = 1.000;
  620.  
  621.     // `offset` is the location of the entire robot in world.
  622.     offset.setIdentity();
  623.     offset.setOrigin(  btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.))    );
  624.  
  625.     // `transform` is the  local body coordinates of appendages.  
  626.     transform.setIdentity();
  627.     transform.setOrigin(
  628.         btVector3(
  629.             btScalar(x) ,
  630.             btScalar(y) ,
  631.             btScalar(z) )
  632.     );
  633.     transform.getBasis().setEulerZYX(
  634.         btScalar(zrot) ,
  635.         btScalar(yrot),
  636.         btScalar(xrot) );
  637.  
  638.     geom[index] = new btCylinderShape(  
  639.             btVector3(
  640.                 btScalar(length) ,
  641.                 btScalar(height) ,
  642.                 btScalar(width ) )
  643.         );
  644.     btCSptr = geom[index];
  645.  
  646.     btCSptr->calculateLocalInertia( btScalar(mass) , localInertia );  
  647.  
  648.     myMotionState = new btDefaultMotionState(offset*transform);
  649.    
  650.     btRigidBody::btRigidBodyConstructionInfo rbInfo(
  651.         btScalar(mass),myMotionState,btCSptr,localInertia);
  652.        
  653.     body[index] = new btRigidBody(rbInfo);
  654.  
  655.     m_dynamicsWorld->addRigidBody( body[index] );
  656. }
  657.  
  658.  
  659. void RagdollDemo
  660. ::CreateSphere(
  661.         int index,
  662.         double x,       double y,       double z,
  663.         double radius )
  664. {
  665.     btCollisionShape * btCSptr;
  666.     btDefaultMotionState * myMotionState;
  667.     btTransform offset;
  668.     btTransform transform;
  669.     btTransform startTransform;
  670.     btVector3 localInertia(0,0,0);
  671.     double mass;
  672.     //.
  673.    
  674.     mass = 0.30;
  675.  
  676.     // `offset` is the location of the entire robot in world.
  677.     offset.setIdentity();
  678.     offset.setOrigin(  btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.))    );
  679.  
  680.     // `transform` is the  local body coordinates of appendages.  
  681.     transform.setIdentity();
  682.     transform.setOrigin(
  683.         btVector3(
  684.             btScalar(x) ,
  685.             btScalar(y) ,
  686.             btScalar(z) )
  687.     );
  688.  
  689.     geom[index] = new btSphereShape(  btScalar(radius)  );
  690.     btCSptr = geom[index];
  691.  
  692.     btCSptr->calculateLocalInertia( btScalar(mass) , localInertia );  
  693.  
  694.     myMotionState = new btDefaultMotionState(offset*transform);
  695.    
  696.     btRigidBody::btRigidBodyConstructionInfo rbInfo(
  697.         btScalar(mass) , myMotionState,btCSptr,localInertia);
  698.        
  699.     body[index] = new btRigidBody(rbInfo);
  700.  
  701.     m_dynamicsWorld->addRigidBody( body[index] );
  702. }
  703.  
  704.  
  705.  
  706.  
  707. void RagdollDemo
  708. ::CreateHinge(int index,
  709.                      int body1, int body2,
  710.                      double x, double y, double z,
  711.                      double ax, double ay, double az)
  712. {
  713.     double convr, uplim, lowlim, defangle;
  714.     btVector3 p;
  715.     btVector3 a;
  716.     //.
  717.  
  718.     p.setX( btScalar(x) );
  719.     p.setY( btScalar(y) );
  720.     p.setZ( btScalar(z) );
  721.  
  722.     a.setX( btScalar(ax) );
  723.     a.setY( btScalar(ay) );
  724.     a.setZ( btScalar(az) );
  725.  
  726.     btVector3 p1 = PointWorldToLocal(body1, p);
  727.     btVector3 p2 = PointWorldToLocal(body2, p);
  728.  
  729.     btVector3 a1 = AxisWorldToLocal(body1, a);
  730.     btVector3 a2 = AxisWorldToLocal(body2, a);
  731.  
  732.     joints[index] =
  733.         new btHingeConstraint(*body[body1], *body[body2],
  734.                                p1, p2,
  735.                                a1, a2, false);
  736.  
  737.     switch(index) {
  738.  
  739.     // Knees
  740.     case 0:   // East
  741.         lowlim = -60.0;   // Swing out   ( Actuator -45 deg)
  742.         uplim  =  25.0;    // Pull in    ( Actuator +45 deg)
  743.         defangle = 270.0;      
  744.         break;
  745.     case 1:   // West
  746.         lowlim =  -25.0;   // pull in    ( Actuator +45 deg)
  747.         uplim  =  60.0;    // Swing out   ( Actuator -45 deg)
  748.         defangle = 270.0;          
  749.         break;
  750.     case 2:   // South
  751.         lowlim = -60.0;    // swing out   ( Actuator -45 deg)
  752.         uplim  =  25.0;     // pull in   ( Actuator +45 deg)
  753.         defangle = 90.0;               
  754.         break;
  755.     case 3:   // North
  756.         lowlim = -25.0;    // pull in   ( Actuator +45 deg)
  757.         uplim  =  60.0;   // swing out    ( Actuator -45 deg)
  758.         defangle = 90.0;           
  759.         break;
  760.  
  761.     // Body joints
  762.     case 4:    // East
  763.         lowlim =     -35.0;  // Leg above body    ( Actuator -45 deg)
  764.         uplim  =      45.0;  // Leg goes below  ( Actuator +45 deg)
  765.         defangle = 90.0;   
  766.         break;
  767.     case 5:  // West  *@*
  768.         lowlim =  -45.0;   // Leg goes below body  ( Actuator +45 deg)  
  769.         uplim  =   35.0;   // Leg goes above body   ( Actuator -45 deg)
  770.         defangle = 90.0;
  771.         break;
  772.     case 6:   // North
  773.         lowlim =   -35.0;  // Leg above body          ( Actuator -45 deg)
  774.         uplim  =    45.0;   // Leg goes below      ( Actuator +45 deg)  
  775.         defangle = 90.0;
  776.         break;
  777.     case 7:   // South *@*
  778.         lowlim =  -45.0;   // Leg goes below body    ( Actuator +45 deg)
  779.         uplim  =   35.0;   // Let goes up above body   ( Actuator -45 deg)
  780.         defangle = 90.0;
  781.         break;
  782.     }
  783.    
  784.     convr  = (double)0.0174532925199433;
  785.  
  786.     (joints[index])->setLimit(
  787.         btScalar((lowlim+defangle)*convr ) ,
  788.         btScalar((uplim +defangle)*convr )
  789.         );
  790.  
  791.     m_dynamicsWorld->addConstraint(joints[index]);
  792. }
  793.  
  794.  
  795.  
  796. void RagdollDemo
  797. ::CreateWeld(int index,
  798.              int body1, int body2 )
  799. {  
  800.     btRigidBody * rbA;
  801.     btRigidBody * rbB;
  802.     btTransform trA,trB;
  803.     btTransform globalFrame;
  804.     //.
  805.  
  806.  
  807.     rbA = body[body1];
  808.     rbB = body[body2];         
  809.     trA.setIdentity();
  810.     trB.setIdentity();
  811.     globalFrame.setIdentity();
  812.     globalFrame.setOrigin(btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.)));
  813.  
  814.     trA = (  ((rbA)->getWorldTransform()).inverse() ) *  (globalFrame);
  815.     trB = (  ((rbB)->getWorldTransform()).inverse() ) *  (globalFrame);
  816.  
  817.     welds[index] = new btFixedConstraint( *rbA , *rbB , trA , trB );
  818.  
  819.     (welds[index])->setOverrideNumSolverIterations(100);
  820.  
  821.     bool disableCollisionsBetweenLinkedBodies=true;
  822.  
  823.     m_dynamicsWorld->addConstraint(welds[index], disableCollisionsBetweenLinkedBodies );   
  824. }
  825.  
  826.  
  827.  
  828.  
  829.  
  830. void RagdollDemo
  831. ::DeleteObject( int index )
  832. {
  833.     m_dynamicsWorld->removeRigidBody( body[index] );
  834.     delete body[index];
  835.     delete geom[index];
  836. }
  837.  
  838.  
  839. void RagdollDemo
  840. ::DestroyHinge( int index )
  841. {
  842.     m_dynamicsWorld->removeConstraint( joints[index] );
  843.     delete joints[index];
  844. }
  845.  
  846. void RagdollDemo
  847. ::DestroyWeld( int index )
  848. {
  849.     m_dynamicsWorld->removeConstraint( welds[index] );
  850.     delete welds[index];
  851. }
  852.  
  853.  
  854.  
  855.  
  856. btVector3 RagdollDemo
  857. ::PointWorldToLocal(int index, btVector3 &p)
  858. {
  859.   btVector3 btvloc;
  860.   btTransform local1  = body[index]->getCenterOfMassTransform().inverse();  
  861.   //.
  862.   btvloc = (local1)*(p);
  863.   return (btvloc);
  864. }
  865.  
  866. btVector3 RagdollDemo
  867. ::AxisWorldToLocal(int index, btVector3 &a) {
  868.   btTransform local1 = body[index]->getCenterOfMassTransform().inverse();
  869.   btVector3 zero(0,0,0);
  870.   local1.setOrigin(zero);
  871.   return (
  872.       local1 * a
  873.     );
  874. }
  875.  
  876.  
  877. double RagdollDemo
  878. ::RandomJoint(void)
  879. {
  880.     unsigned lu, hu;
  881.     unsigned long int HI, LO, J;
  882.     double rJ, rD;
  883.     //.
  884.    
  885.     lu = rand();
  886.     hu = rand();   
  887.     HI = ((unsigned long int)hu) &(0x00006FFF);  
  888.     LO = ((unsigned long int)lu) &(0x0000FFFF);
  889.     J  = (HI<<16) | (LO);
  890.     rJ = (double)J;
  891.     rD = (double)(0x6FFFFFFF);
  892.     return( (90.00L*(rJ/rD)) - (45.00L) );
  893. }
  894.  
  895.  
  896. double RagdollDemo
  897. ::RandomWeight(void)
  898. {
  899.     unsigned lu, hu;
  900.     unsigned long int HI, LO, J;
  901.     double rJ, rD;
  902.     //.
  903.    
  904.     lu = rand();
  905.     hu = rand();   
  906.     HI = ((unsigned long int)hu) &(0x00006FFF);  
  907.     LO = ((unsigned long int)lu) &(0x0000FFFF);
  908.     J  = (HI<<16) | (LO);
  909.     rJ = (double)J;
  910.     rD = (double)(0x6FFFFFFF);
  911.     return( (2.00L*(rJ/rD)) - (1.00L) );
  912. }
  913.  
  914.  
  915.  
  916. // * //  
  917. // ActuateJoint
  918. void RagdollDemo   
  919. ::ActuateJoint(
  920.     int jointIndex,
  921.     double desiredAngle,
  922.     double jointOffset,
  923.     double timeStep)
  924. {
  925.     double udA;
  926.     btHingeConstraint * lHC;
  927.     //.
  928.  
  929.     lHC = joints[jointIndex];
  930.     udA = ConvertActuatorToJoint( jointIndex , desiredAngle );
  931.     udA *= (double)0.0174532925199433L;
  932.  
  933.     lHC->setMaxMotorImpulse( 0.01 );
  934.     lHC->setMotorTarget( btScalar(udA) , btScalar(timeStep) );
  935. }
  936.  
  937.  
  938. // * //
  939. // * // ActuateJoint2  II
  940. void RagdollDemo   
  941. ::ActuateJoint2(   
  942.     int jointIndex,
  943.     double desiredAngle,
  944.     double jointOffset,
  945.     double timeStep)
  946. {
  947.     double udA, cjA, diff, mmi;
  948.     btHingeConstraint * lHC;
  949.     //.
  950.  
  951.     lHC = joints[jointIndex];
  952.     cjA = lHC->getHingeAngle();
  953.     udA = ConvertActuatorToJoint( jointIndex , desiredAngle );
  954.     udA *= (double)0.0174532925199433L;
  955.     diff = udA - cjA;
  956.     mmi = 1.45;
  957.     lHC->setMaxMotorImpulse( btScalar(mmi) );
  958.     lHC->enableAngularMotor(true, btScalar(5.0*diff), btScalar(1.0) );
  959.     lHC->setMaxMotorImpulse( btScalar(mmi) );
  960. }
  961.  
  962.  
  963.  
  964. double RagdollDemo 
  965. ::ConvertActuatorToJoint( int index , double actang )
  966. {
  967.     double usang, ja, station, retang;
  968.  
  969.     usang = actang / 45.0;
  970.  
  971.     switch(index) {
  972.         // Knees
  973.         case 0:   // East
  974.             if( usang < 0.0 )   { ja = -60.0; }
  975.             else                { ja =  25.0; }
  976.             station = -90.0;   
  977.             break;
  978.         case 1:   // West
  979.             if( usang < 0.0 )   { ja =  60.0; }
  980.             else                { ja = -25.0; }
  981.             station = -90.0;           
  982.             break;
  983.         case 2:   // South
  984.             if( usang < 0.0 )   { ja = -60.0; }
  985.             else                { ja =  25.0; }        
  986.             station = 90.0;        
  987.             break;
  988.         case 3:   // North
  989.             if( usang < 0.0 )   { ja =  60.0; }
  990.             else                { ja = -25.0; }        
  991.             station = 90.0;
  992.             break;
  993.  
  994.         // Body joints
  995.         case 4:    // East
  996.             if( usang < 0.0 )   { ja = -35.0; }
  997.             else                { ja =  45.0; }        
  998.             station = 90.0;
  999.             break;
  1000.            
  1001.         case 5:  // West  *@*
  1002.  
  1003.             if( usang < 0.0 )   { ja =  35.0; }
  1004.             else                { ja = -45.0; }        
  1005.             station = 90.0;
  1006.             break; 
  1007.            
  1008.         case 6:   // North
  1009.  
  1010.             if( usang < 0.0 )   { ja =  -35.0; }
  1011.             else                { ja =  45.0; }        
  1012.             station = 90.0;
  1013.             break;
  1014.  
  1015.            
  1016.         case 7:   // South *@*
  1017.             if( usang < 0.0 )   { ja =  35.0; }
  1018.             else                { ja = -45.0; }        
  1019.             station = 90.0;
  1020.             break;
  1021.     }
  1022.    
  1023.     retang = station +  ( (ja) * (fabs(usang)) );
  1024.  
  1025.     return (retang);
  1026. }
  1027.  
  1028.  
  1029.  
  1030.  
  1031.  
  1032.  
  1033. void RagdollDemo::displayCallback()
  1034. {
  1035.     if(OPTdrawgraphics) {
  1036.         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  1037.  
  1038.         renderme();
  1039.  
  1040.         //optional but useful: debug drawing
  1041.         if (m_dynamicsWorld)
  1042.             m_dynamicsWorld->debugDrawWorld();
  1043.  
  1044.         glFlush();
  1045.         glutSwapBuffers();
  1046.     }
  1047. }
  1048.  
  1049. void RagdollDemo::keyboardCallback(unsigned char key, int x, int y)
  1050. {
  1051.     switch (key)
  1052.     {  
  1053.     case 'p':
  1054.         {
  1055.         pause = !pause;
  1056.         break;
  1057.         }
  1058.     case 'o':
  1059.         {
  1060.         oneStep = true;
  1061.         break;
  1062.         }
  1063.     case '1':
  1064.         {
  1065.         globaljointA = 45.0;
  1066.         break;
  1067.         }
  1068.     case '2':
  1069.         {
  1070.         globaljointA = 0.001;
  1071.         break;
  1072.         }
  1073.     case '3':
  1074.         {
  1075.         globaljointA = -45.0;
  1076.         break;
  1077.         }
  1078.     case 'n':
  1079.         {
  1080.             int sen;
  1081.             int mot;
  1082.             mot=0;
  1083.             while(mot<8) {
  1084.                 sen=0;
  1085.                 while(sen<4) {
  1086.                     weights[sen][mot] = RandomWeight();
  1087.                     sen++;
  1088.                 }
  1089.                 mot++;
  1090.             }      
  1091.         }
  1092.         break;
  1093.     default:
  1094.         DemoApplication::keyboardCallback(key, x, y);
  1095.     }
  1096.  
  1097.    
  1098. }
  1099.  
  1100.  
  1101.  
  1102.  
  1103. int RagdollDemo
  1104. ::GetFileWeights(char * GFWname )
  1105. {
  1106.     int wread;
  1107.     int sen, mot;
  1108.     double Wij;
  1109.     char Wijtxt[80];
  1110.     string line;
  1111.     ifstream myfile;
  1112.     //.
  1113.  
  1114.     myfile.open (GFWname, ios::in);
  1115.     if (myfile.is_open())
  1116.     {
  1117.         sen=0;
  1118.         mot=0;
  1119.         wread = 0;
  1120.         while ( getline (myfile,line) )
  1121.         {
  1122.             if( wread < 32 ) {
  1123.                 strcpy(Wijtxt, (char *)(line.c_str()) );
  1124.                 sscanf(Wijtxt, "%Lf,", &Wij );
  1125.                 weights[sen][mot] = Wij;               
  1126.                 sen++;
  1127.                 if( sen >= 4 ) {
  1128.                     sen = 0;
  1129.                     mot++;
  1130.                 }
  1131.             }
  1132.             wread++;
  1133.         }
  1134.         myfile.close();
  1135.         if( wread > 31 ) {
  1136.             return 1;
  1137.         } else {
  1138.             cout << "Failed to read all 32 weights from disk.";
  1139.             pause= true;
  1140.             return -3;
  1141.         }
  1142.     }else {
  1143.         cout << "Unable to open ";
  1144.         cout << GFWname;
  1145.         cout << endl;
  1146.         pause = true;
  1147.         return -1;
  1148.     }
  1149.  
  1150.     return 1;
  1151. }
  1152.  
  1153.  
  1154. int RagdollDemo
  1155. ::SavePosition( btRigidBody * spbody, char * SPname )
  1156. {
  1157.     int k;
  1158.     double zd;
  1159.     char positxt[80];  
  1160.     ofstream fitfile;  
  1161.     btTransform btT;
  1162.     btVector3 mbodypos;
  1163.     //.
  1164.  
  1165.     btT = spbody->getWorldTransform();
  1166.     mbodypos = btT.getOrigin();
  1167.     zd = (double)(mbodypos.getZ());        
  1168.     sprintf(positxt, "%.11Lf\r\n", zd );
  1169.     /*
  1170.     cout << "z = ";
  1171.     ConsoleFloat(zd);
  1172.     k = ConsoleWait();  */
  1173.  
  1174.     fitfile.open(SPname, ios::out );
  1175.     if (fitfile.is_open()) {
  1176.         k=0;
  1177.         while( k < 12 ) {
  1178.             fitfile << positxt;
  1179.             k++;
  1180.         }
  1181.         fitfile.close();       
  1182.         return 1;
  1183.     } else {
  1184.         cout << "Unable to open output fitness file." << endl;
  1185.         k = ConsoleWait();
  1186.         return -1;
  1187.     }
  1188.     return 1;
  1189. }
  1190.  
  1191. double RagdollDemo
  1192. ::Fitness(void)
  1193. {      
  1194.     btRigidBody *  spbody   = body[0];
  1195.     btTransform btT         = spbody->getWorldTransform();
  1196.     btVector3 mbodypos      = btT.getOrigin();
  1197.     return( (double)mbodypos.getZ() );
  1198. }
  1199.  
  1200. bool RagdollDemo
  1201. ::SanityCheck(double maxbound)
  1202. {
  1203.     double xrobo, yrobo, zrobo;
  1204.     btRigidBody *  spbody   ;
  1205.     btTransform btT         ;
  1206.     btVector3 mbodypos      ;
  1207.     //.
  1208.  
  1209.     spbody      = body[0];
  1210.     btT         = spbody->getWorldTransform();
  1211.     mbodypos    = btT.getOrigin();
  1212.     zrobo       = (double)mbodypos.getZ();
  1213.     xrobo       = (double)mbodypos.getX();
  1214.     yrobo       = (double)mbodypos.getY();
  1215.  
  1216.     zrobo = fabs(zrobo);
  1217.     xrobo = fabs(xrobo);
  1218.     yrobo = fabs(yrobo);
  1219.  
  1220.     if( zrobo > maxbound ) position_sane = false;
  1221.     if( xrobo > maxbound ) position_sane = false;
  1222.     if( yrobo > maxbound ) position_sane = false;
  1223.  
  1224.     return position_sane;
  1225. }
  1226.  
  1227. void RagdollDemo
  1228. ::ConsoleFloat(double cf)
  1229. {
  1230.     std::stringstream os;
  1231.     std::string cjstr;
  1232.     //.
  1233.     os.flush();
  1234.     os.precision(7);
  1235.     os << std::fixed << cf;
  1236.     cjstr = os.str();  
  1237.     std::cout << cjstr;
  1238.     std::cout << std::endl;
  1239. }
  1240.  
  1241.  
  1242. void RagdollDemo
  1243. ::ConsoleTouches(void)
  1244. {
  1245.     int t;
  1246.     std::stringstream os;
  1247.     std::string cjstr;
  1248.     //.
  1249.     os.flush();
  1250.     t=0;
  1251.     while(t<9) {
  1252.         if( t == 5 ) {
  1253.             os << " ";
  1254.         }
  1255.         os << touches[t];
  1256.         t++;
  1257.     }
  1258.     cjstr = os.str();
  1259.     cjstr.append("\r\n");
  1260.     std::cout << cjstr;
  1261. }
  1262.  
  1263.  
  1264. void ConsoleCollideIDs(int Acid, int Bcid)
  1265. {
  1266.     std::stringstream os;
  1267.     std::string cjstr; 
  1268.     //.
  1269.     os.flush();
  1270.     os << "ID1 = " << Acid << " , ID2 = " << Bcid << "\r\n";
  1271.     cjstr = os.str();
  1272.     std::cout << cjstr;
  1273. }
  1274.  
  1275.  
  1276. int ConsoleWait(void)
  1277. {
  1278.     int ink, lastgc;
  1279.    
  1280.     lastgc = ' ';  
  1281.     while((ink = getchar()) != EOF) {
  1282.         if (ink == '\n') {
  1283.             break;
  1284.         } else {
  1285.             lastgc = ink;
  1286.         }
  1287.     }
  1288.    
  1289.     return lastgc; 
  1290. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!