/* 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 "dgUniversalConstraint.h"

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

dgUniversalConstraint::dgUniversalConstraint() :
    dgBilateralConstraint()
{
  _ASSERTE((((dgUnsigned64) &m_localMatrix0) & 15) == 0);
  m_maxDOF = 6;
  m_constId = dgUniversalConstraintId;

  m_angle0 = dgFloat32(0.0f);
  m_angle1 = dgFloat32(0.0f);
  m_jointAccelFnt = NULL;
}

dgUniversalConstraint::~dgUniversalConstraint()
{
}

/*
 dgUniversalConstraint* dgUniversalConstraint::Create(dgWorld* world)
 {
 dgUniversalConstraint*	constraint;

 //	constraint = dgUniversalConstraintArray::GetPool().GetElement();

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

 _ASSERTE ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0);
 constraint->Init ();
 constraint->m_maxDOF = 6;
 constraint->m_constId = dgUniversalConstraintId;

 constraint->m_angle0 = dgFloat32 (0.0f);
 constraint->m_angle1 = dgFloat32 (0.0f);
 constraint->m_jointAccelFnt = NULL;
 return constraint;
 }

 void dgUniversalConstraint::Remove(dgWorld* world)
 {
 dgUniversalConstraintArray& array = * world;

 dgBilateralConstraint::Remove (world);
 //	dgUniversalConstraintArray::GetPool().RemoveElement (this);
 array.RemoveElement (this);
 }
 */

void dgUniversalConstraint::SetJointParameterCallBack(
    dgUniversalJointAcceleration callback)
{
  m_jointAccelFnt = callback;
}

dgFloat32 dgUniversalConstraint::GetJointAngle0() const
{
  return m_angle0;
}

dgFloat32 dgUniversalConstraint::GetJointAngle1() const
{
  return m_angle1;
}

dgFloat32 dgUniversalConstraint::GetJointOmega0() const
{
  _ASSERTE(m_body0);
  _ASSERTE(m_body1);

  dgVector dir(m_body0->GetMatrix().RotateVector(m_localMatrix0[0]));
  const dgVector& omega0 = m_body0->GetOmega();
  const dgVector& omega1 = m_body1->GetOmega();

//	dgVector omega1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
//	if (m_body1) {
//		omega1 = m_body1->GetOmega();
//	}
  return (omega0 - omega1) % dir;

}

dgFloat32 dgUniversalConstraint::GetJointOmega1() const
{
  _ASSERTE(m_body0);
  _ASSERTE(m_body1);

  dgVector dir(m_body1->GetMatrix().RotateVector(m_localMatrix1[1]));
  const dgVector& omega0 = m_body0->GetOmega();
  const dgVector& omega1 = m_body1->GetOmega();

//	dgMatrix matrix (dgGetIdentityMatrix()); 
//	if (m_body1) {
//		matrix = m_body1->GetMatrix(); 
//		omega1 = m_body1->GetOmega();
//	}
//	dgVector dir (matrix.RotateVector (m_localMatrix1[1]));

  return (omega0 - omega1) % dir;
}

dgFloat32 dgUniversalConstraint::CalculateStopAlpha0(dgFloat32 angle,
    const dgJointCallBackParam* param) const
{
  dgFloat32 alpha;
  dgFloat32 omega;
  dgFloat32 penetrationErr;

  alpha = dgFloat32(0.0f);
  if (m_angle0 > angle)
  {
    omega = GetJointOmega0();
    if (omega < dgFloat32(0.0f))
    {
      omega = dgFloat32(0.0f);
    }
    penetrationErr = angle - m_angle0;
    alpha = 100.0f * penetrationErr - omega * 1.01f / param->m_timestep;

  }
  else if (m_angle0 < angle)
  {
    omega = GetJointOmega0();
    if (omega > dgFloat32(0.0f))
    {
      omega = dgFloat32(0.0f);
    }

    penetrationErr = angle - m_angle0;
    alpha = 100.0f * penetrationErr - omega * 1.01f / param->m_timestep;
  }
  return alpha;

}

dgFloat32 dgUniversalConstraint::CalculateStopAlpha1(dgFloat32 angle,
    const dgJointCallBackParam* param) const
{
  dgFloat32 alpha;
  dgFloat32 omega;
  dgFloat32 penetrationErr;

  alpha = dgFloat32(0.0f);
  if (m_angle1 > angle)
  {
    omega = GetJointOmega1();
    if (omega > dgFloat32(0.0f))
    {
      omega = dgFloat32(0.0f);
    }
    penetrationErr = m_angle1 - angle;
//		alpha = (penetrationErr + omega * 1.01f) / param->m_timestep;
    alpha = penetrationErr * 100.0f - omega * 1.01f / param->m_timestep;
  }
  else if (m_angle1 < angle)
  {
    omega = GetJointOmega1();
    if (omega < dgFloat32(0.0f))
    {
      omega = dgFloat32(0.0f);
    }

    penetrationErr = m_angle1 - angle;
//		alpha = (penetrationErr + omega * 1.01f) / param->m_timestep;
    alpha = penetrationErr * 100.0f - omega * 1.01f / param->m_timestep;
  }
  return alpha;
}

dgVector dgUniversalConstraint::GetJointForce() const
{
  dgMatrix matrix0;
  dgMatrix matrix1;

  CalculateGlobalMatrixAndAngle(matrix0, matrix1);

  return dgVector(
      matrix0.m_up.Scale(m_jointForce[0])
          + matrix0.m_right.Scale(m_jointForce[1])
          + matrix0.m_up.Scale(m_jointForce[2])
          + matrix0.m_right.Scale(m_jointForce[3]));
}

dgUnsigned32 dgUniversalConstraint::JacobianDerivative(
    dgContraintDescritor& params)
{
  dgInt32 ret;
  dgFloat32 sinAngle;
  dgFloat32 cosAngle;
  dgMatrix matrix0;
  dgMatrix matrix1;

  CalculateGlobalMatrixAndAngle(matrix0, matrix1);

  const dgVector& dir0 = matrix0.m_front;
  const dgVector& dir1 = matrix1.m_up;
  dgVector dir2(dir0 * dir1);

  dgVector dir3(dir2 * dir0);
  dir3 = dir3.Scale(dgRsqrt (dir3 % dir3));

  const dgVector& p0 = matrix0.m_posit;
  const dgVector& p1 = matrix1.m_posit;

  dgVector q0(p0 + dir3.Scale(MIN_JOINT_PIN_LENGTH));
  dgVector q1(p1 + dir1.Scale(MIN_JOINT_PIN_LENGTH));

  dgPointParam pointDataP;
  dgPointParam pointDataQ;
  InitPointParam(pointDataP, m_stiffness, p0, p1);
  InitPointParam(pointDataQ, m_stiffness, q0, q1);

  CalculatePointDerivative(0, params, dir0, pointDataP, &m_jointForce[0]);
  CalculatePointDerivative(1, params, dir1, pointDataP, &m_jointForce[1]);
  CalculatePointDerivative(2, params, dir2, pointDataP, &m_jointForce[2]);
  CalculatePointDerivative(3, params, dir0, pointDataQ, &m_jointForce[3]);
  ret = 4;

//	dgVector sinAngle0 (matrix1.m_up * matrix0.m_up);
//	m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f));
//	if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) {
//		m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0;
//	}

  sinAngle = (matrix1.m_up * matrix0.m_up) % matrix0.m_front;
  cosAngle = matrix0.m_up % matrix1.m_up;
//	_ASSERTE (dgAbsf (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
  m_angle0 = dgAtan2 (sinAngle, cosAngle);

//	dgVector sinAngle1 (matrix0.m_front * matrix1.m_front);
//	m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f));
//	if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) {
//		m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1;
//	}

  sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
  cosAngle = matrix0.m_front % matrix1.m_front;
//	_ASSERTE (dgAbsf (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
  m_angle1 = dgAtan2 (sinAngle, cosAngle);

  if (m_jointAccelFnt)
  {
    dgUnsigned32 code;
    dgJointCallBackParam axisParam[2];

    // linear acceleration
    axisParam[0].m_accel = dgFloat32(0.0f);
    axisParam[0].m_timestep = params.m_timestep;
    axisParam[0].m_minFriction = DG_MIN_BOUND;
    axisParam[0].m_maxFriction = DG_MAX_BOUND;

    // angular acceleration
    axisParam[1].m_accel = dgFloat32(0.0f);
    axisParam[1].m_timestep = params.m_timestep;
    axisParam[1].m_minFriction = DG_MIN_BOUND;
    axisParam[1].m_maxFriction = DG_MAX_BOUND;

    code = m_jointAccelFnt(*this, axisParam);
    if (code & 1)
    {
      if ((axisParam[0].m_minFriction > DG_MIN_BOUND)
          || (axisParam[0].m_maxFriction < DG_MAX_BOUND))
      {
        params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
        params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
        params.m_forceBounds[ret].m_normalIndex =
            DG_BILATERAL_FRICTION_CONSTRAINT;
      }

//			CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]); 
      CalculateAngularDerivative(ret, params, dir0, m_stiffness,
          dgFloat32(0.0f), &m_jointForce[ret]);
      //params.m_jointAccel[ret] = axisParam[0].m_accel;
      SetMotorAcceleration(ret, axisParam[0].m_accel, params);
      ret++;
    }

    if (code & 2)
    {
      if ((axisParam[1].m_minFriction > DG_MIN_BOUND)
          || (axisParam[1].m_maxFriction < DG_MAX_BOUND))
      {
        params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
        params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
        params.m_forceBounds[ret].m_normalIndex =
            DG_BILATERAL_FRICTION_CONSTRAINT;
      }
      CalculateAngularDerivative(ret, params, dir1, m_stiffness,
          dgFloat32(0.0f), &m_jointForce[ret]);
      //params.m_jointAccel[ret] = axisParam[1].m_accel;
      SetMotorAcceleration(ret, axisParam[1].m_accel, params);
      ret++;

    }
  }
  return dgUnsigned32(ret);
}

