1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
|
/********************************
Author: Sravanthi Kota Venkata
********************************/
#include <stdio.h>
#include <stdlib.h>
#include "localization.h"
#include <math.h>
F2D* quat2eul(F2D* quat)
{
F2D *retEul;
int i, j, k;
int rows, cols;
rows = quat->height;
cols = quat->width;
retEul = fSetArray(rows, 3, 0);
for(i=0; i<rows; i++)
{
float temp, temp1, temp2, temp3, temp4;
float quati2, quati3, quati1, quati0;
quati0 = subsref(quat,i,0);
quati1 = subsref(quat,i,1);
quati2 = subsref(quat,i,2);
quati3 = subsref(quat,i,3);
temp = 2 *quati2 * quati3 + quati0 * quati1;
temp1 = pow(quati0,2) - pow(quati1,2) - pow(quati2,2) + pow(quati3,2);
temp2 = -2*quati1 * quati2 + quati0 * quati3;
temp3 = 2*quati1 * quati2 + quati0 * quati3;
temp4 = pow(quati0,2) + pow(quati1,2) - pow(quati2,2) - pow(quati3,2);
asubsref(retEul,k++) = atan2(temp, temp1);
asubsref(retEul,k++) = asin(temp2);
asubsref(retEul,k++) = atan2(temp3, temp4);
}
return retEul;
}
|
