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
48
49
50
|
/********************************
Author: Sravanthi Kota Venkata
********************************/
#include <stdio.h>
#include <stdlib.h>
#include "localization.h"
F2D* eul2quat(F2D* angle)
{
F2D *ret;
F2D *x, *y, *z;
int k, i, j;
int rows, cols;
rows = angle->height;
cols = angle->width;
x = fDeepCopyRange(angle, 0, angle->height, 0, 1);
y = fDeepCopyRange(angle, 0, angle->height, 1, 1);
z = fDeepCopyRange(angle, 0, angle->height, 2, 1);
ret = fSetArray(x->height, 4, 0);
for(i=0; i<rows; i++)
{
float xi, yi, zi;
k = 0;
xi = asubsref(x,i);
yi = asubsref(y,i);
zi = asubsref(z,i);
subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*cos(zi/2)+sin(xi/2)*sin(yi/2)*sin(zi/2);
k++;
subsref(ret,i,k) = sin(xi/2)*cos(yi/2)*cos(zi/2)-cos(xi/2)*sin(yi/2)*sin(zi/2);
k++;
subsref(ret,i,k) = cos(xi/2)*sin(yi/2)*cos(zi/2)+sin(xi/2)*cos(yi/2)*sin(zi/2);
k++;
subsref(ret,i,k) = cos(xi/2)*cos(yi/2)*sin(zi/2)-sin(xi/2)*sin(yi/2)*cos(zi/2);
}
fFreeHandle(x);
fFreeHandle(y);
fFreeHandle(z);
return ret;
}
|
