#include <stdio.

h>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"

#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define SIDE (0.4f) //meters: 0.4 m is the average
link length of the arms of the PA-10 robot
#define MASS (1.0f) // mass of a box
#define DENSITY 1/(0.4*0.4*0.4) // density of all objects
#define NUM 20
#define GPB 3 // maximum number of geometr
ies per body
#define MAX_CONTACTS 8 // maximum number of contact po
ints per body
#define MAX_FEEDBACKNUM 20
#define GRAVITY REAL(0.5)
#define USE_GEOM_OFFSET 1
#define RADIUS (0.1732f) /* sphere radius */
#define INCR (0.001f)
#define MAX_PNTS 20000
#define MAX_VEL (0.05f)//(0.1f)
// some flags
#define IDMASS 1 // flag to identify mass
#define IDINERTIA 2 // mass to identify inertia
#define EXPVERIFY 1 // test experimental results
#define EXPPERFORM 2 // perform the experiment
#define EXPEXIT 0 // no experimental procedure
#define EXPINIT 3
// dynamics and collision objects
static dWorldID world;
static dSpaceID space;
static dBodyID link[NUM];
static dJointID joint[NUM-1];
static dJointGroupID contactgroup;
static dGeomID box[NUM];
static dGeomID sphere[NUM];
static dBodyID bodyC1;
static dBodyID bodyC2;
static dBodyID bodyC3;
static dGeomID cylinder;

// state set by keyboard commands

static int occasional_error = 0;
void printPos(dBodyID obj_body) // print a position
{
const dReal *pos; // Do not forget const to prevent an error
dReal x, y, z;
pos = dBodyGetPosition(obj_body); // Return value is an pointer to a structure
x = pos[0]; y = pos[1]; z = pos[2];
printf("x=%f y=%f z=%f \n", x, y, z);
}
void printRot(dBodyID obj_body) // print a rotation
{
const dReal *rot; // Do not forget const to prevent an error
dReal x1, y1, z1, x2, y2, z2, x3, y3, z3;
rot = dBodyGetRotation(obj_body); // Return value is an pointer to a structure
x1 = rot[0]; y1 = rot[1]; z1 = rot[2];
x2 = rot[3]; y2 = rot[4]; z2 = rot[5];
x3 = rot[7]; y3 = rot[8]; z3 = rot[9];
printf("x1=%f y1=%f z1=%f \n", x1, y1, z1);
printf("x2=%f y2=%f z2=%f \n", x2, y2, z2);
printf("x3=%f y3=%f z3=%f \n", x3, y3, z3);

}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
/* exit without doing anything if the two bodies are connected by a joint */
dBodyID b1,b2;
dContact contact;
b1 = dGeomGetBody(o1);
b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
contact.surface.mode = 0;
contact.surface.mu = 0.1;
contact.surface.mu2 = 0;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world,contactgroup,&contact);
dJointAttach (c,b1,b2);
}
}
// start simulation - set viewpoint
static void start()
{
static float xyz[3] = {0.1640f,-2.3079f,1.0000f};
static float hpr[3] = {90.0000f,-17.0000f,0.0000f};
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
printf("CYLINDER1"); printPos(bodyC1);
printf("CYLINDER2"); printPos(bodyC2);
printf("CYLINDER3"); printPos(bodyC3);
printf("CYLINDER1"); printRot(bodyC1);
 printf("CYLINDER2"); printRot(bodyC2);
printf("CYLINDER3"); printRot(bodyC3);
}

// called when a key pressed

static void command (int cmd)
{
int i;
double period;
double sgn;
}
// simulation loop
static void simLoop (int pause)
{
int k,j;
int pdone;
int dof,col;
char m_string[50];
double period,norm2;

if (!pause) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05); //dWorldStep (world,0.09);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
dsSetColorAlpha (0,0,1,1);
dsSetTexture (DS_WOOD);
dReal radius = 0.5; // radius
dReal length = 0.07; // length
void dJointSetFixed (dJointC);
void dJointSetAMotorMode (dJointC, int dAMotorUser);
int dJointGetAMotorMode (dJointC);
void dJointSetAMotorNumAxes (dJointC, int 0);
int dJointGetAMotorNumAxes (dJointC);
void dJointSetAMotorAxis (dJointC, int 0, int rel, dReal 1, dReal 1, dRe
al 1);
void dJointGetAMotorAxis (dJointC, int 0, dVector3 result);
int dJointGetAMotorAxisRel (dJointC, int 0);
void dJointSetAMotorAngle (dJointC, int 0, dReal 0.2);

/*float pi = 3.1416;
dMatrix3 C1,C2,C3;
dRFromAxisAndAngle(C1,0,0,1,(pi/3));
dRFromAxisAndAngle(C2,0,1,0,(pi/2));
dRFromAxisAndAngle(C3,1,0,0,(pi/2));
dBodySetRotation(bodyC1,C1);
dBodySetRotation(bodyC2,C2);
 dBodySetRotation(bodyC3,C3);*/
dsDrawCylinder(dBodyGetPosition(bodyC1),dBodyGetRotation(bodyC1),radius,leng
th);
dsSetColorAlpha (0,1,0,1);
dsSetTexture (DS_WOOD);
dsDrawCylinder(dBodyGetPosition(bodyC2),dBodyGetRotation(bodyC2),radius,
length);
dsSetColor (1,0,0);
dsSetTexture (DS_WOOD);
dsDrawCylinder(dBodyGetPosition(bodyC3),dBodyGetRotation(bodyC3),radius,
length);
joint = dJointCreateHinge(world, 0); // Create a hinge joint
dJointAttach(joint, C1,C2); // Attach joint to bodies
dJointSetHingeAnchor(joint, 1, 0, 0); // Set a joint anchor
dJointSetHingeAxis(joint, 1, 0, 0); // Set a hinge axis(1,0,0)
joint = dJointCreateHinge(world, 0); // Create a hinge joint
dJointAttach(joint, C2,C3); // Attach joint to bodies
dJointSetHingeAnchor(joint, 0, 1, 0); // Set a joint anchor
dJointSetHingeAxis(joint, 0, 1, 0); // Set a hinge axis(0,1,0)
joint = dJointCreateHinge(world, 0); // Create a hinge joint
dJointAttach(joint, C3,C1,); // Attach joint to bodies
dJointSetHingeAnchor(joint, 0, 0, 1); // Set a joint anchor
dJointSetHingeAxis(joint, 0, 0, 1); // Set a hinge axis(0,0,1)
}
int main (int argc, char **argv)
{
char flgRobot;
int i;
double k;
char path[200];
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (1000000);

bodyC1 = dBodyCreate (world);

bodyC2 = dBodyCreate (world);
 bodyC3 = dBodyCreate (world);
//dBodySetMass (bodyBX,&m);
dBodySetPosition (bodyC1,0,0,1);
dBodySetPosition (bodyC2,1,0,0);
dBodySetPosition (bodyC3,0,-1,0);

// ground
dJointID ground = dJointCreateFixed (world,0);
dJointAttach (ground,link[0],0);
dJointSetFixed (ground);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}