////////////////////////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 ////////////////////////