Linear Algebra

math.c

#include <math.h>
#define op(r,e,a,p,b,i,s) ((r) e (a) p ((b) i (s)))
#define lerp(r,a,b,t) ((r)=(a)+(t)*((b)-(a)))
#define rad(x) ((x)*3.141592653f/180.0f)

#define op3(r,e,a,p,b,i,s)\
    do{op((r)[0],e,(a)[0],p,(b)[0],i,s), op((r)[1],e,(a)[1],p,(b)[1],i,s),\
    op((r)[2],e,(a)[2],p,(b)[2],i,s);} while(0)
#define op3s(r,e,a,p,s)\
    do{op((r)[0],e,(0),+,(a)[0],p,s), op((r)[1],e,(0),+,(a)[1],p,s),\
    op((r)[2],e,(0),+,(a)[2],p,s);} while(0)

#define set3(v,x,y,z)   (v)[0]=(x),(v)[1]=(y),(v)[2]=(z)
#define cpy3(d,s)       (d)[0]=(s)[0],(d)[1]=(s)[1],(d)[2]=(s)[2]
#define add3(d,a,b)     op3(d,=,a,+,b,+,0)
#define sub3(d,a,b)     op3(d,=,a,-,b,+,0)
#define mul3(d,a,s)     op3s(d,=,a,*,s)
#define dot3(a,b)       ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
#define len3(v)         sqrt(dot3(v,v))
#define adds3(d,a,b,s)  op3(d,=,a,+,b,*,s)
#define subs3(d,a,b,s)  op3(d,=,a,-,b,*,s)
#define norm3(n,v)      mul3(n,v,isqrt(dot3(v, v)))
#define normeq3(v)      norm3(v,v)
#define lerp3(r,a,b,t)  lerp(r[0],a[0],b[0],t), lerp(r[1],a[1],b[1],t), lerp(r[2],a[2],b[2],t)
#define cross3(d,a,b) do {\
    (d)[0] = ((a)[1]*(b)[2]) - ((a)[2]*(b)[1]),\
    (d)[1] = ((a)[2]*(b)[0]) - ((a)[0]*(b)[2]),\
    (d)[2] = ((a)[0]*(b)[1]) - ((a)[1]*(b)[0]);}while(0)

#define op4(r,e,a,p,b,i,s)\
    do{op((r)[0],e,(a)[0],p,(b)[0],i,s), op((r)[1],e,(a)[1],p,(b)[1],i,s),\
    op((r)[2],e,(a)[2],p,(b)[2],i,s), op((r)[3],e,(a)[3],p,(b)[3],i,s);} while(0)
#define op4s(r,e,a,p,s)\
    do{op((r)[0],e,0,+,(a)[0],p,s), op((r)[1],e,0,+,(a)[1],p,s),\
    op((r)[2],e,0,+,(a)[2],p,s), op((r)[3],e,0,+,(a)[3],p,s);} while(0)

#define set4(v,x,y,z,w) (v)[0]=(x),(v)[1]=(y),(v)[2]=(z),(v)[3] =(w)
#define set4w(d,s,w)    (d)[0]=(s)[0],(d)[1]=(s)[1],(d)[2]=(s)[2],(d)[3]=w
#define cpy4(d,s)       (d)[0]=(s)[0],(d)[1]=(s)[1],(d)[2]=(s)[2],(d)[3]=(s)[3]
#define add4(d,a,b)     op4(d,=,a,+,b,+,0)
#define sub4(d,a,b)     op4(d,=,a,-,b,+,0)
#define mul4(d,a,s)     op4s(d,=,a,*,s)
#define dot4(a,b)       ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2]+(a)[3]*(b)[3])
#define len4(v)         sqrt(dot4(v,v))
#define adds4(d,a,b,s)  op4(d,=,a,+,b,*,s)
#define subs4(d,a,b,s)  op4(d,=,a,-,b,*,s)
#define norm4(n,v)      mul4(n,v,isqrt(dot4(v, v)))
#define normaleq4(v)    norm4(v,v)
#define lerp4(r,a,b,t)  lerp(r[0],a[0],b[0],t), lerp(r[1],a[1],b[1],t), lerp(r[2],a[2],b[2],t), lerp(r[3],a[3],b[3],t)
#define qid(q)          set4(q,0,0,0,1)
#define qconj(d,s)      do{mul3(d,s,-1.0f);(d)[3]=(s)[3];}while(0);
#define quatf(q,angle,x,y,z)\
    q[0] = (x) * sinf(angle * 0.5f),\
    q[1] = (y) * sinf(angle * 0.5f),\
    q[2] = (z) * sinf(angle * 0.5f),\
    q[3] = cosf(angle * 0.5f)
#define quat(q,angle,axis) quatf(q,axis[0],axis[1],axis[2],angle)
#define qmul(q,a,b)\
    (q)[0] = (a)[3]*(b)[0] + (a)[0]*(b)[3] + (a)[1]*b[2] - (a)[2]*(b)[1],\
    (q)[1] = (a)[3]*(b)[1] + (a)[1]*(b)[3] + (a)[2]*b[0] - (a)[0]*(b)[2],\
    (q)[2] = (a)[3]*(b)[2] + (a)[2]*(b)[3] + (a)[0]*b[1] - (a)[1]*(b)[0],\
    (q)[3] = (a)[3]*(b)[3] - (a)[0]*(b)[0] - (a)[1]*b[1] - (a)[2]*(b)[2];

static float
isqrt(float n)
{
    union {unsigned u; float f;} conv; conv.f = n;
    conv.u = 0x5f375A84 - (conv.u >> 1);
    conv.f = conv.f * (1.5f - (n * 0.5f * conv.f * conv.f));
    return conv.f;
}
static void
qrot3(float *out, const float *q, const float *v)
{
    float a[3]; mul3(a, q, 2.0f * dot3(q,v));
    float b[3]; mul3(b, v, q[3]*q[3] - dot3(q,q));
    float c[3]; cross3(c,q,v);
    float d[3]; mul3(d,c, 2.0f * q[3]);
    float e[3]; add3(e, a, b);
    add3(out, e, d);
}
static void
qrotv3(float *q, const float *u, const float *v)
{
    float w[3] = {0};
    float norm_u_norm_v = sqrtf(dot3(u,u) * dot3(v,v));
    float real_part = norm_u_norm_v + dot3(u,v);
    if (real_part < (1.e-6f * norm_u_norm_v)) {
        real_part = 0;
        if (fabs(u[0]) > fabs(u[2]))
            set3(w, -u[1], u[0], 0.0f);
        else set3(w, 0.0f, -u[2], u[1]);
    } else cross3(w, u,v);
    set4(q, w[0],w[1],w[2], real_part);
    normaleq4(q);
}
static void
qeuler(float *qout, float yaw, float pitch, float roll)
{
    float sr, cr; sincosf(roll * 0.5f, &sr, &cr);
    float sp, cp; sincosf(pitch * 0.5f, &sp, &cp);
    float sy, cy; sincosf(yaw * 0.5f, &sy, &cy);

    qout[0] = cy * sp * cr + sy * cp * sr;
    qout[1] = sy * cp * cr - cy * sp * sr;
    qout[2] = cy * cp * sr - sy * sp * cr;
    qout[3] = cy * cp * cr + sy * sp * sr;
}
static void
qangles(float *out, const float *qin)
{
    float q[4]; cpy4(q, qin);
    float x2 = q[0] * q[0];
    float y2 = q[1] * q[1];
    float z2 = q[2] * q[2];
    float w2 = q[3] * q[3];
    float mag = x2 + y2 + z2 + w2;
    float test = q[0] * q[1] + q[2] * q[3];
    float rad = 3.141592654.0f / 2.0f;

    if (test > rad * mag) {
        out[0] = 0.0f;
        out[1] = 2.0f * atan2(q[0], q[3]);
        out[2] = rad;
        return;
    }
    if (test < -rad * mag) {
        out[0] = 0.0f;
        out[1] = -2.0f * atan2(q[0], q[3]);
        out[2] = -rad;
        return;
    }
    out[0] = atan2(2.0f * q[0] * q[3] - 2.0f * q[1] * q[2], -x2 + y2 - z2 + w2);
    out[1] = atan2(2.0f * q[1] * q[3] - 2.0f * q[0] * q[2],  x2 - y2 - z2 + w2);
    out[2] = atan2(2.0f * test / mag); 
}

static void
transformq(float *o3, const float *v3, const float *q, const float *t3)
{
    float tmp[3]; qrot3(tmp, q, v3);
    add3(o3, tmp, t3);
}
static void
transformqI(float *o3, const float *v3, const float *q, const float *t3)
{
    float tmp[3]; sub3(tmp, v3, t3);
    float inv[4]; qconj(inv, q);
    qrot3(o3, inv, tmp);
}
static void
rotationf(float *rot, float angle, float x, float y, float z)
{
    const float c = cosf(angle);
    const float s = sinf(angle);
    float axis[3]; set3(axis,x,y,z); normeq3(axis);
    float tmp[3]; mul3(tmp, axis, 1.0f - c);

    rot[0*3+0] = c + tmp[0] * axis[0];
    rot[0*3+1] = 0 + tmp[0] * axis[1] + s * axis[2];
    rot[0*3+2] = 0 + tmp[0] * axis[2] - s * axis[1];

    rot[1*3+0] = 0 + tmp[1] * axis[0] - s * axis[2];
    rot[1*3+1] = c + tmp[1] * axis[1];
    rot[1*3+2] = 0 + tmp[1] * axis[2] + s * axis[0];

    rot[2*3+0] = 0 + tmp[2] * axis[0] + s * axis[1];
    rot[2*3+1] = 0 + tmp[2] * axis[1] - s * axis[0];
    rot[2*3+2] = c + tmp[2] * axis[2];
}
static void
transform(float *r3, const float *v3, const float *r33, const float *t3)
{
    r3[0] = v3[0] * r33[0*3+0] + v3[1] * r33[1*3+0] + v3[2] * r33[2*3+0] + t3[0];
    r3[1] = v3[0] * r33[0*3+1] + v3[1] * r33[1*3+1] + v3[2] * r33[2*3+1] + t3[1];
    r3[2] = v3[0] * r33[0*3+2] + v3[1] * r33[1*3+2] + v3[2] * r33[2*3+2] + t3[2];
}
static void
transformI(float *r3, const float *v3, const float *r33, const float *t3)
{
    const float p[3] = {v3[0] - t3[0], v3[1] - t3[1], v3[2] - t3[2] };
    r3[0] = p[0] * r33[0*3+0] + p[1] * r33[0*3+1] + p[2] * r33[0*3+2];
    r3[1] = p[0] * r33[1*3+0] + p[1] * r33[1*3+1] + p[2] * r33[1*3+2];
    r3[2] = p[0] * r33[2*3+0] + p[1] * r33[2*3+1] + p[2] * r33[2*3+2];
}
int main(void)
{
    float p[3] = {1,0,0};
    float t[3] = {3,4,2};
    
#if 1
    float q[4] = {0,0,0,1};
    quatf(q, rad(-90.0f), 0,1,0);

    float w[3] = {0,0,0};
    transformq(w, p, q, t);
    
    float m[3] = {0,0,0};
    transformqI(m, w, q, t);
    
    printf("world: %.6f %.6f %.6f\n", w[0], w[1], w[2]);
    printf("model: %.6f %.6f %.6f\n", m[0], m[1], m[2]);
#endif
    
#if 0
    float r[9] = {0};
    rotationf(r, rad(-90.0f), 0,1,0);
   
    float w[3] = {0,0,0};
    transform(w, p, r, t);
    
    float m[3] = {0,0,0};
    transformI(m, w, r, t);
 
    printf("world: %.6f %.6f %.6f\n", w[0], w[1], w[2]);
    printf("model: %.6f %.6f %.6f\n", m[0], m[1], m[2]);
#endif
    return 0;
}