CPhysicsObject* CreatePhysicsSphere(CPhysicsEnvironment *pEnvironment, float radius, int materialIndex, const Vector &position, const QAngle &angles, objectparams_t *pParams, bool isStatic) {
	btSphereShape* shape = new btSphereShape(ConvertDistanceToBull(radius));
	
	btVector3 vector;
	btMatrix3x3 matrix;
	ConvertPosToBull(position, vector);
	ConvertRotationToBull(angles, matrix);
	btTransform transform(matrix, vector);

	float mass = pParams->mass;
	if (isStatic) mass = 0;

	btMotionState* motionstate = new btMassCenterMotionState(transform);
	btRigidBody::btRigidBodyConstructionInfo info(mass,motionstate,shape);

	btRigidBody* body = new btRigidBody(info);

	if (mass > 0)
		pEnvironment->GetBulletEnvironment()->addRigidBody(body);
	else
		pEnvironment->GetBulletEnvironment()->addRigidBody(body, 2, ~2);

	float volume = pParams->volume;
	if (volume <= 0) {
		volume = 4.0f * radius * radius * radius * M_PI / 3.0f;
	}

	CPhysicsObject *pObject = new CPhysicsObject();
	pObject->Init(pEnvironment, body, materialIndex, volume, 0, 0, NULL);
	pObject->SetGameData(pParams->pGameData);
	pObject->EnableCollisions(pParams->enableCollisions);

	return pObject;
}
void CPhysicsObject::SetPosition(const Vector& worldPosition, const QAngle& angles, bool isTeleport) {
	btVector3 pos;
	btMatrix3x3 matrix;
	ConvertPosToBull(worldPosition, pos);
	ConvertRotationToBull(angles, matrix);
	btTransform transform(matrix, pos);
	((btMassCenterMotionState*)m_pObject->getMotionState())->setGraphicTransform(transform);
}
void ConvertShadowControllerToBull(const hlshadowcontrol_params_t &in, shadowcontrol_params_t &out) {
	ConvertPosToBull(in.targetPosition, out.targetPosition);
	ConvertRotationToBull(in.targetRotation, out.targetRotation);
	out.teleportDistance = ConvertDistanceToBull(in.teleportDistance);

	ConvertForceImpulseToBull(in.maxSpeed, out.maxSpeed);
	out.maxSpeed = out.maxSpeed.absolute();
	ConvertAngularImpulseToBull(in.maxAngular, out.maxAngular);
	out.maxAngular = out.maxAngular.absolute();
	out.dampFactor = in.dampFactor;
}
void ConvertShadowControllerToBull(const hlshadowcontrol_params_t &in, shadowcontrol_params_t &out) {
	ConvertPosToBull(in.targetPosition, out.targetPosition);
	ConvertRotationToBull(in.targetRotation, out.targetRotation);
	out.teleportDistance = ConvertDistanceToBull(in.teleportDistance);

	out.maxSpeed = ConvertDistanceToBull(in.maxSpeed);
	out.maxDampSpeed = ConvertDistanceToBull(in.maxDampSpeed);
	out.maxAngular = ConvertAngleToBull(in.maxAngular);
	out.maxDampAngular = ConvertAngleToBull(in.maxDampAngular);
	out.dampFactor = in.dampFactor;
}
CPhysicsObject* CreatePhysicsObject(CPhysicsEnvironment *pEnvironment, const CPhysCollide *pCollisionModel, int materialIndex, const Vector &position, const QAngle& angles, objectparams_t *pParams, bool isStatic) {
	btCollisionShape* shape = (btCollisionShape*)pCollisionModel;
	
	btVector3 vector;
	btMatrix3x3 matrix;
	ConvertPosToBull(position, vector);
	ConvertRotationToBull(angles, matrix);
	btTransform transform(matrix, vector);

	PhysicsShapeInfo *shapeInfo = (PhysicsShapeInfo*)shape->getUserPointer();
	btTransform masscenter(btMatrix3x3::getIdentity());
	if (shapeInfo) masscenter.setOrigin(shapeInfo->massCenter);

	float mass = pParams->mass;
	if (isStatic) mass = 0;

	btVector3 inertia;

	shape->calculateLocalInertia(mass, inertia);
	btMotionState* motionstate = new btMassCenterMotionState(transform, masscenter);
	btRigidBody::btRigidBodyConstructionInfo info(mass,motionstate,shape,inertia);

	info.m_linearDamping = pParams->damping;
	info.m_angularDamping = pParams->rotdamping;
	//info.m_localInertia = btVector3(pParams->inertia, pParams->inertia, pParams->inertia);

	btRigidBody* body = new btRigidBody(info);

	if (mass > 0)
		pEnvironment->GetBulletEnvironment()->addRigidBody(body);
	else
		pEnvironment->GetBulletEnvironment()->addRigidBody(body, 2, ~2);

	CPhysicsObject *pObject = new CPhysicsObject();
	pObject->Init(pEnvironment, body, materialIndex, pParams->volume, pParams->dragCoefficient, pParams->dragCoefficient, pParams->massCenterOverride);
	pObject->SetGameData(pParams->pGameData);
	pObject->EnableCollisions(pParams->enableCollisions);
	if (!isStatic && pParams->dragCoefficient != 0.0f) pObject->EnableDrag(true);

	/*if (mass > 0)
	{
		btVector3 mins, maxs;
		shape->getAabb(btTransform::getIdentity(), mins, maxs);
		float maxradius = min(min(abs(maxs.getX()), abs(maxs.getY())), abs(maxs.getZ()));
		float minradius = min(min(abs(mins.getX()), abs(mins.getY())), abs(mins.getZ()));
		float radius = min(maxradius,minradius)/2.0f;
		body->setCcdMotionThreshold(radius*0.5f);
		body->setCcdSweptSphereRadius(0.2f*radius);
	}*/
	
	return pObject;
}
void CShadowController::Update(const Vector &position, const QAngle &angles, float timeOffset) {
	btVector3 targetPosition = m_shadow.targetPosition;
	btQuaternion targetRotation = m_shadow.targetRotation;

	ConvertPosToBull(position, m_shadow.targetPosition);
	m_secondsToArrival = timeOffset < 0 ? 0 : timeOffset;
	ConvertRotationToBull(angles, m_shadow.targetRotation);

	m_enable = true;

	if (IsEqual(targetPosition, m_shadow.targetPosition) && IsEqual(targetRotation, m_shadow.targetRotation)) return;

	m_pObject->Wake();
}
void CPhysicsObject::SetPosition(const Vector &worldPosition, const QAngle &angles, bool isTeleport) {
	btVector3 bullPos;
	btMatrix3x3 bullAngles;

	ConvertPosToBull(worldPosition, bullPos);
	ConvertRotationToBull(angles, bullAngles);
	btTransform trans(bullAngles, bullPos);

	m_pObject->setWorldTransform(trans * ((btMassCenterMotionState *)m_pObject->getMotionState())->m_centerOfMassOffset);

	// Assumed this is the behavior of IVP. If you teleport an object, you don't want it to be stupidly frozen in the air.
	// Change this if behavior of IVP is different!
	if (isTeleport)
		m_pObject->activate();
}
CPhysicsObject *CreatePhysicsObject(CPhysicsEnvironment *pEnvironment, const CPhysCollide *pCollisionModel, int materialIndex, const Vector &position, const QAngle &angles, objectparams_t *pParams, bool isStatic) {
	if (!pCollisionModel) return NULL;

	// Some checks
	Assert(position.IsValid() && angles.IsValid());

	btCollisionShape *pShape = (btCollisionShape *)pCollisionModel->GetCollisionShape();

	btTransform massCenterTrans = btTransform::getIdentity();
	massCenterTrans.setOrigin(pCollisionModel->GetMassCenter());
	btMassCenterMotionState *pMotionState = new btMassCenterMotionState(massCenterTrans);

	btVector3 bullPos;
	btMatrix3x3 bullMatrix;
	ConvertPosToBull(position, bullPos);
	ConvertRotationToBull(angles, bullMatrix);

	btTransform initialWordTrans(bullMatrix, bullPos);
	pMotionState->setGraphicTransform(initialWordTrans);

	// Grab some parameters
	btScalar mass = 0.f;
	btVector3 inertia(0, 0, 0);
	btVector3 inertiaCoeff(1, 1, 1);

	if (pParams && !isStatic) {
		mass = pParams->mass;

		// TODO: Grab massCenterOverride and set it up correctly.

		// Don't allow the inertia coefficient to be less than 0!
		if (pParams->inertia >= 0)
			inertiaCoeff.setValue(pParams->inertia, pParams->inertia, pParams->inertia);

		pShape->calculateLocalInertia(mass, inertia);
		//inertia = pCollisionModel->GetRotationInertia();
		//inertia *= inertiaCoeff * mass;
	}

	btRigidBody::btRigidBodyConstructionInfo info(mass, pMotionState, pShape, inertia);
	btRigidBody *pBody = new btRigidBody(info);

	CPhysicsObject *pObject = new CPhysicsObject();
	pObject->Init(pEnvironment, pBody, materialIndex, pParams, isStatic);

	return pObject;
}
CPhysicsObject *CreatePhysicsSphere(CPhysicsEnvironment *pEnvironment, float radius, int materialIndex, const Vector &position, const QAngle &angles, objectparams_t *pParams, bool isStatic) {
	if (!pEnvironment) return NULL;

	// Some checks
	Assert(position.IsValid() && angles.IsValid());

	// Conversion unnecessary as this is an exposed function.
	btSphereShape *shape = (btSphereShape *)g_PhysicsCollision.SphereToConvex(radius);
	
	btVector3 vector;
	btMatrix3x3 matrix;
	ConvertPosToBull(position, vector);
	ConvertRotationToBull(angles, matrix);
	btTransform transform(matrix, vector);

	float mass = 0;
	float volume = 0;

	if (pParams) {
		mass = isStatic ? 0 : pParams->mass;

		volume = pParams->volume;
		if (volume <= 0) {
			pParams->volume = (4 / 3) * M_PI * radius * radius * radius;
		}
	}

	btMassCenterMotionState *motionstate = new btMassCenterMotionState();
	motionstate->setGraphicTransform(transform);
	btRigidBody::btRigidBodyConstructionInfo info(mass, motionstate, shape);

	btRigidBody *body = new btRigidBody(info);

	CPhysicsObject *pObject = new CPhysicsObject;
	pObject->Init(pEnvironment, body, materialIndex, pParams, isStatic, true);

	return pObject;
}