98 yCError(QUATERNION,
"fromRotationMatrix() failed, matrix should be >= 3x3");
102 double tr = R(0, 0) + R(1, 1) + R(2, 2);
106 double sqtrp1 = sqrt(tr + 1.0);
107 double sqtrp12 = 2.0*sqtrp1;
109 q_x = (R(2, 1) - R(1, 2)) / sqtrp12;
110 q_y = (R(0, 2) - R(2, 0)) / sqtrp12;
111 q_z = (R(1, 0) - R(0, 1)) / sqtrp12;
113 else if ((R(1, 1)>R(0, 0)) && (R(1, 1)>R(2, 2)))
115 double sqdip1 = sqrt(R(1, 1) - R(0, 0) - R(2, 2) + 1.0);
119 sqdip1 = 0.5 / sqdip1;
122 q_w = (R(0, 2) - R(2, 0))*sqdip1;
123 q_x = (R(1, 0) + R(0, 1))*sqdip1;
124 q_z = (R(2, 1) + R(1, 2))*sqdip1;
126 else if (R(2, 2)>R(0, 0))
128 double sqdip1 = sqrt(R(2, 2) - R(0, 0) - R(1, 1) + 1.0);
132 sqdip1 = 0.5 / sqdip1;
135 q_w = (R(1, 0) - R(0, 1))*sqdip1;
136 q_x = (R(0, 2) + R(2, 0))*sqdip1;
137 q_y = (R(2, 1) + R(1, 2))*sqdip1;
141 double sqdip1 = sqrt(R(0, 0) - R(1, 1) - R(2, 2) + 1.0);
145 sqdip1 = 0.5 / sqdip1;
148 q_w = (R(2, 1) - R(1, 2))*sqdip1;
149 q_y = (R(1, 0) + R(0, 1))*sqdip1;
150 q_z = (R(0, 2) + R(2, 0))*sqdip1;
160 R(0, 0) = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
161 R(1, 0) = 2.0*(qin[1] * qin[2] + qin[0] * qin[3]);
162 R(2, 0) = 2.0*(qin[1] * qin[3] - qin[0] * qin[2]);
163 R(0, 1) = 2.0*(qin[1] * qin[2] - qin[0] * qin[3]);
164 R(1, 1) = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
165 R(2, 1) = 2.0*(qin[2] * qin[3] + qin[0] * qin[1]);
166 R(0, 2) = 2.0*(qin[1] * qin[3] + qin[0] * qin[2]);
167 R(1, 2) = 2.0*(qin[2] * qin[3] - qin[0] * qin[1]);
168 R(2, 2) = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];
179 R(0, 0) = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
180 R(1, 0) = 2.0*(qin[1] * qin[2] + qin[0] * qin[3]);
181 R(2, 0) = 2.0*(qin[1] * qin[3] - qin[0] * qin[2]);
182 R(0, 1) = 2.0*(qin[1] * qin[2] - qin[0] * qin[3]);
183 R(1, 1) = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
184 R(2, 1) = 2.0*(qin[2] * qin[3] + qin[0] * qin[1]);
185 R(0, 2) = 2.0*(qin[1] * qin[3] + qin[0] * qin[2]);
186 R(1, 2) = 2.0*(qin[2] * qin[3] - qin[0] * qin[1]);
187 R(2, 2) = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];