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

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
dgBallConstraint::dgBallConstraint() :
    dgBilateralConstraint()
{

//	dgBallConstraint* constraint;

//	dgBallConstraintArray& array = * world;
//	constraint = array.GetElement();
  _ASSERTE((((dgUnsigned64) &m_localMatrix0) & 15) == 0);

  //constraint->SetStiffness (dgFloat32 (0.5f));
  m_maxDOF = 6;
  m_jointUserCallback = NULL;
  m_constId = dgBallConstraintId;
  m_ballLimits = 0;
  m_angles = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
      dgFloat32(0.0f));
}

dgBallConstraint::~dgBallConstraint()
{
}

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


 dgBallConstraintArray& array = * world;
 constraint = array.GetElement();
 _ASSERTE ((((dgUnsigned64) &constraint->m_localMatrix0) & 15) == 0);

 constraint->Init();

 //	constraint->SetStiffness (dgFloat32 (0.5f));
 constraint->m_maxDOF = 6;
 constraint->m_jointUserCallback = NULL;
 constraint->m_constId = dgBallConstraintId;
 constraint->m_ballLimits = 0;
 constraint->m_angles = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
 return constraint;
 }


 void dgBallConstraint::Remove(dgWorld* world)
 {
 dgBallConstraintArray& array = *world;
 dgBilateralConstraint::Remove (world);
 array.RemoveElement (this);
 }
 */

void dgBallConstraint::SetJointParameterCallBack(dgBallJointFriction callback)
{
  m_jointUserCallback = callback;
}

dgVector dgBallConstraint::GetJointAngle() const
{
  return m_angles;
}

dgVector dgBallConstraint::GetJointOmega() const
{
  _ASSERTE(m_body0);
  _ASSERTE(m_body1);
  const dgMatrix& matrix = m_body0->GetMatrix();

  dgVector dir0(matrix.RotateVector(m_localMatrix0[0]));
  dgVector dir1(matrix.RotateVector(m_localMatrix0[1]));
  dgVector dir2(matrix.RotateVector(m_localMatrix0[2]));

  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();
//	}

  dgVector relOmega(omega0 - omega1);
  return dgVector(relOmega % dir0, relOmega % dir1, relOmega % dir2,
      dgFloat32(0.0f));

}

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

  CalculateGlobalMatrixAndAngle(matrix0, matrix1);
  return dgVector(
      matrix0.m_front.Scale(m_jointForce[0])
          + matrix0.m_up.Scale(m_jointForce[1])
          + matrix0.m_right.Scale(m_jointForce[2]));
}

bool dgBallConstraint::GetTwistLimitState() const
{
  return m_twistLimit;
}

bool dgBallConstraint::GetConeLimitState() const
{
  return m_coneLimit;
}

bool dgBallConstraint::GetLatealLimitState() const
{
  return m_lateralLimit;
}

void dgBallConstraint::SetTwistLimitState(bool state)
{
  m_twistLimit = dgUnsigned32(state);
}

void dgBallConstraint::SetConeLimitState(bool state)
{
  m_coneLimit = dgUnsigned32(state);
}

void dgBallConstraint::SetLatealLimitState(bool state)
{
  m_lateralLimit = dgUnsigned32(state);
}

void dgBallConstraint::SetPivotPoint(const dgVector &pivot)
{
  _ASSERTE(m_body0);
  _ASSERTE(m_body1);
  const dgMatrix& matrix = m_body0->GetMatrix();

  dgVector pin(pivot - matrix.m_posit);
  if ((pin % pin) < dgFloat32(1.0e-3f))
  {
    pin = matrix.m_front;
  }

  SetPivotAndPinDir(pivot, pin);

  dgMatrix matrix0;
  dgMatrix matrix1;
  CalculateGlobalMatrixAndAngle(matrix0, matrix1);
  SetLimits(matrix0.m_front, -dgPI * dgFloat32(0.5f), dgPI * dgFloat32(0.5f),
      dgPI * dgFloat32(0.5f), matrix0.m_right, dgFloat32(0.0f),
      dgFloat32(0.0f));
}

void dgBallConstraint::SetLimits(const dgVector& coneDir,
    dgFloat32 minConeAngle, dgFloat32 maxConeAngle, dgFloat32 maxTwistAngle,
    const dgVector& bilateralDir, dgFloat32 negativeBilateralConeAngle__,
    dgFloat32 positiveBilateralConeAngle__)
{
  dgMatrix matrix0;
  dgMatrix matrix1;
  CalculateGlobalMatrixAndAngle(matrix0, matrix1);

  _ASSERTE(m_body0);
  _ASSERTE(m_body1);
  const dgMatrix& body0_Matrix = m_body0->GetMatrix();

  dgVector lateralDir(bilateralDir * coneDir);
  if ((lateralDir % lateralDir) < dgFloat32(1.0e-3f))
  {
    dgMatrix tmp(coneDir);
    lateralDir = tmp.m_up;
  }

  m_localMatrix0.m_front = body0_Matrix.UnrotateVector(coneDir);
  m_localMatrix0.m_up = body0_Matrix.UnrotateVector(lateralDir);
  m_localMatrix0.m_posit = body0_Matrix.UntransformVector(matrix1.m_posit);

  m_localMatrix0.m_front =
      m_localMatrix0.m_front.Scale(
          dgFloat32(
              1.0f) / dgSqrt (m_localMatrix0.m_front % m_localMatrix0.m_front));
  m_localMatrix0.m_up = m_localMatrix0.m_up.Scale(
      dgFloat32(1.0f) / dgSqrt (m_localMatrix0.m_up % m_localMatrix0.m_up));
  m_localMatrix0.m_right = m_localMatrix0.m_front * m_localMatrix0.m_up;

  m_localMatrix0.m_front.m_w = dgFloat32(0.0f);
  m_localMatrix0.m_up.m_w = dgFloat32(0.0f);
  m_localMatrix0.m_right.m_w = dgFloat32(0.0f);
  m_localMatrix0.m_posit.m_w = dgFloat32(1.0f);

//	dgMatrix body1_Matrix (dgGetIdentityMatrix());
//	if (m_body1) {
//		body1_Matrix = m_body1->GetMatrix();
//	}
  const dgMatrix& body1_Matrix = m_body1->GetMatrix();

  m_twistAngle = ClampValue(maxTwistAngle, dgFloat32(5.0f) * dgDEG2RAD,
      dgFloat32(90.0f) * dgDEG2RAD);
  m_coneAngle = ClampValue((maxConeAngle - minConeAngle) * dgFloat32(0.5f),
      dgFloat32(5.0f) * dgDEG2RAD, 175.0f * dgDEG2RAD);
  m_coneAngleCos = dgCos (m_coneAngle);

  dgMatrix coneMatrix(
      dgPitchMatrix((maxConeAngle + minConeAngle) * dgFloat32(0.5f)));

  m_localMatrix0 = coneMatrix * m_localMatrix0;

  m_localMatrix1 = m_localMatrix0 * body0_Matrix * body1_Matrix.Inverse();

}

dgUnsigned32 dgBallConstraint::JacobianDerivative(dgContraintDescritor& params)
{
  dgInt32 ret;
  dgFloat32 relVelocErr;
  dgFloat32 penetrationErr;
  dgMatrix matrix0;
  dgMatrix matrix1;

  if (m_jointUserCallback)
  {
    m_jointUserCallback(*this, params.m_timestep);
  }

  dgVector angle(CalculateGlobalMatrixAndAngle(matrix0, matrix1));
  m_angles = angle.Scale(-dgFloat32(1.0f));

  const dgVector& dir0 = matrix0.m_front;
  const dgVector& dir1 = matrix0.m_up;
  const dgVector& dir2 = matrix0.m_right;
  const dgVector& p0 = matrix0.m_posit;
  const dgVector& p1 = matrix1.m_posit;

  dgPointParam pointData;
  InitPointParam(pointData, m_stiffness, p0, p1);
  CalculatePointDerivative(0, params, dir0, pointData, &m_jointForce[0]);
  CalculatePointDerivative(1, params, dir1, pointData, &m_jointForce[1]);
  CalculatePointDerivative(2, params, dir2, pointData, &m_jointForce[2]);
  ret = 3;

  if (m_twistLimit)
  {
    if (angle.m_x > m_twistAngle)
    {
      dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);

      const dgVector& dir = matrix0.m_right;
      CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      relVelocErr = velocError % dir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
    else if (angle.m_x < -m_twistAngle)
    {
      dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);
      dgVector dir(matrix0.m_right.Scale(-dgFloat32(1.0f)));
      CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      relVelocErr = velocError % dir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH * (-m_twistAngle - angle.m_x);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
  }

  if (m_coneLimit)
  {

    dgFloat32 coneCos;
    coneCos = matrix0.m_front % matrix1.m_front;
    if (coneCos < m_coneAngleCos)
    {
      dgVector p0(
          matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);

      dgVector tangentDir(matrix0.m_front * matrix1.m_front);
      tangentDir = tangentDir.Scale(
          dgRsqrt ((tangentDir % tangentDir) + 1.0e-8f));
      CalculatePointDerivative(ret, params, tangentDir, pointData,
          &m_jointForce[ret]);
      ret++;

      dgVector normalDir(tangentDir * matrix0.m_front);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      //restitution = contact.m_restitution;
      relVelocErr = velocError % normalDir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH
          * (dgAcos (GetMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      CalculatePointDerivative(ret, params, normalDir, pointData,
          &m_jointForce[ret]);
      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
  }

  return dgUnsigned32(ret);
}

