4 * Copyright (c) 2014 Samsung Electronics Co., Ltd.
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
10 * http://www.apache.org/licenses/LICENSE-2.0
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
20 #if defined (_ROTATION_MATRIX_H) && defined (_MATRIX_H)
23 #define ROT_MAT_ROWS 3
24 #define ROT_MAT_COLS 3
26 template <typename T> T get_sign(T val)
28 return (val >= (T) 0) ? (T) 1 : (T) -1;
31 template <typename TYPE>
32 rotation_matrix<TYPE>::rotation_matrix() : m_rot_mat(ROT_MAT_ROWS, ROT_MAT_COLS)
37 template <typename TYPE>
38 rotation_matrix<TYPE>::rotation_matrix(const matrix<TYPE> m)
43 template <typename TYPE>
44 rotation_matrix<TYPE>::rotation_matrix(const int rows, const int cols, TYPE *mat_data)
46 matrix<TYPE> m(rows, cols, mat_data);
50 template <typename TYPE>
51 rotation_matrix<TYPE>::rotation_matrix(const rotation_matrix<TYPE>& rm)
53 m_rot_mat = rm.m_rot_mat;
56 template <typename TYPE>
57 rotation_matrix<TYPE>::~rotation_matrix()
62 template <typename TYPE>
63 rotation_matrix<TYPE> rotation_matrix<TYPE>::operator =(const rotation_matrix<TYPE>& rm)
65 m_rot_mat = rm.m_rot_mat;
71 rotation_matrix<T> quat2rot_mat(quaternion<T> q)
74 T R[ROT_MAT_ROWS][ROT_MAT_COLS];
76 w = q.m_quat.m_vec[0];
77 x = q.m_quat.m_vec[1];
78 y = q.m_quat.m_vec[2];
79 z = q.m_quat.m_vec[3];
81 R[0][0] = (2 * w * w) - 1 + (2 * x * x);
82 R[0][1] = 2 * ((x * y) + (w * z));
83 R[0][2] = 2 * ((x * z) - (w * y));
84 R[1][0] = 2 * ((x * y) - (w * z));
85 R[1][1] = (2 * w * w) - 1 + (2 * y * y);
86 R[1][2] = 2 * ((y * z) + (w * x));
87 R[2][0] = 2 * ((x * z) + (w * y));
88 R[2][1] = 2 * ((y * z) - (w * x));
89 R[2][2] = (2 * w * w) - 1 + (2 * z * z);
91 rotation_matrix<T> rm(ROT_MAT_ROWS, ROT_MAT_COLS, &R[0][0]);
97 quaternion<T> rot_mat2quat(rotation_matrix<T> rm)
101 T diag_sum = rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + rm.m_rot_mat.m_mat[2][2];
105 T val = (T) 0.5 / sqrt(diag_sum + (T) 1.0);
107 q1 = ( rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) * val;
108 q2 = ( rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) * val;
109 q3 = ( rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) * val;
113 if ( rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[1][1] && rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[2][2] )
115 T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[2][2]);
116 q0 = (rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) / val;
118 q2 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val;
119 q3 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val;
121 else if (rm.m_rot_mat.m_mat[1][1] > rm.m_rot_mat.m_mat[2][2])
123 T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[2][2]);
124 q0 = (rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) / val;
125 q1 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val;
127 q3 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val;
131 T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[2][2] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] );
132 q0 = (rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) / val;
133 q1 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val;
134 q2 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val;
139 quaternion<T> q(q0, q1, q2, q3);
146 #endif /* _ROTATION_MATRIX_H */