typedef struct {
  union {
    struct { double x, y, z; };
    struct { double a[3]; };
  };
} Vector3;

typedef struct {
  union {
    struct { Vector3 xyz; };
    struct { double x, y, z, w; };
    struct { double a[4]; };
  };
} Vector4;

typedef struct {
  union {
    struct { double m[4][4]; };
    struct { double m00, m01, m02, m03,
                    m10, m11, m12, m13,
                    m20, m21, m22, m23,
                    m30, m31, m32, m33; };
    struct { Vector4 ox, oy, oz, ow; };
    struct { double a[16]; };
  };
} Matrix;


void vector3Set(Vector3 *vector, double x, double y, double z)
  { vector->x = x; vector->y = y; vector->z = z; }

void vector3Copy(Vector3 *vector, const Vector3 *a)
  { vector3Set(vector, a->x, a->y, a->z); }

void vector3Zero(Vector3 *vector)
  { vector3Set(vector, 0, 0, 0); }

double vector3Dot(Vector3 *a, Vector3 *b)
  { return a->x*b->x + a->y*b->y + a->z*b->z; }


void vector4Set(Vector4 *vector, double x, double y, double z, double w)
  { vector->x = x; vector->y = y; vector->z = z; vector->w = w; }

void vector4Copy(Vector4 *vector, const Vector4 *a)
  { vector4Set(vector, a->x, a->y, a->z, a->w); }

void vector4Zero(Vector4 *vector)
  { vector4Set(vector, 0, 0, 0, 0); }

void vector4PespectiveDivide(Vector4 *vector, const Vector4 *a)
  { double k = 1/a->w; vector4Set(vector, a->x*k, a->y*k, a->z*k, 1); }

void vector4MultMatrix(Vector4 *vector, const Vector4 *a, const Matrix *b) {
  vector4Set(vector, b->ox.x*a->x + b->oy.x*a->y + b->oz.x*a->z + b->ow.x*a->w,
                     b->ox.y*a->x + b->oy.y*a->y + b->oz.y*a->z + b->ow.y*a->w,
                     b->ox.z*a->x + b->oy.z*a->y + b->oz.z*a->z + b->ow.z*a->w,
                     b->ox.w*a->x + b->oy.w*a->y + b->oz.w*a->z + b->ow.w*a->w );
}


void matrixCopy(Matrix *matrix, const Matrix *a) {
  vector4Copy(&matrix->ox, &a->ox);
  vector4Copy(&matrix->oy, &a->oy);
  vector4Copy(&matrix->oz, &a->oz);
  vector4Copy(&matrix->ow, &a->ow);
}

void matrixMultMatrix(Matrix *matrix, const Matrix *a, const Matrix *b) {
  if (matrix == a || matrix == b) {
    Matrix m;
    matrixMultMatrix(&m, a, b);
    matrixCopy(matrix, &m);
    return;
  }
  vector4MultMatrix(&matrix->ox, &a->ox, b);
  vector4MultMatrix(&matrix->oy, &a->oy, b);
  vector4MultMatrix(&matrix->oz, &a->oz, b);
  vector4MultMatrix(&matrix->ow, &a->ow, b);
}

void matrixZero(Matrix *matrix) {
  vector4Zero( &matrix->ox );
  vector4Zero( &matrix->oy );
  vector4Zero( &matrix->oz );
  vector4Zero( &matrix->ow );
}

void matrixSetIdentity(Matrix *matrix) {
  vector4Set( &matrix->ox, 1, 0, 0, 0 );
  vector4Set( &matrix->oy, 0, 1, 0, 0 );
  vector4Set( &matrix->oz, 0, 0, 1, 0 );
  vector4Set( &matrix->ow, 0, 0, 0, 1 );
}

void matrixSetRotation(Matrix *matrix, double angle, double x, double y, double z) {
  double s = sin(angle/180.0*PI);
  double c = cos(angle/180.0*PI);
  double cc = 1 - c, cx = x*cc, cy = y*cc, cz=z*cc;
  vector4Set( &matrix->ox, x*cx + c  , y*cx + z*s, z*cx - y*s, 0 );
  vector4Set( &matrix->oy, x*cy - z*s, y*cy + c  , z*cy + x*s, 0 );
  vector4Set( &matrix->oz, x*cz + y*s, y*cz - x*s, z*cz + c  , 0 );
  vector4Set( &matrix->ow, 0         , 0         , 0         , 1 );
}

void matrixSetPerspective(Matrix *matrix, double fovy, double aspect, double zNear, double zFar) {
  double f = 1/tan(0.5*fovy/180.0*PI);
  double d = 1/(zNear - zFar);
  vector4Set( &matrix->ox, f/aspect, 0, 0               ,  0 );
  vector4Set( &matrix->oy, 0       , f, 0               ,  0 );
  vector4Set( &matrix->oz, 0       , 0, (zNear + zFar)*d, -1 );
  vector4Set( &matrix->ow, 0       , 0, 2*zNear*zFar*d  ,  0 );
}

void matrixInvert(Matrix *matrix, const Matrix *a) {
  if (matrix == a) {
    Matrix m;
    matrixInvert(&m, a);
    matrixCopy(matrix, &m);
    return;
  }

  matrix->m00 = a->m11*(a->m22*a->m33 - a->m23*a->m32) + a->m12*(a->m23*a->m31 - a->m21*a->m33) + a->m13*(a->m21*a->m32 - a->m22*a->m31);
  matrix->m10 = a->m10*(a->m23*a->m32 - a->m22*a->m33) + a->m12*(a->m20*a->m33 - a->m23*a->m30) + a->m13*(a->m22*a->m30 - a->m20*a->m32);
  matrix->m20 = a->m10*(a->m21*a->m33 - a->m23*a->m31) + a->m11*(a->m23*a->m30 - a->m20*a->m33) + a->m13*(a->m20*a->m31 - a->m21*a->m30);
  matrix->m30 = a->m10*(a->m22*a->m31 - a->m21*a->m32) + a->m11*(a->m20*a->m32 - a->m22*a->m30) + a->m12*(a->m21*a->m30 - a->m20*a->m31);

  double det = a->m00*matrix->m00 + a->m01*matrix->m10 + a->m02*matrix->m20 + a->m03*matrix->m30;
  if (fabs(det) <= 1e-10)
    { matrixZero(matrix); return; }
  det = 1/det;
  matrix->m00 *= det;
  matrix->m10 *= det;
  matrix->m20 *= det;
  matrix->m30 *= det;

  matrix->m01 = det*(a->m01*(a->m23*a->m32 - a->m22*a->m33) + a->m02*(a->m21*a->m33 - a->m23*a->m31) + a->m03*(a->m22*a->m31 - a->m21*a->m32));
  matrix->m11 = det*(a->m00*(a->m22*a->m33 - a->m23*a->m32) + a->m02*(a->m23*a->m30 - a->m20*a->m33) + a->m03*(a->m20*a->m32 - a->m22*a->m30));
  matrix->m21 = det*(a->m00*(a->m23*a->m31 - a->m21*a->m33) + a->m01*(a->m20*a->m33 - a->m23*a->m30) + a->m03*(a->m21*a->m30 - a->m20*a->m31));
  matrix->m31 = det*(a->m00*(a->m21*a->m32 - a->m22*a->m31) + a->m01*(a->m22*a->m30 - a->m20*a->m32) + a->m02*(a->m20*a->m31 - a->m21*a->m30));
  matrix->m02 = det*(a->m01*(a->m12*a->m33 - a->m13*a->m32) + a->m02*(a->m13*a->m31 - a->m11*a->m33) + a->m03*(a->m11*a->m32 - a->m12*a->m31));
  matrix->m12 = det*(a->m00*(a->m13*a->m32 - a->m12*a->m33) + a->m02*(a->m10*a->m33 - a->m13*a->m30) + a->m03*(a->m12*a->m30 - a->m10*a->m32));
  matrix->m22 = det*(a->m00*(a->m11*a->m33 - a->m13*a->m31) + a->m01*(a->m13*a->m30 - a->m10*a->m33) + a->m03*(a->m10*a->m31 - a->m11*a->m30));
  matrix->m32 = det*(a->m00*(a->m12*a->m31 - a->m11*a->m32) + a->m01*(a->m10*a->m32 - a->m12*a->m30) + a->m02*(a->m11*a->m30 - a->m10*a->m31));
  matrix->m03 = det*(a->m01*(a->m13*a->m22 - a->m12*a->m23) + a->m02*(a->m11*a->m23 - a->m13*a->m21) + a->m03*(a->m12*a->m21 - a->m11*a->m22));
  matrix->m13 = det*(a->m00*(a->m12*a->m23 - a->m13*a->m22) + a->m02*(a->m13*a->m20 - a->m10*a->m23) + a->m03*(a->m10*a->m22 - a->m12*a->m20));
  matrix->m23 = det*(a->m00*(a->m13*a->m21 - a->m11*a->m23) + a->m01*(a->m10*a->m23 - a->m13*a->m20) + a->m03*(a->m11*a->m20 - a->m10*a->m21));
  matrix->m33 = det*(a->m00*(a->m11*a->m22 - a->m12*a->m21) + a->m01*(a->m12*a->m20 - a->m10*a->m22) + a->m02*(a->m10*a->m21 - a->m11*a->m20));
}