#include "MyCustomMotorizedHinge.h"

// constructor.  we must pass the 2 bodies (2nd can be NULL), also the max DOF for the joint, up to 6.
MyCustomMotorizedHinge::MyCustomMotorizedHinge( OgreNewt::Body* child, OgreNewt::Body* parent, Ogre::Vector3 point, Ogre::Vector3 pin) 
	: OgreNewt::Hinge( child, parent, point, pin )
{
	// init our variables to keep track of the joint, so we get the joint orientation in local orientation of the bodies.
}


MyCustomMotorizedHinge::~MyCustomMotorizedHinge()
{
}

// the important function that applies the joint.
void MyCustomMotorizedHinge::submitConstraint(Ogre::Real timestep, int threadindex)
{
	// do some custom here!!

	//for this example make the hinge spin at constant velocity

	Ogre::Real desiredOmega = 10.0f * 10.0f * 3.1416 /180.0f; 

	Ogre::Real relativeOmega = 0.5f * (desiredOmega - getJointAngularVelocity().valueRadians());

	Ogre::Real acceleration = relativeOmega / timestep;
	Ogre::Vector3 pin (getJointPin());

	// add the axis constraint to control the pin velocity
	addAngularRow (Ogre::Radian (0.0f), pin);
	setRowAcceleration (acceleration);
}