Friday, June 19, 2009

Calculating orientation

After looking at the API the getRotationMatrix method is really simple, it just assumes the acceleratometer data indicates down(only precisely true when you aren't moving it at all) and then the cross product of down and the geomagnetic reading gives you a vector pointing east. Another cross prouct between down and easy gives you a vector pointing north. Normalize them and then they make up the rotation matrix.

Typically aligning one coordinate system to another by a rotation to just match a unit vector means there are an infinite number of possible rotations. IE you could spin the coordinate system about that vector and any rotation would satisfy the constraints. But for my game I can add the constraint that they are holding the phone in landscape view and not rotating it, so rotation around the Z axis is 0, and I only need to find rotation around Y and Z to calculate the orientation.

So the equation representing the rotation of the device is M' = Rx * Ry * M, where M' is the measured magnetic field in the coordinate frame of the device and M is the reference magnetic field given by the WMM.

Expanding this out yields(where sx,cx,sy,cy are sin(x), cos(x)...):

[M'x] [cy, 0, -sy] [Mx]
[M'y]=[sx*sy, cx, sx*cy]*[My]
[M'z] [cx*sy,-sx, cx*cy] [Mz]

[M'x] [Mx*cy - Mz*sy]
[M'y]=[My*cx + (Mz*cy + Mx*sy)*sx]
[M'z] [(Mz*cy + Mx*sy)*cx - My*sx]

Using some handy trig identities you can use the M'x line to solve for y and then substitute that in either of the other two equations to solve for x. ie for y it would be

y = asin(-M'x/sqrt(Mz^2+Mx^2)) + atan(Mx/Mz)

A = (Mz*cy + Mx*sy)
x = asin(M'y/sqrt(A^2+My^2)) - atan(My/A)

Although this method requires 2 asin and 1 atan calculations, so I'm not sure how fast/stable this approach will be.

1 comment: