#include "CPhysicsCustom.h"

IPhysicsCustom::IPhysicsCustom()
{

}
	
IPhysicsCustom::~IPhysicsCustom()
{

}

void IPhysicsCustom::init(NewtonWorld* world, SPhysicsCustomJoint* jnt)
{
	m_world = world;

	m_joint = NewtonConstraintCreateUserJoint(m_world, jnt->maxDOF, customJointCallback, jnt->child->getBody(), jnt->parent->getBody());

	m_child = jnt->child;
	m_parent = jnt->parent;
	
	NewtonJointSetUserData(m_joint, this);
}

void IPhysicsCustom::addLinearRow(vector3df pivot1, vector3df pivot2, vector3df direction)
{
	NewtonUserJointAddLinearRow(m_joint, &pivot1.X, &pivot2.X, &direction.X);
}

void IPhysicsCustom::addAngularRow(f32 relativeAngleError, vector3df pin)
{
	NewtonUserJointAddAngularRow(m_joint, relativeAngleError, &pin.X);
}

void IPhysicsCustom::addGeneralRow(vector3df linear1, vector3df angular1, vector3df linear2, vector3df angular2)
{
	f32 jacobian1[6], jacobian2[6];

	jacobian1[0] = linear1.X;
	jacobian1[1] = linear1.Y;
	jacobian1[2] = linear1.Z;
	jacobian1[3] = angular1.X;
	jacobian1[4] = angular1.Y;
	jacobian1[5] = angular1.Z;

	jacobian2[0] = linear2.X;
	jacobian2[1] = linear2.Y;
	jacobian2[2] = linear2.Z;
	jacobian2[3] = angular2.X;
	jacobian2[4] = angular2.Y;
	jacobian2[5] = angular2.Z;

	NewtonUserJointAddGeneralRow(m_joint, jacobian1, jacobian2);
}

void IPhysicsCustom::setRowMinFriction(f32 friction)
{
	NewtonUserJointSetRowMinimumFriction(m_joint, friction);
}

void IPhysicsCustom::setRowMaxFriction(f32 friction)
{
	NewtonUserJointSetRowMaximumFriction(m_joint, friction);
}

void IPhysicsCustom::setRowAcceleration(f32 acceleration)
{
	NewtonUserJointSetRowAcceleration(m_joint, acceleration);
}

void IPhysicsCustom::setRowStiffness(f32 stiffness)
{
	NewtonUserJointSetRowStiffness(m_joint, stiffness);
}

void IPhysicsCustom::setRowSpringDamperAcceleration(f32 springStiffness, f32 springDamper)
{
	NewtonUserJointSetRowSpringDamperAcceleration(m_joint, springStiffness, springDamper);
}

f32 IPhysicsCustom::getRowForce(int row)
{
	return NewtonUserJointGetRowForce(m_joint, row);
}

void IPhysicsCustom::customJointCallback(const NewtonJoint* userJoint)
{
	IPhysicsCustom* jnt = (IPhysicsCustom*)NewtonJointGetUserData(userJoint);

	jnt->submitConstraint();
}

void IPhysicsCustom::pinAndDirToLocal( const vector3df& pinpt, const vector3df& pindir, 
								   vector3df& localOrient0, vector3df& localPos0 ,vector3df& localOrient1, vector3df& localPos1 )
{
/*	localPos0 = localPos1 = vector3df(0,0,0);

	vector3df bodyOrient0 = vector3df(0,0,0);
	vector3df bodyOrient1 = vector3df(0,0,0);
	vector3df bodyPos0 = vector3df(0,0,0);
	vector3df bodyPos1 = vector3df(0,0,0);

	vector3df pinOrient = grammSchmidt(pindir);

	bodyPos0 = m_child->getPosition();
	bodyOrient0 = m_child->getRotation();

	if (m_parent)
	{
		bodyPos1 = m_parent->getPosition();
		bodyOrient1 = m_parent->getRotation();
	}

	localPos0 = bodyOrient0 * (pinpt - bodyPos0);
	localOrient0 = pinOrient * bodyOrient0;

	localPos1 = bodyOrient1 * (pinpt - bodyPos1);
	localOrient1 = pinOrient * bodyOrient1;*/
}

inline void setFrontVector(matrix4 m, vector3df V){ 
   m.pointer()[0] = V.X; m.pointer()[1] = V.Y; m.pointer()[2] = V.Z; 
} 

inline void setUpVector(matrix4 m, vector3df V){ 
   m.pointer()[4] = V.X; m.pointer()[5] = V.Y; m.pointer()[6] = V.Z; 
} 

inline void setRightVector(matrix4 m, vector3df V){ 
   m.pointer()[8] = V.X; m.pointer()[9] = V.Y; m.pointer()[10] = V.Z; 
} 

//! Builds GrammSchmidt - Tree orthonormal axes with front vector aligned to the given vector 
matrix4 IPhysicsCustom::grammSchmidt(const vector3df& pin){ 
	matrix4 m;
   vector3df Front, Up, Right; 
   Front = pin; 
   Front.normalize(); 
   if(abs_<f32>(Front.Z) > 0.577f) 
      Right = Front.crossProduct(vector3df(-Front.Y, Front.Z, 0.0f)); 
   else 
      Right = Front.crossProduct(vector3df(-Front.Y, Front.X, 0.0f)); 
   Right.normalize(); 
   Up = Right.crossProduct( Front ); 
   setFrontVector(m, Front ); 
   setUpVector(m, Up ); 
   setRightVector(m, Right ); 
   return m;
} 
