--- /dev/null
+#pragma once
+
+#include <cmath>
+#include <iostream>
+#include <sstream>
+#include <type_traits>
+#include "Vector.hxx"
+
+template <typename MT>
+class Quaternion {
+public:
+ // --- public data structurs ---
+
+ union {
+ struct {
+ MT s;
+ MT x;
+ MT y;
+ MT z;
+ };
+ MT sxyz[4];
+ } __attribute__((packed));
+
+ // --- constructors and deconstructors ---
+
+ Quaternion()
+ : sxyz{1, 0, 0, 0}
+ {
+ }
+
+ Quaternion(const MT& val)
+ : sxyz{val, val, val, val}
+ {
+ }
+
+ Quaternion(const MT& s, const MT& val)
+ : sxyz{s, val, val, val}
+ {
+ }
+
+ Quaternion(const MT& s, const MT& x, const MT& y, const MT& z)
+ : sxyz{s, x, y, z}
+ {
+ }
+
+ Quaternion(const MT& s, const Vector3<MT>& vec)
+ : sxyz{s, vec.x, vec.y, vec.z}
+ {
+ }
+
+ Quaternion(const Vector4<MT>& vec)
+ : sxyz{vec.w, vec.x, vec.y, vec.z}
+ {
+ }
+
+ Quaternion(const Quaternion& rhs)
+ : sxyz{rhs.s, rhs.x, rhs.y, rhs.z}
+ {
+ }
+
+ Quaternion(Quaternion&& rhs)
+ : sxyz{std::move(rhs.s), std::move(rhs.x), std::move(rhs.y), std::move(rhs.z)}
+ {
+ }
+
+ virtual ~Quaternion()
+ {
+ }
+
+ // --- default operators ---
+
+ Quaternion operator=(const Quaternion& rhs)
+ {
+ if (this != &rhs)
+ {
+ s = rhs.s;
+ x = rhs.x;
+ y = rhs.y;
+ z = rhs.z;
+ }
+
+ return *this;
+ }
+
+ Quaternion operator=(Quaternion&& rhs)
+ {
+ if (this != &rhs)
+ {
+ s = std::move(rhs.s);
+ x = std::move(rhs.x);
+ y = std::move(rhs.y);
+ z = std::move(rhs.z);
+ }
+
+ return *this;
+ }
+
+ bool operator==(const Quaternion& rhs) const
+ {
+ return s == rhs.s && x == rhs.x && y == rhs.y && z == rhs.z;
+ }
+
+ bool operator!=(const Quaternion& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ const MT operator[](const size_t& index) const
+ {
+ return sxyz[index];
+ }
+
+ MT& operator[](const size_t& index)
+ {
+ return sxyz[index];
+ }
+
+ Quaternion& operator~()
+ {
+ s = -s;
+ x = -x;
+ y = -y;
+ z = -z;
+
+ return *this;
+ }
+
+ Quaternion operator-() const
+ {
+ return {-s, -x, -y, -z};
+ }
+
+ // --- other operators ---
+
+ Quaternion& operator+=(const Quaternion& rhs)
+ {
+ s += rhs.s;
+ x += rhs.x;
+ y += rhs.y;
+ z += rhs.z;
+
+ return *this;
+ }
+
+ template <typename T>
+ Quaternion& operator+=(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ s += val;
+ x += val;
+ y += val;
+ z += val;
+
+ return *this;
+ }
+
+ Quaternion operator+(const Quaternion& rhs) const
+ {
+ return {s + rhs.s, x + rhs.x, y + rhs.y, z + rhs.z};
+ }
+
+ template <typename T>
+ Quaternion operator+(const T& val) const
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {s + val, x + val, y + val, z + val};
+ }
+
+ // ---
+
+ Quaternion& operator-=(const Quaternion& rhs)
+ {
+ s -= rhs.s;
+ x -= rhs.x;
+ y -= rhs.y;
+ z -= rhs.z;
+
+ return *this;
+ }
+
+ template <typename T>
+ Quaternion& operator-=(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ s -= val;
+ x -= val;
+ y -= val;
+ z -= val;
+
+ return *this;
+ }
+
+ Quaternion operator-(const Quaternion& rhs) const
+ {
+ return {s - rhs.s, x - rhs.x, y - rhs.y, z - rhs.z};
+ }
+
+ template <typename T>
+ Quaternion operator-(const T& val) const
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {s - val, x - val, y - val, z - val};
+ }
+
+ // ---
+
+ Quaternion& operator*=(const Quaternion& rhs)
+ {
+ *this = *this * rhs;
+
+ return *this;
+ }
+
+ template <typename T>
+ Quaternion& operator*=(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ s *= val;
+ x *= val;
+ y *= val;
+ z *= val;
+
+ return *this;
+ }
+
+ Quaternion operator*(const Quaternion& rhs) const
+ {
+ const MT cs = (z + x) * (rhs.x + rhs.y);
+ const MT cy = (s - y) * (rhs.s + rhs.z);
+ const MT cz = (s + y) * (rhs.s - rhs.z);
+ const MT cx = cs + cy + cz;
+ const MT cq = 0.5 * (cx + (z - x) * (rhs.x - rhs.y));
+
+ return {cq - cs + (z - y) * (rhs.y - rhs.z),
+ cq - cx + (x + s) * (rhs.x + rhs.s),
+ cq - cy + (s - x) * (rhs.y + rhs.z),
+ cq - cz + (z + y) * (rhs.s - rhs.x)};
+ }
+
+ template <typename T>
+ Quaternion operator*(const T& val) const
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {s * val, x * val, y * val, z * val};
+ }
+
+ // ---
+ // just to make it clear, there is no Q/Q division because quaternions are non-commutative
+ // (q1 * q2^-1 is not the same like q1^-1 * q2)
+
+ template <typename T>
+ Quaternion& operator/=(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ s /= val;
+ x /= val;
+ y /= val;
+ z /= val;
+
+ return *this;
+ }
+
+ template <typename T>
+ Quaternion operator/(const T& val) const
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {s / val, x / val, y / val, z / val};
+ }
+
+ // --- public methods ---
+
+ void conjugate()
+ {
+ sxyz = {s, -x, -y, -z};
+ }
+
+ Quaternion conjugated() const
+ {
+ return {s, -x, -y, -z};
+ }
+
+ bool isIdentity() const
+ {
+ return s == 1 && x == 0 && y == 0 && z == 0;
+ }
+
+ bool isNull() const
+ {
+ return s == 0 && x == 0 && y == 0 && z == 0;
+ }
+
+ MT length() const
+ {
+ return std::sqrt(s * s + x * x + y * y + z * z);
+ }
+
+ MT lengthSquared() const
+ {
+ return {s * s + x * x + y * y + z * z};
+ }
+
+ void normalize()
+ {
+ *this /= std::sqrt(s * s + x * x + y * y + z * z);
+ }
+
+ Quaternion normalized() const
+ {
+ return sxyz / std::sqrt(s * s + x * x + y * y + z * z);
+ }
+
+ Vector3<MT> rotatedVector3(const Vector3<MT>& vec) const
+ {
+ return (*this * Quaternion(0, vec) * conjugated()).vector3();
+ }
+
+ std::string string() const
+ {
+ std::stringstream result;
+
+ result << s << ", " << x << "," << y << ", " << z;
+
+ return result.str();
+ }
+
+ Vector3<MT> vector3() const
+ {
+ return {x, y, z};
+ }
+
+ Vector4<MT> vector4() const
+ {
+ return {x, y, z, s};
+ }
+
+ // --- static methods ---
+
+ static std::string toString(const Quaternion& quat)
+ {
+ std::stringstream result;
+
+ result << quat.s << ", " << quat.x << ", " << quat.y << ", " << quat.z;
+
+ return result.str();
+ }
+
+ static Vector3<MT> toVector3(const Quaternion& quat)
+ {
+ return {quat.x, quat.y, quat.z};
+ }
+
+ static Vector4<MT> toVector4(const Quaternion& quat)
+ {
+ return {quat.x, quat.y, quat.z, quat.s};
+ }
+};
+
+// --- global operators ---
+
+template <typename T>
+bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs)
+{
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return lhs.s == rhs.s && lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z;
+}
+
+template <typename T>
+bool operator!=(const Quaternion<T>& lhs, const Quaternion<T>& rhs)
+{
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return !(lhs == rhs);
+}
+
+template <typename T>
+std::ostream& operator<<(std::ostream& lhs, const Quaternion<T>& rhs)
+{
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ lhs << rhs.s << rhs.x << rhs.y << rhs.z;
+
+ return lhs;
+}
+
+template <typename T>
+std::istream& operator>>(std::istream& lhs, Quaternion<T>& rhs)
+{
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ lhs >> rhs.s >> rhs.x >> rhs.y >> rhs.z;
+
+ return lhs;
+}
+
+// ---
+
+template <typename T>
+Quaternion<T> operator+(const Quaternion<T>& lhs, const Quaternion<T>& rhs)
+{
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {lhs.s + rhs.s, lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z};
+}
+
+template <typename Quat, typename Val>
+Quaternion<Quat> operator+(const Quaternion<Quat>& quat, const Val& val)
+{
+ static_assert (std::is_integral<Quat>::value || std::is_floating_point<Quat>::value ||
+ std::is_integral<Val>::value || std::is_floating_point<Val>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {quat.s + val, quat.x + val, quat.y + val, quat.z + val};
+}
+
+template <typename Val, typename Quat>
+Quaternion<Quat> operator+(const Val& val, const Quaternion<Quat>& quat)
+{
+ static_assert (std::is_integral<Val>::value || std::is_floating_point<Val>::value ||
+ std::is_integral<Quat>::value || std::is_floating_point<Quat>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {val + quat.s, val + quat.x, val + quat.y, val + quat.z};
+}
+
+// ---
+
+template <typename T>
+Quaternion<T> operator-(const Quaternion<T>& lhs, const Quaternion<T>& rhs)
+{
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {lhs.s - rhs.s, lhs.x - rhs.x, lhs.y - rhs.y, lhs.x - rhs.y};
+}
+
+template <typename Quat, typename Val>
+Quaternion<Quat> operator-(const Quaternion<Quat>& quat, const Val& val)
+{
+ static_assert (std::is_integral<Quat>::value || std::is_floating_point<Quat>::value ||
+ std::is_integral<Val>::value || std::is_floating_point<Val>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {quat.s - val, quat.x - val, quat.y - val, quat.z - val};
+}
+
+template <typename Val, typename Quat>
+Quaternion<Quat> operator-(const Val& val, const Quaternion<Quat>& quat)
+{
+ static_assert (std::is_integral<Val>::value || std::is_floating_point<Val>::value ||
+ std::is_integral<Quat>::value || std::is_floating_point<Quat>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {val - quat.s, val - quat.x, val - quat.y, val - quat.z};
+}
+
+template <typename T>
+Quaternion<T> operator-(const Quaternion<T> rhs)
+{
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {-rhs.s, -rhs.x, -rhs.y, -rhs.z};
+}
+
+// ---
+
+template <typename T>
+Quaternion<T> operator*(const Quaternion<T>& lhs, const Quaternion<T>& rhs)
+{
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ const T cs = (lhs.z + lhs.x) * (rhs.x + rhs.y);
+ const T cy = (lhs.s - lhs.y) * (rhs.s + rhs.z);
+ const T cz = (lhs.s + lhs.y) * (rhs.s - rhs.z);
+ const T cx = cs + cy + cz;
+ const T cq = 0.5 * (cx + (lhs.z - lhs.x) * (rhs.x - rhs.y));
+}
+
+template <typename Quat, typename Val>
+Quaternion<Quat> operator*(const Quaternion<Quat>& quat, const Val& val)
+{
+ static_assert (std::is_integral<Quat>::value || std::is_floating_point<Quat>::value ||
+ std::is_integral<Val>::value || std::is_floating_point<Val>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {quat.s * val, quat.x * val, quat.y * val, quat.z * val};
+}
+
+template <typename Val, typename Quat>
+Quaternion<Quat> operator*(const Val& val, const Quaternion<Quat>& quat)
+{
+ static_assert (std::is_integral<Val>::value || std::is_floating_point<Val>::value ||
+ std::is_integral<Quat>::value || std::is_floating_point<Quat>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {val + quat.s, val * quat.x, val * quat.y, val * quat.z};
+}
+
+// ---
+
+template <typename Quat, typename Val>
+Quaternion<Quat>& operator/(const Quaternion<Quat>& quat, const Val& val)
+{
+ static_assert (std::is_integral<Quat>::value || std::is_floating_point<Quat>::value ||
+ std::is_integral<Val>::value || std::is_floating_point<Val>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {quat.s / val, quat.x / val, quat.y / val, quat.z / val};
+}
+
+template <typename Val, typename Quat>
+Quaternion<Quat>& operator/(const Val& val, const Quaternion<Quat>& quat)
+{
+ static_assert (std::is_integral<Val>::value || std::is_floating_point<Val>::value ||
+ std::is_integral<Quat>::value || std::is_floating_point<Quat>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {val / quat.s, val / quat.x, val / quat.y, val / quat.z};
+}
static_assert (std::is_integral<MT>::value || std::is_floating_point<MT>::value,
"ERROR: template parameter is not an integral or floating point type");
public:
- // --- data structures ---
+ // --- Vector2: data structures ---
union {
struct {
MT xy[2];
} __attribute__((packed));
- // --- constructors and deconstructors ---
+ // --- Vector2: constructors and deconstructors ---
Vector2()
: xy{0, 0}
{
}
- Vector2(const Vector2&& rhs)
+ Vector2(Vector2&& rhs)
: xy{std::move(rhs.x), std::move(rhs.y)}
{
}
{
}
- // --- default operators ---
+ // --- Vector2: default operators ---
Vector2& operator=(const Vector2& rhs)
{
return *this;
}
- Vector2& operator=(const Vector2&& rhs)
+ Vector2& operator=(Vector2&& rhs)
{
if (this != &rhs)
{
return {-x, -y};
}
- // --- other operators ---
+ // --- Vector2: other operators ---
Vector2& operator+=(const Vector2& rhs)
{
return {x / val, y / val};
}
- // --- public methods ---
+ // --- Vector2: public methods ---
MT distanceToLine(const Vector2& origin, const Vector2& direction) const
{
return {x, y, 0, 0};
}
- // --- static methods ---
+ // --- Vector2: static methods ---
static MT toDotProduct(const Vector2& vec1, const Vector2& vec2)
{
}
};
-// --- global operators ---
+// --- Vector2: global operators ---
template <typename T>
bool operator==(const Vector2<T>& lhs, const Vector2<T>& rhs)
static_assert (std::is_integral<MT>::value || std::is_floating_point<MT>::value,
"ERROR: template parameter is not an integral or floating point type");
public:
- // --- data structures ---
+ // --- Vector3: data structures ---
union {
struct {
MT xyz[3];
} __attribute__((packed));
- // --- constructors and deconstructors ---
+ // --- Vector3: constructors and deconstructors ---
Vector3()
: xyz{0, 0, 0}
{
}
- Vector3(const Vector3&& rhs)
+ Vector3(Vector3&& rhs)
: xyz{std::move(rhs.x), std::move(rhs.y), std::move(rhs.z)}
{
}
{
}
- // --- default operators ---
+ // --- Vector3: default operators ---
Vector3& operator=(const Vector3& rhs)
{
return *this;
}
- Vector3& operator=(const Vector3&& rhs)
+ Vector3& operator=(Vector3&& rhs)
{
if (this != &rhs)
{
return {-x, -y, -z};
}
- // --- other operators ---
+ // --- Vector3: other operators ---
Vector3& operator+=(const Vector3& rhs)
{
return {x / val, y / val, z / val};
}
- // --- public methods ---
+ // --- Vector3: public methods ---
Vector3 crossProduct(const Vector3& vec) const
{
return {x, y, z, 0};
}
- // --- static methods ---
+ // --- Vector3: static methods ---
static Vector3 toCrossProduct(const Vector3& vec1, const Vector3& vec2)
{
}
};
-// --- global operators ---
+// --- Vector3: global operators ---
template <typename T>
bool operator==(const Vector3<T>& lhs, const Vector3<T>& rhs)
static_assert (std::is_integral<MT>::value || std::is_floating_point<MT>::value,
"ERROR: template parameter is not an integral or floating point type");
public:
- // --- data structures ---
+ // --- Vector4: data structures ---
union {
struct {
MT xyzw[4];
} __attribute__((packed));
- // --- constructors and deconstructors ---
+ // --- Vector4: constructors and deconstructors ---
Vector4()
: xyzw{0, 0, 0, 0}
{
}
- Vector4(const Vector4&& rhs)
+ Vector4(Vector4&& rhs)
: xyzw{std::move(rhs.x), std::move(rhs.y), std::move(rhs.z), std::move(rhs.w)}
{
}
{
}
- // --- default operators ---
+ // --- Vector4: default operators ---
Vector4& operator=(const Vector4& rhs)
{
return *this;
}
- Vector4& operator=(const Vector4&& rhs)
+ Vector4& operator=(Vector4&& rhs)
{
if (this != &rhs)
{
return {-x, -y, -z, -w};
}
- // --- other operators ---
+ // --- Vector4: other operators ---
Vector4& operator+=(const Vector4& rhs)
{
return {x / val, y / val, z / val, w / val};
}
- // --- public methods ---
+ // --- Vector4: public methods ---
MT dotProduct(const Vector4& vec) const
{
return {x, y, z};
}
- // --- static methods ---
+ // --- Vector4: static methods ---
static MT toDotProduct(const Vector4& vec1, const Vector4& vec2)
{
}
};
-// --- global operators ---
+// --- Vector4: global operators ---
template <typename T>
bool operator==(const Vector4<T>& lhs, const Vector4<T>& rhs)