squad for quaternions

client/Game.cpp

@@ -20,14 +20,14 @@ pointIndex(0), moveSpeed(0.125f), movedLength(0.0f), mode(Mode::AUTO) {
     rotation = Quaternion(Vector(1, 0, 0), -80);
     lastRotation = rotation;
 
-    Quaternion q(0.0f, 0.0f, 0.0f, 1.0f);
+    Quaternion q;
     for(uint i = 0; i < cameraPoints.getCapacity(); i++) {
         Vector offset(mid, h, mid);
         offset.add(Vector(r.nextFloat(randLength), r.nextFloat(randLength), r.nextFloat(randLength)));
         Vector v;
         v.setAngles(i * 360.0f / cameraPoints.getCapacity(), 0.0f).mul(mid * 0.5f).add(offset);
 
-        q.mul(Quaternion(Vector(r.nextFloat() * 360.0f, r.nextFloat() * -90.0f), -5.0f));
+        q.mul(Quaternion(Vector(r.nextFloat() * 360.0f, r.nextFloat() * -90.0f), -10.0f));
         cameraPoints.add( {v, q, 0.0f});
     }
     for(uint i = 0; i < cameraPoints.getLength(); i++) {
@@ -120,7 +120,18 @@ void Game::renderWorld(float lag, Renderer& renderer) const {
         }
         float t = leftLength / cameraPoints[index].distance;
         Vector interpolatedPos = pointUntilDistance(leftLength, index, 4000);
-        renderer.update(interpolatedPos, cameraPoints[index].q.slerp(t, cameraPoints[(index + 1) % cameraPoints.getLength()].q));
+        
+        
+        uint a = index == 0 ? cameraPoints.getLength() - 1 : index - 1;
+        uint b = (a + 1) %  cameraPoints.getLength();
+        uint c = (a + 2) %  cameraPoints.getLength();
+        uint d = (a + 3) %  cameraPoints.getLength();
+        
+        renderer.update(interpolatedPos, cameraPoints[b].q.squad(t, cameraPoints[a].q, cameraPoints[c].q, cameraPoints[d].q));
+        
+        //renderer.update(interpolatedPos, cameraPoints[index].q.slerp(t, cameraPoints[(index + 1) % cameraPoints.getLength()].q));
+        
+        
         pos = interpolatedPos;
     } else if(mode == Mode::PLAYER) {
         Vector v(lastPos);

client/math/Quaternion.cpp

@@ -1,12 +1,12 @@
 #include <cmath>
+#include <iostream>
 
 #include "client/math/Quaternion.h"
 
-Quaternion::Quaternion() : xyz(0.0f, 0.0f, 0.0f), w(0.0f) {
+Quaternion::Quaternion() : xyz(0.0f, 0.0f, 0.0f), w(1.0f) {
 }
 
 Quaternion::Quaternion(float x, float y, float z, float w) : xyz(x, y, z), w(w) {
-    normalize();
 }
 
 Quaternion::Quaternion(const Vector& axis, float angle) {
@@ -18,7 +18,7 @@ Quaternion::Quaternion(const Vector& axis, float angle) {
 }
 
 Quaternion Quaternion::lerp(float f, const Quaternion& other) const {
-    return interpolate(1 - f, f, other);
+    return interpolate(1 - f, f, other).normalize();
 }
 
 Quaternion Quaternion::slerp(float f, const Quaternion& other) const {
@@ -39,10 +39,25 @@ Quaternion Quaternion::interpolate(float a, float b, const Quaternion& other) co
     return Quaternion(interpolated.getX(), interpolated.getY(), interpolated.getZ(), w * a + other.w * b);
 }
 
-void Quaternion::normalize() {
+Quaternion Quaternion::squad(float f, const Quaternion& prev, const Quaternion& next, const Quaternion& nextNext) const {
+    Quaternion s1 = squadControl(prev, next);
+    Quaternion s2 = next.squadControl(*this, nextNext);
+    return slerp(f, next).slerp(2.0f * f * (1.0f - f), s1.slerp(f, s2));
+}
+
+Quaternion Quaternion::squadControl(const Quaternion& prev, const Quaternion& next) const {
+    Quaternion conjugate(*this);
+    conjugate.conjugate();
+    Quaternion s1(next);
+    s1.mul(conjugate).log().add(Quaternion(prev).mul(conjugate).log()).mul(-0.25f).exp().mul(*this);
+    return s1;
+}
+
+Quaternion& Quaternion::normalize() {
     float f = 1.0f / sqrtf(xyz.dot(xyz) + w * w);
     xyz.mul(f);
     w *= f;
+    return *this;
 }
 
 Matrix Quaternion::toMatrix() const {
@@ -68,9 +83,54 @@ Matrix Quaternion::toMatrix() const {
     return m;
 }
 
-void Quaternion::mul(const Quaternion& other) {
+Quaternion& Quaternion::mul(const Quaternion& other) {
     Vector v;
     v.addMul(other.xyz, w).addMul(xyz, other.w).add(Vector(xyz).cross(other.xyz));
     w = w * other.w - xyz.dot(other.xyz);
     xyz.set(v);
+    return *this;
+}
+
+Quaternion& Quaternion::mul(float f) {
+    xyz.mul(f);
+    w *= f;
+    return *this;
+}
+
+Quaternion& Quaternion::add(const Quaternion& other) {
+    xyz.add(other.xyz);
+    w += other.w;
+    return *this;
+}
+
+Quaternion& Quaternion::conjugate() {
+    xyz.setInverse(xyz);
+    return *this;
+}
+
+Quaternion& Quaternion::log() {
+    // general log
+    //float length = sqrtf(xyz.dot(xyz) + w * w);
+    //float arccos = acos(w / length);
+    //w = logf(length);
+    //xyz.mul(arccos / xyz.length());
+    
+    // optimized use case log
+    xyz.normalize().mul(acos(w));
+    w = 0.0f;
+    return *this;
+}
+
+Quaternion& Quaternion::exp() {
+    // general exp
+    //float expw = expf(w);
+    //float length = xyz.length();
+    //w = expw * cosf(length);
+    //xyz.mul((expw * sin(length)) / length);
+    
+    // optimized use case exp
+    float length = xyz.length();
+    w = cosf(length);
+    xyz.mul((sin(length)) / length);
+    return *this;
 }

client/math/Quaternion.h

@@ -7,17 +7,28 @@
 class Quaternion {
 public:
     Quaternion();
-    Quaternion(float x, float y, float z, float w);
     Quaternion(const Vector& axis, float angle);
     
     Quaternion lerp(float f, const Quaternion& other) const;
     Quaternion slerp(float f, const Quaternion& other) const;
-    void normalize();
+    Quaternion squad(float f, const Quaternion& prev, const Quaternion& next, const Quaternion& nextNext) const;
+    
     Matrix toMatrix() const;
-    void mul(const Quaternion& other);
+    Quaternion& mul(const Quaternion& other);
 
-//private:
+private:
+    Quaternion(float x, float y, float z, float w);
+    
     Quaternion interpolate(float a, float b, const Quaternion& other) const;
+    Quaternion& normalize();
+    
+    Quaternion squadControl(const Quaternion& prev, const Quaternion& next) const;
+    
+    Quaternion& conjugate();
+    Quaternion& log();
+    Quaternion& exp();
+    Quaternion& mul(float f);
+    Quaternion& add(const Quaternion& other);
     
     Vector xyz;
     float w;