[javascript]understanding quaternion

quaternion.js

//[understanding quaternion]
// - basic vector knowledge
// - quaternion basics
// - quaternion multiply
// - rotation with quaternion
// - slerp
// - convert quaternion to matrix

//[(basic) vector func]
var negate = function (vec) {
    return vec.map(function (e) {return -e;});
};
var norm = function (vec) {
    return vec.reduce(function (s, e) {return s + e * e;}, 0);
};
var distance = function (vec) {
    return Math.sqrt(norm(vec));
};
//[normalize]
// for any v, distance(normalize(v)) === 1
var normalize = function (vec) {
    var dist = distance(vec);
    // assert(dist !== 0);
    return vec.map(function (e) {return e / dist;});
};
//[dot product]
//- dot(a, b) = distance(a)*distance(b)*angle(a, b)
//- a * dot(a, b): means a-parallel part of b (if a is x-axis, just [b.x, 0])
var dot = function (a, b) {
    var s = 0;
    for (var i = 0; i < a.length; i++) {
        s += a[i] * b[i];
    }
    return s;
};
//[(reference) angle between two vectors]
var angle = function (a, b) {
    var da = distance(a);
    var db = distance(b);
    // assert(da !== 0 && db !== 0);
    var cos = dot(a, b) / (da * db);
    return Math.acos(cos);
};
//[add]
var add = function (a, b) {
    var s = [];
    for (var i = 0; i < a.length; i++) {
        s.push(a[i] + b[i]);
    }
    return s;
};
//[factor]
var factor = function (n, v) {
    return v.map(function (e) {return n * e;});
};

//[quaternion]
//imaginary units: i, j, k
// quaternion q = q0 + q1i + q2j + q3k
// as vector4: [q0, q1, q2, q3]
//
// (on 2D point [x, y] as imaginery number: x + yi
//
//[unit quaternion = 1]
var unit = Object.freeze([1,0,0,0]);

//[make quaternion from axis and angle]
// normarized axis a = [x, y, z] and angle p
// rotation quaternion q become:
//   q = cos(p/2) + sin(p/2)*(x*i + y*j + z*k) 
var a2q = function (axis, phi) {
    var cos = Math.cos(phi / 2);
    var sin = Math.sin(phi / 2);
    var normal = normalize(axis);
    return [cos, sin * normal[0], sin * normal[1], sin * normal[2]];
};

//[conjugation]
// q = q0 + q1i + q2j + q3k
// q~ = conj(q) = q0 - q1i - q2i - q3k
var conj = function (q) {
    return [q[0], -q[1], -q[2], -q[3]];
};

// [multiply for quaternion]
// [basics of quaternion imaginary units]
// q = q0 + q1i + q2j + q3j
// ii = jj = kk = ijk = -1
//
// [advanced]
// ijk * k = -1 * k
//     -ij = -k
//      ij = k
//  ij * j = k * j
//      -i = k*j
//  -i * i = k * j * i
//       1 = kji
//     k*1 = k * kji
//       k = -ji
//
// ij = k = -ji
// jk = i = -kj
// ki = j = -ik
// kji = 1
// (mult order of i j k must be preserved)
//
// (a1i + a2j + a3k) * (b1i + b2j + b3k) 
//   = (a1b1ii + a1b2ij + a1b3ik) +
//     (a2b1ji + a2b2jj + a2b3jk) +
//     (a3b1ki + a3b2kj + a2b3kk)
//   = (-a1b1 + a1b2k - a1b3j) +
//     (-a2b1k - a2b2 + a2b3i) +
//     (a3b1j - a3b2i - a2b3)
//   = -(a1b1 + a2b2 + a3b3) + (a2b3 - a3b2)i + (a3b1 - a1b3)j + (a1b2 - a2b1)k
//
// [mult]
// (a0 + a1i + a2j + a3k) * (b0 + b1i + b2j + b3k)
//   = a0b0 + a0(b1i + b2j + b3k) + (a1i + a2j + a3k)b0 + 
//     (a1i + a2j + a3k) * (b1i + b2j + b3k) 
//   = a0b0 + a0b1i + a0b2j + a0b3k + a1b0i + a2b0j + a3b0k +
//     -(a1b1 + a2b2 + a3b3) + (a2b3 - a3b2)i + (a3b1 - a1b3)j + (a1b2 - a2b1)k
//   = (a0b0 - a1b1 - a2b2 - a3b3) +
//     (a0b1 + a1b0 + a2b3 - a3b2)i + 
//     (a0b2 + a2b0 + a3b1 - a1b3)j +
//     (a0b3 + a3b0 + a1b2 - a2b1)k
var mult = function (a, b) {
    return [
        a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
        a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2],
        a[0] * b[2] + a[2] * b[0] + a[3] * b[1] - a[1] * b[3],
        a[0] * b[3] + a[3] * b[0] + a[1] * b[2] - a[2] * b[1]];
};


//[vector rotataion with quaternion]
// v = 0 + v0i + v1j + v2k
// r (rotated v with q) = q * v * q~ 
//                      = r0 + r1i + r2j + r3k
// rot(v, q) = [r1, r2, r3]
var rot_direct = function (vec, q) {
    var v = [0, vec[0], vec[1], vec[2]];
    var r = mult(mult(q, v), conj(q));
    return [r[1], r[2], r[3]];
};
// [extact mult]
// q*v with v[0] = 0
// (q*v)[0] = - q[1] * v[1] - q[2] * v[2] - q[3] * v[3]
// (q*v)[1] = q[0] * v[1] + q[2] * v[3] - q[3] * v[2]
// (q*v)[2] = q[0] * v[2] + q[3] * v[1] - q[1] * v[3]
// (q*v)[3] = q[0] * v[3] + q[1] * v[2] - q[2] * v[1]
// (qv*q~)[1] = qv[0] * -q[1] + qv[1] * q[0] + qv[2] * -q[3] - qv[3] * -q[2]
// (qv*q~)[2] = qv[0] * -q[2] + qv[2] * q[0] + qv[3] * -q[1] - qv[1] * -q[3]
// (qv*q~)[3] = qv[0] * -q[3] + qv[3] * q[0] + qv[1] * -q[2] - qv[2] * -q[1]
var rot = function (vec, q) {
    var qv0 = -q[1] * vec[0] - q[2] * vec[1] - q[3] * vec[2];
    var qv1 = q[0] * vec[0] + q[2] * vec[2] - q[3] * vec[1];
    var qv2 = q[0] * vec[1] + q[3] * vec[0] - q[1] * vec[2];
    var qv3 = q[0] * vec[2] + q[1] * vec[1] - q[2] * vec[0];
    var r1 = qv0 * -q[1] + qv1 * q[0] + qv2 * -q[3] - qv3 * -q[2];
    var r2 = qv0 * -q[2] + qv2 * q[0] + qv3 * -q[1] - qv1 * -q[3];
    var r3 = qv0 * -q[3] + qv3 * q[0] + qv1 * -q[2] - qv2 * -q[1];
    return [r1, r2, r3];
};


//[connect rotation quaternions]
// rot(rot(v, a), b) = rot(v, connect(a, b))
// connect(a, b) = b * a
var connect = function (qs) {
    return qs.reduce(function (r, q) {
        return mult(q, r);
    }, unit);
};

var r2q = function (roll, pitch, yaw) {
    // from roll-pitch-yaw to quaternion
    return connect([
        a2q([1,0,0], roll),
        a2q([0,1,0], pitch),
        a2q([0,0,1], yaw),
    ]);
};
var e2q = function (z1, x, z2) {
    // from eular angles to quaternion
    return connect([
        a2q([0,0,1], z1),
        a2q([1,0,0], x),
        a2q([0,0,1], z2),
    ]);
};

// [(reference) another mult definition and rotation]
// there is another mult definition of quaternions
// (e.g. O'Reilly's 3D Math Primer for Graphics and Game Development)
// the mult is:
// a (*) b = (a0b0 - a1b1 - a2b2 - a3b3) +
//           (a0b1 + a1b0 - a2b3 + a3b2)i + 
//           (a0b2 + a2b0 - a3b1 + a1b3)j +
//           (a0b3 + a3b0 - a1b2 + a2b1)k
//         = b * a
// 
// it changes rot(v, q) as: q~ (*) v (*) q
// and connection of rotation quaternions connect([a, b]) as: a (*) b


/*
//example
var z90q = a2q([0,0,1], Math.PI/2);
var x90q = a2q([1,0,0], Math.PI/2);
console.log(rot([1,0,0], z90q));
console.log(rot([1,0,0], x90q));
console.log(rot(rot([1,0,0], z90q), x90q)); //=> [0,0,1]
console.log(rot([1,0,0], connect([z90q, x90q]))); //=> [0,0,1]
console.log(rot([1,0,0], mult(x90q, z90q))); //=> [0,0,1]
*/

// [slerp]
// [(reference) lerp: linear interpolation]
var lerp = function (s, e, t) {
    var ps = 1 - t;
    var pe = t;
    var r = [];
    for (var i = 0; i < s.length; i++) {
        r.push(ps * s[i] + pe * e[i]);
    };
    return r;
};
// [slerp: spherical linear interpolation]
var slerp = function (s, e, t) {
    var cos = dot(s, e);
    var sin = Math.sqrt(1 - cos * cos);
    if (sin === 0) return lerp(s, e, t);
    var angle = Math.acos(cos);
    var ps = Math.sin(angle * (1 - t)) / sin;
    var pe = Math.sin(angle * t) / sin;
    var r = [];
    for (var i = 0; i < s.length; i++) {
        r.push(ps * s[i] + pe * e[i]);
    };
    return r;
};

//example
/*
var z90q = a2q([0,0,1], Math.PI/2);
var x90q = a2q([1,0,0], Math.PI/2);
var p = [0,1,0];
console.log(rot(p, slerp(z90q, x90q, 0.0)));
console.log(rot(p, slerp(z90q, x90q, 0.2)));
console.log(rot(p, slerp(z90q, x90q, 0.4)));
console.log(rot(p, slerp(z90q, x90q, 0.6)));
console.log(rot(p, slerp(z90q, x90q, 0.8)));
console.log(rot(p, slerp(z90q, x90q, 1.0)));
*/

//[quaternion to matrix]
//
// [(basics) trigonometric formulas]
// sin(t)*sin(t) + cos(t)*cos(t) = 1
//                 sin(t)*sin(t) = 1 - cos(t)*cos(t)
//                 cos(t)*cos(t) = 1 - sin(t)*sin(t)
// sin(t) = 2*sin(t/2)*cos(t/2)
// cos(t) = cos(t/2)*cos(t/2) - sin(t/2)*sin(t/2)
//        = 1 - 2*sin(t/2)*sin(t/2)
//        = 2*cos(t/2)*cos(t/2) - 1
// 
// [(reference) from vector to matrix as 2D rotation]
// p = [p[0], p[1]]
// nv = [-p[1], p[0]]
//      (orthogonal vector against p)
//
// rot2d(t, p) = p * cos(t) + nv * sin(t)
//
//   nv: norm(nv) = 1, dot(p, nv) = 0, angle(p, nv) = PI/2
//
// rot2d(t, p) = p * cos(t) + nv * sin(t)
//             = [cos(t)*p[0], cos(t)*p[1]] + [-sin(t)*p[1], sin(t)*p[0]]
//             = [cos(t)*p[0] -sin(t)*p[1], sin(t)*p[0] + cos(t)*p[1]]
//             = [cos(t), -sin(t),
//                sin(t), cos(t)] * [p[0], p[1]]
//             = [cos(t), -sin(t),
//                sin(t), cos(t)] * p
//             = [m00, m01,
//                m10, m11] * p
//             = m * p
// m = [m00 = cos(t), m01 = -sin(t), 
//      m10 = sin(t), m11 = cos(t)]
//
//[(reference) 2D rotation as z-axis 3D rotation]
// when 2D extends to 3D;
//   rot2d(t, [p0, p1]) 
//     => rotXY(t, [p0, p1, p2]) 
//        = [cos(t)*p[0] -sin(t)*p[1], sin(t)*p[0] + cos(t)*p[1], p[2]]
//
// 2d(x-y-plane) rotation becomes z-axis rotation:
// 
//   rotXY(t, p) => rot([0,0,1], t, p)
//
// z-part keeps its value after rotated, so
// the matrix m is extended 3D as: [m00, m01, 0,
//                                  m10, m11, 0,
//                                  0,   0,   1]
//
// then think rotation as quaternion:
//
// quaternion([0,0,1], t) = cos(t/2) + 0*i + 0*j + sin(t/2)*k 
//                         = q0 + q1*i + q2*j + q3*k
//
// m00 = m11 = cos(t) 
//           = 2*cos(t/2)*cos(t/2) - 1
//           = 2*q0*q0 - 1
// m10 = -m01 = sin(t) 
//            = 2*sin(t/2)*cos(t/2)
//            = 2*q3*q0
//
// m = [2*q0*q0 - 1, -2*q3*q0, 0,
//      2*q3*q0, 2*q0*q0 - 1, 0,
//      0, 0, 1]
//
// (convert from z-axis quaternion to 3D rotation matrix)
//
//
// [decompose 3D rotation with axis-a and angle-t]
// rot(a, t, p) = (2D rot a-ortho part of p on a-ortho plane) + (a-para part of p)
//              = (pv * cos(t) + nv * sin(t)) + ph
//
// nv = cross(a, p)
// ph = a * dot(a, p) 
//   (= a* distance(p) * cos(angle(a, p)) : norm(a) is 1)
// pv = p - ph
//
//   nv: orthogonal vector against both a and p
//   ph: a-parallel part of p ([0, 0, p2] when a is z-axis) 
//   pv: a-orthogonal part of p ([p0, p1, 0] when a is z-axis) 
//
// rot(a, t, p)
//   = (p - a * dot(a, p)) * cos(t) + cross(a, p) * sin(t) + (a * dot(a, p))
//   = p * cos(t) + cross(a, p) * sin(t) + (a * dot(a, p)) * (1 - cos(t))
//     (rodorigues rotation formula)
// 
// [convert to matrix form]
// p * cos(t) = [cos(t)*p[0], cos(t)*p[1], cos(t)*p[2]
//            = cos(t) * [1, 0, 0,
//                        0, 1, 0,
//                        0, 0, 1] * p
// 
// cross(a, p) = [a[1] * p[2] - a[2] * p[1],
//                a[2] * p[0] - a[0] * p[2],
//                a[0] * p[1] - a[1] * p[0]]
//             = [0. -a[2], a[1],
//                a[2], 0, -a[0],
//                -a[1], a[0], 0] * p
//
// a * dot(a, p) = [a[0], a[1], a[2]]  * ([a[0],
//                                        a[1],
//                                        a[2]] * [p[0], p[1], p[2]])
//               = ([a[0], a[1], a[2]]  * [a[0],
//                                         a[1],
//                                         a[2]]) * [p[0], p[1], p[2]]
//               = [a[0]*a[0], a[0]*a[1], a[0]*a[2],
//                  a[1]*a[0], a[1]*a[1], a[1]*a[2],
//                  a[2]*a[0], a[2]*a[1], a[2]*a[2]] * p
//
// [rotation with axis and angle as matrix]
// rot(a, t, p)
//   = p * cos(t) + cross(a, p) * sin(t) + (a * dot(a, p)) * (1 - cos(t))
//   = [cos(t) + a[0]*a[0]*(1-cos(t)), -a[2]*sin(t) + a[0]*a[1]*(1-cos(t)), a[1]*sin(t) + a[0]*a[2]*(1-cos(t)),
//      a[2]*sin(t) + a[1]*a[0]*(1-cos(t)), cos(t) + a[1]*a[1]*(1-cos(t)), -a[0]*sin(t) + a[1]*a[2]*(1-cos(t)),
//      -a[1]*sin(t) + a[2]*a[0]*(1-cos(t)), a[0]*sin(t) + a[2]*a[1]*(1-cos(t)), cos(t) + a[2]*a[2]*(1-cos(t))] * p
//   = [m00, m01, m02,
//      m10, m11, m12,
//      m20, m21, m22] *p
//   = m * p
//
// [rotation matrix for quaternion elements]
//
// [revisit quaternion]
// a2q(a, t) = cos(t/2) + sin(t/2)*a[0]*i + sin(t/2)*a[1]*j + sin(t/2)*a[2]*k
//           = q0 + q1*i + q2*j + q3*k
// q0 = cos(t/2)
// q1 = a[0]*sin(t/2)
// q2 = a[1]*sin(t/2)
// q3 = a[2]*sin(t/2)
// (q0*q0 + q1*q1 + q2*q2 + q3*q3 = 1)
//
// [transform matrix element]
// m00 = cos(t) + a[0]*a[0]*(1-cos(t)) 
//     = 1 - (1-cos(t)) + a[0]*a[0]*(1-cos(t))
//     = 1 - (1-a[0]*a[0])*(1-cos(t))
//     = 1 - (1-a[0]*a[0])*(1-(1-2*sin(t/2)*sin(t/2)))
//     = 1 - (1-a[0]*a[0])*2*sin(t)*sin(t/2)
//     = 1 - 2*sin(t/2)*sin(t/2) + 2*a[0]*a[0]*sin(t/2)*sin(t/2)
//     = 1 - 2*(1-cos(t/2)*cos(t/2)) + 2*a[0]*a[0]*sin(t/2)*sin(t/2)
//     = 1 - 2*(1-q0*q0) + 2*q1*q1
//     = -1 + 2*(q0*q0 + q1*q1)
//     = 2*(q0*q0 + q1*q1) - 1
//     = 2*(1 - q2*q2 - q3*q3) - 1
//     = 1 - 2*(q2*q2 + q3*q3)
// m11 = cos(t) + a[1]*a[1]*(1-cos(t)) 
//     = 2*(q0*q0 + q2*q2) - 1
//     = 1 - 2*(q3*q3 + q1*q1)
// m22 = cos(t) + a[2]*a[2]*(1-cos(t)) 
//     = 2*(q0*q0 + q3*q3) - 1
//     = 1 - 2*(q1*q1 + q2*q2)
// 
// m01 = -a[2]*sin(t) + a[0]*a[1]*(1-cos(t))
//     = -a[2]*2*sin(t/2)*cos(t/2) + a[0]*a[1]*2*sin(t/2)*sin(t/2)
//     = -2*q0*q3 + 2*q1*q2
// m02 = a[1]*sin(t) + a[0]*a[2]*(1-cos(t))
//     = 2*q0*q2 + 2*q1*q3
// m10 = a[2]*sin(t) + a[1]*a[0]*(1-cos(t)), cos(t)
//     = 2*q0*q3 + 2*q2*q1
// m12 = -a[0]*sin(t) + a[1]*a[2]*(1-cos(t))
//     = -2*q0*q1 + 2*q2*q3
// m20 = -a[1]*sin(t) + a[2]*a[0]*(1-cos(t))
//     = -2*q0*q2 + 2*q3*q1
// m21 = a[0]*sin(t) + a[2]*a[1]*(1-cos(t))
//     = 2*q0*q1 + 2*q3*q2
//
// m = [2*(q0*q0+q1*q1)-1, 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2),
//      2*(q2*q1+q0*q3), 2*(q0*q0+q2*q2)-1, 2*(q2*q3-q0*q1),
//      2*(q3*q1-q0*q2), 2*(q3*q2+q0*q1), 2*(q0*q0+q3*q3)-1]
var q2m = function (q) {
    var q01 = q[0]*q[1], q02 = q[0]*q[2], q03 = q[0]*q[3];
    var q11 = q[1]*q[1], q12 = q[1]*q[2], q13 = q[1]*q[3];
    var q21 = q12, q22 = q[2]*q[2], q23 = q[2]*q[3];
    var q31 = q13, q32 = q23, q33 = q[3]*q[3];
    
    return [1-2*(q22+q33), 2*(q12-q03), 2*(q13+q02),
            2*(q21+q03), 1-2*(q33+q11), 2*(q23-q01),
            2*(q31-q02), 2*(q32+q01), 1-2*(q11+q22)];
};

var mulmv = function (m, v) {
    var len = v.length;
    return v.map(function (ve, i) {
        return v.map(function (e, j) {
            return m[len*i+j]*e;
        }).reduce(function (s, e) {return s + e}, 0);
    });
};

/*
//example
var q = a2q([0.2,0.5,1], Math.PI/2);
console.log(rot([1,2,3], q));
var m = q2m(q);
console.log(m);
console.log(mulmv(m, [1,2,3]));
*/

//[matrix to quaternion]
// m00 = 2*(q0*q0 + q1*q1) - 1
// m11 = 2*(q0*q0 + q2*q2) - 1
// m22 = 2*(q0*q0 + q3*q3) - 1
//
// m00 + m11 + m22 = 2*(3*q0*q0 + q1*q1 + q2*q2 + q3*q3) - 3
//                 = 2*(3*q0*q0 + (1 - q0*q0) - 3
//                 = 2*2*q0*q0 -1
// q0*q0 = (1 + m00 + m11 + m22) / 4
// +-q0 = sqrt(1 + m00 + m11 + m22) / 2
//
// m10 = 2*q0*q3 + 2*q2*q1
// m01 = -2*q0*q3 + 2*q1*q2
// m02 = 2*q0*q2 + 2*q1*q3
// m20 = -2*q0*q2 + 2*q1*q3
// m21 = 2*q0*q1 + 2*q3*q2
// m12 = -2*q0*q1 + 2*q3*q2
//
// (assert q0 != 0)
// m10 - m01 = 2*2*q0*q3
// q3 = (m10 - m01) / (4*q0)
// q2 = (m02 - m20) / (4*q0)
// q1 = (m21 - m12) / (4*q0)
//
// [calc q1 1st]
// m00 - m11 - m22 =  2*(-q0*q0 +q1*q1 - q2*q2 - q3*q3) + 1
//                  = 2*(q1*q1 - (q0*q0 + q2*q2 + q3*q3) + 1
//                  = 2*(q1*q1 - (1 - q1*q1)) + 1
//                  = 2*2*q1*q1 - 1
// q1*q1 = (1 + m00 - m11 - m22)/4
// +-q1 = sqrt(1 + m00 - m11 - m22) / 2
//      != 0
//
// m10 + m01 = 2*2*q1*a2
// q2 = (m10 + m01) / (4*q1)
// q3 = (m02 + m20) / (4*q1)
// q0 = (m21 - m12) / (4*q1)
//
// [calc q2 1st]
// +-q2 = sqrt(1 - m00 + m11 - m22) / 2 
//      != 0
// q1 = (m10 + m01) / (4*q2)
// q3 = (m21 + m12) / (4*q2)
// q0 = (m02 - m20) / (4*q2)
// 
// [calc q3 1st]
// +-q3 = sqrt(1 - m00 - m11 + m22) / 2 
//      != 0
// q1 = (m02 + m20) / (4*q3)
// q2 = (m21 + m12) / (4*q3)
// q0 = (m10 - m01) / (4*q3)
// 
// (choose qn 1st when qn=sqrt(...)/2 is max one, then convert as q0 >= 0)
var m2q = function (m) {
    var m00 = m[0], m01 = m[1], m02 = m[2];
    var m10 = m[3], m11 = m[4], m12 = m[5];
    var m20 = m[6], m21 = m[7], m22 = m[8];
    
    var q0 = Math.sqrt(1 + m00 + m11 + m22) / 2;
    var q1 = Math.sqrt(1 + m00 - m11 - m22) / 2;
    var q2 = Math.sqrt(1 - m00 + m11 - m22) / 2;
    var q3 = Math.sqrt(1 - m00 - m11 + m22) / 2;
    if (q0 >= q1 && q0 >= q2 && q0 >= q3) { 
        q3 = (m10 - m01) / (4 * q0);
        q2 = (m02 - m20) / (4 * q0);
        q1 = (m21 - m12) / (4 * q0);
    } else if (q1 >= q0 && q1 >= q2 && q1 >= q3) {
        q2 = (m10 + m01) / (4 * q1);
        q3 = (m02 + m20) / (4 * q1);
        q0 = (m21 - m12) / (4 * q1);
    } else if (q2 >= q0 && q2 >= q1 && q2 >= q3) {
        q1 = (m10 + m01) / (4 * q2);
        q3 = (m21 + m12) / (4 * q2);
        q0 = (m02 - m20) / (4 * q2);
    } else if (q3 >= q0 && q3 >= q1 && q3 >= q2) {
        q1 = (m02 + m20) / (4 * q3);
        q2 = (m21 + m12) / (4 * q3);
        q0 = (m10 - m01) / (4 * q3);
    } else throw Error("unreachable point");
    var q = [q0, q1, q2, q3];
    return (q0 < 0) ? negate(q) : q;
};


//[squad]
//
//[exp and log for quaternion]
// exp(log(q)) == q
// log(1) = 0
// log(a * b) = log(a) + log(b)
// exp(0) = 1
// exp(a + b) = exp(a) * exp(p)
var log = function (q) {
    //assert norm(q) === 1 
    var a = Math.acos(q[0]);
    var sin = Math.sin(a);
    if (sin === 0) return [0, 0, 0, 0];
    var t = a / sin;
    return [0, t * q[1], t * q[2], t * q[3]];
};
var exp = function (q) {
    // assert q[0] === 0
    var a = distance(q);
    if (a === 0) return unit;
    var sin = Math.sin(a);
    var t = sin / a;
    return [Math.cos(a), t * q[1], t * q[2], t * q[3]];
};
//[squad]
// from http://theory.org/software/qfa/writeup/node12.html
var quadrangle = function (qprev, q, qnext) {
    var a = log(mult(conj(q), qnext));
    var b = log(mult(conj(q), qprev));
    return mult(q, exp(factor(-1/4, add(a, b))));
};
var quadrangles = function (qs) {
    //assert qs.length >= 4
    var qrs = [];
    for (var i = 1; i < qs.length - 1; i++) {
        qrs.push(quadrangle(qs[i-1], qs[i], qs[i+1]));
    }
    return qrs;
};
var squad = function (qs, qrs, t) {
    //assert qs.length >= 4 && qrs.length === qs.length - 2
    //assert 0 <= t && t <= 1
    if (t === 1) return qs[qs.length - 2];
    //assert 0 <= t && t < 1
    var tl = (t * (qrs.length - 1));
    var i = 0| tl;
    //assert 0 <= i && i < qrs.length - 1
    var h = tl - i;
    //assert 0 <= h && h < 1
    var a = slerp(qs[i+1], qs[i+2], h);
    var b = slerp(qrs[i], qrs[i+1], h);
    return slerp(a, b, 2*h*(1-h));
};