[ODE] Amotor fix (dJointAddAMotorTorques bug)

[Patrick McColgan]

Hello, having had struggled with amotors in the past I think I've found 
a mistake in joint.cpp, a mistake which makes amotors of more that 1 
axis unusable.

On searching the mailing list I found Peng Zhao had pointed it out last 
year (http://q12.org/pipermail/ode/2003-December/010569.html) but it 
appears to have been overlooked.

Anyway in joint.cpp in the function dJointAddAMotorTorques are the 
following lines:

/  if (joint->num >= 2) {
    axes[0][0] += axes[1][0] * torque2;
    axes[0][1] += axes[1][0] * torque2;
    axes[0][2] += axes[1][0] * torque2;
    if (joint->num >= 3) {
      axes[0][0] += axes[2][0] * torque3;
      axes[0][1] += axes[2][0] * torque3;
      axes[0][2] += axes[2][0] * torque3;
    }
  }

/This multiplies the given torque of the second and third axis by the x 
component of the axis and adds it to overall toque of the motor in all 
directions.

Surely it should be:

/  if (joint->num >= 2) {
    axes[0][0] += axes[1][0] * torque2;
    axes[0][1] += axes[1][1] * torque2;
    axes[0][2] += axes[1][2] * torque2;
    if (joint->num >= 3) {
      axes[0][0] += axes[2][0] * torque3;
      axes[0][1] += axes[2][1] * torque3;
      axes[0][2] += axes[2][2] * torque3;
    }
  }
/
This modification certainly gave me the results I expected of the AMotor.

Could this please be verified.  Thanks.

Paddy