Vector rotation (no SIMD)

#include "test2.h"

static void cross_product (const float v1[], const float v2[], float res[])
{
    res[0] =  v1[1]*v2[2] - v1[2]*v2[1];
    res[1] = -v1[0]*v2[2] + v1[2]*v2[0];
    res[2] =  v1[0]*v2[1] - v1[1]*v2[0];
}

void quat_mul (float q1[], float q2[], float res[])
{
    res[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
    res[1] = q1[2]*q2[3] - q1[3]*q2[2] + q1[1]*q2[0] + q1[0]*q2[1];
    res[2] = q1[3]*q2[1] - q1[1]*q2[3] + q1[2]*q2[0] + q1[0]*q2[2];
    res[3] = q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0] + q1[0]*q2[3];
}

void rotate_vector (float base[], float vector[], float res[])
{
    float tmp[3], tmp2[3];
    int i;
    cross_product (base+1, vector, tmp);
    for (i=0; i<3; i++) tmp[i] *= 2.0;
    cross_product (base+1, tmp, tmp2);
    for (i=0; i<3; i++) res[i] = vector[i] + base[0]*tmp[i] + tmp2[i];
}