added new vector related functions

code/common/Math.cpp

@@ -0,0 +1,26 @@
+#include "Math.h"
+#include "Random.h"
+#include <catch2/catch.hpp>
+
+namespace common {
+
+float rsqrt(float number) {
+  float x2 = number * 0.5F;
+  float y = number;
+  long i = *(long *)&y;
+  i = 0x5f3759df - (i >> 1);
+  y = *(float *)&i;
+  y = y * (1.5F - (x2 * y * y));
+  return y;
+}
+
+} // namespace common
+
+TEST_CASE("math_rsqrt") {
+  REQUIRE(common::rsqrt(4) == Approx(0.5f).epsilon(0.01));
+
+  for (auto i = 0; i < 100; i++) {
+    const auto f = common::getRandom<float>();
+    REQUIRE(common::rsqrt(f) == Approx(1.0f / sqrt(f)).epsilon(0.01));
+  }
+}

code/common/Math.h

@@ -0,0 +1,8 @@
+#pragma once
+
+namespace common {
+
+float rsqrt(float number);
+
+}; // namespace common
+

code/common/Vector.cpp

@@ -1,5 +1,7 @@
 #include "Vector.h"
+#include "Math.h"
 #include "Random.h"
+#include <base_game/q_math.h>
 #include <catch2/catch.hpp>
 
 namespace common {
@@ -17,6 +19,95 @@ constexpr float Vector::length_squared() const {
   return dotProduct(*this, *this);
 }
 
+float Vector::normalize() {
+  const auto len = length();
+
+  if (len < 0 || len > 0) {
+    const auto ilen = 1 / len;
+    x_ *= ilen;
+    y_ *= ilen;
+    z_ *= ilen;
+  }
+
+  return len;
+}
+
+Vector Vector::normalized() const {
+  auto result = *this;
+  result.normalize();
+  return result;
+}
+
+void Vector::normalizeFast() {
+  const auto ilen = common::rsqrt(length_squared());
+  x_ *= ilen;
+  y_ *= ilen;
+  z_ *= ilen;
+}
+
+Vector Vector::normalizedFast() const {
+  auto result = *this;
+  result.normalizeFast();
+  return result;
+}
+
+constexpr void Vector::invert() {
+  x_ = -x_;
+  y_ = -y_;
+  z_ = -z_;
+}
+
+Vector Vector::inverted() const {
+  auto result = *this;
+  result.invert();
+  return result;
+}
+
+float distance(const Vector &a, const Vector &b) {
+  return sqrt(dotProduct(b - a, b - a));
+}
+
+constexpr float distance_squared(const Vector &a, const Vector &b) {
+  return dotProduct(b - a, b - a);
+}
+
+float normalize2(const Vector &v, Vector &out) {
+  const auto len = v.length();
+
+  if (len < 0 || len > 0) {
+    const auto ilen = 1 / len;
+    out.x_ = v.x_ * ilen;
+    out.y_ = v.y_ * ilen;
+    out.z_ = v.z_ * ilen;
+  } else {
+    out.clear();
+  }
+
+  return len;
+}
+
+std::int32_t dirToByte(const Vector &dir) {
+  auto bestd = 0.0f;
+  auto best = 0;
+  for (auto i = 0; i < NUMVERTEXNORMALS; i++) {
+    auto d = dotProduct(dir, bytedirs[i]);
+    if (d > bestd) {
+      bestd = d;
+      best = i;
+    }
+  }
+
+  return best;
+}
+
+Vector byteToDir(std::int32_t b) {
+  if (b < 0 || b >= NUMVERTEXNORMALS) {
+    return vec3_origin;
+  }
+
+  return bytedirs[b];
+}
+
 } // namespace common
 
 static_assert(common::Vector().x_ == 0.0f);
@@ -220,3 +311,76 @@ TEST_CASE("vector_to_vector_distance", "[common::Vector]") {
   REQUIRE(common::distance({33, 27, 1}, {11, -27, 2}) ==
           Approx(58.3180932472f));
 }
+
+static_assert(common::crossProduct({1, 2, 3}, {4, 5, 6}).x_ == -3.0f);
+static_assert(common::crossProduct({1, 2, 3}, {4, 5, 6}).y_ == 6.0f);
+static_assert(common::crossProduct({1, 2, 3}, {4, 5, 6}).z_ == -3.0f);
+
+TEST_CASE("vector_cross_product", "[common::Vector]") {
+  REQUIRE(common::crossProduct({1, 2, 3}, {4, 5, 6}).x_ == -3.0f);
+  REQUIRE(common::crossProduct({1, 2, 3}, {4, 5, 6}).y_ == 6.0f);
+  REQUIRE(common::crossProduct({1, 2, 3}, {4, 5, 6}).z_ == -3.0f);
+}
+
+TEST_CASE("vector_normalize", "[common::Vector]") {
+  auto vec = common::Vector{4, -1, 7};
+  auto res = vec.normalize();
+  REQUIRE(vec.length() == Approx(1.0f));
+  REQUIRE(res == Approx(8.1240384046f));
+}
+
+TEST_CASE("vector_normalize_fast", "[common::Vector]") {
+  auto vec = common::Vector{4, -1, 7};
+  vec.normalizeFast();
+  REQUIRE(vec.length() == Approx(1.0f).epsilon(0.01));
+}
+
+TEST_CASE("vector_normalized", "[common::Vector]") {
+  auto vec = common::Vector{4, -1, 7};
+  REQUIRE(vec.normalized().length() == Approx(1.0f));
+}
+
+TEST_CASE("vector_normalized_fast", "[common::Vector]") {
+  auto vec = common::Vector{4, -1, 7};
+  REQUIRE(vec.normalizedFast().length() == Approx(1.0f).epsilon(0.01));
+}
+
+TEST_CASE("vector_normalize2", "[common::Vector]") {
+  auto vec = common::Vector{4, -1, 7};
+  auto vecr = common::Vector{};
+  auto res = common::normalize2(vec, vecr);
+  REQUIRE(res == Approx(8.1240384046f));
+  REQUIRE(vecr.length() == Approx(1.0f));
+}
+
+TEST_CASE("vector_invert", "[common::Vector]") {
+  auto vec = common::Vector{1, -1, 1};
+  vec.invert();
+  REQUIRE(vec.x_ == Approx(-1.0f));
+  REQUIRE(vec.y_ == Approx(1.0f));
+  REQUIRE(vec.z_ == Approx(-1.0f));
+}
+
+TEST_CASE("vector_inverted", "[common::Vector]") {
+  REQUIRE(common::Vector{1, -1, 1}.inverted().x_ == Approx(-1.0f));
+  REQUIRE(common::Vector{1, -1, 1}.inverted().y_ == Approx(1.0f));
+  REQUIRE(common::Vector{1, -1, 1}.inverted().z_ == Approx(-1.0f));
+}
+
+TEST_CASE("vector_rotate", "[common::Vector]") {
+  auto vec = common::Vector{15, 2, -4};
+  auto rotated = common::rotate(
+      vec, std::array<common::Vector, 3>{
+               common::Vector{4.5, 3, -6}, {1, 0, 7.6}, {1.3, -65, 7.5}});
+  REQUIRE(rotated.x_ == Approx(97.5));
+  REQUIRE(rotated.y_ == Approx(-15.4));
+  REQUIRE(rotated.z_ == Approx(-140.5));
+}
+
+TEST_CASE("byte_to_dir", "[common::Vector]") {
+  // TODO write unit test for common::byteToDir
+}
+
+TEST_CASE("dir_to_byte", "[common::Vector]") {
+  // TODO write unit test for common::dirToByte
+}

code/common/Vector.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <array>
 #include <base_game/q_shared.h>
 
 namespace common {
@@ -24,6 +25,18 @@ public:
 
   [[nodiscard]] constexpr float length_squared() const;
 
+  float normalize();
+
+  void normalizeFast();
+
+  [[nodiscard]] Vector normalized() const;
+
+  [[nodiscard]] Vector normalizedFast() const;
+
+  constexpr void invert();
+
+  [[nodiscard]] Vector inverted() const;
+
   union {
     vec3_t values{};
     struct {
@@ -62,12 +75,24 @@ inline constexpr Vector operator-(const Vector &a) {
   return {-a.x_, -a.y_, -a.z_};
 }
 
-constexpr float distance(const Vector &a, const Vector &b) {
-  return sqrt(dotProduct(b - a, b - a));
+float distance(const Vector &a, const Vector &b);
+
+constexpr float distance_squared(const Vector &a, const Vector &b);
+
+inline constexpr Vector crossProduct(const Vector &v1, const Vector &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_};
 }
 
-constexpr float distance_squared(const Vector &a, const Vector &b) {
-  return dotProduct(b - a, b - a);
+float normalize2(const Vector &v, Vector &out);
+
+constexpr Vector rotate(const Vector &v, const std::array<Vector, 3> &matrix) {
+  return {dotProduct(v, matrix[0]), dotProduct(v, matrix[1]),
+          dotProduct(v, matrix[2])};
 }
 
+std::int32_t dirToByte(const Vector& dir);
+
+Vector byteToDir(std::int32_t b);
+
 } // namespace common