10 #include <argos3/core/utility/math/vector3.h>
32 m_fValues[0] = f_real;
33 m_fValues[1] = f_img1;
34 m_fValues[2] = f_img2;
35 m_fValues[3] = f_img3;
118 m_fValues[0] *= fInvLength;
119 m_fValues[1] *= fInvLength;
120 m_fValues[2] *= fInvLength;
121 m_fValues[3] *= fInvLength;
127 CRadians cHalfAngle = c_angle * 0.5;
130 SinCos(cHalfAngle, fSin, fCos);
132 fSin =
Sin(cHalfAngle);
133 fCos =
Cos(cHalfAngle);
136 m_fValues[1] = c_vector.
GetX() * fSin;
137 m_fValues[2] = c_vector.
GetY() * fSin;
138 m_fValues[3] = c_vector.
GetZ() * fSin;
148 if(fSquareLength > 0.0f) {
149 c_angle = 2.0f *
ACos(m_fValues[0]);
150 Real fInvLength = 1.0f / ::sqrt(fSquareLength);
151 c_vector.
Set(m_fValues[1] * fInvLength,
152 m_fValues[2] * fInvLength,
153 m_fValues[3] * fInvLength);
185 m_fValues[1] * m_fValues[3] +
186 m_fValues[0] * m_fValues[2];
192 c_z_angle =
ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]),
193 1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3]));
195 else if(fTest < -0.499f) {
199 c_z_angle =
ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]),
200 1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3]));
204 Real fSqValues[4] = {
211 c_x_angle =
ATan2(2.0 * (m_fValues[0] * m_fValues[1] - m_fValues[2] * m_fValues[3]),
212 fSqValues[0] - fSqValues[1] - fSqValues[2] + fSqValues[3]);
213 c_y_angle =
ASin(2.0 * (m_fValues[1] * m_fValues[3] + m_fValues[0] * m_fValues[2]));
214 c_z_angle =
ATan2(2.0 * (m_fValues[0] * m_fValues[3] - m_fValues[1] * m_fValues[2]),
215 fSqValues[0] + fSqValues[1] - fSqValues[2] - fSqValues[3]);
224 if(fProd > 0.999999f) {
231 else if(fProd < -0.999999f) {
253 m_fValues[1] = cCrossProd.
GetX();
254 m_fValues[2] = cCrossProd.
GetY();
255 m_fValues[3] = cCrossProd.
GetZ();
262 return (m_fValues[0] == c_quaternion.m_fValues[0] &&
263 m_fValues[1] == c_quaternion.m_fValues[1] &&
264 m_fValues[2] == c_quaternion.m_fValues[2] &&
265 m_fValues[3] == c_quaternion.m_fValues[3]);
269 if (&c_quaternion !=
this) {
270 m_fValues[0] = c_quaternion.m_fValues[0];
271 m_fValues[1] = c_quaternion.m_fValues[1];
272 m_fValues[2] = c_quaternion.m_fValues[2];
273 m_fValues[3] = c_quaternion.m_fValues[3];
279 m_fValues[0] += c_quaternion.m_fValues[0];
280 m_fValues[1] += c_quaternion.m_fValues[1];
281 m_fValues[2] += c_quaternion.m_fValues[2];
282 m_fValues[3] += c_quaternion.m_fValues[3];
287 m_fValues[0] -= c_quaternion.m_fValues[0];
288 m_fValues[1] -= c_quaternion.m_fValues[1];
289 m_fValues[2] -= c_quaternion.m_fValues[2];
290 m_fValues[3] -= c_quaternion.m_fValues[3];
296 newv[0] = m_fValues[0] * c_quaternion.m_fValues[0] -
297 m_fValues[1] * c_quaternion.m_fValues[1] -
298 m_fValues[2] * c_quaternion.m_fValues[2] -
299 m_fValues[3] * c_quaternion.m_fValues[3];
300 newv[1] = m_fValues[0] * c_quaternion.m_fValues[1] +
301 m_fValues[1] * c_quaternion.m_fValues[0] +
302 m_fValues[2] * c_quaternion.m_fValues[3] -
303 m_fValues[3] * c_quaternion.m_fValues[2];
304 newv[2] = m_fValues[0] * c_quaternion.m_fValues[2] -
305 m_fValues[1] * c_quaternion.m_fValues[3] +
306 m_fValues[2] * c_quaternion.m_fValues[0] +
307 m_fValues[3] * c_quaternion.m_fValues[1];
308 newv[3] = m_fValues[0] * c_quaternion.m_fValues[3] +
309 m_fValues[1] * c_quaternion.m_fValues[2] -
310 m_fValues[2] * c_quaternion.m_fValues[1] +
311 m_fValues[3] * c_quaternion.m_fValues[0];
312 m_fValues[0] = newv[0];
313 m_fValues[1] = newv[1];
314 m_fValues[2] = newv[2];
315 m_fValues[3] = newv[3];
321 result += c_quaternion;
327 result -= c_quaternion;
333 result *= c_quaternion;
362 ParseValues<Real>(c_is, 3, fValues,
',');