
// zlib open source license
//
// Copyright (c) 2010 to 2013 David Forsgren Piuva
// 
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// 
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 
//    1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 
//    2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 
//    3. This notice may not be removed or altered from any source
//    distribution.

int CDFPGECtrl::RigidBody_Create_Static(int CollisionShape,float Pos_X,float Pos_Y,float Pos_Z,float XAxis_X,float XAxis_Y,float XAxis_Z,float YAxis_X,float YAxis_Y,float YAxis_Z) {
	int Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(CollisionShape,CollisionShape)
		if BAD_REF(CollisionShape) {
			REPORT_TYPE(CollisionShape,L"RigidBody_Create_Static",CollisionShape)
			Result = 0;
		} else {
			Result = DGE.IDFromPointer(DGE.RigidBody_Create(pCollisionShape,0.0f,DVector3(Pos_X,Pos_Y,Pos_Z),DVector3(XAxis_X,XAxis_Y,XAxis_Z),DVector3(YAxis_X,YAxis_Y,YAxis_Z),false,false));
		}
	)
	return Result;
}
int CDFPGECtrl::RigidBody_Create_Kinematic(int CollisionShape,float Pos_X,float Pos_Y,float Pos_Z,float XAxis_X,float XAxis_Y,float XAxis_Z,float YAxis_X,float YAxis_Y,float YAxis_Z) {
	int Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(CollisionShape,CollisionShape)
		if BAD_REF(CollisionShape) {
			REPORT_TYPE(CollisionShape,L"RigidBody_Create_Kinematic",CollisionShape)
			Result = 0;
		} else {
			Result = DGE.IDFromPointer(DGE.RigidBody_Create(pCollisionShape,0.0f,DVector3(Pos_X,Pos_Y,Pos_Z),DVector3(XAxis_X,XAxis_Y,XAxis_Z),DVector3(YAxis_X,YAxis_Y,YAxis_Z),false,true));
		}
	)
	return Result;
}
int CDFPGECtrl::RigidBody_Create_Dynamic(int CollisionShape, float Mass,float Pos_X,float Pos_Y,float Pos_Z,float XAxis_X,float XAxis_Y,float XAxis_Z,float YAxis_X,float YAxis_Y,float YAxis_Z) {
	int Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(CollisionShape,CollisionShape)
		if BAD_REF(CollisionShape) {
			REPORT_TYPE(CollisionShape,L"RigidBody_Create_Dynamic",CollisionShape)
			Result = 0;
		} else if (pCollisionShape->CollisionShape->isNonMoving()) {
			MQ->InsertMessage(L"RigidBody_Create_Dynamic: Dynamic rigid bodies can not use static shapes.");
			Result = 0;
		} else {
			float NewMass;
			if (Mass < 0.00001f) {
				NewMass = 0.00001f;
				MQ->InsertMessage(L"RigidBody_Create_Dynamic: The mass in dynamic bodies must be at least 0.00001. The mass have been set to the minimum.");
			} else {
				NewMass = Mass;
			}
			Result = DGE.IDFromPointer(DGE.RigidBody_Create(pCollisionShape,NewMass,DVector3(Pos_X,Pos_Y,Pos_Z),DVector3(XAxis_X,XAxis_Y,XAxis_Z),DVector3(YAxis_X,YAxis_Y,YAxis_Z),true,false));
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_Delete(int RigidBody) {
	START_OF_CRITICAL_SECTION
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Delete",RigidBody)
		} else {
			// Remove any connections from instances to remove a few annoying error messages.
			DoubleLinkListBody<Instance_Struct>* InstanceIterator;
			DoubleLinkListBody<Instance_Struct>* NextInstanceIterator;
			Instance_Struct* FollowingInstance;
			InstanceIterator = pRigidBody->LL_Follow.Head;
			while (InstanceIterator) {
				NextInstanceIterator = InstanceIterator->Next;
				FollowingInstance = InstanceIterator->Content;
					// Remove the relation
					assert(FollowingInstance->FollowedRigidBody);
					StopUsing(pRigidBody->useCount);
					pRigidBody->LL_Follow.Remove(&(FollowingInstance->LL_Follow));
					FollowingInstance->FollowedRigidBody = NULL;
				InstanceIterator = NextInstanceIterator;
			}
			DGE.Delete_RigidBody(pRigidBody);
		}
	END_OF_CRITICAL_SECTION
}

void CDFPGECtrl::RigidBody_SetTransformation(int RigidBody,float Pos_X,float Pos_Y,float Pos_Z,float XAxis_X,float XAxis_Y,float XAxis_Z,float YAxis_X,float YAxis_Y,float YAxis_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetTransformation",RigidBody)
		} else {
			DGE.RigidBody_SetTransformation(pRigidBody,DVector3(Pos_X,Pos_Y,Pos_Z),DVector3(XAxis_X,XAxis_Y,XAxis_Z),DVector3(YAxis_X,YAxis_Y,YAxis_Z));
			FollowRigidBody(pRigidBody); // In case that the body is not dynamic or moved after the synchronization call
		}
	)
}

void CDFPGECtrl::RigidBody_GetTransformation_OutM4(int RigidBody) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetTransformation_OutM4",RigidBody)
		} else {
			btTransform trans = pRigidBody->Body->getWorldTransform();
			SetBulletTransformToMB(&trans);
		}
	)
}

void CDFPGECtrl::RigidBody_GetTransformation_Interpolated_OutM4(int RigidBody, float TimeOffset) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetTransformation_Interpolated_OutM4",RigidBody)
		} else {
			btTransform Trans = pRigidBody->Body->getWorldTransform();
			if (pRigidBody->Body->isStaticOrKinematicObject() || pRigidBody->Body->getActivationState() == ISLAND_SLEEPING) {
				// No interpolation needed
				SetBulletTransformToMB(&Trans);
			} else {
				// Use interpolation
				btTransform InterpolatedTrans;
				btTransformUtil::integrateTransform(Trans,pRigidBody->Body->getInterpolationLinearVelocity(),pRigidBody->Body->getInterpolationAngularVelocity(),TimeOffset*pRigidBody->Body->getHitFraction(),InterpolatedTrans);
				SetBulletTransformToMB(&InterpolatedTrans);
			}
		}
	)
}

void CDFPGECtrl::RigidBody_SetPosition(int RigidBody, float X, float Y, float Z) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetPosition",RigidBody)
		} else {
			btTransform trans = pRigidBody->Body->getWorldTransform();
			trans.setOrigin(btVector3(X,Y,Z));
			DGE.RigidBody_SetTransformation_Aux(pRigidBody,&trans);
		}
	)
}

void CDFPGECtrl::RigidBody_GetPosition_OutV3(int RigidBody) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetPosition_OutV3",RigidBody)
		} else {
			btTransform trans = pRigidBody->Body->getWorldTransform();
			btVector3 origin = trans.getOrigin();
			SetBulletVector3ToMB(&origin);
		}
	)
}

void CDFPGECtrl::RigidBody_SetLinearSleepingThreshold(int RigidBody, float LinearTreshold) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetLinearSleepingThreshold",RigidBody)
		} else if (LinearTreshold <= 0.0f) {
			MQ->InsertMessage(L"RigidBody_SetLinearSleepingThreshold: LinearTreshold must be greater than zero. Disable deactivation using RigidBody_SetActivationState if you don't want the rigid body to deactivate.");
		} else {
			pRigidBody->Body->setSleepingThresholds(LinearTreshold,pRigidBody->Body->getAngularSleepingThreshold());
		}
	)
}

void CDFPGECtrl::RigidBody_SetAngularSleepingThreshold(int RigidBody, float AngularTreshold) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetAngularSleepingThreshold",RigidBody)
		} else if (AngularTreshold <= 0.0f) {
			MQ->InsertMessage(L"RigidBody_SetAngularSleepingThreshold: AngularTreshold must be greater than zero. Disable deactivation using RigidBody_SetActivationState if you don't want the rigid body to deactivate.");
		} else {
			pRigidBody->Body->setSleepingThresholds(pRigidBody->Body->getLinearSleepingThreshold(),AngularTreshold);
		}
	)
}

float CDFPGECtrl::RigidBody_GetLinearSleepingThreshold(int RigidBody) {
	float Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetLinearSleepingThreshold",RigidBody)
			Result = 0;
		} else {
			Result = pRigidBody->Body->getLinearSleepingThreshold();
		}
	)
	return Result;
}

float CDFPGECtrl::RigidBody_GetAngularSleepingThreshold(int RigidBody) {
	float Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetAngularSleepingThreshold",RigidBody)
			Result = 0;
		} else {
			Result = pRigidBody->Body->getAngularSleepingThreshold();
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_SetLinearDamping(int RigidBody, float LinearDamping) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetLinearDamping",RigidBody)
		} else if (LinearDamping < 0.0f || LinearDamping > 1.0f) {
			MQ->InsertMessage(L"RigidBody_SetLinearDamping: LinearDamping must be from 0 to 1.");
		} else {
			pRigidBody->Body->setDamping(LinearDamping,pRigidBody->Body->getAngularDamping());
		}
	)
}

float CDFPGECtrl::RigidBody_GetLinearDamping(int RigidBody) {
	float Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetLinearDamping",RigidBody)
			Result = 0;
		} else {
			Result = pRigidBody->Body->getLinearDamping();
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_SetAngularDamping(int RigidBody, float AngularDamping) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetAngularDamping",RigidBody)
		} else if (AngularDamping < 0.0f || AngularDamping > 1.0f) {
			MQ->InsertMessage(L"RigidBody_SetAngularDamping: AngularDamping must be from 0 to 1.");
		} else {
			pRigidBody->Body->setDamping(pRigidBody->Body->getLinearDamping(),AngularDamping);
		}
	)
}

float CDFPGECtrl::RigidBody_GetAngularDamping(int RigidBody) {
	float Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetAngularDamping",RigidBody)
			Result = 0;
		} else {
			Result = pRigidBody->Body->getAngularDamping();
		}
	)
	return Result;
}

/*
#define ACTIVE_TAG 1
#define ISLAND_SLEEPING 2
#define WANTS_DEACTIVATION 3
#define DISABLE_DEACTIVATION 4
#define DISABLE_SIMULATION 5
*/
int CDFPGECtrl::RigidBody_GetActivationState(int RigidBody) {
	int Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetActivationState",RigidBody)
			Result = 0;
		} else {
			Result = pRigidBody->Body->getActivationState();
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_SetActivationState(int RigidBody, int ActivationState) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetActivationState",RigidBody)
		} else if (ActivationState < ACTIVE_TAG || ActivationState > DISABLE_SIMULATION) {
			MQ->InsertMessage(L"RigidBody_SetActivationState: Invalid activation state. Use 1 for ACTIVE_TAG, 2 for ISLAND_SLEEPING, 3 for WANTS_DEACTIVATION, 4 for DISABLE_DEACTIVATION or 5 for DISABLE_SIMULATION.");
		} else {
			pRigidBody->Body->setActivationState(ActivationState);
			if (ActivationState == ISLAND_SLEEPING || ActivationState == DISABLE_SIMULATION) {
				FollowRigidBody(pRigidBody);
			} else if (ActivationState == ACTIVE_TAG) {
				pRigidBody->Body->activate(false);
			}
		}
	)
}

void CDFPGECtrl::RigidBody_GetBoundingBoxMinimum_OutV3(int RigidBody) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetBoundingBoxMinimum_OutV3",RigidBody)
		} else {
			btVector3 Min;
			btVector3 Max;
			pRigidBody->Body->getAabb(Min,Max);
			SetBulletVector3ToMB(&Min);
		}
	)
}

void CDFPGECtrl::RigidBody_GetBoundingBoxMaximum_OutV3(int RigidBody) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetBoundingBoxMaximum_OutV3",RigidBody)
		} else {
			btVector3 Min;
			btVector3 Max;
			pRigidBody->Body->getAabb(Min,Max);
			SetBulletVector3ToMB(&Max);
		}
	)
}

void CDFPGECtrl::RigidBody_Dynamic_ApplyForce_Linear(int RigidBody,float Acc_X,float Acc_Y,float Acc_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Dynamic_ApplyForce_Linear",RigidBody)
		} else if (!(pRigidBody->Dynamic)) {
			MQ->InsertMessage(L"RigidBody_Dynamic_ApplyForce_Linear: RigidBody is not dynamic.");
		} else {
			pRigidBody->Body->applyCentralForce(btVector3(Acc_X,Acc_Y,Acc_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_Dynamic_ApplyForce_Angular(int RigidBody,float Acc_X,float Acc_Y,float Acc_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Dynamic_ApplyForce_Angular",RigidBody)
		} else if (!(pRigidBody->Dynamic)) {
			MQ->InsertMessage(L"RigidBody_Dynamic_ApplyForce_Angular: RigidBody is not dynamic.");
		} else {
			pRigidBody->Body->applyTorque(btVector3(Acc_X,Acc_Y,Acc_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_Dynamic_ApplyForce_Point(int RigidBody,float Acc_X,float Acc_Y,float Acc_Z,float Pos_X,float Pos_Y,float Pos_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Dynamic_ApplyForce_Point",RigidBody)
		} else if (!(pRigidBody->Dynamic)) {
			MQ->InsertMessage(L"RigidBody_Dynamic_ApplyForce_Point: RigidBody is not dynamic.");
		} else {
			pRigidBody->Body->applyForce(btVector3(Acc_X,Acc_Y,Acc_Z),btVector3(Pos_X,Pos_Y,Pos_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_Dynamic_ApplyImpulse_Linear(int RigidBody,float Vel_X,float Vel_Y,float Vel_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Dynamic_ApplyImpulse_Linear",RigidBody)
		} else if (!(pRigidBody->Dynamic)) {
			MQ->InsertMessage(L"RigidBody_Dynamic_ApplyImpulse_Linear: RigidBody is not dynamic.");
		} else {
			pRigidBody->Body->applyCentralImpulse(btVector3(Vel_X,Vel_Y,Vel_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_Dynamic_ApplyImpulse_Angular(int RigidBody,float Vel_X,float Vel_Y,float Vel_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Dynamic_ApplyImpulse_Angular",RigidBody)
		} else if (!(pRigidBody->Dynamic)) {
			MQ->InsertMessage(L"RigidBody_Dynamic_ApplyImpulse_Angular: RigidBody is not dynamic.");
		} else {
			pRigidBody->Body->applyTorqueImpulse(btVector3(Vel_X,Vel_Y,Vel_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_Dynamic_ApplyImpulse_Point(int RigidBody,float Vel_X,float Vel_Y,float Vel_Z,float Pos_X,float Pos_Y,float Pos_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Dynamic_ApplyImpulse_Point",RigidBody)
		} else if (!(pRigidBody->Dynamic)) {
			MQ->InsertMessage(L"RigidBody_Dynamic_ApplyImpulse_Point: RigidBody is not dynamic.");
		} else {
			pRigidBody->Body->applyImpulse(btVector3(Vel_X,Vel_Y,Vel_Z),btVector3(Pos_X,Pos_Y,Pos_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_GetLinearVelocity_OutV3(int RigidBody) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetLinearVelocity_OutV3",RigidBody)
		} else {
			btVector3 Vel;
			Vel = pRigidBody->Body->getLinearVelocity();
			SetBulletVector3ToMB(&Vel);
		}
	)
}

void CDFPGECtrl::RigidBody_GetAngularVelocity_OutV3(int RigidBody) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetAngularVelocity_OutV3",RigidBody)
		} else {
			btVector3 Vel;
			Vel = pRigidBody->Body->getAngularVelocity();
			SetBulletVector3ToMB(&Vel);
		}
	)
}

void CDFPGECtrl::RigidBody_SetLinearVelocity(int RigidBody,float Vel_X,float Vel_Y,float Vel_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetLinearVelocity",RigidBody)
		} else {
			pRigidBody->Body->setLinearVelocity(btVector3(Vel_X,Vel_Y,Vel_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_SetAngularVelocity(int RigidBody,float Vel_X,float Vel_Y,float Vel_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetAngularVelocity",RigidBody)
		} else {
			pRigidBody->Body->setAngularVelocity(btVector3(Vel_X,Vel_Y,Vel_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_SetContinuousCollisionDetectionMotionTreshold(int RigidBody, float Treshold) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetContinuousCollisionDetectionMotionTreshold",RigidBody)
		} else if (Treshold <= 0.0f) {
			MQ->InsertMessage(L"RigidBody_SetContinuousCollisionDetectionMotionTreshold: Treshold must be greater than zero.");
		} else {
			pRigidBody->Body->setCcdMotionThreshold(Treshold);
		}
	)
}

float CDFPGECtrl::RigidBody_GetContinuousCollisionDetectionMotionTreshold(int RigidBody) {
	float Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetContinuousCollisionDetectionMotionTreshold",RigidBody)
			Result = 0.0f;
		} else {
			Result = pRigidBody->Body->getCcdMotionThreshold();
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_SetFriction(int RigidBody, float Friction) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetFriction",RigidBody)
		} else if (Friction < 0.0f) {
			MQ->InsertMessage(L"RigidBody_SetFriction: Friction must be at least zero.");
		} else {
			pRigidBody->Body->setFriction(Friction);
		}
	)
}

float CDFPGECtrl::RigidBody_GetFriction(int RigidBody) {
	float Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetFriction",RigidBody)
			Result = 0.0f;
		} else {
			Result = pRigidBody->Body->getFriction();
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_SetAnisotropicFriction(int RigidBody, float Friction_X, float Friction_Y, float Friction_Z) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetAnisotropicFriction",RigidBody)
		} else if (Friction_X < 0.0f || Friction_Y < 0.0f || Friction_Z < 0.0f) {
			MQ->InsertMessage(L"RigidBody_SetAnisotropicFriction: Each element in the anisotropic friction must be at least zero.");
		} else {
			pRigidBody->Body->setAnisotropicFriction(btVector3(Friction_X,Friction_Y,Friction_Z));
		}
	)
}

void CDFPGECtrl::RigidBody_GetAnisotropicFriction_OutV3(int RigidBody) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetAnisotropicFriction_OutV3",RigidBody)
		} else {
			btVector3 Fric;
			Fric = pRigidBody->Body->getAnisotropicFriction();
			SetBulletVector3ToMB(&Fric);
		}
	)
}

void CDFPGECtrl::RigidBody_Dynamic_SetLinearMass(int RigidBody, float LinearMass) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Dynamic_SetLinearMass",RigidBody)
		} else if (!(pRigidBody->Dynamic)) {
			MQ->InsertMessage(L"RigidBody_Dynamic_SetLinearMass: You can only set the mass for dynamic bodies.");
		} else {
			float NewMass;
			if (LinearMass < 0.00001f) {
				NewMass = 0.00001f;
				MQ->InsertMessage(L"RigidBody_Dynamic_SetLinearMass: The mass in dynamic bodies must be at least 0.00001. The mass have been set to the minimum.");
			} else {
				NewMass = LinearMass;
			}
			pRigidBody->LinearMass = NewMass;
			pRigidBody->Body->setMassProps(NewMass,btVector3(pRigidBody->LocalInertia.x,pRigidBody->LocalInertia.y,pRigidBody->LocalInertia.z));
		}
	)
}

float CDFPGECtrl::RigidBody_GetLinearMass(int RigidBody) {
	float Result;
	CRITICAL_CODE_SECTION(
		Result = 0.0f;
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetLinearMass",RigidBody)
		} else {
			Result = pRigidBody->LinearMass;
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_Dynamic_SetLocalInertia(int RigidBody, float LocalInertia_X, float LocalInertia_Y, float LocalInertia_Z) {
	START_OF_CRITICAL_SECTION
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_Dynamic_SetLocalInertia",RigidBody)
		} else if (!(pRigidBody->Dynamic)) {
			MQ->InsertMessage(L"RigidBody_Dynamic_SetLocalInertia: You can only set the mass for dynamic bodies.");
		} else if (LocalInertia_X < 0.0f || LocalInertia_Y < 0.0f || LocalInertia_Z < 0.0f) {
			MQ->InsertMessage(L"RigidBody_Dynamic_SetLocalInertia: Each element in the local inertia must be at least zero.");
		} else {
			pRigidBody->LocalInertia = DVector3(LocalInertia_X,LocalInertia_Y,LocalInertia_Z);
			pRigidBody->Body->setMassProps(pRigidBody->LinearMass,btVector3(LocalInertia_X,LocalInertia_Y,LocalInertia_Z));
		}
	END_OF_CRITICAL_SECTION
}

void CDFPGECtrl::RigidBody_GetLocalInertia_OutV3(int RigidBody) {
	CRITICAL_CODE_SECTION(
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetLocalInertia_OutV3",RigidBody)
		} else {
			SetVector3ToMB(pRigidBody->LocalInertia);
		}
	)
}

int CDFPGECtrl::RigidBody_GetCollisionPointsFromBody_OutCB(int RigidBody) {
	int Result;
	CRITICAL_CODE_SECTION(
		CB.Clear();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetCollisionPointsFromBody_OutCB",RigidBody)
			Result = 0;
		} else {
			DGE.m_PhysicsCore.dynamicsWorld->contactTest(pRigidBody->Body,CB);
			Result = CB.ContactArray.size();
		}
	)
	return Result;
}

int CDFPGECtrl::RigidBody_GetCollisionPointsBetweenBodies_OutCB(int RigidBodyA, int RigidBodyB) {
	int Result;
	CRITICAL_CODE_SECTION(
		CB.Clear();
		GET_FROM_REF(RigidBody,RigidBodyA)
		GET_FROM_REF(RigidBody,RigidBodyB)
		if BAD_REF(RigidBodyA) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetCollisionPointsBetweenBodies_OutCB",RigidBodyA)
			Result = 0;
		} else if BAD_REF(RigidBodyB) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetCollisionPointsBetweenBodies_OutCB",RigidBodyB)
			Result = 0;
		} else {
			DGE.m_PhysicsCore.dynamicsWorld->contactPairTest(pRigidBodyA->Body,pRigidBodyB->Body,CB);
			Result = CB.ContactArray.size();
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_GetVelocityAtPoint_OutV3(int RigidBody, float WorldSpacePoint_X, float WorldSpacePoint_Y, float WorldSpacePoint_Z) {
	START_OF_CRITICAL_SECTION
		ClearMatrixBuffer();
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetVelocityAtPoint_OutV3",RigidBody)
		} else {
			btVector3 BodyCenter = pRigidBody->Body->getWorldTransform().getOrigin();
			btVector3 RelPos = btVector3(WorldSpacePoint_X - BodyCenter.getX(),WorldSpacePoint_Y - BodyCenter.getY(),WorldSpacePoint_Z - BodyCenter.getZ());
			btVector3 LinVel = pRigidBody->Body->getLinearVelocity();
			btVector3 AngVel = pRigidBody->Body->getAngularVelocity();
			btVector3 Vel = LinVel + AngVel.cross(RelPos);
			SetBulletVector3ToMB(&Vel);
		}
	END_OF_CRITICAL_SECTION
}

int CDFPGECtrl::RigidBody_GetCollisionShape(int RigidBody) {
	int Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetCollisionShape",RigidBody)
			Result = -1;
		} else {
			Result = pRigidBody->CollisionShape->ID;
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_SetUserData(int RigidBody, int Index, int NewValue) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetUserData",RigidBody)
		} else if (Index < 0 || Index >= NumberOfUserElementsPerRigidBody) {
			MQ->InsertMessage(L"RigidBody_SetUserData: Index is out of the bound from 0 to 7.");
		} else {
			pRigidBody->UserData[Index] = NewValue;
		}
	)
}

int CDFPGECtrl::RigidBody_GetUserData(int RigidBody, int Index) {
	int Result;
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetUserData",RigidBody)
			Result = -1;
		} else if (Index < 0 || Index >= NumberOfUserElementsPerRigidBody) {
			MQ->InsertMessage(L"RigidBody_GetUserData: Index is out of the bound from 0 to 7.");
			Result = -1;
		} else {
			Result = pRigidBody->UserData[Index];
		}
	)
	return Result;
}

void CDFPGECtrl::RigidBody_SetRestitution(int RigidBody, float Restitution) {
	CRITICAL_CODE_SECTION(
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_SetRestitution",RigidBody)
		} else {
			pRigidBody->Body->setRestitution(ClampFloat(0.0f,Restitution,1.0f));
		}
	)
}

float CDFPGECtrl::RigidBody_GetRestitution(int RigidBody) {
	float Result;
	CRITICAL_CODE_SECTION(
		Result = 0.0f;
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"RigidBody_GetRestitution",RigidBody)
		} else {
			Result = pRigidBody->Body->getRestitution();
		}
	)
	return Result;
}

// Modified from the raytracer example by Erwin Coumans
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
int CDFPGECtrl::RigidBody_GetFirstIntersection_Out2V3(float Start_X, float Start_Y, float Start_Z, float End_X, float End_Y, float End_Z, int RigidBody) {
	btVector3 rayFrom;
	btVector3 rayTo;
	int Result;
	START_OF_CRITICAL_SECTION
		ClearMatrixBuffer();
		Result = 0;
		GET_FROM_REF(RigidBody,RigidBody)
		if BAD_REF(RigidBody) {
			REPORT_TYPE(RigidBody,L"Physics_GetClosestIntersection_SingleBody_Out2V3",RigidBody)
		} else {
			rayFrom = btVector3(Start_X,Start_Y,Start_Z);
			rayTo = btVector3(End_X,End_Y,End_Z);

			btCollisionWorld::ClosestRayResultCallback resultCallback(rayFrom,rayTo);
			btConvexCast::CastResult rayResult;
			btSphereShape pointShape(0.0f);
			btTransform rayFromTrans;
			btTransform rayToTrans;

			rayFromTrans.setIdentity();
			rayFromTrans.setOrigin(rayFrom);
			rayToTrans.setIdentity();
			rayToTrans.setOrigin(rayTo);

			//comment-out next line to get all hits, instead of just the closest hit
			resultCallback.m_closestHitFraction = 1.0f;
			
			//do some culling, ray versus aabb
			btVector3 aabbMin,aabbMax;
			btCollisionShape* Shape = pRigidBody->Body->getCollisionShape();
			btTransform Trans = pRigidBody->Body->getWorldTransform();
			Shape->getAabb(Trans,aabbMin,aabbMax);
			btScalar hitLambda = 1.0f;
			btVector3 hitNormal;
			btCollisionObject	tmpObj;
			tmpObj.setWorldTransform(Trans);

			bool PassAABB;
			if (Shape->isInfinite()) {
				PassAABB = true;
			} else {
				PassAABB = btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal);
			}
			if (PassAABB) {
				//reset previous result
				if (pRigidBody->CollisionShape->ShapeType == ShapeType_HeightField) {
					ModifiedWorld::rayTestHeightField(rayFromTrans,rayToTrans, &tmpObj, Shape, Trans, resultCallback);
				} else {
					btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, &tmpObj, Shape, Trans, resultCallback);
				}
				if (resultCallback.hasHit()) {
					resultCallback.m_hitNormalWorld.normalize();

					// Position in vector (X1,Y1,Z1)
					X1 = resultCallback.m_hitPointWorld.getX();
					Y1 = resultCallback.m_hitPointWorld.getY();
					Z1 = resultCallback.m_hitPointWorld.getZ();

					// Normal in vector (X2,Y2,Z2)
					X2 = resultCallback.m_hitNormalWorld.getX();
					Y2 = resultCallback.m_hitNormalWorld.getY();
					Z3 = resultCallback.m_hitNormalWorld.getZ();
			
					// Return the ID of the rigid body for consistency with Physics_GetClosestIntersection_World_Out2V3
					Result = 1;
				} else {
					Result = 0;
				}
			} else {
				Result = 0;
			}
		}
	END_OF_CRITICAL_SECTION
	return Result;
}
