diff --git a/include/onogawa/compass.hpp b/include/onogawa/compass.hpp new file mode 100644 index 0000000..c620415 --- /dev/null +++ b/include/onogawa/compass.hpp @@ -0,0 +1,22 @@ +#pragma once + +namespace onogawa { + +template struct Compass { + T x; + T y; + + constexpr Compass(T x, T y) : x(x), y(y) {} + + constexpr static Compass ex() { return Compass(1.0, 0.0); } + constexpr static Compass ey() { return Compass(0.0, 1.0); } +}; + +template +constexpr Compass operator*(const Compass &a, const Compass &b) { + return Compass(a.x * b.x - a.y * b.y, a.x * b.y + a.y * b.x); +} + +using Compassd = Compass; + +} // namespace onogawa diff --git a/include/onogawa/onogawa.hpp b/include/onogawa/onogawa.hpp index 0ffd9b0..3bfa161 100644 --- a/include/onogawa/onogawa.hpp +++ b/include/onogawa/onogawa.hpp @@ -1,145 +1,11 @@ #pragma once -namespace onogawa { - -template struct Compass { - T x; - T y; - - constexpr Compass(T x, T y) : x(x), y(y) {} - - constexpr static Compass ex() { return Compass(1.0, 0.0); } - constexpr static Compass ey() { return Compass(0.0, 1.0); } -}; - -template -constexpr Compass operator*(const Compass &a, const Compass &b) { - return Compass(a.x * b.x - a.y * b.y, a.x * b.y + a.y * b.x); -} - -template struct Point2 { - T x; - T y; - - constexpr Point2(T x, T y) : x(x), y(y) {} - - constexpr static Point2 ex() { return Point2(1.0, 0.0); } - constexpr static Point2 ey() { return Point2(0.0, 1.0); } -}; - -template -constexpr Point2 operator+(const Point2 &a, const Point2 &b) { - return Point2(a.x + b.x, a.y + b.y); -} - -template -constexpr Point2 operator*(const Compass &compass, - const Point2 &point) { - return Point2(compass.x * point.x - compass.y * point.y, - compass.x * point.y - compass.y * point.x); -} - -template struct Pose2 { - Compass orientation; - Point2 position; - - constexpr Pose2(const Compass &orientation, const Point2 &position) - : orientation(orientation), position(position) {} -}; - -template -constexpr Pose2 operator*(const Pose2 &a, const Pose2 &b) { - return Pose2(a.orientation * b.orientation, - a.orientation * b.position + a.position); -} - -template struct Quaternion { - T w; - T x; - T y; - T z; - - constexpr Quaternion(T w, T x, T y, T z) : w(w), x(x), y(y), z(z) {} - - constexpr static Quaternion ew() { return Quaternion(1.0, 0.0, 0.0, 0.0); }; - constexpr static Quaternion ex() { return Quaternion(0.0, 1.0, 0.0, 0.0); }; - constexpr static Quaternion ey() { return Quaternion(0.0, 0.0, 1.0, 0.0); }; - constexpr static Quaternion ez() { return Quaternion(0.0, 0.0, 0.0, 1.0); }; -}; - -template -constexpr Quaternion operator*(const Quaternion &a, - const Quaternion &b) { - return Quaternion(a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z, - a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, - a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, - a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w); -} - -template struct Point3 { - T x; - T y; - T z; - - constexpr Point3(T x, T y, T z) : x(x), y(y), z(z) {} - - constexpr static Point3 ex() { return Point3(1.0, 0.0, 0.0); } - constexpr static Point3 ey() { return Point3(0.0, 1.0, 0.0); } - constexpr static Point3 ez() { return Point3(0.0, 0.0, 1.0); } -}; - -template -constexpr Point3 operator+(const Point3 &a, const Point3 &b) { - return Point3(a.x + b.x, a.y + b.y, a.z + b.z); -} - -template -constexpr Point3 operator*(const Quaternion &q, const Point3 &point) { - const T s = 2.0 / (q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); - - const T xs = q.x * s; - const T ys = q.y * s; - const T zs = q.z * s; - - const T wxs = q.w * xs; - const T wys = q.w * ys; - const T wzs = q.w * zs; - - const T xxs = q.x * xs; - const T xys = q.x * ys; - const T xzs = q.x * zs; - - const T yys = q.y * ys; - const T yzs = q.y * zs; - - const T zzs = q.z * zs; - - return Point3( - (1 - yys - zzs) * point.x + (xys - wzs) * point.y + (xzs + wys) * point.z, - (xys + wzs) * point.x + (1 - xxs - zzs) * point.y + (yzs - wxs) * point.z, - (xzs - wys) * point.x + (yzs + wxs) * point.y + - (1 - xxs - yys) * point.z); -} - -template struct Pose3 { - Quaternion orientation; - Point3 position; - - constexpr Pose3(const Quaternion &orientation, const Point3 &position) - : orientation(orientation), position(position) {} -}; - -template -constexpr Pose3 operator*(const Pose3 &a, const Pose3 &b) { - return Pose3(a.orientation * b.orientation, - a.orientation * b.position + a.position); -} - -using Compassd = Compass; -using Point2d = Point2; -using Pose2d = Pose2; -using Quaterniond = Quaternion; -using Point3d = Point3; -using Pose3d = Pose3; - -} // namespace onogawa +// 2-D +#include "onogawa/compass.hpp" +#include "onogawa/point2.hpp" +#include "onogawa/pose2.hpp" + +// 3-D +#include "onogawa/point3.hpp" +#include "onogawa/pose3.hpp" +#include "onogawa/quaternion.hpp" diff --git a/include/onogawa/point2.hpp b/include/onogawa/point2.hpp new file mode 100644 index 0000000..5510d28 --- /dev/null +++ b/include/onogawa/point2.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include "onogawa/compass.hpp" + +namespace onogawa { + +template struct Point2 { + T x; + T y; + + constexpr Point2(T x, T y) : x(x), y(y) {} + + constexpr static Point2 ex() { return Point2(1.0, 0.0); } + constexpr static Point2 ey() { return Point2(0.0, 1.0); } +}; + +template +constexpr Point2 operator+(const Point2 &a, const Point2 &b) { + return Point2(a.x + b.x, a.y + b.y); +} + +template +constexpr Point2 operator*(const Compass &compass, + const Point2 &point) { + return Point2(compass.x * point.x - compass.y * point.y, + compass.x * point.y - compass.y * point.x); +} + +using Point2d = Point2; + +} // namespace onogawa \ No newline at end of file diff --git a/include/onogawa/point3.hpp b/include/onogawa/point3.hpp new file mode 100644 index 0000000..a076013 --- /dev/null +++ b/include/onogawa/point3.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include "onogawa/quaternion.hpp" + +namespace onogawa { + +template struct Point3 { + T x; + T y; + T z; + + constexpr Point3(T x, T y, T z) : x(x), y(y), z(z) {} + + constexpr static Point3 ex() { return Point3(1.0, 0.0, 0.0); } + constexpr static Point3 ey() { return Point3(0.0, 1.0, 0.0); } + constexpr static Point3 ez() { return Point3(0.0, 0.0, 1.0); } +}; + +template +constexpr Point3 operator+(const Point3 &a, const Point3 &b) { + return Point3(a.x + b.x, a.y + b.y, a.z + b.z); +} + +template +constexpr Point3 operator*(const Quaternion &q, const Point3 &point) { + const T s = 2.0 / (q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); + + const T xs = q.x * s; + const T ys = q.y * s; + const T zs = q.z * s; + + const T wxs = q.w * xs; + const T wys = q.w * ys; + const T wzs = q.w * zs; + + const T xxs = q.x * xs; + const T xys = q.x * ys; + const T xzs = q.x * zs; + + const T yys = q.y * ys; + const T yzs = q.y * zs; + + const T zzs = q.z * zs; + + return Point3( + (1 - yys - zzs) * point.x + (xys - wzs) * point.y + (xzs + wys) * point.z, + (xys + wzs) * point.x + (1 - xxs - zzs) * point.y + (yzs - wxs) * point.z, + (xzs - wys) * point.x + (yzs + wxs) * point.y + + (1 - xxs - yys) * point.z); +} + +using Point3d = Point3; + +} // namespace onogawa diff --git a/include/onogawa/pose2.hpp b/include/onogawa/pose2.hpp new file mode 100644 index 0000000..f8667e3 --- /dev/null +++ b/include/onogawa/pose2.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "onogawa/compass.hpp" +#include "onogawa/point2.hpp" + +namespace onogawa { + +template struct Pose2 { + Compass orientation; + Point2 position; + + constexpr Pose2(const Compass &orientation, const Point2 &position) + : orientation(orientation), position(position) {} +}; + +template +constexpr Pose2 operator*(const Pose2 &a, const Pose2 &b) { + return Pose2(a.orientation * b.orientation, + a.orientation * b.position + a.position); +} + +using Pose2d = Pose2; + +} // namespace onogawa diff --git a/include/onogawa/pose3.hpp b/include/onogawa/pose3.hpp new file mode 100644 index 0000000..087957b --- /dev/null +++ b/include/onogawa/pose3.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "onogawa/point3.hpp" +#include "onogawa/quaternion.hpp" + +namespace onogawa { + +template struct Pose3 { + Quaternion orientation; + Point3 position; + + constexpr Pose3(const Quaternion &orientation, const Point3 &position) + : orientation(orientation), position(position) {} +}; + +template +constexpr Pose3 operator*(const Pose3 &a, const Pose3 &b) { + return Pose3(a.orientation * b.orientation, + a.orientation * b.position + a.position); +} + +using Pose3d = Pose3; + +} // namespace onogawa \ No newline at end of file diff --git a/include/onogawa/quaternion.hpp b/include/onogawa/quaternion.hpp new file mode 100644 index 0000000..4984d5e --- /dev/null +++ b/include/onogawa/quaternion.hpp @@ -0,0 +1,30 @@ +#pragma once + +namespace onogawa { + +template struct Quaternion { + T w; + T x; + T y; + T z; + + constexpr Quaternion(T w, T x, T y, T z) : w(w), x(x), y(y), z(z) {} + + constexpr static Quaternion ew() { return Quaternion(1.0, 0.0, 0.0, 0.0); }; + constexpr static Quaternion ex() { return Quaternion(0.0, 1.0, 0.0, 0.0); }; + constexpr static Quaternion ey() { return Quaternion(0.0, 0.0, 1.0, 0.0); }; + constexpr static Quaternion ez() { return Quaternion(0.0, 0.0, 0.0, 1.0); }; +}; + +template +constexpr Quaternion operator*(const Quaternion &a, + const Quaternion &b) { + return Quaternion(a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z, + a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, + a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, + a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w); +} + +using Quaterniond = Quaternion; + +} // namespace onogawa \ No newline at end of file