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));

}