VEX Squad implee

#define is_zero(x) abs(x) < 0.0001
#define is_one(x) abs(x) > 0.9999

// quaternion dot product (vex version omits last component)
function float qdot(vector4 left; vector4 right)
{
    return left.x * right.x + left.y * right.y
        + left.z * right.z + left.w * right.w;
}

// slerp with no invert
function vector4 slerp_non_shortest(vector4 from; vector4 to; float factor)
{
    float dot_product = qdot(from, to);
    if (is_one(dot_product)) return from;

    float th = acos(dot_product);
    float sinTh = 1.0 / sin(th);

    float t = sin(factor * th) * sinTh;
    float invt = sin((1.0 - factor) * th) * sinTh;

    return set(
        invt * from.x + t * to.x,
        invt * from.y + t * to.y,
        invt * from.z + t * to.z,
        invt * from.w + t * to.w
    );
}

// logarithmic quaternion
function vector4 qlog(vector4 q)
{
    float a = acos(q.w);
    float s = sin(a);

    if (is_zero(s)) return 0;

    float f = a / s;

    return set(f * q.x, f * q.y, f * q.z, 0);
}

// exponential quaternion
function vector4 qexp(vector4 q)
{
    float l = length(vector(q));
    float s = sin(l);
    float c = cos(l);

    if (is_zero(l)) return set(0, 0, 0, c);

    float t = s / l;

    return set(t * q.x, t * q.y, t * q.z, c);
}

// get keyframe control point for squad based on previous and next keyframes
function vector4 get_squad_control(vector4 prev; vector4 current; vector4 next)
{
    vector4 qinv = qinvert(current);
    vector4 qprev = qmultiply(qinv, prev);
    vector4 qnext = qmultiply(qinv, next);
    vector4 qlogsum = -0.25 * (qlog(qprev) + qlog(qnext));
    vector4 qe = qexp(qlogsum);
    return qmultiply(current, qe);
}

// squad interpolation: from_control and to_control must be calculated beforehand with get_squad_control()
vector4 qsquad(vector4 from; vector4 to; vector4 from_control; vector4 to_control; float factor)
{
    return slerp_non_shortest(
        slerp_non_shortest(from, to, factor),
        slerp_non_shortest(from_control, to_control, factor),
        2.0 * factor * (1 - factor)
    );
}

// example for 4 keys

vector key1 = chv("key1");
vector key2 = chv("key2");
vector key3 = chv("key3");
vector key4 = chv("key4");

vector keys[] = array(key1, key2, key3, key4, key1);
vector4 qkeys[] = array(0, 0, 0, 0, 0);
vector4 qcontrols[] = array(0, 0, 0, 0, 0);

for (int i = 0; i < 5; i++)
{
    qkeys[i] = eulertoquaternion(radians(keys[i]), XFORM_XYZ);
}

for (int i = 0; i < 5; i++)
{
    int prev_index = i - 1 < 0 ? 3 : i - 1;
    int next_index = i + 1 > 4 ? 1 : i + 1;
    qcontrols[i] = get_squad_control(qkeys[prev_index], qkeys[i], qkeys[next_index]);
}

float time_normalized = chf("totaltime");
float time_step = 1.0 / 4.0;

int key_index = (int)fit01(time_normalized, 0, 4);
float local_time = time_normalized / time_step - key_index;

vector4 current_orientation = qsquad(qkeys[key_index], qkeys[key_index + 1],
    qcontrols[key_index], qcontrols[key_index + 1], local_time);

vector angles = quaterniontoeuler(current_orientation, XFORM_XYZ);

matrix transform = ident();
rotate(transform, angles, XFORM_XYZ);
setpackedtransform(0, @primnum, transform);