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
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
|
#include <assert.h>
#include <math.h>
#include "ray_3f.h"
#include "ray_euler.h"
/* produce orthonormal basis vectors from euler angles, rotated in the specified order */
void ray_euler_basis(const ray_euler_t *e, ray_3f_t *forward, ray_3f_t *up, ray_3f_t *left)
{
float cos_yaw = cosf(e->yaw);
float sin_yaw = sinf(e->yaw);
float cos_roll = cosf(e->roll);
float sin_roll = sinf(e->roll);
float cos_pitch = cosf(e->pitch);
float sin_pitch = sinf(e->pitch);
/* Rotation matrices from http://www.songho.ca/opengl/gl_anglestoaxes.html */
switch (e->order) {
case RAY_EULER_ORDER_PYR:
/* pitch, yaw, roll */
up->x = -cos_yaw * sin_roll;
up->y = -sin_pitch * sin_yaw * sin_roll + cos_pitch * cos_roll;
up->z = cos_pitch * sin_yaw * sin_roll + sin_pitch * cos_roll;
forward->x = sin_yaw;
forward->y = -sin_pitch * cos_yaw;
forward->z = cos_pitch * cos_yaw;
break;
case RAY_EULER_ORDER_YRP:
/* yaw, roll, pitch */
up->x = -cos_yaw * sin_roll * cos_pitch + sin_yaw * sin_pitch;
up->y = cos_roll * cos_pitch;
up->z = sin_yaw * sin_roll * cos_pitch + cos_yaw * sin_pitch;
forward->x = cos_yaw * sin_roll * sin_pitch + sin_yaw * cos_pitch;
forward->y = -cos_roll * sin_pitch;
forward->z = -sin_yaw * sin_roll * sin_pitch + cos_yaw * cos_pitch;
break;
case RAY_EULER_ORDER_RPY:
/* roll, pitch, yaw */
up->x = -sin_roll * cos_pitch;
up->y = cos_roll * cos_pitch;
up->z = sin_pitch;
forward->x = cos_roll * sin_yaw + sin_roll * sin_pitch * cos_yaw;
forward->y = sin_roll * sin_yaw - cos_roll * sin_pitch * cos_yaw;
forward->z = cos_pitch * cos_yaw;
break;
case RAY_EULER_ORDER_PRY:
/* pitch, roll, yaw */
up->x = -sin_roll;
up->y = cos_pitch * cos_roll;
up->z = sin_pitch * cos_roll;
forward->x = cos_roll * sin_yaw;
forward->y = cos_pitch * sin_roll * sin_yaw - sin_pitch * cos_yaw;
forward->z = sin_pitch * sin_roll * sin_yaw + cos_pitch * cos_yaw;
break;
case RAY_EULER_ORDER_RYP:
/* roll, yaw, pitch */
up->x = -sin_roll * cos_pitch + cos_roll * sin_yaw * sin_pitch;
up->y = cos_roll * cos_pitch + sin_roll * sin_yaw * sin_pitch;
up->z = cos_yaw * sin_pitch;
forward->x = sin_roll * sin_pitch + cos_roll * sin_yaw * cos_pitch;
forward->y = -cos_roll * sin_pitch + sin_roll * sin_yaw * cos_pitch;
forward->z = cos_yaw * cos_pitch;
break;
case RAY_EULER_ORDER_YPR:
/* yaw, pitch, roll */
up->x = -cos_yaw * sin_roll + sin_yaw * sin_pitch * cos_roll;
up->y = cos_pitch * cos_roll;
up->z = sin_yaw * sin_roll + cos_yaw * sin_pitch * cos_roll;
forward->x = sin_yaw * cos_pitch;
forward->y = -sin_pitch;
forward->z = cos_yaw * cos_pitch;
break;
default:
assert(0);
}
*left = ray_3f_cross(up, forward);
}
|
