add 3x3 rotation matrix to quaternion function #265
Conversation
|
Thank you for the patch. Where is this new function used? I cannot find references to |
|
Hi @christian-rauch Thank you for reviewing
Yes. |
| @@ -453,6 +453,40 @@ static inline void TFN(s_mat_to_xyz)(const TNAME M[16], TNAME xyz[3]) | |||
| xyz[2] = M[11]; | |||
| } | |||
|
|
|||
| static inline void TFN(s_mat33_to_quat)(const TNAME M[9], TNAME q[4]) | |||
There was a problem hiding this comment.
Maybe call this function "rot_mat_to_quat"? "mat33" sounds as if it accepts any 3x3 matrix. Do you think we need to check that the matrix provided is actually a rotation matrix?
| if (T > 0.0000001) { | ||
| S = sqrt(T) * 2; | ||
| q[0] = (TNAME)(0.25 * S); | ||
| q[1] = (TNAME)((M[7] - M[5]) / S); | ||
| q[2] = (TNAME)((M[2] - M[6]) / S); | ||
| q[3] = (TNAME)((M[3] - M[1]) / S); | ||
| } else if (M[0] > M[4] && M[0] > M[8]) { // Column 0: | ||
| S = sqrt(1.0 + M[0] - M[4] - M[8]) * 2; | ||
| q[0] = (TNAME)((M[7] - M[5]) / S); | ||
| q[1] = (TNAME)(0.25 * S); | ||
| q[2] = (TNAME)((M[3] + M[1]) / S); | ||
| q[3] = (TNAME)((M[2] + M[6]) / S); | ||
| } else if (M[4] > M[8]) { // Column 1: | ||
| S = sqrt(1.0 + M[4] - M[0] - M[8]) * 2; | ||
| q[0] = (TNAME)((M[2] - M[6]) / S); | ||
| q[1] = (TNAME)((M[3] + M[1]) / S); | ||
| q[2] = (TNAME)(0.25 * S); | ||
| q[3] = (TNAME)((M[7] + M[5]) / S); | ||
| } else { // Column 2: | ||
| S = sqrt(1.0 + M[8] - M[0] - M[4]); | ||
| q[0] = (TNAME)((M[3] - M[1]) / S); | ||
| q[1] = (TNAME)((M[2] + M[6]) / S); | ||
| q[2] = (TNAME)((M[7] + M[5]) / S); | ||
| q[3] = (TNAME)(0.25 * S); | ||
| } |
There was a problem hiding this comment.
Let's try to avoid code duplication with mat_to_quat here. Either have mat_to_quat reuse this very function with a submatrix (the best option IMHO) or extend the 3x3 rotation to 4x4 transformation matrix and reuse mat_to_quat here.
In #90 (comment), @mkrogius suggests using
doubles_mat_to_quatto get a quaternion from the matrix. However, rotation matrix inapriltag_pose_tis a 3x3 matrix anddoubles_mat_to_quatexpects a 4x4 matrix. It would be easier to use if we have adoubles_mat33_to_quat.