diff --git a/CMakeLists.txt b/CMakeLists.txt index 390e044..6689614 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,10 +5,13 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) add_library(vectormatrix + VectorMatrixLib/Vector2f.cpp VectorMatrixLib/Vector3f.cpp VectorMatrixLib/Vector4f.cpp + VectorMatrixLib/Matrix2f.cpp VectorMatrixLib/Matrix3f.cpp VectorMatrixLib/Matrix4f.cpp + VectorMatrixLib/Quaternionf.cpp ) target_include_directories(vectormatrix PUBLIC VectorMatrixLib) diff --git a/VectorMatrixLib/Matrix2f.cpp b/VectorMatrixLib/Matrix2f.cpp new file mode 100644 index 0000000..88b4378 --- /dev/null +++ b/VectorMatrixLib/Matrix2f.cpp @@ -0,0 +1,247 @@ +// +// MIT License +// Copyright(c) 2021 - 2026 Olivier BARRY +// +// This file is part of the C++ Aventura Project +// +// VectorMatrix Math Library +// + +#include +#include +#include "Matrix2f.h" +#include "Vector2f.h" +#include "MathTools.h" +#include "Exceptions.h" +#include "GaussJordanSolver.h" + +namespace vectormatrix +{ + Matrix2f::Matrix2f() + { + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + array_[i][j] = 0; + } + + Matrix2f::Matrix2f(float a) + { + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + array_[i][j] = a; + } + + Matrix2f::Matrix2f(const float array[SIZE2][SIZE2]) + { + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + array_[i][j] = array[i][j]; + } + + Matrix2f Matrix2f::operator+(const Matrix2f& m) const + { + Matrix2f r; + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + r.array_[i][j] = array_[i][j] + m.array_[i][j]; + return r; + } + + Matrix2f& Matrix2f::operator+=(const Matrix2f& m) + { + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + array_[i][j] += m.array_[i][j]; + return *this; + } + + Matrix2f Matrix2f::operator-(const Matrix2f& m) const + { + Matrix2f r; + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + r.array_[i][j] = array_[i][j] - m.array_[i][j]; + return r; + } + + Matrix2f& Matrix2f::operator-=(const Matrix2f& m) + { + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + array_[i][j] -= m.array_[i][j]; + return *this; + } + + Matrix2f Matrix2f::operator*(const Matrix2f& m) const + { + Matrix2f r; + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + for (int k = 0; k < SIZE2; k++) + r.array_[i][j] += array_[i][k] * m.array_[k][j]; + return r; + } + + Matrix2f& Matrix2f::operator*=(const Matrix2f& m) + { + *this = *this * m; + return *this; + } + + Matrix2f Matrix2f::operator*(float a) const + { + Matrix2f r; + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + r.array_[i][j] = array_[i][j] * a; + return r; + } + + Matrix2f& Matrix2f::operator*=(float a) + { + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + array_[i][j] *= a; + return *this; + } + + Vector2f Matrix2f::operator*(const Vector2f& v) const + { + return v * *this; + } + + bool Matrix2f::operator==(const Matrix2f& m) const + { + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + if (!MathTools::equals(array_[i][j], m.array_[i][j])) return false; + return true; + } + + bool Matrix2f::operator!=(const Matrix2f& m) const + { + return !(*this == m); + } + + float Matrix2f::get(int x, int y) const + { + return array_[x][y]; + } + + void Matrix2f::set(int x, int y, float a) + { + array_[x][y] = a; + } + + Matrix2f Matrix2f::identity() + { + Matrix2f r; + r.setDiagonal(1); + return r; + } + + void Matrix2f::setDiagonal(float v) + { + for (int i = 0; i < SIZE2; i++) + array_[i][i] = v; + } + + Vector2f Matrix2f::getRow(int r) const + { + if (r < 0 || r >= SIZE2) throw IndexOutOfBoundException("Index out of bound while getting Row (" + std::to_string(r) + ") of Matrix2f"); + return Vector2f(array_[r][0], array_[r][1]); + } + + Vector2f Matrix2f::getColumn(int c) const + { + if (c < 0 || c >= SIZE2) throw IndexOutOfBoundException("Index out of bound while getting Column (" + std::to_string(c) + ") of Matrix2f"); + return Vector2f(array_[0][c], array_[1][c]); + } + + void Matrix2f::setRow(int r, const Vector2f& v) + { + if (r < 0 || r >= SIZE2) throw IndexOutOfBoundException("Index out of bound while setting Row (" + std::to_string(r) + ") of Matrix2f"); + for (int j = 0; j < SIZE2; j++) + array_[r][j] = v.get(j); + } + + void Matrix2f::setColumn(int c, const Vector2f& v) + { + if (c < 0 || c >= SIZE2) throw IndexOutOfBoundException("Index out of bound while setting Column (" + std::to_string(c) + ") of Matrix2f"); + for (int i = 0; i < SIZE2; i++) + array_[i][c] = v.get(i); + } + + std::vector> Matrix2f::getArray() const + { + std::vector> a(SIZE2, std::vector(SIZE2)); + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + a[i][j] = array_[i][j]; + return a; + } + + void Matrix2f::setArray(const std::vector>& a) + { + if (a.size() != SIZE2) throw MatrixArrayWrongSizeException("Wrong array row size (" + std::to_string(a.size()) + ") while setting Matrix2f from array"); + for (int i = 0; i < SIZE2; i++) + if (a[i].size() != SIZE2) throw MatrixArrayWrongSizeException("Wrong array column size (" + std::to_string(a[i].size()) + ") while setting Matrix2f from array"); + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + array_[i][j] = a[i][j]; + } + + float Matrix2f::trace() const + { + float t = 0; + for (int i = 0; i < SIZE2; i++) + t += array_[i][i]; + return t; + } + + bool Matrix2f::isIdentity() const + { + return *this == identity(); + } + + float Matrix2f::determinant() const + { + return array_[0][0] * array_[1][1] - array_[0][1] * array_[1][0]; + } + + Matrix2f Matrix2f::transpose() const + { + Matrix2f r; + for (int i = 0; i < SIZE2; i++) + for (int j = 0; j < SIZE2; j++) + r.array_[i][j] = array_[j][i]; + return r; + } + + Matrix2f& Matrix2f::transposeEquals() + { + *this = transpose(); + return *this; + } + + Matrix2f Matrix2f::inverse() const + { + Matrix2f r; + GaussJordanSolver::invert(array_, r.array_, EPSILON); + return r; + } + + std::ostream& operator<<(std::ostream& strm, const Matrix2f& m) + { + strm << "Matrix2f(" << std::endl; + for (int i = 0; i < Matrix2f::SIZE2; i++) + { + for (int j = 0; j < Matrix2f::SIZE2; j++) + { + strm << m.array_[i][j] << " "; + } + strm << std::endl; + } + return strm << ")"; + } +} diff --git a/VectorMatrixLib/Matrix2f.h b/VectorMatrixLib/Matrix2f.h new file mode 100644 index 0000000..85ebc56 --- /dev/null +++ b/VectorMatrixLib/Matrix2f.h @@ -0,0 +1,75 @@ +// +// MIT License +// Copyright(c) 2021 - 2026 Olivier BARRY +// +// This file is part of the C++ Aventura Project +// +// VectorMatrix Math Library +// + +#ifndef MATRIX2F_H +#define MATRIX2F_H + +#include +#include +#include "Vector2f.h" + +namespace vectormatrix +{ + class Matrix2f + { + public: + // Constants + static const int SIZE2 = 2; + + // Identity matrix (a new copy is returned each time) + static Matrix2f identity(); + + // Constructors + Matrix2f(); + explicit Matrix2f(float a); + explicit Matrix2f(const float array[SIZE2][SIZE2]); + + // Operators + Matrix2f operator+(const Matrix2f& m) const; + Matrix2f& operator+=(const Matrix2f& m); + Matrix2f operator-(const Matrix2f& m) const; + Matrix2f& operator-=(const Matrix2f& m); + Matrix2f operator*(const Matrix2f& m) const; + Matrix2f& operator*=(const Matrix2f& m); + Matrix2f operator*(float a) const; + Matrix2f& operator*=(float a); + // W = A.V + Vector2f operator*(const Vector2f& v) const; + // Equality within EPSILON tolerance + bool operator==(const Matrix2f& m) const; + bool operator!=(const Matrix2f& m) const; + + // Getters and Setters + float get(int x, int y) const; + void set(int x, int y, float a); + void setDiagonal(float v); + // Rows and columns (throw IndexOutOfBoundException if the index is out of bound) + Vector2f getRow(int r) const; + Vector2f getColumn(int c) const; + void setRow(int r, const Vector2f& v); + void setColumn(int c, const Vector2f& v); + // Copy of the elements, and set from an array (throws MatrixArrayWrongSizeException if not 2x2) + std::vector> getArray() const; + void setArray(const std::vector>& a); + // Other methods + float trace() const; + bool isIdentity() const; + float determinant() const; + Matrix2f transpose() const; + Matrix2f& transposeEquals(); + // Throws NotInvertibleMatrixException if this matrix is singular + Matrix2f inverse() const; + + private: + float array_[SIZE2][SIZE2]; + friend std::ostream& operator<<(std::ostream&, const Matrix2f&); + }; +} + +#endif // MATRIX2F_H diff --git a/VectorMatrixLib/Quaternionf.cpp b/VectorMatrixLib/Quaternionf.cpp new file mode 100644 index 0000000..a547552 --- /dev/null +++ b/VectorMatrixLib/Quaternionf.cpp @@ -0,0 +1,315 @@ +// +// MIT License +// Copyright(c) 2021 - 2026 Olivier BARRY +// +// This file is part of the C++ Aventura Project +// +// VectorMatrix Math Library +// + +#include +#include +#include +#include "Quaternionf.h" +#include "Matrix3f.h" +#include "Matrix4f.h" +#include "Constants.h" +#include "Exceptions.h" +#include "MathTools.h" + +namespace vectormatrix +{ + Quaternionf::Quaternionf() : x(0), y(0), z(0), w(1) + { + } + + Quaternionf::Quaternionf(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) + { + } + + Quaternionf::Quaternionf(const Vector3f& axis, float angleRadians) + { + Vector3f a(axis); + a.normalize(); + float half = angleRadians / 2; + float sinHalf = std::sin(half); + x = a.getX() * sinHalf; + y = a.getY() * sinHalf; + z = a.getZ() * sinHalf; + w = std::cos(half); + } + + Quaternionf::Quaternionf(const Matrix3f& m) + { + initFromRotationMatrix( + m.get(0, 0), m.get(0, 1), m.get(0, 2), + m.get(1, 0), m.get(1, 1), m.get(1, 2), + m.get(2, 0), m.get(2, 1), m.get(2, 2)); + } + + Quaternionf::Quaternionf(const Matrix4f& m) + { + initFromRotationMatrix( + m.get(0, 0), m.get(0, 1), m.get(0, 2), + m.get(1, 0), m.get(1, 1), m.get(1, 2), + m.get(2, 0), m.get(2, 1), m.get(2, 2)); + } + + void Quaternionf::initFromRotationMatrix(float m00, float m01, float m02, + float m10, float m11, float m12, + float m20, float m21, float m22) + { + float trace = m00 + m11 + m22; + if (trace > 0) + { + float s = std::sqrt(trace + 1.0f) * 2; // s = 4*w + w = 0.25f * s; + x = (m21 - m12) / s; + y = (m02 - m20) / s; + z = (m10 - m01) / s; + } + else if (m00 > m11 && m00 > m22) + { + float s = std::sqrt(1.0f + m00 - m11 - m22) * 2; // s = 4*x + x = 0.25f * s; + y = (m01 + m10) / s; + z = (m02 + m20) / s; + w = (m21 - m12) / s; + } + else if (m11 > m22) + { + float s = std::sqrt(1.0f + m11 - m00 - m22) * 2; // s = 4*y + y = 0.25f * s; + x = (m01 + m10) / s; + z = (m12 + m21) / s; + w = (m02 - m20) / s; + } + else + { + float s = std::sqrt(1.0f + m22 - m00 - m11) * 2; // s = 4*z + z = 0.25f * s; + x = (m02 + m20) / s; + y = (m12 + m21) / s; + w = (m10 - m01) / s; + } + } + + Quaternionf Quaternionf::operator*(const Quaternionf& q) const + { + float rw = w * q.w - x * q.x - y * q.y - z * q.z; + float rx = w * q.x + x * q.w + y * q.z - z * q.y; + float ry = w * q.y - x * q.z + y * q.w + z * q.x; + float rz = w * q.z + x * q.y - y * q.x + z * q.w; + return Quaternionf(rx, ry, rz, rw); + } + + Quaternionf& Quaternionf::operator*=(const Quaternionf& q) + { + *this = *this * q; + return *this; + } + + float Quaternionf::dot(const Quaternionf& q) const + { + return x * q.x + y * q.y + z * q.z + w * q.w; + } + + bool Quaternionf::operator==(const Quaternionf& q) const + { + return MathTools::equals(x, q.x) && MathTools::equals(y, q.y) && MathTools::equals(z, q.z) && MathTools::equals(w, q.w); + } + + bool Quaternionf::operator!=(const Quaternionf& q) const + { + return !(*this == q); + } + + float Quaternionf::get(int i) const + { + switch (i) { + case 0: + return x; + case 1: + return y; + case 2: + return z; + case 3: + return w; + default: + throw IndexOutOfBoundException("Index out of bound while getting coordinate (" + std::to_string(i) + ") of Quaternionf"); + } + } + + float Quaternionf::getX() const + { + return x; + } + + float Quaternionf::getY() const + { + return y; + } + + float Quaternionf::getZ() const + { + return z; + } + + float Quaternionf::getW() const + { + return w; + } + + void Quaternionf::set(int i, float v) + { + switch (i) { + case 0: + x = v; + break; + case 1: + y = v; + break; + case 2: + z = v; + break; + case 3: + w = v; + break; + default: + throw IndexOutOfBoundException("Index out of bound while setting coordinate (" + std::to_string(i) + ") of Quaternionf"); + } + } + + void Quaternionf::setX(float v) + { + x = v; + } + + void Quaternionf::setY(float v) + { + y = v; + } + + void Quaternionf::setZ(float v) + { + z = v; + } + + void Quaternionf::setW(float v) + { + w = v; + } + + float Quaternionf::length() const + { + return std::sqrt(lengthSquared()); + } + + float Quaternionf::lengthSquared() const + { + return x * x + y * y + z * z + w * w; + } + + Quaternionf& Quaternionf::normalize() + { + float length = this->length(); + x /= length; + y /= length; + z /= length; + w /= length; + return *this; + } + + Quaternionf Quaternionf::conjugate() const + { + return Quaternionf(-x, -y, -z, w); + } + + Quaternionf Quaternionf::inverse() const + { + float ls = lengthSquared(); + return Quaternionf(-x / ls, -y / ls, -z / ls, w / ls); + } + + Matrix3f Quaternionf::toMatrix3() const + { + float xx = x * x, yy = y * y, zz = z * z; + float xy = x * y, xz = x * z, yz = y * z; + float wx = w * x, wy = w * y, wz = w * z; + float a[3][3] = { + { 1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy) }, + { 2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx) }, + { 2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy) } + }; + return Matrix3f(a); + } + + Matrix4f Quaternionf::toMatrix4() const + { + Matrix4f r(toMatrix3()); + r.set(3, 3, 1); + return r; + } + + float Quaternionf::toAxisAngle(Vector3f& axis) const + { + // Guard w slightly past +/-1 (float rounding on a quaternion that is only approximately + // unit length) so acos() never receives an out-of-domain argument and returns NaN + float cw = w; + if (cw > 1.0f) cw = 1.0f; + if (cw < -1.0f) cw = -1.0f; + float angle = 2 * std::acos(cw); + float sinHalf = std::sqrt(1 - cw * cw); + if (sinHalf < EPSILON) + { + // No meaningful axis for a null (or near-null) rotation + axis.set(1, 0, 0); + } + else + { + axis.set(x / sinHalf, y / sinHalf, z / sinHalf); + } + return angle; + } + + // static member function (see declaration) + Quaternionf Quaternionf::slerp(const Quaternionf& q1, const Quaternionf& q2, float t) + { + float cosOmega = q1.dot(q2); + float x2 = q2.x, y2 = q2.y, z2 = q2.z, w2 = q2.w; + if (cosOmega < 0) + { + // q2 and -q2 represent the same rotation; negate to take the shorter path + cosOmega = -cosOmega; + x2 = -x2; y2 = -y2; z2 = -z2; w2 = -w2; + } + + float scale0, scale1; + if (cosOmega > 1.0f - EPSILON) + { + // q1 and q2 are almost identical: sin(omega) would be ~0, fall back to a plain lerp + scale0 = 1.0f - t; + scale1 = t; + } + else + { + float omega = std::acos(cosOmega); + float sinOmega = std::sin(omega); + scale0 = std::sin((1 - t) * omega) / sinOmega; + scale1 = std::sin(t * omega) / sinOmega; + } + + Quaternionf result( + scale0 * q1.x + scale1 * x2, + scale0 * q1.y + scale1 * y2, + scale0 * q1.z + scale1 * z2, + scale0 * q1.w + scale1 * w2); + result.normalize(); + return result; + } + + std::ostream& operator<<(std::ostream& strm, const Quaternionf& q) + { + return strm << "Quaternionf [" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "]"; + } +} diff --git a/VectorMatrixLib/Quaternionf.h b/VectorMatrixLib/Quaternionf.h new file mode 100644 index 0000000..50ee780 --- /dev/null +++ b/VectorMatrixLib/Quaternionf.h @@ -0,0 +1,87 @@ +// +// MIT License +// Copyright(c) 2021 - 2026 Olivier BARRY +// +// This file is part of the C++ Aventura Project +// +// VectorMatrix Math Library +// + +#ifndef QUATERNIONF_H +#define QUATERNIONF_H + +#include +#include "Vector3f.h" + +namespace vectormatrix +{ + class Matrix3f; + class Matrix4f; + + // Quaternionf q = w + xi + yj + zk, used to represent rotations (same as Aventura Quaternionf) + class Quaternionf + { + public: + + // Constructors + // Identity quaternion (0, 0, 0, 1): no rotation + Quaternionf(); + Quaternionf(float x, float y, float z, float w); + // Rotation of angleRadians around axis (the axis is normalized) + Quaternionf(const Vector3f& axis, float angleRadians); + // From a rotation matrix (upper-left 3x3 part for a Matrix4f) + explicit Quaternionf(const Matrix3f& m); + explicit Quaternionf(const Matrix4f& m); + + // Operators + // Hamilton product + Quaternionf operator*(const Quaternionf& q) const; + Quaternionf& operator*=(const Quaternionf& q); + float dot(const Quaternionf& q) const; + // Equality within EPSILON tolerance + bool operator==(const Quaternionf& q) const; + bool operator!=(const Quaternionf& q) const; + + // getter and setter (get and set throw IndexOutOfBoundException if i is out of bound) + float get(int i) const; + float getX() const; + float getY() const; + float getZ() const; + float getW() const; + + void set(int i, float v); + void setX(float v); + void setY(float v); + void setZ(float v); + void setW(float v); + + // Other methods + float length() const; + float lengthSquared() const; + // Normalize this quaternion (modified) and return it + Quaternionf& normalize(); + Quaternionf conjugate() const; + Quaternionf inverse() const; + Matrix3f toMatrix3() const; + Matrix4f toMatrix4() const; + // Return the rotation angle (radians) and set axis to the rotation axis (x axis for a null rotation) + float toAxisAngle(Vector3f& axis) const; + + // Static methods + // Spherical linear interpolation, taking the shorter path + static Quaternionf slerp(const Quaternionf& q1, const Quaternionf& q2, float t); + + private: + void initFromRotationMatrix(float m00, float m01, float m02, + float m10, float m11, float m12, + float m20, float m21, float m22); + + float x; + float y; + float z; + float w; + friend std::ostream& operator<<(std::ostream&, const Quaternionf&); + }; +} + +#endif // QUATERNIONF_H diff --git a/VectorMatrixLib/Vector2f.cpp b/VectorMatrixLib/Vector2f.cpp new file mode 100644 index 0000000..0445251 --- /dev/null +++ b/VectorMatrixLib/Vector2f.cpp @@ -0,0 +1,220 @@ +// +// MIT License +// Copyright(c) 2021 - 2026 Olivier BARRY +// +// This file is part of the C++ Aventura Project +// +// VectorMatrix Math Library +// + +#include +#include +#include +#include "Vector2f.h" +#include "Matrix2f.h" +#include "Exceptions.h" +#include "MathTools.h" + +namespace vectormatrix +{ + Vector2f::Vector2f() : x(0), y(0) + { + } + + Vector2f::Vector2f(float a) : x(a), y(a) + { + } + + Vector2f::Vector2f(float x, float y) : x(x), y(y) + { + } + + Vector2f::Vector2f(const float array[2]) : x(array[0]), y(array[1]) + { + } + + Vector2f::Vector2f(const std::vector& array) + { + if (array.size() < 2) throw VectorArrayWrongSizeException("Array passed in parameter of Vector2f constructor is out of bound: " + std::to_string(array.size())); + x = array[0]; + y = array[1]; + } + + Vector2f Vector2f::operator+(const Vector2f& v) const + { + return Vector2f(x + v.x, y + v.y); + } + + Vector2f& Vector2f::operator+=(const Vector2f& v) + { + x += v.x; + y += v.y; + return *this; + } + + Vector2f Vector2f::operator-(const Vector2f& v) const + { + return Vector2f(x - v.x, y - v.y); + } + + Vector2f& Vector2f::operator-=(const Vector2f& v) + { + x -= v.x; + y -= v.y; + return *this; + } + + Vector2f Vector2f::operator*(const Matrix2f& m) const + { + return Vector2f( + x * m.get(0, 0) + y * m.get(0, 1), + x * m.get(1, 0) + y * m.get(1, 1)); + } + + Vector2f& Vector2f::operator*=(const Matrix2f& m) + { + *this = *this * m; + return *this; + } + + Vector2f Vector2f::operator*(float a) const + { + return Vector2f(x * a, y * a); + } + + Vector2f& Vector2f::operator*=(float a) + { + x *= a; + y *= a; + return *this; + } + + Vector2f Vector2f::operator/(float a) const + { + return Vector2f(x / a, y / a); + } + + Vector2f& Vector2f::operator/=(float a) + { + x /= a; + y /= a; + return *this; + } + + float Vector2f::dot(const Vector2f& v) const + { + return x * v.x + y * v.y; + } + + bool Vector2f::operator==(const Vector2f& v) const + { + return MathTools::equals(x, v.x) && MathTools::equals(y, v.y); + } + + bool Vector2f::operator!=(const Vector2f& v) const + { + return !(*this == v); + } + + float Vector2f::get(int i) const + { + switch (i) { + case 0: + return x; + case 1: + return y; + default: + throw IndexOutOfBoundException("Index out of bound while getting coordinate (" + std::to_string(i) + ") of Vector2f"); + } + } + + float Vector2f::getX() const + { + return x; + } + + float Vector2f::getY() const + { + return y; + } + + void Vector2f::set(int i, float a) + { + switch (i) { + case 0: + x = a; + break; + case 1: + y = a; + break; + default: + throw IndexOutOfBoundException("Index out of bound while setting coordinate (" + std::to_string(i) + ") of Vector2f"); + } + } + + void Vector2f::setX(float v) + { + x = v; + } + + void Vector2f::setY(float v) + { + y = v; + } + + float Vector2f::length() const + { + return std::sqrt(x * x + y * y); + } + + float Vector2f::lengthSquared() const + { + return x * x + y * y; + } + + float Vector2f::distance(const Vector2f& v) const + { + return std::sqrt(distanceSquared(v)); + } + + float Vector2f::distanceSquared(const Vector2f& v) const + { + float dx = x - v.x, dy = y - v.y; + return dx * dx + dy * dy; + } + + Vector2f& Vector2f::normalize() + { + float length = this->length(); + x /= length; + y /= length; + return *this; + } + + std::array Vector2f::toArray() const + { + return { x, y }; + } + + float* Vector2f::toArray(float* dest) const + { + dest[0] = x; + dest[1] = y; + return dest; + } + + // static member function (see declaration) + Vector2f Vector2f::interpolate(const Vector2f& v1, const Vector2f& v2, float t) + { + return v1 * (1 - t) + v2 * t; + } + + std::ostream& operator<<(std::ostream& strm, const Vector2f& v) + { + strm << "Vector2f(" << std::endl; + strm << v.x << " "; + strm << v.y << " "; + strm << std::endl; + return strm << ")"; + } +} diff --git a/VectorMatrixLib/Vector2f.h b/VectorMatrixLib/Vector2f.h new file mode 100644 index 0000000..68c3fac --- /dev/null +++ b/VectorMatrixLib/Vector2f.h @@ -0,0 +1,80 @@ +// +// MIT License +// Copyright(c) 2021 - 2026 Olivier BARRY +// +// This file is part of the C++ Aventura Project +// +// VectorMatrix Math Library +// + +#ifndef VECTOR2F_H +#define VECTOR2F_H + +#include +#include +#include + +namespace vectormatrix +{ + class Matrix2f; + + class Vector2f + { + public: + + // Constructors + Vector2f(); + explicit Vector2f(float a); + Vector2f(float x, float y); + explicit Vector2f(const float array[2]); + // Throws VectorArrayWrongSizeException if the array has less than 2 elements + explicit Vector2f(const std::vector& array); + + // Operators + Vector2f operator+(const Vector2f& v) const; + Vector2f& operator+=(const Vector2f& v); + Vector2f operator-(const Vector2f& v) const; + Vector2f& operator-=(const Vector2f& v); + // W = A.V (same semantic as Java Vector2.times(Matrix2)) + Vector2f operator*(const Matrix2f& m) const; + Vector2f& operator*=(const Matrix2f& m); + Vector2f operator*(float a) const; + Vector2f& operator*=(float a); + Vector2f operator/(float a) const; + Vector2f& operator/=(float a); + float dot(const Vector2f& v) const; + // Equality within EPSILON tolerance + bool operator==(const Vector2f& v) const; + bool operator!=(const Vector2f& v) const; + + // getter and setter (get and set throw IndexOutOfBoundException if i is out of bound) + float get(int i) const; + float getX() const; + float getY() const; + + void set(int i, float v); + void setX(float v); + void setY(float v); + + // Other methods + float length() const; + float lengthSquared() const; + float distance(const Vector2f& v) const; + float distanceSquared(const Vector2f& v) const; + // Normalize this vector (modified) and return it + Vector2f& normalize(); + std::array toArray() const; + // Copy the coordinates in dest (at least 2 elements) and return dest + float* toArray(float* dest) const; + + // Static methods + static Vector2f interpolate(const Vector2f& v1, const Vector2f& v2, float t); + + private: + float x; + float y; + friend std::ostream& operator<<(std::ostream&, const Vector2f&); + }; +} + +#endif // VECTOR2F_H diff --git a/VectorMatrixLib/VectorMatrixLib.vcxproj b/VectorMatrixLib/VectorMatrixLib.vcxproj index 9589dcd..4155f9f 100644 --- a/VectorMatrixLib/VectorMatrixLib.vcxproj +++ b/VectorMatrixLib/VectorMatrixLib.vcxproj @@ -151,6 +151,9 @@ + + + @@ -158,6 +161,9 @@ + + + diff --git a/VectorMatrixLib/VectorMatrixLib.vcxproj.filters b/VectorMatrixLib/VectorMatrixLib.vcxproj.filters index 4195d10..0d2f74d 100644 --- a/VectorMatrixLib/VectorMatrixLib.vcxproj.filters +++ b/VectorMatrixLib/VectorMatrixLib.vcxproj.filters @@ -39,6 +39,15 @@ Header Files + + Header Files + + + Header Files + + + Header Files + @@ -56,5 +65,14 @@ Source Files + + Source Files + + + Source Files + + + Source Files + \ No newline at end of file diff --git a/tests/TestVectorMatrix.cpp b/tests/TestVectorMatrix.cpp index e846f29..5641fcd 100644 --- a/tests/TestVectorMatrix.cpp +++ b/tests/TestVectorMatrix.cpp @@ -19,6 +19,9 @@ #include "Vector4f.h" #include "Matrix3f.h" #include "Matrix4f.h" +#include "Vector2f.h" +#include "Matrix2f.h" +#include "Quaternionf.h" #include "MathTools.h" #include "Exceptions.h" @@ -411,6 +414,187 @@ static void testMatrix4fMethods() CHECK_THROWS(b.setArray(std::vector>(3, std::vector(3))), MatrixArrayWrongSizeException); } +static void testVector2f() +{ + Vector2f a(1, 2); + Vector2f b(3, 5); + + CHECK(a + b == Vector2f(4, 7)); + CHECK(b - a == Vector2f(2, 3)); + CHECK(a * 2 == Vector2f(2, 4)); + CHECK(b / 2 == Vector2f(1.5f, 2.5f)); + CHECK_NEAR(a.dot(b), 13.0f); + CHECK(a != b); + + Vector2f c(a); + (c += b) -= a; + CHECK(c == b); + c *= 2; + c /= 2; + CHECK(c == b); + + CHECK_NEAR(Vector2f(3, 4).length(), 5.0f); + CHECK_NEAR(Vector2f(3, 4).lengthSquared(), 25.0f); + CHECK_NEAR(a.distance(Vector2f(4, 6)), 5.0f); + CHECK_NEAR(a.distanceSquared(Vector2f(4, 6)), 25.0f); + Vector2f n(3, 4); + CHECK(n.normalize() == Vector2f(0.6f, 0.8f)); + CHECK(n == Vector2f(0.6f, 0.8f)); // normalize() modifies this vector + + c.setX(7); + c.setY(8); + CHECK(c == Vector2f(7, 8)); + c.set(1, 0); + CHECK_NEAR(c.getY(), 0.0f); + CHECK_THROWS(c.get(2), IndexOutOfBoundException); + CHECK_THROWS(c.set(-1, 0), IndexOutOfBoundException); + + std::array array = b.toArray(); + CHECK_NEAR(array[1], 5.0f); + float dest[2]; + b.toArray(dest); + CHECK_NEAR(dest[0], 3.0f); + float arr[2] = { 1, 2 }; + CHECK(Vector2f(arr) == a); + CHECK(Vector2f(std::vector{ 1, 2 }) == a); + CHECK_THROWS(Vector2f(std::vector{ 1 }), VectorArrayWrongSizeException); + CHECK(Vector2f::interpolate(a, b, 0.5f) == Vector2f(2, 3.5f)); + + // W = A.V + float va[2][2] = { { 1, 2 }, { 3, 4 } }; + Matrix2f m(va); + CHECK(m * a == Vector2f(5, 11)); + CHECK(a * m == m * a); + c = a; + c *= m; + CHECK(c == Vector2f(5, 11)); +} + +static void testMatrix2f() +{ + float va[2][2] = { { 1, 2 }, { 3, 4 } }; + float vb[2][2] = { { 5, 6 }, { 7, 8 } }; + float vab[2][2] = { { 19, 22 }, { 43, 50 } }; + Matrix2f a(va); + Matrix2f b(vb); + + CHECK(a + b == Matrix2f(vab) - Matrix2f(vab) + a + b); + CHECK((a + b) - b == a); + CHECK(a * b == Matrix2f(vab)); + CHECK(a * 2.0f == a + a); + Matrix2f c(a); + c *= b; + CHECK(c == Matrix2f(vab)); + c -= Matrix2f(vab); + CHECK(c == Matrix2f()); + + CHECK(Matrix2f::identity().isIdentity()); + CHECK(a * Matrix2f::identity() == a); + Matrix2f d; + d.setDiagonal(3); + CHECK(d == Matrix2f::identity() * 3.0f); + + CHECK(a.getRow(1) == Vector2f(3, 4)); + CHECK(a.getColumn(1) == Vector2f(2, 4)); + CHECK_THROWS(a.getRow(2), IndexOutOfBoundException); + c = a; + c.setRow(0, Vector2f(9, 9)); + c.setColumn(1, Vector2f(0, 0)); + CHECK(c.getRow(0) == Vector2f(9, 0)); + CHECK(c.getRow(1) == Vector2f(3, 0)); + CHECK_THROWS(c.setColumn(2, Vector2f()), IndexOutOfBoundException); + + std::vector> array = a.getArray(); + Matrix2f e; + e.setArray(array); + CHECK(e == a); + CHECK_THROWS(e.setArray(std::vector>(3, std::vector(2))), MatrixArrayWrongSizeException); + + CHECK_NEAR(a.trace(), 5.0f); + CHECK_NEAR(a.determinant(), -2.0f); + float vt[2][2] = { { 1, 3 }, { 2, 4 } }; + CHECK(a.transpose() == Matrix2f(vt)); + c = a; + c.transposeEquals(); + CHECK(c == Matrix2f(vt)); + + float vi[2][2] = { { -2, 1 }, { 1.5f, -0.5f } }; + CHECK(a.inverse() == Matrix2f(vi)); + CHECK((a * a.inverse()).isIdentity()); + float vs[2][2] = { { 1, 2 }, { 2, 4 } }; + CHECK_THROWS(Matrix2f(vs).inverse(), NotInvertibleMatrixException); +} + +static void testQuaternionf() +{ + const float PI = 3.14159265f; + + Quaternionf id; + CHECK(id == Quaternionf(0, 0, 0, 1)); + CHECK(id.toMatrix3().isIdentity()); + CHECK(id.toMatrix4().isIdentity()); + + // Rotation of 90 degrees around z: x axis goes to y axis + Quaternionf rz(Vector3f(0, 0, 2), PI / 2); // the axis is normalized + CHECK_NEAR(rz.length(), 1.0f); + CHECK(rz.toMatrix3() * Vector3f::xAxis() == Vector3f::yAxis()); + CHECK(rz.toMatrix4() * Vector4f::xAxis() == Vector4f::yAxis()); + CHECK_NEAR(rz.toMatrix4().get(3, 3), 1.0f); + CHECK_NEAR(rz.toMatrix3().determinant(), 1.0f); + + // Back and forth with rotation matrices + CHECK(Quaternionf(rz.toMatrix3()) == rz); + CHECK(Quaternionf(rz.toMatrix4()) == rz); + Quaternionf rx(Vector3f::xAxis(), PI); // trace < 0 branches + CHECK(Quaternionf(rx.toMatrix3()) == rx); + Quaternionf ry(Vector3f::yAxis(), 0.9f * PI); + CHECK(Quaternionf(ry.toMatrix3()) == ry); + Quaternionf rzz(Vector3f::zAxis(), 0.9f * PI); + CHECK(Quaternionf(rzz.toMatrix3()) == rzz); + + // Composition: 2 rotations of 90 degrees = 1 rotation of 180 degrees + Quaternionf r180(Vector3f::zAxis(), PI); + CHECK(rz * rz == r180); + Quaternionf q(rz); + q *= rz; + CHECK(q == r180); + CHECK((rz * rz).toMatrix3() == rz.toMatrix3() * rz.toMatrix3()); + + // Conjugate and inverse + CHECK(rz.conjugate() == Quaternionf(-rz.getX(), -rz.getY(), -rz.getZ(), rz.getW())); + CHECK(rz * rz.inverse() == id); + Quaternionf big(1, 2, 3, 4); + CHECK(big * big.inverse() == id); + CHECK_NEAR(big.dot(big), big.lengthSquared()); + CHECK_NEAR(big.normalize().length(), 1.0f); + + // Axis and angle + Vector3f axis; + CHECK_NEAR(rz.toAxisAngle(axis), PI / 2); + CHECK(axis == Vector3f::zAxis()); + CHECK_NEAR(id.toAxisAngle(axis), 0.0f); + CHECK(axis == Vector3f::xAxis()); // no meaningful axis for a null rotation + + // Slerp + Quaternionf r120(Vector3f::zAxis(), 2 * PI / 3); + Quaternionf r60(Vector3f::zAxis(), PI / 3); + CHECK(Quaternionf::slerp(id, r120, 0) == id); + CHECK(Quaternionf::slerp(id, r120, 1) == r120); + CHECK(Quaternionf::slerp(id, r120, 0.5f) == r60); + CHECK(Quaternionf::slerp(rz, rz, 0.3f) == rz); + // q and -q are the same rotation: slerp takes the shorter path + Quaternionf negRz(-rz.getX(), -rz.getY(), -rz.getZ(), -rz.getW()); + CHECK(Quaternionf::slerp(id, negRz, 1) == rz); + + // Accessors + q.set(0, 1); + q.set(3, 2); + CHECK_NEAR(q.getX(), 1.0f); + CHECK_NEAR(q.get(3), 2.0f); + CHECK_THROWS(q.get(4), IndexOutOfBoundException); + CHECK_THROWS(q.set(-1, 0), IndexOutOfBoundException); +} + int main() { testMathTools(); @@ -422,6 +606,9 @@ int main() testVector4fMethods(); testMatrix3fMethods(); testMatrix4fMethods(); + testVector2f(); + testMatrix2f(); + testQuaternionf(); if (nb_failures == 0) {