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

#include "../compileconfig.h"
#ifdef PhysX
#include "PhysXWorld.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
*/
PhysXWorld::PhysXWorld() : World(){
	setupWorld();
}

/**
	destructor
*/
PhysXWorld::~PhysXWorld(void){
}

void PhysXWorld::destroyWorld() {
	//delete[] cps;
	delete pcQuery;
	physxToRbs.clear();
	//destroy the PhysX physical world, simulation space and joint group
	if(gPhysicsSDK != NULL)
	{
		if(gScene != NULL) gPhysicsSDK->releaseScene(*gScene);
		gScene = NULL;
		NxReleasePhysicsSDK(gPhysicsSDK);
		gPhysicsSDK = NULL;
	}
	World::destroyWorld();
}

#define min(x, y) (((x)>(y))?(y):(x))

/**
	this method is used to process the collision between the two objects passed in as parameters. More generally,
	it is used to determine if the collision should take place, and if so, it calls the method that generates the
	contact points.
*/
void PhysXWorld::collisionsPostProcessing(NxContactPair & pair, NxU32 events){
	// Iterate through contact points
	NxContactStreamIterator i(pair.stream);
	//user can call getNumPairs() here
	while(i.goNextPair())
	{
		NxShape *o1,*o2;
		o1 = i.getShape(0);
		o2 = i.getShape(1);

		NxActor *b1, *b2;
		RigidBody *rb1, *rb2;
		b1 = &(o1->getActor());
		b2 = &(o2->getActor());
		rb1 = (RigidBody*) o1->userData;
		rb2 = (RigidBody*) o2->userData;

		//we'll use the minimum of the two coefficients of friction of the two bodies.
		double mu1 = rb1->getFrictionCoefficient();
		double mu2 = rb2->getFrictionCoefficient();
		double mu_to_use = min(mu1, mu2);
		double eps1 = rb1->getRestitutionCoefficient();
		double eps2 = rb2->getRestitutionCoefficient();
		double eps_to_use = min(eps1, eps2);

		double groundSoftness = 0, groundPenalty = 0;
		if (rb1){
			groundSoftness = rb1->props.groundSoftness;
			groundPenalty = rb1->props.groundPenalty;
		}else{
			groundSoftness = rb2->props.groundSoftness;
			groundPenalty = rb2->props.groundPenalty;
		}
			
		//int j = 0;
		//user can also call getShape() and getNumPatches() here
		while(i.goNextPatch())
		{
			//user can also call getPatchNormal() and getNumPoints() here
			const NxVec3& contactNormal = i.getPatchNormal();
			while(i.goNextPoint())
			{
				//user can also call getPoint() and getSeparation() here
				const NxVec3& contactPoint = i.getPoint();
				if (jointFeedbackCount >= MAX_CONTACT_FEEDBACK){}
					//tprintf("Warning: too many contacts are established. Some of them will not be reported.\n");
				else{
					if (contactPoints.size() != jointFeedbackCount){
						//tprintf("Warning: Contact forces need to be cleared after each simulation, otherwise the results are not predictable.\n");
					}
					contactPoints.push_back(ContactPoint());
					//now we'll set up the feedback for this contact joint
					contactPoints[jointFeedbackCount].rb1 = rb1;
					contactPoints[jointFeedbackCount].rb2 = rb2;
					contactPoints[jointFeedbackCount].cp = Point3d(i.getPoint().x, i.getPoint().y, i.getPoint().z);
					NxReal pointnormalforce = i.getPointNormalForce();
					NxVec3 force = contactNormal * i.getPointNormalForce(); 
					Vector3d contactforce(force.x,force.y,force.z); 
					contactPoints[jointFeedbackCount].f = contactforce;
					//make sure that the force always points away from the static objects
					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++;
				}
			}
		}
	}
}

class MyContactReport : public NxUserContactReport
{
	public:
	PhysXWorld * theworld;

	virtual void  onContactNotify(NxContactPair& pair, NxU32 events)
	{
		if(!theworld->testmode)
			theworld->collisionsPostProcessing(pair,events);
		
	}

}gMyContactReport;

void PhysXWorld::setupWorld() {
	int maxCont = 4;

	NxPhysicsSDKDesc desc;
	//desc.flags &= ~NX_SDKF_NO_HARDWARE;
	NxSDKCreateError errorCode = NXCE_NO_ERROR;
	gPhysicsSDK = NxCreatePhysicsSDK(NX_PHYSICS_SDK_VERSION, NULL, NULL, desc, &errorCode);
	gPhysicsSDK->setParameter( NX_VISUALIZATION_SCALE, 0.25f);
	gPhysicsSDK->setParameter( NX_VISUALIZE_COLLISION_SHAPES, 0.25f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_JOINT_WORLD_AXES, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_JOINT_LOCAL_AXES, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_JOINT_LIMITS, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_BODY_JOINT_GROUPS, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_BODY_AXES, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_BODY_MASS_AXES, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_BODY_ANG_VELOCITY, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_CONTACT_POINT, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_CONTACT_FORCE, 1.0f);
	//gPhysicsSDK->setParameter( NX_VISUALIZE_CONTACT_NORMAL, 1.0f);
	// the ammount of interpenetration allowed between objects???
	gPhysicsSDK->setParameter(NX_SKIN_WIDTH, 0.001);
	gPhysicsSDK->setParameter(NX_BOUNCE_THRESHOLD, 0);
	/*if(gPhysicsSDK->getHWVersion() != NX_HW_VERSION_NONE){
		printf("code reached\n");
	} else {
		printf("no acceleration available\n");
	}*/

	// Create a scene
	NxSceneDesc sceneDesc;

	//set gravity
	Vector3d gravity = PhysicsGlobals::up * PhysicsGlobals::gravity;
	sceneDesc.gravity = NxVec3(gravity.x, gravity.y, gravity.z);
	
	sceneDesc.userContactReport	= &gMyContactReport;
	gScene = gPhysicsSDK->createScene(sceneDesc);
	gMyContactReport.theworld = this;
	gScene->setUserContactReport(&gMyContactReport);
	gPhysicsSDK->getFoundationSDK().getRemoteDebugger()->connect("localhost", 5425, NX_DBG_EVENTMASK_EVERYTHING);

	
	// Set default material
	NxMaterial* defaultMaterial = gScene->getMaterialFromIndex(0);
	defaultMaterial->setRestitution(0.35f);
	defaultMaterial->setStaticFriction(0.8f);
	defaultMaterial->setDynamicFriction(0.8f);

	maxContactCount = maxCont;

	pcQuery = NULL;

	pcQuery = new PreCollisionQuery();
}

void PhysXWorld::destroyAllObjects() {
	destroyWorld();
	setupWorld();
	if(testmode){
		this->drawBoxes();
	}
}



/**
	this method is used to copy the state of the ith rigid body to its PhysX counterpart.
*/
void PhysXWorld::setPhysXStateFromRB(int i){
	if (i<0 || (uint)i>=physxToRbs.size())
		return;

	//if it is a locked object, we update its CDPS
	if (physxToRbs[i].rb->isLocked() == true) {

		for (uint j=0;j<physxToRbs[i].collisionVolumes.size();j++){
			NxShape * t = physxToRbs[i].collisionVolumes[j];
			t->setGlobalPosition(NxVec3(physxToRbs[i].rb->state.position.x, physxToRbs[i].rb->state.position.y, physxToRbs[i].rb->state.position.z));
			NxQuat q;
			q.setw(physxToRbs[i].rb->state.orientation.s);
			q.setx(physxToRbs[i].rb->state.orientation.v.x);
			q.sety(physxToRbs[i].rb->state.orientation.v.y);
			q.setz(physxToRbs[i].rb->state.orientation.v.z);
			t->setGlobalOrientation(q);
		}
		return;
	}
	
	physxToRbs[i].id->setGlobalPosition(NxVec3(physxToRbs[i].rb->state.position.x, physxToRbs[i].rb->state.position.y, physxToRbs[i].rb->state.position.z));
	physxToRbs[i].id->setGlobalOrientation(NxQuat(NxVec3(physxToRbs[i].rb->state.orientation.v.x,physxToRbs[i].rb->state.orientation.v.y,physxToRbs[i].rb->state.orientation.v.z),physxToRbs[i].rb->state.orientation.s));
	physxToRbs[i].id->setLinearVelocity(NxVec3(physxToRbs[i].rb->state.velocity.x, physxToRbs[i].rb->state.velocity.y, physxToRbs[i].rb->state.velocity.z));
	physxToRbs[i].id->setAngularVelocity(NxVec3(physxToRbs[i].rb->state.angularVelocity.x, physxToRbs[i].rb->state.angularVelocity.y, physxToRbs[i].rb->state.angularVelocity.z));
}

/**
	this method is used to copy the state of the ith rigid body, from the PhysX object to its rigid body counterpart 
*/
void PhysXWorld::setRBStateFromPhysX(int i){
	NxVec3 tempData;

	//if it is a locked object, we won't do anything about it
	if (physxToRbs[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 (physxToRbs[i].rb->props.isPlanar){
		printf("rb planar\n");
	   NxVec3 rot = physxToRbs[i].id->getAngularVelocity();
	   NxQuat quat_ptr;
	   NxQuat quat;
	   NxReal quat_len;
	   quat_ptr = physxToRbs[i].id->getGlobalOrientationQuat();
	   quat.w = quat_ptr.w;
	   quat.x = quat_ptr.x;
	   quat.y = 0; 
	   quat.z = 0; 
	   quat_len = sqrt( quat.w * quat.w + quat.x * quat.x );
	   quat.w /= quat_len;
	   quat.x /= quat_len;
	   physxToRbs[i].id->setGlobalOrientation(quat);
	   rot.y = 0;
	   rot.z = 0;
	   physxToRbs[i].id->setAngularVelocity(rot);
	}
	//printf("before x : %f\n",physxToRbs[i].rb->state.position.x);
	//printf("before y : %f\n",physxToRbs[i].rb->state.position.y);
	//printf("before z : %f\n",physxToRbs[i].rb->state.position.z);

	tempData = physxToRbs[i].id->getGlobalPosition();
	physxToRbs[i].rb->state.position.x = tempData.x;
	physxToRbs[i].rb->state.position.y = tempData.y;
    physxToRbs[i].rb->state.position.z = tempData.z;

	//printf("after x : %f\n",physxToRbs[i].rb->state.position.x);
	//printf("after y : %f\n",physxToRbs[i].rb->state.position.y);
	//printf("after z : %f\n",physxToRbs[i].rb->state.position.z);

	NxQuat orientation;
	orientation = physxToRbs[i].id->getGlobalOrientationQuat();
	physxToRbs[i].rb->state.orientation.s = orientation.w;
	physxToRbs[i].rb->state.orientation.v.x = orientation.x;
	physxToRbs[i].rb->state.orientation.v.y = orientation.y;
	physxToRbs[i].rb->state.orientation.v.z = orientation.z;

	tempData = physxToRbs[i].id->getLinearVelocity();
	physxToRbs[i].rb->state.velocity.x = tempData.x;
	physxToRbs[i].rb->state.velocity.y = tempData.y;
	physxToRbs[i].rb->state.velocity.z = tempData.z;

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

/**
	this method is used to set up a PhysX plane shape. 
*/
NxPlaneShapeDesc PhysXWorld::getPlaneGeom(PlaneCDP* p, RigidBody* parent){
	//and create the ground plane
	NxPlaneShapeDesc g;
	Vector3d n = parent->getWorldCoordinates(p->getNormal());
	Vector3d o = /*Vector3d(0,-10,0);//*/Vector3d(parent->getWorldCoordinates(p->getOrigin()));
	g.normal = NxVec3(n.x,n.y,n.z);
	g.d = o.dotProductWith(n);
	return g;
}

/**
	this method is used to set up a PhysX sphere shape. It is properly placed in body coordinates.
*/
NxSphereShapeDesc PhysXWorld::getSphereGeom(SphereCDP* s){
	NxSphereShapeDesc g;
	//g.group = 0;
	Point3d c = s->getCenter();
	g.radius = s->getRadius();
	g.localPose.t = NxVec3(c.x,c.y,c.z);
	return g;
}

/**
	this method is used to set up a PhysX box shape. It is properly placed in body coordinates.
*/
NxBoxShapeDesc PhysXWorld::getBoxGeom(BoxCDP* b){
	/*Point3d a1 = b->getPoint1();
	Point3d b1 = b->getPoint2();
	Vector3d a1b1(a1, b1);*/
	NxBoxShapeDesc g;
	//g.group = 0;
	g.dimensions = NxVec3(b->getXLen() / 2, b->getYLen() / 2, b->getZLen() / 2);
	Point3d c = b->getCenter();
	g.localPose.t = NxVec3(c.x, c.y, c.z);

	return g;
}

/**
	this method is used to set up a PhysX capsule shape. It is properly placed in body coordinates.
*/
NxCapsuleShapeDesc PhysXWorld::getCapsuleGeom(CapsuleCDP* c){
	//printf("code reached j\n");
	Point3d a = c->getPoint1();
	Point3d b = c->getPoint2();
	Vector3d ab(a, b);
	NxCapsuleShapeDesc g;
	//g.group = 0;
	g.radius = c->getRadius();
	g.height = ab.length();
	
	Point3d cen = a + ab/2.0;
	g.localPose.t = NxVec3(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);

	g.localPose.M = NxQuat(NxVec3(relOrientation.v.x,relOrientation.v.y,relOrientation.v.z),relOrientation.s);

	return g;
}

/**
	This method is used to set up a PhysX fixed joint, based on the information in the fixed joint passed in as a parameter
*/
void PhysXWorld::setupPhysXFixedJoint(StiffJoint* hj){
	//printf("code reached k\n");
	NxFixedJointDesc fixedDesc;
	fixedDesc.actor[0] = physxToRbs[(int)(hj->parent->id)].id;
	fixedDesc.actor[1] = physxToRbs[(int)(hj->child->id)].id;

	if(fixedDesc.actor[0] == NULL){
		printf("actor 0 null\n");
	}
	if(fixedDesc.actor[1] == NULL){
		printf("actor 1 null\n");
	}

	NxFixedJoint *fixedJoint=(NxFixedJoint*)gScene->createJoint(fixedDesc);
	if(fixedJoint == NULL){
		printf("Failed to create fixed joint\n");
	}
}

/**
	This method is used to set up a PhysX hinge joint, based on the information in the hinge joint passed in as a parameter
*/
void PhysXWorld::setupPhysXHingeJoint(HingeJoint* hj){
	/*NxD6JointDesc d6Desc;
	d6Desc.actor[0] = physxToRbs[(int)(hj->child->id)].id;
	d6Desc.actor[1] = physxToRbs[(int)(hj->parent->id)].id;
	d6Desc.xMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.yMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.zMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.twistMotion = NX_D6JOINT_MOTION_FREE;	
	d6Desc.swing1Motion = NX_D6JOINT_MOTION_LOCKED;	
	d6Desc.swing2Motion = NX_D6JOINT_MOTION_LOCKED;
	Point3d p = hj->child->getWorldCoordinates(hj->cJPos);
	d6Desc.setGlobalAnchor(NxVec3(p.x,p.y,p.z));
	Vector3d a = hj->parent->getWorldCoordinates(hj->a);
	d6Desc.setGlobalAxis(NxVec3(a.x,a.y,a.z));
	NxD6Joint * d6Joint=(NxD6Joint*)gScene->createJoint(d6Desc);*/

	NxRevoluteJointDesc revoluteDesc;
	revoluteDesc.actor[0] = physxToRbs[(int)(hj->parent->id)].id;
	revoluteDesc.actor[1] = physxToRbs[(int)(hj->child->id)].id;
	Point3d p = hj->child->getWorldCoordinates(hj->cJPos);
	revoluteDesc.setGlobalAnchor(NxVec3(p.x,p.y,p.z));
	Vector3d a = hj->parent->getWorldCoordinates(hj->a);
	revoluteDesc.setGlobalAxis(NxVec3(a.x,a.y,a.z));

	if(hj->useJointLimits == true && !testmode){
		revoluteDesc.flags |= NX_RJF_LIMIT_ENABLED;
		//revoluteDesc.jointFlags = !NX_JF_VISUALIZATION;
		revoluteDesc.limit.low.value = -hj->maxAngle;
		revoluteDesc.limit.high.value = -hj->minAngle;
		//printf("%f %f\n",hj->minAngle,hj->maxAngle);
	}

	NxRevoluteJoint * revoluteJoint = (NxRevoluteJoint *) gScene->createJoint(revoluteDesc);
}


/**
	This method is used to set up a PhysX universal joint, based on the information in the universal joint passed in as a parameter
*/
void PhysXWorld::setupPhysXUniversalJoint(UniversalJoint* uj){
	//printf("code reached m\n");
	NxD6JointDesc d6Desc;
	d6Desc.actor[0] = physxToRbs[(int)(uj->parent->id)].id;
	d6Desc.actor[1] = physxToRbs[(int)(uj->child->id)].id;
	Point3d p = uj->parent->getWorldCoordinates(uj->pJPos);
	d6Desc.setGlobalAnchor(NxVec3(p.x,p.y,p.z));
	Vector3d b = uj->child->getWorldCoordinates(uj->b);
	d6Desc.setGlobalAxis(NxVec3(b.x,b.y,b.z));
	d6Desc.xMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.yMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.zMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.twistMotion = NX_D6JOINT_MOTION_FREE;	
	d6Desc.swing1Motion = NX_D6JOINT_MOTION_FREE;	
	d6Desc.swing2Motion = NX_D6JOINT_MOTION_LOCKED;

	if(uj->useJointLimits == true && !testmode){
		Vector3d a = (uj->a);
		d6Desc.localNormal[0] = NxVec3(a.x,a.y,a.z);
		d6Desc.localNormal[1] = NxVec3(a.x,a.y,a.z);
		d6Desc.twistMotion = NX_D6JOINT_MOTION_LIMITED;	
		d6Desc.swing1Motion = NX_D6JOINT_MOTION_LIMITED;	
		d6Desc.swing2Motion = NX_D6JOINT_MOTION_LOCKED;
		d6Desc.twistLimit.low.value = (uj->minAngleB);
		d6Desc.twistLimit.high.value = (uj->maxAngleB);
		d6Desc.swing1Limit.value = uj->maxAngleA;
	}

	NxD6Joint * d6Joint=(NxD6Joint*)gScene->createJoint(d6Desc);
	//printf("create universal joint\n");
}

/**
	This method is used to set up a PhysX ball-and-socket joint, based on the information in the ball in socket joint passed in as a parameter
*/
void PhysXWorld::setupPhysXBallAndSocketJoint(BallInSocketJoint* basj){
	NxD6JointDesc d6Desc;
	d6Desc.actor[0] = physxToRbs[(int)(basj->parent->id)].id;
	d6Desc.actor[1] = physxToRbs[(int)(basj->child->id)].id;
	d6Desc.xMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.yMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.zMotion = NX_D6JOINT_MOTION_LOCKED;
	d6Desc.twistMotion = NX_D6JOINT_MOTION_FREE;	
	d6Desc.swing1Motion = NX_D6JOINT_MOTION_FREE;	
	d6Desc.swing2Motion = NX_D6JOINT_MOTION_FREE;
	Point3d p = basj->child->getWorldCoordinates(basj->cJPos);
	d6Desc.setGlobalAnchor(NxVec3(p.x,p.y,p.z));
	
	if(basj->useJointLimits == true && !testmode){
		//d6Desc.jointFlags = !NX_JF_VISUALIZATION;
		d6Desc.twistMotion = NX_D6JOINT_MOTION_LIMITED;	
		if(basj->maxTwistAngle > 2 * 22 / 7){
			d6Desc.twistMotion = NX_D6JOINT_MOTION_FREE;
		}
		d6Desc.swing1Motion = NX_D6JOINT_MOTION_LIMITED;	
		d6Desc.swing2Motion = NX_D6JOINT_MOTION_LIMITED;
		Vector3d a = basj->parent->getWorldCoordinates(basj->swingAxis1);
		Vector3d b = basj->child->getWorldCoordinates(basj->twistAxis);
		d6Desc.setGlobalAxis(NxVec3(b.x,b.y,b.z));
		d6Desc.localNormal[0] = (NxVec3(a.x,a.y,a.z));
		d6Desc.localNormal[1] = (NxVec3(a.x,a.y,a.z));
		d6Desc.twistLimit.low.value = (basj->minTwistAngle);
		d6Desc.twistLimit.high.value = (basj->maxTwistAngle);
		d6Desc.swing1Limit.value = basj->maxSwingAngle2;
		d6Desc.swing2Limit.value = basj->maxSwingAngle2;
		//printf("twist limit %f %f\n",NxMath::degToRad(basj->minTwistAngle),NxMath::degToRad(basj->maxTwistAngle));
		//printf("swing 1 limit %f %f\n",basj->minSwingAngle1,basj->maxSwingAngle1);
		//printf("swing 2 limit %f %f\n",basj->minSwingAngle2,basj->maxSwingAngle2);
	}

	//Vector3d a = basj->getTwistAxis();//parent->getWorldCoordinates(basj->a);
	//d6Desc.setGlobalAxis(NxVec3(a.x,a.y,a.z));
	NxD6Joint * d6Joint=(NxD6Joint*)gScene->createJoint(d6Desc);


	/*NxSphericalJointDesc sphericalDesc;
	sphericalDesc.actor[0] = physxToRbs[(int)(basj->parent->id)].id;
	sphericalDesc.actor[1] = physxToRbs[(int)(basj->child->id)].id;
	Point3d p = basj->child->getWorldCoordinates(basj->cJPos);
	sphericalDesc.setGlobalAnchor(NxVec3(p.x,p.y,p.z));

	if(basj->useJointLimits == true){
		sphericalDesc.flags |= NX_SJF_TWIST_LIMIT_ENABLED;
		sphericalDesc.twistLimit.low.value = basj->maxTwistAngle;
		sphericalDesc.twistLimit.high.value = basj->minTwistAngle;
		
	}

	//sphericalDesc.projectionMode = NX_JPM_LINEAR_MINDIST;
	//sphericalDesc.projectionDistance = 0.01f;    
	//sphericalDesc.projectionAngle = 0.0872f;
	//Vector3d a = basj->getTwistAxis();//parent->getWorldCoordinates(basj->a);
	//sphereDesc.setGlobalAxis(NxVec3(a.x,a.y,a.z));
	NxSphericalJoint * sphericalJoint = (NxSphericalJoint *) gScene->createJoint(sphericalDesc);
	if(sphericalJoint == NULL){
		printf("Failed to create spherical joint\n");
	}*/
}

/**
	this method is used to create PhysX shapes for all the collision primitives of the rigid body that is passed in as a paramter
*/
void PhysXWorld::createPhysXCollisionPrimitives(RigidBody* body, int index){
	//printf("code reached o\n");
	//now we'll set up the body's collision detection primitives
	
	NxActorDesc actordesc;
	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.
		NxShapeDesc * g;

		if(cdpType == SPHERE_CDP){
			//NxSphereShapeDesc 
			g = &getSphereGeom((SphereCDP*)body->cdps[j]);			
		} else if(cdpType == CAPSULE_CDP){
			//NxCapsuleShapeDesc 
			g = &getCapsuleGeom((CapsuleCDP*)body->cdps[j]);
		} else if(cdpType == BOX_CDP){
			//NxBoxShapeDesc 
			g = &getBoxGeom((BoxCDP*)body->cdps[j]);
		} else if(cdpType == PLANE_CDP){
			//NOTE: only static objects can have planes as their collision primitives - if this isn't static, force it!!
			//NxPlaneShapeDesc 
			g = &getPlaneGeom((PlaneCDP*)body->cdps[j], body);
		} else {
			throwError("Ooppps... No collision detection primitive was created rb: %s, cdp: %d", body->name, j);
		}	
		
		g->userData = 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){
			actordesc.shapes.pushBack(g);
			//gScene->createActor(actordesc);
			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){
			g->localPose.t = NxVec3(body->state.position.x, body->state.position.y, body->state.position.z);
			//dGeomSetPosition(t, body->state.position.x, body->state.position.y, body->state.position.z);
			//dQuaternion q;
			g->localPose.M = NxQuat(NxVec3(body->state.orientation.v.x, body->state.orientation.v.y, body->state.orientation.v.z),body->state.orientation.s);
			// Save collision volumes for eventual updates
		}
		actordesc.shapes.pushBack(g);
	}

	if (body->isLocked() == true){
		NxActor * temp = gScene->createActor(actordesc);
		for(uint j = 0; j < temp->getNbShapes();j++){
			physxToRbs[index].collisionVolumes.push_back(temp->getShapes()[j]);
		}
	} else {
		NxBodyDesc bodydesc;
		
		//bodydesc.angularDamping = 0.5f;
		//bodydesc.linearVelocity = NxVec3(0.0f,0.0f,0.0f);
		//actordesc.body = &bodydesc;
		//actordesc.density = 10.0f;
		bodydesc.mass = physxToRbs[index].rb->getMass();
		Vector3d principalMoments = physxToRbs[index].rb->getPMI();
		bodydesc.massSpaceInertia = NxVec3(principalMoments.x,principalMoments.y,principalMoments.z);
		//bodydesc.maxAngularVelocity = 7;
		actordesc.body = &bodydesc;
		physxToRbs[body->id].id = gScene->createActor(actordesc);
		physxToRbs[body->id].id->setContactReportFlags(NX_NOTIFY_ON_START_TOUCH_FORCE_THRESHOLD | NX_NOTIFY_ON_TOUCH |NX_NOTIFY_ON_END_TOUCH_FORCE_THRESHOLD );
		physxToRbs[body->id].id->setContactReportThreshold(100);
		physxToRbs[body->id].id->setContactReportThreshold(0);
		if(physxToRbs[body->id].id == NULL){
			printf("create actor failed\n");
		}
	}
}

/**
	This method reads a list of rigid bodies from the specified file.
*/
void PhysXWorld::loadRBsFromFile(char* fName){
	//printf("code reached p\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 ODE
	for (uint i=index;i<objects.size();i++){
		RigidBody* rigidBody = objects[i];
		// Add a placeholder in the odeToRbs mapping
		physxToRbs.push_back(PhysX_RB_Map(0, rigidBody));
		if( !rigidBody->isArticulated() )
			linkRigidBodyToPhysX(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 PhysX	
		for (uint j=0;j<objects.size();j++){
			if( !objects[j]->isArticulated() )
				continue;
			ArticulatedRigidBody* arb = (ArticulatedRigidBody*)objects[j];
			if( arb->getAFParent() == AFs[i] )
				linkRigidBodyToPhysX(j);
		}

		//now we will go through all the new joints, and create and link their PhysX 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:
					setupPhysXFixedJoint((StiffJoint*)joints[j]);
					break;
				case BALL_IN_SOCKET_JOINT:
					setupPhysXBallAndSocketJoint((BallInSocketJoint*)joints[j]);
					break;
				case HINGE_JOINT:
					setupPhysXHingeJoint((HingeJoint*)joints[j]);
					break;
				case UNIVERSAL_JOINT:
					setupPhysXUniversalJoint((UniversalJoint*)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 PhysXWorld::addRigidBody( RigidBody* rigidBody ) {
//printf("code reached q\n");
	World::addRigidBody(rigidBody);

	// Add a placeholder in the physxToRbs mapping
	physxToRbs.push_back(PhysX_RB_Map(0, rigidBody));

	// Non-articulated rigid body are already well-positioned, link them to PhysX
	if( !rigidBody->isArticulated() )
		linkRigidBodyToPhysX(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 PhysXWorld::addArticulatedFigure(ArticulatedFigure* articulatedFigure){
	World::addArticulatedFigure( articulatedFigure );

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

	DynamicArray<Joint*> joints;

	//now we will go through all the new joints, and create and link their PhysX 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:
				setupPhysXFixedJoint((StiffJoint*)joints[j]);
				break;
			case BALL_IN_SOCKET_JOINT:
				setupPhysXBallAndSocketJoint((BallInSocketJoint*)joints[j]);
				break;
			case HINGE_JOINT:
				setupPhysXHingeJoint((HingeJoint*)joints[j]);
				//setupPhysXHingeJoint((HingeJoint*)joints[j]);
				break;
			case UNIVERSAL_JOINT:
				setupPhysXUniversalJoint((UniversalJoint*)joints[j]);
				//setupPhysXUniversalJoint((UniversalJoint*)joints[j]);
				break;
			default:
				throwError("Ooops.... Only BallAndSocket, Hinge, Universal and Stiff joints are currently supported.\n");
		}
	}

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


/**
	This methods creates an PhysX object and links it to the passed RigidBody
*/
void PhysXWorld::linkRigidBodyToPhysX( int index ) {
	//printf("code reached s\n");
	RigidBody* rigidBody = physxToRbs[index].rb; 

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


	//if the body is fixed, we'll only create the colission detection primitives
	if (!rigidBody->isLocked()){
		rigidBody->setBodyID( index );
		//set the data
		physxToRbs[index].id->userData = (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));
		setPhysXStateFromRB(index);
	}
}


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


/**
	This method is a simple call back function that passes the message to the world whose objects are being acted upon. 
*/
void collisionCallBack(void* physxWorld, NxShape * o1, NxShape * o2){
	//printf("code reached v\n");
	//((PhysXWorld*)physxWorld)->processCollisions(o1,o2);
}

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

	//printf("code reached x\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() )
			physxToRbs[objects[j]->id].id->addForce(NxVec3(f.x, f.y, f.z));
		const Vector3d& t = /*Vector3d(0,0,1);//*/objects[j]->externalTorque;
		if( !t.isZeroVector() )
			//if(objects[j]->id == 1)
			physxToRbs[objects[j]->id].id->addTorque(NxVec3(t.x, t.y, t.z));
		/*if(perturb && j == 3){
			//printf("code reached\n");
			nperturb++;
			physxToRbs[objects[j]->id].id->addForce(NxVec3(-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;
		physxToRbs[jts[j]->parent->id].id->addTorque(NxVec3((float)t.x,(float)t.y, (float)t.z));
		physxToRbs[jts[j]->child->id].id->addTorque(NxVec3((float)-t.x,(float)-t.y, (float)-t.z));
	}

	//clear the previous list of contact forces
	contactPoints.clear();
	gScene->setTiming(deltaT, 1, NX_TIMESTEP_FIXED);
	gScene->simulate(deltaT);
	gScene->flushStream();
	gScene->fetchResults(NX_RIGID_BODY_FINISHED, true);

	//copy over the state of the PhysX bodies to the rigid bodies...
	setRBStateFromEngine();

	//printf("%d\n",countto1000);
	/*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 PhysXWorld::printAllCOMPosition(){
	//printf("printing com position PhysX\n");
}

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

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

/**
	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 PhysXWorld::applyRelForceTo(RigidBody* b, const Vector3d& f, const Point3d& p){
	printf("code reached a1\n");
	if (!b)
		return;
	physxToRbs[b->id].id->addLocalForceAtLocalPos(NxVec3(f.x, f.y, f.z),NxVec3(p.x, p.y, p.z));
}

/**
	this method applies a force to a rigid body, at the specified point. The point is specified in local coordinates,
	and the force is specified in world coordinates.
*/
void PhysXWorld::applyForceTo(RigidBody* b, const Vector3d& f, const Point3d& p){
	printf("code reached a2\n");
	if (!b)
		return;
	physxToRbs[b->id].id->addForceAtLocalPos(NxVec3(f.x, f.y, f.z),NxVec3(p.x, p.y, p.z));
}

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


void PhysXWorld::drawAdditionalInformation(){
	//printf("add additional information for physxworld\n");
	DebugRenderer dr;
	const NxDebugRenderable *dbgRenderable=gScene->getDebugRenderable();
	dr.renderData(*dbgRenderable);
}

void PhysXWorld::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);
		}
	}
	/*NxBoxShapeDesc g;
	g.dimensions = NxVec3(0.5, 0.5, 0.5);
	g.localPose.t = NxVec3(2, 2, 2);
	NxBodyDesc bodydesc;
	bodydesc.mass = 1;
	NxActorDesc ad;
	ad.body = &bodydesc;
	ad.shapes.pushBack(&g);
	NxActor * newact = gScene->createActor(ad);*/
	
}
#endif
