I have a system that is giving me a point in 3D space (call it (x, y, z)) and a quaternion (call it (qw, qx, qy, qz)). I want to create a point at (x+1, y, z), and then rotate that point using the quaternion. How would I do this?
The specific application that I am using this for is with a Kinect. The Kinect produces the x,y,z points and provides the quaternion data. What I am trying to do then is add axes arrows to the kinect output so that we can visually see how "noisy" the kinect output is.
The answer to this question starts with the answer in this thread. Specifically, the formula posted as P' = Q(P-G)Q'+G, where P is the coordinates of the point being rotated, G is the point around which P is being rotated, Q is the quaternion, Q' is the quaternion inverse, and P' is the new location of the point after rotation.
The next step then is to break to formula into components.
Q = <a, b, c, d>, where a is the scalar component of the quaternion, b is the x component, c is the y component, and d is the z component.
Q' = <a/(a2+b2+c2+d2), -b/(a2+b2+c2+d2), -c/(a2+b2+c2+d2), -d/(a2+b2+c2+d2)>
G = (x, y, z)
P = (x+m, y+n, z+o)
By taking and rewriting all of the above with i, j, k, we get an equation that we can use with the formula at the start. Specifically, i denotes the x coordinates, j the y coordinates, and k the z coordinates. Therefore, we end up with the following components:
Q = a+bi+cj+dk
Q' = (a-bi-cj-dk)/(a2+b2+c2+d2)
G = xi+yj+zk
P = (x+m)i+(y+n)j+(z+o)k
After that, everything else is just algebra.
P' = (a+bi+cj+dk)((x+m)i+(y+n)j+(z+o)k-(xi+yj+zk))(a-bi-cj-dk)/(a2+b2+c2+d2)+xi+yj+zk
P' = 1/(a2+b2+c2+d2) * (a+bi+cj+dk)(mi+nj+ok)(a-bi-cj-dk)+xi+yj+zk
after multiplying everything out and condensing it, we get the following results:
P' = 1/(a2+b2+c2+d2) * (((a2+b2-c2-d2)m+2(bc-ad)n+2(ac+bd)o)i + (2(ad+bc)m+(a2-b2+c2-d2)n+2(cd-ab)o)j + (2(bd-ac)m+2(ab+cd)n+(a2-b2-c2+d2)o)k)+xi+yj+zk
Breaking this into component form, P' = (x', y', z'), then:
x' = x+1/(a2+b2+c2+d2)*((a2+b2-c2-d2)m+2(bc-ad)n+2(ac+bd)o)
y' = y+1/(a2+b2+c2+d2)*(2(ad+bc)m+(a2-b2+c2-d2)n+2(cd-ab)o)
z' = z+1/(a2+b2+c2+d2)*(2(bd-ac)m+2(ab+cd)n+(a2-b2-c2+d2)o)
From this then, we can easily just plug in our point of rotation, our offset from the point of rotation, and the quaternion data being received to determine the offset point's new position after rotation.