9:41

Saturday, March 21, 2009

ODE - 3/19 - stepUp - contact joints works wel

////////////////////////starting ts3_stepup.cpp ////////////////////////

//ts3_stepup.cpp 

//key 

//f - force // e - reverse force

//s - stop

//n - reset


//2009/3/12

//two box and joint, joint move by the dJointAddHingeTorque


//changed axis to z, and added cylinder

//but it's need to implement the colision detection of object with other object.


//d key and g key are deleted


//2009/3/19

//modified the nearCallback func to work properly



#include

#include

#include "ts3_stepup.h"





static void nearCallback(void *data, dGeomID o1, dGeomID o2)

{

  const int N = 10;

  dContact contact[N];


  int isGround = ((groundsGeom == o1) || (groundsGeom == o2));


  int n =  dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));


   for (int i = 0; i <>

     contact[i].surface.mu   = dInfinity;

     contact[i].surface.mode = 

 dContactBounce | 

 dContactSlip1 | 

 dContactSoftERP |

 dContactSoftCFM;

     contact[i].surface.bounce     = 0.5; // (0.0~1.0)

     contact[i].surface.bounce_vel = 0.1;

 contact[i].surface.slip1 = 0.001;

 contact[i].surface.soft_erp = 0.01;

 contact[i].surface.soft_cfm = 0.00001;

     dJointID c = dJointCreateContact(world,jointGroup,&contact[i]);

     dJointAttach (c,dGeomGetBody(contact[i].geom.g1),

     dGeomGetBody(contact[i].geom.g2));

  }


}




void start(void)

{

setInit();

setMass();

setBody();

setGeom();

setCondition();

setJoint();

}


void setInit()

{

world = dWorldCreate();

space = dHashSpaceCreate(0);

groundsGeom = dCreatePlane(space, 0, 0, 1, 0);

jointGroup = dJointGroupCreate(0);

}


void setMass()

{

dMassSetZero(&mass);

dMassSetBoxTotal(&mass, simTotalMass, simX, simY, simZ);


dMassSetZero(&mass2);

dMassSetBoxTotal(&mass2, simTotalMass2, simX2, simY2, simZ2);


dMassSetZero(&massTrash);

dMassSetCylinderTotal(&massTrash, simTotalMassTrash, 3, simRadTrash, simLenTrash);

}



void setBody()

{

body = dBodyCreate(world);

{

dBodySetPosition(body, simInitX, simInitY, simInitZ);

//dBodySetRotation(body, dMatrix3

dBodySetMass(body, &mass);

}


body2 = dBodyCreate(world);

{

dBodySetPosition(body2, simInitX2, simInitY2, simInitZ2 );

dBodySetMass(body2, &mass2);

}


bodyTrash = dBodyCreate(world);

{

dBodySetPosition(bodyTrash, simInitXTrash, simInitYTrash, simInitZTrash);

dBodySetMass(bodyTrash, &massTrash);

}

}


void setGeom()

{

bodysGeom = dCreateBox(space, simX, simY, simZ);

bodysGeom2 = dCreateBox(space, simX2, simY2, simZ2);

bodyTrashsGeom = dCreateCylinder(space, simRadTrash, simLenTrash);


dGeomSetBody(bodysGeom, body);

dGeomSetBody(bodysGeom2, body2);

dGeomSetBody(bodyTrashsGeom, bodyTrash);

}


void setCondition()

{


dWorldSetGravity(world, 0, 0, -9.8);

//dWorldSetCFM(world, 0.6);

//dWorldSetContactMaxCorrectingVel(world, 2);

}



void setJoint()

{

myJointGroup = dJointGroupCreate(0);

myJoint = dJointCreateHinge( world, myJointGroup );

dJointAttach( myJoint, body, body2 );

//dJointSetFixed( myJoint );

dJointSetHingeAnchor( myJoint, -0.5, 0.5, 1 );

//dJointSetHingeAxis( myJoint, 1, 0, 0);

dJointSetHingeAxis( myJoint, 0, 0, 1);



dJointSetHingeParam( myJoint, dParamHiStop, 3.14/2 );

dJointSetHingeParam( myJoint, dParamLoStop, -3.14/2 );


//dJointSetHingeParam( myJoint, dParamVel, 0.1 );

dJointSetHingeParam( myJoint, dParamFMax, 0.1 );

dJointSetHingeParam( myJoint, dParamFudgeFactor, 0.2 );

}




void step(int pause)

{

dSpaceCollide(space, 0, &nearCallback);

dWorldStep(world, 0.01);

dJointGroupEmpty(jointGroup);


if( MESSAGE_ON == true )

printMessage();


drawObject();


}


void giveForce()

{

printf("giveForce is called\n");


//dBodyAddForce(body, simFX, simFY, simFZ);

//dBodyAddRelForceAtPos(body, simFX, simFY, simFZ, simPX, simPY, simPZ);


if( forceDirection == true )

dJointAddHingeTorque( myJoint, torqueForce );

else

dJointAddHingeTorque( myJoint, -1*torqueForce );

}


void printMessage()

{

dVector3 bodyVel;

dBodyGetPointVel(body, position[0], position[1], position[2], bodyVel);

//printf( "%f\n", bodyVel[2] );


printf("%d\n", dBodyGetNumJoints(body));



}



void drawObject()

{

position = dBodyGetPosition(body);

rotation = dBodyGetRotation(body);

const double sides[3] = {simX, simY, simZ};

dsDrawBoxD(position, rotation, sides);


const dReal* position2 = dBodyGetPosition(body2);

const dReal* rotation2 = dBodyGetRotation(body2);

const double sides2[3] = {simX2, simY2, simZ2};

dsSetColor(1,0,0);

dsDrawBoxD(position2, rotation2, sides2);


const dReal* positionTrash = dBodyGetPosition(bodyTrash);

const dReal* rotationTrash = dBodyGetRotation(bodyTrash);

dsSetColor(0, 1, 0);

dsDrawCylinderD( positionTrash, rotationTrash, (float)simLenTrash, (float)simRadTrash );


}



void command(int cmd)

{

switch(cmd)

{

case 'f':

case 'F'://normal force

if( forceDirection == false ) forceDirection = true;

giveForce();

break;

case 'r':

case 'R'://inverse force

if( forceDirection == true ) forceDirection = false;

giveForce();

break;




case 's':

case 'S':

dsStop();

break;


case 'p':

case 'P':

if( MESSAGE_ON == true )

MESSAGE_ON = false;

else

MESSAGE_ON = true;

break;


case 'n':

case 'N':

dBodySetPosition(body, simInitX, simInitY, simInitZ);

dBodySetRotation(body, simRot);

dBodySetPosition(body2, simInitX2, simInitY2, simInitZ2);

dBodySetRotation(body2, simRot2);

//dJointSetHingeAnchor( myJoint, -0.5, -0.5, 0.5 );

//dJointSetHingeAxis( myJoint, 0, 0, 1);

break;


default :

break;

}

}




int main( int argc, char* argv[] )

{

dsFunc.version = DS_VERSION;

dsFunc.start = start;

dsFunc.step = step;

dsFunc.command = command;

dsFunc.stop = NULL;

dsFunc.path_to_textures = "c:/ode-0.11/drawstuff/textures";


dInitODE();

dsSimulationLoop( argc, argv, 1024, 768, &dsFunc );


dBodyDestroy(body);

dWorldDestroy(world);


dCloseODE();

}


////////////////////////ending ts3_stepup.cpp ////////////////////////



////////////////////////starting ts3_stepup.h ////////////////////////

#ifndef TS3_STEPUP_H

#define TS3_STEPUP_H


static void nearCallback(void *data, dGeomID o1, dGeomID o2);

void start(void);


void setInit();

void setMass();

void setBody();

void setGeom();

void setCondition();

void setJoint();


void step(int pause);

void giveForce();

void printMessage();


void drawObject();


void command(int cmd);



//drawstuff function

dsFunctions dsFunc;


//ode instances

dWorldID world;


dBodyID body;

dBodyID body2;

dBodyID bodyTrash; //body for trash can


dGeomID bodysGeom;

dGeomID bodysGeom2;

dGeomID bodyTrashsGeom; //geom for trash can body


dGeomID groundsGeom;


dSpaceID space;


dJointGroupID jointGroup;


dJointID myJoint;

dJointID myJoint2;

dJointGroupID myJointGroup;


//variables

static bool MESSAGE_ON = false;

static const dReal* position;

static const dReal* rotation;





//simulation parameter for simulation body

static dMass mass;

static dMass mass2;

static dMass massTrash;


static dReal simTotalMass = 100;

static dReal simTotalMass2 = 1;

static dReal simTotalMassTrash = 1;


static dReal simX = 1;

static dReal simY = 1;

static dReal simZ = 1;


static dReal simX2 = 1;

static dReal simY2 = 1;

static dReal simZ2 = 1;


static dReal simRadTrash = 0.2;

static dReal simLenTrash = 3;



static dReal simInitX = 0;

static dReal simInitY = 0;

static dReal simInitZ = simZ/2;


static dReal simInitX2 = -1;

static dReal simInitY2 = 1;

static dReal simInitZ2 = simZ2*1.5;


static dReal simInitXTrash = -1;

static dReal simInitYTrash = 0;

static dReal simInitZTrash = simLenTrash/2;



static dReal simFX = 50;

static dReal simFY = 0;

static dReal simFZ = 0;


static dReal simPX = simX/2;

static dReal simPY = simY/2;

static dReal simPZ = simZ/2;


static dMatrix3 simRot = {0};

static dMatrix3 simRot2 = {0};


//force flag and value

static bool forceDirection = true;

static double torqueForce = -10;




#endif

////////////////////////ending ts3_stepup.h ////////////////////////


Followers