10 #include <argos3/core/utility/math/vector3.h>
11 #include <argos3/core/utility/math/matrix/rotationmatrix3.h>
33 m_fValues[0] = f_real;
34 m_fValues[1] = f_img1;
35 m_fValues[2] = f_img2;
36 m_fValues[3] = f_img3;
119 m_fValues[0] *= fInvLength;
120 m_fValues[1] *= fInvLength;
121 m_fValues[2] *= fInvLength;
122 m_fValues[3] *= fInvLength;
128 CRadians cHalfAngle = c_angle * 0.5;
131 SinCos(cHalfAngle, fSin, fCos);
133 fSin =
Sin(cHalfAngle);
134 fCos =
Cos(cHalfAngle);
137 m_fValues[1] = c_vector.
GetX() * fSin;
138 m_fValues[2] = c_vector.
GetY() * fSin;
139 m_fValues[3] = c_vector.
GetZ() * fSin;
149 if(fSquareLength > 0.0f) {
150 c_angle = 2.0f *
ACos(m_fValues[0]);
151 Real fInvLength = 1.0f / ::sqrt(fSquareLength);
152 c_vector.
Set(m_fValues[1] * fInvLength,
153 m_fValues[2] * fInvLength,
154 m_fValues[3] * fInvLength);
186 m_fValues[1] * m_fValues[3] +
187 m_fValues[0] * m_fValues[2];
193 c_z_angle =
ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]),
194 1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3]));
196 else if(fTest < -0.499f) {
200 c_z_angle =
ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]),
201 1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3]));
205 Real fSqValues[4] = {
212 c_x_angle =
ATan2(2.0 * (m_fValues[0] * m_fValues[1] - m_fValues[2] * m_fValues[3]),
213 fSqValues[0] - fSqValues[1] - fSqValues[2] + fSqValues[3]);
214 c_y_angle =
ASin(2.0 * (m_fValues[1] * m_fValues[3] + m_fValues[0] * m_fValues[2]));
215 c_z_angle =
ATan2(2.0 * (m_fValues[0] * m_fValues[3] - m_fValues[1] * m_fValues[2]),
216 fSqValues[0] + fSqValues[1] - fSqValues[2] - fSqValues[3]);
225 if(fProd > 0.999999f) {
232 else if(fProd < -0.999999f) {
253 cCrossProd.CrossProduct(c_vector2);
254 m_fValues[1] = cCrossProd.GetX();
255 m_fValues[2] = cCrossProd.GetY();
256 m_fValues[3] = cCrossProd.GetZ();
263 return (m_fValues[0] == c_quaternion.m_fValues[0] &&
264 m_fValues[1] == c_quaternion.m_fValues[1] &&
265 m_fValues[2] == c_quaternion.m_fValues[2] &&
266 m_fValues[3] == c_quaternion.m_fValues[3]);
270 if (&c_quaternion !=
this) {
271 m_fValues[0] = c_quaternion.m_fValues[0];
272 m_fValues[1] = c_quaternion.m_fValues[1];
273 m_fValues[2] = c_quaternion.m_fValues[2];
274 m_fValues[3] = c_quaternion.m_fValues[3];
280 m_fValues[0] += c_quaternion.m_fValues[0];
281 m_fValues[1] += c_quaternion.m_fValues[1];
282 m_fValues[2] += c_quaternion.m_fValues[2];
283 m_fValues[3] += c_quaternion.m_fValues[3];
288 m_fValues[0] -= c_quaternion.m_fValues[0];
289 m_fValues[1] -= c_quaternion.m_fValues[1];
290 m_fValues[2] -= c_quaternion.m_fValues[2];
291 m_fValues[3] -= c_quaternion.m_fValues[3];
297 newv[0] = m_fValues[0] * c_quaternion.m_fValues[0] -
298 m_fValues[1] * c_quaternion.m_fValues[1] -
299 m_fValues[2] * c_quaternion.m_fValues[2] -
300 m_fValues[3] * c_quaternion.m_fValues[3];
301 newv[1] = m_fValues[0] * c_quaternion.m_fValues[1] +
302 m_fValues[1] * c_quaternion.m_fValues[0] +
303 m_fValues[2] * c_quaternion.m_fValues[3] -
304 m_fValues[3] * c_quaternion.m_fValues[2];
305 newv[2] = m_fValues[0] * c_quaternion.m_fValues[2] -
306 m_fValues[1] * c_quaternion.m_fValues[3] +
307 m_fValues[2] * c_quaternion.m_fValues[0] +
308 m_fValues[3] * c_quaternion.m_fValues[1];
309 newv[3] = m_fValues[0] * c_quaternion.m_fValues[3] +
310 m_fValues[1] * c_quaternion.m_fValues[2] -
311 m_fValues[2] * c_quaternion.m_fValues[1] +
312 m_fValues[3] * c_quaternion.m_fValues[0];
313 m_fValues[0] = newv[0];
314 m_fValues[1] = newv[1];
315 m_fValues[2] = newv[2];
316 m_fValues[3] = newv[3];
322 result += c_quaternion;
328 result -= c_quaternion;
334 result *= c_quaternion;
343 Real fXS = m_fValues[1] * fS;
Real fYS = m_fValues[2] * fS;
Real fZS = m_fValues[3] * fS;
344 Real fWX = m_fValues[0] * fXS;
Real fWY = m_fValues[0] * fYS;
Real fWZ = m_fValues[0] * fZS;
345 Real fXX = m_fValues[1] * fXS;
Real fXY = m_fValues[1] * fYS;
Real fXZ = m_fValues[1] * fZS;
346 Real fYY = m_fValues[2] * fYS;
Real fYZ = m_fValues[2] * fZS;
Real fZZ = m_fValues[3] * fZS;
349 fXY + fWZ, 1.0f - (fXX + fZZ), fYZ - fWX,
350 fXZ - fWY, fYZ + fWX, 1.0f - (fXX + fYY));
378 ParseValues<Real>(c_is, 3, fValues,
',');
CQuaternion & operator*=(const CQuaternion &c_quaternion)
T Square(const T &t_v)
Returns the square of the value of the passed argument.
CQuaternion Conjugate() const
CQuaternion(const CRadians &c_radians, const CVector3 &c_vector3)
void ToAngleAxis(CRadians &c_angle, CVector3 &c_vector) const
CQuaternion & operator+=(const CQuaternion &c_quaternion)
CQuaternion operator+(const CQuaternion &c_quaternion) const
static const CVector3 X
The x axis.
CDegrees ToDegrees(const CRadians &c_radians)
Converts CRadians to CDegrees.
friend std::istream & operator>>(std::istream &c_is, CQuaternion &c_quaternion)
Deserializes the contents of a stream and stores it into the passed quaternion.
float Real
Collects all ARGoS code.
CRadians ATan2(const Real f_y, const Real f_x)
Computes the arctangent of the passed values.
Real GetX() const
Returns the x coordinate of this vector.
CQuaternion & operator-=(const CQuaternion &c_quaternion)
CQuaternion & BetweenTwoVectors(const CVector3 &c_vector1, const CVector3 &c_vector2)
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Real GetY() const
Returns the y coordinate of this vector.
Real GetValue() const
Returns the value in degrees.
Real SquareLength() const
Returns the square length of this vector.
CQuaternion(Real f_real, Real f_img1, Real f_img2, Real f_img3)
Real Cos(const CRadians &c_radians)
Computes the cosine of the passed value in radians.
CRadians ASin(Real f_value)
Computes the arcsine of the passed value.
Real SquareLength() const
It defines the basic type CDegrees, used to store an angle value in degrees.
CQuaternion & operator=(const CQuaternion &c_quaternion)
It defines the basic type CRadians, used to store an angle value in radians.
CQuaternion & FromEulerAngles(const CRadians &c_z_angle, const CRadians &c_y_angle, const CRadians &c_x_angle)
friend std::ostream & operator<<(std::ostream &c_os, const CQuaternion &c_quaternion)
Serializes the contents of the passed quaternion into a stream as Euler angles in the Z...
CQuaternion operator-(const CQuaternion &c_quaternion) const
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
CQuaternion & Normalize()
Real Sin(const CRadians &c_radians)
Computes the sine of the passed value in radians.
static const CRadians PI
The PI constant.
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
CQuaternion Inverse() const
bool operator==(const CQuaternion &c_quaternion)
void SinCos(const CRadians &c_radians, Real &f_sin, Real &f_cos)
Computes the sine and cosine of the passed value in radians.
static const CVector3 Z
The z axis.
static const CRadians ZERO
Set to zero radians.
static const CVector3 Y
The y axis.
CQuaternion(const CVector3 &c_vector1, const CVector3 &c_vector2)
Real DotProduct(const CVector3 &c_vector3) const
Returns the dot product between this vector and the passed one.
CQuaternion operator*(const CQuaternion &c_quaternion) const
CVector3 & CrossProduct(const CVector3 &c_vector3)
Calculates the cross product between this vector and the passed one.
CQuaternion(const CQuaternion &c_quaternion)
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
The namespace containing all the ARGoS related code.
Real GetZ() const
Returns the z coordinate of this vector.
CRadians ACos(Real f_value)
Computes the arccosine of the passed value.
void Set(Real f_w, Real f_x, Real f_y, Real f_z)
static const CRadians PI_OVER_TWO
Set to PI / 2.