void rotation_matrix_to_quaternion(double* u, double* q)
{
double r11 = u[0];
double r12 = u[1];
double r13 = u[2];
double r21 = u[3];
double r22 = u[4];
double r23 = u[5];
double r31 = u[6];
double r32 = u[7];
double r33 = u[8];
q[0] = (1.0 + r11 + r22 + r33) / 4.0;
q[1] = (1.0 + r11 - r22 - r33) / 4.0;
q[2] = (1.0 - r11 + r22 - r33) / 4.0;
q[3] = (1.0 - r11 - r22 + r33) / 4.0;
q[0] = sqrt(MAX(0, q[0]));
q[1] = sqrt(MAX(0, q[1]));
q[2] = sqrt(MAX(0, q[2]));
q[3] = sqrt(MAX(0, q[3]));
double m0 = MAX(q[0], q[1]);
double m1 = MAX(q[2], q[3]);
double max = MAX(m0, m1);
int i = 0;
for (i=0;i<4;i++)
if (q[i] == max)
break;
if (i == 0)
{
q[1] *= SIGN(r32 - r23);
q[2] *= SIGN(r13 - r31);
q[3] *= SIGN(r21 - r12);
}
else if (i == 1)
{
q[0] *= SIGN(r32 - r23);
q[2] *= SIGN(r21 + r12);
q[3] *= SIGN(r13 + r31);
}
else if (i == 2)
{
q[0] *= SIGN(r13 - r31);
q[1] *= SIGN(r21 + r12);
q[3] *= SIGN(r32 + r23);
}
else if (i == 3)
{
q[0] *= SIGN(r21 - r12);
q[1] *= SIGN(r31 + r13);
q[2] *= SIGN(r32 + r23);
}
normalize_quaternion(q);
}
/** Rotate the particle p around the NORMALIZED axis aSpaceFrame by amount phi */
void rotate_particle(Particle* p, double* aSpaceFrame, double phi)
{
// Convert rotation axis to body-fixed frame
double a[3];
convert_vec_space_to_body(p,aSpaceFrame,a);
// Apply restrictions from the rotation_per_particle feature
#ifdef ROTATION_PER_PARTICLE
// printf("%g %g %g - ",a[0],a[1],a[2]);
// Rotation turned off entirely?
if (p->p.rotation <2) return;
// Per coordinate fixing
if (!(p->p.rotation & 2)) a[0]=0;
if (!(p->p.rotation & 4)) a[1]=0;
if (!(p->p.rotation & 8)) a[2]=0;
// Re-normalize rotation axis
double l=sqrt(sqrlen(a));
// Check, if the rotation axis is nonzero
if (l<1E-10) return;
for (int i=0;i<3;i++)
a[i]/=l;
// printf("%g %g %g\n",a[0],a[1],a[2]);
#endif
double q[4];
q[0]=cos(phi/2);
double tmp=sin(phi/2);
q[1]=tmp*a[0];
q[2]=tmp*a[1];
q[3]=tmp*a[2];
// Normalize
normalize_quaternion(q);
// Rotate the particle
double qn[4]; // Resulting quaternion
multiply_quaternions(p->r.quat,q,qn);
for (int k=0; k<4; k++)
p->r.quat[k]=qn[k];
convert_quat_to_quatu(p->r.quat, p->r.quatu);
#ifdef DIPOLES
// When dipoles are enabled, update dipole moment
convert_quatu_to_dip(p->r.quatu, p->p.dipm, p->r.dip);
#endif
}
//generartes target quaternion from control inputs
int control_quat(struct quad_state *curr, struct quad_state *past, struct quad_static *stat){
//convert control inputs to pitch roll and yaw angles
if(stat->controlflag == 1){
stat->controlflag = 0;
float pitch = (stat->control[1] - 11059) * ( 3.14159265358979 / (2949.0 * 5.0));
float roll = (stat->control[0] - 11059) * ( 3.14159265358979 / (2949.0 * 5.0));
//float yaw = (stat->control[3] - 11059) * ( 3.14159265358979 / (2949.0 * 6.0));
stat->throttle = (stat->control[2] - 8110) / 5.898;
/*
outi0(pitch / 3.1415926535 * 180);
out0(" ");
outi0(roll / 3.1415926535 * 180);
out0(" ");
outi0(yaw / 3.1415926535 * 180);
out0("\n");
*/
//generate roll/pitch quaternion given control inputs
struct quaternion tmpquat;
float angle;
if(pitch != 0 || roll != 0){
angle = sqrt(pitch*pitch + roll*roll);
pitch /= angle;
roll /= angle;
float sinangle = sin(angle/2.0);
tmpquat.a = cos(angle/2.0);
tmpquat.b = pitch * sinangle;
tmpquat.c = roll * sinangle;
tmpquat.d = 0;
}else{
tmpquat.a = 1;
tmpquat.b = 0;
tmpquat.c = 0;
tmpquat.d = 0;
}
//make quaternion consisting of only the yaw component of bgquat
struct quaternion yawquat;
clone_quaternion(&curr->gbquat, &yawquat);
yawquat.b = 0;
yawquat.c = 0;
normalize_quaternion(&yawquat);
//calculate target quaternion from yaw position and desired attitudes
mult_quaternion(&yawquat, &tmpquat, &curr->gtquat);
}else{
//if no new control input use the past input
clone_quaternion(&past->gtquat, &curr->gtquat);
}
return 0;
}
