3D flight

1 files changed, 49 insertions, 10 deletions

diff --git a/src/math/axis.cc b/src/math/axis.cc
index 7bd5262..cbca99b 100644
--- a/src/math/axis.cc
+++ b/src/math/axis.cc
@@ -6,6 +6,7 @@
 
 // project headers
 #include "math/axis.h"
+#include "math/mathlib.h"
 
 namespace math
 {
@@ -37,18 +38,56 @@ Axis & Axis::operator=(const Axis & other) {
 	return *this;
 }
 
-// alter heading, rotate around Z-axis (positive is left)
-void Axis::direction(const float angle) {
-	for (size_t i=0; i < 3; i++) {
-		//axis_vector[i].rotate(axis_vector[2], angle);
-	}
+// change heading, rotate around Z-axis (positive is left)
+void Axis::change_direction(const float angle) {
+	float cosa = cosf(angle * M_PI / 180.0f);
+	float sina = sinf(angle * M_PI / 180.0f);
+
+	Vector3f forward = axis_vector[0] * cosa + axis_vector[1] * sina;
+	Vector3f left = axis_vector[1] *cosa - axis_vector[0] * sina;
+	
+	axis_vector[0].assign(forward);
+	axis_vector[1].assign(left);
 }
 
-// alter heading, rotate around negative Y-axis (positive is up)
-void Axis::pitch(const float pitch) {
-	for (size_t i=0; i < 3; i++) {
-		//axis_vector[i].rotate(axis_vector[1], -angle);
-	}
+// change pitch, rotate around negative Y-axis (positive is up)
+void Axis::change_pitch(const float angle) {
+	float cosa = cosf(angle * M_PI / 180.0f);
+	float sina = sinf(angle * M_PI / 180.0f);
+
+	Vector3f forward = axis_vector[0] * cosa + axis_vector[2] * sina;
+	Vector3f up = axis_vector[2] * cosa - axis_vector[0] * sina;
+
+	axis_vector[0].assign(forward);
+	axis_vector[2].assign(up);
+}
+
+// change roll, rotate around forward vector (positive is left)
+void Axis::change_roll(const float angle) {
+	float cosa = cosf(angle * M_PI / 180.0f);
+	float sina = sinf(angle * M_PI / 180.0f);
+
+	Vector3f forward = axis_vector[2] * cosa + axis_vector[1] * sina;
+	Vector3f up = axis_vector[1] * cosa - axis_vector[2] * sina;
+
+	axis_vector[2].assign(forward);
+	axis_vector[1].assign(up);
+}
+
+// write an axis to a std::ostream
+std::ostream &operator<<(std::ostream & os, Axis const & axis)
+{
+	os << axis.forward() << " " << axis.left() << " " << axis.up();
+	return os;
+}
+
+// read an axis from a std::istream
+std::istream &operator>>(std::istream & is, Axis & axis)
+{
+	is >> axis[0];
+	is >> axis[1];
+	is >> axis[2];
+	return is;
 }
 
 }