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
  */
  
  
  
  // CustomPulley.cpp: implementation of the CustomPulley class.
  //
  //////////////////////////////////////////////////////////////////////
  #include "CustomJointLibraryStdAfx.h"
  #include "CustomPulley.h"
  
  //////////////////////////////////////////////////////////////////////
  // Construction/Destruction
  //////////////////////////////////////////////////////////////////////
  CustomPulley::CustomPulley(
  	dFloat gearRatio, 
  	const dVector& childPin, 
  	const dVector& parentPin, 
  	NewtonBody* child, 
  	NewtonBody* parent)
  	:NewtonCustomJoint(1, child, parent)
  {
  	m_gearRatio = gearRatio;
  
  	// calculate the two local matrix of the pivot point
  //	dVector pivot (0.0f, 0.0f, 0.0f);
  
  	dMatrix dommyMatrix;
  	// calculate the local matrix for body body0
  	dMatrix pinAndPivot0 (dgGrammSchmidt (childPin));
  //	CalculateLocalMatrix (pivot, childPin, m_localMatrix0, dommyMatrix);
  	CalculateLocalMatrix (pinAndPivot0, m_localMatrix0, dommyMatrix);
  
  
  	// calculate the local matrix for body body1  
  //_ASSERTE (0);
  	dMatrix pinAndPivot1 (dgGrammSchmidt (parentPin));
  //	CalculateLocalMatrix (pivot, parentPin, dommyMatrix, m_localMatrix1);
  	CalculateLocalMatrix (pinAndPivot1, dommyMatrix, m_localMatrix1);
  }
  
  CustomPulley::~CustomPulley()
  {
  	
  }
  
  
  void CustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
  {
  	dFloat w0;
  	dFloat w1;
  	dFloat relAccel;
  	dFloat relVeloc;
  	dVector veloc0;
  	dVector veloc1;
  	dMatrix matrix0;
  	dMatrix matrix1;
  	dFloat jacobian0[6];
  	dFloat jacobian1[6];
  
  	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
  	CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
  	
  	// calculate the angular velocity for both bodies
  	NewtonBodyGetVelocity(m_body0, &veloc0[0]);
  	NewtonBodyGetVelocity(m_body1, &veloc1[0]);
  
  	// get angular velocity relative to the pin vector
  	w0 = veloc0 % matrix0.m_front;
  	w1 = veloc1 % matrix1.m_front;
  
  	// establish the gear equation.
  	relVeloc = w0 + m_gearRatio * w1;
  
  	// calculate the relative angular acceleration by dividing by the time step
  	// ideally relative acceleration should be zero, but is practice there will always 
  	// be a small difference in velocity that need to be compensated. 
  	// using the full acceleration will make the to over show a oscillate 
  	// so use only fraction of the acceleration
  	relAccel = - 0.3f * relVeloc / timestep;
  
  	// set the linear part of Jacobian 0 to translational pin vector	
  	jacobian0[0] = 	matrix0.m_front[0];
  	jacobian0[1] = 	matrix0.m_front[1];
  	jacobian0[2] = 	matrix0.m_front[2];
  
  	// set the rotational part of Jacobian 0 to zero
  	jacobian0[3] = 	0.0f;
  	jacobian0[4] = 	0.0f;
  	jacobian0[5] = 	0.0f;
  
  	// set the linear part of Jacobian 1 to translational pin vector	
  	jacobian1[0] = 	matrix1.m_front[0];
  	jacobian1[1] = 	matrix1.m_front[1];
  	jacobian1[2] = 	matrix1.m_front[2];
  
  	// set the rotational part of Jacobian 1 to zero
  	jacobian1[3] = 	0.0f;
  	jacobian1[4] = 	0.0f;
  	jacobian1[5] = 	0.0f;
  
  	// add a angular constraint
  	NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
  
  	// set the desired angular acceleration between the two bodies
  	NewtonUserJointSetRowAcceleration (m_joint, relAccel);
  }
  
  
  void CustomPulley::GetInfo (NewtonJointRecord* info) const
  {
  	strcpy (info->m_descriptionType, "pulley");
  
  	info->m_extraParameters[0] = m_gearRatio;
  
  	info->m_attachBody_0 = m_body0;
  	info->m_attachBody_1 = m_body1;
  
  	info->m_minLinearDof[0] = -FLT_MAX;
  	info->m_maxLinearDof[0] = FLT_MAX;
  
  	info->m_minLinearDof[1] = -FLT_MAX;
  	info->m_maxLinearDof[1] = FLT_MAX;
  
  	info->m_minLinearDof[2] = -FLT_MAX;
  	info->m_maxLinearDof[2] = FLT_MAX;
  
  	info->m_minAngularDof[0] = -FLT_MAX;
  	info->m_maxAngularDof[0] =  FLT_MAX;
  
  	info->m_minAngularDof[1] = -FLT_MAX;;
  	info->m_maxAngularDof[1] =  FLT_MAX;
  
  	info->m_minAngularDof[2] = -FLT_MAX;;
  	info->m_maxAngularDof[2] =  FLT_MAX;
  
  	memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
  	memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
  }
  