Add some useful stuff

2 files changed, 21 insertions, 1 deletions

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();
+    }
 }