From 6718de7e40e35e8e66d5311d51acd7944139143e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 29 Jul 2015 15:44:05 +0200 Subject: ucoo/math: add 3D vector --- ucoo/math/Module | 0 ucoo/math/test/Makefile | 9 +++ ucoo/math/test/test_math.cc | 164 ++++++++++++++++++++++++++++++++++++++++++++ ucoo/math/vect3d.hh | 69 +++++++++++++++++++ ucoo/math/vect3d.tcc | 148 +++++++++++++++++++++++++++++++++++++++ 5 files changed, 390 insertions(+) create mode 100644 ucoo/math/Module create mode 100644 ucoo/math/test/Makefile create mode 100644 ucoo/math/test/test_math.cc create mode 100644 ucoo/math/vect3d.hh create mode 100644 ucoo/math/vect3d.tcc (limited to 'ucoo/math') diff --git a/ucoo/math/Module b/ucoo/math/Module new file mode 100644 index 0000000..e69de29 diff --git a/ucoo/math/test/Makefile b/ucoo/math/test/Makefile new file mode 100644 index 0000000..353bc10 --- /dev/null +++ b/ucoo/math/test/Makefile @@ -0,0 +1,9 @@ +BASE = ../../.. + +TARGETS = host stm32f4 +PROGS = test_math +test_math_SOURCES = test_math.cc + +MODULES = ucoo/math ucoo/utils ucoo/base/test ucoo/hal/usb + +include $(BASE)/build/top.mk diff --git a/ucoo/math/test/test_math.cc b/ucoo/math/test/test_math.cc new file mode 100644 index 0000000..685387e --- /dev/null +++ b/ucoo/math/test/test_math.cc @@ -0,0 +1,164 @@ +// ucoolib - Microcontroller object oriented library. {{{ +// +// Copyright (C) 2015 Nicolas Schodet +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. +// +// }}} +#include "ucoo/math/vect3d.hh" +#include "ucoo/arch/arch.hh" +#include "ucoo/base/test/test.hh" +#include "ucoo/common.hh" + +#include +#include + +template +bool +almost_eq (T a, double b); + +template<> +bool +almost_eq (int a, double b) +{ + return a == (int) b; +} + +template<> +bool +almost_eq (float a, double b) +{ + float fb = b; + float pfb = std::fabs (fb); + ucoo::assert (pfb >= FLT_MIN); + float diff = std::fabs (a - fb); + return diff <= pfb * 4 * FLT_EPSILON; +} + +template<> +bool +almost_eq (double a, double b) +{ + double pb = std::fabs (b); + ucoo::assert (pb >= DBL_MIN); + double diff = std::fabs (a - b); + return diff <= pb * 4 * DBL_EPSILON; +} + +template +void +test_group_vect3d (ucoo::TestSuite &tsuite, const char *tname) +{ + tsuite.group (tname); + ucoo::vect3d a (1, 2, 3); + ucoo::vect3d b (4, 6, 8); + do { + ucoo::Test test (tsuite, "constructor add sub"); + test_fail_break_unless (test, a.x == 1 && a.y == 2 && a.z == 3); + test_fail_break_unless (test, b.x == 4 && b.y == 6 && b.z == 8); + ucoo::vect3d c (a, b); + test_fail_break_unless (test, c.x == 3 && c.y == 4 && c.z == 5); + ucoo::vect3d d (b + a); + test_fail_break_unless (test, d.x == 5 && d.y == 8 && d.z == 11); + d = b; + test_fail_break_unless (test, d.x == 4 && d.y == 6 && d.z == 8); + d += a; + test_fail_break_unless (test, d.x == 5 && d.y == 8 && d.z == 11); + d = b - a; + test_fail_break_unless (test, d.x == 3 && d.y == 4 && d.z == 5); + d = b; + d -= a; + test_fail_break_unless (test, d.x == 3 && d.y == 4 && d.z == 5); + d -= d; + test_fail_break_unless (test, d.x == 0 && d.y == 0 && d.z == 0); + test_fail_break_unless (test, a.x == 1 && a.y == 2 && a.z == 3); + test_fail_break_unless (test, b.x == 4 && b.y == 6 && b.z == 8); + } while (0); + do + { + ucoo::Test test (tsuite, "scaling"); + ucoo::vect3d c (a * 2); + test_fail_break_unless (test, c.x == 2 && c.y == 4 && c.z == 6); + c = a; + c *= 3; + test_fail_break_unless (test, c.x == 3 && c.y == 6 && c.z == 9); + c = b / 2; + test_fail_break_unless (test, c.x == 2 && c.y == 3 && c.z == 4); + c = b; + c /= 2; + test_fail_break_unless (test, c.x == 2 && c.y == 3 && c.z == 4); + test_fail_break_unless (test, a.x == 1 && a.y == 2 && a.z == 3); + test_fail_break_unless (test, b.x == 4 && b.y == 6 && b.z == 8); + } while (0); + do + { + ucoo::Test test (tsuite, "dot product"); + T d = a.dot (a); + test_fail_break_unless (test, d == 14); + d = a.dot (b); + test_fail_break_unless (test, d == 40); + test_fail_break_unless (test, a.x == 1 && a.y == 2 && a.z == 3); + test_fail_break_unless (test, b.x == 4 && b.y == 6 && b.z == 8); + } while (0); + do + { + ucoo::Test test (tsuite, "cross product"); + ucoo::vect3d c = a.cross (b); + test_fail_break_unless (test, c.x == -2 && c.y == 4 && c.z == -2); + c = a.cross (a); + test_fail_break_unless (test, c.x == 0 && c.y == 0 && c.z == 0); + test_fail_break_unless (test, a.x == 1 && a.y == 2 && a.z == 3); + test_fail_break_unless (test, b.x == 4 && b.y == 6 && b.z == 8); + } while (0); + do + { + ucoo::Test test (tsuite, "norm"); + T n = a.norm (); + test_fail_break_unless (test, almost_eq (n, 3.74165738677394)); + } while (0); + do + { + ucoo::Test test (tsuite, "unit int"); + ucoo::vect3d c (42, 0, 0); + c = c.unit (); + test_fail_break_unless (test, c.x == 1 && c.y == 0 && c.z == 0); + } while (0); + if (static_cast (0.1) != 0) + { + do + { + ucoo::Test test (tsuite, "unit"); + ucoo::vect3d c = a.unit (); + test_fail_break_unless (test, almost_eq (c.x, 0.2672612419124244) + && almost_eq (c.y, 0.53452248382484879) + && almost_eq (c.z, 0.80178372573727319)); + } while (0); + } +} + +int +main (int argc, const char **argv) +{ + ucoo::arch_init (argc, argv); + ucoo::TestSuite tsuite ("math"); + test_group_vect3d (tsuite, "vect3d int"); + test_group_vect3d (tsuite, "vect3d float"); + test_group_vect3d (tsuite, "vect3d double"); + return tsuite.report () ? 0 : 1; +} diff --git a/ucoo/math/vect3d.hh b/ucoo/math/vect3d.hh new file mode 100644 index 0000000..0cbc97f --- /dev/null +++ b/ucoo/math/vect3d.hh @@ -0,0 +1,69 @@ +#ifndef ucoo_math_vect3d_hh +#define ucoo_math_vect3d_hh +// ucoolib - Microcontroller object oriented library. {{{ +// +// Copyright (C) 2015 Nicolas Schodet +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. +// +// }}} + +namespace ucoo { + +template +struct vect3d +{ + T x, y, z; + public: + /// Default constructor, coordinates are default initialized. + constexpr vect3d (); + /// Constructor from coordinates. + constexpr vect3d (T x_, T y_, T z_); + /// Constructor from two points (B - A). + vect3d (const vect3d &a, const vect3d &b); + /// Addition (translate). + vect3d operator+ (const vect3d &o) const; + vect3d operator+= (const vect3d &o); + /// Subtraction. + vect3d operator- (const vect3d &o) const; + vect3d operator-= (const vect3d &o); + /// Scaling. + template + vect3d operator* (U s) const; + template + vect3d operator*= (U s); + template + vect3d operator/ (U s) const; + template + vect3d operator/= (U s); + /// Dot product. + T dot (const vect3d &o) const; + /// Cross product. + vect3d cross (const vect3d &o) const; + /// Euclidean norm. + T norm () const; + /// Return a vector of norm 1. + vect3d unit () const; +}; + +} // namespace ucoo + +#include "vect3d.tcc" + +#endif // ucoo_math_vect3d_hh diff --git a/ucoo/math/vect3d.tcc b/ucoo/math/vect3d.tcc new file mode 100644 index 0000000..cb698a8 --- /dev/null +++ b/ucoo/math/vect3d.tcc @@ -0,0 +1,148 @@ +#ifndef ucoo_math_vect3d_tcc +#define ucoo_math_vect3d_tcc +// ucoolib - Microcontroller object oriented library. {{{ +// +// Copyright (C) 2015 Nicolas Schodet +// +// Permission is hereby granted, free of charge, to any person obtaining a +// copy of this software and associated documentation files (the "Software"), +// to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, +// and/or sell copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +// DEALINGS IN THE SOFTWARE. +// +// }}} + +#include + +namespace ucoo { + +template +constexpr +vect3d::vect3d () +{ +} + +template +constexpr +vect3d::vect3d (T x_, T y_, T z_) + : x (x_), y (y_), z (z_) +{ +} + +template +vect3d::vect3d (const vect3d &a, const vect3d &b) +{ + *this = b - a; +} + +template +vect3d +vect3d::operator+ (const vect3d &o) const +{ + return vect3d (x + o.x, y + o.y, z + o.z); +} + +template +vect3d +vect3d::operator+= (const vect3d &o) +{ + *this = *this + o; + return *this; +} + +template +vect3d +vect3d::operator- (const vect3d &o) const +{ + return vect3d (x - o.x, y - o.y, z - o.z); +} + +template +vect3d +vect3d::operator-= (const vect3d &o) +{ + *this = *this - o; + return *this; +} + +template +template +vect3d +vect3d::operator* (U s) const +{ + return vect3d (x * s, y * s, z * s); +} + +template +template +vect3d +vect3d::operator*= (U s) +{ + *this = *this * s; + return *this; +} + +template +template +vect3d +vect3d::operator/ (U s) const +{ + return vect3d (x / s, y / s, z / s); +} + +template +template +vect3d +vect3d::operator/= (U s) +{ + *this = *this / s; + return *this; +} + +template +T +vect3d::dot (const vect3d &o) const +{ + return x * o.x + y * o.y + z * o.z; +} + +template +vect3d +vect3d::cross (const vect3d &o) const +{ + return vect3d ( + y * o.z - z * o.y, + z * o.x - x * o.z, + x * o.y - y * o.x + ); +} + +template +T +vect3d::norm () const +{ + return std::sqrt (dot (*this)); +} + +template +vect3d +vect3d::unit () const +{ + return *this / norm (); +} + +} // namespace ucoo + +#endif // ucoo_math_vect3d_tcc -- cgit v1.2.3