From 05c9bcd4f8e2467294377904e648fce7d1381594 Mon Sep 17 00:00:00 2001 From: Walter Julius Hennecke Date: Sun, 22 Sep 2019 11:27:55 +0200 Subject: [PATCH] added new vector related functions --- code/common/Math.cpp | 26 +++++++ code/common/Math.h | 8 ++ code/common/Vector.cpp | 166 ++++++++++++++++++++++++++++++++++++++++- code/common/Vector.h | 33 +++++++- 4 files changed, 228 insertions(+), 5 deletions(-) create mode 100644 code/common/Math.cpp create mode 100644 code/common/Math.h diff --git a/code/common/Math.cpp b/code/common/Math.cpp new file mode 100644 index 0000000..c18127f --- /dev/null +++ b/code/common/Math.cpp @@ -0,0 +1,26 @@ +#include "Math.h" +#include "Random.h" +#include + +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(); + REQUIRE(common::rsqrt(f) == Approx(1.0f / sqrt(f)).epsilon(0.01)); + } +} diff --git a/code/common/Math.h b/code/common/Math.h new file mode 100644 index 0000000..44a8f74 --- /dev/null +++ b/code/common/Math.h @@ -0,0 +1,8 @@ +#pragma once + +namespace common { + +float rsqrt(float number); + +}; // namespace common + diff --git a/code/common/Vector.cpp b/code/common/Vector.cpp index b8c2dc3..ed2a268 100644 --- a/code/common/Vector.cpp +++ b/code/common/Vector.cpp @@ -1,5 +1,7 @@ #include "Vector.h" +#include "Math.h" #include "Random.h" +#include #include 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); @@ -219,4 +310,77 @@ TEST_CASE("vector_to_vector_distance", "[common::Vector]") { REQUIRE(common::distance({2, 2, 2}, {1, 1, 1}) == Approx(1.7320508075f)); REQUIRE(common::distance({33, 27, 1}, {11, -27, 2}) == Approx(58.3180932472f)); -} \ No newline at end of file +} + +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{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 +} diff --git a/code/common/Vector.h b/code/common/Vector.h index 078abc9..e2ff571 100644 --- a/code/common/Vector.h +++ b/code/common/Vector.h @@ -1,5 +1,6 @@ #pragma once +#include #include 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 &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