Add some useful stuff

5 files changed, 42 insertions, 2 deletions

diff --git a/README.md b/README.md
index 4484210..43b5e05 100644
--- a/README.md
+++ b/README.md
@@ -2,3 +2,9 @@
 
 Just a collection of useful C++ math routines I've accumulated over
 the years. Used by some of my personal programs. MIT licensed.
+
+What's here:
+
+ - 2-D and 3-D vector classes
+ - Quaternions
+ - 1- and 2-manifolds with numeric integration
diff --git a/include/fml/quat.h b/include/fml/quat.h
index 14c966e..67a89d5 100644
--- a/include/fml/quat.h
+++ b/include/fml/quat.h
@@ -15,11 +15,15 @@ namespace fml {
         quat(vec3 vec);
         quat();
 
-        operator vec3();
+        operator vec3() const;
 
         quat conjugate() const;
 
+        /* returns <cos / 2, x * sin / 2, y * sin / 2, z * sin / 2> */
         static quat from_angleaxis(scalar angle, vec3 axis);
+
+        /* returns rotquat * this * rotquat.conj */
+        quat rotateby(const quat &rotquat) const;
     };
 
     quat operator*(const quat &, const quat &);
diff --git a/include/fml/vec3.h b/include/fml/vec3.h
index 3460c56..a006619 100644
--- a/include/fml/vec3.h
+++ b/include/fml/vec3.h
@@ -5,6 +5,8 @@
 #include "fml.h"
 
 namespace fml {
+    class quat;
+
     class vec3 {
     public:
         scalar v[3];
@@ -27,7 +29,15 @@ namespace fml {
         scalar magnitudeSquared() const;
         vec3 normalize() const;
         scalar dot(const vec3 &other) const;
+
+        /* order is this x other. */
         vec3 cross(const vec3 &other) const;
+
+        /* rotate by a rotation quaternion */
+        vec3 rotateby(const quat &rotquat) const;
+
+        /* return an arbitrary unit vector normal to other */
+        static vec3 any_unit_normal(const vec3 &other);
     };
 
     vec3 operator*(scalar scale, const vec3 &v);
diff --git a/src/quat.cpp b/src/quat.cpp
index dfb2823..140256a 100644
--- a/src/quat.cpp
+++ b/src/quat.cpp
@@ -10,7 +10,7 @@ namespace fml {
     quat::quat(vec3 vec) : w(0), x(vec[0]), y(vec[1]), z(vec[2]) { }
     quat::quat() : w(0), x(0), y(0), z(0) { }
 
-    quat::operator vec3()
+    quat::operator vec3() const
     {
         return vec3(this->x, this->y, this->z);
     }
@@ -28,6 +28,11 @@ namespace fml {
         return quat(this->w, -this->x, -this->y, -this->z);
     }
 
+    quat quat::rotateby(const quat &rotquat) const
+    {
+        return rotquat * (*this) * rotquat.conjugate();
+    }
+
     quat quat::from_angleaxis(scalar angle, vec3 axis)
     {
         scalar si = std::sin(angle / 2);
diff --git a/src/vec3.cpp b/src/vec3.cpp
index bf0b021..c03bdd9 100644
--- a/src/vec3.cpp
+++ b/src/vec3.cpp
@@ -102,4 +102,19 @@ namespace fml{
     {
         return v * scale;
     }
+
+    vec3 vec3::rotateby(const quat &rotquat) const
+    {
+        return rotquat * (*this) * rotquat.conjugate();
+    }
+
+#define EPS 1e-5
+#define ISZERO(a) ((fabs((a)) < EPS))
+
+    vec3 vec3::any_unit_normal(const vec3 &v)
+    {
+        if(ISZERO(v[0]) && ISZERO(v[1]))
+            return vec3(1, 0, 0);
+        return vec3(-v[1], v[0], 0).normalize();
+    }
 }