/**
Author : Stevie Giovanni (steviegiovanni@gmail.com)
**/

#include "../compileconfig.h"
#ifdef Bullet
#include "BulletWorld.h"
#include <Utils/utils.h>
#include <Physics/Joint.h>
#include <Physics/StiffJoint.h>
#include <Physics/HingeJoint.h>
#include <Physics/UniversalJoint.h>
#include <Physics/BallInSocketJoint.h>
#include <Physics/PhysicsGlobals.h>


/**
	Default constructor
*/
BulletWorld::BulletWorld() : World(){
	//printf("BulletWorld::BulletWorld\n");
	setupWorld();
}

/**
	destructor
*/
BulletWorld::~BulletWorld(void){
	printf("code reached a\n");
}

void BulletWorld::destroyWorld() {
	//printf("BulletWorld::destroyWorld\n");
	//delete[] cps;
	delete pcQuery;
	bulletToRbs.clear();
	
	//destroy the world
	delete worldID;

	World::destroyWorld();
}

void BulletWorld::setupWorld() {
	//printf("BulletWorld::setupWorld\n");
	int maxCont = 4;

	///collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration.
	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	btCollisionDispatcher* dispatcher = new	btCollisionDispatcher(collisionConfiguration);
	///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep.
	btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase();
	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
	worldID = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);

	Vector3d gravity = PhysicsGlobals::up * PhysicsGlobals::gravity;
	worldID->setGravity(btVector3(gravity.x, gravity.y, gravity.z));
	worldID->setDebugDrawer(&sDebugDrawer);
	//worldID->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits);
	maxContactCount = maxCont;

	pcQuery = NULL;

	pcQuery = new PreCollisionQuery();
}


void BulletWorld::destroyAllObjects() {
	//printf("BulletWorld::destroyAllObjects\n");
	destroyWorld();
	setupWorld();
	if(testmode){
		this->drawBoxes();
	}
}

/**
	this method is used to copy the state of the ith rigid body to its Bullet counterpart.
*/
void BulletWorld::setBulletStateFromRB(int i){
	//printf("BulletWorld::setBulletStateFromRB");
	if (i<0 || (uint)i>=bulletToRbs.size())
		return;

	//if it is a locked object, we update its CDPS
	if (bulletToRbs[i].rb->isLocked() == true) {
		for (uint j=0;j<bulletToRbs[i].collisionVolumes.size();j++){
			btCompoundShape* t = (btCompoundShape*)bulletToRbs[i].collisionVolumes[j];
			btTransform transform = t->getChildTransform(0);
			transform.setOrigin(btVector3(bulletToRbs[i].rb->state.position.x, bulletToRbs[i].rb->state.position.y, bulletToRbs[i].rb->state.position.z));
			btQuaternion q;
			q.setW(bulletToRbs[i].rb->state.orientation.s);
			q.setX(bulletToRbs[i].rb->state.orientation.v.x);
			q.setY(bulletToRbs[i].rb->state.orientation.v.y);
			q.setZ(bulletToRbs[i].rb->state.orientation.v.z);
			transform.setRotation(q);
			t->updateChildTransform(0,transform);
		}
		return;
	}

	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(btVector3(bulletToRbs[i].rb->state.position.x, bulletToRbs[i].rb->state.position.y, bulletToRbs[i].rb->state.position.z));
	transform.setRotation(btQuaternion(bulletToRbs[i].rb->state.orientation.v.x,bulletToRbs[i].rb->state.orientation.v.y,bulletToRbs[i].rb->state.orientation.v.z,bulletToRbs[i].rb->state.orientation.s));
	bulletToRbs[i].id->setWorldTransform(transform);
	bulletToRbs[i].id->setLinearVelocity(btVector3(bulletToRbs[i].rb->state.velocity.x, bulletToRbs[i].rb->state.velocity.y, bulletToRbs[i].rb->state.velocity.z));
	bulletToRbs[i].id->setAngularVelocity(btVector3(bulletToRbs[i].rb->state.angularVelocity.x, bulletToRbs[i].rb->state.angularVelocity.y, bulletToRbs[i].rb->state.angularVelocity.z));
}

/**
	this method is used to copy the state of the ith rigid body, from the Bullet object to its rigid body counterpart 
*/
void BulletWorld::setRBStateFromBullet(int i){
	//printf("BulletWorld::setRBStateFromBullet\n");
	btVector3 tempData;

	//if it is a locked object, we won't do anything about it
	if (bulletToRbs[i].rb->isLocked() == true){
		//printf("rb locked\n");
		return;
	}

	//if the objects is supposed to be planar, make sure we don't let drift accumulate
	if (bulletToRbs[i].rb->props.isPlanar){
		btVector3 rot = bulletToRbs[i].id->getAngularVelocity();
		btQuaternion quat_ptr;
		btQuaternion quat;
		btScalar quat_len;
		btTransform transform = bulletToRbs[i].id->getWorldTransform(); 
		quat_ptr = transform.getRotation();
		quat.setW(quat_ptr.getW());
		quat.setX(quat_ptr.getX());
		quat.setY(0); 
		quat.setZ(0); 
		quat_len = sqrt( quat.getW() * quat.getW() + quat.getX() * quat.getX() );
		quat.setW(quat.getW() / quat_len);
		quat.setX(quat.getX() / quat_len);
		transform.setRotation(quat);
		bulletToRbs[i].id->setWorldTransform(transform);
		rot.setX(0);
		rot.setZ(0);
		bulletToRbs[i].id->setAngularVelocity(rot);
	}
	btTransform transform = bulletToRbs[i].id->getWorldTransform();

	tempData = transform.getOrigin();
	bulletToRbs[i].rb->state.position.x = tempData.getX();
	bulletToRbs[i].rb->state.position.y = tempData.getY();
    bulletToRbs[i].rb->state.position.z = tempData.getZ();

	btQuaternion orientation;
	orientation = transform.getRotation();
	bulletToRbs[i].rb->state.orientation.s = orientation.getW();
	bulletToRbs[i].rb->state.orientation.v.x = orientation.getX();
	bulletToRbs[i].rb->state.orientation.v.y = orientation.getY();
	bulletToRbs[i].rb->state.orientation.v.z = orientation.getZ();

	tempData = bulletToRbs[i].id->getLinearVelocity();
	bulletToRbs[i].rb->state.velocity.x = tempData.getX();
	bulletToRbs[i].rb->state.velocity.y = tempData.getY();
	bulletToRbs[i].rb->state.velocity.z = tempData.getZ();

	tempData = bulletToRbs[i].id->getAngularVelocity();
	bulletToRbs[i].rb->state.angularVelocity.x = tempData.getX();
	bulletToRbs[i].rb->state.angularVelocity.y = tempData.getY();
	bulletToRbs[i].rb->state.angularVelocity.z = tempData.getZ();
}

/**
	this method is used to set up a Bullet plane shape. 
*/
btCollisionShape * BulletWorld::getPlaneGeom(PlaneCDP* p, RigidBody* parent){
	//printf("BulletWorld::getPlaneGeom\n");
	//and create the ground plane
	Vector3d n = parent->getWorldCoordinates(p->getNormal());
	Vector3d o = /*Vector3d(0,-10,0);//*/Vector3d(parent->getWorldCoordinates(p->getOrigin()));
	btCollisionShape * ps = new btStaticPlaneShape(btVector3(n.x,n.y,n.z),o.dotProductWith(n));
	btCompoundShape * g = new btCompoundShape();
	btTransform transform;
	transform.setIdentity();
	g->addChildShape(transform,ps);
	return g;
}

/**
	this method is used to set up a Bullet sphere shape. It is properly placed in body coordinates.
*/
btCollisionShape * BulletWorld::getSphereGeom(SphereCDP* s){
	//printf("BulletWorld::getSphereGeom\n");
	btCollisionShape * ss = new btSphereShape(s->getRadius());
	btCompoundShape * g = new btCompoundShape();
	Point3d c = s->getCenter();
	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(btVector3(c.x,c.y,c.z));
	g->addChildShape(transform,ss);
	//dGeomSetPosition(g, c.x, c.y, c.z);
	return g;
}

/**
	this method is used to set up a Bullet box shape. It is properly placed in body coordinates.
*/
btCollisionShape * BulletWorld::getBoxGeom(BoxCDP* b){
	//printf("BulletWorld::getBoxGeom");
	btCollisionShape * bs = new btBoxShape(btVector3(b->getXLen()/2, b->getYLen()/2, b->getZLen()/2));
	btCompoundShape * g = new btCompoundShape();
	Point3d c = b->getCenter();
	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(btVector3(c.x,c.y,c.z));
	g->addChildShape(transform,bs);
	return g;
}

/**
	this method is used to set up a Bullet capsule shape. It is properly placed in body coordinates.
*/
btCollisionShape * BulletWorld::getCapsuleGeom(CapsuleCDP* c){
	//printf("BulletWorld::getCapsuleGeom\n");
	Point3d a = c->getPoint1();
	Point3d b = c->getPoint2();
	Vector3d ab(a, b);
	btCollisionShape * cs = new btCapsuleShape(c->getRadius(),ab.length());
	
	Point3d cen = a + ab/2.0;
	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(btVector3(cen.x,cen.y,cen.z));

	//now, the default orientation for this is along the y-axis. We need to rotate this to make it match the direction
	//of ab, so we need an angle and an axis...
	Vector3d defA(0, 1, 0);
	Vector3d axis = defA.crossProductWith(ab);
	axis.toUnit();
	double rotAngle = defA.angleWith(ab);

	Quaternion relOrientation = Quaternion::getRotationQuaternion(rotAngle, axis);
	
	btQuaternion q;
	q.setW(relOrientation.s);
	q.setX(relOrientation.v.x);
	q.setY(relOrientation.v.y);
	q.setZ(relOrientation.v.z);

	transform.setRotation(q);

	btCompoundShape * g = new btCompoundShape();
	g->addChildShape(transform,cs);
	return g;
}

/**
	This method is used to set up a Bullet fixed joint, based on the information in the fixed joint passed in as a parameter
*/
void BulletWorld::setupBulletFixedJoint(StiffJoint* hj){
	Point3d pchild = hj->getChildJointPosition();
	Point3d pparent = hj->getParentJointPosition();
	btTransform childtransform;
	childtransform.setIdentity();
	childtransform.setOrigin(btVector3(pchild.x,pchild.y,pchild.z));
	btTransform parenttransform;
	parenttransform.setIdentity();
	parenttransform.setOrigin(btVector3(pparent.x,pparent.y,pparent.z));
	btTypedConstraint * constraint = new btGeneric6DofConstraint(*(bulletToRbs[(int)(hj->parent->id)].id),*( bulletToRbs[(int)(hj->child->id)].id),parenttransform,childtransform,true);
	((btGeneric6DofConstraint*)constraint)->setLimit(0,0,0);
	((btGeneric6DofConstraint*)constraint)->setLimit(1,0,0);
	((btGeneric6DofConstraint*)constraint)->setLimit(2,0,0);
	((btGeneric6DofConstraint*)constraint)->setLimit(3,0,0);
	((btGeneric6DofConstraint*)constraint)->setLimit(4,0,0);
	((btGeneric6DofConstraint*)constraint)->setLimit(5,0,0);
	worldID->addConstraint(constraint,true);
	/*dJointID j = dJointCreateFixed(worldID, 0);
	dJointAttach(j, odeToRbs[(int)(hj->child->id)].id, odeToRbs[(int)(hj->parent->id)].id);
	dJointSetFixed(j);*/
}

/**
	This method is used to set up a Bullet hinge joint, based on the information in the hinge joint passed in as a parameter
*/
void BulletWorld::setupBulletHingeJoint(HingeJoint* hj){
	Vector3d pa = hj->getParentJointPosition();
	Vector3d pb = hj->getChildJointPosition();
	btVector3 pivotina(pa.x,pa.y,pa.z);
	btVector3 pivotinb(pb.x,pb.y,pb.z);
	Vector3d aa = hj->a;
	Vector3d bb = hj->child->getLocalCoordinates(hj->parent->getWorldCoordinates(hj->a));
	btVector3 axisina(aa.x,aa.y,aa.z);
	btVector3 axisinb(bb.x,bb.y,bb.z);
	btTypedConstraint * constraint = new btHingeConstraint(*(bulletToRbs[(int)(hj->parent->id)].id),*( bulletToRbs[(int)(hj->child->id)].id),pivotina,pivotinb,axisina,axisinb);

	if(hj->useJointLimits == true && !testmode){
		((btHingeConstraint*)constraint)->setLimit(-hj->maxAngle,-hj->minAngle);
	}

	worldID->addConstraint(constraint,true);
	/*dJointID j = dJointCreateHinge(worldID, 0);
	dJointAttach(j, odeToRbs[(int)(hj->child->id)].id, odeToRbs[(int)(hj->parent->id)].id);
	Point3d p = hj->child->getWorldCoordinates(hj->cJPos);
	dJointSetHingeAnchor(j, p.x, p.y, p.z);
	Vector3d a = hj->parent->getWorldCoordinates(hj->a);
	dJointSetHingeAxis(j, a.x, a.y, a.z);

	//now set the joint limits
	//if (hj->useJointLimits == false)
		return;
	dJointSetHingeParam(j, dParamLoStop, hj->minAngle);
	dJointSetHingeParam(j, dParamHiStop, hj->maxAngle);*/
}

/**
	This method is used to set up a Bullet universal joint, based on the information in the universal joint passed in as a parameter
*/
void BulletWorld::setupBulletUniversalJoint(UniversalJoint* uj){
	/*Vector3d pa = hj->getParentJointPosition();
	Vector3d pb = hj->getChildJointPosition();
	btVector3 pivotina(pa.x,pa.y,pa.z);
	btVector3 pivotinb(pb.x,pb.y,pb.z);*/
	Point3d p = uj->child->getWorldCoordinates(uj->cJPos);
	Vector3d a = uj->parent->getWorldCoordinates(uj->a);
	Vector3d b = uj->child->getWorldCoordinates(uj->b);
	btTypedConstraint * constraint = new btUniversalConstraint(*(bulletToRbs[(int)(uj->parent->id)].id),*( bulletToRbs[(int)(uj->child->id)].id),btVector3(p.x,p.y,p.z),btVector3(a.x,a.y,a.z),btVector3(b.x,b.y,b.z));
	
	if(uj->useJointLimits == true){
		((btUniversalConstraint*)constraint)->setLowerLimit(uj->minAngleA,uj->minAngleB);
		((btUniversalConstraint*)constraint)->setUpperLimit(uj->maxAngleA,uj->maxAngleB);
	}

	worldID->addConstraint(constraint,true);
	/*dJointID j = dJointCreateUniversal(worldID, 0);
	dJointAttach(j, odeToRbs[(int)(uj->child->id)].id, odeToRbs[(int)(uj->parent->id)].id);
	Point3d p = uj->child->getWorldCoordinates(uj->cJPos);
	dJointSetUniversalAnchor(j, p.x, p.y, p.z);

	Vector3d a = uj->parent->getWorldCoordinates(uj->a);
	Vector3d b = uj->child->getWorldCoordinates(uj->b);
    
	dJointSetUniversalAxis1(j, a.x, a.y, a.z);
	dJointSetUniversalAxis2(j, b.x, b.y, b.z);

	//now set the joint limits
	//if (uj->useJointLimits == false)
		return;

	dJointSetUniversalParam(j, dParamLoStop, uj->minAngleA);
	dJointSetUniversalParam(j, dParamHiStop, uj->maxAngleA);
	dJointSetUniversalParam(j, dParamLoStop2, uj->minAngleB);
	dJointSetUniversalParam(j, dParamHiStop2, uj->maxAngleB);*/
}

/**
	This method is used to set up a Bullet ball-and-socket joint, based on the information in the ball in socket joint passed in as a parameter
*/
void BulletWorld::setupBulletBallAndSocketJoint(BallInSocketJoint* basj){
	/*//printf("BulletWorld::setupBulletBallAndSocketJoint\n");
	Vector3d pa = basj->getChildJointPosition();
	Vector3d pb = basj->getParentJointPosition();
	btVector3 pivotina(pa.x,pa.y,pa.z);
	btVector3 pivotinb(pb.x,pb.y,pb.z);
	btTypedConstraint * constraint = new btPoint2PointConstraint(*(bulletToRbs[(int)(basj->child->id)].id),*( bulletToRbs[(int)(basj->parent->id)].id),pivotina,pivotinb);
	
	if(basj->useJointLimits == true){
		
	}*/
	

	Point3d pchild = basj->getChildJointPosition();
	Point3d pparent = basj->getParentJointPosition();
	btTransform childtransform;
	childtransform.setIdentity();
	childtransform.setOrigin(btVector3(pchild.x,pchild.y,pchild.z));
	btTransform parenttransform;
	parenttransform.setIdentity();
	parenttransform.setOrigin(btVector3(pparent.x,pparent.y,pparent.z));
	btTypedConstraint * constraint = new btGeneric6DofConstraint(*(bulletToRbs[(int)(basj->parent->id)].id),*( bulletToRbs[(int)(basj->child->id)].id),parenttransform,childtransform,true);
	
	((btGeneric6DofConstraint*)constraint)->setLimit(0,0,0);
	((btGeneric6DofConstraint*)constraint)->setLimit(1,0,0);
	((btGeneric6DofConstraint*)constraint)->setLimit(2,0,0);

	if(basj->useJointLimits == true && !testmode){
		((btGeneric6DofConstraint*)constraint)->setLimit(3,basj->minTwistAngle,basj->maxTwistAngle);
		((btGeneric6DofConstraint*)constraint)->setLimit(4,basj->minSwingAngle1,basj->maxSwingAngle1);
		((btGeneric6DofConstraint*)constraint)->setLimit(5,basj->minSwingAngle2,basj->maxSwingAngle2);
	}

	worldID->addConstraint(constraint,true);
	/*dJointID j = dJointCreateBall(worldID, 0);
	dJointAttach(j, odeToRbs[(int)(basj->child->id)].id, odeToRbs[(int)(basj->parent->id)].id);
	Point3d p = basj->child->getWorldCoordinates(basj->cJPos);
	//now we'll set the world position of the ball-and-socket joint. It is important that the bodies are placed in the world
	//properly at this point
	dJointSetBallAnchor(j, p.x, p.y, p.z);

	//now deal with the joint limits
	//if (basj->useJointLimits == false)
		return;

	Vector3d a = basj->parent->getWorldCoordinates(basj->swingAxis1);
	Vector3d b =  basj->child->getWorldCoordinates(basj->twistAxis);

	//we'll assume that:
	//b is the twisting axis of the joint, and the joint limits will be (in magnitude) less than 90 degrees, otherwise
	//the simulation will go unstable!!!


	dJointID aMotor = dJointCreateAMotor(worldID, 0);
	dJointAttach(aMotor, odeToRbs[(int)(basj->parent->id)].id, odeToRbs[(int)(basj->child->id)].id);
	dJointSetAMotorMode(aMotor, dAMotorEuler);

	dJointSetAMotorParam(aMotor, dParamStopCFM, 0.1);
	dJointSetAMotorParam(aMotor, dParamStopCFM2, 0.1);
	dJointSetAMotorParam(aMotor, dParamStopCFM3, 0.1);


	dJointSetAMotorAxis (aMotor, 0, 1, a.x, a.y, a.z);
	dJointSetAMotorAxis (aMotor, 2, 2, b.x, b.y, b.z);

	dJointSetAMotorParam(aMotor, dParamLoStop, basj->minSwingAngle1);
	dJointSetAMotorParam(aMotor, dParamHiStop, basj->maxSwingAngle1);
	
	dJointSetAMotorParam(aMotor, dParamLoStop2, basj->minSwingAngle2);
	dJointSetAMotorParam(aMotor, dParamHiStop2, basj->maxSwingAngle1);

	dJointSetAMotorParam(aMotor, dParamLoStop3, basj->minTwistAngle);
	dJointSetAMotorParam(aMotor, dParamHiStop3, basj->maxTwistAngle);*/
}

/**
	this method is used to create Bullet shapes for all the collision primitives of the rigid body that is passed in as a paramter
*/
void BulletWorld::createBulletCollisionPrimitives(RigidBody* body, int index){
	//printf("BulletWorld::createBulletCollisionPrimitives\n");
	btCompoundShape * cs = new btCompoundShape();

	//now we'll set up the body's collision detection primitives
	for (uint j=0;j<body->cdps.size();j++){
		int cdpType = body->cdps[j]->getType();

		//depending on the type of collision primitive, we'll now create g.
		btCollisionShape * g;

		switch (cdpType){
			case SPHERE_CDP:
				g = getSphereGeom((SphereCDP*)body->cdps[j]);
				break;
			case CAPSULE_CDP:
				g = getCapsuleGeom((CapsuleCDP*)body->cdps[j]);
				break;
			case BOX_CDP:
				g = getBoxGeom((BoxCDP*)body->cdps[j]);
				break;
			case PLANE_CDP:
				//NOTE: only static objects can have planes as their collision primitives - if this isn't static, force it!!
				g = getPlaneGeom((PlaneCDP*)body->cdps[j], body);
				break;
			default:
				throwError("Ooppps... No collision detection primitive was created rb: %s, cdp: %d", body->name, j);
		}
	
		//now associate the geom to the rigid body that it belongs to, so that we can look up the properties we need later...
		cs->setUserPointer(body);

		//if it's a plane, it means it must be static, so we can't attach a transform to it...
		if (cdpType == PLANE_CDP){
			btTransform transform;
			transform.setIdentity();
			cs->addChildShape(transform,g);
			continue;
		}

		//if the object is fixed, then we want the geometry to take into account the initial position and orientation of the rigid body
		if (body->isLocked() == true ){
			btCompoundShape * compoundshape = (btCompoundShape *)g;
			btTransform transform = compoundshape->getChildTransform(0);
			transform.setOrigin(btVector3(body->state.position.x, body->state.position.y, body->state.position.z));
			transform.setRotation(btQuaternion(body->state.orientation.v.x, body->state.orientation.v.y, body->state.orientation.v.z,body->state.orientation.s));
			compoundshape->updateChildTransform(0,transform);
			g = compoundshape;
		}

		btTransform transform;
		transform.setIdentity();
		cs->addChildShape(transform,g);
	}

	if (body->isLocked() == true){
		btCollisionObject * colobject = new btCollisionObject();
			colobject->setFriction(0.8);
			colobject->setRestitution(0.35);
			colobject->setCollisionShape(cs);
			worldID->addCollisionObject(colobject/*);//*/,COL_WALL,COL_BODY | COL_BOXES);
		for(uint j = 0; j < cs->getNumChildShapes();j++){
			bulletToRbs[index].collisionVolumes.push_back(cs->getChildShape(j));
		}
	} else {
		btTransform transform;
		transform.setIdentity();
		btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
		Vector3d principalMoments = bulletToRbs[index].rb->getPMI();
		btRigidBody::btRigidBodyConstructionInfo rbInfo(bulletToRbs[index].rb->getMass(),myMotionState,cs,btVector3(principalMoments.x,principalMoments.x,principalMoments.x));
		bulletToRbs[body->id].id = new btRigidBody(rbInfo);
		bulletToRbs[body->id].id->setFriction(0.8);
		bulletToRbs[body->id].id->setRestitution(0.35);
		if(body->isArticulated()){
			worldID->addRigidBody(bulletToRbs[body->id].id,COL_BODY,COL_WALL);
		} else{
			worldID->addRigidBody(bulletToRbs[body->id].id,COL_BOXES,COL_BOXES | COL_BODY | COL_WALL);
		}
	}
}


/**
	This methods creates an Bullet object and links it to the passed RigidBody
*/
void BulletWorld::linkRigidBodyToBullet( int index ) {
	//printf("BulletWorld::linkRigidBodyToBullet\n");
	RigidBody* rigidBody = bulletToRbs[index].rb; 

	if(!rigidBody->isLocked()){
		rigidBody->setBodyID( index );
	}
	//CREATE AND LINK THE Bullet BODY WITH OUR RIGID BODY
	//PROCESS THE COLLISION PRIMITIVES OF THE BODY
	createBulletCollisionPrimitives(rigidBody, index);


	//if the body is fixed, we'll only create the colission detection primitives
	if (!rigidBody->isLocked()){
		rigidBody->setBodyID( index );
		//set the data
		bulletToRbs[index].id->setUserPointer((void*) index);
	}

	//if this is a planar object, make sure we constrain it to always stay planar
	if (rigidBody->props.isPlanar){
		/*NxPointInPlaneJointDesc pipdesc;
		pipdesc.actor[0] = NULL;
		pipdesc.actor[1] = physxToRbs[index].id; 
		gScene->createJoint(pipdesc);*/
	}

	//SET THE INERTIAL PARAMETERS

	if (rigidBody->isLocked() == false){
		//set the mass and principal moments of inertia for this object
		//physxToRbs[index].id->setMass(physxToRbs[index].rb->getMass());
		//Vector3d principalMoments = physxToRbs[index].rb->getPMI();
		//physxToRbs[index].id->setMassSpaceInertiaTensor(NxVec3(principalMoments.x,principalMoments.y,principalMoments.z));
		setBulletStateFromRB(index);
	}
}


/**
	This method reads a list of rigid bodies from the specified file.
*/
void BulletWorld::loadRBsFromFile(char* fName){
	//printf("BulletWorld::loadRBsFromFile\n");
	//make sure we don't go over the old articulated figures in case this method gets called multiple times.
	int index = objects.size();
	int index_afs = AFs.size();

	World::loadRBsFromFile(fName);

	// Add all non-articulated rigid bodies in Bullet
	for (uint i=index;i<objects.size();i++){
		RigidBody* rigidBody = objects[i];
		// Add a placeholder in the odeToRbs mapping
		bulletToRbs.push_back(Bullet_RB_Map(0, rigidBody));
		if( !rigidBody->isArticulated() )
			linkRigidBodyToBullet(objects.size()-1);
	}

	DynamicArray<Joint*> joints;

	// Check every newly added articulated figures
	for (uint i=index_afs;i<AFs.size();i++){

		// For each, add the articulated bodies they contain to Bullet		
		for (uint j=0;j<objects.size();j++){
			if( !objects[j]->isArticulated() )
				continue;
			ArticulatedRigidBody* arb = (ArticulatedRigidBody*)objects[j];
			if( arb->getAFParent() == AFs[i] )
				linkRigidBodyToBullet(j);
		}

		//now we will go through all the new joints, and create and link their ode equivalents
		joints.clear();
		AFs[i]->addJointsToList(&joints);
		for (uint j=0;j<joints.size();j++){
			//connect the joint to the two bodies
			int jointType = joints[j]->getJointType();
			switch (jointType){
				case STIFF_JOINT:
					setupBulletFixedJoint((StiffJoint*)joints[j]);
					//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
					break;
				case BALL_IN_SOCKET_JOINT:
					setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
					break;
				case HINGE_JOINT:
					setupBulletHingeJoint((HingeJoint*)joints[j]);
					//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
					break;
				case UNIVERSAL_JOINT:
					setupBulletUniversalJoint((UniversalJoint*)joints[j]);
					//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
					break;
				default:
					throwError("Ooops.... Only BallAndSocket, Hinge, Universal and Stiff joints are currently supported.\n");
			}
		}
	}
}

/**
	This method adds one rigid body (not articulated).
*/
void BulletWorld::addRigidBody( RigidBody* rigidBody ) {
	//printf("BulletWorld::addRigidBody\n");
	World::addRigidBody(rigidBody);

	// Add a placeholder in the odeToRbs mapping
	bulletToRbs.push_back(Bullet_RB_Map(0, rigidBody));

	// Non-articulated rigid body are already well-positioned, link them to ODE
	if( !rigidBody->isArticulated() )
		linkRigidBodyToBullet(objects.size()-1);
	// For articulated rigid bodies, we will only add them when (and if) they appear in an ArticulatedFigure
}

/**
	This method adds one rigid body (not articulated).
*/
void BulletWorld::addArticulatedFigure(ArticulatedFigure* articulatedFigure){
	//printf("BulletWorld::addArticulatedFigure\n");
	World::addArticulatedFigure( articulatedFigure );

	// Add the articulated bodies contained into that figure to Bullet	
	for (uint j=0;j<objects.size();j++){
		if( !objects[j]->isArticulated() )
			continue;
		ArticulatedRigidBody* arb = (ArticulatedRigidBody*)objects[j];
		if( arb->getAFParent() == articulatedFigure )
			linkRigidBodyToBullet(j);
	}

	DynamicArray<Joint*> joints;

	//now we will go through all the new joints, and create and link their ode equivalents
	articulatedFigure->addJointsToList(&joints);
	for (uint j=0;j<joints.size();j++){
		//connect the joint to the two bodies
		int jointType = joints[j]->getJointType();
		switch (jointType){
			case STIFF_JOINT:
				setupBulletFixedJoint((StiffJoint*)joints[j]);
				//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				break;
			case BALL_IN_SOCKET_JOINT:
				setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				break;
			case HINGE_JOINT:
				//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				setupBulletHingeJoint((HingeJoint*)joints[j]);
				//setupPhysXHingeJoint((HingeJoint*)joints[j]);
				break;
			case UNIVERSAL_JOINT:
				//setupBulletBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				setupBulletUniversalJoint((UniversalJoint*)joints[j]);
				//setupPhysXUniversalJoint((UniversalJoint*)joints[j]);
				break;
			default:
				throwError("Ooops.... Only BallAndSocket, Hinge, Universal and Stiff joints are currently supported.\n");
		}
	}

	updateCharacterHeight();

	/*for (uint j=0;j<objects.size();j++){
		if( !objects[j]->isArticulated() )
			continue;
		for(uint k = 0; k<j;k++){
			if( !objects[k]->isArticulated() )
				continue;
			gScene->setActorPairFlags(*(physxToRbs[objects[j]->id].id),*(physxToRbs[objects[k]->id].id),NX_IGNORE_PAIR);
		}
	}*/
}

/**
	This method is used to set the state of all the rigid body in this collection.
*/
void BulletWorld::setState(DynamicArray<double>* state, int start){
	//printf("BulletWorld::setState\n");
	World::setState(state, start);
}

/**
	this method is used to transfer the state of the rigid bodies, from ODE to the rigid body wrapper
*/
void BulletWorld::setRBStateFromEngine(){
	//printf("BulletWorld::setRBStateFromEngine\n");
	//now update all the rigid bodies...
	for (uint i=0;i<objects.size();i++){
		setRBStateFromBullet(i);
//		objects[i]->updateToWorldTransformation();
	}
}

/**
	this method is used to transfer the state of the rigid bodies, from the rigid body wrapper to ODE's rigid bodies
*/
void BulletWorld::setEngineStateFromRB(){
	//printf("BulletWorld::setEngineStateFromRB\n");
	//now update all the rigid bodies...
	for (uint i=0;i<objects.size();i++){
		setBulletStateFromRB(i);
	}
}

/**
	This method is used to integrate the forward simulation in time.
*/
void BulletWorld::advanceInTime(double deltaT){
	/*countto1000++;
	LARGE_INTEGER ticksPerSecond;
	QueryPerformanceFrequency(&ticksPerSecond);
	LARGE_INTEGER tickbegin;
	LARGE_INTEGER tickend;
	LARGE_INTEGER time;
	QueryPerformanceFrequency(&ticksPerSecond);
	QueryPerformanceCounter(&tickbegin);*/

	//printf("BulletWorld::advanceInTime\n");
	if( deltaT <= 0 )
		return;

	//make sure that the state of the RB's is synchronized with the engine...
	setEngineStateFromRB();

	//restart the counter for the joint feedback terms
	jointFeedbackCount = 0;

	//go through all the rigid bodies in the world, and apply their external force
	for (uint j=0;j<objects.size();j++){
		if( objects[j]->isLocked() ) 
			continue;
		const Vector3d& f = objects[j]->externalForce;
		if( !f.isZeroVector() )
			bulletToRbs[objects[j]->id].id->applyCentralForce(btVector3(f.x, f.y, f.z));
		const Vector3d& t = /*Vector3d(0,0,1);//*/objects[j]->externalTorque;
		if( !t.isZeroVector() )
			//if(objects[j]->id == 1)
			bulletToRbs[objects[j]->id].id->applyTorque(btVector3(t.x, t.y, t.z));
		/*if(perturb && j == 3){
			//printf("code reached\n");
			nperturb++;
			bulletToRbs[objects[j]->id].id->applyCentralForce(btVector3(-200, 0, -150));
			if(nperturb == 400){
				nperturb = 0;
				perturb = false;
			}
		}*/
	}

	//go through all the joints in the world, and apply their torques to the parent and child rb's
	for (uint j=0;j<jts.size();j++){
		Vector3d t = jts[j]->torque;
		bulletToRbs[jts[j]->parent->id].id->applyTorque(btVector3((float)t.x,(float)t.y, (float)t.z));
		bulletToRbs[jts[j]->child->id].id->applyTorque(btVector3((float)-t.x,(float)-t.y, (float)-t.z));
	}

	//clear the previous list of contact forces
	contactPoints.clear();

	/*gScene->simulate(deltaT);
	gScene->flushStream();
	gScene->fetchResults(NX_RIGID_BODY_FINISHED, true);*/
	worldID->stepSimulation(deltaT,1,deltaT);


	if(!testmode)
		collisionsPostProcessing(deltaT);
	//copy over the state of the ODE bodies to the rigid bodies...
	setRBStateFromEngine();

	/*QueryPerformanceCounter(&tickend);
	after1000 += ((float)tickend.QuadPart - tickbegin.QuadPart)/ticksPerSecond.QuadPart;
	if(countto1000 >= 1000){
		printf("time of simulation : %f s\n",after1000/countto1000);
		after1000 = 0.0f;
		countto1000 = 0;
	}*/
}

/**
		This method is for performance analysis
	*/
void BulletWorld::printAllCOMPosition(){
	//printf("printing com position Bullet\n");
}

/**
	this method applies a force to a rigid body, at the specified point. The point is specified in local coordinates,
	and the force is also specified in local coordinates.
*/
void BulletWorld::applyRelForceTo(RigidBody* b, const Vector3d& f, const Point3d& p){
	//printf("BulletWorld::applyRelForceTo\n");
	if (!b)
		return;
	bulletToRbs[b->id].id->applyForce(btVector3(f.x, f.y, f.z),btVector3(p.x, p.y, p.z));
}

void BulletWorld::applyForceTo(RigidBody* b, const Vector3d& f, const Point3d& p){
	//printf("BulletWorld::applyForceTo\n");
	if (!b)
		return;
	bulletToRbs[b->id].id->applyForce(btVector3(f.x, f.y, f.z),btVector3(p.x, p.y, p.z));
}

/**
	this method applies a torque to a rigid body. The torque is specified in world coordinates.
*/
void BulletWorld::applyTorqueTo(RigidBody* b, const Vector3d& t){
	//printf("BulletWorld::applyTorqueTo\n");
	if (!b)
		return;
	bulletToRbs[b->id].id->applyTorque(btVector3(t.x, t.y, t.z));
	//dBodyAddTorque(odeToRbs[b->id].id, t.x, t.y, t.z);
}

void BulletWorld::drawAdditionalInformation(){
	//printf("add additional information for physxworld\n");
	/*DebugRenderer dr;
	const NxDebugRenderable *dbgRenderable=gScene->getDebugRenderable();
	dr.renderData(*dbgRenderable);*/
	GLboolean lighting = glIsEnabled(GL_LIGHTING);
	if (lighting)
		glDisable(GL_LIGHTING);

	worldID->debugDrawWorld();

	if (lighting)
		glEnable(GL_LIGHTING);
}

void BulletWorld::collisionsPostProcessing(btScalar dt){
	int numManifolds = worldID->getDispatcher()->getNumManifolds();
	for (int i=0;i<numManifolds;i++)
	{
		btPersistentManifold* contactManifold = worldID->getDispatcher()->getManifoldByIndexInternal(i);
		btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
		btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
	
		int numContacts = contactManifold->getNumContacts();
		for (int j=0;j<numContacts;j++)
		{
			btManifoldPoint& pt = contactManifold->getContactPoint(j);	
			btVector3 ptA = pt.getPositionWorldOnA();
			btVector3 ptB = pt.getPositionWorldOnB();
			contactPoints.push_back(ContactPoint());
			//printf("code reached 1\n");
			contactPoints[jointFeedbackCount].rb1 = (RigidBody*)obA->getRootCollisionShape()->getUserPointer();
			contactPoints[jointFeedbackCount].rb2 = (RigidBody*)obB->getRootCollisionShape()->getUserPointer();
			//printf("code reached 2\n");
			contactPoints[jointFeedbackCount].cp = Point3d(ptA.getX(),ptA.getY(),ptA.getZ());//i.getPoint().x, i.getPoint().y, i.getPoint().z);
			btVector3 force = pt.m_appliedImpulse * pt.m_normalWorldOnB /dt;
			Vector3d contactforce(force.getX(),force.getY(),force.getZ()); 
			contactPoints[jointFeedbackCount].f = contactforce;
			//printf("code reached 3\n");
			/*if(contactPoints[jointFeedbackCount].rb1 == NULL){
				printf("rb1 null\n");
			}
			if(contactPoints[jointFeedbackCount].rb2 == NULL){
				printf("rb2 null\n");
			}*/
			if (contactPoints[jointFeedbackCount].rb1->isLocked() && !contactPoints[jointFeedbackCount].rb2->isLocked()){
				contactPoints[jointFeedbackCount].f = contactPoints[jointFeedbackCount].f * (-1);
				RigidBody* tmpBdy = contactPoints[jointFeedbackCount].rb1;
				contactPoints[jointFeedbackCount].rb1 = contactPoints[jointFeedbackCount].rb2;
				contactPoints[jointFeedbackCount].rb2 = tmpBdy;
			}
			jointFeedbackCount++;
		}

		//you can un-comment out this line, and then all points are removed
		contactManifold->clearManifold();	
	}
}

void BulletWorld::drawBoxes(){
	for(int i = 0; i < 50;i++){
		for(int j = 0;j < 50;j++){
			Point3d p1(-0.125,-0.125,-0.125);
			Point3d p2(0.125,0.125,0.125);
			BoxCDP * bcdp =  new BoxCDP(p1,p2);
			RigidBody * rb = new RigidBody();
			rb->addCollisionDetectionPrimitive(bcdp);
			rb->state.position.x =2;
			rb->state.position.y =0.125 + 0.3 * i;
			rb->state.position.z = - (0.3 * 50 / 2) + 0.3 * j ;
			this->addRigidBody(rb);
		}
	}
}

#endif