Mani Monajjemi
143 linhas
3.5 KiB
C
143 linhas
3.5 KiB
C
/**
|
|
* \file quaternions.c
|
|
* \brief Quaternions library used by Mykonos
|
|
* \author Francois Callou <francois.callou@parrot.com>
|
|
* \version 1.0
|
|
*/
|
|
|
|
#include <VP_Os/vp_os_assert.h>
|
|
#include <Maths/quaternions.h>
|
|
#include <Maths/maths.h>
|
|
|
|
const quaternion_t quat_unitary = { 1.0f, {{{ 0.0f, 0.0f, 0.0f}}} };
|
|
|
|
void mul_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2)
|
|
{
|
|
vector31_t temp_v;
|
|
|
|
/// You can't have output & input pointing to the same location
|
|
VP_OS_ASSERT( out != q1 );
|
|
VP_OS_ASSERT( out != q2 );
|
|
|
|
// scalar result
|
|
out->a = q1->a*q2->a - (q1->v.x*q2->v.x + q1->v.y*q2->v.y + q1->v.z*q2->v.z);
|
|
|
|
// pure quaternion result
|
|
cross_vec( &out->v , &q1->v, &q2->v );
|
|
mulconst_vec( &temp_v, &q2->v, q1->a );
|
|
add_vec( &out->v, &out->v, &temp_v);
|
|
mulconst_vec( &temp_v, &q1->v, q2->a );
|
|
add_vec( &out->v, &out->v, &temp_v);
|
|
}
|
|
|
|
void add_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2 )
|
|
{
|
|
// scalar result
|
|
out->a = q1->a + q2->a;
|
|
// pure quaternion result
|
|
add_vec( &out->v, &q1->v, &q2->v );
|
|
}
|
|
|
|
void mulconst_quat( quaternion_t* out, quaternion_t* q, float32_t k )
|
|
{
|
|
out->a = (q->a) * k;
|
|
mulconst_vec( &out->v, &q->v, k );
|
|
}
|
|
|
|
void conjugate_quat( quaternion_t* out, quaternion_t* q )
|
|
{
|
|
out->a = q->a;
|
|
out->v.x = -q->v.x;
|
|
out->v.y = -q->v.y;
|
|
out->v.z = -q->v.z;
|
|
}
|
|
|
|
float32_t norm_quat( quaternion_t *q )
|
|
{
|
|
return sqrtf( q->a*q->a + q->v.x * q->v.x + q->v.y * q->v.y + q->v.z * q->v.z );
|
|
}
|
|
|
|
bool_t normalize_quat( quaternion_t* q )
|
|
{
|
|
bool_t ret;
|
|
float32_t norm;
|
|
|
|
norm = norm_quat( q );
|
|
|
|
if( f_is_zero( norm ) )
|
|
{
|
|
q->a = 0.0f;
|
|
q->v.x = 0.0f;
|
|
q->v.y = 0.0f;
|
|
q->v.z = 0.0f;
|
|
|
|
ret = FALSE;
|
|
}
|
|
else
|
|
{
|
|
q->a = f_zero( q->a / norm );
|
|
q->v.x = f_zero( q->v.x / norm );
|
|
q->v.y = f_zero( q->v.y / norm );
|
|
q->v.z = f_zero( q->v.z / norm );
|
|
|
|
ret = TRUE;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
void quat_to_euler_rot_mat(matrix33_t* m, quaternion_t* q)
|
|
{
|
|
//to use with normalised quaternion
|
|
m->m11 = 1.0f - 2*q->v.y*q->v.y - 2*q->v.z*q->v.z;
|
|
m->m12 = 2*q->v.x*q->v.y - 2*q->v.z*q->a;
|
|
m->m13 = 2*q->v.z*q->v.x + 2*q->v.y*q->a;
|
|
m->m21 = 2*q->v.x*q->v.y + 2*q->v.z*q->a;
|
|
m->m22 = 1.0f - 2*q->v.x*q->v.x - 2*q->v.z*q->v.z;
|
|
m->m23 = 2*q->v.z*q->v.y - 2*q->v.x*q->a;
|
|
m->m31 = 2*q->v.z*q->v.x - 2*q->v.y*q->a;
|
|
m->m32 = 2*q->v.z*q->v.y + 2*q->v.x*q->a;
|
|
m->m33 = 1.0f - 2*q->v.x*q->v.x - 2*q->v.y*q->v.y;
|
|
}
|
|
|
|
|
|
|
|
void quat_to_euler_angles(angles_t* a, quaternion_t* q)
|
|
{
|
|
//to use with normalised quaternion
|
|
float32_t sqvx = q->v.x*q->v.x;
|
|
float32_t sqvy = q->v.y*q->v.y;
|
|
float32_t sqvz = q->v.z*q->v.z;
|
|
|
|
/* if ( f_is_zero(test -0.5) ) { // singularity at north pole
|
|
a->psi = 2 * atan2(q->a,q->v.z);
|
|
a->theta = PI/2;
|
|
a->phi = 0;
|
|
return;
|
|
}
|
|
if ( f_is_zero(test + 0.5) ) { // singularity at south pole
|
|
a->psi = -2 * atan2(q->a,q->v.z);
|
|
a->theta = - PI/2;
|
|
a->phi = 0;
|
|
return;
|
|
}*/
|
|
|
|
a->phi = atan2f(2*q->v.y*q->v.z+2*q->a*q->v.x , 1 - 2*sqvx - 2*sqvy);
|
|
a->theta = asinf(2*(q->a*q->v.y - q->v.x*q->v.z ));
|
|
a->psi = atan2f(2*q->v.x*q->v.y+2*q->a*q->v.z , 1 - 2*sqvy - 2*sqvz);
|
|
}
|
|
|
|
void euler_angles_to_quat(angles_t* a, quaternion_t* q)
|
|
{
|
|
float32_t cphi_2, sphi_2, cthe_2, sthe_2, cpsi_2, spsi_2;
|
|
sincosf((a->phi)*0.5f, &sphi_2, &cphi_2);
|
|
sincosf((a->theta)*0.5f, &sthe_2, &cthe_2);
|
|
sincosf((a->psi)*0.5f, &spsi_2, &cpsi_2);
|
|
|
|
q->a = cphi_2*cthe_2*cpsi_2 + sphi_2*sthe_2*spsi_2;
|
|
q->v.x = sphi_2*cthe_2*cpsi_2 - cphi_2*sthe_2*spsi_2;
|
|
q->v.y = cphi_2*sthe_2*cpsi_2 + sphi_2*cthe_2*spsi_2;
|
|
q->v.z = cphi_2*cthe_2*spsi_2 - sphi_2*sthe_2*cpsi_2;
|
|
}
|