diff --git a/.gitignore b/.gitignore index f6d6c4c..b7707ec 100644 --- a/.gitignore +++ b/.gitignore @@ -32,4 +32,7 @@ *.app # Build directory -build/ \ No newline at end of file +build/ + +# vscode +.vscode/ diff --git a/course/.gitignore b/course/.gitignore new file mode 100644 index 0000000..046c078 --- /dev/null +++ b/course/.gitignore @@ -0,0 +1,2 @@ +/build +/.vscode diff --git a/course/.vscode/c_cpp_properties.json b/course/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..ee202d8 --- /dev/null +++ b/course/.vscode/c_cpp_properties.json @@ -0,0 +1,16 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**" + ], + "defines": [], + "compilerPath": "/usr/bin/clang", + "cStandard": "c11", + "cppStandard": "c++17", + "intelliSenseMode": "clang-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/course/CMakeLists.txt b/course/CMakeLists.txt index 3c19060..4ba663c 100644 --- a/course/CMakeLists.txt +++ b/course/CMakeLists.txt @@ -16,6 +16,9 @@ include_directories( # Library sources. set(LIBRARY_SOURCES src/foo.cc + src/Vector3.cc + src/Matrix3.cc + src/isometry.cc ) # Library creation. diff --git a/course/include/Matrix3.h b/course/include/Matrix3.h new file mode 100644 index 0000000..3ca79a8 --- /dev/null +++ b/course/include/Matrix3.h @@ -0,0 +1,65 @@ +#pragma once +#include +#include +#include "Vector3.h" + +namespace ekumen +{ + + class Matrix3 + { + public: + Matrix3(); + explicit Matrix3(const Vector3 &f1, const Vector3 &f2, const Vector3 &f3); + Matrix3(const std::initializer_list &list); + Matrix3(const std::initializer_list &r1, const std::initializer_list &r2, const std::initializer_list &r3); + Matrix3(const Matrix3 &m); + Matrix3(Matrix3 &&m); + + double det() const; + const Vector3 row(const int index) const; + Vector3 &row(const int index); + const Vector3 col(const int index) const; + + Matrix3 mul(const Matrix3 &m2) const; + Matrix3 inverse() const; + + Matrix3 &operator+=(const Matrix3 &q); + Matrix3 &operator-=(const Matrix3 &q); + + Matrix3 &operator=(const Matrix3 &other); // copy assignment + Matrix3 &operator=(Matrix3 &&other); //move assignment + + //operator overload + const Vector3 &operator[](const int &index) const; + Vector3 &operator[](const int &index); + + friend bool operator==(const Matrix3 &p, const Matrix3 &q); + friend bool operator==(const Matrix3 &p, const std::initializer_list &list); + friend bool operator==(const std::initializer_list &list, const Matrix3 &p); + + friend Matrix3 operator*(const double &cte, const Matrix3 &p); + friend Matrix3 operator*(const Matrix3 &p, const double &cte); + friend Matrix3 operator*(const Matrix3 &p, const Matrix3 &q); + friend Vector3 operator*(const Matrix3 &p, const Vector3 &v); + + friend Matrix3 operator/(const Matrix3 &p, const Matrix3 &q); + friend Matrix3 operator/(const Matrix3 &p, const double &value); + + friend bool operator!=(const Matrix3 &p, const Matrix3 &q); + friend bool operator!=(const Matrix3 &p, const std::initializer_list &list); + friend bool operator!=(const std::initializer_list &list, const Matrix3 &p); + + friend std::ostream &operator<<(std::ostream &os, const Matrix3 &p); + static const Matrix3 kIdentity; + static const Matrix3 kZero; + static const Matrix3 kOnes; + + private: + Vector3 *row_values = new Vector3[3]; + }; + + Matrix3 operator+(const Matrix3 &p, const Matrix3 &q); + Matrix3 operator-(const Matrix3 &p, const Matrix3 &q); + +} // namespace ekumen diff --git a/course/include/Vector3.h b/course/include/Vector3.h new file mode 100644 index 0000000..83184d0 --- /dev/null +++ b/course/include/Vector3.h @@ -0,0 +1,71 @@ +#pragma once +#include +#include +#include +#include + +namespace ekumen +{ + + class Vector3 + { + public: + Vector3(); + explicit Vector3(const double x, const double y, const double z); + Vector3(const std::initializer_list &list); + Vector3(const Vector3 &p2); //copy constructor + Vector3(Vector3&& other); //move constructor + ~Vector3(); + + const double &x() const { return values[0]; } + double &x() { return values[0]; } + + const double &y() const { return values[1]; } + double &y() { return values[1]; } + + const double &z() const { return values[2]; } + double &z() { return values[2]; } + + double norm() const; + double dot(const Vector3 &q) const; + Vector3 cross(const Vector3 &q) const; + //operator overload + const double &operator[](const int index) const; + double &operator[](const int index); + + Vector3 &operator+=(const Vector3 &q); + Vector3 &operator-=(const Vector3 &q); + Vector3 &operator=(const Vector3 &r); // copy assignment + Vector3 &operator=(Vector3 &&other); //move assignment + + friend Vector3 operator*(const double &cte, const Vector3 &p); + friend Vector3 operator*(const Vector3 &p, const double &cte); + friend Vector3 operator*(const Vector3 &p, const Vector3 &q); + friend Vector3 operator/(const Vector3 &p, const Vector3 &q); + + friend bool operator==(const Vector3 &p, const Vector3 &q); + friend bool operator==(const Vector3 &p, const std::initializer_list &list); + friend bool operator==(const std::initializer_list &list, const Vector3 &p); + + friend bool operator!=(const Vector3 &p, const Vector3 &q); + friend bool operator!=(const Vector3 &p, const std::initializer_list &list); + friend bool operator!=(const std::initializer_list &list, const Vector3 &p); + + friend std::ostream &operator<<(std::ostream &os, const Vector3 &p); + + static const Vector3 kUnitX; + static const Vector3 kUnitY; + static const Vector3 kUnitZ; + static const Vector3 kZero; + static const Vector3 kOnes; + + private: + double *values; + + + }; + + Vector3 operator+(const Vector3 &p, const Vector3 &q); + Vector3 operator-(const Vector3 &p, const Vector3 &q); + +} // namespace ekumen diff --git a/course/include/isometry.h b/course/include/isometry.h new file mode 100644 index 0000000..cc7f289 --- /dev/null +++ b/course/include/isometry.h @@ -0,0 +1,39 @@ +#pragma once +#include "Vector3.h" +#include "Matrix3.h" +#include + +namespace ekumen +{ + class Isometry + { + public: + Isometry(); + explicit Isometry(const Vector3 &v, const Matrix3 &m); + explicit Isometry(const std::initializer_list &list, Matrix3 &m); + + static Isometry FromTranslation(const std::initializer_list &list); + static Isometry FromTranslation(const Vector3 &v); + static Isometry RotateAround(const Vector3 &v, const double &a); + static Isometry FromEulerAngles(const double &x, const double &y, const double &z); + + Vector3 transform(const Vector3 &v) const; + Vector3 transform(const std::initializer_list &list) const; + Isometry inverse() const; + Isometry compose(const Isometry &I1) const; + Vector3 translation() const; + Matrix3 rotation() const; + + friend bool operator==(const Isometry &p, const Isometry &q); + friend bool operator!=(const Isometry &p, const Isometry &q); + friend std::ostream &operator<<(std::ostream &os, const Isometry &m); + + friend Vector3 operator*(const Isometry &i, const Vector3 &v); + friend Isometry operator*(const Isometry &i, const Isometry &i2); + + private: + Vector3 vector3_isometry; + Matrix3 matrix3_isometry; + }; + +} // namespace ekumen diff --git a/course/src/Matrix3.cc b/course/src/Matrix3.cc new file mode 100644 index 0000000..3d5bcfd --- /dev/null +++ b/course/src/Matrix3.cc @@ -0,0 +1,297 @@ +#include "Matrix3.h" + +namespace ekumen +{ + + Matrix3::Matrix3() + { + row(0) = Vector3::kZero; + row(1) = Vector3::kZero; + row(2) = Vector3::kZero; + } + + Matrix3::Matrix3(const Vector3 &f1, const Vector3 &f2, const Vector3 &f3) + { + row(0) = f1; + row(1) = f2; + row(2) = f3; + } + + Matrix3::Matrix3(const std::initializer_list &list) + { + int i = 0, j = 0; + if (list.size() == 9) + { + for (const auto &v : list) + { + row_values[i][j] = v; + if (j++ == 2) + { + ++i; + j = 0; + } + } + } + } + + Matrix3::Matrix3(const std::initializer_list &r0, const std::initializer_list &r1, const std::initializer_list &r2) + { + row(0) = Vector3(r0); + row(1) = Vector3(r1); + row(2) = Vector3(r2); + } + + Matrix3::Matrix3(const Matrix3 &m) + { + row(0) = m.row(0); + row(1) = m.row(1); + row(2) = m.row(2); + } + + Matrix3::Matrix3(Matrix3 &&m) { + row_values[0] = std::move(m.row_values[0]); + row_values[1] = std::move(m.row_values[1]); + row_values[2] = std::move(m.row_values[2]); + } + + double Matrix3::det() const + { + const Vector3 &i = row(0); + Vector3 j = row(1).cross(row(2)); + return (i.x() * j.x() + i.y() * j.y() + i.z() * j.z()); + } + + const Vector3 Matrix3::row(const int index) const + { + if ((index >= 0) && (index < 3)) + return row_values[index]; + else + return Vector3(); + } + + Vector3 &Matrix3::row(const int index) + { + if ((index >= 0) && (index < 3)) + return row_values[index]; + else + return row_values[0]; + } + + const Vector3 Matrix3::col(const int index) const + { + + if ((index >= 0) && (index < 3)) + { + Vector3 p(row_values[0][index], row_values[1][index], row_values[2][index]); + return p; + } + + return Vector3(); + } + + const Vector3 &Matrix3::operator[](const int &index) const + { + try + { + if ((index >= 0) && (index < 3)) + { + return row_values[index]; + } + } + catch (const std::exception &ex) + { + std::cerr << "Error occurred: " << ex.what() << std::endl; + } + return row_values[0]; + } + + Vector3 &Matrix3::operator[](const int &index) + { + try + { + if ((index >= 0) && (index < 3)) + { + return row(index); + } + } + catch (const std::exception &ex) + { + std::cerr << "Error occurred: " << ex.what() << std::endl; + } + return row(0); + } + + Matrix3 &Matrix3::operator+=(const Matrix3 &q) + { + row(0) += q.row(0); + row(1) += q.row(1); + row(2) += q.row(2); + return *this; + } + + Matrix3 &Matrix3::operator-=(const Matrix3 &q) + { + row(0) -= q.row(0); + row(1) -= q.row(1); + row(2) -= q.row(2); + return *this; + } + + Matrix3 &Matrix3::operator=(const Matrix3 &other){ + row(0) = other.row(0); + row(1) = other.row(1); + row(2) = other.row(2); + return *this; + } + Matrix3 &Matrix3::operator=(Matrix3 &&other){ + if (&other != this) { + delete[] row_values; + row_values[0] = other.row_values[0]; + row_values[1] = other.row_values[1]; + row_values[2] = other.row_values[2]; + other.row_values = nullptr; + + } + } + + Matrix3 operator+(const Matrix3 &p, const Matrix3 &q) + { + Matrix3 r(p); + r += q; + return r; + } + + Matrix3 operator-(const Matrix3 &p, const Matrix3 &q) + { + Matrix3 r(p); + r -= q; + return r; + } + + Matrix3 Matrix3::mul(const Matrix3 &m2) const + { + Matrix3 r; + for (size_t i = 0; i < 3; ++i) + { + for (size_t j = 0; j < 3; ++j) + { + r[i][j] = row(i).dot(m2.col(j)); + } + } + return r; + } + + Matrix3 Matrix3::inverse() const + { + Matrix3 m; + m.row(0) = row(1).cross(row(2)); + m.row(1) = (-1) * (row(0).cross(row(2))); + m.row(2) = row(0).cross(row(1)); + Matrix3 n; + for (size_t i = 0; i < 3; ++i) + { + for (size_t j = 0; j < 3; ++j) + n[i][j] = m[j][i]; + } + return (n / det()); + } + bool operator==(const Matrix3 &p, const Matrix3 &q) + { + bool row0 = (p.row(0) == q.row(0)); + bool row1 = (p.row(1) == q.row(1)); + bool row2 = (p.row(2) == q.row(2)); + return (row0 && row1 && row2); + } + + bool operator==(const Matrix3 &p, const std::initializer_list &list) + { + + Matrix3 q(list); + return (p == q); + } + + bool operator==(const std::initializer_list &list, const Matrix3 &p) + { + Matrix3 q(list); + return (p == q); + } + + Matrix3 operator*(const double &cte, const Matrix3 &p) + { + Matrix3 res(cte * p.row(0), cte * p.row(1), cte * p.row(2)); + return res; + } + + Matrix3 operator*(const Matrix3 &p, const double &cte) + { + Matrix3 res(cte * p.row(0), cte * p.row(1), cte * p.row(2)); + return res; + } + + Matrix3 operator*(const Matrix3 &p, const Matrix3 &q) + { + + Matrix3 res(p.row(0) * q.row(0), p.row(1) * q.row(1), p.row(2) * q.row(2)); + return res; + } + + Vector3 operator*(const Matrix3 &p, const Vector3 &v) + { + + Vector3 r; + r.x() = p.row(0).dot(v); + r.y() = p.row(1).dot(v); + r.z() = p.row(2).dot(v); + + return (r); + } + + Matrix3 operator/(const Matrix3 &p, const Matrix3 &q) + { + Matrix3 res; + for (size_t i = 0; i < 3; ++i) + { + res.row(i) = p.row(i) / q.row(i); + } + return res; + } + + Matrix3 operator/(const Matrix3 &p, const double &value) + { + double v = 1. / value; + return v * p; + } + + bool operator!=(const Matrix3 &p, const Matrix3 &q) + { + return !(p == q); + } + + bool operator!=(const Matrix3 &p, const std::initializer_list &list) + { + Matrix3 q(list); + return !(p == q); + } + + bool operator!=(const std::initializer_list &list, const Matrix3 &p) + { + Matrix3 q(list); + return !(p == q); + } + + std::ostream &operator<<(std::ostream &os, const Matrix3 &p) + { + os << "["; + for (size_t i = 0; i < 2; ++i) + { + os << "[" << p[i][0] << ", " << p[i][1] << ", " << p[i][2] << "], "; + } + os << "[" << p[2][0] << ", " << p[2][1] << ", " << p[2][2] << "]]"; + + return os; + } + + const Matrix3 Matrix3::kIdentity = Matrix3(Vector3::kUnitX, Vector3::kUnitY, Vector3::kUnitZ); + const Matrix3 Matrix3::kZero = Matrix3(Vector3::kZero, Vector3::kZero, Vector3::kZero); + const Matrix3 Matrix3::kOnes = Matrix3(Vector3::kOnes, Vector3::kOnes, Vector3::kOnes); +} // namespace ekumen diff --git a/course/src/Vector3.cc b/course/src/Vector3.cc new file mode 100644 index 0000000..3820912 --- /dev/null +++ b/course/src/Vector3.cc @@ -0,0 +1,208 @@ +#include "Vector3.h" + +namespace ekumen +{ + + Vector3::Vector3(const double x, const double y, const double z) + { + values = new double[3]; + values[0] = x; + values[1] = y; + values[2] = z; + } + + Vector3::Vector3() + { + values = new double[3]; + values[0] = 0.; + values[1] = 0.; + values[2] = 0.; + } + + Vector3::Vector3(const std::initializer_list &list) + { + values = new double[3]; + if (list.size() == 3) + { + auto i = 0; + for (const auto &v : list) + { + values[i] = v; + ++i; + } + + } + } + + Vector3::Vector3(const Vector3 &p2) + { + values = new double[3]; + x() = p2.x(); + y() = p2.y(); + z() = p2.z(); + } + + Vector3::Vector3(Vector3&& other) { + values = other.values; + other.values = nullptr; + } + + Vector3::~Vector3(){ + delete[] values; + } + + double Vector3::norm() const + { + return sqrt(dot(*this)); + } + + double Vector3::dot(const Vector3 &q) const + { + return ((x() * q.x()) + (y() * q.y()) + (z() * q.z())); + } + + Vector3 Vector3::cross(const Vector3 &q) const + { + double a = y() * q.z() - z() * q.y(); + double b = z() * q.x() - x() * q.z(); + double c = x() * q.y() - y() * q.x(); + return Vector3(a, b, c); + } + + Vector3 &Vector3::operator+=(const Vector3 &q) + { + x() += q.x(); + y() += q.y(); + z() += q.z(); + return *this; + } + + Vector3 &Vector3::operator-=(const Vector3 &q) + { + x() -= q.x(); + y() -= q.y(); + z() -= q.z(); + return *this; + } + + Vector3 &Vector3::operator=(const Vector3 &other){ + values[0] = other.x(); + values[1] = other.y(); + values[2] = other.z(); + return *this; + } + + Vector3 &Vector3::operator=(Vector3 &&other){ + if (&other != this) { + delete[] values; + values = other.values; + other.values = nullptr; + + } + return *this; + } + + + Vector3 operator+(const Vector3 &p, const Vector3 &q) + { + Vector3 r(p.x(), p.y(), p.z()); + return (r += q); + } + + Vector3 operator-(const Vector3 &p, const Vector3 &q) + { + Vector3 r(p.x(), p.y(), p.z()); + return (r -= q); + } + + Vector3 operator*(const double &cte, const Vector3 &p) + { + Vector3 res(cte * p.x(), cte * p.y(), cte * p.z()); + return res; + } + + Vector3 operator*(const Vector3 &p, const double &cte) + { + Vector3 res(cte * p.x(), cte * p.y(), cte * p.z()); + return res; + } + + Vector3 operator*(const Vector3 &p, const Vector3 &q) + { + Vector3 res(p.x() * q.x(), p.y() * q.y(), p.z() * q.z()); + return res; + } + + Vector3 operator/(const Vector3 &p, const Vector3 &q) + { + Vector3 res(p.x() / q.x(), p.y() / q.y(), p.z() / q.z()); + return res; + } + + bool operator==(const Vector3 &p, const Vector3 &q) + { + bool res = (p.x() == q.x()) && (p.y() == q.y()) && (p.z() == q.z()); + return res; + } + + bool operator==(const Vector3 &p, const std::initializer_list &list) + { + Vector3 q(list); + return (p == q); //res; + } + + bool operator==(const std::initializer_list &list, const Vector3 &p) + { + Vector3 q(list); + return (p == q); //res; + } + + bool operator!=(const Vector3 &p, const Vector3 &q) + { + + return !(p == q); + } + + bool operator!=(const Vector3 &p, const std::initializer_list &list) + { + Vector3 q = Vector3(list); + return !(p == q); + } + + bool operator!=(const std::initializer_list &list, const Vector3 &p) + { + Vector3 q = Vector3(list); + return !(p == q); + } + + std::ostream &operator<<(std::ostream &os, const Vector3 &p) + { + os << "(x: " << p.x() << ", y: " << p.y() << ", z: " << p.z() << ")"; + return os; + } + + const double &Vector3::operator[](const int index) const + { + if ((index >= 0)&&(index < 3)) + { + return values[index]; + } + return values[0]; + } + + double &Vector3::operator[](const int index) + { + if ((index >= 0)&&(index < 3)) + { + return values[index]; + } + return values[0]; + } + + const Vector3 Vector3::kUnitX = Vector3(1., 0., 0.); + const Vector3 Vector3::kUnitY = Vector3(0., 1., 0.); + const Vector3 Vector3::kUnitZ = Vector3(0., 0., 1.); + const Vector3 Vector3::kZero = Vector3(0., 0., 0.); + const Vector3 Vector3::kOnes = Vector3(1., 1., 1.); + +} // namespace ekumen diff --git a/course/src/isometry.cc b/course/src/isometry.cc new file mode 100644 index 0000000..5a47782 --- /dev/null +++ b/course/src/isometry.cc @@ -0,0 +1,169 @@ +#include "isometry.h" + +namespace ekumen +{ + Isometry::Isometry() + { + vector3_isometry = Vector3::kZero; + matrix3_isometry = Matrix3::kIdentity; + } + + Isometry::Isometry(const Vector3 &v, const Matrix3 &m) + { + vector3_isometry = v; + matrix3_isometry = m; + } + + Isometry::Isometry(const std::initializer_list &list, Matrix3 &m) + { + if (list.size() == 3) + { + try + { + vector3_isometry = Vector3(list); + matrix3_isometry = m; + } + catch (const std::exception &ex) + { + std::cerr << "Error occurred: " << ex.what() << std::endl; + } + } + } + + Isometry Isometry::FromTranslation(const std::initializer_list &list) + { + if (list.size() == 3) + { + Isometry I(Vector3(list), Matrix3::kIdentity); + return I; + } + + return Isometry(); + } + + Isometry Isometry::FromTranslation(const Vector3 &v) + { + return Isometry(v, Matrix3::kIdentity); + } + + Isometry Isometry::RotateAround(const Vector3 &v, const double &a) + { + Matrix3 m; + double co = cos(a); + double s = sin(a); + double C = 1. - co; + double x = v.x(); + double y = v.y(); + double z = v.z(); + + m.row(0) = Vector3(x * x * C + co, x * y * C - z * s, x * z * C + y * s); + m.row(1) = Vector3(y * x * C + z * s, y * y * C + co, y * z * C - x * s); + m.row(2) = Vector3(z * x * C - y * s, z * y * C + x * s, z * z * C + co); + + return Isometry(Vector3(), m); + } + + Vector3 Isometry::translation() const + { + return vector3_isometry; + } + + Matrix3 Isometry::rotation() const + { + return matrix3_isometry; + } + + Isometry Isometry::FromEulerAngles(const double &x, const double &y, const double &z) + { + double cx = cos(x); + double sx = sin(x); + double cy = cos(y); + double sy = sin(y); + double cz = cos(z); + double sz = sin(z); + + double m00 = cy * cz; + double m01 = -cy * sz; + double m02 = sy; + double m10 = sx * sy * cz + cz * cx; + double m11 = -sx * sy * sz + cx * cz; + double m12 = -sx * sy; + double m20 = -cx * sy * cz + sx * sz; + double m21 = cx * sy * sz + sx * cz; + double m22 = cx * cy; + + Vector3 vv1 = Vector3(m00, m01, m02); + Vector3 vv2 = Vector3(m10, m11, m12); + Vector3 vv3 = Vector3(m20, m21, m22); + + return Isometry(Vector3::kZero, Matrix3(vv1, vv2, vv3)); + } + + Vector3 Isometry::transform(const Vector3 &v) const + { + return ((*this) * v); + } + + Vector3 Isometry::transform(const std::initializer_list &list) const + { + if (list.size() == 3) + { + return ((*this) * Vector3(list)); + } + return Vector3(); + } + + Isometry Isometry::inverse() const + { + Matrix3 m = matrix3_isometry.inverse(); + Vector3 v = m * vector3_isometry * (-1); + + return Isometry(v, m); + } + + Isometry Isometry::compose(const Isometry &I1) const + { + return (*this) * I1; + } + + bool operator==(const Isometry &p, const Isometry &q) + { + for (size_t i = 0; i < 2; ++i) + { + if (p.vector3_isometry[i] != p.vector3_isometry[i]) + return false; + if (p.matrix3_isometry[i] != p.matrix3_isometry[i]) + return false; + } + return true; + } + + bool operator!=(const Isometry &p, const Isometry &q) + { + return !(p == q); + } + + Vector3 operator*(const Isometry &i, const Vector3 &v) + { + double rx = i.matrix3_isometry.row(0).dot(v) + i.vector3_isometry.x(); + double ry = i.matrix3_isometry.row(1).dot(v) + i.vector3_isometry.y(); + double rz = i.matrix3_isometry.row(2).dot(v) + i.vector3_isometry.z(); + return Vector3(rx, ry, rz); + } + + Isometry operator*(const Isometry &i1, const Isometry &i2) + { + Matrix3 m1{i1.matrix3_isometry.mul(i2.matrix3_isometry)}; + Vector3 v1 = i1 * i2.vector3_isometry; + return Isometry(v1, m1); + } + + std::ostream &operator<<(std::ostream &os, const Isometry &p) + { + os.precision(9); + os << "[T: " << p.vector3_isometry << ", "; + os << "R:" << p.matrix3_isometry << "]"; + return os; + } + +} // namespace ekumen diff --git a/course/test/CMakeLists.txt b/course/test/CMakeLists.txt index 59f8ec6..395fb77 100644 --- a/course/test/CMakeLists.txt +++ b/course/test/CMakeLists.txt @@ -39,7 +39,7 @@ macro (cppcourse_build_tests) add_executable(${BINARY_NAME} ${GTEST_SOURCE_file}) add_dependencies(${BINARY_NAME} - foo + foo gtest gtest_main ) diff --git a/course/test/src/CMakeLists.txt b/course/test/src/CMakeLists.txt index 57f4acd..a7f926e 100644 --- a/course/test/src/CMakeLists.txt +++ b/course/test/src/CMakeLists.txt @@ -5,6 +5,7 @@ include_directories( # Test sources. set (GTEST_SOURCES + isometry_TEST.cc foo_TEST.cc ) diff --git a/course/test/src/isometry_TEST.cc b/course/test/src/isometry_TEST.cc index 974b169..ef9c4c4 100644 --- a/course/test/src/isometry_TEST.cc +++ b/course/test/src/isometry_TEST.cc @@ -3,26 +3,70 @@ // needed to implement an isometry. // Consider including other header files if needed. -#include "isometry.h" +//#include "isometry.h" #include #include #include +#include +#include "Vector3.h" +#include "Matrix3.h" +#include "isometry.h" #include "gtest/gtest.h" -namespace ekumen { -namespace math { -namespace test { -namespace { +namespace ekumen +{ +namespace math +{ +namespace cppcourse +{ +namespace test +{ +namespace +{ + +testing::AssertionResult areAlmostEqual(const Isometry &I1, const Isometry &I2, const double kT) +{ + Matrix3 IR = I1.rotation() - I2.rotation(); + Vector3 IT = I1.translation() - I2.translation(); + for (size_t i = 0; i < 2; i++) + { + if (fabs(IT[i]) > kT) + return ::testing::AssertionFailure() << "Isometry Matrix I1: " << I1 << std::endl + << "Isometry Matrix I2: " << I2 << std::endl; + for (size_t j = 0; j < 2; j++) + { + if (fabs(IR[i][j]) > kT) + return ::testing::AssertionFailure() << "Isometry Matrix I1: " << I1 << std::endl + << "Isometry Matrix I2: " << I2 << std::endl; + } + } + return ::testing::AssertionSuccess(); +} -GTEST_TEST(Vector3Test, Vector3Operations) { +testing::AssertionResult areAlmostEqual(const Matrix3 &M1, const Matrix3 &M2, const double kT) +{ + Matrix3 MR = M1 - M2; + for (size_t i = 0; i < 2; i++) + { + for (size_t j = 0; j < 2; j++) + { + if (fabs(MR[i][j]) > kT) + return ::testing::AssertionFailure(); + } + } + + return ::testing::AssertionSuccess(); +} +GTEST_TEST(Vector3Test, Vector3Operations) +{ const double kTolerance{1e-12}; const Vector3 p{1., 2., 3.}; const Vector3 q{4., 5., 6.}; - EXPECT_EQ(p + q, {5., 7., 9.}); - EXPECT_EQ(p - q, {-3., -3., -3.}); + EXPECT_EQ(p + q, std::initializer_list({5., 7., 9.})); + EXPECT_EQ(p - q, std::initializer_list({-3., -3., -3.})); EXPECT_EQ(p * 2., Vector3(2., 4., 6)); EXPECT_EQ(2 * q, Vector3(8., 10., 12.)); EXPECT_EQ(p * q, Vector3(4., 10., 18.)); @@ -40,8 +84,8 @@ GTEST_TEST(Vector3Test, Vector3Operations) { EXPECT_EQ(ss.str(), "(x: 1, y: 2, z: 3)"); EXPECT_TRUE(Vector3::kUnitX == Vector3(1., 0., 0)); - EXPECT_TRUE(Vector3::kUnitX != {1., 1., 0}); - EXPECT_TRUE(Vector3::kUnitY == {0., 1., 0}); + EXPECT_TRUE(Vector3::kUnitX != std::initializer_list({1., 1., 0})); + EXPECT_TRUE(Vector3::kUnitY == std::initializer_list({0., 1., 0})); EXPECT_TRUE(Vector3::kUnitZ == Vector3::kUnitX.cross(Vector3::kUnitY)); EXPECT_NEAR(Vector3::kUnitX.dot(Vector3::kUnitZ), 0., kTolerance); @@ -53,42 +97,53 @@ GTEST_TEST(Vector3Test, Vector3Operations) { EXPECT_EQ(t, p); } -GTEST_TEST(Matrix3Test, Matrix3Operations) { +GTEST_TEST(Matrix3Test, Matrix3Operations) +{ const double kTolerance{1e-12}; Matrix3 m1{{1., 2., 3.}, {4., 5., 6.}, {7., 8., 9.}}; const Matrix3 m2{1., 2., 3., 4., 5., 6., 7., 8., 9.}; const Matrix3 m3 = Matrix3::kIdentity; - EXPECT_EQ(m1, m2); EXPECT_EQ(m1 - m2, Matrix3::kZero); EXPECT_EQ(m1 + m2, m1 * 2.); EXPECT_EQ(m1 + m2, 2. * m2); - EXPECT_EQ(m1 * m2, {1., 4., 9., 16., 25., 36., 49., 64., 81.}); + EXPECT_EQ(m1 * m2, std::initializer_list({1., 4., 9., 16., 25., 36., 49., 64., 81.})); EXPECT_EQ(m1 / m2, Matrix3::kOnes); EXPECT_NEAR(m1.det(), 0., kTolerance); + + EXPECT_EQ(m1.mul(m2), std::initializer_list({30., 36., 42., 66., 81., 96., 102., 126., 150.})); + const Matrix3 m4{1., 2., 3., 2., 2., 1., 1., 1., 1.}; + EXPECT_EQ(m4.inverse(), std::initializer_list({-1., -1., 4., 1., 2., -5., 0., -1., 2.})); + m1[2][2] = 10.; EXPECT_NEAR(m1.det(), -2.9999999999999996, kTolerance); - std::stringstream ss; ss << m3; EXPECT_EQ(ss.str(), "[[1, 0, 0], [0, 1, 0], [0, 0, 1]]"); const std::vector kExpectedRows{Vector3(1., 2., 3.), Vector3(4., 5., 6.), Vector3(7., 8., 9.)}; const std::vector kExpectedCols{Vector3(1., 4., 7.), Vector3(2., 5., 8.), Vector3(3., 6., 9.)}; - for (const Vector3& r : kExpectedRows) { + for (const Vector3 &r : kExpectedRows) + { bool found{false}; - for (int i = 0; i < 3; ++i) { - if (r == m2.row(i)) { + for (int i = 0; i < 3; ++i) + { + if (r == m2.row(i)) + { found = true; break; } } ASSERT_TRUE(found); } - for (const Vector3& c : kExpectedCols) { + + for (const Vector3 &c : kExpectedCols) + { bool found{false}; - for (int i = 0; i < 3; ++i) { - if (c == m2.col(i)) { + for (int i = 0; i < 3; ++i) + { + if (c == m2.col(i)) + { found = true; break; } @@ -97,13 +152,12 @@ GTEST_TEST(Matrix3Test, Matrix3Operations) { } } -GTEST_TEST(IsometryTest, IsometryOperations) { +GTEST_TEST(IsometryTest, IsometryOperations) +{ const double kTolerance{1e-12}; const Isometry t1 = Isometry::FromTranslation({1., 2., 3.}); - const Isometry t2{{1., 2., 3.}, Matrix3::kIdentity}; - + const Isometry t2{std::initializer_list({1., 2., 3.}), Matrix3::kIdentity}; EXPECT_EQ(t1, t2); - // This is not mathematically correct but it could be a nice to have. EXPECT_EQ(t1 * Vector3(1., 1., 1.), Vector3(2., 3., 4.)); EXPECT_EQ(t1.transform({1., 1., 1.}), Vector3(2., 3., 4.)); @@ -115,27 +169,32 @@ GTEST_TEST(IsometryTest, IsometryOperations) { const Isometry t3{Isometry::RotateAround(Vector3::kUnitX, M_PI / 2.)}; const Isometry t4{Isometry::RotateAround(Vector3::kUnitY, M_PI / 4.)}; const Isometry t5{Isometry::RotateAround(Vector3::kUnitZ, M_PI / 8.)}; + const Isometry t6{Isometry::FromEulerAngles(M_PI / 2., M_PI / 4., M_PI / 8.)}; + // See https://github.com/google/googletest/blob/master/googletest/docs/advanced.md#using-a-function-that-returns-an-assertionresult EXPECT_TRUE(areAlmostEqual(t6, t3 * t4 * t5, kTolerance)); - EXPECT_EQ(t3.translation(), Vector3::kZero); const double pi_8{M_PI / 8.}; - const double cpi_8{std::cos(pi_8)}; // 0.923879532 - const double spi_8{std::sin(pi_8)}; // 0.382683432 - EXPECT_TRUE(areAlmostEqual(t5.rotation(), Matrix3{cpi_8, -spi_8, 0., spi_8, cpi_8, 0., 0., 0., 1.}, kTolerance)); + const double cpi_8{std::cos(pi_8)}; // 0.923879532 + const double spi_8{std::sin(pi_8)}; // 0.382683432 + Matrix3 pp = Matrix3(std::initializer_list({cpi_8, -spi_8, 0., spi_8, cpi_8, 0., 0., 0., 1.})); + EXPECT_TRUE(areAlmostEqual(t5.rotation(), pp, kTolerance)); + EXPECT_EQ(t5.rotation(), pp); std::stringstream ss; ss << t5; EXPECT_EQ(ss.str(), "[T: (x: 0, y: 0, z: 0), R:[[0.923879533, -0.382683432, 0], [0.382683432, 0.923879533, 0], [0, 0, 1]]]"); } -} // namespace -} // namespace test -} // namespace math -} // namespace ekumen +} // namespace +} // namespace test +} // namespace cppcourse +} // namespace math +} // namespace ekumen -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }