10 #include <argos3/core/utility/math/quaternion.h>
11 #include <argos3/core/utility/math/vector3.h>
19 if (trace > 0.00001f) {
75 Real xs = c_quaternion.
GetX() * s, ys = c_quaternion.
GetY() * s, zs = c_quaternion.
GetZ() * s;
76 Real wx = c_quaternion.
GetW() * xs, wy = c_quaternion.
GetW() * ys, wz = c_quaternion.
GetW() * zs;
77 Real xx = c_quaternion.
GetX() * xs, xy = c_quaternion.
GetX() * ys, xz = c_quaternion.
GetX() * zs;
78 Real yy = c_quaternion.
GetY() * ys, yz = c_quaternion.
GetY() * zs, zz = c_quaternion.
GetZ() * zs;
104 m_pfValues[1] = (fCosZ * fSinY * fSinX) - (fCosX * fSinZ);
105 m_pfValues[2] = (fSinZ * fSinX) + (fCosZ * fCosX * fSinY);
107 m_pfValues[4] = (fCosZ * fCosX) + (fSinZ * fSinY * fSinX);
108 m_pfValues[5] = (fCosX * fSinZ * fSinY) - (fCosZ * fSinX);