From 8410394d4bd90f8783b68ab064b13d838e29d6a1 Mon Sep 17 00:00:00 2001
From: Phillip Stephens <antidote.crk@gmail.com>
Date: Fri, 13 May 2022 23:46:19 -0700
Subject: [PATCH] Add toRGBA and toRGB5A3 to CColor

---
 include/zeus/CColor.hpp | 37 ++++++++++++++++++++++++++++---------
 include/zeus/CPlane.hpp |  2 +-
 include/zeus/Math.hpp   |  4 ++--
 src/Math.cpp            |  4 ++--
 4 files changed, 33 insertions(+), 14 deletions(-)

diff --git a/include/zeus/CColor.hpp b/include/zeus/CColor.hpp
index b8814b4..c25cd0b 100644
--- a/include/zeus/CColor.hpp
+++ b/include/zeus/CColor.hpp
@@ -43,15 +43,12 @@ public:
 
   constexpr CColor(float r, float g, float b, float a = 1.0f) : mSimd(r, g, b, a) {}
 
-  constexpr CColor(Comp32 rgba) : mSimd(((COLOR(rgba) >> 0) & 0xff) * OneOver255,
-                                        ((COLOR(rgba) >> 8) & 0xff) * OneOver255,
-                                        ((COLOR(rgba) >> 16) & 0xff) * OneOver255,
-                                        ((COLOR(rgba) >> 24) & 0xff) * OneOver255) {}
+  constexpr CColor(Comp32 rgba)
+  : mSimd(((COLOR(rgba) >> 0) & 0xff) * OneOver255, ((COLOR(rgba) >> 8) & 0xff) * OneOver255,
+          ((COLOR(rgba) >> 16) & 0xff) * OneOver255, ((COLOR(rgba) >> 24) & 0xff) * OneOver255) {}
 
-  constexpr CColor(const Comp8* rgba) : mSimd(rgba[0] * OneOver255,
-                                              rgba[1] * OneOver255,
-                                              rgba[2] * OneOver255,
-                                              rgba[3] * OneOver255) {}
+  constexpr CColor(const Comp8* rgba)
+  : mSimd(rgba[0] * OneOver255, rgba[1] * OneOver255, rgba[2] * OneOver255, rgba[3] * OneOver255) {}
 
   constexpr CColor(const CVector4f& other) : mSimd(other.mSimd) {}
 
@@ -248,6 +245,28 @@ public:
     ao = Comp8(a() * 255);
   }
 
+  Comp32 toRGBA() const {
+    RGBA32 ret;
+    ret.r = r() * 255;
+    ret.g = g() * 255;
+    ret.b = b() * 255;
+    ret.a = a() * 255;
+    return ret.rgba;
+  }
+
+  [[nodiscard]] unsigned short toRGB5A3() const {
+    Comp8 r;
+    Comp8 g;
+    Comp8 b;
+    Comp8 a;
+    toRGBA8(r, g, b, a);
+    if (a == 255) {
+      return static_cast<unsigned short>((r & 0xf8) << 7 | (g & 0xf8) << 2 | b >> 3 | 0x8000);
+    }
+
+    return static_cast<unsigned short>((r & 0xf0) << 4 | (g & 0xf0) | b >> 4 | (a & 0xe0) << 7);
+  }
+
   /**
    * @brief Assigns rgba from hsv
    * @param h[0-1] The hue percentage of the color.
@@ -340,4 +359,4 @@ struct hash<zeus::CColor> {
     return ret;
   }
 };
-}
+} // namespace std
diff --git a/include/zeus/CPlane.hpp b/include/zeus/CPlane.hpp
index c45f884..23bd732 100644
--- a/include/zeus/CPlane.hpp
+++ b/include/zeus/CPlane.hpp
@@ -38,7 +38,7 @@ public:
     mSimd[3] = nd * mag;
   }
 
-  [[nodiscard]] float pointToPlaneDist(const CVector3f& pos) const { return pos.dot(normal()) - d(); }
+  [[nodiscard]] float pointToPlaneDist(const CVector3f& pos) const { return normal().dot(pos) - d(); }
 
   [[nodiscard]] bool rayPlaneIntersection(const CVector3f& from, const CVector3f& to, CVector3f& point) const;
 
diff --git a/include/zeus/Math.hpp b/include/zeus/Math.hpp
index 3fdefd8..e551ac4 100644
--- a/include/zeus/Math.hpp
+++ b/include/zeus/Math.hpp
@@ -184,10 +184,10 @@ template <typename E>
 [[nodiscard]] bool close_enough(const CVector2f& a, const CVector2f& b, float epsilon = FLT_EPSILON);
 
 [[nodiscard]] inline bool close_enough(float a, float b, double epsilon = FLT_EPSILON) {
-  return std::fabs(a - b) < epsilon;
+  return std::fabs(a - b) <= epsilon;
 }
 
 [[nodiscard]] inline bool close_enough(double a, double b, double epsilon = FLT_EPSILON) {
-  return std::fabs(a - b) < epsilon;
+  return std::fabs(a - b) <= epsilon;
 }
 } // namespace zeus
diff --git a/src/Math.cpp b/src/Math.cpp
index 42f4ec5..3399f79 100644
--- a/src/Math.cpp
+++ b/src/Math.cpp
@@ -272,11 +272,11 @@ CVector3f baryToWorld(const CVector3f& p0, const CVector3f& p1, const CVector3f&
 }
 
 bool close_enough(const CVector3f& a, const CVector3f& b, float epsilon) {
-  return std::fabs(a.x() - b.x()) < epsilon && std::fabs(a.y() - b.y()) < epsilon && std::fabs(a.z() - b.z()) < epsilon;
+  return std::fabs(a.x() - b.x()) <= epsilon && std::fabs(a.y() - b.y()) <= epsilon && std::fabs(a.z() - b.z()) <= epsilon;
 }
 
 bool close_enough(const CVector2f& a, const CVector2f& b, float epsilon) {
-  return std::fabs(a.x() - b.x()) < epsilon && std::fabs(a.y() - b.y()) < epsilon;
+  return std::fabs(a.x() - b.x()) <= epsilon && std::fabs(a.y() - b.y()) <= epsilon;
 }
 
 template <>