summaryrefslogtreecommitdiff
path: root/ucoo/math
diff options
context:
space:
mode:
authorNicolas Schodet2015-07-29 15:44:05 +0200
committerNicolas Schodet2019-10-07 00:44:50 +0200
commit6718de7e40e35e8e66d5311d51acd7944139143e (patch)
tree6f92b2bb0bd0a218c50f6b01b80abb28dde09e89 /ucoo/math
parent6152c3dd8879e8dd0ecb354b442d16123c1de3db (diff)
ucoo/math: add 3D vector
Diffstat (limited to 'ucoo/math')
-rw-r--r--ucoo/math/Module0
-rw-r--r--ucoo/math/test/Makefile9
-rw-r--r--ucoo/math/test/test_math.cc164
-rw-r--r--ucoo/math/vect3d.hh69
-rw-r--r--ucoo/math/vect3d.tcc148
5 files changed, 390 insertions, 0 deletions
diff --git a/ucoo/math/Module b/ucoo/math/Module
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/ucoo/math/Module
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 <cmath>
+#include <cfloat>
+
+template<typename T>
+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<typename T>
+void
+test_group_vect3d (ucoo::TestSuite &tsuite, const char *tname)
+{
+ tsuite.group (tname);
+ ucoo::vect3d<T> a (1, 2, 3);
+ ucoo::vect3d<T> 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<T> c (a, b);
+ test_fail_break_unless (test, c.x == 3 && c.y == 4 && c.z == 5);
+ ucoo::vect3d<T> 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<T> 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<T> 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<T> 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<T> (0.1) != 0)
+ {
+ do
+ {
+ ucoo::Test test (tsuite, "unit");
+ ucoo::vect3d<T> 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<int> (tsuite, "vect3d int");
+ test_group_vect3d<float> (tsuite, "vect3d float");
+ test_group_vect3d<double> (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<typename T>
+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<T> &a, const vect3d<T> &b);
+ /// Addition (translate).
+ vect3d<T> operator+ (const vect3d<T> &o) const;
+ vect3d<T> operator+= (const vect3d<T> &o);
+ /// Subtraction.
+ vect3d<T> operator- (const vect3d<T> &o) const;
+ vect3d<T> operator-= (const vect3d<T> &o);
+ /// Scaling.
+ template<typename U>
+ vect3d<T> operator* (U s) const;
+ template<typename U>
+ vect3d<T> operator*= (U s);
+ template<typename U>
+ vect3d<T> operator/ (U s) const;
+ template<typename U>
+ vect3d<T> operator/= (U s);
+ /// Dot product.
+ T dot (const vect3d<T> &o) const;
+ /// Cross product.
+ vect3d<T> cross (const vect3d<T> &o) const;
+ /// Euclidean norm.
+ T norm () const;
+ /// Return a vector of norm 1.
+ vect3d<T> 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 <cmath>
+
+namespace ucoo {
+
+template<typename T>
+constexpr
+vect3d<T>::vect3d ()
+{
+}
+
+template<typename T>
+constexpr
+vect3d<T>::vect3d (T x_, T y_, T z_)
+ : x (x_), y (y_), z (z_)
+{
+}
+
+template<typename T>
+vect3d<T>::vect3d (const vect3d<T> &a, const vect3d<T> &b)
+{
+ *this = b - a;
+}
+
+template<typename T>
+vect3d<T>
+vect3d<T>::operator+ (const vect3d<T> &o) const
+{
+ return vect3d<T> (x + o.x, y + o.y, z + o.z);
+}
+
+template<typename T>
+vect3d<T>
+vect3d<T>::operator+= (const vect3d<T> &o)
+{
+ *this = *this + o;
+ return *this;
+}
+
+template<typename T>
+vect3d<T>
+vect3d<T>::operator- (const vect3d<T> &o) const
+{
+ return vect3d<T> (x - o.x, y - o.y, z - o.z);
+}
+
+template<typename T>
+vect3d<T>
+vect3d<T>::operator-= (const vect3d<T> &o)
+{
+ *this = *this - o;
+ return *this;
+}
+
+template<typename T>
+template<typename U>
+vect3d<T>
+vect3d<T>::operator* (U s) const
+{
+ return vect3d<T> (x * s, y * s, z * s);
+}
+
+template<typename T>
+template<typename U>
+vect3d<T>
+vect3d<T>::operator*= (U s)
+{
+ *this = *this * s;
+ return *this;
+}
+
+template<typename T>
+template<typename U>
+vect3d<T>
+vect3d<T>::operator/ (U s) const
+{
+ return vect3d<T> (x / s, y / s, z / s);
+}
+
+template<typename T>
+template<typename U>
+vect3d<T>
+vect3d<T>::operator/= (U s)
+{
+ *this = *this / s;
+ return *this;
+}
+
+template<typename T>
+T
+vect3d<T>::dot (const vect3d<T> &o) const
+{
+ return x * o.x + y * o.y + z * o.z;
+}
+
+template<typename T>
+vect3d<T>
+vect3d<T>::cross (const vect3d<T> &o) const
+{
+ return vect3d<T> (
+ y * o.z - z * o.y,
+ z * o.x - x * o.z,
+ x * o.y - y * o.x
+ );
+}
+
+template<typename T>
+T
+vect3d<T>::norm () const
+{
+ return std::sqrt (dot (*this));
+}
+
+template<typename T>
+vect3d<T>
+vect3d<T>::unit () const
+{
+ return *this / norm ();
+}
+
+} // namespace ucoo
+
+#endif // ucoo_math_vect3d_tcc