src/gl/tinyrenderer/geometry.h

#include <math.h>
#include <assert.h>

typedef double real;

typedef struct { real x, y; } vec2;

static inline vec2 vec2_sub (vec2 v1, vec2 v2)
{
  return (vec2){v1.x - v2.x, v1.y - v2.y};
}

static inline real vec2_cross (vec2 v1, vec2 v2)
{
  return v1.x*v2.y - v1.y*v2.x;
}

static inline real vec2_norm2 (vec2 v)
{
  return v.x*v.x + v.y*v.y;
}

static inline real vec2_norm  (vec2 v)
{
  return sqrt (vec2_norm2 (v));
}

static inline vec2 vec2_normalized (vec2 v)
{
  real norm = vec2_norm (v);
  if (norm)
    return (vec2){v.x/norm, v.y/norm};
  return v;
}

typedef struct { real x, y, z; } vec3;

static inline real vec3_norm2 (vec3 v)
{
  return v.x*v.x + v.y*v.y + v.z*v.z;
}

static inline real vec3_norm  (vec3 v)
{
  return sqrt (vec3_norm2 (v));
}

static inline vec3 vec3_normalized (vec3 v)
{
  real norm = vec3_norm (v);
  if (norm)
    return (vec3){v.x/norm, v.y/norm, v.z/norm};
  return v;
}

static inline vec3 vec3_embed (vec2 v)
{
  return (vec3) { v.x, v.y, 1. };
}

static inline vec3 vec3_sub (vec3 v1, vec3 v2)
{
  return (vec3){v1.x - v2.x, v1.y - v2.y, v1.z - v2.z};
}

static inline vec3 vec3_div (vec3 v, real a)
{
  assert (a);
  return (vec3){v.x/a, v.y/a, v.z/a};
}

static inline vec3 vec3_mul (vec3 v, real a)
{
  return (vec3){v.x*a, v.y*a, v.z*a};
}

static inline real vec3_scalar (vec3 v1, vec3 v2)
{
  return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}

typedef struct { real x, y, z, t; } vec4;

static inline vec4 vec4_div (vec4 v, real a)
{
  assert (a);
  return (vec4){v.x/a, v.y/a, v.z/a, v.t/a};  
}

static inline vec2 vec4_proj2 (vec4 v)
{
  return (vec2) { v.x, v.y };
}

static inline vec3 vec4_proj3 (vec4 v)
{
  return (vec3) { v.x, v.y, v.z };
}

static inline vec4 vec4_embed (vec3 v, real fill)
{
  return (vec4) { v.x, v.y, v.z, fill };
}

typedef struct { vec3 x, y, z; } mat3;

static inline mat3 mat3_transpose (mat3 m)
{
  return (mat3) {
    { m.x.x, m.y.x, m.z.x },
    { m.x.y, m.y.y, m.z.y },
    { m.x.z, m.y.z, m.z.z }
  };
}

static inline real mat3_det (mat3 m)
{
  return (m.x.x*(m.y.y*m.z.z - m.z.y*m.y.z) - 
	  m.x.y*(m.y.x*m.z.z - m.z.x*m.y.z) + 
	  m.x.z*(m.y.x*m.z.y - m.z.x*m.y.y));
}

static inline mat3 mat3_invert (mat3 m)
{
  real det = mat3_det (m);
  assert (det);

  mat3 mi;
  mi.x.x = (m.y.y*m.z.z - m.y.z*m.z.y)/det; 
  mi.x.y = (m.z.y*m.x.z - m.x.y*m.z.z)/det;
  mi.x.z = (m.x.y*m.y.z - m.y.y*m.x.z)/det; 
  mi.y.x = (m.y.z*m.z.x - m.y.x*m.z.z)/det; 
  mi.y.y = (m.x.x*m.z.z - m.z.x*m.x.z)/det; 
  mi.y.z = (m.y.x*m.x.z - m.x.x*m.y.z)/det; 
  mi.z.x = (m.y.x*m.z.y - m.z.x*m.y.y)/det; 
  mi.z.y = (m.z.x*m.x.y - m.x.x*m.z.y)/det; 
  mi.z.z = (m.x.x*m.y.y - m.x.y*m.y.x)/det; 
  return mi;
}

static inline mat3 mat3_invert_transpose (const mat3 m)
{
  return mat3_transpose (mat3_invert (m));
}

static inline vec3 mat3_mul (mat3 m, vec3 v)
{
  return (vec3) {
    m.x.x*v.x + m.x.y*v.y + m.x.z*v.z,
    m.y.x*v.x + m.y.y*v.y + m.y.z*v.z,
    m.z.x*v.x + m.z.y*v.y + m.z.z*v.z
  };
}

static inline vec3 mat3_col (mat3 m, int idx)
{
  assert (idx >= 0 && idx < 3);
  return (vec3) { *(&m.x.x + idx), *(&m.y.x + idx), *(&m.z.x + idx) };
}

static inline int set_col3 (mat3 * m, int idx, vec3 v)
{
  assert (idx >= 0 && idx < 3);
  *(&m->x.x + idx) = v.x;
  *(&m->y.x + idx) = v.y;
  *(&m->z.x + idx) = v.z;
  return 0; // fixme
}

typedef struct { vec4 x, y, z, t; } mat4;

static inline vec4 mat4_mul (mat4 m, vec4 v)
{
  return (vec4) {
    m.x.x*v.x + m.x.y*v.y + m.x.z*v.z + m.x.t*v.t,
    m.y.x*v.x + m.y.y*v.y + m.y.z*v.z + m.y.t*v.t,
    m.z.x*v.x + m.z.y*v.y + m.z.z*v.z + m.z.t*v.t,
    m.t.x*v.x + m.t.y*v.y + m.t.z*v.z + m.t.t*v.t
  };
}

static inline mat4 mat4_mul4 (mat4 m1, mat4 m2)
{
  mat4 ret;
  const real * p1 = &m1.x.x, * p2 = &m2.x.x;
  real * p = &ret.x.x;
  for (int i = 0; i < 4; i++, p1 += 4, p += 4)
    for (int j = 0; j < 4; j++) {
      real a = 0.;
      for (int k = 0; k < 4; k++)
	a += p1[k]*(p2 + 4*k)[j];
      p[j] = a;
    }
  return ret;
}

static inline mat4 mat4_invert (mat4 M)
{
  mat4 I;
  real * m = &M.x.x, * i = &I.x.x;

  i[0] = m[5]*m[10]*m[15] - m[5]*m[11]*m[14] - m[9]*m[6]*m[15] + m[9]*m[7]*m[14] + m[13]*m[6]*m[11] - m[13]*m[7]*m[10];
  i[4] = -m[4]*m[10]*m[15] + m[4]*m[11]*m[14] + m[8]*m[6]*m[15] - m[8]*m[7]*m[14] - m[12]*m[6]*m[11] + m[12]*m[7]*m[10];
  i[8] = m[4]*m[9]*m[15] - m[4]*m[11]*m[13] - m[8]*m[5]*m[15] + m[8]*m[7]*m[13] + m[12]*m[5]*m[11] - m[12]*m[7]*m[9];
  i[12] = -m[4]*m[9]*m[14] + m[4]*m[10]*m[13] +m[8]*m[5]*m[14] - m[8]*m[6]*m[13] - m[12]*m[5]*m[10] + m[12]*m[6]*m[9];
  i[1] = -m[1]*m[10]*m[15] + m[1]*m[11]*m[14] + m[9]*m[2]*m[15] - m[9]*m[3]*m[14] - m[13]*m[2]*m[11] + m[13]*m[3]*m[10];
  i[5] = m[0]*m[10]*m[15] - m[0]*m[11]*m[14] - m[8]*m[2]*m[15] + m[8]*m[3]*m[14] + m[12]*m[2]*m[11] - m[12]*m[3]*m[10];
  i[9] = -m[0]*m[9]*m[15] + m[0]*m[11]*m[13] + m[8]*m[1]*m[15] - m[8]*m[3]*m[13] - m[12]*m[1]*m[11] + m[12]*m[3]*m[9];
  i[13] = m[0]*m[9]*m[14] - m[0]*m[10]*m[13] - m[8]*m[1]*m[14] + m[8]*m[2]*m[13] + m[12]*m[1]*m[10] - m[12]*m[2]*m[9];
  i[2] = m[1]*m[6]*m[15] - m[1]*m[7]*m[14] - m[5]*m[2]*m[15] + m[5]*m[3]*m[14] + m[13]*m[2]*m[7] - m[13]*m[3]*m[6];
  i[6] = -m[0]*m[6]*m[15] + m[0]*m[7]*m[14] + m[4]*m[2]*m[15] - m[4]*m[3]*m[14] - m[12]*m[2]*m[7] + m[12]*m[3]*m[6];
  i[10] = m[0]*m[5]*m[15] - m[0]*m[7]*m[13] - m[4]*m[1]*m[15] + m[4]*m[3]*m[13] + m[12]*m[1]*m[7] - m[12]*m[3]*m[5];
  i[14] = -m[0]*m[5]*m[14] + m[0]*m[6]*m[13] + m[4]*m[1]*m[14] - m[4]*m[2]*m[13] - m[12]*m[1]*m[6] + m[12]*m[2]*m[5];
  i[3] = -m[1]*m[6]*m[11] + m[1]*m[7]*m[10] + m[5]*m[2]*m[11] - m[5]*m[3]*m[10] - m[9]*m[2]*m[7] + m[9]*m[3]*m[6];
  i[7] = m[0]*m[6]*m[11] - m[0]*m[7]*m[10] - m[4]*m[2]*m[11] + m[4]*m[3]*m[10] + m[8]*m[2]*m[7] - m[8]*m[3]*m[6];
  i[11] = -m[0]*m[5]*m[11] + m[0]*m[7]*m[9] + m[4]*m[1]*m[11] - m[4]*m[3]*m[9] - m[8]*m[1]*m[7] + m[8]*m[3]*m[5];
  i[15] = m[0]*m[5]*m[10] - m[0]*m[6]*m[9] - m[4]*m[1]*m[10] + m[4]*m[2]*m[9] + m[8]*m[1]*m[6] - m[8]*m[2]*m[5];

  real det = m[0]*i[0] + m[1]*i[4] + m[2]*i[8] + m[3]*i[12];
  if (det == 0)
    return M;

  for (int j = 0; j < 16; j++)
    i[j] /= det;

  return I;
}

static inline mat4 mat4_transpose (mat4 m)
{
  return (mat4) {
    { m.x.x, m.y.x, m.z.x, m.t.x },
    { m.x.y, m.y.y, m.z.y, m.t.y },
    { m.x.z, m.y.z, m.z.z, m.t.z },
    { m.x.t, m.y.t, m.z.t, m.t.t }
  };
}

static inline mat4 mat4_invert_transpose (const mat4 m)
{
  return mat4_transpose (mat4_invert (m));
}

typedef struct { vec3 x, y; } mat23;

static inline int set_col23 (mat23 * m, int idx, vec2 v)
{
  assert (idx >= 0 && idx < 3);
  *(&m->x.x + idx) = v.x;
  *(&m->y.x + idx) = v.y;
  return 0; // fixme  
}

static inline vec2 mat23_mul (mat23 m, vec3 v)
{
  return (vec2) {
    m.x.x*v.x + m.x.y*v.y + m.x.z*v.z,
    m.y.x*v.x + m.y.y*v.y + m.y.z*v.z
  };
}

static inline vec3 cross(vec3 v1, vec3 v2)
{
  return (vec3){
    v1.y*v2.z - v1.z*v2.y,
    v1.z*v2.x - v1.x*v2.z,
    v1.x*v2.y - v1.y*v2.x
  };
}