[ODE] Code: applying torque to joints, and universal bug fix

[Martin C. Martin]

Hey all,

First, I found some bugs in the universal joint, when one of the bodies is
null.  Fixes are below.  Note that only body2 can be null, but I handle
body1 == null, just in case.  Second, there is only a single "getAnchor"
function per joint.  In a perfect world, the constraints will be perfectly
satisfied and the anchor point on each body will be in the same location. 
But when they diverge, it can be useful to get both, e.g. to measure how
badly the joint is coming apart.  Russ, does it make sense to add a
dJointGetUniversalAnchor2() function for this?  Same for ball and socket,
and maybe the others?

Finally, while there are still no motors on the universal joint, I've
whipped up some functions for applying torques along the axes of the
joints.  Look at the diagram of the universal joint in the docs:  It's as
if you had a motor on the end of each axis of the cross.  I've also done
it for the hinge joint.  Russ, people find these sorts of calculations
confusing, so it might be good to include these in the API.  I'm not sure
how you'd specify the three torques for the ball-and-socket joint, but the
other joints are straight forward.  This might really help people who want
to power their joints manually.

Below you'll find the fixed routines for joint.cpp, followed by the torque
functions.

Enjoy,
Martin


extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint,
					 dReal x, dReal y, dReal z)
{
  dUASSERT(joint,"bad joint argument");
  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a
universal");
  if (joint->node[0].body) {
    dReal q[4];
    q[0] = x;
    q[1] = y;
    q[2] = z;
    q[3] = 0;
    dNormalize3 (q);
    dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
  }
  else {
    joint->axis1[0] = x;
    joint->axis1[1] = y;
    joint->axis1[2] = z;
  }
  joint->axis1[3] = 0;
}


extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint,
					 dReal x, dReal y, dReal z)
{
  dUASSERT(joint,"bad joint argument");
  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a
universal");
  if (joint->node[1].body) {
    dReal q[4];
    q[0] = x;
    q[1] = y;
    q[2] = z;
    q[3] = 0;
    dNormalize3 (q);
    dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
  } 
  else {
    joint->axis2[0] = x;
    joint->axis2[1] = y;
    joint->axis2[2] = z;
  }
  joint->axis2[3] = 0;
}


extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint,
					 dVector3 result)
{
  dUASSERT(joint,"bad joint argument");
  dUASSERT(result,"bad result argument");
  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a
universal");
  if (joint->node[0].body) {
    dMULTIPLY0_331 (result, joint->node[0].body->R, joint->axis1);
  }
  else {
    result[0] = joint->axis1[0];
    result[1] = joint->axis1[1];
    result[2] = joint->axis1[2];
  }
}

extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint,
					 dVector3 result)
{
  dUASSERT(joint,"bad joint argument");
  dUASSERT(result,"bad result argument");
  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a
universal");
  if (joint->node[1].body) {
    dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2);
  }
  else {
    result[0] = joint->axis2[0];
    result[1] = joint->axis2[1];
    result[2] = joint->axis2[2];
  }
}


void ApplyTorquesToHinge(dJointID joint, dReal torque)
{
	dBodyID body;
	dVector3 axis, total_torque;

	dJointGetHingeAxis(joint, axis);

	total_torque[0] = axis[0] * torque;
	total_torque[1] = axis[1] * torque;
	total_torque[2] = axis[2] * torque;

	body = dJointGetBody(joint, 0);
	if (body)
		dBodyAddTorque(body, total_torque[0], total_torque[1], total_torque[2]);
	body = dJointGetBody(joint, 1);
	if (body)
		dBodyAddTorque(body, - total_torque[0], - total_torque[1], -
total_torque[2]);
}

void ApplyTorquesToUniversal(dJointID joint, dReal torque1, dReal torque2)
{
	dBodyID body;
	dVector3 axis1, axis2, total_torque;

	dJointGetUniversalAxis1(joint, axis1);
	dJointGetUniversalAxis2(joint, axis2);

	total_torque[0] = axis1[0] * torque1 + axis2[0] * torque2;
	total_torque[1] = axis1[1] * torque1 + axis2[1] * torque2;
	total_torque[2] = axis1[2] * torque1 + axis2[2] * torque2;

	body = dJointGetBody(joint, 0);
	if (body)
		dBodyAddTorque(body, total_torque[0], total_torque[1], total_torque[2]);
	body = dJointGetBody(joint, 1);
	if (body)
		dBodyAddTorque(body, - total_torque[0], - total_torque[1], -
total_torque[2]);
}