/*************************************************************************


*																		 *


* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.		 *


* All rights reserved.  Email: russ@q12.org   Web: www.q12.org 		 *


*																		 *


* This library is free software; you can redistribute it and/or		 *


* modify it under the terms of EITHER: 								 *


*	 (1) The GNU Lesser General Public License as published by the Free  *


*		 Software Foundation; either version 2.1 of the License, or (at  *


*		 your option) any later version. The text of the GNU Lesser 	 *


*		 General Public License is included with this library in the	 *


*		 file LICENSE.TXT.												 *


*	 (2) The BSD-style license that is included with this library in	 *


*		 the file LICENSE-BSD.TXT.										 *


*																		 *


* This library is distributed in the hope that it will be useful,		 *


* but WITHOUT ANY WARRANTY; without even the implied warranty of		 *


* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files	 *


* LICENSE.TXT and LICENSE-BSD.TXT for more details.					 *


*																		 *


*************************************************************************/





#include <ode/ode.h>


#include <drawstuff/drawstuff.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 dsDrawCappedCylinder dsDrawCappedCylinderD


#endif








// some constants





const dReal vTerrainLength = 4.f;


const dReal vTerrainHeight = 0.5f;


const int TERRAINNODES = 4;


dReal pTerrainHeights[TERRAINNODES*TERRAINNODES];





dGeomID terrainZ = NULL;


dGeomID terrainY = NULL;





#define NUM 20			// max number of objects


#define DENSITY (5.0)		// density of all objects


#define GPB 3			// maximum number of geometries per body


#define MAX_CONTACTS 4		// maximum number of contact points per body








// dynamics and collision objects





struct MyObject {


	dBodyID body; 		// the body


	dGeomID geom[GPB];		// geometries representing this body


};





static int num=0;		// number of objects in simulation


static int nextobj=0;		// next object to recycle if num==NUM


static dWorldID world;


static dSpaceID space;


static MyObject obj[NUM];


static dJointGroupID contactgroup;


static int selected = -1;	// selected object


static int show_aabb = 0;	// show geom AABBs?


static int show_contacts = 0;	// show contact points?


static int random_pos = 1;	// drop objects from random position?








// this is called by dSpaceCollide when two objects in space are


// potentially colliding.





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


{


	int i;


	// if (o1->body && o2->body) return;


	


	// exit without doing anything if the two bodies are connected by a joint


	dBodyID b1 = dGeomGetBody(o1);


	dBodyID b2 = dGeomGetBody(o2);


	if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;


	


	dContact contact[MAX_CONTACTS];	// up to MAX_CONTACTS contacts per box-box


	for (i=0; i<MAX_CONTACTS; i++) {


		contact[i].surface.mode = dContactBounce | dContactApprox1;	//dContactSoftCFM;


		contact[i].surface.mu = dInfinity;


		contact[i].surface.mu2 = 0;


		contact[i].surface.bounce = 0.1;


		contact[i].surface.bounce_vel = 0.1;


		contact[i].surface.soft_cfm = 0.01;


	}


	if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,


		sizeof(dContact))) {


		dMatrix3 RI;


		dRSetIdentity (RI);


		const dReal ss[3] = {0.02,0.02,0.02};


		for (i=0; i<numc; i++) {


			dJointID c = dJointCreateContact (world,contactgroup,contact+i);


			dJointAttach (c,b1,b2);


			if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);


		}


	}


}








// start simulation - set viewpoint





static void start()


{


	static float xyz[3] = {2.1640f,-1.3079f,1.7600f};


	static float hpr[3] = {125.5000f,-17.0000f,0.0000f};


	dsSetViewpoint (xyz,hpr);


	printf ("To drop another object, press:\n");


	printf ("   b for box.\n");


	printf ("   s for sphere.\n");


	printf ("   c for cylinder.\n");


	printf ("   x for a composite object.\n");


	printf ("To select an object, press space.\n");


	printf ("To disable the selected object, press d.\n");


	printf ("To enable the selected object, press e.\n");


	printf ("To toggle showing the geom AABBs, press a.\n");


	printf ("To toggle showing the contact points, press t.\n");


	printf ("To toggle dropping from random position/orientation, press r.\n");


}








char locase (char c)


{


	if (c >= 'A' && c <= 'Z') return c - ('a'-'A');


	else return c;


}








// called when a key pressed





static void command (int cmd)


{


	int i,j,k;


	dReal sides[3];


	dMass m;


	


	cmd = locase (cmd);


	if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'


		/* || cmd == 'l' */) {


		if (num < NUM) {


			i = num;


			num++;


		}


		else {


			i = nextobj;


			nextobj++;


			if (nextobj >= num) nextobj = 0;


			


			// destroy the body and geoms for slot i


			dBodyDestroy (obj[i].body);


			for (k=0; k < GPB; k++) {


				if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);


			}


			memset (&obj[i],0,sizeof(obj[i]));


		}


		


		obj[i].body = dBodyCreate (world);


		for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;


		


		dMatrix3 R;


		if (random_pos) {


			dBodySetPosition (obj[i].body,


				dRandReal()*2-1,dRandReal()*2+1,dRandReal()+3);


			dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,


				dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);


		}


		else {


			dReal maxheight = 0;


			for (k=0; k<num; k++) {


				const dReal *pos = dBodyGetPosition (obj[k].body);


				if (pos[2] > maxheight) maxheight = pos[2];


			}


			dBodySetPosition (obj[i].body, 0,maxheight+1,maxheight+3);


			dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);


		}


		dBodySetRotation (obj[i].body,R);


		dBodySetData (obj[i].body,(void*) i);


		


		if (cmd == 'b') {


			dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);


			obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);


		}


		else if (cmd == 'c') {


			sides[0] *= 0.5;


			dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);


			obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);


		}


		/*


		// cylinder option not yet implemented


		else if (cmd == 'l') {


		sides[1] *= 0.5;


		dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);


		obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);


		}


		*/


		else if (cmd == 's') {


			sides[0] *= 0.5;


			dMassSetSphere (&m,DENSITY,sides[0]);


			obj[i].geom[0] = dCreateSphere (space,sides[0]);


		}


		else if (cmd == 'x') {


			dGeomID g2[GPB];		// encapsulated geometries


			dReal dpos[GPB][3];	// delta-positions for encapsulated geometries


			


			// start accumulating masses for the encapsulated geometries


			dMass m2;


			dMassSetZero (&m);


			


			// set random delta positions


			for (j=0; j<GPB; j++) {


				for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;


			}


			


			for (k=0; k<GPB; k++) {


				obj[i].geom[k] = dCreateGeomTransform (space);


				dGeomTransformSetCleanup (obj[i].geom[k],1);


				if (k==0) {


					dReal radius = dRandReal()*0.25+0.05;


					g2[k] = dCreateSphere (0,radius);


					dMassSetSphere (&m2,DENSITY,radius);


				}


				else if (k==1) {


					g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);


					dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);


				}


				else {


					dReal radius = dRandReal()*0.1+0.05;


					dReal length = dRandReal()*1.0+0.1;


					g2[k] = dCreateCCylinder (0,radius,length);


					dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);


				}


				dGeomTransformSetGeom (obj[i].geom[k],g2[k]);


				


				// set the transformation (adjust the mass too)


				dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);


				dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);


				dMatrix3 Rtx;


				dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,


					dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);


				dGeomSetRotation (g2[k],Rtx);


				dMassRotate (&m2,Rtx);


				


				// add to the total mass


				dMassAdd (&m,&m2);


			}


			


			// move all encapsulated objects so that the center of mass is (0,0,0)


			for (k=0; k<2; k++) {


				dGeomSetPosition (g2[k],


					dpos[k][0]-m.c[0],


					dpos[k][1]-m.c[1],


					dpos[k][2]-m.c[2]);


			}


			dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);


		}


		


		for (k=0; k < GPB; k++) {


			if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);


		}


		


		dBodySetMass (obj[i].body,&m);


  }


  


  if (cmd == ' ') {


	  selected++;


	  if (selected >= num) selected = 0;


	  if (selected < 0) selected = 0;


  }


  else if (cmd == 'd' && selected >= 0 && selected < num) {


	  dBodyDisable (obj[selected].body);


  }


  else if (cmd == 'e' && selected >= 0 && selected < num) {


	  dBodyEnable (obj[selected].body);


  }


  else if (cmd == 'a') {


	  show_aabb ^= 1;


  }


  else if (cmd == 't') {


	  show_contacts ^= 1;


  }


  else if (cmd == 'r') {


	  random_pos ^= 1;


  }


}








// draw a geom





void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)


{


	int i;


	


	if (!g) return;


	if (!pos) pos = dGeomGetPosition (g);


	if (!R) R = dGeomGetRotation (g);


	


	int type = dGeomGetClass (g);


	if (type == dBoxClass) {


		dVector3 sides;


		dGeomBoxGetLengths (g,sides);


		dsDrawBox (pos,R,sides);


	}


	else if (type == dSphereClass) {


		dsDrawSphere (pos,R,dGeomSphereGetRadius (g));


	}


	else if (type == dCCylinderClass) {


		dReal radius,length;


		dGeomCCylinderGetParams (g,&radius,&length);


		dsDrawCappedCylinder (pos,R,length,radius);


	}


	/*


	// cylinder option not yet implemented


	else if (type == dCylinderClass) {


	dReal radius,length;


	dGeomCylinderGetParams (g,&radius,&length);


	dsDrawCylinder (pos,R,length,radius);


	}


	*/


	else if (type == dGeomTransformClass) {


		dGeomID g2 = dGeomTransformGetGeom (g);


		const dReal *pos2 = dGeomGetPosition (g2);


		const dReal *R2 = dGeomGetRotation (g2);


		dVector3 actual_pos;


		dMatrix3 actual_R;


		dMULTIPLY0_331 (actual_pos,R,pos2);


		actual_pos[0] += pos[0];


		actual_pos[1] += pos[1];


		actual_pos[2] += pos[2];


		dMULTIPLY0_333 (actual_R,R,R2);


		drawGeom (g2,actual_pos,actual_R,0);


	}


	


	if (show_aabb) {


		// draw the bounding box for this geom


		dReal aabb[6];


		dGeomGetAABB (g,aabb);


		dVector3 bbpos;


		for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);


		dVector3 bbsides;


		for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];


		dMatrix3 RI;


		dRSetIdentity (RI);


		dsSetColorAlpha (1,0,0,0.5);


		dsDrawBox (bbpos,RI,bbsides);


	}


}








// simulation loop





static void simLoop (int pause)


{


	dsSetColor (0,0,2);


	dSpaceCollide (space,0,&nearCallback);


	if (!pause) dWorldStep (world,0.05);





	dAASSERT(terrainY);


	dAASSERT(terrainZ);


	dsSetColor (0,1,0);


	dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));


	dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));





	if (show_aabb) 


	{


		dReal aabb[6];


		dGeomGetAABB (terrainY,aabb);


		dVector3 bbpos;


		int i;


		for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);


		dVector3 bbsides;


		for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];


		dMatrix3 RI;


		dRSetIdentity (RI);


		dsSetColorAlpha (1,0,0,0.5);


		dsDrawBox (bbpos,RI,bbsides);





		dGeomGetAABB (terrainZ,aabb);


		for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);


		for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];


		dsDrawBox (bbpos,RI,bbsides);


	}





	dsSetColor (1,1,0);


	


	// remove all contact joints


	dJointGroupEmpty (contactgroup);


	


	dsSetColor (1,1,0);


	dsSetTexture (DS_WOOD);


	for (int i=0; i<num; i++) {


		for (int j=0; j < GPB; j++) {


			if (i==selected) {


				dsSetColor (0,0.7,1);


			}


			else if (! dBodyIsEnabled (obj[i].body)) {


				dsSetColor (1,0,0);


			}


			else {


				dsSetColor (1,1,0);


			}


			drawGeom (obj[i].geom[j],0,0,show_aabb);


		}


	}


}








int main (int argc, char **argv)


{


	// 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/textures";


  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }
	


	// create world


	


	world = dWorldCreate();


	space = dHashSpaceCreate (0);


	contactgroup = dJointGroupCreate (0);


	dWorldSetGravity (world,0,0,-0.5); //-0.5


	dWorldSetCFM (world,1e-5);


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


	memset (obj,0,sizeof(obj));





	for (int i=0;i<TERRAINNODES*TERRAINNODES;i++)	pTerrainHeights[i] = vTerrainHeight * dRandReal();


	terrainY = dCreateTerrainY(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);


	terrainZ = dCreateTerrainZ(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);





	dMatrix3 R;


	dRFromZAxis(R, 0.2f, 0.2f, 0.2f);


	dGeomSetPosition(terrainY,0.f,0.f,0.5f);


	dGeomSetRotation(terrainY,R);


	dGeomSetPosition(terrainZ,0.f,0.f,0.5f);


	dGeomSetRotation(terrainZ,R);





	// run simulation


	dsSimulationLoop (argc,argv,352,288,&fn);


	


	dJointGroupDestroy (contactgroup);


	dSpaceDestroy (space);


	dWorldDestroy (world);


	


	return 0;


}