Quaternions to Rotation Matrix problem

Post Reply
JosPietersen
Posts: 1
Joined: Wed Mar 06, 2013 10:25 am

Quaternions to Rotation Matrix problem

Post by JosPietersen »

Hi,

I use the tracking tools to stream the position of a ridig body and its orientation to my application. In my application, I simply want to render the rigidbody as a small plane with the right orientation and position in space. I'm using Natnet in C# and some emgu functionalities for matrix operations. Positioning the rigidbody works perfectly but there is something wrong with the orientation. I use the same coordinate system as the tracking system so I just transform the quaternion to a rotation matrix and transform (rotate) the four corners of my plane using this rotation matrix. Everything works almost fine (turning the rigidbody around its axis, to the front and side) but there is one problem: When I first turn the rigid body 90 degrees and then turn the rigidbody to the front it will still turn to the side...

I'm trying to solve this issue for a couple of days already the thing is that I dont do a lot of things in my code so it must be something small... (maybe the transformation from quaternion to the rotation matrix? I found some alternatives on the internet to do this but none of them solved my problem).


private Emgu.CV.Matrix m_rotationMatrix = new Emgu.CV.Matrix(4, 4);
private Emgu.CV.Matrix[] m_corners3D = new Emgu.CV.Matrix[4];

public void updateRigidBody(NatNetML.RigidBodyData update)
{
m_rigidBody = update;
updateRotationMatrix();
calculatePositionCorners();
}

private void updateRotationMatrix()
{
Quaternion quat = new Quaternion(m_rigidBody.qx, m_rigidBody.qy, m_rigidBody.qz, m_rigidBody.qw);

m_rotationMatrix[0, 0] = 1.0f - 2.0f * quat.Y * quat.Y - 2.0f * quat.Z * quat.Z;
m_rotationMatrix[0, 1] = 2.0f * quat.X * quat.Y - 2.0f * quat.W * quat.Z;
m_rotationMatrix[0, 2] = 2.0f * quat.X * quat.Z + 2.0f * quat.W * quat.Y;
m_rotationMatrix[0, 3] = 0.0;
m_rotationMatrix[1, 0] = 2.0f * quat.X * quat.Y + 2.0f * quat.W * quat.Z;
m_rotationMatrix[1, 1] = 1.0f - 2.0f * quat.X * quat.X - 2.0f * quat.Z * quat.Z;
m_rotationMatrix[1, 2] = 2.0f * quat.Y * quat.Z - 2.0f * quat.W * quat.X;
m_rotationMatrix[1, 3] = 0.0;
m_rotationMatrix[2, 0] = 2.0f * quat.X * quat.Z - 2.0f * quat.W * quat.Y;
m_rotationMatrix[2, 1] = 2.0f * quat.Y * quat.Z + 2.0f * quat.W * quat.X;
m_rotationMatrix[2, 2] = 1.0f - 2.0f * quat.X * quat.X - 2.0f * quat.Y * quat.Y;
m_rotationMatrix[2, 3] = 0.0;
m_rotationMatrix[3, 0] = 0.0;
m_rotationMatrix[3, 1] = 0.0;
m_rotationMatrix[3, 2] = 0.0;
m_rotationMatrix[3, 3] = 1.0f;
}

private void calculatePositionCorners()
{
Emgu.CV.Matrix[] cornersOrigin = new Emgu.CV.Matrix[4];

for(int i = 0; i(4,1);

cornersOrigin[0][0, 0] = -m_widthTile/2.0;
cornersOrigin[0][1, 0] = 0.0;
cornersOrigin[0][2, 0] = -m_heightTile / 2.0;
cornersOrigin[0][3, 0] = 1.0;

cornersOrigin[1][0, 0] = m_widthTile / 2.0;
cornersOrigin[1][1, 0] = 0.0;
cornersOrigin[1][2, 0] = -m_heightTile / 2.0;
cornersOrigin[1][3, 0] = 1.0;

cornersOrigin[2][0, 0] = m_widthTile / 2.0;
cornersOrigin[2][1, 0] = 0.0;
cornersOrigin[2][2, 0] = m_heightTile / 2.0;
cornersOrigin[2][3, 0] = 1.0;

cornersOrigin[3][0, 0] = -m_widthTile / 2.0;
cornersOrigin[3][1, 0] = 0.0;
cornersOrigin[3][2, 0] = m_heightTile / 2.0;
cornersOrigin[3][3, 0] = 1.0;

for (int i = 0; i (4, 1);
m_corners3D = m_rotationMatrix * cornersOrigin;

m_corners3D[0, 0] += m_rigidBody.x;
m_corners3D[1, 0] += m_rigidBody.y;
m_corners3D[2, 0] += m_rigidBody.z;
}
}
Aranda
Posts: 3
Joined: Sun Jan 12, 2014 10:02 pm

Re: Quaternions to Rotation Matrix problem

Post by Aranda »

Not sure if this will help you buy I had exactly the same problem using the quaternion on a streamed rigid body in Unity. It was solved by re-ordering the quaternion components then negating the euler angle directions:

Code: Select all

			// Rigid body rotation streamed from Motive Tracker
			Quaternion rot = lastRigidBody.ori;

			// Re-order quaternion components
			rot = new Quaternion(rot.z, rot.y, rot.x, rot.w);
			
			// Invert pitch and yaw
			Vector3 euler = rot.eulerAngles;
			rot.eulerAngles = new Vector3(-euler.x, -euler.y, euler.z);

			// Apply rotation
			transform.rotation = rot;
Post Reply