text: /* Copyright (c) <2009> <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
  */
  
  
  
  // CustomHinge.cpp: implementation of the CustomHinge class.
  //
  //////////////////////////////////////////////////////////////////////
  #include "CustomJointLibraryStdAfx.h"
  #include "CustomHinge.h"
  
  //////////////////////////////////////////////////////////////////////
  // Construction/Destruction
  //////////////////////////////////////////////////////////////////////
  
  #define MIN_JOINT_PIN_LENGTH	50.0f
  
  
  CustomHinge::CustomHinge (const dMatrix& pinAndPivotFrame, const NewtonBody* child, const NewtonBody* parent)
  	:NewtonCustomJoint(6, child, parent), m_curJointAngle()
  {
  	m_limitsOn = false;
  	m_jointOmega = 0.0f;
  	m_minAngle = -45.0f * 3.141592f / 180.0f;
  	m_maxAngle =  45.0f * 3.141592f / 180.0f;
  
  //	// the joint current angle is zero at joint creation time
  //	m_curJointAngle = 0.0f;
  
  	// calculate the two local matrix of the pivot point
  	CalculateLocalMatrix (pinAndPivotFrame, m_localMatrix0, m_localMatrix1);
  }
  
  CustomHinge::~CustomHinge()
  {
  }
  
  
  void CustomHinge::EnableLimits(bool state)
  {
  	m_limitsOn = state;
  }
  
  void CustomHinge::SetLimis(dFloat minAngle, dFloat maxAngle)
  {
  	//_ASSERTE (minAngle < 0.0f);
  	//_ASSERTE (maxAngle > 0.0f);
  	m_minAngle = minAngle;
  	m_maxAngle = maxAngle;
  }
  
  
  dFloat CustomHinge::GetJointAngle () const
  {
  	return m_curJointAngle.m_angle;
  }
  
  dVector CustomHinge::GetPinAxis () const
  {
  	dMatrix matrix;
  	NewtonBodyGetMatrix (m_body0, &matrix[0][0]);
  	return matrix.RotateVector (m_localMatrix0.m_front);
  }
  
  dFloat CustomHinge::GetJointOmega () const
  {
  	return m_jointOmega;
  }
  
  
  
  void CustomHinge::SubmitConstraints (dFloat timestep, int threadIndex)
  {
  //	dFloat angle;
  //	dFloat sinAngle;
  //	dFloat cosAngle;
  	dMatrix matrix0;
  	dMatrix matrix1;
  
  	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
  	CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
  
  	// Restrict the movement on the pivot point along all tree orthonormal direction
  	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
  	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
  	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
  	
  	// get a point along the pin axis at some reasonable large distance from the pivot
  	dVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
  	dVector q1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
  
  	// two constraints row perpendicular to the pin vector
   	NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
  	NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
  
  	// the joint angle can be determine by getting the angle between any two non parallel vectors
  	dFloat angle;
  	dFloat sinAngle;
  	dFloat cosAngle;
  	sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
  	cosAngle = matrix0.m_up % matrix1.m_up;
  	angle = m_curJointAngle.CalculateJointAngle (cosAngle, sinAngle);
  
  	// if limit are enable ...
  	if (m_limitsOn) {
  
  		// the joint angle can be determine by getting the angle between any two non parallel vectors
  //		sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
  //		cosAngle = matrix0.m_up % matrix1.m_up;
  //		angle = dAtan2 (sinAngle, cosAngle);
  
  
  //		if (angle < m_minAngle) {
  		if (angle < m_minAngle) {
  			dFloat relAngle;
  
  //			relAngle = angle - m_minAngle;
  			relAngle = angle - m_minAngle;
  			// the angle was clipped save the new clip limit
  			m_curJointAngle.m_angle = m_minAngle;
  
  			// tell joint error will minimize the exceeded angle error
  			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
  
  			// need high stiffness here
  			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
  
  			// allow the joint to move back freely 
  			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
  
  
  		} else if (angle  > m_maxAngle) {
  			dFloat relAngle;
  
  //			relAngle = angle - m_maxAngle;
  
  			relAngle = angle - m_maxAngle;
  
  			// the angle was clipped save the new clip limit
  			m_curJointAngle.m_angle = m_maxAngle;
  			
  			// tell joint error will minimize the exceeded angle error
  			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
  
  			// need high stiffness here
  			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
  
  			// allow the joint to move back freely
  			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
  
  		}
  	}
  
  	// save the current joint Omega
  	dVector omega0(0.0f, 0.0f, 0.0f, 0.0f);
  	dVector omega1(0.0f, 0.0f, 0.0f, 0.0f);
  	NewtonBodyGetOmega(m_body0, &omega0[0]);
  	if (m_body1) {
  		NewtonBodyGetOmega(m_body1, &omega1[0]);
  	}
  	m_jointOmega = (omega0 - omega1) % matrix0.m_front;
   }
  
  
  void CustomHinge::GetInfo (NewtonJointRecord* info) const
  {
  	strcpy (info->m_descriptionType, "hinge");
  
  	info->m_attachBody_0 = m_body0;
  	info->m_attachBody_1 = m_body1;
  
  	info->m_minLinearDof[0] = 0.0f;
  	info->m_maxLinearDof[0] = 0.0f;
  
  	info->m_minLinearDof[1] = 0.0f;
  	info->m_maxLinearDof[1] = 0.0f;;
  
  	info->m_minLinearDof[2] = 0.0f;
  	info->m_maxLinearDof[2] = 0.0f;
  
  
  	// the joint angle can be determine by getting the angle between any two non parallel vectors
  
  
  	if (m_limitsOn) {
  		dFloat angle;
  		dFloat sinAngle;
  		dFloat cosAngle;
  		dMatrix matrix0;
  		dMatrix matrix1;
  		// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
  		CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
  		sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
  		cosAngle = matrix0.m_up % matrix1.m_up;
  		angle = dAtan2 (sinAngle, cosAngle);
  		info->m_minAngularDof[0] = (m_minAngle - angle) * 180.0f / 3.141592f ;
  		info->m_maxAngularDof[0] = (m_maxAngle - angle) * 180.0f / 3.141592f ;
  	} else {
  		info->m_minAngularDof[0] = -FLT_MAX ;
  		info->m_maxAngularDof[0] = FLT_MAX ;
  	}
  
  	info->m_minAngularDof[1] = 0.0f;
  	info->m_maxAngularDof[1] = 0.0f;
  
  	info->m_minAngularDof[2] = 0.0f;
  	info->m_maxAngularDof[2] = 0.0f;
  
  	memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
  	memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
  }
  