mathlib.cpp

#include "mathlib.h"

/**
 * Vector4 Class
 */

Vector4::Vector4 (void)
{
  this->x = 0;
  this->y = 0;
  this->z = 0;
  this->w = 0;
}

Vector4::Vector4 (float X, float Y, float Z)
{
  this->x = X;
  this->y = Y;
  this->z = Z;
  this->w = 1;
}

Vector4::Vector4 (float X, float Y, float Z, float W)
{
  this->x = X;
  this->y = Y;
  this->z = Z;
  this->w = W;
}

Vector4 Vector4::operator+ (Vector4 v) const
{
  return Vector4{
      this->x + v.x,
      this->y + v.y,
      this->z + v.z,
      this->w + v.w
  };
}

Vector4 Vector4::operator*= (float scalar)
{
  this->x *= scalar;
  this->y *= scalar;
  this->z *= scalar;
  this->w *= scalar;

  return *this;
}

Vector4 Vector4::operator+= (Vector4 &v)
{
  this->x += v.x;
  this->y += v.y;
  this->z += v.z;
  this->w += v.w;

  return *this;
}


Vector4 Vector4::operator- (Vector4 v) const
{
  return Vector4{
      this->x - v.x,
      this->y - v.y,
      this->z - v.z,
      this->w - v.w
  };
}

Vector4 Vector4::operator-= (Vector4 &v)
{
  this->x -= v.x;
  this->y -= v.y;
  this->z -= v.z;
  this->w -= v.w;

  return *this;
}


Vector4 Vector4::operator* (Vector4 v) const
{
  return Vector4{
      this->x * v.x,
      this->y * v.y,
      this->z * v.z,
      this->w * v.w
  };
}

Vector4 Vector4::operator* (float scalar) const
{
  return Vector4{
      this->x * scalar,
      this->y * scalar,
      this->z * scalar,
      this->w
  };
}

float Vector4::operator[] (int index) const
{
  switch (index) {
    case 0:
      return this->x;
    case 1:
      return this->y;
    case 2:
      return this->z;
    case 3:
      return this->w;
  }

  return this->w;
}

Vector4::operator std::string (void) const
{
  const auto MAXLEN = 32;
  char str[MAXLEN]{};

#ifdef MATHLIB_PRINT_3D
  std::snprintf (
      str, MAXLEN,
      "(%.2f, %.2f, %.2f)",
      this->x, this->y, this->z
  );
#else
  std::snprintf (
      str, MAXLEN,
      "(%.2f, %.2f, %.2f, %.2f)",
      this->x, this->y, this->z, this->w
  );
#endif

  // Trim off the excess
  size_t length = 0;
  for (auto ptr = str; *ptr; ptr++ && length++);

  return std::string{str, length};
}


Vector4 Vector4::Cross (const Vector4 &v) const
{
  return Vector4{
      this->y * v.z - this->z * v.y,
      this->z * v.x - this->x * v.z,
      this->x * v.y - this->y * v.x
  };
}

float Vector4::Dot3D (Vector4 v) const
{
  return this->x * v.x
         + this->y * v.y
         + this->z * v.z;
}

float Vector4::Dot (Vector4 v) const
{
  return this->x * v.x
         + this->y * v.y
         + this->z * v.z
         + this->w * v.w;
}

/**
 * Matrix3x3 Structure
 */

#define a 0
#define b 1
#define c 2
#define r1 m[0]
#define r2 m[1]
#define r3 m[2]

matrix3x3::matrix3x3_t (void)
{
//@formatter:off
  m[a][0] = 1; m[b][0] = 0; m[c][0] = 0;
  m[a][1] = 0; m[b][1] = 1; m[c][1] = 0;
  m[a][2] = 0; m[b][2] = 0; m[c][2] = 1;
//@formatter:on
}

matrix3x3::matrix3x3_t (
    float a0, float a1, float a2,
    float b0, float b1, float b2,
    float c0, float c1, float c2
)
{
// @formatter:off
    m[a][0] = a0; m[a][1] = a1; m[a][2] = a2;
    m[b][0] = b0; m[b][1] = b1; m[b][2] = b2;
    m[c][0] = c0; m[c][1] = c1; m[c][2] = c2;
// @formatter:on

}

matrix3x3 matrix3x3::ConjugateTranspose () const
{
  return matrix3x3{
      r2[b] * r3[c] - r3[b] * r2[c], -r2[a] * r3[c] - r3[a] * r2[c], r2[a] * r3[b] - r3[a] * r2[b],
      -r1[b] * r3[c] - r3[b] * r1[c], r1[a] * r3[c] - r3[a] * r1[c], -r1[a] * r3[b] - r3[a] * r1[b],
      r1[b] * r2[c] - r2[b] * r1[c], -r1[a] * r2[c] - r2[a] * r1[c], r1[a] * r2[b] - r2[a] * r1[b]
  };
}

matrix3x3 matrix3x3::Scale (float scale) const
{
  return matrix3x3{
      r1[a] * scale, r2[a] * scale, r3[a] * scale,
      r1[b] * scale, r2[b] * scale, r3[b] * scale,
      r1[c] * scale, r2[c] * scale, r3[c] * scale
  };
}

float matrix3x3::Determinant (void) const
{
  return (r1[a] * (r2[b] * r3[c] - r2[c] * r3[b]))
         - (r1[b] * (r2[a] * r3[c] - r2[c] * r3[a]))
         + (r1[c] * (r2[a] * r3[c] - r2[b] * r3[a]));
}

Vector4 matrix3x3::ColumnVector (int index) const
{
  index = index % 3;
  return {m[a][index], m[b][index], m[c][index], 0};
}

Vector4 matrix3x3::operator[] (int index) const
{
  index = index % 3;
  return {m[index][a], m[index][b], m[index][c], 0};

  // john: I'm not sure why this messes up everything but this needs to be at infinity I guess.
  //return {m[index][a], m[index][b], m[index][c], 1};
}

#define d 2

matrix3x4::matrix3x4_t (
    float a0, float a1, float a2,
    float b0, float b1, float b2,
    float c0, float c1, float c2,
    float d0, float d1, float d2
)
{
// @formatter:off
  m[a][0]= a0; m[a][1]= a1; m[a][2]=a2;
  m[b][0]= b0; m[b][1]= b1; m[b][2]=b2;
  m[c][0]= c0; m[c][1]= c1; m[c][2]=c2;
  m[d][0]= d0; m[d][1]= d1; m[d][2]=d2;
// @formatter:on
}

Vector4 matrix3x4::ColumnVector (int index) const
{
  index = index % 3;
  return {m[a][index], m[b][index], m[c][index], m[d][index]};
}


Vector4 matrix3x4::operator[] (int index) const
{
  index = index % 3;
  return Vector4{m[index][a], m[index][b], m[index][c], m[index][d]};
}

#undef a
#undef b
#undef c
#undef d
#undef r1
#undef r2
#undef r3

/**
 * Matrix Class}
 */

Matrix::Matrix (const matrix3x3 &lowerMatrix)
{
  this->Populate (
      lowerMatrix.m[0][0], lowerMatrix.m[0][1], lowerMatrix.m[0][2], 1,
      lowerMatrix.m[1][0], lowerMatrix.m[1][1], lowerMatrix.m[1][2], 1,
      lowerMatrix.m[2][0], lowerMatrix.m[2][1], lowerMatrix.m[2][2], 1,
      0, 0, 0, 1
  );
}

Matrix::Matrix (const Matrix &originalMatrix)
{
  this->Populate (
      originalMatrix.m[0][0], originalMatrix.m[0][1], originalMatrix.m[0][2], originalMatrix.m[0][3],
      originalMatrix.m[1][0], originalMatrix.m[1][1], originalMatrix.m[1][2], originalMatrix.m[1][3],
      originalMatrix.m[2][0], originalMatrix.m[2][1], originalMatrix.m[2][2], originalMatrix.m[2][3],
      originalMatrix.m[3][0], originalMatrix.m[3][1], originalMatrix.m[3][2], originalMatrix.m[3][3]
  );
}

void Matrix::Populate (
    float a0, float a1, float a2, float a3,
    float b0, float b1, float b2, float b3,
    float c0, float c1, float c2, float c3,
    float d0, float d1, float d2, float d3
)
{
  const Vector4 a = Vector4 (a0, a1, a2, a3),
      b = Vector4 (b0, b1, b2, b3),
      c = Vector4 (c0, c1, c2, c3),
      d = Vector4 (d0, d1, d2, d3);

  this->Populate (a, b, c, d);
}

void Matrix::Populate (const Vector4 &a, const Vector4 &b, const Vector4 &c, const Vector4 &d)
{
  MatrixWriteColumn (*this, 0, a);
  MatrixWriteColumn (*this, 1, b);
  MatrixWriteColumn (*this, 2, c);
  MatrixWriteColumn (*this, 3, d);
}


bool Matrix::IsInvertible (void) const
{
  return this->Determinant () != 0;
}


Vector4 Matrix::ColumnVector (int i) const
{
  // john: better than crashing? /shrug
  i = i % 4;

  return Vector4{m[0][i], m[1][i], m[2][i], m[3][i]};
}

Vector4 Matrix::operator[] (int index) const
{
  return this->ColumnVector (index);
}


Vector4 Matrix::RowVector (int i) const
{
  // john: better than crashing? /shrug
  i = i % 4;

  return Vector4{m[i][0], m[i][1], m[i][2], m[i][3]};
}


Matrix::operator std::string (void) const
{
  auto const MAXLEN = 120;
  char str[MAXLEN]{};

#ifdef MATHLIB_PRINT_3D
  std::snprintf (
      str, MAXLEN,
      "(%s, %s, %s)",
      ((std::string) this->vectors[0]).c_str (),
      ((std::string) this->vectors[1]).c_str (),
      ((std::string) this->vectors[2]).c_str ()
  );
#else
  std::snprintf (
      str, MAXLEN,
      "(%s, %s, %s, %s)",
      ((std::string) this->vectors[0]).c_str (),
      ((std::string) this->vectors[1]).c_str (),
      ((std::string) this->vectors[2]).c_str (),
      ((std::string) this->vectors[3]).c_str ()
  );
#endif

  // Trim off the excess
  size_t length = 0;
  for (auto ptr = str; *ptr; ptr++ && length++);

  return std::string{str, length};
}

Matrix Matrix::Transpose () const
{
  Matrix result{};
  for (int row = 0; row < 4; row++)
    for (int col = 0; col < 4; col++)
      result.m[row][col] = this->m[col][row];

  return result;
}

Matrix Matrix::ConjugateTranspose () const
{
  Matrix result{};
  for (int row = 0; row < 4; row++)
    for (int col = 0; col < 4; col++)
      result.m[row][col] = this->Friends3x3 (row, col).Determinant ();

  return result;
}

matrix3x3 Matrix::ConjugateTranspose3x3 () const
{
  return this->Friends3x3 (3, 3).ConjugateTranspose ();
}


matrix3x3 Matrix::Friends3x3 (int row, int column) const
{
  return matrix3x3{
      m[(row + 1) % 4][(column + 1) % 4], m[(row + 1) % 4][(column + 2) % 4], m[(row + 1) % 4][(column + 3) % 4],
      m[(row + 2) % 4][(column + 1) % 4], m[(row + 2) % 4][(column + 2) % 4], m[(row + 2) % 4][(column + 3) % 4],
      m[(row + 3) % 4][(column + 1) % 4], m[(row + 3) % 4][(column + 2) % 4], m[(row + 3) % 4][(column + 3) % 4]
  };
}

Matrix Matrix::operator+ (Matrix neighbor) const
{
  Matrix result{};
  result.Populate (
      this->vectors[0] + neighbor.vectors[0],
      this->vectors[1] + neighbor.vectors[1],
      this->vectors[2] + neighbor.vectors[2],
      this->vectors[3] + neighbor.vectors[3]
  );

  return result;
}

Matrix Matrix::operator- (Matrix neighbor) const
{
  Matrix result{};
  result.Populate (
      this->vectors[0] - neighbor.vectors[0],
      this->vectors[1] - neighbor.vectors[1],
      this->vectors[2] - neighbor.vectors[2],
      this->vectors[3] - neighbor.vectors[3]
  );

  return result;
}

Matrix Matrix::operator* (float scalar) const
{
  Matrix result{};
  result.Populate (
      this->vectors[0] * scalar,
      this->vectors[1] * scalar,
      this->vectors[2] * scalar,
      this->vectors[3] * scalar
  );

  return result;
}

Matrix Matrix::operator* (const Matrix &neighbor) const
{
  Matrix result{};
  Vector4 c1 = this->RowVector (0),
      c2 = this->RowVector (1),
      c3 = this->RowVector (2),
      c4 = this->RowVector (3);

  result.Populate (
      c1.Dot (neighbor[0]), c1.Dot (neighbor[1]), c1.Dot (neighbor[2]), c1.Dot (neighbor[3]),
      c2.Dot (neighbor[0]), c2.Dot (neighbor[1]), c2.Dot (neighbor[2]), c2.Dot (neighbor[3]),
      c3.Dot (neighbor[0]), c3.Dot (neighbor[1]), c3.Dot (neighbor[2]), c3.Dot (neighbor[3]),
      c4.Dot (neighbor[0]), c4.Dot (neighbor[1]), c4.Dot (neighbor[2]), c4.Dot (neighbor[3])
  );

  return result;
}


Matrix Matrix::operator* (matrix3x4 neighbor) const
{
  Matrix result{};
  Vector4 c1 = this->RowVector (0), r1 = neighbor.ColumnVector (0),
      c2 = this->RowVector (1), r2 = neighbor.ColumnVector (1),
      c3 = this->RowVector (2), r3 = neighbor.ColumnVector (2),
      r4 = neighbor.ColumnVector (2);

  result.Populate (
      r1.Dot (c1), r1.Dot (c2), r1.Dot (c3), 0,
      r2.Dot (c1), r2.Dot (c2), r2.Dot (c3), 0,
      r3.Dot (c1), r3.Dot (c2), r3.Dot (c3), 0,
      r4.Dot (c1), r4.Dot (c2), r4.Dot (c3), 1
  );

  return result.Transpose ();
}

Matrix Matrix::operator* (matrix3x3 neighbor) const
{
  Matrix result{};
  Vector4 c1 = this->ColumnVector (0),
      c2 = this->ColumnVector (1),
      c3 = this->ColumnVector (2),
      c4 = this->ColumnVector (3),
      r1 = neighbor[0], r2 = neighbor[1], r3 = neighbor[2];

  result.Populate (
      r1.Dot (c1), r1.Dot (c2), r1.Dot (c3), r1.Dot (c4),
      r2.Dot (c1), r2.Dot (c2), r2.Dot (c3), r2.Dot (c4),
      r3.Dot (c1), r3.Dot (c2), r3.Dot (c3), r3.Dot (c4),
      0, 0, 0, 1
  );

  return result;
}

//Matrix Matrix::operator* (matrix3x3 neighbor) const
//{
//	Matrix result{};
//	Vector4 c1 = this->RowVector (0), r1 = neighbor.ColumnVector (0),
//			c2 = this->RowVector (1), r2 = neighbor.ColumnVector (1),
//			c3 = this->RowVector (2), r3 = neighbor.ColumnVector (2);
//
//	//@formatter:off
//	result.Populate (
//			r1.Dot(c1), r1.Dot(c2), r1.Dot(c3), 0,
//			r2.Dot(c1), r2.Dot(c2), r2.Dot(c3), 0,
//			r3.Dot(c1), r3.Dot(c2), r3.Dot(c3), 0,
//			0,0,0,1
//	);
//	//@formatter:on
//
//	return result.Transpose ();
//}


float Matrix::Determinant () const
{
  auto const a = 0, b = 1, c = 2, d = 3;
  return m[a][0] * this->Friends3x3 (a, 0).Determinant ()
         - m[b][0] * this->Friends3x3 (b, 0).Determinant ()
         + m[c][0] * this->Friends3x3 (c, 0).Determinant ()
         - m[d][0] * this->Friends3x3 (d, 0).Determinant ();
}

float Matrix::Determinant3x3 () const
{
  return this->Friends3x3 (3, 3).Determinant ();
}

Matrix Matrix::Invert () const
{
  Matrix result{};

  float det = this->Determinant ();
  if (det == 0.0f) return result;

  return this->ConjugateTranspose ().Scale (1 / det);
}

matrix3x3 Matrix::Invert3x3 () const
{
  matrix3x3 result{};

  float det = this->Determinant3x3 ();
  if (det == 0.0f) return result;

  result = this->ConjugateTranspose3x3 ();
  result = result.Scale (1 / det);
  return result;
}

Matrix Matrix::Scale (float scalar) const
{
  Matrix result{};
  result.Populate (
      this->vectors[0] * scalar,
      this->vectors[1] * scalar,
      this->vectors[2] * scalar,
      this->vectors[3] * scalar
  );

  return result;
}

Matrix Matrix::Scale (const Vector4 &v) const
{
  Matrix result{};
  result.Populate (
      this->vectors[0] * v,
      this->vectors[1] * v,
      this->vectors[2] * v,
      this->vectors[3] * Vector4 (v.x, v.y, v.z, 1.0f)
  );

  return result;

}

inline void MatrixWriteRow (Matrix &matrix, int row, const Vector4 &input)
{
  auto m = matrix.m;
  m[0][row] = input.x;
  m[1][row] = input.y;
  m[2][row] = input.z;
  m[3][row] = input.w;

  // TODO(John): matrix.vectors[0 .. 3][column] = input
}

inline void MatrixWriteColumn (Matrix &matrix, int column, const Vector4 &input)
{
  auto m = matrix.m;
  m[column][0] = input.x;
  m[column][1] = input.y;
  m[column][2] = input.z;
  m[column][3] = input.w;

  matrix.vectors[column] = input;
}

mathlib.h

#pragma clang diagnostic push
#pragma ide diagnostic ignored "ArgumentSelectionDefects"

#define MATHLIB_PRINT_3D

#ifndef MATHLIB_H
#define MATHLIB_H

#include <iostream>

#define NUMERIC_MIN(P, Q) (P < Q ? P : Q)
#define NUMERIC_MAX(P, Q) (P > Q ? P : Q)
#define NUMERIC_CLAMP(P, N, Q) NUMERIC_MIN(NUMERIC_MAX(P, i), Q)

class Vector4
{
public:
    float x, y, z, w;

    Vector4 (void);
    Vector4 (float X, float Y, float Z);
    Vector4 (float X, float Y, float Z, float W);

    // basic operations
    Vector4 operator+ (Vector4 v) const;
    Vector4 operator- (Vector4 v) const;
    Vector4 operator* (Vector4 v) const;
    Vector4 operator* (float scalar) const;

    Vector4 operator*= (float scalar);
    Vector4 operator+= (Vector4 &v);
    Vector4 operator-= (Vector4 &v);

    // vector operations
    float Dot (Vector4 v) const;
    float Dot3D (Vector4 v) const;
    Vector4 Cross (const Vector4 &v) const;

    // other stuff
    float operator[] (int index) const;

    explicit operator std::string (void) const;
};

typedef struct matrix3x3_t
{
    float m[3][3]{};

    matrix3x3_t (void);

    matrix3x3_t (
        float a0, float a1, float a2,
        float b0, float b1, float b2,
        float c0, float c1, float c2
    );

    matrix3x3_t ConjugateTranspose () const;
    matrix3x3_t Scale (float scale) const;
    float Determinant (void) const;
    
    Vector4 operator[] (int index) const;
    Vector4 ColumnVector (int index) const;
} matrix3x3;

typedef struct matrix3x4_t
{
    float m[4][3]{};

    matrix3x4_t (
        float a0, float a1, float a2,
        float b0, float b1, float b2,
        float c0, float c1, float c2,
        float d0, float d1, float d2
    );

    Vector4 operator[] (int index) const;
    Vector4 ColumnVector (int index) const;
} matrix3x4;


class Matrix
{
public:
    float m[4][4]{};
    Vector4 vectors[4]{};

    explicit Matrix (void) = default;
    explicit Matrix (const matrix3x3 &lowerMatrix);

    // Copy constructor
    Matrix (const Matrix &originalMatrix);

    void Populate (
        float a0, float a1, float a2, float a3,
        float b0, float b1, float b2, float b3,
        float c0, float c1, float c2, float c3,
        float d0, float d1, float d2, float d3
    );

    void Populate (const Vector4 &a, const Vector4 &b, const Vector4 &c, const Vector4 &d);

    bool IsInvertible (void) const;

    // basic operations
    Matrix operator+ (Matrix neighbor) const;
    Matrix operator- (Matrix neighbor) const;
    Matrix operator* (float scalar) const;
    Matrix operator* (const Matrix &neighbor) const;
    Matrix operator* (matrix3x3 neighbor) const;
    Matrix operator* (matrix3x4 m) const;
    Vector4 operator[] (int index) const;


    // matrix operations
    Matrix Invert () const;

    matrix3x3 Invert3x3 () const;

    Matrix Scale (float scalar) const;
    Matrix Scale (const Vector4 &v) const;
    Matrix Transpose (void) const;
    Matrix ConjugateTranspose (void) const;
    matrix3x3 ConjugateTranspose3x3 (void) const;
    matrix3x3 Friends3x3 (int row, int column) const;

    float Determinant (void) const;
    float Determinant3x3 (void) const;

    // basic row operations
    Vector4 ColumnVector (int i) const;
    Vector4 RowVector (int i) const;

    explicit operator std::string (void) const;
};

inline void MatrixWriteRow (Matrix &matrix, int row, const Vector4 &input);
inline void MatrixWriteColumn (Matrix &matrix, int column, const Vector4 &input);

#endif // MATHLIB_H
#pragma clang diagnostic pop