[ Foro de C++ ]
Entiendo poco este listado en C++, sino me equivoco es algebra?
/**
* @addtogroup quaternions Library for 3D Vectors & Quaternions
* @{
* @file
* @brief Generic header that provides data types for 3D vectors and quaternions
* @author Krishna Vedala
*/
#ifndef __LIBQUAT_H_
#define __LIBQUAT_H_
/** Minimum recognizable value. Any value less than this is considered to be
* @f$=0@f$ */
#define EPSILON 1e-9
/**
* @addtogroup vec_3d 3D Vector operations
* @{
*/
/** 3D vector type */
typedef struct vec_3d_
{
float x; /**< X co-ordinate */
float y; /**< Y co-ordinate */
float z; /**< Z co-ordinate */
} vec_3d;
/** @} */
/**
* @addtogroup matrix Matrix operations
* @{
*/
/** A 3x3 Matrix type definition */
typedef struct mat_3x3_
{
union
{ /**< 3 element row 1 */
float row1[3];
vec_3d vec1;
};
union
{ /**< 3 element row 2 */
float row2[3];
vec_3d vec2;
};
union
{ /**< 3 element row 3 */
float row3[3];
vec_3d vec3;
};
} mat_3x3;
/** @} */
/** @addtogroup quats 3D Quaternion operations
* @{
*/
/** a Quaternion type represented using a scalar \f$w\f$ or \f$q_0\f$ and a
* 3D vector \f$\left(q_1,q_2,q_3\right)\f$
*/
typedef struct quaternion_
{
union
{
float w; /**< real part of quaternion */
float q0; /**< real part of quaternion */
};
/**< dual part of quaternion */
union
{
vec_3d dual; /**< can be a 3D vector */
/** or individual values */
struct
{
float q1, q2, q3;
};
};
} quaternion;
/** 3D Euler or Tait-Bryan angles (in radian) */
typedef struct euler_
{
union
{
float roll; /**< or bank \f$\phi\f$ = rotation about X axis */
float bank; /**< or roll \f$\phi\f$ = rotation about X axis */
};
union
{
float pitch; /**< or elevation \f$\theta\f$ = rotation about Y axis */
float elevation; /**< or pitch \f$\theta\f$ = rotation about Y axis */
};
union
{
float yaw; /**< or heading \f$\psi\f$ = rotation about Z axis */
float heading; /**< or yaw \f$\psi\f$ = rotation about Z axis */
};
} euler;
/** @} */
/** @addtogroup dual_quats 3D Dual-Quaternion operations
* @{
*/
/** a dual quaternion type */
typedef struct dual_quat_
{
quaternion real; /**< real part of dual quaternion */
quaternion dual; /**< dual part of dual quaternion */
} dual_quat;
/** @} */
#endif // __LIBQUAT_H_
/** @} */
/**
* @file
* @brief Functions related to 3D quaternions and Euler angles.
* @author Krishna Vedala
*/
#include <stdio.h>
#ifdef __arm__ // if compiling for ARM-Cortex processors
#define LIBQUAT_ARM
#include <arm_math.h>
#else
#include <math.h>
#endif
#include <assert.h>
#include "geometry_datatypes.h"
/**
* @addtogroup quats 3D Quaternion operations
* @{
*/
/**
* Function to convert given Euler angles to a quaternion.
* \f{eqnarray*}{
* q_{0} & =
* &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
* +
* \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
* q_{1} & =
* &\sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
* -
* \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
* q_{2} & =
* &\cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
* +
* \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
* q_{3} & =
* &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)
* -
* \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)\\
* \f}
*
* @param [in] in_euler input Euler angles instance
* @returns converted quaternion
*/
quaternion quat_from_euler(const euler *in_euler)
{
quaternion out_quat;
if (!in_euler) // if null
{
fprintf(stderr, "%s: Invalid input.", __func__);
return out_quat;
}
quaternion temp;
float cy = cosf(in_euler->yaw * 0.5f);
float sy = sinf(in_euler->yaw * 0.5f);
float cp = cosf(in_euler->pitch * 0.5f);
float sp = sinf(in_euler->pitch * 0.5f);
float cr = cosf(in_euler->roll * 0.5f);
float sr = sinf(in_euler->roll * 0.5f);
temp.w = cr * cp * cy + sr * sp * sy;
temp.q1 = sr * cp * cy - cr * sp * sy;
temp.q2 = cr * sp * cy + sr * cp * sy;
temp.q3 = cr * cp * sy - sr * sp * cy;
return temp;
}
/**
* Function to convert given quaternion to Euler angles.
* \f{eqnarray*}{
* \phi & = &
* \tan^{-1}\left[\frac{2\left(q_0q_1+q_2q_3\right)}{1-2\left(q_1^2+q_2^2\right)}\right]\\
* \theta & =
* &-\sin^{-1}\left[2\left(q_0q_2-q_3q_1\right)\right]\\
* \psi & = &
* \tan^{-1}\left[\frac{2\left(q_0q_3+q_1q_2\right)}{1-2\left(q_2^2+q_3^2\right)}\right]\\
* \f}
*
* @param [in] in_quat input quaternion instance
* @returns converted euler angles
*/
euler euler_from_quat(const quaternion *in_quat)
{
euler out_euler;
if (!in_quat) // if null
{
fprintf(stderr, "%s: Invalid input.", __func__);
return out_euler;
}
out_euler.roll = atan2f(
2.f * (in_quat->w * in_quat->q1 + in_quat->q2 * in_quat->q3),
1.f - 2.f * (in_quat->q1 * in_quat->q1 + in_quat->q2 * in_quat->q2));
out_euler.pitch =
asinf(2.f * (in_quat->w * in_quat->q2 + in_quat->q1 * in_quat->q3));
out_euler.yaw = atan2f(
2.f * (in_quat->w * in_quat->q3 + in_quat->q1 * in_quat->q2),
1.f - 2.f * (in_quat->q2 * in_quat->q2 + in_quat->q3 * in_quat->q3));
return out_euler;
}
/**
* Function to multiply two quaternions.
* \f{eqnarray*}{
* \mathbf{c} & = & \mathbf{a}\otimes\mathbf{b}\\
* & = & \begin{bmatrix}a_{0} & a_{1} & a_{2} &
* a_{3}\end{bmatrix}\otimes\begin{bmatrix}b_{0} & b_{1} & b_{2} &
* b_{3}\end{bmatrix}\\
* & = &
* \begin{bmatrix}
* a_{0}b_{0}-a_{1}b_{1}-a_{2}b_{2}-a_{3}b_{3}\\
* a_{0}b_{1}+a_{1}b_{0}+a_{2}b_{3}-a_{3}b_{2}\\
* a_{0}b_{2}-a_{1}b_{3}+a_{2}b_{0}+a_{3}b_{1}\\
* a_{0}b_{3}+a_{1}b_{2}-a_{2}b_{1}+a_{3}b_{0}
* \end{bmatrix}^{T}
* \f}
*
* @param [in] in_quat1 first input quaternion instance
* @param [in] in_quat2 second input quaternion instance
* @returns resultant quaternion
*/
quaternion quaternion_multiply(const quaternion *in_quat1,
const quaternion *in_quat2)
{
quaternion out_quat;
if (!in_quat1 || !in_quat2) // if null
{
fprintf(stderr, "%s: Invalid input.", __func__);
return out_quat;
}
out_quat.w = in_quat1->w * in_quat2->w - in_quat1->q1 * in_quat2->q1 -
in_quat1->q2 * in_quat2->q2 - in_quat1->q3 * in_quat2->q3;
out_quat.q1 = in_quat1->w * in_quat2->q1 + in_quat1->q1 * in_quat2->w +
in_quat1->q2 * in_quat2->q3 - in_quat1->q3 * in_quat2->q2;
out_quat.q2 = in_quat1->w * in_quat2->q2 - in_quat1->q1 * in_quat2->q3 +
in_quat1->q2 * in_quat2->w + in_quat1->q3 * in_quat2->q1;
out_quat.q3 = in_quat1->w * in_quat2->q3 + in_quat1->q1 * in_quat2->q2 -
in_quat1->q2 * in_quat2->q1 + in_quat1->q3 * in_quat2->w;
return out_quat;
}
/** @} */
static void test()
{
quaternion quat = {0.7071f, 0.7071f, 0.f, 0.f};
euler eul = euler_from_quat(&quat);
printf("Euler: %.4g, %.4g, %.4g\n", eul.pitch, eul.roll, eul.yaw);
quaternion test_quat = quat_from_euler(&eul);
printf("Quaternion: %.4g %+.4g %+.4g %+.4g\n", test_quat.w,
test_quat.dual.x, test_quat.dual.y, test_quat.dual.z);
assert(fabsf(test_quat.w - quat.w) < .01);
assert(fabsf(test_quat.q1 - quat.q1) < .01);
assert(fabsf(test_quat.q2 - quat.q2) < .01);
assert(fabsf(test_quat.q3 - quat.q3) < .01);
}
int main()
{
test();
return 0;
}
noentras2@debian:~/humillacion13/varios_alogaritmos$ ./qua
Euler: 0, 1.571, 0
Quaternion: 0.7071 +0.7071 +0 +0
(No se puede continuar esta discusión porque tiene más de dos meses de antigüedad. Si tienes dudas parecidas, abre un nuevo hilo.)