1 files changed, 90 insertions, 0 deletions
diff --git a/src/libs/ray/ray_euler.c b/src/libs/ray/ray_euler.c
new file mode 100644
index 0000000..f143e25
--- /dev/null
+++ b/src/libs/ray/ray_euler.c
@@ -0,0 +1,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);
+}