1. Lars Viklund
  2. slade

Commits

Lars Viklund  committed ba6c7be

Add math code

  • Participants
  • Parent commits 2ef912a
  • Branches master

Comments (0)

Files changed (4)

File src/MathIO.cc

View file
+#include "MathIO.h"
+
+#include <iostream>
+
+struct PosNegAppend {
+  PosNegAppend(float A) : A(A) {}
+  float A;
+
+  template <typename CharT, typename TraitsT>
+  inline friend std::basic_ostream<CharT, TraitsT>&
+  operator << (std::basic_ostream<CharT, TraitsT>& OS, PosNegAppend const& PNA) {
+    if (PNA.A < 0.0f) return OS << "- " << fabs(PNA.A);
+    return OS << "+ " << PNA.A;
+  }
+};
+
+std::ostream&
+operator << (std::ostream& OS, Quaternion Q) {
+  OS << "(" << Q.R << " ";
+  OS << PosNegAppend(Q.V.X) << "i ";
+  OS << PosNegAppend(Q.V.Y) << "j ";
+  OS << PosNegAppend(Q.V.Z) << "k)";
+  return OS;
+}

File src/MathIO.h

View file
+#pragma once
+
+#include "MathTypes.h"
+
+#include <iosfwd>
+
+std::ostream&
+operator << (std::ostream& OS, Quaternion Q);

File src/MathOps.h

View file
+#pragma once
+
+#include "Common.h"
+#include "MathTypes.h"
+#include <cmath>
+#include <random>
+
+inline float
+min(float A, float B) {
+  return (A<B) ? A : B;
+}
+
+inline float
+max(float A, float B) {
+  return (A<B) ? B : A;
+}
+
+inline float
+clamp(float X, float A, float B) {
+  return min(max(X, A), B);
+}
+
+inline Vec3
+operator + (Vec3 V1, Vec3 V2) {
+  return {
+    V1.X + V2.X,
+    V1.Y + V2.Y,
+    V1.Z + V2.Z
+  };
+}
+
+inline Vec3
+operator - (Vec3 V1, Vec3 V2) {
+  return {
+    V1.X - V2.X,
+    V1.Y - V2.Y,
+    V1.Z - V2.Z
+  };
+}
+
+inline Vec3
+operator - (Vec3 V) {
+  return {
+    -V.X,
+    -V.Y,
+    -V.Z
+  };
+}
+
+inline Vec3
+operator * (Vec3 V, float K) {
+  return {
+    K*V.X,
+    K*V.Y,
+    K*V.Z
+  };
+}
+
+inline Vec3
+operator * (float K, Vec3 V) {
+  return {
+    K*V.X,
+    K*V.Y,
+    K*V.Z
+  };
+}
+
+inline float
+dot(Vec3 V1, Vec3 V2) {
+  return V1.X*V2.X + V1.Y*V2.Y + V1.Z*V2.Z;
+}
+
+inline Vec3
+cross(Vec3 V1, Vec3 V2) {
+  return {
+    V1.Y*V2.Z - V1.Z*V2.Y,
+    V1.Z*V2.X - V1.X*V2.Z,
+    V1.X*V2.Y - V1.Y*V2.X
+  };
+}
+
+inline float
+norm(Vec3 V) {
+  return sqrt(dot(V, V));
+}
+
+inline Quaternion
+operator + (Quaternion Q1, Quaternion Q2) {
+  return {
+    Q1.R + Q2.R,
+    Q1.V + Q2.V
+  };
+}
+
+inline Quaternion
+operator * (Quaternion Q1, Quaternion Q2) {
+  return {
+    Q1.R*Q2.R - dot(Q1.V, Q2.V),
+    Q1.R*Q2.V + Q2.R*Q1.V + cross(Q1.V, Q2.V)
+  };
+}
+
+inline Quaternion
+conjugate(Quaternion Q) {
+  return {
+    Q.R,
+    -Q.V
+  };
+}
+
+inline float
+norm(Quaternion Q) {
+  return sqrtf(Q.R*Q.R + dot(Q.V, Q.V));
+}
+
+inline Quaternion
+quaternionFromAxisAngle(float Angle, Vec3 Axis) {
+  return {
+    cosf(Angle/2.0f),
+    sinf(Angle/2.0f)*Axis
+  };
+}
+
+inline Quaternion
+quaternionIdentity() {
+  return {
+    1.0f,
+    { -.0f, 0.0f, 0.0f }
+  };
+}
+
+inline Mat4
+matrixFromQuaternion(Quaternion Q) {
+  auto A = Q.R, B = Q.V.X, C = Q.V.Y, D = Q.V.Z;
+  /* | aa + bb - cc - dd | 2bc - 2ad         | 2bd + 2ac         |
+     | 2bc + 2ad         | aa - bb + cc - dd | 2cd - 2ab         |
+     | 2bd - 2ac         | 2cd + 2ab         | aa - bb - cc + dd |
+  */
+  Mat4 Ret = { // column vectors
+    Vec4{ A*A + B*B - C*C - D*D, 2*B*C + 2*A*D, 2*B*D - 2*A*C, 0.0f },
+    Vec4{ 2*B*C - 2*A*D, A*A - B*B + C*C - D*D, 2*C*D + 2*A*B, 0.0f },
+    Vec4{ 2*B*D + 2*A*C, 2*C*D - 2*A*B, A*A - B*B - C*C + D*D, 0.0f },
+    Vec4{ 0.0f, 0.0f, 0.0f, 1.0f }
+  };
+  return Ret;
+}
+
+inline Mat4
+matrixIdentity() {
+  Mat4 Ret = {};
+  Ret(0, 0) = Ret(1, 1) = Ret(2, 2) = Ret(3, 3) = 1.0f;
+  return Ret;
+}
+
+inline float
+radToDeg(float A) {
+  return A * 180.0f / pi<float>();
+}
+
+inline float
+degToRad(float A) {
+  return A * pi<float>() / 180.0f;
+}
+
+template <typename Generator>
+Vec3
+randomAxis(Generator& RNG) {
+  std::uniform_real<float> AngleDist(0.0f, 2*pi<float>());
+  std::uniform_real<float> ZDist(-1.0f, 1.0f);
+  float Theta = AngleDist(RNG);
+  float Z = ZDist(RNG);
+  return {
+    sqrt(1-Z*Z)*cos(Theta),
+    sqrt(1-Z*Z)*sin(Theta),
+    Z
+  };
+}

File src/MathTypes.h

View file
+#pragma once
+
+struct Vec3 {
+  float X, Y, Z;
+};
+
+struct Quaternion {
+  float R;
+  Vec3 V;
+};
+
+struct Vec4 {
+  float X, Y, Z, W;
+
+  float&
+  operator () (size_t Index) {
+    if (Index == 0) return X;
+    if (Index == 1) return Y;
+    if (Index == 2) return Z;
+    return W;
+  }
+
+  float
+  operator () (size_t Index) const {
+    if (Index == 0) return X;
+    if (Index == 1) return Y;
+    if (Index == 2) return Z;
+    return W;
+  }
+};
+
+struct Mat3 {
+  Vec3 Col[3];
+};
+
+struct Mat4 {
+  Vec4 Col[4];
+
+  Vec4&
+  operator () (size_t Column) {
+    return Col[Column];
+  }
+
+  Vec4
+  operator () (size_t Column) const {
+    return Col[Column];
+  }
+
+  float&
+  operator () (size_t Row, size_t Column) {
+    return Col[Column](Row);
+  }
+
+  float
+  operator () (size_t Row, size_t Column) const {
+    return Col[Column](Row);
+  }
+};