Test suite cleanups, convert Coord2D to Vector<T, 2>.
- The standard SimpleSuite setup now include flags to suppress printing the report in addition to silencing the test runs. This is useful in automated testing. - Point2D and Polar2D in Coord2D have been converted from custom types to Vector<int, 2> and Vector<double, 2>, respectively.
This commit is contained in:
parent
68ed5e0aca
commit
4b1007123a
|
|
@ -84,12 +84,18 @@ double RotateRadians(double theta0, double theta1);
|
|||
/// \param epsilon The variable to store the epsilon value in.
|
||||
void DefaultEpsilon(double &epsilon);
|
||||
|
||||
/// Get the default epsilon value.
|
||||
/// \brief Get the default epsilon value.
|
||||
///
|
||||
/// \param epsilon The variable to store the epsilon value in.
|
||||
void DefaultEpsilon(float &epsilon);
|
||||
|
||||
|
||||
/// \brief Get the default epsilon for integer types.
|
||||
///
|
||||
/// \param epsilon The variable to store the epsilon value in.
|
||||
void DefaultEpsilon(int& epsilon);
|
||||
|
||||
|
||||
/// \brief Return whether the two values of type T are equal to within
|
||||
/// some tolerance.
|
||||
///
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
///
|
||||
/// \file Madgwick.h
|
||||
/// \file include/scmp/filter/Madgwick.h
|
||||
/// \author K. Isom <kyle@imap.cc>
|
||||
/// \date 2019-08-06
|
||||
/// \brief Implementation of a Madgwick filter.
|
||||
|
|
|
|||
|
|
@ -28,6 +28,8 @@
|
|||
#include <ostream>
|
||||
#include <vector>
|
||||
|
||||
#include <scmp/geom/Vector.h>
|
||||
|
||||
|
||||
namespace scmp {
|
||||
namespace geom {
|
||||
|
|
@ -39,61 +41,79 @@ class Polar2D;
|
|||
|
||||
/// \brief Point2D is a logical grouping of a set of 2D cartesian
|
||||
/// coordinates.
|
||||
class Point2D {
|
||||
class Point2D : public Vector<int, 2> {
|
||||
public:
|
||||
int x, y;
|
||||
|
||||
// A Point2D can be initialised by setting its members to 0, by providing the
|
||||
// x and y coordiantes, or through translation from a polar coordinate.
|
||||
/// \brief A Point2D defaults to (0,0).
|
||||
Point2D();
|
||||
Point2D(int _x, int _y);
|
||||
Point2D(const Polar2D &);
|
||||
|
||||
/// \brief Initialize a Point2D at (_x, _y).
|
||||
Point2D(int _x, int _y);
|
||||
|
||||
/// \brief Initialize a Point2D from a Polar2D coordinate.
|
||||
Point2D(const Polar2D &pol);
|
||||
|
||||
/// \brief Return the X component of the point.
|
||||
int X() const;
|
||||
|
||||
/// \brief Set the X component of the point.
|
||||
void X(int _x);
|
||||
|
||||
/// \brief Return the Y component of the point.
|
||||
int Y() const;
|
||||
|
||||
/// Set the Y component of the point.
|
||||
void Y(int _y);
|
||||
|
||||
/// \brief ToString returns a string in the format (x,y).
|
||||
std::string ToString();
|
||||
|
||||
/// \brief ToPolar converts the Point2D to a polar coordinate
|
||||
/// in-place.
|
||||
void ToPolar(Polar2D &);
|
||||
|
||||
// Rotate rotates the point by theta radians. Alternatively, a rotation
|
||||
// can use this point as the centre, with a polar coordinate and a rotation
|
||||
// amount (in radians). The latter is used to specify a central point
|
||||
// of rotation with vertices specified as polar coordinates from the centre.
|
||||
// Both forms take a reference to a Point2D to store the rotated point.
|
||||
void Rotate(Point2D &rotated, double theta);
|
||||
std::vector<Point2D> Rotate(std::vector<Polar2D>, double);
|
||||
/// \brief Rotate rotates the point by theta radians.
|
||||
///
|
||||
/// \param rotated Stores the rotated point.
|
||||
/// \param theta The angle (in radians) to rotate the point.
|
||||
void Rotate(Point2D& rotated, double theta);
|
||||
|
||||
// Translate adds this point to the first argument, storing the result in the
|
||||
// second argument.
|
||||
/// \brief Rotate this point around a series of vertices.
|
||||
///
|
||||
/// \param vertices A series of vertices to rotate this point around.
|
||||
/// \param theta The angle to rotate by.
|
||||
/// \return A series of rotated points.
|
||||
std::vector<Point2D> Rotate(std::vector<Polar2D> vertices, double theta);
|
||||
|
||||
/// \brief Translate adds this point to the first argument,
|
||||
/// storing the result in the second argument.
|
||||
///
|
||||
/// \param other The point to translate by.
|
||||
/// \param translated The point to store the translation in.
|
||||
void Translate(const Point2D &other, Point2D &translated);
|
||||
|
||||
// Distance returns the distance from this point to another.
|
||||
int Distance(const Point2D &other);
|
||||
/// \brief Distance returns the distance from this point to another.
|
||||
int Distance(const Point2D &other) const;
|
||||
|
||||
Point2D operator+(const Point2D &rhs) const
|
||||
{ return Point2D(x + rhs.x, y + rhs.y); }
|
||||
Point2D operator-(const Point2D &rhs) const
|
||||
{ return Point2D(x - rhs.x, y - rhs.y); }
|
||||
Point2D operator*(const int k) const
|
||||
{ return Point2D(x * k, y * k); }
|
||||
bool operator==(const Point2D &rhs) const;
|
||||
bool operator!=(const Point2D &rhs) const
|
||||
{ return !(*this == rhs); }
|
||||
friend std::ostream &operator<<(std::ostream &outs, const Point2D &pt);
|
||||
};
|
||||
|
||||
// A Polar2D is a 2D polar coordinate, specified in terms of the radius from
|
||||
// some origin and the angle from the positive X axis of a cartesian coordinate
|
||||
// system.
|
||||
class Polar2D {
|
||||
class Polar2D : public Vector<double, 2> {
|
||||
public:
|
||||
double r, theta;
|
||||
|
||||
// A Polar2D can be initialised as a zeroised polar coordinate, by specifying
|
||||
// the radius and angle directly, or via conversion from a Point2D.
|
||||
Polar2D() : r(0.0), theta(0.0)
|
||||
{}
|
||||
Polar2D(double _r, double _theta) : r(_r), theta(_theta)
|
||||
{}
|
||||
Polar2D();
|
||||
Polar2D(double _r, double _theta);
|
||||
Polar2D(const Point2D &);
|
||||
|
||||
double R() const;
|
||||
void R(const double _r);
|
||||
|
||||
double Theta() const;
|
||||
void Theta(const double _theta);
|
||||
|
||||
std::string ToString();
|
||||
void ToPoint(Point2D &);
|
||||
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#ifndef SCMATH_VECTORS_H
|
||||
#define SCMATH_VECTORS_H
|
||||
|
||||
|
||||
#include <array>
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
|
|
@ -53,15 +54,15 @@ namespace geom {
|
|||
///
|
||||
/// Vectors can be indexed like arrays, and they contain an epsilon value
|
||||
/// that defines a tolerance for equality.
|
||||
template <typename T, size_t N>
|
||||
template<typename T, size_t N>
|
||||
class Vector {
|
||||
public:
|
||||
/// The default constructor creates a unit vector for a given type
|
||||
/// and size.
|
||||
Vector()
|
||||
{
|
||||
T unitLength = (T)1.0 / std::sqrt(N);
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
T unitLength = (T) 1.0 / std::sqrt(N);
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
this->arr[i] = unitLength;
|
||||
}
|
||||
|
||||
|
|
@ -72,7 +73,7 @@ public:
|
|||
/// If given an initializer_list, the vector is created with
|
||||
/// those values. There must be exactly N elements in the list.
|
||||
/// @param ilst An intializer list with N elements of type T.
|
||||
Vector(std::initializer_list<T> ilst)
|
||||
Vector(std::initializer_list<T> ilst)
|
||||
{
|
||||
assert(ilst.size() == N);
|
||||
|
||||
|
|
@ -81,10 +82,51 @@ public:
|
|||
}
|
||||
|
||||
|
||||
/// \brief Return the element at index i.
|
||||
///
|
||||
/// \throws std::out_of_range if the index is out of bounds.
|
||||
///
|
||||
/// \param index The index of the item to retrieve.
|
||||
/// \return The value at the index.
|
||||
T at(size_t index) const
|
||||
{
|
||||
if (index > this->arr.size()) {
|
||||
throw std::out_of_range("index " +
|
||||
std::to_string(index) + " > " +
|
||||
std::to_string(this->arr.size()));
|
||||
}
|
||||
return this->arr.at(index);
|
||||
}
|
||||
|
||||
|
||||
/// \brief Set a new value for the vector.
|
||||
///
|
||||
/// This is used to modify the vector in place.
|
||||
///
|
||||
/// \throws std::out_of_range if the index is out of bounds.
|
||||
///
|
||||
/// \param index The index to insert the value at.
|
||||
/// \param value
|
||||
void Set(size_t index, T value)
|
||||
{
|
||||
if (index > this->arr.size()) {
|
||||
throw std::out_of_range("index " +
|
||||
std::to_string(index) + " > " +
|
||||
std::to_string(this->arr.size()));
|
||||
}
|
||||
|
||||
this->arr[index] = value;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/// Compute the length of the vector.
|
||||
/// @return The length of the vector.
|
||||
T magnitude() const {
|
||||
T result = 0;
|
||||
T magnitude() const
|
||||
{
|
||||
T result = 0;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
result += (this->arr[i] * this->arr[i]);
|
||||
|
|
@ -110,7 +152,7 @@ public:
|
|||
isZero() const
|
||||
{
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (!scmp::WithinTolerance(this->arr[i], (T)0.0, this->epsilon)) {
|
||||
if (!scmp::WithinTolerance(this->arr[i], (T) 0.0, this->epsilon)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
@ -132,7 +174,7 @@ public:
|
|||
bool
|
||||
isUnitVector() const
|
||||
{
|
||||
return scmp::WithinTolerance(this->magnitude(), (T)1.0, this->epsilon);
|
||||
return scmp::WithinTolerance(this->magnitude(), (T) 1.0, this->epsilon);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -142,8 +184,8 @@ public:
|
|||
T
|
||||
angle(const Vector<T, N> &other) const
|
||||
{
|
||||
Vector<T, N> unitA = this->unitVector();
|
||||
Vector<T, N> unitB = other.unitVector();
|
||||
Vector<T, N> unitA = this->unitVector();
|
||||
Vector<T, N> unitB = other.unitVector();
|
||||
|
||||
// Can't compute angles with a zero vector.
|
||||
assert(!this->isZero());
|
||||
|
|
@ -163,7 +205,7 @@ public:
|
|||
}
|
||||
|
||||
T angle = this->angle(other);
|
||||
if (scmp::WithinTolerance(angle, (T)0.0, this->epsilon)) {
|
||||
if (scmp::WithinTolerance(angle, (T) 0.0, this->epsilon)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -182,7 +224,7 @@ public:
|
|||
return true;
|
||||
}
|
||||
|
||||
return scmp::WithinTolerance(*this * other, (T)0.0, this->epsilon);
|
||||
return scmp::WithinTolerance(*this * other, (T) 0.0, this->epsilon);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -193,7 +235,7 @@ public:
|
|||
Vector
|
||||
projectParallel(const Vector<T, N> &basis) const
|
||||
{
|
||||
Vector<T, N> unit_basis = basis.unitVector();
|
||||
Vector<T, N> unit_basis = basis.unitVector();
|
||||
|
||||
return unit_basis * (*this * unit_basis);
|
||||
}
|
||||
|
|
@ -207,7 +249,7 @@ public:
|
|||
Vector
|
||||
projectOrthogonal(const Vector<T, N> &basis)
|
||||
{
|
||||
Vector<T, N> spar = this->projectParallel(basis);
|
||||
Vector<T, N> spar = this->projectParallel(basis);
|
||||
return *this - spar;
|
||||
}
|
||||
|
||||
|
|
@ -220,10 +262,10 @@ public:
|
|||
cross(const Vector<T, N> &other) const
|
||||
{
|
||||
assert(N == 3);
|
||||
return Vector<T, N> {
|
||||
(this->arr[1] * other.arr[2]) - (other.arr[1] * this->arr[2]),
|
||||
-((this->arr[0] * other.arr[2]) - (other.arr[0] * this->arr[2])),
|
||||
(this->arr[0] * other.arr[1]) - (other.arr[0] * this->arr[1])
|
||||
return Vector<T, N>{
|
||||
(this->arr[1] * other.arr[2]) - (other.arr[1] * this->arr[2]),
|
||||
-((this->arr[0] * other.arr[2]) - (other.arr[0] * this->arr[2])),
|
||||
(this->arr[0] * other.arr[1]) - (other.arr[0] * this->arr[1])
|
||||
};
|
||||
}
|
||||
|
||||
|
|
@ -235,7 +277,7 @@ public:
|
|||
Vector
|
||||
operator+(const Vector<T, N> &other) const
|
||||
{
|
||||
Vector<T, N> vec;
|
||||
Vector<T, N> vec;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
vec.arr[i] = this->arr[i] + other.arr[i];
|
||||
|
|
@ -252,7 +294,7 @@ public:
|
|||
Vector
|
||||
operator-(const Vector<T, N> &other) const
|
||||
{
|
||||
Vector<T, N> vec;
|
||||
Vector<T, N> vec;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
vec.arr[i] = this->arr[i] - other.arr[i];
|
||||
|
|
@ -268,7 +310,7 @@ public:
|
|||
Vector
|
||||
operator*(const T k) const
|
||||
{
|
||||
Vector<T, N> vec;
|
||||
Vector<T, N> vec;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
vec.arr[i] = this->arr[i] * k;
|
||||
|
|
@ -284,7 +326,7 @@ public:
|
|||
Vector
|
||||
operator/(const T k) const
|
||||
{
|
||||
Vector<T, N> vec;
|
||||
Vector<T, N> vec;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
vec.arr[i] = this->arr[i] / k;
|
||||
|
|
@ -300,7 +342,7 @@ public:
|
|||
T
|
||||
operator*(const Vector<T, N> &other) const
|
||||
{
|
||||
T result = 0;
|
||||
T result = 0;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
result += (this->arr[i] * other.arr[i]);
|
||||
|
|
@ -317,7 +359,7 @@ public:
|
|||
bool
|
||||
operator==(const Vector<T, N> &other) const
|
||||
{
|
||||
for (size_t i = 0; i<N; i++) {
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (!scmp::WithinTolerance(this->arr[i], other.arr[i], this->epsilon)) {
|
||||
return false;
|
||||
}
|
||||
|
|
@ -349,7 +391,7 @@ public:
|
|||
///
|
||||
/// @param i The component index.
|
||||
/// @return The value of the vector component at i.
|
||||
const T&
|
||||
const T &
|
||||
operator[](size_t i) const
|
||||
{
|
||||
return this->arr[i];
|
||||
|
|
@ -360,13 +402,13 @@ public:
|
|||
/// @param outs An output stream.
|
||||
/// @param vec The vector to be formatted.
|
||||
/// @return The output stream.
|
||||
friend std::ostream&
|
||||
operator<<(std::ostream& outs, const Vector<T, N>& vec)
|
||||
friend std::ostream &
|
||||
operator<<(std::ostream &outs, const Vector<T, N> &vec)
|
||||
{
|
||||
outs << "<";
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
outs << vec.arr[i];
|
||||
if (i < (N-1)) {
|
||||
if (i < (N - 1)) {
|
||||
outs << ", ";
|
||||
}
|
||||
}
|
||||
|
|
@ -375,9 +417,9 @@ public:
|
|||
}
|
||||
|
||||
private:
|
||||
static const size_t dim = N;
|
||||
T epsilon;
|
||||
std::array<T, N> arr;
|
||||
static const size_t dim = N;
|
||||
T epsilon;
|
||||
std::array<T, N> arr;
|
||||
};
|
||||
|
||||
///
|
||||
|
|
@ -391,27 +433,27 @@ private:
|
|||
|
||||
/// \ingroup vector_aliases
|
||||
/// @brief Type alias for a two-dimensional float vector.
|
||||
typedef Vector<float, 2> Vector2f;
|
||||
typedef Vector<float, 2> Vector2f;
|
||||
|
||||
/// \ingroup vector_aliases
|
||||
/// @brief Type alias for a three-dimensional float vector.
|
||||
typedef Vector<float, 3> Vector3f;
|
||||
typedef Vector<float, 3> Vector3f;
|
||||
|
||||
/// \ingroup vector_aliases
|
||||
/// @brief Type alias for a four-dimensional float vector.
|
||||
typedef Vector<float, 4> Vector4f;
|
||||
typedef Vector<float, 4> Vector4f;
|
||||
|
||||
/// \ingroup vector_aliases
|
||||
/// @brief Type alias for a two-dimensional double vector.
|
||||
typedef Vector<double, 2> Vector2d;
|
||||
typedef Vector<double, 2> Vector2d;
|
||||
|
||||
/// \ingroup vector_aliases
|
||||
/// @brief Type alias for a three-dimensional double vector.
|
||||
typedef Vector<double, 3> Vector3d;
|
||||
typedef Vector<double, 3> Vector3d;
|
||||
|
||||
/// \ingroup vector_aliases
|
||||
/// @brief Type alias for a four-dimensional double vector.
|
||||
typedef Vector<double, 4> Vector4d;
|
||||
typedef Vector<double, 4> Vector4d;
|
||||
|
||||
|
||||
} // namespace geom
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
|
||||
#include <scmp/Math.h>
|
||||
#include <scmp/geom/Coord2D.h>
|
||||
#include <scmp/geom/Vector.h>
|
||||
|
||||
|
||||
// coord2d.cpp contains 2D geometric functions and data structures, such as
|
||||
|
|
@ -43,19 +44,52 @@ namespace geom {
|
|||
// Point2D
|
||||
|
||||
|
||||
Point2D::Point2D() : x(0), y(0) {}
|
||||
Point2D::Point2D() : Vector<int, 2>{0, 0}
|
||||
{
|
||||
|
||||
Point2D::Point2D(int _x, int _y) : x(_x), y(_y) {}
|
||||
};
|
||||
|
||||
Point2D::Point2D(int _x, int _y) : Vector<int, 2>{_x, _y}
|
||||
{}
|
||||
|
||||
Point2D::Point2D(const Polar2D &pol)
|
||||
: x(std::rint(std::cos(pol.theta) * pol.r)),
|
||||
y(std::rint(std::sin(pol.theta) * pol.r)) {}
|
||||
: Vector<int, 2>{static_cast<int>(std::rint(std::cos(pol.Theta()) * pol.R())),
|
||||
static_cast<int>(std::rint(std::sin(pol.Theta()) * pol.R()))}
|
||||
{}
|
||||
|
||||
|
||||
std::ostream&
|
||||
operator<<(std::ostream& outs, const Point2D& pt)
|
||||
int
|
||||
Point2D::X() const
|
||||
{
|
||||
outs << "(" << std::to_string(pt.x) << ", " << std::to_string(pt.y) << ")";
|
||||
return this->at(0);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Point2D::X(int _x)
|
||||
{
|
||||
this->Set(0, _x);
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
Point2D::Y() const
|
||||
{
|
||||
return this->at(1);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Point2D::Y(int _y)
|
||||
{
|
||||
this->Set(1, _y);
|
||||
}
|
||||
|
||||
|
||||
std::ostream &
|
||||
operator<<(std::ostream &outs, const Point2D &pt)
|
||||
{
|
||||
outs << "(" << std::to_string(pt[0]) << ", " << std::to_string(pt[1]) << ")";
|
||||
return outs;
|
||||
}
|
||||
|
||||
|
|
@ -63,51 +97,43 @@ operator<<(std::ostream& outs, const Point2D& pt)
|
|||
std::string
|
||||
Point2D::ToString()
|
||||
{
|
||||
return "(" + std::to_string(x) + ", " + std::to_string(y) + ")";
|
||||
return "(" + std::to_string(this->X()) + ", " + std::to_string(this->Y()) + ")";
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Point2D::ToPolar(Polar2D& pol)
|
||||
Point2D::ToPolar(Polar2D &pol)
|
||||
{
|
||||
pol.r = std::sqrt((x * x) + (y * y));
|
||||
pol.theta = std::atan2(y, x);
|
||||
pol.R(std::sqrt(this->X() * this->X() + this->Y() * this->Y()));
|
||||
pol.Theta(std::atan2(this->Y(), this->X()));
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Point2D::Rotate(Point2D& pt, double theta)
|
||||
Point2D::Rotate(Point2D &pt, double theta)
|
||||
{
|
||||
Polar2D pol(*this);
|
||||
Polar2D pol(*this);
|
||||
pol.Rotate(pol, theta);
|
||||
pol.ToPoint(pt);
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
Point2D::operator==(const Point2D& rhs) const
|
||||
{
|
||||
return (x == rhs.x) && (y == rhs.y);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Point2D::Translate(const Point2D& origin, Point2D &translated)
|
||||
Point2D::Translate(const Point2D &origin, Point2D &translated)
|
||||
{
|
||||
translated.x = origin.x + x;
|
||||
translated.y = origin.y + y;
|
||||
translated.X(origin.X() + this->X());
|
||||
translated.Y(origin.Y() + this->Y());
|
||||
}
|
||||
|
||||
|
||||
std::vector<Point2D>
|
||||
Point2D::Rotate(std::vector<Polar2D> vertices, double theta)
|
||||
{
|
||||
std::vector<Point2D> rotated;
|
||||
std::vector<Point2D> rotated;
|
||||
|
||||
for (auto& v : vertices) {
|
||||
Point2D p;
|
||||
for (auto &v: vertices) {
|
||||
Point2D p;
|
||||
v.RotateAround(*this, p, theta);
|
||||
rotated.push_back(p) ;
|
||||
rotated.push_back(p);
|
||||
}
|
||||
|
||||
return rotated;
|
||||
|
|
@ -115,70 +141,105 @@ Point2D::Rotate(std::vector<Polar2D> vertices, double theta)
|
|||
|
||||
|
||||
int
|
||||
Point2D::Distance(const Point2D& other)
|
||||
Point2D::Distance(const Point2D& other) const
|
||||
{
|
||||
auto dx = other.x - x;
|
||||
auto dy = other.y - y;
|
||||
return std::sqrt(dx * dx + dy + dy);
|
||||
auto dx = other.X() - this->X();
|
||||
auto dy = other.Y() - this->Y();
|
||||
return static_cast<int>(std::rint(std::sqrt(dx * dx + dy * dy)));
|
||||
}
|
||||
|
||||
|
||||
// Polar2D
|
||||
|
||||
Polar2D::Polar2D() : Vector<double, 2>{0.0, 0.0} {};
|
||||
|
||||
Polar2D::Polar2D(double _r, double _theta) : Vector<double, 2>{_r, _theta}
|
||||
{}
|
||||
|
||||
Polar2D::Polar2D(const Point2D &pt)
|
||||
: r(std::sqrt((pt.x * pt.x) + (pt.y * pt.y))),
|
||||
theta(std::atan2(pt.y, pt.x)) {}
|
||||
: Vector<double, 2>{std::sqrt((pt.X() * pt.X()) + (pt.Y() * pt.Y())),
|
||||
std::atan2(pt.Y(), pt.X())}
|
||||
{}
|
||||
|
||||
|
||||
double
|
||||
Polar2D::R() const
|
||||
{
|
||||
return this->at(0);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Polar2D::ToPoint(Point2D& pt)
|
||||
Polar2D::R(const double _r)
|
||||
{
|
||||
pt.y = std::rint(std::sin(theta) * r);
|
||||
pt.x = std::rint(std::cos(theta) * r);
|
||||
this->Set(0, _r);
|
||||
}
|
||||
|
||||
|
||||
double
|
||||
Polar2D::Theta() const
|
||||
{
|
||||
return this->at(1);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Polar2D::Theta(const double _theta)
|
||||
{
|
||||
this->Set(1, _theta);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Polar2D::ToPoint(Point2D &pt)
|
||||
{
|
||||
pt.Y(std::rint(std::sin(this->Theta()) * this->R()));
|
||||
pt.X(std::rint(std::cos(this->Theta()) * this->R()));
|
||||
}
|
||||
|
||||
|
||||
std::string
|
||||
Polar2D::ToString()
|
||||
{
|
||||
return "(" + std::to_string(r) + ", " + std::to_string(theta) + ")";
|
||||
return "(" + std::to_string(this->R()) +
|
||||
", " + std::to_string(this->Theta()) + ")";
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Polar2D::Rotate(Polar2D& rot, double delta)
|
||||
Polar2D::Rotate(Polar2D &rot, double delta)
|
||||
{
|
||||
rot.r = r;
|
||||
rot.theta = RotateRadians(theta, delta);
|
||||
rot.R(this->R());
|
||||
rot.Theta(RotateRadians(this->Theta(), delta));
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
Polar2D::operator==(const Polar2D& rhs) const
|
||||
Polar2D::operator==(const Polar2D &rhs) const
|
||||
{
|
||||
static double eps = 0.0;
|
||||
if (eps == 0.0) {
|
||||
scmp::DefaultEpsilon(eps);
|
||||
}
|
||||
return scmp::WithinTolerance(r, rhs.r, eps) &&
|
||||
scmp::WithinTolerance(theta, rhs.theta, eps);
|
||||
return scmp::WithinTolerance(this->R(), rhs.R(), eps) &&
|
||||
scmp::WithinTolerance(this->Theta(), rhs.Theta(), eps);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Polar2D::RotateAround(const Point2D &origin, Point2D &point, double delta)
|
||||
{
|
||||
Polar2D rot;
|
||||
Polar2D rot;
|
||||
this->Rotate(rot, delta);
|
||||
rot.ToPoint(point);
|
||||
rot.ToPoint(point);
|
||||
point.Translate(origin, point);
|
||||
}
|
||||
|
||||
|
||||
std::ostream&
|
||||
operator<<(std::ostream& outs, const Polar2D& pol)
|
||||
std::ostream &
|
||||
operator<<(std::ostream &outs, const Polar2D &pol)
|
||||
{
|
||||
outs << "(" << pol.r << ", " << pol.theta << ")";
|
||||
outs << "(" << pol.R() << ", " << pol.Theta() << ")";
|
||||
return outs;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -131,18 +131,25 @@ static constexpr float Epsilon_float = 0.0001;
|
|||
|
||||
|
||||
void
|
||||
DefaultEpsilon(double &epsilon)
|
||||
DefaultEpsilon(double& epsilon)
|
||||
{
|
||||
epsilon = Epsilon_double;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DefaultEpsilon(float &epsilon)
|
||||
DefaultEpsilon(float& epsilon)
|
||||
{
|
||||
epsilon = Epsilon_float;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DefaultEpsilon(int& epsilon)
|
||||
{
|
||||
epsilon = 0;
|
||||
}
|
||||
|
||||
|
||||
} // namespace scmp
|
||||
|
||||
|
|
|
|||
|
|
@ -219,15 +219,37 @@ geomRotatePointsAboutOrigin()
|
|||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
pointDistances()
|
||||
{
|
||||
const Point2D origin;
|
||||
const Point2D y2(0, 2);
|
||||
const Point2D x2(2, 0);
|
||||
const int dist2 = 2;
|
||||
const int dist10 = 10;
|
||||
const Point2D deg45{8, 6};
|
||||
|
||||
SCTEST_CHECK_EQ(y2.Distance(origin), dist2);
|
||||
SCTEST_CHECK_EQ(x2.Distance(origin), dist2);
|
||||
SCTEST_CHECK_EQ(deg45.Distance(origin), dist10);
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
|
||||
} // anonymous namespace
|
||||
|
||||
|
||||
int
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
auto noReport = false;
|
||||
auto quiet = false;
|
||||
auto flags = new scsl::Flags("test_orientation",
|
||||
"This test validates various orientation-related components in scmp.");
|
||||
flags->Register("-n", false, "don't print the report");
|
||||
flags->Register("-q", false, "suppress test output");
|
||||
|
||||
auto parsed = flags->Parse(argc, argv);
|
||||
|
|
@ -237,6 +259,7 @@ main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
SimpleSuite suite;
|
||||
flags->GetBool("-n", noReport);
|
||||
flags->GetBool("-q", quiet);
|
||||
if (quiet) {
|
||||
suite.Silence();
|
||||
|
|
@ -248,9 +271,10 @@ main(int argc, char *argv[])
|
|||
suite.AddTest("geomComparePoint2D", geomComparePoint2D);
|
||||
suite.AddTest("geomRotatePoint2D", geomRotatePoint2D);
|
||||
suite.AddTest("geomRotatePointsAboutOrigin", geomRotatePointsAboutOrigin);
|
||||
suite.AddTest("pointDistances", pointDistances);
|
||||
|
||||
delete flags;
|
||||
auto result = suite.Run();
|
||||
std::cout << suite.GetReport() << "\n";
|
||||
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
||||
return result ? 0 : 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,15 +1,18 @@
|
|||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
||||
#include <scsl/Flags.h>
|
||||
|
||||
#include <scmp/geom/Vector.h>
|
||||
#include <scmp/geom/Quaternion.h>
|
||||
#include <scmp/Math.h>
|
||||
#include <scmp/filter/Madgwick.h>
|
||||
|
||||
#include <scmp/filter/Madgwick.h>
|
||||
#include <sctest/Assert.h>
|
||||
#include <sctest/Checks.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
|
||||
|
||||
using namespace std;
|
||||
using namespace scmp;
|
||||
|
||||
|
|
@ -231,10 +234,25 @@ SimpleAngularOrientation2InitialQuaterniond()
|
|||
int
|
||||
main(int argc, char **argv)
|
||||
{
|
||||
(void)argc;
|
||||
(void)argv;
|
||||
auto quiet = false;
|
||||
auto noReport = false;
|
||||
auto flags = new scsl::Flags("test_madgwick",
|
||||
"This test validates the Madgwick filter code");
|
||||
flags->Register("-n", false, "don't print the report");
|
||||
flags->Register("-q", false, "suppress test output");
|
||||
|
||||
auto parsed = flags->Parse(argc, argv);
|
||||
if (parsed != scsl::Flags::ParseStatus::OK) {
|
||||
std::cerr << "Failed to parse flags: "
|
||||
<< scsl::Flags::ParseStatusToString(parsed) << "\n";
|
||||
}
|
||||
|
||||
sctest::SimpleSuite suite;
|
||||
flags->GetBool("-n", noReport);
|
||||
flags->GetBool("-q", quiet);
|
||||
if (quiet) {
|
||||
suite.Silence();
|
||||
}
|
||||
|
||||
suite.AddTest("SimpleAngularOrientationFloat",
|
||||
SimpleAngularOrientationFloat);
|
||||
|
|
@ -253,8 +271,8 @@ main(int argc, char **argv)
|
|||
suite.AddTest("SimpleAngularOrientationDouble (inital quaterniond)",
|
||||
SimpleAngularOrientation2InitialQuaterniond);
|
||||
|
||||
delete flags;
|
||||
auto result = suite.Run();
|
||||
|
||||
std::cout << suite.GetReport() << "\n";
|
||||
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
||||
return result ? 0 : 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -122,9 +122,11 @@ RotateRadians()
|
|||
int
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
auto noReport = false;
|
||||
auto quiet = false;
|
||||
auto flags = new scsl::Flags("test_orientation",
|
||||
"This test validates various orientation-related components in scmp.");
|
||||
flags->Register("-n", false, "don't print the report");
|
||||
flags->Register("-q", false, "suppress test output");
|
||||
|
||||
auto parsed = flags->Parse(argc, argv);
|
||||
|
|
@ -134,6 +136,7 @@ main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
sctest::SimpleSuite suite;
|
||||
flags->GetBool("-n", noReport);
|
||||
flags->GetBool("-q", quiet);
|
||||
if (quiet) {
|
||||
suite.Silence();
|
||||
|
|
@ -147,6 +150,6 @@ main(int argc, char *argv[])
|
|||
|
||||
delete flags;
|
||||
auto result = suite.Run();
|
||||
std::cout << suite.GetReport() << "\n";
|
||||
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
||||
return result ? 0 : 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -88,9 +88,11 @@ Orientation3d_Heading()
|
|||
int
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
auto noReport = false;
|
||||
auto quiet = false;
|
||||
auto flags = new scsl::Flags("test_orientation",
|
||||
"This test validates various orientation-related components in scmp.");
|
||||
flags->Register("-n", false, "don't print the report");
|
||||
flags->Register("-q", false, "suppress test output");
|
||||
|
||||
auto parsed = flags->Parse(argc, argv);
|
||||
|
|
@ -100,6 +102,7 @@ main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
SimpleSuite suite;
|
||||
flags->GetBool("-n", noReport);
|
||||
flags->GetBool("-q", quiet);
|
||||
if (quiet) {
|
||||
suite.Silence();
|
||||
|
|
@ -114,6 +117,6 @@ main(int argc, char *argv[])
|
|||
|
||||
delete flags;
|
||||
auto result = suite.Run();
|
||||
std::cout << suite.GetReport() << "\n";
|
||||
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
||||
return result ? 0 : 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,8 +1,8 @@
|
|||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
||||
#include <scsl/Flags.h>
|
||||
#include <scmp/geom/Quaternion.h>
|
||||
|
||||
#include <sctest/Checks.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
|
||||
|
|
@ -427,9 +427,27 @@ QuaternionMiscellanous_InitializerConstructor()
|
|||
|
||||
|
||||
int
|
||||
main(void)
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
auto noReport = false;
|
||||
auto quiet = false;
|
||||
auto flags = new scsl::Flags("test_quaternion",
|
||||
"This test validates the Quaternion class.");
|
||||
flags->Register("-n", false, "don't print the report");
|
||||
flags->Register("-q", false, "suppress test output");
|
||||
|
||||
auto parsed = flags->Parse(argc, argv);
|
||||
if (parsed != scsl::Flags::ParseStatus::OK) {
|
||||
std::cerr << "Failed to parse flags: "
|
||||
<< scsl::Flags::ParseStatusToString(parsed) << "\n";
|
||||
}
|
||||
|
||||
SimpleSuite suite;
|
||||
flags->GetBool("-n", noReport);
|
||||
flags->GetBool("-q", quiet);
|
||||
if (quiet) {
|
||||
suite.Silence();
|
||||
}
|
||||
|
||||
suite.AddTest("Quaternion_SelfTest", Quaternion_SelfTest);
|
||||
suite.AddTest("QuaternionMiscellanous_InitializerConstructor",
|
||||
|
|
@ -465,7 +483,8 @@ main(void)
|
|||
suite.AddTest("Quaternionf_Unit", Quaternionf_Unit);
|
||||
suite.AddTest("Quaternionf_UtilityCreator", Quaternionf_UtilityCreator);
|
||||
|
||||
delete flags;
|
||||
auto result = suite.Run();
|
||||
std::cout << suite.GetReport() << "\n";
|
||||
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
||||
return result ? 0 : 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@
|
|||
|
||||
#include <iostream>
|
||||
|
||||
#include <scsl/Flags.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
|
||||
|
||||
|
|
@ -52,16 +53,36 @@ static bool nope() { return 2 + 2 == 5; }
|
|||
|
||||
|
||||
int
|
||||
main()
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
auto noReport = false;
|
||||
auto quiet = false;
|
||||
auto flags = new scsl::Flags("test_orientation",
|
||||
"This test validates various orientation-related components in scmp.");
|
||||
flags->Register("-n", false, "don't print the report");
|
||||
flags->Register("-q", false, "suppress test output");
|
||||
|
||||
auto parsed = flags->Parse(argc, argv);
|
||||
if (parsed != scsl::Flags::ParseStatus::OK) {
|
||||
std::cerr << "Failed to parse flags: "
|
||||
<< scsl::Flags::ParseStatusToString(parsed) << "\n";
|
||||
}
|
||||
|
||||
sctest::SimpleSuite suite;
|
||||
flags->GetBool("-n", noReport);
|
||||
flags->GetBool("-q", quiet);
|
||||
if (quiet) {
|
||||
suite.Silence();
|
||||
}
|
||||
|
||||
suite.Setup(prepareTests);
|
||||
suite.Teardown(destroyTests);
|
||||
suite.AddTest("1 + 1", addOne);
|
||||
suite.AddTest("fourness", four);
|
||||
suite.AddFailingTest("self-evident truth", nope);
|
||||
auto result = suite.Run();
|
||||
|
||||
std::cout << suite.GetReport() << "\n";
|
||||
delete flags;
|
||||
auto result = suite.Run();
|
||||
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
||||
return result ? 0 : 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <functional>
|
||||
#include <iostream>
|
||||
|
||||
#include <scsl/Flags.h>
|
||||
#include <scsl/StringUtil.h>
|
||||
#include <sctest/Assert.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
|
|
@ -135,9 +136,27 @@ TestWrapping()
|
|||
|
||||
|
||||
int
|
||||
main()
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
auto noReport = false;
|
||||
auto quiet = false;
|
||||
auto flags = new scsl::Flags("test_orientation",
|
||||
"This test validates various orientation-related components in scmp.");
|
||||
flags->Register("-n", false, "don't print the report");
|
||||
flags->Register("-q", false, "suppress test output");
|
||||
|
||||
auto parsed = flags->Parse(argc, argv);
|
||||
if (parsed != scsl::Flags::ParseStatus::OK) {
|
||||
std::cerr << "Failed to parse flags: "
|
||||
<< scsl::Flags::ParseStatusToString(parsed) << "\n";
|
||||
}
|
||||
|
||||
sctest::SimpleSuite suite;
|
||||
flags->GetBool("-n", noReport);
|
||||
flags->GetBool("-q", quiet);
|
||||
if (quiet) {
|
||||
suite.Silence();
|
||||
}
|
||||
|
||||
TestTrimming(" foo\t ", "foo\t ", " foo", "foo");
|
||||
TestTrimming(" foo\tbar ", "foo\tbar ", " foo\tbar", "foo\tbar");
|
||||
|
|
@ -155,7 +174,9 @@ main()
|
|||
std::vector<std::string>{"abc", "", "def", "ghi"}));
|
||||
suite.AddTest("TestSplitKV(char)", TestSplitChar);
|
||||
suite.AddTest("TextWrapping", TestWrapping);
|
||||
|
||||
delete flags;
|
||||
auto result = suite.Run();
|
||||
std::cout << suite.GetReport() << "\n";
|
||||
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
||||
return result ? 0 : 1;
|
||||
}
|
||||
|
|
@ -26,6 +26,7 @@
|
|||
#include <scmp/geom/Vector.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
#include <sctest/Checks.h>
|
||||
#include "scsl/Flags.h"
|
||||
|
||||
|
||||
using namespace scmp;
|
||||
|
|
@ -428,9 +429,28 @@ Vector3DoubleTests_CrossProduct()
|
|||
|
||||
|
||||
int
|
||||
main()
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
auto noReport = false;
|
||||
auto quiet = false;
|
||||
auto flags = new scsl::Flags("test_vector",
|
||||
"This test validates the vector implementation.");
|
||||
flags->Register("-n", false, "don't print the report");
|
||||
flags->Register("-q", false, "suppress test output");
|
||||
|
||||
auto parsed = flags->Parse(argc, argv);
|
||||
if (parsed != scsl::Flags::ParseStatus::OK) {
|
||||
std::cerr << "Failed to parse flags: "
|
||||
<< scsl::Flags::ParseStatusToString(parsed) << "\n";
|
||||
}
|
||||
|
||||
SimpleSuite suite;
|
||||
flags->GetBool("-n", noReport);
|
||||
flags->GetBool("-q", quiet);
|
||||
if (quiet) {
|
||||
suite.Silence();
|
||||
}
|
||||
|
||||
suite.AddTest("Vector3Miscellaneous_ExtractionOperator3d",
|
||||
Vector3Miscellaneous_ExtractionOperator3d);
|
||||
suite.AddTest("Vector3Miscellaneous_ExtractionOperator3f",
|
||||
|
|
@ -485,8 +505,9 @@ main()
|
|||
Vector3DoubleTests_Projections);
|
||||
suite.AddTest("Vector3DoubleTests_CrossProduct",
|
||||
Vector3DoubleTests_CrossProduct);
|
||||
auto result = suite.Run();
|
||||
|
||||
std::cout << suite.GetReport() << "\n";
|
||||
delete flags;
|
||||
auto result = suite.Run();
|
||||
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
||||
return result ? 0 : 1;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue