Get angle from setMotor rotations

Hi All,

I’m building a simulator ( for two wheels differential drive robots. These robots are commonly used in competitions such as First Lego League and World Robot Olympaid.

To maximize realism, I’m using a hinge joint for the wheels and driving them using setMotor. Works ok with Ammo, but I also need to know how much the wheels has rotated (…the real robots have a rotation encoder to track wheel rotations).

After a couple of hours reading up on quaternions, I managed to work something out using…

// s and e are the start and end rotationQuaternions of the wheel mesh
function getRotation(s, e) {
var r = e.multiply(BABYLON.Quaternion.Inverse(s));

var axis0 = new BABYLON.Quaternion(0,1,0,0);
var axis1 = s.multiply(axis0).multiply(BABYLON.Quaternion.Inverse(s));
var axis2 = e.multiply(axis0).multiply(BABYLON.Quaternion.Inverse(e));

var v1 = new BABYLON.Vector3(axis1.x, axis1.y, axis1.z);
var v2 = new BABYLON.Vector3(axis2.x, axis2.y, axis2.z);

var q1_xyz = v1.cross(v2);
var q1_w = 1 + BABYLON.Vector3.Dot(v1, v2);
var q1 = new BABYLON.Quaternion(q1_xyz.x, q1_xyz.y, q1_xyz.z, q1_w);

var q2 = BABYLON.Quaternion.Inverse®.multiply(q1);
var d = 2 * Math.acos(q2.w)

return d;

Results looks correct, but this seems way too complicated. Am I doing it wrong? Is there a better way? Perhaps a getMotorRotation() hidden somewhere?

As far as I know there is no built in function. Your way is the way to go.