/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
* 
* 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.
*/

#include "dgPhysicsStdafx.h"
#include "dgBody.h"
#include "dgWorld.h"
#include "dgUpVectorConstraint.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

dgUpVectorConstraint::dgUpVectorConstraint ()
	:dgBilateralConstraint() 
{
	_ASSERTE ((sizeof (dgUpVectorConstraint) & 15) == 0);
	_ASSERTE ((((dgUnsigned64) &m_localMatrix0) & 15) == 0);

//	dgUpVectorConstraintArray& array = * world;
//	constraint = array.GetElement();

	SetStiffness (dgFloat32 (0.995f));
	m_maxDOF = 2;
	m_constId = dgUpVectorConstraintId;
	m_callBack = NULL;
}

dgUpVectorConstraint::~dgUpVectorConstraint ()
{
}

/*
dgUpVectorConstraint* dgUpVectorConstraint::Create(dgWorld* world)
{
	dgUpVectorConstraint* constraint;
//	constraint = dgUpVectorConstraintArray::GetPool().GetElement();

	dgUpVectorConstraintArray& array = * world;
	constraint = array.GetElement();

	_ASSERTE ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0);

	constraint->Init ();


	constraint->SetStiffness (dgFloat32 (0.995f));
	constraint->m_maxDOF = 2;
	constraint->m_constId = dgUpVectorConstraintId;
	constraint->m_callBack = NULL;
	return constraint;
}

void dgUpVectorConstraint::Remove(dgWorld* world)
{
	dgUpVectorConstraintArray& array = * world;
	dgBilateralConstraint::Remove (world);
//	dgUpVectorConstraintArray::GetPool().RemoveElement (this);
	array.RemoveElement (this);
}
*/

bool dgUpVectorConstraint::IsBilateral () const
{
	return false;
}

void dgUpVectorConstraint::InitPinDir (const dgVector& pin)
{

	const dgMatrix& matrix = m_body0->GetMatrix();

	dgVector pivot (matrix.m_posit); 
	SetPivotAndPinDir (pivot, pin);

}

void dgUpVectorConstraint::SetPinDir (const dgVector& pin)
{
	m_localMatrix1 = dgMatrix (pin);
}

dgVector dgUpVectorConstraint::GetPinDir () const
{
	return m_localMatrix1.m_front;
}


void dgUpVectorConstraint::SetJointParameterCallBack (dgUpVectorJointCallBack callback)
{
	m_callBack = callback;
}


dgUnsigned32 dgUpVectorConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgInt32 ret;
	dgFloat32 mag; 
	dgFloat32 angle; 
	dgMatrix matrix0;
	dgMatrix matrix1;

	CalculateGlobalMatrixAndAngle (matrix0, matrix1);

	dgVector lateralDir (matrix0.m_front * matrix1.m_front);

	ret = 0;
	mag = lateralDir % lateralDir;
	if (mag > 1.0e-6f) {
		mag = dgSqrt (mag);
		lateralDir = lateralDir.Scale (dgFloat32 (1.0f) / mag);
		angle = dgAsin (mag);
		CalculateAngularDerivative (0, params, lateralDir, m_stiffness, angle, &m_jointForce[0]);

		dgVector frontDir (lateralDir * matrix1.m_front);
		CalculateAngularDerivative (1, params, frontDir, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
		ret = 2;
	} else {
		CalculateAngularDerivative (0, params, matrix0.m_up, m_stiffness, 0.0, &m_jointForce[0]);
		CalculateAngularDerivative (1, params, matrix0.m_right, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
		ret = 2;
	}
	return dgUnsigned32 (ret);
}