toys/benchtest/bench/vmath.c

#include "vmath.h"

/* Vector Operations */
/* Vector2f */
void Vector2f_AddInPlace(Vector2f *o, Vector2f other) {
  o->x += other.x;
  o->y += other.y;
}

Vector2f Vector2f_Add(Vector2f o, Vector2f other) {
  Vector2f r;
  r.x = o.x + other.x;
  r.y = o.y + other.y;
  return r;
}

void Vector2f_SubInPlace(Vector2f *o, Vector2f s) {
  o->x = o->x - s.x;
  o->y = o->y - s.y;
}

Vector2f Vector2f_Sub(Vector2f o, Vector2f s) {
  Vector2f r;
  r.x = o.x - s.x;
  r.y = o.y - s.y;
  return r;
}

void Vector2f_MulInPlace(Vector2f *o, float s) {
  o->x *= s;
  o->y *= s;
}

Vector2f Vector2f_Mul(Vector2f o, float s) {
  Vector2f r;
  r.x = o.x * s;
  r.y = o.y * s;
  return r;
}

void Vector2f_DivInPlace(Vector2f *o, float s) {
  Vector2f_MulInPlace(o, 1.f / s);
}

Vector2f Vector2f_Div(Vector2f o, float s) {
  return Vector2f_Mul(o, 1.f / s);
}

float Vector2f_Dot(Vector2f u, Vector2f v) {
  return u.x * v.x + u.y * v.y;
}

float Vector2f_Cross(Vector2f u, Vector2f v) {
  return u.x * v.y - u.y * v.x;
}

float Vector2f_Length(Vector2f v) {
  return (float)sqrt(v.x * v.x + v.y * v.y);
}

Vector2f Vector2f_Reflect(Vector2f o, Vector2f normal) {
  float idotn2 = (normal.x * o.x + normal.y * o.y) * -2.f;
  return (Vector2f){o.x + idotn2 * normal.x, o.y + idotn2 * normal.y};
}

void Vector2f_NormalizeInPlace(Vector2f *o) {
  float length = Vector2f_Length(*o);
  if (length > 0.f) {
    float inv = 1.f / length;
    o->x *= inv;
    o->y *= inv;
  }
}

Vector2f Vector2f_Normalize(Vector2f o) {
  float length = Vector2f_Length(o);
  if (length > 0) {
    float inv = 1.f / length;
    return (Vector2f){o.x * inv, o.y * inv};
  }
  return (Vector2f){0.f, 0.f};
}

Vector2f Vector2f_Lerp(Vector2f a, Vector2f b, float t) {
  return (Vector2f){Lerpf(a.x, b.x, t), Lerpf(a.y, b.y, t)};
}

Vector2f Vector2f_Saturate(Vector2f o) {
  return (Vector2f){Saturatef(o.x), Saturatef(o.y)};
}

/* Vector3f */
void Vector3f_AddInPlace(Vector3f *o, Vector3f other) {
  o->x += other.x;
  o->y += other.y;
  o->z += other.z;
}

Vector3f Vector3f_Add(Vector3f o, Vector3f other) {
  Vector3f r;
  r.x = o.x + other.x;
  r.y = o.y + other.y;
  r.z = o.z + other.z;
  return r;
}

void Vector3f_SubInPlace(Vector3f *o, Vector3f s) {
  o->x = o->x - s.x;
  o->y = o->y - s.y;
  o->z = o->z - s.z;
}

Vector3f Vector3f_Sub(Vector3f o, Vector3f s) {
  Vector3f r;
  r.x = o.x - s.x;
  r.y = o.y - s.y;
  r.z = o.z - s.z;
  return r;
}

void Vector3f_MulInPlace(Vector3f *o, float s) {
  o->x *= s;
  o->y *= s;
  o->z *= s;
}

Vector3f Vector3f_Mul(Vector3f o, float s) {
  Vector3f r;
  r.x = o.x * s;
  r.y = o.y * s;
  r.z = o.z * s;
  return r;
}

void Vector3f_DivInPlace(Vector3f *o, float s) {
  Vector3f_MulInPlace(o, 1.f / s);
}

Vector3f Vector3f_Div(Vector3f o, float s) {
  return Vector3f_Mul(o, 1.f / s);
}

float Vector3f_Dot(Vector3f u, Vector3f v) {
  return u.x * v.x + u.y * v.y + u.z * v.z;
}

Vector3f Vector3f_Cross(Vector3f u, Vector3f v) {
  return (Vector3f){u.y * v.z - u.z * v.y, u.z * v.x - u.x * v.z,
                    u.x * v.y - u.y * v.x};
}

float Vector3f_Length(Vector3f v) {
  return (float)sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
}

void Vector3f_NormalizeInPlace(Vector3f *o) {
  float length = Vector3f_Length(*o);
  if (length > 0.f) {
    float inv = 1.f / length;
    o->x *= inv;
    o->y *= inv;
    o->z *= inv;
  }
}

Vector3f Vector3f_Normalize(Vector3f o) {
  float length = Vector3f_Length(o);
  if (length > 0.f) {
    float inv = 1.f / length;
    return (Vector3f){o.x * inv, o.y * inv, o.z * inv};
  }
  return (Vector3f){0.f, 0.f, 0.f};
}

Vector3f Vector3f_Lerp(Vector3f a, Vector3f b, float t) {
  return (Vector3f){Lerpf(a.x, b.x, t), Lerpf(a.y, b.y, t), Lerpf(a.z, b.z, t)};
}

Vector3f Vector3f_Saturate(Vector3f o) {
  return (Vector3f){Saturatef(o.x), Saturatef(o.y), Saturatef(o.z)};
}

/* Ray */
Point3f Rayf_At(Rayf *o, float t) {
  return Vector3f_Add(o->origin, Vector3f_Mul(o->direction, t));
}

/* Matrix Operations */
/* Matrix 3x3 */
double Matrix3x3f_Det(const Matrix3x3f *o) {
  float ret = 0;
  float **m = (float **)o->m;

  ret += m[0][0] * m[1][1] * m[2][2] + m[1][0] * m[2][1] * m[0][2] +
         m[2][0] * m[0][1] * m[1][2];
  ret -= m[2][0] * m[1][1] * m[0][2] + m[0][0] * m[2][1] * m[1][2] +
         m[1][0] * m[0][1] * m[2][2];

  return ret;
}

void Matrix3x3f_ScaleInPlace(Matrix3x3f *o, float s) {
  o->m11 *= s;
  o->m12 *= s;
  o->m13 *= s;

  o->m21 *= s;
  o->m22 *= s;
  o->m23 *= s;

  o->m31 *= s;
  o->m32 *= s;
  o->m33 *= s;
}

Matrix3x3f Matrix3x3f_Scale(const Matrix3x3f *o, float s) {
  Matrix3x3f r;
  r.m11 = o->m11 * s;
  r.m12 = o->m12 * s;
  r.m13 = o->m13 * s;

  r.m21 = o->m21 * s;
  r.m22 = o->m22 * s;
  r.m23 = o->m23 * s;

  r.m31 = o->m31 * s;
  r.m32 = o->m32 * s;
  r.m33 = o->m33 * s;

  return r;
}

void Matrix3x3f_Invert(const Matrix3x3f *o, Matrix3x3f *dest) {
  float det;
  float **m = (float **)o->m;
  float a = m[0][0], b = m[0][1], c = m[0][2], d = m[1][0], e = m[1][1],
        f = m[1][2], g = m[2][0], h = m[2][1], i = m[2][2];

  float **dm = (float **)dest->m;

  dm[0][0] = e * i - f * h;
  dm[0][1] = -(b * i - h * c);
  dm[0][2] = b * f - e * c;
  dm[1][0] = -(d * i - g * f);
  dm[1][1] = a * i - c * g;
  dm[1][2] = -(a * f - d * c);
  dm[2][0] = d * h - g * e;
  dm[2][1] = -(a * h - g * b);
  dm[2][2] = a * e - b * d;

  det = 1.0f / (a * dm[0][0] + b * dm[1][0] + c * dm[2][0]);

  Matrix3x3f_Scale(dest, det);
}

void Matrix3x3f_TransposeInPlace(Matrix3x3f *o) {
#define _SWAP(a, b) (((a) ^ (b)) && ((b) ^= (a) ^= (b), (a) ^= (b)))
  union {
    float f;
    int i;
  } x, y;

  x.f = o->m12, y.f = o->m21;
  _SWAP(x.i, y.i);
  x.f = o->m13, y.f = o->m31;
  _SWAP(x.i, y.i);
  x.f = o->m21, y.f = o->m12;
  _SWAP(x.i, y.i);
  x.f = o->m23, y.f = o->m32;
  _SWAP(x.i, y.i);
  x.f = o->m31, y.f = o->m13;
  _SWAP(x.i, y.i);
  x.f = o->m32, y.f = o->m23;
  _SWAP(x.i, y.i);
#undef _SWAP
}

Vector3f Matrix3x3f_MulVector3f(const Matrix3x3f *o, Vector3f v) {
  Vector3f dest;

  dest.x = o->m11 * v.x + o->m21 * v.y + o->m31 * v.z;
  dest.y = o->m12 * v.x + o->m22 * v.y + o->m32 * v.z;
  dest.x = o->m13 * v.x + o->m23 * v.y + o->m33 * v.z;

  return dest;
}

Matrix3x3f Matrix3x3f_InvertTranspose(const Matrix3x3f *o) {
  Matrix3x3f dest;
  Matrix3x3f_Invert(o, &dest);
  Matrix3x3f_TransposeInPlace(&dest);
  return dest;
}

Vector3f Vector3f_Barycentric(const Vector2i tri[3], Vector2i p) {
  Matrix3x3f a = {(float)tri[0].x, (float)tri[0].y, 1.0f,
                  (float)tri[1].x, (float)tri[1].y, 1.0f,
                  (float)tri[2].x, (float)tri[2].y, 1.0f};
  if (Matrix3x3f_Det(&a) < 1e-3) {
    return (Vector3f){
        -1, 1, 1}; // for a degenerate triangle generate negative coordinates,
                   // it will be thrown away by the rasterizator
  }
  Matrix3x3f dest = Matrix3x3f_InvertTranspose(&a);
  return Matrix3x3f_MulVector3f(&dest,
                                (Vector3f){(float)p.x, (float)p.y, 1.0f});
}

Vector3f Vector2i_Barycentric(const Vector2i tri[3], const Vector2i p) {
  Vector3f r;
  float det = (tri[1].y - tri[2].y) * (tri[0].x - tri[2].x) +
              (tri[2].x - tri[1].x) * (tri[0].y - tri[2].y);
  r.x = ((tri[1].y - tri[2].y) * (p.x - tri[2].x) +
         (tri[2].x - tri[1].x) * (p.y - tri[2].y)) /
        det;
  r.y = ((tri[2].y - tri[0].y) * (p.x - tri[2].x) +
         (tri[0].x - tri[2].x) * (p.y - tri[2].y)) /
        det;
  r.z = 1.f - r.v[0] - r.v[1];

  return r;
}

float Vector4f_Length(Vector4f v) { return Vector3f_Length(v.xyz); }

Vector4f Vector4f_Add(Vector4f x, Vector4f y) {
  Vector4f z;
  z.xyz = Vector3f_Add(x.xyz, y.xyz);
  z.w = 1.f;
  return z;
}

Vector4f Vector4f_Sub(Vector4f x, Vector4f y) {
  Vector4f z;
  z.xyz = Vector3f_Sub(x.xyz, y.xyz);
  z.w = 1.f;
  return z;
}

float Vector4f_Dot(Vector4f x, Vector4f y) {
  return Vector3f_Dot(x.xyz, y.xyz);
}

Vector4f Vector4f_Cross(Vector4f x, Vector4f y) {
  Vector4f z;
  z.xyz = Vector3f_Cross(x.xyz, y.xyz);
  z.w = 1.f;
  return z;
}

Vector4f Vector4f_Lerp(Vector4f x1, Vector4f x2, float t) {
  Vector4f z;
  z.xyz = Vector3f_Lerp(x1.xyz, x2.xyz, t);
  z.w = 1.0f;
  return z;
}

void Vector4f_NormalizeInPlace(Vector4f *v) {
  Vector3f_NormalizeInPlace(&v->xyz);
}

Vector4f Vector4f_Normalize(Vector4f v) {
  Vector4f r = {0.f, 0.f, 0.f, 1.f};
  r.xyz = Vector3f_Normalize(v.xyz);
  return r;
}

/* Quaternion Operations */
float Quaternionf_Dot(Quaternionf a, Quaternionf b) {
  return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}

float Quaternionf_Length(Quaternionf q) {
  return (float)sqrt(Quaternionf_Dot(q, q));
}

Quaternionf Quaternionf_Normalize(Quaternionf q) {
  float factor = 1 / Quaternionf_Length(q);
  return (Quaternionf){q.x * factor, q.y * factor, q.z * factor, q.w * factor};
}

void Quaternionf_SetIdentity(Quaternionf *q) {
  q->x = 0.f;
  q->y = 0.f;
  q->z = 0.f;
  q->w = 1.f;
}

// From D3DXMath
//
// Reference:
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code
Quaternionf
Quaternionf_RotationRollPitchYaw(float pitch, float yaw, float roll) {
  Quaternionf q;

  // The math format: // roll (x), pitch (y), yaw (z), angles are in radians
  // double cr = cos(roll * 0.5);
  // double sr = sin(roll * 0.5);
  // double cp = cos(pitch * 0.5);
  // double sp = sin(pitch * 0.5);
  // double cy = cos(yaw * 0.5);
  // double sy = sin(yaw * 0.5);
  //
  // q.w = cr * cp * cy + sr * sp * sy;
  // q.x = sr * cp * cy - cr * sp * sy;
  // q.y = cr * sp * cy + sr * cp * sy;
  // q.z = cr * cp * sy - sr * sp * cy;

  // The D3DXMath format(game format): pitch (x), yaw (y), roll (z)
  double cp = cos(pitch * 0.5);
  double sp = sin(pitch * 0.5);
  double cy = cos(yaw * 0.5);
  double sy = sin(yaw * 0.5);
  double cr = cos(roll * 0.5);
  double sr = sin(roll * 0.5);

  q.w = cr * cp * cy + sr * sp * sy;
  q.x = cr * sp * cy + sr * cp * sy;
  q.y = cr * cp * sy - sr * sp * cy;
  q.z = sr * cp * cy - cr * sp * sy;

  return q;
}

/*
 * For spherical linear interpolation, see
 * 3D Math Primer for Graphics and Game Development, 2nd Edition, Chapter 8
 */
Quaternionf Quaternionf_Slerp(Quaternionf a, Quaternionf b,
                                            float t) {
  float cos_angle = Quaternionf_Dot(a, b);
  if (cos_angle < 0) {
    b = (Quaternionf){-b.x, -b.y, -b.z, -b.w};
    cos_angle = -cos_angle;
  }
  if (cos_angle > 1 - EPSILON) {
    float x = Lerpf(a.x, b.x, t);
    float y = Lerpf(a.y, b.y, t);
    float z = Lerpf(a.z, b.z, t);
    float w = Lerpf(a.w, b.w, t);
    return (Quaternionf){x, y, z, w};
  } else {
    float angle = (float)acos(cos_angle);
    float sin_angle_inv = 1.f / (float)sin(angle);
    float angle_a = (1 - t) * angle;
    float angle_b = t * angle;
    float factor_a = (float)sin(angle_a) * sin_angle_inv;
    float factor_b = (float)sin(angle_b) * sin_angle_inv;
    float x = factor_a * a.x + factor_b * b.x;
    float y = factor_a * a.y + factor_b * b.y;
    float z = factor_a * a.z + factor_b * b.z;
    float w = factor_a * a.w + factor_b * b.w;
    return (Quaternionf){x, y, z, w};
  }
}

Matrix4x4f Matrix4x4f_Add(Matrix4x4f a, Matrix4x4f b) {
  Matrix4x4f r;
  int i, j;
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      r.m[i][j] = a.m[i][j] + b.m[i][j];
    }
  }
  return r;
}

Matrix4x4f Matrix4x4f_Sub(Matrix4x4f a, Matrix4x4f b) {
  Matrix4x4f r;
  int i, j;
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      r.m[i][j] = a.m[i][j] - b.m[i][j];
    }
  }
  return r;
}

Matrix4x4f Matrix4x4f_MulMatrix4x4(Matrix4x4f o,
                                                 Matrix4x4f other) {
  Matrix4x4f r;
  int i, j;
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      r.m[j][i] = (o.m[j][0] * other.m[0][i]) + (o.m[j][1] * other.m[1][i]) +
                  (o.m[j][2] * other.m[2][i]) + (o.m[j][3] * other.m[3][i]);
    }
  }
  return r;
}

void Matrix4x4f_ScaleInPlace(Matrix4x4f *o, float f) {
  int i, j;
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      o->m[i][j] *= f;
    }
  }
}

Matrix4x4f Matrix4x4f_Scale(Matrix4x4f o, float f) {
  Matrix4x4f r;
  int i, j;
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      r.m[i][j] = o.m[i][j] * f;
    }
  }
  return r;
}

int Matrix4x4f_Invert(Matrix4x4f o, Matrix4x4f *inv_out) {
  Matrix4x4f inv;
  float det;

  inv.m11 = o.m22 * o.m33 * o.m44 - o.m22 * o.m34 * o.m43 -
            o.m32 * o.m23 * o.m44 + o.m32 * o.m24 * o.m43 +
            o.m42 * o.m23 * o.m34 - o.m43 * o.m24 * o.m33;

  inv.m21 = -o.m21 * o.m33 * o.m44 + o.m21 * o.m34 * o.m43 + o.m31 * o.m23 * o.m44 -
            o.m31 * o.m24 * o.m43 - o.m41 * o.m23 * o.m34 + o.m41 * o.m24 * o.m33;

  inv.m31 = o.m21 * o.m32 * o.m44 - o.m21 * o.m34 * o.m43 - o.m31 * o.m22 * o.m44 +
            o.m31 * o.m24 * o.m43 + o.m41 * o.m22 * o.m34 - o.m41 * o.m24 * o.m32;

  inv.m41 = -o.m21 * o.m32 * o.m43 + o.m21 * o.m33 * o.m43 + o.m31 * o.m22 * o.m43 -
            o.m31 * o.m23 * o.m43 - o.m41 * o.m22 * o.m33 + o.m41 * o.m23 * o.m32;

  inv.m12 = -o.m12 * o.m33 * o.m44 + o.m12 * o.m34 * o.m43 + o.m32 * o.m13 * o.m44 -
            o.m32 * o.m14 * o.m43 - o.m43 * o.m13 * o.m34 + o.m43 * o.m14 * o.m33;

  inv.m22 = o.m11 * o.m33 * o.m44 - o.m11 * o.m34 * o.m43 - o.m31 * o.m13 * o.m44 +
            o.m31 * o.m14 * o.m43 + o.m41 * o.m13 * o.m34 - o.m41 * o.m14 * o.m33;

  inv.m32 = -o.m11 * o.m32 * o.m44 + o.m11 * o.m34 * o.m43 + o.m31 * o.m12 * o.m44 -
            o.m31 * o.m14 * o.m43 - o.m41 * o.m12 * o.m34 + o.m41 * o.m14 * o.m32;

  inv.m42 = o.m11 * o.m32 * o.m43 - o.m11 * o.m33 * o.m43 - o.m31 * o.m12 * o.m43 +
            o.m31 * o.m13 * o.m43 + o.m41 * o.m12 * o.m33 - o.m41 * o.m13 * o.m32;

  inv.m13 = o.m12 * o.m23 * o.m44 - o.m12 * o.m24 * o.m43 - o.m22 * o.m13 * o.m44 +
            o.m22 * o.m14 * o.m43 + o.m43 * o.m13 * o.m24 - o.m43 * o.m14 * o.m23;

  inv.m23 = -o.m11 * o.m23 * o.m44 + o.m11 * o.m24 * o.m43 + o.m21 * o.m13 * o.m44 -
            o.m21 * o.m14 * o.m43 - o.m41 * o.m13 * o.m24 + o.m41 * o.m14 * o.m23;

  inv.m33 = o.m11 * o.m22 * o.m44 - o.m11 * o.m24 * o.m43 - o.m21 * o.m12 * o.m44 +
            o.m21 * o.m14 * o.m43 + o.m41 * o.m12 * o.m24 - o.m41 * o.m14 * o.m22;

  inv.m43 = -o.m11 * o.m22 * o.m43 + o.m11 * o.m23 * o.m43 + o.m21 * o.m12 * o.m43 -
            o.m21 * o.m13 * o.m43 - o.m41 * o.m12 * o.m23 + o.m41 * o.m13 * o.m22;

  inv.m14 = -o.m12 * o.m23 * o.m34 + o.m12 * o.m24 * o.m33 + o.m22 * o.m13 * o.m34 -
            o.m22 * o.m14 * o.m33 - o.m32 * o.m13 * o.m24 + o.m32 * o.m14 * o.m23;

  inv.m24 = o.m11 * o.m23 * o.m34 - o.m11 * o.m24 * o.m33 - o.m21 * o.m13 * o.m34 +
            o.m21 * o.m14 * o.m33 + o.m31 * o.m13 * o.m24 - o.m31 * o.m14 * o.m23;

  inv.m34 = -o.m11 * o.m22 * o.m34 + o.m11 * o.m24 * o.m32 + o.m21 * o.m12 * o.m34 -
            o.m21 * o.m14 * o.m32 - o.m31 * o.m12 * o.m24 + o.m31 * o.m14 * o.m22;

  inv.m44 = o.m11 * o.m22 * o.m33 - o.m11 * o.m23 * o.m32 - o.m21 * o.m12 * o.m33 +
            o.m21 * o.m13 * o.m32 + o.m31 * o.m12 * o.m23 - o.m31 * o.m13 * o.m22;

  det = o.m11 * inv.m11 + o.m12 * inv.m21 + o.m13 * inv.m31 + o.m14 * inv.m41;

  if (det == 0.f) {
    return 0;
  }

  *inv_out = Matrix4x4f_Scale(inv, 1.f / det);

  return 1;
}

// y = x * m
void Matrix4x4f_Apply(Vector4f *y, const Vector4f *x,
                                    const Matrix4x4f *m) {
  float X = x->x, Y = x->y, Z = x->z, W = x->w;
  y->x = X * m->m[0][0] + Y * m->m[1][0] + Z * m->m[2][0] + W * m->m[3][0];
  y->y = X * m->m[0][1] + Y * m->m[1][1] + Z * m->m[2][1] + W * m->m[3][1];
  y->z = X * m->m[0][2] + Y * m->m[1][2] + Z * m->m[2][2] + W * m->m[3][2];
  y->w = X * m->m[0][3] + Y * m->m[1][3] + Z * m->m[2][3] + W * m->m[3][3];
}

void Matrix4x4f_SetZero(Matrix4x4f *m) {
  memset(m->m, 0, sizeof(float) * 16);
}

void Matrix4x4f_SetIdentity(Matrix4x4f *m) {
  Matrix4x4f_SetZero(m);
  m->m11 = m->m22 = m->m33 = m->m44 = 1.0f;
}

void Matrix4x4f_SetTranslate(Matrix4x4f *m, float x, float y,
                                           float z) {
  Matrix4x4f_SetIdentity(m);
  m->m41 = x;
  m->m42 = y;
  m->m43 = z;
}

void Matrix4x4f_SetScale(Matrix4x4f *m, float x, float y,
                                       float z) {
  Matrix4x4f_SetIdentity(m);
  m->m11 = x;
  m->m22 = y;
  m->m33 = z;
}

void Matrix4x4f_SetRotate(Matrix4x4f *m, float x, float y,
                                        float z, float theta) {
  float qsin = (float)sin(theta * 0.5f);
  float qcos = (float)cos(theta * 0.5f);
  Vector4f vec = {x, y, z, 1.0f};
  float w = qcos;
  Vector4f_NormalizeInPlace(&vec);
  x = vec.x * qsin;
  y = vec.y * qsin;
  z = vec.z * qsin;
  m->m11 = 1 - 2 * y * y - 2 * z * z;
  m->m21 = 2 * x * y - 2 * w * z;
  m->m31 = 2 * x * z + 2 * w * y;
  m->m12 = 2 * x * y + 2 * w * z;
  m->m22 = 1 - 2 * x * x - 2 * z * z;
  m->m32 = 2 * y * z - 2 * w * x;
  m->m13 = 2 * x * z - 2 * w * y;
  m->m23 = 2 * y * z + 2 * w * x;
  m->m33 = 1 - 2 * x * x - 2 * y * y;
  m->m14 = m->m24 = m->m34 = 0.f;
  m->m41 = m->m42 = m->m43 = 0.f;
  m->m44 = 1.f;
}

void Matrix4x4f_SetLookAt(Matrix4x4f *m, Vector4f eye, Vector4f at,
                                 Vector4f up) {
  Vector4f xaxis, yaxis, zaxis;

  zaxis = Vector4f_Sub(at, eye);
  Vector4f_NormalizeInPlace(&zaxis);
  xaxis = Vector4f_Cross(up, zaxis);
  Vector4f_NormalizeInPlace(&xaxis);
  yaxis = Vector4f_Cross(zaxis, xaxis);

  m->m11 = xaxis.x;
  m->m21 = xaxis.y;
  m->m31 = xaxis.z;
  m->m41 = -Vector4f_Dot(xaxis, eye);

  m->m12 = yaxis.x;
  m->m22 = yaxis.y;
  m->m32 = yaxis.z;
  m->m42 = -Vector4f_Dot(yaxis, eye);

  m->m13 = zaxis.x;
  m->m23 = zaxis.y;
  m->m33 = zaxis.z;
  m->m43 = -Vector4f_Dot(zaxis, eye);

  m->m14 = m->m24 = m->m34 = 0.f;
  m->m44 = 1.f;
}

// D3DXMatrixPerspectiveFovLH
void Matrix4x4f_SetPerspective(Matrix4x4f *m, float fovy, float aspect,
                                      float zn, float zf) {
  float fax = 1.0f / (float)tan(fovy * 0.5f);
  Matrix4x4f_SetZero(m);
  m->m11 = (float)(fax / aspect);
  m->m22 = (float)(fax);
  m->m33 = zf / (zf - zn);
  m->m43 = -zn * zf / (zf - zn);
  m->m34 = 1;
}

typedef struct {
  Matrix4x4f world;      // 世界坐标变换
  Matrix4x4f view;       // 摄影机坐标变换
  Matrix4x4f projection; // 投影变换
  Matrix4x4f transform;  // transform = world * view * projection
  float w, h;            // 屏幕大小
} Transform;

// 矩阵更新，计算 transform = world * view * projection
void Transform_Update(Transform *ts) {
  ts->transform = Matrix4x4f_MulMatrix4x4(
      Matrix4x4f_MulMatrix4x4(ts->world, ts->view), ts->projection);
}

// 初始化，设置屏幕长宽
void Transform_Init(Transform *ts, int width, int height) {
  float aspect = (float)width / ((float)height);
  Matrix4x4f_SetIdentity(&ts->world);
  Matrix4x4f_SetIdentity(&ts->view);
  Matrix4x4f_SetPerspective(&ts->projection, 3.1415926f * 0.5f, aspect, 1.0f,
                            500.0f);
  ts->w = (float)width;
  ts->h = (float)height;
  Transform_Update(ts);
}

// 将矢量 x 进行 project
void Transform_Apply(const Transform *ts, Vector4f *y,
                            const Vector4f *x) {
  Matrix4x4f_Apply(y, x, &ts->transform);
}

// 检查齐次坐标同 cvv 的边界用于视锥裁剪
int Transform_CheckCvv(const Vector4f *v) {
  float w = v->w;
  int check = 0;
  if (v->z < 0.0f)
    check |= 1;
  if (v->z > w)
    check |= 2;
  if (v->x < -w)
    check |= 4;
  if (v->x > w)
    check |= 8;
  if (v->y < -w)
    check |= 16;
  if (v->y > w)
    check |= 32;

  return check;
}

// 归一化，得到屏幕坐标
void Transform_Homogenize(const Transform *ts, Vector4f *y,
                                 const Vector4f *x) {
  float rhw = 1.0f / x->w;
  y->x = (x->x * rhw + 1.0f) * ts->w * 0.5f;
  y->y = (1.0f - x->y * rhw) * ts->h * 0.5f;
  y->z = x->z * rhw;
  y->w = 1.0f;
}

//=====================================================================
// 几何计算：顶点、扫描线、边缘、矩形、步长计算
//=====================================================================
typedef struct {
  float r, g, b;
} Color3f;

typedef struct {
  float u, v;
} Texcoord2f;

typedef struct {
  Point4f pos;
  Texcoord2f tc;
  Color3f color;
  float rhw;
} Vertexf;

typedef struct {
  Vertexf v, v1, v2;
} Edgef;

typedef struct {
  float top, bottom;
  Edgef left, right;
} Trapezoidf;

typedef struct {
  Vertexf v, step;
  int x, y, w;
} Scanlinef;

void Vertexf_RhwInit(Vertexf *v) {
  float rhw = 1.0f / v->pos.w;
  v->rhw = rhw;
  v->tc.u *= rhw;
  v->tc.v *= rhw;
  v->color.r *= rhw;
  v->color.g *= rhw;
  v->color.b *= rhw;
}

void Vertexf_Interpolate(Vertexf *y, const Vertexf *x1,
                                const Vertexf *x2, float t) {
  y->pos = Vector4f_Lerp(x1->pos, x2->pos, t);
  y->tc.u = Interpolatef(x1->tc.u, x2->tc.u, t);
  y->tc.v = Interpolatef(x1->tc.v, x2->tc.v, t);
  y->color.r = Interpolatef(x1->color.r, x2->color.r, t);
  y->color.g = Interpolatef(x1->color.g, x2->color.g, t);
  y->color.b = Interpolatef(x1->color.b, x2->color.b, t);
  y->rhw = Interpolatef(x1->rhw, x2->rhw, t);
}

void Vertexf_Division(Vertexf *y, const Vertexf *x1, const Vertexf *x2,
                             float w) {
  float inv = 1.0f / w;
  y->pos.x = (x2->pos.x - x1->pos.x) * inv;
  y->pos.y = (x2->pos.y - x1->pos.y) * inv;
  y->pos.z = (x2->pos.z - x1->pos.z) * inv;
  y->pos.w = (x2->pos.w - x1->pos.w) * inv;
  y->tc.u = (x2->tc.u - x1->tc.u) * inv;
  y->tc.v = (x2->tc.v - x1->tc.v) * inv;
  y->color.r = (x2->color.r - x1->color.r) * inv;
  y->color.g = (x2->color.g - x1->color.g) * inv;
  y->color.b = (x2->color.b - x1->color.b) * inv;
  y->rhw = (x2->rhw - x1->rhw) * inv;
}

void Vertexf_Add(Vertexf *y, const Vertexf *x) {
  y->pos.x += x->pos.x;
  y->pos.y += x->pos.y;
  y->pos.z += x->pos.z;
  y->pos.w += x->pos.w;
  y->rhw += x->rhw;
  y->tc.u += x->tc.u;
  y->tc.v += x->tc.v;
  y->color.r += x->color.r;
  y->color.g += x->color.g;
  y->color.b += x->color.b;
}

// 根据三角形生成 0-2 个梯形，并且返回合法梯形的数量
int Trapezoidf_InitTriangle(Trapezoidf *trap, const Vertexf *p1,
                                   const Vertexf *p2, const Vertexf *p3) {
  const Vertexf *p;
  float k, x;

  if (p1->pos.y > p2->pos.y) {
    p = p1, p1 = p2, p2 = p;
  }

  if (p1->pos.y > p3->pos.y) {
    p = p1, p1 = p3, p3 = p;
  }

  if (p2->pos.y > p3->pos.y) {
    p = p2, p2 = p3, p3 = p;
  }

  if (p1->pos.y == p2->pos.y && p1->pos.y == p3->pos.y) {
    return 0;
  }

  if (p1->pos.x == p2->pos.x && p1->pos.x == p3->pos.x) {
    return 0;
  }

  if (p1->pos.y == p2->pos.y) { // triangle down
    if (p1->pos.x > p2->pos.x) {
      p = p1, p1 = p2, p2 = p;
    }
    trap[0].top = p1->pos.y;
    trap[0].bottom = p3->pos.y;
    trap[0].left.v1 = *p1;
    trap[0].left.v2 = *p3;
    trap[0].right.v1 = *p2;
    trap[0].right.v2 = *p3;

    return (trap[0].top < trap[0].bottom) ? 1 : 0;
  }

  if (p2->pos.y == p3->pos.y) { // triangle up
    if (p2->pos.x > p3->pos.x) {
      p = p2, p2 = p3, p3 = p;
    }
    trap[0].top = p1->pos.y;
    trap[0].bottom = p3->pos.y;
    trap[0].left.v1 = *p1;
    trap[0].left.v2 = *p2;
    trap[0].right.v1 = *p1;
    trap[0].right.v2 = *p3;

    return (trap[0].top < trap[0].bottom) ? 1 : 0;
  }

  trap[0].top = p1->pos.y;
  trap[0].bottom = p2->pos.y;
  trap[1].top = p2->pos.y;
  trap[1].bottom = p3->pos.y;

  k = (p3->pos.y - p1->pos.y) / (p2->pos.y - p1->pos.y);
  x = p1->pos.x + (p2->pos.x - p1->pos.x) * k;

  if (x <= p3->pos.x) { // triangle left
    trap[0].left.v1 = *p1;
    trap[0].left.v2 = *p2;
    trap[0].right.v1 = *p1;
    trap[0].right.v2 = *p3;
    trap[1].left.v1 = *p2;
    trap[1].left.v2 = *p3;
    trap[1].right.v1 = *p1;
    trap[1].right.v2 = *p3;
  } else { // triangle right
    trap[0].left.v1 = *p1;
    trap[0].left.v2 = *p3;
    trap[0].right.v1 = *p1;
    trap[0].right.v2 = *p2;
    trap[1].left.v1 = *p1;
    trap[1].left.v2 = *p3;
    trap[1].right.v1 = *p2;
    trap[1].right.v2 = *p3;
  }

  return 2;
}

// 按照 Y 坐标计算出左右两条边纵坐标等于 Y 的顶点
void Trapezoidf_EdgeInterpolate(Trapezoidf *trap, float y) {
  float s1 = trap->left.v2.pos.y - trap->left.v1.pos.y;
  float s2 = trap->right.v2.pos.y - trap->right.v1.pos.y;
  float t1 = (y - trap->left.v1.pos.y) / s1;
  float t2 = (y - trap->right.v1.pos.y) / s2;
  Vertexf_Interpolate(&trap->left.v, &trap->left.v1, &trap->left.v2, t1);
  Vertexf_Interpolate(&trap->right.v, &trap->right.v1, &trap->right.v2, t2);
}

// 根据左右两边的端点，初始化计算出扫描线的起点和步长
void Trapezoidf_InitScanline(const Trapezoidf *trap, Scanlinef *scanline,
                                    int y) {
  float width = trap->right.v.pos.x - trap->left.v.pos.x;
  scanline->x = (int)(trap->left.v.pos.x + 0.5f);
  scanline->w = (int)(trap->right.v.pos.x + 0.5f) - scanline->x;
  scanline->y = y;
  scanline->v = trap->left.v;
  if (trap->left.v.pos.x >= trap->right.v.pos.x) {
    scanline->w = 0;
  }
  Vertexf_Division(&scanline->step, &trap->left.v, &trap->right.v, width);
}