summaryrefslogtreecommitdiff
path: root/src/modules/ray/ray_euler.c
blob: f143e2546d7553ac0cd2f24f936f10f8c8e561aa (plain)
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);
}
© All Rights Reserved