sc/scsl
sc
/
scsl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Document and refactor geom code. 

- Doxygenate headers.
- Rename to bring methods and functions in line with everything else.
This commit is contained in:
Kyle Isom 2023-10-20 20:45:39 -07:00
parent 4b1007123a
commit 6a421d6adf
27 changed files with 802 additions and 680 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -29,7 +29,7 @@ add_compile_options(
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
add_compile_options("-stdlib=libc++") add_compile_options("-stdlib=libc++")
else () else ()
# nothing special for gcc at the moment # nothing special for gcc At the moment
endif () endif ()
add_compile_definitions(SCSL_DESKTOP_BUILD) add_compile_definitions(SCSL_DESKTOP_BUILD)

16
include/scmp/Math.h
View File

@ -49,26 +49,26 @@ int BestDie(int k, int m, int n);
/// \brief Convert radians to degrees. /// \brief Convert radians to degrees.
/// ///
/// \param rads the angle in radians /// \param rads the Angle in radians
/// \return the angle in degrees. /// \return the Angle in degrees.
float RadiansToDegreesF(float rads); float RadiansToDegreesF(float rads);
/// \brief Convert radians to degrees. /// \brief Convert radians to degrees.
/// ///
/// \param rads the angle in radians /// \param rads the Angle in radians
/// \return the angle in degrees. /// \return the Angle in degrees.
double RadiansToDegreesD(double rads); double RadiansToDegreesD(double rads);
/// \brief Convert degrees to radians. /// \brief Convert degrees to radians.
/// ///
/// \param degrees the angle in degrees /// \param degrees the Angle in degrees
/// \return the angle in radians. /// \return the Angle in radians.
float DegreesToRadiansF(float degrees); float DegreesToRadiansF(float degrees);
/// \brief Convert degrees to radians. /// \brief Convert degrees to radians.
/// ///
/// \param degrees the angle in degrees /// \param degrees the Angle in degrees
/// \return the angle in radians. /// \return the Angle in radians.
double DegreesToRadiansD(double degrees); double DegreesToRadiansD(double degrees);
/// \brief RotateRadians rotates theta0 by theta1 radians, wrapping /// \brief RotateRadians rotates theta0 by theta1 radians, wrapping

2
include/scmp/Motion2D.h
View File

@ -12,7 +12,7 @@ namespace scmp {
namespace basic { namespace basic {
scmp::geom::Vector2d Acceleration(double speed, double heading); scmp::geom::Vector2D Acceleration(double speed, double heading);
} // namespace basic } // namespace basic

20
include/scmp/filter/Madgwick.h
View File

@ -40,8 +40,8 @@ namespace filter {
/// @brief Madgwick implements an efficient Orientation filter for IMUs. /// @brief Madgwick implements an efficient Orientation filter for IMUs.
/// ///
/// Madgwick is a novel Orientation filter applicable to IMUs /// Madgwick is a novel Orientation filter applicable to IMUs
/// consisting of tri-axis gyroscopes and accelerometers, and MARG /// consisting of tri-Axis gyroscopes and accelerometers, and MARG
/// sensor arrays that also include tri-axis magnetometers. The MARG /// sensor arrays that also include tri-Axis magnetometers. The MARG
/// implementation incorporates magnetic distortionand gyroscope bias /// implementation incorporates magnetic distortionand gyroscope bias
/// drift compensation. /// drift compensation.
/// ///
@ -51,24 +51,24 @@ namespace filter {
template <typename T> template <typename T>
class Madgwick { class Madgwick {
public: public:
/// \brief The Madgwick filter is initialised with an identity quaternion. /// \brief The Madgwick filter is initialised with an identity MakeQuaternion.
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame()
{}; {};
/// \brief The Madgwick filter is initialised with a sensor frame. /// \brief The Madgwick filter is initialised with a sensor frame.
/// ///
/// \param sf A sensor frame; if zero, the sensor frame will be /// \param sf A sensor frame; if zero, the sensor frame will be
/// initialised as an identity quaternion. /// initialised as an identity MakeQuaternion.
Madgwick(scmp::geom::Vector<T, 3> sf) : deltaT(0.0), previousSensorFrame() Madgwick(scmp::geom::Vector<T, 3> sf) : deltaT(0.0), previousSensorFrame()
{ {
if (!sf.isZero()) { if (!sf.IsZero()) {
sensorFrame = scmp::geom::quaternion<T>(sf, 0.0); sensorFrame = scmp::geom::MakeQuaternion<T>(sf, 0.0);
} }
} }
/// \brief Initialise the filter with a sensor frame quaternion. /// \brief Initialise the filter with a sensor frame MakeQuaternion.
/// ///
/// \param sf A quaternion representing the current Orientation. /// \param sf A MakeQuaternion representing the current Orientation.
Madgwick(scmp::geom::Quaternion<T> sf) : Madgwick(scmp::geom::Quaternion<T> sf) :
deltaT(0.0), previousSensorFrame(), sensorFrame(sf) deltaT(0.0), previousSensorFrame(), sensorFrame(sf)
{}; {};
@ -91,7 +91,7 @@ public:
/// ///
/// \param gyro A three-dimensional vector containing gyro readings /// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z. /// as w_x, w_y, w_z.
/// \return A quaternion representing the rate of angular change. /// \return A MakeQuaternion representing the rate of angular change.
scmp::geom::Quaternion<T> scmp::geom::Quaternion<T>
AngularRate(const scmp::geom::Vector<T, 3> &gyro) const AngularRate(const scmp::geom::Vector<T, 3> &gyro) const
{ {
@ -166,7 +166,7 @@ public:
scmp::geom::Vector<T, 3> scmp::geom::Vector<T, 3>
Euler() Euler()
{ {
return this->sensorFrame.euler(); return this->sensorFrame.Euler();
} }
/// \brief Set the default Δt. /// \brief Set the default Δt.

2
include/scmp/geom.h
View File

@ -2,7 +2,7 @@
/// \file include/scmp/geom.h /// \file include/scmp/geom.h
/// \author K. Isom <kyle@imap.cc> /// \author K. Isom <kyle@imap.cc>
/// \date 2023-10-20 /// \date 2023-10-20
/// \brief 2D point and polar coordinate systems. /// \brief Vector-based geometry code.
/// ///
/// Copyright 2023 K. Isom <kyle@imap.cc> /// Copyright 2023 K. Isom <kyle@imap.cc>
/// ///

12
include/scmp/geom/Coord2D.h
View File

@ -46,7 +46,7 @@ public:
/// \brief A Point2D defaults to (0,0). /// \brief A Point2D defaults to (0,0).
Point2D(); Point2D();
/// \brief Initialize a Point2D at (_x, _y). /// \brief Initialize a Point2D At (_x, _y).
Point2D(int _x, int _y); Point2D(int _x, int _y);
/// \brief Initialize a Point2D from a Polar2D coordinate. /// \brief Initialize a Point2D from a Polar2D coordinate.
@ -74,13 +74,13 @@ public:
/// \brief Rotate rotates the point by theta radians. /// \brief Rotate rotates the point by theta radians.
/// ///
/// \param rotated Stores the rotated point. /// \param rotated Stores the rotated point.
/// \param theta The angle (in radians) to rotate the point. /// \param theta The Angle (in radians) to Rotate the point.
void Rotate(Point2D& rotated, double theta); void Rotate(Point2D& rotated, double theta);
/// \brief Rotate this point around a series of vertices. /// \brief Rotate this point around a series of vertices.
/// ///
/// \param vertices A series of vertices to rotate this point around. /// \param vertices A series of vertices to Rotate this point around.
/// \param theta The angle to rotate by. /// \param theta The Angle to Rotate by.
/// \return A series of rotated points. /// \return A series of rotated points.
std::vector<Point2D> Rotate(std::vector<Polar2D> vertices, double theta); std::vector<Point2D> Rotate(std::vector<Polar2D> vertices, double theta);
@ -98,12 +98,12 @@ public:
}; };
// A Polar2D is a 2D polar coordinate, specified in terms of the radius from // 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 // some origin and the Angle from the positive X Axis of a cartesian coordinate
// system. // system.
class Polar2D : public Vector<double, 2> { class Polar2D : public Vector<double, 2> {
public: public:
// A Polar2D can be initialised as a zeroised polar coordinate, by specifying // A Polar2D can be initialised as a zeroised polar coordinate, by specifying
// the radius and angle directly, or via conversion from a Point2D. // the radius and Angle directly, or via conversion from a Point2D.
Polar2D(); Polar2D();
Polar2D(double _r, double _theta); Polar2D(double _r, double _theta);
Polar2D(const Point2D &); Polar2D(const Point2D &);

116
include/scmp/geom/Orientation.h
View File

@ -1,11 +1,31 @@
/** ///
* orientation.h concerns itself with computing the Orientation of some /// \file include/scmp/geom/Orientation.h
* vector with respect to a reference plane that is assumed to be the /// \author K. Isom <kyle@imap.cc>
* of the Earth. /// \date 2017-06-05
*/ /// \brief Orientation of vectors w.r.t. a reference plane, assumed to
/// be the Earth.
///
/// Copyright 2023 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
#include <cstdint> #include <cstdint>
#include <scmp/geom/Vector.h>
#ifndef SCMATH_GEOM_ORIENTATION_H #ifndef SCMATH_GEOM_ORIENTATION_H
#define SCMATH_GEOM_ORIENTATION_H #define SCMATH_GEOM_ORIENTATION_H
@ -19,71 +39,75 @@ namespace geom {
/// and three-dimensional vectors. /// and three-dimensional vectors.
/// \ingroup basis /// \ingroup basis
/// Convenience constant for the x index. /// \brief Convenience constant for the x index.
constexpr uint8_t Basis_x = 0; constexpr uint8_t BasisX = 0;
/// \ingroup basis /// \ingroup basis
/// Convenience constant for the y index. /// \brief Convenience constant for the y index.
constexpr uint8_t Basis_y = 1; constexpr uint8_t BasisY = 1;
/// \ingroup basis /// \ingroup basis
/// Convenience constant for the z index. /// \brief Convenience constant for the z index.
constexpr uint8_t Basis_z = 2; constexpr uint8_t BasisZ = 2;
/// @brief Basis2d provides basis vectors for Vector2ds. /// \brief Basis2D provides basis vectors for Vector2ds.
static const Vector2d Basis2d[] = { static const Vector2D Basis2D[] = {
Vector2d{1, 0}, Vector2D{1, 0},
Vector2d{0, 1}, Vector2D{0, 1},
}; };
/// @brief Basis2d provides basis vectors for Vector2fs. /// \brief Basis2D provides basis vectors for Vector2fs.
static const Vector2f Basis2f[] = { static const Vector2F Basis2F[] = {
Vector2f{1, 0}, Vector2F{1, 0},
Vector2f{0, 1}, Vector2F{0, 1},
}; };
/// @brief Basis2d provides basis vectors for Vector3ds. /// \brief Basis2D provides basis vectors for Vector3ds.
static const Vector3d Basis3d[] = { static const Vector3D Basis3D[] = {
Vector3d{1, 0, 0}, Vector3D{1, 0, 0},
Vector3d{0, 1, 0}, Vector3D{0, 1, 0},
Vector3d{0, 0, 1}, Vector3D{0, 0, 1},
}; };
/// @brief Basis2d provides basis vectors for Vector3fs. /// \brief Basis2D provides basis vectors for Vector3fs.
static const Vector3f Basis3f[] = { static const Vector3F Basis3F[] = {
Vector3f{1, 0, 0}, Vector3F{1, 0, 0},
Vector3f{0, 1, 0}, Vector3F{0, 1, 0},
Vector3f{0, 0, 1}, Vector3F{0, 0, 1},
}; };
/// Heading2f returns a compass heading for a Vector2f. /// \brief Compass heading for a Vector2F.
/// @param vec A vector Orientation. ///
/// @return The compass heading of the vector in radians. /// \param vec A vector Orientation.
float Heading2f(Vector2f vec); /// \return The compass heading of the vector in radians.
float Heading2F(Vector2F vec);
/// Heading2d returns a compass heading for a Vector2d. /// \brief Compass heading for a Vector2D.
/// @param vec A vector Orientation. ///
/// @return The compass heading of the vector in radians. /// \param vec A vector Orientation.
double Heading2d(Vector2d vec); /// \return The compass heading of the vector in radians.
double Heading2d(Vector2D vec);
/// Heading3f returns a compass heading for a Vector2f. /// \brief Compass heading for a Vector2F.
/// @param vec A vector Orientation. ///
/// @return The compass heading of the vector in radians. /// \param vec A vector Orientation.
float Heading3f(Vector3f vec); /// \return The compass heading of the vector in radians.
float Heading3f(Vector3F vec);
/// Heading3d returns a compass heading for a Vector2f. /// Heading3d returns a compass heading for a Vector2F.
/// @param vec A vector Orientation. ///
/// @return The compass heading of the vector in radians. /// \param vec A vector Orientation.
double Heading3d(Vector3d vec); /// \return The compass heading of the vector in radians.
double Heading3d(Vector3D vec);
} // namespace geom } // namespace geom
} // namespace scmp } // namespace scmp
#endif // __WRMATH_ORIENTATION_H #endif // SCMATH_GEOM_ORIENTATION_H

416
include/scmp/geom/Quaternion.h
View File

@ -1,7 +1,27 @@
/// quaternion.h contains an implementation of quaternions suitable ///
/// for navigation in R3. /// \file include/scmp/geom/Quaternion.h
#ifndef SCMATH_QUATERNION_H /// \author K. Isom <kyle@imap.cc>
#define SCMATH_QUATERNION_H /// \date 2019-08-05
/// \brief Quaternion implementation suitable for navigation in R3.
///
/// Copyright 2019 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
#ifndef SCMATH_GEOM_QUATERNION_H
#define SCMATH_GEOM_QUATERNION_H
#include <cassert> #include <cassert>
@ -13,77 +33,79 @@
#include <scmp/Math.h> #include <scmp/Math.h>
#include <scmp/geom/Vector.h> #include <scmp/geom/Vector.h>
/// math contains the shimmering clarity math library.
namespace scmp { namespace scmp {
/// geom contains geometric classes and functions.
namespace geom { namespace geom {
/// @brief Quaternions provide a representation of Orientation and rotations /// \brief Quaternions provide a representation of Orientation
/// in three dimensions. /// and rotations in three dimensions.
/// ///
/// Quaternions encode rotations in three-dimensional space. While technically /// Quaternions encode rotations in three-dimensional space. While
/// a quaternion is comprised of a real element and a complex vector<3>, for /// technically a MakeQuaternion is comprised of a real element and a
/// the purposes of this library, it is modeled as a floating point 4D vector /// complex vector<3>, for the purposes of this library, it is modeled
/// of the form <w, x, y, z>, where x, y, and z represent an axis of rotation in /// as a floating point 4D vector of the form <w, x, y, z>, where x, y,
/// R3 and w the angle, in radians, of the rotation about that axis. Where Euler /// and z represent an Axis of rotation in R3 and w the Angle, in
/// angles are concerned, the ZYX (or yaw, pitch, roll) sequence is used. /// radians, of the rotation about that Axis. Where Euler angles are
/// concerned, the ZYX (or yaw, pitch, roll) sequence is used.
/// ///
/// For information on the underlying vector type, see the documentation for /// For information on the underlying vector type, see the
/// wr::geom::Vector. /// documentation for scmp::geom::Vector.
/// ///
/// The constructors are primarily intended for intended operations; in practice, /// Like vectors, quaternions carry an internal tolerance value ε that
/// the quaternionf() and quaterniond() functions are more useful for constructing /// is used for floating point comparisons. The math namespace contains
/// quaternions from vectors and angles. /// the default values used for this; generally, a tolerance of 0.0001
/// /// is considered appropriate for the uses of this library. The
/// Like vectors, quaternions carry an internal tolerance value ε that is used for /// tolerance can be explicitly set with the SetEpsilon method.
/// floating point comparisons. The math namespace contains the default values
/// used for this; generally, a tolerance of 0.0001 is considered appropriate for
/// the uses of this library. The tolerance can be explicitly set with the
/// setEpsilon method.
template<typename T> template<typename T>
class Quaternion { class Quaternion {
public: public:
/// The default Quaternion constructor returns an identity quaternion. /// \brief Construct an identity MakeQuaternion.
Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0) Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0)
{ {
scmp::DefaultEpsilon(this->eps); scmp::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps); v.SetEpsilon(this->eps);
}; };
/// A Quaternion may be initialised with a Vector<T, 3> axis of rotation /// \brief Construct a MakeQuaternion with an Axis and Angle of
/// and an angle of rotation. This doesn't do the angle transforms to simplify /// rotation.
/// internal operations.
/// ///
/// @param _axis A three-dimensional vector of the same type as the Quaternion. /// A Quaternion may be initialised with a Vector<T, 3> Axis
/// @param _angle The angle of rotation about the axis of rotation. /// of rotation and an Angle of rotation. This doesn't do the
/// Angle transforms to simplify internal operations.
///
/// \param _axis A three-dimensional vector of the same type as
/// the Quaternion.
/// \param _angle The Angle of rotation about the Axis of
/// rotation.
Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle) Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle)
{ {
this->constrainAngle(); this->constrainAngle();
scmp::DefaultEpsilon(this->eps); scmp::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps); v.SetEpsilon(this->eps);
}; };
/// A Quaternion may be initialised with a Vector<T, 4> comprised of /// A Quaternion may be initialised with a Vector<T, 4> comprised of
/// the axis of rotation followed by the angle of rotation. /// the Axis of rotation followed by the Angle of rotation.
/// ///
/// @param vector A vector in the form <w, x, y, z>. /// \param vector A vector in the form <w, x, y, z>.
Quaternion(Vector<T, 4> vector) : Quaternion(Vector<T, 4> vector) :
v(Vector<T, 3>{vector[1], vector[2], vector[3]}), v(Vector<T, 3>{vector[1], vector[2], vector[3]}),
w(vector[0]) w(vector[0])
{ {
this->constrainAngle(); this->constrainAngle();
scmp::DefaultEpsilon(this->eps); scmp::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps); v.SetEpsilon(this->eps);
} }
/// A Quaternion may be constructed with an initializer list of /// \brief An initializer list containing values for w, x, y,
/// type T, which must have exactly N elements. /// and z.
/// ///
/// @param ilst An initial set of values in the form <w, x, y, z>. /// \param ilst An initial set of values in the form
/// <w, x, y, z>.
Quaternion(std::initializer_list<T> ilst) Quaternion(std::initializer_list<T> ilst)
{ {
auto it = ilst.begin(); auto it = ilst.begin();
@ -93,47 +115,47 @@ public:
this->constrainAngle(); this->constrainAngle();
scmp::DefaultEpsilon(this->eps); scmp::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps); v.SetEpsilon(this->eps);
} }
/// Set the comparison tolerance for this quaternion. /// \brief Set the comparison tolerance for this MakeQuaternion.
/// ///
/// @param epsilon A tolerance value. /// \param epsilon A tolerance value.
void void
setEpsilon(T epsilon) SetEpsilon(T epsilon)
{ {
this->eps = epsilon; this->eps = epsilon;
this->v.setEpsilon(epsilon); this->v.SetEpsilon(epsilon);
} }
/// Return the axis of rotation of this quaternion. /// \brief Return the Axis of rotation of this MakeQuaternion.
/// ///
/// @return The axis of rotation of this quaternion. /// \return The Axis of rotation of this MakeQuaternion.
Vector<T, 3> Vector<T, 3>
axis() const Axis() const
{ {
return this->v; return this->v;
} }
/// Return the angle of rotation of this quaternion. /// \brief Return the Angle of rotation of this MakeQuaternion.
/// ///
/// @return the angle of rotation of this quaternion. /// \return the Angle of rotation of this MakeQuaternion.
T T
angle() const Angle() const
{ {
return this->w; return this->w;
} }
/// Compute the dot product of two quaternions. /// \brief Compute the Dot product of two quaternions.
/// ///
/// \param other Another quaternion. /// \param other Another MakeQuaternion.
/// \return The dot product between the two quaternions. /// \return The Dot product between the two quaternions.
T T
dot(const Quaternion<T> &other) const Dot(const Quaternion<T> &other) const
{ {
double innerProduct = this->v[0] * other.v[0]; double innerProduct = this->v[0] * other.v[0];
@ -144,12 +166,14 @@ public:
} }
/// Compute the norm of a quaternion. Treating the Quaternion as a /// \brief Compute the Norm of a MakeQuaternion.
/// Vector<T, 4>, it's the same as computing the magnitude.
/// ///
/// @return A non-negative real number. /// Treating the Quaternion as a Vector<T, 4>, this is the same
/// process as computing the Magnitude.
///
/// \return A non-negative real number.
T T
norm() const Norm() const
{ {
T n = 0; T n = 0;
@ -162,86 +186,92 @@ public:
} }
/// Return the unit quaternion. /// \brief Return the unit MakeQuaternion.
/// ///
/// \return The unit quaternion. /// \return The unit MakeQuaternion.
Quaternion Quaternion
unitQuaternion() UnitQuaternion()
{ {
return *this / this->norm(); return *this / this->Norm();
} }
/// Compute the conjugate of a quaternion. /// \brief Compute the Conjugate of a MakeQuaternion.
/// ///
/// @return The conjugate of this quaternion. /// \return The Conjugate of this MakeQuaternion.
Quaternion Quaternion
conjugate() const Conjugate() const
{ {
return Quaternion(Vector<T, 4>{this->w, -this->v[0], -this->v[1], -this->v[2]}); return Quaternion(Vector<T, 4>{this->w, -this->v[0], -this->v[1], -this->v[2]});
} }
/// Compute the inverse of a quaternion. /// \brief Compute the Inverse of a MakeQuaternion.
/// ///
/// @return The inverse of this quaternion. /// \return The Inverse of this MakeQuaternion.
Quaternion Quaternion
inverse() const Inverse() const
{ {
T _norm = this->norm(); T _norm = this->Norm();
return this->conjugate() / (_norm * _norm); return this->Conjugate() / (_norm * _norm);
} }
/// Determine whether this is an identity quaternion. /// \brief Determine whether this is an identity MakeQuaternion.
/// ///
/// \return true if this is an identity quaternion. /// \return true if this is an identity MakeQuaternion.
bool bool
isIdentity() const { IsIdentity() const {
return this->v.isZero() && return this->v.IsZero() &&
scmp::WithinTolerance(this->w, (T)1.0, this->eps); scmp::WithinTolerance(this->w, (T)1.0, this->eps);
} }
/// Determine whether this is a unit quaternion. /// \brief Determine whether this is a unit MakeQuaternion.
/// ///
/// @return true if this is a unit quaternion. /// \return true if this is a unit MakeQuaternion.
bool bool
isUnitQuaternion() const IsUnitQuaternion() const
{ {
return scmp::WithinTolerance(this->norm(), (T) 1.0, this->eps); return scmp::WithinTolerance(this->Norm(), (T) 1.0, this->eps);
} }
/// Return the quaternion as a Vector<T, 4>, with the axis of rotation /// \brief Convert to Vector form.
/// followed by the angle of rotation.
/// ///
/// @return A vector representation of the quaternion. /// Return the MakeQuaternion as a Vector<T, 4>, with the Axis of
/// rotation followed by the Angle of rotation.
///
/// \return A vector representation of the MakeQuaternion.
Vector<T, 4> Vector<T, 4>
asVector() const AsVector() const
{ {
return Vector<T, 4>{this->w, this->v[0], this->v[1], this->v[2]}; return Vector<T, 4>{this->w, this->v[0], this->v[1], this->v[2]};
} }
/// Rotate vector vr about this quaternion. /// \brief Rotate Vector vr about this MakeQuaternion.
/// ///
/// @param vr The vector to be rotated. /// \param vr The vector to be rotated.
/// @return The rotated vector. /// \return The rotated vector.
Vector<T, 3> Vector<T, 3>
rotate(Vector<T, 3> vr) const Rotate(Vector<T, 3> vr) const
{ {
return (this->conjugate() * vr * (*this)).axis(); return (this->Conjugate() * vr * (*this)).Axis();
} }
/// Return the Euler angles for this quaternion as a vector of /// \brief Return Euler angles for this MakeQuaternion.
/// <yaw, pitch, roll>. Users of this function should watch out
/// for gimbal lock.
/// ///
/// @return A vector<T, 3> containing <yaw, pitch, roll> /// Return the Euler angles for this MakeQuaternion as a vector of
/// <yaw, pitch, roll>.
///
/// \warn Users of this function should watch out for gimbal
/// lock.
///
/// \return A vector<T, 3> containing <yaw, pitch, roll>
Vector<T, 3> Vector<T, 3>
euler() const Euler() const
{ {
T yaw, pitch, roll; T yaw, pitch, roll;
T a = this->w, a2 = a * a; T a = this->w, a2 = a * a;
@ -257,10 +287,10 @@ public:
} }
/// Perform quaternion addition with another quaternion. /// \brief Quaternion addition.
/// ///
/// @param other The quaternion to be added with this one. /// \param other The MakeQuaternion to be added with this one.
/// @return The result of adding the two quaternions together. /// \return The result of adding the two quaternions together.
Quaternion Quaternion
operator+(const Quaternion<T> &other) const operator+(const Quaternion<T> &other) const
{ {
@ -268,10 +298,10 @@ public:
} }
/// Perform quaternion subtraction with another quaternion. /// \brief Quaternion subtraction.
/// ///
/// @param other The quaternion to be subtracted from this one. /// \param other The MakeQuaternion to be subtracted from this one.
/// @return The result of subtracting the other quaternion from this one. /// \return The result of subtracting the other MakeQuaternion from this one.
Quaternion Quaternion
operator-(const Quaternion<T> &other) const operator-(const Quaternion<T> &other) const
{ {
@ -279,10 +309,10 @@ public:
} }
/// Perform scalar multiplication. /// \brief Scalar multiplication.
/// ///
/// @param k The scaling value. /// \param k The scaling value.
/// @return A scaled quaternion. /// \return A scaled MakeQuaternion.
Quaternion Quaternion
operator*(const T k) const operator*(const T k) const
{ {
@ -290,10 +320,10 @@ public:
} }
/// Perform scalar division. /// \brief Scalar division.
/// ///
/// @param k The scalar divisor. /// \param k The scalar divisor.
/// @return A scaled quaternion. /// \return A scaled MakeQuaternion.
Quaternion Quaternion
operator/(const T k) const operator/(const T k) const
{ {
@ -301,23 +331,25 @@ public:
} }
/// Perform quaternion Hamilton multiplication with a three- /// \brief Quaternion Hamilton multiplication with a three-
/// dimensional vector; this is done by treating the vector /// dimensional vector.
/// as a pure quaternion (e.g. with an angle of rotation of 0).
/// ///
/// @param vector The vector to multiply with this quaternion. /// This is done by treating the vector as a pure MakeQuaternion
/// @return The Hamilton product of the quaternion and vector. /// (e.g. with an Angle of rotation of 0).
///
/// \param vector The vector to multiply with this MakeQuaternion.
/// \return The Hamilton product of the MakeQuaternion and vector.
Quaternion Quaternion
operator*(const Vector<T, 3> &vector) const operator*(const Vector<T, 3> &vector) const
{ {
return Quaternion(vector * this->w + this->v.cross(vector), return Quaternion(vector * this->w + this->v.Cross(vector),
(T) 0.0); (T) 0.0);
} }
/// Perform quaternion Hamilton multiplication. /// \brief Quaternion Hamilton multiplication.
/// ///
/// @param other The other quaternion to multiply with this one. /// \param other The other MakeQuaternion to multiply with this one.
/// @result The Hamilton product of the two quaternions. /// @result The Hamilton product of the two quaternions.
Quaternion Quaternion
operator*(const Quaternion<T> &other) const operator*(const Quaternion<T> &other) const
@ -326,14 +358,15 @@ public:
(this->v * other.v); (this->v * other.v);
Vector<T, 3> axis = (other.v * this->w) + Vector<T, 3> axis = (other.v * this->w) +
(this->v * other.w) + (this->v * other.w) +
(this->v.cross(other.v)); (this->v.Cross(other.v));
return Quaternion(axis, angle); return Quaternion(axis, angle);
} }
/// Perform quaternion equality checking. /// \brief Quaternion equivalence.
/// @param other The quaternion to check equality against. ///
/// @return True if the two quaternions are equal within their tolerance. /// \param other The MakeQuaternion to check equality against.
/// \return True if the two quaternions are equal within their tolerance.
bool bool
operator==(const Quaternion<T> &other) const operator==(const Quaternion<T> &other) const
{ {
@ -342,10 +375,10 @@ public:
} }
/// Perform quaternion inequality checking. /// \brief Quaternion non-equivalence.
/// ///
/// @param other The quaternion to check inequality against. /// \param other The MakeQuaternion to check inequality against.
/// @return True if the two quaternions are unequal within their tolerance. /// \return True if the two quaternions are unequal within their tolerance.
bool bool
operator!=(const Quaternion<T> &other) const operator!=(const Quaternion<T> &other) const
{ {
@ -353,12 +386,14 @@ public:
} }
/// Support stream output of a quaternion in the form `a + <i, j, k>`. /// \brief Output a MakeQuaternion to a stream in the form
/// `a + <i, j, k>`.
///
/// \todo improve the formatting. /// \todo improve the formatting.
/// ///
/// @param outs An output stream /// \param outs An output stream
/// @param q A quaternion /// \param q A MakeQuaternion
/// @return The output stream /// \return The output stream
friend std::ostream & friend std::ostream &
operator<<(std::ostream &outs, const Quaternion<T> &q) operator<<(std::ostream &outs, const Quaternion<T> &q)
{ {
@ -370,8 +405,8 @@ private:
static constexpr T minRotation = -4 * M_PI; static constexpr T minRotation = -4 * M_PI;
static constexpr T maxRotation = 4 * M_PI; static constexpr T maxRotation = 4 * M_PI;
Vector<T, 3> v; // axis of rotation Vector<T, 3> v; // Axis of rotation
T w; // angle of rotation T w; // Angle of rotation
T eps; T eps;
void void
@ -393,76 +428,93 @@ private:
/// ///
/// \ingroup quaternion_aliases /// \ingroup quaternion_aliases
/// Type alias for a float Quaternion. /// \brief Type alias for a float Quaternion.
typedef Quaternion<float> Quaternionf; typedef Quaternion<float> Quaternionf;
/// \ingroup quaternion_aliases /// \ingroup quaternion_aliases
/// Type alias for a double Quaternion. /// \brief Type alias for a double Quaternion.
typedef Quaternion<double> Quaterniond; typedef Quaternion<double> Quaterniond;
/// Return a float quaternion scaled appropriately from a vector and angle, /// \brief Convenience Quaternion construction function.
/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
/// ///
/// @param axis The axis of rotation. /// Return a float MakeQuaternion scaled appropriately from a vector and
/// @param angle The angle of rotation. /// Angle, e.g.
/// @return A quaternion. /// angle = cos(Angle / 2),
/// @relatesalso Quaternion /// Axis.UnitVector() * sin(Angle / 2).
Quaternionf quaternionf(Vector3f axis, float angle);
/// Return a double quaternion scaled appropriately from a vector and angle,
/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
/// ///
/// @param axis The axis of rotation. /// \param axis The Axis of rotation.
/// @param angle The angle of rotation. /// \param angle The Angle of rotation.
/// @return A quaternion. /// \return A MakeQuaternion.
/// @relatesalso Quaternion /// \relatesalso Quaternion
Quaterniond quaterniond(Vector3d axis, double angle); Quaternionf MakeQuaternion(Vector3F axis, float angle);
/// \brief Convience Quaternion construction function.
/// Return a double quaternion scaled appropriately from a vector and angle,
/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
/// ///
/// @param axis The axis of rotation. /// Return a double MakeQuaternion scaled appropriately from a vector and
/// @param angle The angle of rotation. /// Angle, e.g.
/// @return A quaternion. /// Angle = cos(Angle / 2),
/// @relatesalso Quaternion /// Axis.UnitVector() * sin(Angle / 2).
///
/// \param axis The Axis of rotation.
/// \param angle The Angle of rotation.
/// \return A MakeQuaternion.
/// \relatesalso Quaternion
Quaterniond MakeQuaternion(Vector3D axis, double angle);
/// \brief Convience Quaternion construction function.
///
/// Return a double MakeQuaternion scaled appropriately from a vector and
/// Angle, e.g.
/// Angle = cos(Angle / 2),
/// Axis.UnitVector() * sin(Angle / 2).
///
/// \param axis The Axis of rotation.
/// \param angle The Angle of rotation.
/// \return A MakeQuaternion.
/// \relatesalso Quaternion
template <typename T> template <typename T>
Quaternion<T> Quaternion<T>
quaternion(Vector<T, 3> axis, T angle) MakeQuaternion(Vector<T, 3> axis, T angle)
{ {
return Quaternion<T>(axis.unitVector() * std::sin(angle / (T)2.0), return Quaternion<T>(axis.UnitVector() * std::sin(angle / (T)2.0),
std::cos(angle / (T)2.0)); std::cos(angle / (T)2.0));
} }
/// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll), /// \brief COnstruct a Quaternion from Euler angles.
/// return a quaternion.
/// ///
/// @param euler A vector Euler angle in ZYX sequence. /// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch,
/// @return A Quaternion representation of the Orientation represented /// roll), return a Quaternion.
/// by the Euler angles.
/// @relatesalso Quaternion
Quaternionf quaternionf_from_euler(Vector3f euler);
/// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll),
/// return a quaternion.
/// ///
/// @param euler A vector Euler angle in ZYX sequence. /// \param euler A vector Euler Angle in ZYX sequence.
/// @return A Quaternion representation of the Orientation represented /// \return A Quaternion representation of the Orientation represented
/// by the Euler angles. /// by the Euler angles.
/// @relatesalso Quaternion /// \relatesalso Quaternion
Quaterniond quaterniond_from_euler(Vector3d euler); Quaternionf QuaternionFromEuler(Vector3F euler);
/// LERP computes the linear interpolation of two quaternions at some /// \brief COnstruct a Quaternion from Euler angles.
///
/// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch,
/// roll), return a Quaternion.
///
/// \param euler A vector Euler Angle in ZYX sequence.
/// \return A Quaternion representation of the Orientation represented
/// by the Euler angles.
/// \relatesalso Quaternion
Quaterniond QuaternionFromEuler(Vector3D euler);
/// \brief Linear interpolation for two Quaternions.
///
/// LERP computes the linear interpolation of two quaternions At some
/// fraction of the distance between them. /// fraction of the distance between them.
/// ///
/// \tparam T /// \tparam T
/// \param p The starting quaternion. /// \param p The starting MakeQuaternion.
/// \param q The ending quaternion. /// \param q The ending MakeQuaternion.
/// \param t The fraction of the distance between the two quaternions to /// \param t The fraction of the distance between the two quaternions to
/// interpolate. /// interpolate.
/// \return A Quaternion representing the linear interpolation of the /// \return A Quaternion representing the linear interpolation of the
@ -471,17 +523,19 @@ template <typename T>
Quaternion<T> Quaternion<T>
LERP(Quaternion<T> p, Quaternion<T> q, T t) LERP(Quaternion<T> p, Quaternion<T> q, T t)
{ {
return (p + (q - p) * t).unitQuaternion(); return (p + (q - p) * t).UnitQuaternion();
} }
/// \brief Shortest distance spherical linear interpolation.
///
/// ShortestSLERP computes the shortest distance spherical linear /// ShortestSLERP computes the shortest distance spherical linear
/// interpolation between two quaternions at some fraction of the /// interpolation between two unit quaternions At some fraction of the
/// distance between them. /// distance between them.
/// ///
/// \tparam T /// \tparam T
/// \param p The starting quaternion. /// \param p The starting MakeQuaternion.
/// \param q The ending quaternion.Short /// \param q The ending MakeQuaternion.Short
/// \param t The fraction of the distance between the two quaternions /// \param t The fraction of the distance between the two quaternions
/// to interpolate. /// to interpolate.
/// \return A Quaternion representing the shortest path between two /// \return A Quaternion representing the shortest path between two
@ -490,10 +544,10 @@ template <typename T>
Quaternion<T> Quaternion<T>
ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t) ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t)
{ {
assert(p.isUnitQuaternion()); assert(p.IsUnitQuaternion());
assert(q.isUnitQuaternion()); assert(q.IsUnitQuaternion());
T dp = p.dot(q); T dp = p.Dot(q);
T sign = dp < 0.0 ? -1.0 : 1.0; T sign = dp < 0.0 ? -1.0 : 1.0;
T omega = std::acos(dp * sign); T omega = std::acos(dp * sign);
T sin_omega = std::sin(omega); // Compute once. T sin_omega = std::sin(omega); // Compute once.
@ -507,14 +561,16 @@ ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t)
} }
/// \brief Internal consistency check.
///
/// Run a quick self test to exercise basic functionality of the Quaternion /// Run a quick self test to exercise basic functionality of the Quaternion
/// class to verify correct operation. Note that if \#NDEBUG is defined, the /// class to verify correct operation. Note that if \#NDEBUG is defined, the
/// self test is disabled. /// self test is disabled.
void Quaternion_SelfTest(); void QuaternionSelfTest();
} // namespace geom } // namespace geom
} // namespace wr } // namespace wr
#endif // WRMATH_QUATERNION_H #endif // SCMATH_GEOM_QUATERNION_H

307
include/scmp/geom/Vector.h
View File

@ -1,28 +1,27 @@
// ///
// Project: scccl /// \file include/scmp/geom/Vector.h
// File: include/math/vectors.h /// \author K. Isom <kyle@imap.cc>
// Author: Kyle Isom /// \date 2017-06-05
// Date: 2017-06-05 /// \brief Linear algebraic vector class.
// Namespace: math::vectors. ///
// /// Copyright 2017 K. Isom <kyle@imap.cc>
// vectors.h defines the Vector2D class and associated functions in the ///
// namespace scmp::vectors. /// Permission to use, copy, modify, and/or distribute this software for
// /// any purpose with or without fee is hereby granted, provided that
// Copyright 2017 Kyle Isom <kyle@imap.cc> /// the above copyright notice and this permission notice appear in all /// copies.
// ///
// Licensed under the Apache License, Version 2.0 (the "License"); /// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
// you may not use this file except in compliance with the License. /// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
// You may obtain a copy of the License at /// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
// /// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
// http://www.apache.org/licenses/LICENSE-2.0 /// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
// /// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
// Unless required by applicable law or agreed to in writing, software /// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
// distributed under the License is distributed on an "AS IS" BASIS, /// PERFORMANCE OF THIS SOFTWARE.
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ///
// See the License for the specific language governing permissions and
// limitations under the License. #ifndef SCMATH_GEOM_VECTORS_H
#ifndef SCMATH_VECTORS_H #define SCMATH_GEOM_VECTORS_H
#define SCMATH_VECTORS_H
#include <array> #include <array>
@ -44,21 +43,20 @@ namespace scmp {
namespace geom { namespace geom {
/// @brief Vectors represent a direction and magnitude. /// \brief Vectors represent a direction and Magnitude.
/// ///
/// Vector provides a standard interface for dimensionless fixed-size /// Vector provides a standard interface for dimensionless fixed-size
/// vectors. Once instantiated, they cannot be modified. /// vectors. Once instantiated, they cannot be modified.
/// ///
/// Note that while the class is templated, it's intended to be used with /// Note that while the class is templated, it's intended to be used
/// floating-point types. /// with floating-point types.
/// ///
/// Vectors can be indexed like arrays, and they contain an epsilon value /// Vectors can be indexed like arrays, and they contain an epsilon
/// that defines a tolerance for equality. /// value that defines a tolerance for equality.
template<typename T, size_t N> template<typename T, size_t N>
class Vector { class Vector {
public: public:
/// The default constructor creates a unit vector for a given type /// \brief Construct a unit vector of a given type and size.
/// and size.
Vector() Vector()
{ {
T unitLength = (T) 1.0 / std::sqrt(N); T unitLength = (T) 1.0 / std::sqrt(N);
@ -70,9 +68,12 @@ public:
} }
/// \brief Construct a Vector with initial values.
///
/// If given an initializer_list, the vector is created with /// If given an initializer_list, the vector is created with
/// those values. There must be exactly N elements in the list. /// those values. There must be exactly N elements in the list.
/// @param ilst An intializer list with N elements of type T. ///
/// \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); assert(ilst.size() == N);
@ -82,13 +83,13 @@ public:
} }
/// \brief Return the element at index i. /// \brief Return the element At index i.
/// ///
/// \throws std::out_of_range if the index is out of bounds. /// \throws std::out_of_range if the index is out of bounds.
/// ///
/// \param index The index of the item to retrieve. /// \param index The index of the item to retrieve.
/// \return The value at the index. /// \return The value At the index.
T at(size_t index) const T At(size_t index) const
{ {
if (index > this->arr.size()) { if (index > this->arr.size()) {
throw std::out_of_range("index " + throw std::out_of_range("index " +
@ -105,7 +106,7 @@ public:
/// ///
/// \throws std::out_of_range if the index is out of bounds. /// \throws std::out_of_range if the index is out of bounds.
/// ///
/// \param index The index to insert the value at. /// \param index The index to insert the value At.
/// \param value /// \param value
void Set(size_t index, T value) void Set(size_t index, T value)
{ {
@ -122,9 +123,10 @@ public:
/// Compute the length of the vector. /// \brief Compute the length of the vector.
/// @return The length of the vector. ///
T magnitude() const /// \return The length of the vector.
T Magnitude() const
{ {
T result = 0; T result = 0;
@ -135,21 +137,25 @@ public:
} }
/// Set the tolerance for equality checks. At a minimum, this allows /// \brief Set equivalence tolerance.
/// for systemic errors in floating math arithmetic. ///
/// @param eps is the maximum difference between this vector and /// Set the tolerance for equality checks. At a minimum, this
/// accounts for systemic errors in floating math arithmetic.
///
/// \param eps is the maximum difference between this vector and
/// another. /// another.
void void
setEpsilon(T eps) SetEpsilon(T eps)
{ {
this->epsilon = eps; this->epsilon = eps;
} }
/// Determine whether this is a zero vector. /// \brief Determine whether this is a zero vector.
/// @return true if the vector is zero. ///
/// \return true if the vector is zero.
bool bool
isZero() const IsZero() const
{ {
for (size_t i = 0; i < N; i++) { 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)) {
@ -160,51 +166,55 @@ public:
} }
/// Obtain the unit vector for this vector. /// \brief Obtain the unit vector for this vector.
/// @return The unit vector ///
/// \return The unit vector
Vector Vector
unitVector() const UnitVector() const
{ {
return *this / this->magnitude(); return *this / this->Magnitude();
} }
/// Determine if this is a unit vector, e.g. if its length is 1. /// \brief Determine if this is a unit vector.
/// @return true if the vector is a unit vector. ///
/// \return true if the vector is a unit vector.
bool bool
isUnitVector() const IsUnitVector() const
{ {
return scmp::WithinTolerance(this->magnitude(), (T) 1.0, this->epsilon); return scmp::WithinTolerance(this->Magnitude(), (T) 1.0, this->epsilon);
} }
/// Compute the angle between two other vectors. /// \brief Compute the Angle between two vectors.
/// @param other Another vector. ///
/// @return The angle in radians between the two vectors. /// \param other Another vector.
/// \return The Angle in radians between the two vectors.
T T
angle(const Vector<T, N> &other) const Angle(const Vector<T, N> &other) const
{ {
Vector<T, N> unitA = this->unitVector(); Vector<T, N> unitA = this->UnitVector();
Vector<T, N> unitB = other.unitVector(); Vector<T, N> unitB = other.UnitVector();
// Can't compute angles with a zero vector. // Can't compute angles with a zero vector.
assert(!this->isZero()); assert(!this->IsZero());
assert(!other.isZero()); assert(!other.IsZero());
return std::acos(unitA * unitB); return static_cast<T>(std::acos(unitA * unitB));
} }
/// Determine whether two vectors are parallel. /// \brief Determine whether two vectors are parallel.
/// @param other Another vector ///
/// @return True if the angle between the vectors is zero. /// \param other Another vector
/// \return True if the Angle between the vectors is zero.
bool bool
isParallel(const Vector<T, N> &other) const IsParallel(const Vector<T, N> &other) const
{ {
if (this->isZero() || other.isZero()) { if (this->IsZero() || other.IsZero()) {
return true; return true;
} }
T angle = this->angle(other); 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; return true;
} }
@ -213,14 +223,15 @@ public:
} }
/// Determine if two vectors are orthogonal or perpendicular to each /// \brief Determine if two vectors are orthogonal or
/// other. /// perpendicular to each other.
/// @param other Another vector ///
/// @return True if the two vectors are orthogonal. /// \param other Another vector
/// \return True if the two vectors are orthogonal.
bool bool
isOrthogonal(const Vector<T, N> &other) const IsOrthogonal(const Vector<T, N> &other) const
{ {
if (this->isZero() || other.isZero()) { if (this->IsZero() || other.IsZero()) {
return true; return true;
} }
@ -228,40 +239,51 @@ public:
} }
/// Project this vector onto some basis vector. /// \brief Project this vector onto some basis vector.
/// @param basis The basis vector to be projected onto. ///
/// @return A vector that is the projection of this onto the basis /// \param basis The basis vector to be projected onto.
/// \return A vector that is the projection of this onto the basis
/// vector. /// vector.
Vector Vector
projectParallel(const Vector<T, N> &basis) const 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); return unit_basis * (*this * unit_basis);
} }
/// Project this vector perpendicularly onto some basis vector. /// \brief Project this vector perpendicularly onto some basis vector.
/// This is also called the rejection of the vector. ///
/// @param basis The basis vector to be projected onto. /// This is also called the *rejection* of the vector.
/// @return A vector that is the orthogonal projection of this onto ///
/// \param basis The basis vector to be projected onto.
/// \return A vector that is the orthogonal projection of this onto
/// the basis vector. /// the basis vector.
Vector Vector
projectOrthogonal(const Vector<T, N> &basis) ProjectOrthogonal(const Vector<T, N> &basis)
{ {
Vector<T, N> spar = this->projectParallel(basis); Vector<T, N> spar = this->ProjectParallel(basis);
return *this - spar; return *this - spar;
} }
/// Compute the cross product of two vectors. This is only defined /// \brief Compute the cross product of two vectors.
/// over three-dimensional vectors. ///
/// @param other Another 3D vector. /// This is only defined over three-dimensional vectors.
/// @return The cross product vector. ///
/// \throw std::out_of_range if this is not a three-dimensional vector.
///
/// \param other Another 3D vector.
/// \return The Cross product vector.
Vector Vector
cross(const Vector<T, N> &other) const Cross(const Vector<T, N> &other) const
{ {
assert(N == 3); assert(N == 3);
if (N != 3) {
throw std::out_of_range("Cross-product can only called on Vector<T, 3>.");
}
return Vector<T, N>{ return Vector<T, N>{
(this->arr[1] * other.arr[2]) - (other.arr[1] * this->arr[2]), (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[2]) - (other.arr[0] * this->arr[2])),
@ -270,9 +292,10 @@ public:
} }
/// Perform vector addition with another vector. /// \brief Vector addition.
/// @param other The vector to be added. ///
/// @return A new vector that is the result of adding this and the /// \param other The vector to be added.
/// \return A new vector that is the result of adding this and the
/// other vector. /// other vector.
Vector Vector
operator+(const Vector<T, N> &other) const operator+(const Vector<T, N> &other) const
@ -287,9 +310,10 @@ public:
} }
/// Perform vector subtraction with another vector. /// \brief Vector subtraction.
/// @param other The vector to be subtracted from this vector. ///
/// @return A new vector that is the result of subtracting the /// \param other The vector to be subtracted from this vector.
/// \return A new vector that is the result of subtracting the
/// other vector from this one. /// other vector from this one.
Vector Vector
operator-(const Vector<T, N> &other) const operator-(const Vector<T, N> &other) const
@ -304,9 +328,10 @@ public:
} }
/// Perform scalar multiplication of this vector by some scale factor. /// \brief Scalar multiplication.
/// @param k The scaling value. ///
/// @return A new vector that is this vector scaled by k. /// \param k The scaling value.
/// \return A new vector that is this vector scaled by k.
Vector Vector
operator*(const T k) const operator*(const T k) const
{ {
@ -320,9 +345,10 @@ public:
} }
/// Perform scalar division of this vector by some scale factor. /// \brief Scalar division.
/// @param k The scaling value ///
/// @return A new vector that is this vector scaled by 1/k. /// \param k The scaling value
/// \return A new vector that is this vector scaled by 1/k.
Vector Vector
operator/(const T k) const operator/(const T k) const
{ {
@ -336,9 +362,10 @@ public:
} }
/// Compute the dot product between two vectors. /// \brief Compute the Dot product between two vectors.
/// @param other The other vector. ///
/// @return A scalar value that is the dot product of the two vectors. /// \param other The other vector.
/// \return A scalar value that is the Dot product of the two vectors.
T T
operator*(const Vector<T, N> &other) const operator*(const Vector<T, N> &other) const
{ {
@ -352,9 +379,10 @@ public:
} }
/// Compare two vectors for equality. /// \brief Vector equivalence
/// @param other The other vector. ///
/// @return Return true if all the components of both vectors are /// \param other The other vector.
/// \return Return true if all the components of both vectors are
/// within the tolerance value. /// within the tolerance value.
bool bool
operator==(const Vector<T, N> &other) const operator==(const Vector<T, N> &other) const
@ -368,9 +396,10 @@ public:
} }
/// Compare two vectors for inequality. /// \brief Vector non-equivalence.
/// @param other The other vector. ///
/// @return Return true if any of the components of both vectors are /// \param other The other vector.
/// \return Return true if any of the components of both vectors are
/// not within the tolerance value. /// not within the tolerance value.
bool bool
operator!=(const Vector<T, N> &other) const operator!=(const Vector<T, N> &other) const
@ -379,18 +408,18 @@ public:
} }
/// Support array indexing into vector. /// \brief Array indexing into vector.
/// ///
/// Note that the values of the vector cannot be modified. Instead, /// Note that the values of the vector cannot be modified.
/// it's required to do something like the following: /// Instead, something like the following must be done:
/// ///
/// ``` /// ```
/// Vector3d a {1.0, 2.0, 3.0}; /// Vector3D a {1.0, 2.0, 3.0};
/// Vector3d b {a[0], a[1]*2.0, a[2]}; /// Vector3D b {a[0], a[1]*2.0, a[2]};
/// ``` /// ```
/// ///
/// @param i The component index. /// \param i The component index.
/// @return The value of the vector component at i. /// \return The value of the vector component At i.
const T & const T &
operator[](size_t i) const operator[](size_t i) const
{ {
@ -398,10 +427,11 @@ public:
} }
/// Support outputting vectors in the form "<i, j, ...>". /// \brief Write a vector a stream in the form "<i, j, ...>".
/// @param outs An output stream. ///
/// @param vec The vector to be formatted. /// \param outs An output stream.
/// @return The output stream. /// \param vec The vector to be formatted.
/// \return The output stream.
friend std::ostream & friend std::ostream &
operator<<(std::ostream &outs, const Vector<T, N> &vec) operator<<(std::ostream &outs, const Vector<T, N> &vec)
{ {
@ -429,36 +459,35 @@ private:
/// \ingroup vector_aliases /// \ingroup vector_aliases
/// A number of shorthand aliases for vectors are provided. They follow /// A number of shorthand aliases for vectors are provided. They follow
/// the form of VectorNt, where N is the dimension and t is the type. /// the form of VectorNt, where N is the dimension and t is the type.
/// For example, a 2D float vector is Vector2f. /// For example, a 2D float vector is Vector2F.
/// \ingroup vector_aliases /// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional float vector. /// \brief Type alias for a two-dimensional float vector.
typedef Vector<float, 2> Vector2f; typedef Vector<float, 2> Vector2F;
/// \ingroup vector_aliases /// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional float vector. /// \brief Type alias for a three-dimensional float vector.
typedef Vector<float, 3> Vector3f; typedef Vector<float, 3> Vector3F;
/// \ingroup vector_aliases /// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional float vector. /// \brief Type alias for a four-dimensional float vector.
typedef Vector<float, 4> Vector4f; typedef Vector<float, 4> Vector4F;
/// \ingroup vector_aliases /// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional double vector. /// \brief Type alias for a two-dimensional double vector.
typedef Vector<double, 2> Vector2d; typedef Vector<double, 2> Vector2D;
/// \ingroup vector_aliases /// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional double vector. /// \brief Type alias for a three-dimensional double vector.
typedef Vector<double, 3> Vector3d; typedef Vector<double, 3> Vector3D;
/// \ingroup vector_aliases /// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional double vector. /// \brief Type alias for a four-dimensional double vector.
typedef Vector<double, 4> Vector4d; typedef Vector<double, 4> Vector4D;
} // namespace geom } // namespace geom
} // namespace scmp } // namespace scmp
#endif // SCMATH_GEOM_VECTORS_H
#endif // SCMATH_VECTORS_H_H

2
include/scsl/Arena.h
View File

@ -70,7 +70,7 @@ enum class ArenaType
/// The arena should be initialized with one of the Set methods (SetStatic, /// The arena should be initialized with one of the Set methods (SetStatic,
/// SetAlloc) or one of the file-based options (Create, Open, MemoryMap). At /// SetAlloc) or one of the file-based options (Create, Open, MemoryMap). At
/// this point, no further memory management should be done until the end of the /// this point, no further memory management should be done until the end of the
/// arena's life, at which point Destroy should be called. /// arena's life, At which point Destroy should be called.
class Arena { class Arena {
public: public:
/// An Arena is initialized with no backing memory. /// An Arena is initialized with no backing memory.

18
include/scsl/Buffer.h
View File

@ -102,37 +102,37 @@ public:
/// \return True if the Buffer was resized. /// \return True if the Buffer was resized.
bool Append(const uint8_t c); bool Append(const uint8_t c);
/// Insert copies a C-style string into the buffer at index. /// Insert copies a C-style string into the buffer At index.
/// ///
/// \param index The index to insert the string at. /// \param index The index to insert the string At.
/// \param s The string to insert. /// \param s The string to insert.
/// \return True if the Buffer was resized. /// \return True if the Buffer was resized.
bool Insert(const size_t index, const char *s); bool Insert(const size_t index, const char *s);
/// Insert copies a string into the buffer at index. /// Insert copies a string into the buffer At index.
/// ///
/// \param index The index the string should be inserted at. /// \param index The index the string should be inserted At.
/// \param s The string to insert. /// \param s The string to insert.
/// \return True if the Buffer was resized. /// \return True if the Buffer was resized.
bool Insert(const size_t index, const std::string s); bool Insert(const size_t index, const std::string s);
/// Insert copies a uint8_t buffer into the buffer at index. /// Insert copies a uint8_t buffer into the buffer At index.
/// ///
/// \param index The index to insert the buffer at. /// \param index The index to insert the buffer At.
/// \param data The buffer to insert. /// \param data The buffer to insert.
/// \param datalen The size of the data buffer. /// \param datalen The size of the data buffer.
/// \return True if the Buffer was resized. /// \return True if the Buffer was resized.
bool bool
Insert(const size_t index, const uint8_t *data, const size_t datalen); Insert(const size_t index, const uint8_t *data, const size_t datalen);
/// Insert copies a character into the buffer at index. /// Insert copies a character into the buffer At index.
/// ///
/// \param index The index to insert the character at. /// \param index The index to insert the character At.
/// \param c The character to insert. /// \param c The character to insert.
/// \return True if the Buffer was resized. /// \return True if the Buffer was resized.
bool Insert(const size_t index, const uint8_t c); bool Insert(const size_t index, const uint8_t c);
/// Remove removes `count` bytes from the buffer at `index`. /// Remove removes `count` bytes from the buffer At `index`.
/// ///
/// \param index The starting index to remove bytes from. /// \param index The starting index to remove bytes from.
/// \param count The number of bytes to remove. /// \param count The number of bytes to remove.

2
include/scsl/Flags.h
View File

@ -282,7 +282,7 @@ public:
/// Return a particular argument. /// Return a particular argument.
/// ///
/// \param index The argument number to extract. /// \param index The argument number to extract.
/// \return The argument at index i. If the index is greater than /// \return The argument At index i. If the index is greater than
/// the number of arguments present, an out_of_range /// the number of arguments present, an out_of_range
/// exception is thrown. /// exception is thrown.
std::string Arg(size_t index); std::string Arg(size_t index);

14
include/scsl/StringUtil.h
View File

@ -39,27 +39,27 @@ namespace U {
namespace S { namespace S {
/// Remove any whitespace at the beginning of the string. The string /// Remove any whitespace At the beginning of the string. The string
/// is modified in-place. /// is modified in-place.
void TrimLeadingWhitespace(std::string &s); void TrimLeadingWhitespace(std::string &s);
/// Remove any whitespace at the end of the string. The string is /// Remove any whitespace At the end of the string. The string is
/// modified in-place. /// modified in-place.
void TrimTrailingWhitespace(std::string &s); void TrimTrailingWhitespace(std::string &s);
/// Remove any whitespace at the beginning and end of the string. The /// Remove any whitespace At the beginning and end of the string. The
/// string is modified in-place. /// string is modified in-place.
void TrimWhitespace(std::string &s); void TrimWhitespace(std::string &s);
/// Remove any whitespace at the beginning of the string. The original /// Remove any whitespace At the beginning of the string. The original
/// string isn't modified, and a copy is returned. /// string isn't modified, and a copy is returned.
std::string TrimLeadingWhitespaceDup(std::string s); std::string TrimLeadingWhitespaceDup(std::string s);
/// Remove any whitespace at the end of the string. The original string /// Remove any whitespace At the end of the string. The original string
/// isn't modified, and a copy is returned. /// isn't modified, and a copy is returned.
std::string TrimTrailingWhitespaceDup(std::string s); std::string TrimTrailingWhitespaceDup(std::string s);
/// Remove any whitespace at the beginning and end of the string. The /// Remove any whitespace At the beginning and end of the string. The
/// original string isn't modified, and a copy is returned. /// original string isn't modified, and a copy is returned.
std::string TrimWhitespaceDup(std::string s); std::string TrimWhitespaceDup(std::string s);
@ -92,7 +92,7 @@ std::vector<std::string> SplitKeyValuePair(std::string line, char delimiter);
std::vector<std::string> SplitN(std::string, std::string delimiter, size_t maxCount=0); std::vector<std::string> SplitN(std::string, std::string delimiter, size_t maxCount=0);
/// WrapText is a very simple wrapping function that breaks the line into /// WrapText is a very simple wrapping function that breaks the line into
/// lines of at most lineLength characters. It does this by breaking the /// lines of At most lineLength characters. It does this by breaking the
/// line into individual words (splitting on whitespace). /// line into individual words (splitting on whitespace).
std::vector<std::string> WrapText(std::string& line, size_t lineLength); std::vector<std::string> WrapText(std::string& line, size_t lineLength);

8
include/scsl/TLV.h
View File

@ -46,7 +46,7 @@ struct Record {
uint8_t Val[TLV_MAX_LEN]; uint8_t Val[TLV_MAX_LEN];
}; };
/// WriteToMemory writes the TLV record into the arena at the location pointed /// WriteToMemory writes the TLV record into the arena At the location pointed
/// to in the arena. /// to in the arena.
/// ///
/// \param arena The backing memory store. /// \param arena The backing memory store.
@ -86,7 +86,7 @@ void DeleteRecord(Arena &arena, uint8_t *cursor);
/// ///
/// \param arena The backing memory for the TLV store. /// \param arena The backing memory for the TLV store.
/// \param cursor A pointer to memory inside the arena; if it's NULL, the /// \param cursor A pointer to memory inside the arena; if it's NULL, the
/// search starts at the beginning of the arena. /// search starts At the beginning of the arena.
/// \param rec The record to be filled. /// \param rec The record to be filled.
/// \return If the tag is found, a cursor pointing to the next record is /// \return If the tag is found, a cursor pointing to the next record is
/// returned; otherwise nullptr is returned. /// returned; otherwise nullptr is returned.
@ -97,7 +97,7 @@ uint8_t *FindTag(Arena &arena, uint8_t *cursor, Record &rec);
/// ///
/// \param arena The backing memory for the TLV store. /// \param arena The backing memory for the TLV store.
/// \param cursor A pointer to memory inside the arena; if it's NULL, the /// \param cursor A pointer to memory inside the arena; if it's NULL, the
/// search starts at the beginning of the arena. /// search starts At the beginning of the arena.
/// \param rec The record to be filled. /// \param rec The record to be filled.
/// \return If the tag is found, a cursor pointing to the record is /// \return If the tag is found, a cursor pointing to the record is
/// returned; otherwise nullptr is returned. /// returned; otherwise nullptr is returned.
@ -110,7 +110,7 @@ uint8_t *LocateTag(Arena &arena, uint8_t *cursor, Record &rec);
/// ///
/// \param arena The backing memory for the TLV store. /// \param arena The backing memory for the TLV store.
/// \param cursor A pointer to memory inside the arena; if it's NULL, the /// \param cursor A pointer to memory inside the arena; if it's NULL, the
/// search starts at the beginning of the arena. /// search starts At the beginning of the arena.
/// \return If the arena has space available, a cursor pointing the start /// \return If the arena has space available, a cursor pointing the start
/// of empty space; otherwise, nullptr is returned. /// of empty space; otherwise, nullptr is returned.
uint8_t *FindEmpty(Arena &arena, uint8_t *cursor); uint8_t *FindEmpty(Arena &arena, uint8_t *cursor);

2
include/sctest/Report.h
View File

@ -12,7 +12,7 @@
// //
// Licensed under the Apache License, Version 2.0 (the "License"); // Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License. // you may not use this file except in compliance with the License.
// You may obtain a copy of the License at // You may obtain a copy of the License At
// //
// http://www.apache.org/licenses/LICENSE-2.0 // http://www.apache.org/licenses/LICENSE-2.0
// //

4
include/sctest/SimpleSuite.h
View File

@ -59,14 +59,14 @@ public:
/// \brief Define a suite setup function. /// \brief Define a suite setup function.
/// ///
/// If present, this setup function is called at the start of /// If present, this setup function is called At the start of
/// the Run method, before tests are run. It should be a /// the Run method, before tests are run. It should be a
/// predicate: if it returns false, tests automatically fail. /// predicate: if it returns false, tests automatically fail.
void Setup(std::function<bool(void)> setupFn) { fnSetup = setupFn; } void Setup(std::function<bool(void)> setupFn) { fnSetup = setupFn; }
/// \brief Define a teardown function. /// \brief Define a teardown function.
/// ///
/// If present, this teardown function is called at the end of /// If present, this teardown function is called At the end of
/// the Run method, after all tests have run. /// the Run method, after all tests have run.
void Teardown(std::function<bool(void)> teardownFn) { fnTeardown = teardownFn; } void Teardown(std::function<bool(void)> teardownFn) { fnTeardown = teardownFn; }

55
src/scmp/Coord2D.cc
View File

@ -1,33 +1,32 @@
///
/// \file include/scmp/geom/Coord2D.h
/// \author K. Isom <kyle@imap.cc>
/// \date 2017-06-05
/// \brief 2D point and polar coordinate systems.
///
/// Copyright 2023 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
//
// Project: scccl
// File: src/math/geom2d.cpp
// Author: Kyle Isom
// Date: 2017-06-05
// Namespace: math::geom
//
// geom2d.cpp contains the implementation of 2D geometry in the math::geom
// namespace.
//
// Copyright 2017 Kyle Isom <kyle@imap.cc>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <cmath> #include <cmath>
#include <iostream> #include <iostream>
#include <vector> #include <vector>
#include <scmp/Math.h> #include <scmp/Math.h>
#include <scmp/geom/Coord2D.h> #include <scmp/geom/Coord2D.h>
#include <scmp/geom/Orientation.h>
#include <scmp/geom/Vector.h> #include <scmp/geom/Vector.h>
@ -61,21 +60,21 @@ Point2D::Point2D(const Polar2D &pol)
int int
Point2D::X() const Point2D::X() const
{ {
return this->at(0); return this->At(0);
} }
void void
Point2D::X(int _x) Point2D::X(int _x)
{ {
this->Set(0, _x); this->Set(BasisX, _x);
} }
int int
Point2D::Y() const Point2D::Y() const
{ {
return this->at(1); return this->At(1);
} }
@ -165,7 +164,7 @@ Polar2D::Polar2D(const Point2D &pt)
double double
Polar2D::R() const Polar2D::R() const
{ {
return this->at(0); return this->At(0);
} }
@ -179,7 +178,7 @@ Polar2D::R(const double _r)
double double
Polar2D::Theta() const Polar2D::Theta() const
{ {
return this->at(1); return this->At(1);
} }

4
src/scmp/Motion2D.cc
View File

@ -5,13 +5,13 @@ namespace scmp {
namespace basic { namespace basic {
scmp::geom::Vector2d scmp::geom::Vector2D
Acceleration(double speed, double heading) Acceleration(double speed, double heading)
{ {
auto dx = std::cos(heading) * speed; auto dx = std::cos(heading) * speed;
auto dy = std::sin(heading) * speed; auto dy = std::sin(heading) * speed;
return scmp::geom::Vector2d({dx, dy}); return scmp::geom::Vector2D({dx, dy});
} }

18
src/scmp/Orientation.cc
View File

@ -7,31 +7,31 @@ namespace geom {
float float
Heading2f(Vector2f vec) Heading2F(Vector2F vec)
{ {
return vec.angle(Basis2f[Basis_x]); return vec.Angle(Basis2F[BasisX]);
} }
float float
Heading3f(Vector3f vec) Heading3f(Vector3F vec)
{ {
Vector2f vec2f {vec[0], vec[1]}; Vector2F vec2f {vec[0], vec[1]};
return Heading2f(vec2f); return Heading2F(vec2f);
} }
double double
Heading2d(Vector2d vec) Heading2d(Vector2D vec)
{ {
return vec.angle(Basis2d[Basis_x]); return vec.Angle(Basis2D[BasisX]);
} }
double double
Heading3d(Vector3d vec) Heading3d(Vector3D vec)
{ {
Vector2d vec2d {vec[0], vec[1]}; Vector2D vec2d {vec[0], vec[1]};
return Heading2d(vec2d); return Heading2d(vec2d);
} }

24
src/scmp/Quaternion.cc
View File

@ -8,23 +8,23 @@ namespace geom {
Quaternionf Quaternionf
quaternionf(Vector3f axis, float angle) MakeQuaternion(Vector3F axis, float angle)
{ {
return Quaternionf(axis.unitVector() * std::sin(angle / 2.0), return Quaternionf(axis.UnitVector() * std::sin(angle / 2.0),
std::cos(angle / 2.0)); std::cos(angle / 2.0));
} }
Quaterniond Quaterniond
quaterniond(Vector3d axis, double angle) MakeQuaternion(Vector3D axis, double angle)
{ {
return Quaterniond(axis.unitVector() * std::sin(angle / 2.0), return Quaterniond(axis.UnitVector() * std::sin(angle / 2.0),
std::cos(angle / 2.0)); std::cos(angle / 2.0));
} }
Quaternionf Quaternionf
quaternionf_from_euler(Vector3f euler) QuaternionFromEuler(Vector3F euler)
{ {
float x, y, z, w; float x, y, z, w;
euler = euler / 2.0; euler = euler / 2.0;
@ -41,12 +41,12 @@ quaternionf_from_euler(Vector3f euler)
z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll); z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll);
w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll); w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll);
return Quaternionf(Vector4f{w, x, y, z}); return Quaternionf(Vector4F{w, x, y, z});
} }
Quaterniond Quaterniond
quaterniond_from_euler(Vector3d euler) QuaternionFromEuler(Vector3D euler)
{ {
double x, y, z, w; double x, y, z, w;
euler = euler / 2.0; euler = euler / 2.0;
@ -63,21 +63,21 @@ quaterniond_from_euler(Vector3d euler)
z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll); z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll);
w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll); w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll);
return Quaterniond(Vector4d{w, x, y, z}); return Quaterniond(Vector4D{w, x, y, z});
} }
void void
Quaternion_SelfTest() QuaternionSelfTest()
{ {
#ifndef NDEBUG #ifndef NDEBUG
Vector3f v {1.0, 0.0, 0.0}; Vector3F v {1.0, 0.0, 0.0};
Vector3f yAxis {0.0, 1.0, 0.0}; Vector3F yAxis {0.0, 1.0, 0.0};
float angle = M_PI / 2; float angle = M_PI / 2;
Quaternionf p = quaternionf(yAxis, angle); Quaternionf p = quaternionf(yAxis, angle);
Quaternionf q; Quaternionf q;
Vector3f vr {0.0, 0.0, 1.0}; Vector3F vr {0.0, 0.0, 1.0};
assert(p.isUnitQuaternion()); assert(p.isUnitQuaternion());
std::cerr << p.rotate(v) << std::endl; std::cerr << p.rotate(v) << std::endl;

2
src/test/Assert.cc
View File

@ -58,7 +58,7 @@ Assert(bool condition)
#else #else
std::stringstream msg; std::stringstream msg;
msg << "assertion failed at " << __FILE__ << ":" << __LINE__; msg << "assertion failed At " << __FILE__ << ":" << __LINE__;
throw AssertionFailed(msg.str()); throw AssertionFailed(msg.str());
#endif #endif
#else #else

12
test/coord2d.cc
View File

@ -90,6 +90,18 @@ geomConversionIdentities()
SCTEST_CHECK(point == points.at(i)); SCTEST_CHECK(point == points.at(i));
} }
Point2D point(3, 5);
Point2D point2;
Polar2D polar;
Polar2D polar2;
point.ToPolar(polar);
polar.ToPoint(point2);
SCTEST_CHECK_EQ(point, point2);
point2.ToPolar(polar2);
SCTEST_CHECK_EQ(polar, polar2);
return true; return true;
} }

38
test/madgwick.cc
View File

@ -20,8 +20,8 @@ using namespace scmp;
bool bool
SimpleAngularOrientationFloat() SimpleAngularOrientationFloat()
{ {
filter::Madgwickf mflt; filter::Madgwickf mflt;
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation. const geom::Vector3F gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation. const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper. const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0); const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -47,8 +47,8 @@ SimpleAngularOrientationFloat()
bool bool
SimpleAngularOrientationFloatDefaultDT() SimpleAngularOrientationFloatDefaultDT()
{ {
filter::Madgwickf mflt; filter::Madgwickf mflt;
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation. const geom::Vector3F gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation. const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper. const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0); const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -75,8 +75,8 @@ SimpleAngularOrientationFloatDefaultDT()
bool bool
VerifyUpdateWithZeroDeltaTFails() VerifyUpdateWithZeroDeltaTFails()
{ {
filter::Madgwickf mflt; filter::Madgwickf mflt;
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation. const geom::Vector3F gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation. const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0); const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -100,8 +100,8 @@ VerifyUpdateWithZeroDeltaTFails()
bool bool
SimpleAngularOrientationDouble() SimpleAngularOrientationDouble()
{ {
filter::Madgwickd mflt; filter::Madgwickd mflt;
const geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation. const geom::Vector3D gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation. const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const double delta = 0.00917; // assume 109 updates per second, as per the paper. const double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0); const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
@ -126,9 +126,9 @@ SimpleAngularOrientationDouble()
bool bool
SimpleAngularOrientation2InitialVector3f() SimpleAngularOrientation2InitialVector3f()
{ {
const geom::Vector3f initialFrame{0, 0, 0}; const geom::Vector3F initialFrame{0, 0, 0};
filter::Madgwickf mflt(initialFrame); filter::Madgwickf mflt(initialFrame);
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation. const geom::Vector3F gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation. const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper. const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0); const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -153,9 +153,9 @@ SimpleAngularOrientation2InitialVector3f()
bool bool
SimpleAngularOrientation2InitialQuaternionf() SimpleAngularOrientation2InitialQuaternionf()
{ {
const auto initialFrame = geom::quaternionf_from_euler({0, 0, 0}); const auto initialFrame = geom::QuaternionFromEuler({0, 0, 0});
filter::Madgwickf mflt(initialFrame); filter::Madgwickf mflt(initialFrame);
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation. const geom::Vector3F gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation. const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper. const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0); const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -180,9 +180,9 @@ SimpleAngularOrientation2InitialQuaternionf()
bool bool
SimpleAngularOrientation2InitialVector3d() SimpleAngularOrientation2InitialVector3d()
{ {
const geom::Vector3d initialFrame{0, 0, 0}; const geom::Vector3D initialFrame{0, 0, 0};
filter::Madgwickd mflt(initialFrame); filter::Madgwickd mflt(initialFrame);
const geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation. const geom::Vector3D gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation. const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const double delta = 0.00917; // assume 109 updates per second, as per the paper. const double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0); const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
@ -207,9 +207,9 @@ SimpleAngularOrientation2InitialVector3d()
bool bool
SimpleAngularOrientation2InitialQuaterniond() SimpleAngularOrientation2InitialQuaterniond()
{ {
const auto initialFrame = geom::quaterniond_from_euler({0, 0, 0}); const auto initialFrame = geom::QuaternionFromEuler({0, 0, 0});
filter::Madgwickd mflt(initialFrame); filter::Madgwickd mflt(initialFrame);
const geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation. const geom::Vector3D gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation. const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const double delta = 0.00917; // assume 109 updates per second, as per the paper. const double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0); const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
@ -266,9 +266,9 @@ main(int argc, char **argv)
SimpleAngularOrientation2InitialVector3f); SimpleAngularOrientation2InitialVector3f);
suite.AddTest("SimpleAngularOrientationDouble (inital vector3d)", suite.AddTest("SimpleAngularOrientationDouble (inital vector3d)",
SimpleAngularOrientation2InitialVector3d); SimpleAngularOrientation2InitialVector3d);
suite.AddTest("SimpleAngularOrientationFloat (inital quaternionf)", suite.AddTest("SimpleAngularOrientationFloat (inital MakeQuaternion)",
SimpleAngularOrientation2InitialQuaternionf); SimpleAngularOrientation2InitialQuaternionf);
suite.AddTest("SimpleAngularOrientationDouble (inital quaterniond)", suite.AddTest("SimpleAngularOrientationDouble (inital MakeQuaternion)",
SimpleAngularOrientation2InitialQuaterniond); SimpleAngularOrientation2InitialQuaterniond);
delete flags; delete flags;

10
test/orientation.cc
View File

@ -45,9 +45,9 @@ UnitConversions_RadiansToDegreesD()
bool bool
Orientation2f_Heading() Orientation2f_Heading()
{ {
geom::Vector2f a{2.0, 2.0}; geom::Vector2F a{2.0, 2.0};
SCTEST_CHECK_FEQ(geom::Heading2f(a), scmp::DegreesToRadiansF(45)); SCTEST_CHECK_FEQ(geom::Heading2F(a), scmp::DegreesToRadiansF(45));
return true; return true;
} }
@ -56,7 +56,7 @@ Orientation2f_Heading()
bool bool
Orientation3f_Heading() Orientation3f_Heading()
{ {
geom::Vector3f a{2.0, 2.0, 2.0}; geom::Vector3F a{2.0, 2.0, 2.0};
SCTEST_CHECK_FEQ(geom::Heading3f(a), scmp::DegreesToRadiansF(45)); SCTEST_CHECK_FEQ(geom::Heading3f(a), scmp::DegreesToRadiansF(45));
@ -67,7 +67,7 @@ Orientation3f_Heading()
bool bool
Orientation2d_Heading() Orientation2d_Heading()
{ {
geom::Vector2d a{2.0, 2.0}; geom::Vector2D a{2.0, 2.0};
return scmp::WithinTolerance(geom::Heading2d(a), scmp::DegreesToRadiansD(45), 0.000001); return scmp::WithinTolerance(geom::Heading2d(a), scmp::DegreesToRadiansD(45), 0.000001);
} }
@ -76,7 +76,7 @@ Orientation2d_Heading()
bool bool
Orientation3d_Heading() Orientation3d_Heading()
{ {
geom::Vector3d a{2.0, 2.0, 2.0}; geom::Vector3D a{2.0, 2.0, 2.0};
return scmp::WithinTolerance(geom::Heading3d(a), scmp::DegreesToRadiansD(45), 0.000001); return scmp::WithinTolerance(geom::Heading3d(a), scmp::DegreesToRadiansD(45), 0.000001);
} }

126
test/quaternion.cc
View File

@ -15,7 +15,7 @@ using namespace sctest;
static bool static bool
Quaternion_SelfTest() Quaternion_SelfTest()
{ {
geom::Quaternion_SelfTest(); geom::QuaternionSelfTest();
return true; return true;
} }
@ -23,9 +23,9 @@ Quaternion_SelfTest()
static bool static bool
Quaterniond_Addition() Quaterniond_Addition()
{ {
geom::Quaterniond p(geom::Vector4d {3.0, 1.0, -2.0, 1.0}); geom::Quaterniond p(geom::Vector4D {3.0, 1.0, -2.0, 1.0});
geom::Quaterniond q(geom::Vector4d {2.0, -1.0, 2.0, 3.0}); geom::Quaterniond q(geom::Vector4D {2.0, -1.0, 2.0, 3.0});
geom::Quaterniond expected(geom::Vector4d{5.0, 0.0, 0.0, 4.0}); geom::Quaterniond expected(geom::Vector4D{5.0, 0.0, 0.0, 4.0});
SCTEST_CHECK_EQ(p + q, expected); SCTEST_CHECK_EQ(p + q, expected);
SCTEST_CHECK_EQ(expected - q, p); SCTEST_CHECK_EQ(expected - q, p);
@ -41,7 +41,7 @@ Quaterniond_Conjugate()
geom::Quaterniond p {2.0, 3.0, 4.0, 5.0}; geom::Quaterniond p {2.0, 3.0, 4.0, 5.0};
geom::Quaterniond q {2.0, -3.0, -4.0, -5.0}; geom::Quaterniond q {2.0, -3.0, -4.0, -5.0};
SCTEST_CHECK_EQ(p.conjugate(), q); SCTEST_CHECK_EQ(p.Conjugate(), q);
return true; return true;
} }
@ -49,8 +49,9 @@ Quaterniond_Conjugate()
static bool static bool
Quaterniond_Euler() Quaterniond_Euler()
{ {
geom::Quaterniond p = geom::quaterniond(geom::Vector3d{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI/4.0); geom::Quaterniond p = geom::MakeQuaternion(
geom::Quaterniond q = geom::quaterniond_from_euler(p.euler()); geom::Vector3D{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI / 4.0);
geom::Quaterniond q = geom::QuaternionFromEuler(p.Euler());
SCTEST_CHECK_EQ(p, q); SCTEST_CHECK_EQ(p, q);
@ -64,7 +65,7 @@ Quaterniond_Identity()
geom::Quaterniond p {3.0, 1.0, -2.0, 1.0}; geom::Quaterniond p {3.0, 1.0, -2.0, 1.0};
geom::Quaterniond q; geom::Quaterniond q;
SCTEST_CHECK(q.isIdentity()); SCTEST_CHECK(q.IsIdentity());
SCTEST_CHECK_EQ(p * q, p); SCTEST_CHECK_EQ(p * q, p);
return true; return true;
@ -77,7 +78,7 @@ Quaterniond_Inverse()
geom::Quaterniond p {2.0, 3.0, 4.0, 5.0}; geom::Quaterniond p {2.0, 3.0, 4.0, 5.0};
geom::Quaterniond q {0.03704, -0.05556, -0.07407, -0.09259}; geom::Quaterniond q {0.03704, -0.05556, -0.07407, -0.09259};
SCTEST_CHECK_EQ(p.inverse(), q); SCTEST_CHECK_EQ(p.Inverse(), q);
return true; return true;
} }
@ -89,7 +90,7 @@ Quaterniond_Norm()
geom::Quaterniond p {5.563199889674063, 0.9899139811480784, 9.387110042325054, 6.161341707794767}; geom::Quaterniond p {5.563199889674063, 0.9899139811480784, 9.387110042325054, 6.161341707794767};
double norm = 12.57016663729933; double norm = 12.57016663729933;
SCTEST_CHECK_DEQ(p.norm(), norm); SCTEST_CHECK_DEQ(p.Norm(), norm);
return true; return true;
} }
@ -111,27 +112,27 @@ Quaterniond_Product()
static bool static bool
Quaterniond_Rotate() Quaterniond_Rotate()
{ {
// This test aims to rotate a vector v using a quaternion. // This test aims to Rotate a vector v using a MakeQuaternion.
// c.f. https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion // c.f. https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion
// If we assume a standard IMU frame of reference following the // If we assume a standard IMU frame of reference following the
// right hand rule: // right hand rule:
// + The x axis points toward magnetic north // + The x Axis points toward magnetic north
// + The y axis points toward magnentic west // + The y Axis points toward magnentic west
// + The z axis points toward the sky // + The z Axis points toward the sky
// Given a vector pointing due north, rotating by 90º about // Given a vector pointing due north, rotating by 90º about
// the y-axis should leave us pointing toward the sky. // the y-Axis should leave us pointing toward the sky.
geom::Vector3d v {1.0, 0.0, 0.0}; // a vector pointed north geom::Vector3D v {1.0, 0.0, 0.0}; // a vector pointed north
geom::Vector3d yAxis {0.0, 1.0, 0.0}; // a vector representing the y axis. geom::Vector3D yAxis {0.0, 1.0, 0.0}; // a vector representing the y Axis.
double angle = M_PI / 2; // 90º rotation double angle = M_PI / 2; // 90º rotation
// A quaternion representing a 90º rotation about the y axis. // A MakeQuaternion representing a 90º rotation about the y Axis.
geom::Quaterniond p = geom::quaterniond(yAxis, angle); geom::Quaterniond p = geom::MakeQuaternion(yAxis, angle);
geom::Vector3d vr {0.0, 0.0, 1.0}; // expected rotated vector. geom::Vector3D vr {0.0, 0.0, 1.0}; // expected rotated vector.
// A rotation quaternion should be a unit quaternion. // A rotation quaternion should be a unit MakeQuaternion.
SCTEST_CHECK(p.isUnitQuaternion()); SCTEST_CHECK(p.IsUnitQuaternion());
SCTEST_CHECK_EQ(p.rotate(v), vr); SCTEST_CHECK_EQ(p.Rotate(v), vr);
return true; return true;
} }
@ -141,13 +142,13 @@ static bool
Quaterniond_ShortestSLERP() Quaterniond_ShortestSLERP()
{ {
// Our starting point is an Orientation that is yawed 45° - our // Our starting point is an Orientation that is yawed 45° - our
// Orientation is pointed π/4 radians in the X axis. // Orientation is pointed π/4 radians in the X Axis.
geom::Quaterniond p {0.92388, 0.382683, 0, 0}; geom::Quaterniond p {0.92388, 0.382683, 0, 0};
// Our ending point is an Orientation that is yawed -45° - or // Our ending point is an Orientation that is yawed -45° - or
// pointed -π/4 radians in the X axis. // pointed -π/4 radians in the X Axis.
geom::Quaterniond q {0.92388, -0.382683, 0, 0}; geom::Quaterniond q {0.92388, -0.382683, 0, 0};
// The halfway point should be oriented midway about the X axis. It turns // The halfway point should be oriented midway about the X Axis. It turns
// out this is an identity quaternion. // out this is an identity MakeQuaternion.
geom::Quaterniond r; geom::Quaterniond r;
SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, 0.0), p); SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, 0.0), p);
@ -185,7 +186,7 @@ Quaterniond_Unit()
{ {
geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258}; geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
SCTEST_CHECK(q.isUnitQuaternion()); SCTEST_CHECK(q.IsUnitQuaternion());
return true; return true;
} }
@ -194,9 +195,9 @@ Quaterniond_Unit()
static bool static bool
Quaterniond_UtilityCreator() Quaterniond_UtilityCreator()
{ {
geom::Vector3d v {1.0, 1.0, 1.0}; geom::Vector3D v {1.0, 1.0, 1.0};
double w = M_PI; double w = M_PI;
geom::Quaterniond p = geom::quaterniond(v, w); geom::Quaterniond p = geom::MakeQuaternion(v, w);
geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258}; geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
SCTEST_CHECK_EQ(p, q); SCTEST_CHECK_EQ(p, q);
@ -226,7 +227,7 @@ Quaternionf_Conjugate()
geom::Quaternionf p {2.0, 3.0, 4.0, 5.0}; geom::Quaternionf p {2.0, 3.0, 4.0, 5.0};
geom::Quaternionf q {2.0, -3.0, -4.0, -5.0}; geom::Quaternionf q {2.0, -3.0, -4.0, -5.0};
SCTEST_CHECK_EQ(p.conjugate(), q); SCTEST_CHECK_EQ(p.Conjugate(), q);
return true; return true;
} }
@ -235,8 +236,9 @@ Quaternionf_Conjugate()
static bool static bool
Quaternionf_Euler() Quaternionf_Euler()
{ {
geom::Quaternionf p = geom::quaternionf(geom::Vector3f{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI/4.0); geom::Quaternionf p = geom::MakeQuaternion(
geom::Quaternionf q = geom::quaternionf_from_euler(p.euler()); geom::Vector3F{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI / 4.0);
geom::Quaternionf q = geom::QuaternionFromEuler(p.Euler());
SCTEST_CHECK_EQ(p, q); SCTEST_CHECK_EQ(p, q);
@ -262,7 +264,7 @@ Quaternionf_Inverse()
geom::Quaternionf p {2.0, 3.0, 4.0, 5.0}; geom::Quaternionf p {2.0, 3.0, 4.0, 5.0};
geom::Quaternionf q {0.03704, -0.05556, -0.07407, -0.09259}; geom::Quaternionf q {0.03704, -0.05556, -0.07407, -0.09259};
SCTEST_CHECK_EQ(p.inverse(), q); SCTEST_CHECK_EQ(p.Inverse(), q);
return true; return true;
} }
@ -274,7 +276,7 @@ Quaternionf_Norm()
geom::Quaternionf p {0.9899139811480784, 9.387110042325054, 6.161341707794767, 5.563199889674063}; geom::Quaternionf p {0.9899139811480784, 9.387110042325054, 6.161341707794767, 5.563199889674063};
float norm = 12.57016663729933; float norm = 12.57016663729933;
SCTEST_CHECK_FEQ(p.norm(), norm); SCTEST_CHECK_FEQ(p.Norm(), norm);
return true; return true;
} }
@ -296,15 +298,15 @@ Quaternionf_Product()
static bool static bool
Quaternionf_Rotate() Quaternionf_Rotate()
{ {
geom::Vector3f v {1.0, 0.0, 0.0}; geom::Vector3F v {1.0, 0.0, 0.0};
geom::Vector3f yAxis {0.0, 1.0, 0.0}; geom::Vector3F yAxis {0.0, 1.0, 0.0};
float angle = M_PI / 2; float angle = M_PI / 2;
geom::Quaternionf p = geom::quaternionf(yAxis, angle); geom::Quaternionf p = geom::MakeQuaternion(yAxis, angle);
geom::Vector3f vr {0.0, 0.0, 1.0}; geom::Vector3F vr {0.0, 0.0, 1.0};
SCTEST_CHECK(p.isUnitQuaternion()); SCTEST_CHECK(p.IsUnitQuaternion());
SCTEST_CHECK_EQ(p.rotate(v), vr); SCTEST_CHECK_EQ(p.Rotate(v), vr);
return true; return true;
} }
@ -314,13 +316,13 @@ static bool
Quaternionf_ShortestSLERP() Quaternionf_ShortestSLERP()
{ {
// Our starting point is an Orientation that is yawed 45° - our // Our starting point is an Orientation that is yawed 45° - our
// Orientation is pointed π/4 radians in the X axis. // Orientation is pointed π/4 radians in the X Axis.
geom::Quaternionf p {0.92388, 0.382683, 0, 0}; geom::Quaternionf p {0.92388, 0.382683, 0, 0};
// Our ending point is an Orientation that is yawed -45° - or // Our ending point is an Orientation that is yawed -45° - or
// pointed -π/4 radians in the X axis. // pointed -π/4 radians in the X Axis.
geom::Quaternionf q {0.92388, -0.382683, 0, 0}; geom::Quaternionf q {0.92388, -0.382683, 0, 0};
// The halfway point should be oriented midway about the X axis. It turns // The halfway point should be oriented midway about the X Axis. It turns
// out this is an identity quaternion. // out this is an identity MakeQuaternion.
geom::Quaternionf r; geom::Quaternionf r;
SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, (float)0.0), p); SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, (float)0.0), p);
@ -358,7 +360,7 @@ Quaternionf_Unit()
{ {
geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258}; geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
SCTEST_CHECK(q.isUnitQuaternion()); SCTEST_CHECK(q.IsUnitQuaternion());
return true; return true;
} }
@ -367,9 +369,9 @@ Quaternionf_Unit()
static bool static bool
Quaternionf_UtilityCreator() Quaternionf_UtilityCreator()
{ {
geom::Vector3f v {1.0, 1.0, 1.0}; geom::Vector3F v {1.0, 1.0, 1.0};
float w = M_PI; float w = M_PI;
geom::Quaternionf p = geom::quaternionf(v, w); geom::Quaternionf p = geom::MakeQuaternion(v, w);
geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258}; geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
SCTEST_CHECK_EQ(p, q); SCTEST_CHECK_EQ(p, q);
@ -381,15 +383,15 @@ Quaternionf_UtilityCreator()
static bool static bool
QuaternionMiscellaneous_SanityChecks() QuaternionMiscellaneous_SanityChecks()
{ {
geom::Vector4d q {4.0, 1.0, 2.0, 3.0}; geom::Vector4D q {4.0, 1.0, 2.0, 3.0};
geom::Vector3d v {1.0, 2.0, 3.0}; geom::Vector3D v {1.0, 2.0, 3.0};
double w = 4.0; double w = 4.0;
geom::Quaterniond p(q); geom::Quaterniond p(q);
geom::Quaterniond u = p.unitQuaternion(); geom::Quaterniond u = p.UnitQuaternion();
SCTEST_CHECK_EQ(p.axis(), v); SCTEST_CHECK_EQ(p.Axis(), v);
SCTEST_CHECK_DEQ(p.angle(), w); SCTEST_CHECK_DEQ(p.Angle(), w);
SCTEST_CHECK(u.isUnitQuaternion()); SCTEST_CHECK(u.IsUnitQuaternion());
return true; return true;
} }
@ -417,10 +419,10 @@ static bool
QuaternionMiscellanous_InitializerConstructor() QuaternionMiscellanous_InitializerConstructor()
{ {
geom::Quaternionf p {1.0, 1.0, 1.0, 1.0}; geom::Quaternionf p {1.0, 1.0, 1.0, 1.0};
geom::Quaternionf q(geom::Vector4f {1.0, 1.0, 1.0, 1.0}); geom::Quaternionf q(geom::Vector4F {1.0, 1.0, 1.0, 1.0});
SCTEST_CHECK_EQ(p, q); SCTEST_CHECK_EQ(p, q);
SCTEST_CHECK_FEQ(p.norm(), (float)2.0); SCTEST_CHECK_FEQ(p.Norm(), (float)2.0);
return true; return true;
} }
@ -449,7 +451,7 @@ main(int argc, char *argv[])
suite.Silence(); suite.Silence();
} }
suite.AddTest("Quaternion_SelfTest", Quaternion_SelfTest); suite.AddTest("QuaternionSelfTest", Quaternion_SelfTest);
suite.AddTest("QuaternionMiscellanous_InitializerConstructor", suite.AddTest("QuaternionMiscellanous_InitializerConstructor",
QuaternionMiscellanous_InitializerConstructor); QuaternionMiscellanous_InitializerConstructor);
suite.AddTest("QuaternionMiscellaneous_SanityChecks", suite.AddTest("QuaternionMiscellaneous_SanityChecks",

2
test/tlv.cc
View File

@ -40,7 +40,7 @@ tlvTestSuite(Arena &backend)
sctest::Assert(cursor != nullptr); sctest::Assert(cursor != nullptr);
cursor = nullptr; cursor = nullptr;
// the cursor should point at the next record, // the cursor should point At the next record,
// and rec4 should contain the same data as rec1. // and rec4 should contain the same data as rec1.
std::cout << "\tFindTag 1" << "\n"; std::cout << "\tFindTag 1" << "\n";
cursor = TLV::FindTag(backend, cursor, rec4); cursor = TLV::FindTag(backend, cursor, rec4);

248
test/vector.cc
View File

@ -11,7 +11,7 @@
// //
// Licensed under the Apache License, Version 2.0 (the "License"); // Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License. // you may not use this file except in compliance with the License.
// You may obtain a copy of the License at // You may obtain a copy of the License At
// //
// http://www.apache.org/licenses/LICENSE-2.0 // http://www.apache.org/licenses/LICENSE-2.0
// //
@ -37,8 +37,8 @@ using namespace std;
static bool static bool
Vector3Miscellaneous_ExtractionOperator3d() Vector3Miscellaneous_ExtractionOperator3d()
{ {
geom::Vector3d vec {1.0, 2.0, 3.0}; geom::Vector3D vec {1.0, 2.0, 3.0};
stringstream vecBuffer; stringstream vecBuffer;
vecBuffer << vec; vecBuffer << vec;
SCTEST_CHECK_EQ(vecBuffer.str(), "<1, 2, 3>"); SCTEST_CHECK_EQ(vecBuffer.str(), "<1, 2, 3>");
@ -49,8 +49,8 @@ Vector3Miscellaneous_ExtractionOperator3d()
static bool static bool
Vector3Miscellaneous_ExtractionOperator3f() Vector3Miscellaneous_ExtractionOperator3f()
{ {
geom::Vector3f vec {1.0, 2.0, 3.0}; geom::Vector3F vec {1.0, 2.0, 3.0};
stringstream vecBuffer; stringstream vecBuffer;
vecBuffer << vec; vecBuffer << vec;
SCTEST_CHECK_EQ(vecBuffer.str(), "<1, 2, 3>"); SCTEST_CHECK_EQ(vecBuffer.str(), "<1, 2, 3>");
@ -60,10 +60,10 @@ Vector3Miscellaneous_ExtractionOperator3f()
static bool static bool
Vector3Miscellaneous_SetEpsilon() { Vector3Miscellaneous_SetEpsilon() {
geom::Vector3f a {1.0, 1.0, 1.0}; geom::Vector3F a {1.0, 1.0, 1.0};
geom::Vector3f b; geom::Vector3F b;
a.setEpsilon(1.1); a.SetEpsilon(1.1);
SCTEST_CHECK_EQ(a, b); SCTEST_CHECK_EQ(a, b);
return true; return true;
} }
@ -72,10 +72,10 @@ Vector3Miscellaneous_SetEpsilon() {
static bool static bool
Vector3FloatTests_Magnitude() Vector3FloatTests_Magnitude()
{ {
geom::Vector3f v3f {1.0, -2.0, 3.0}; geom::Vector3F v3f {1.0, -2.0, 3.0};
const float expected = 3.74165738677394; const float expected = 3.74165738677394;
SCTEST_CHECK_FEQ(v3f.magnitude(), expected); SCTEST_CHECK_FEQ(v3f.Magnitude(), expected);
return true; return true;
} }
@ -84,9 +84,9 @@ Vector3FloatTests_Magnitude()
static bool static bool
Vector3FloatTests_Equality() Vector3FloatTests_Equality()
{ {
geom::Vector3f a {1.0, 2.0, 3.0}; geom::Vector3F a {1.0, 2.0, 3.0};
geom::Vector3f b {1.0, 2.0, 3.0}; geom::Vector3F b {1.0, 2.0, 3.0};
geom::Vector3f c {1.0, 2.0, 1.0}; geom::Vector3F c {1.0, 2.0, 1.0};
SCTEST_CHECK_EQ(a, b); SCTEST_CHECK_EQ(a, b);
SCTEST_CHECK_EQ(b, a); SCTEST_CHECK_EQ(b, a);
@ -100,9 +100,9 @@ Vector3FloatTests_Equality()
static bool static bool
Vector3FloatTests_Addition() Vector3FloatTests_Addition()
{ {
geom::Vector3f a {1.0, 2.0, 3.0}; geom::Vector3F a {1.0, 2.0, 3.0};
geom::Vector3f b {4.0, 5.0, 6.0}; geom::Vector3F b {4.0, 5.0, 6.0};
geom::Vector3f expected {5.0, 7.0, 9.0}; geom::Vector3F expected {5.0, 7.0, 9.0};
SCTEST_CHECK_EQ(a+b, expected); SCTEST_CHECK_EQ(a+b, expected);
@ -113,9 +113,9 @@ Vector3FloatTests_Addition()
static bool static bool
Vector3FloatTests_Subtraction() Vector3FloatTests_Subtraction()
{ {
geom::Vector3f a {1.0, 2.0, 3.0}; geom::Vector3F a {1.0, 2.0, 3.0};
geom::Vector3f b {4.0, 5.0, 6.0}; geom::Vector3F b {4.0, 5.0, 6.0};
geom::Vector3f c {5.0, 7.0, 9.0}; geom::Vector3F c {5.0, 7.0, 9.0};
SCTEST_CHECK_EQ(c-b, a); SCTEST_CHECK_EQ(c-b, a);
@ -126,8 +126,8 @@ Vector3FloatTests_Subtraction()
static bool static bool
Vector3FloatTests_ScalarMultiplication() Vector3FloatTests_ScalarMultiplication()
{ {
geom::Vector3f a {1.0, 2.0, 3.0}; geom::Vector3F a {1.0, 2.0, 3.0};
geom::Vector3f expected {3.0, 6.0, 9.0}; geom::Vector3F expected {3.0, 6.0, 9.0};
SCTEST_CHECK_EQ(a * 3.0, expected); SCTEST_CHECK_EQ(a * 3.0, expected);
@ -138,8 +138,8 @@ Vector3FloatTests_ScalarMultiplication()
static bool static bool
Vector3FloatTests_ScalarDivision() Vector3FloatTests_ScalarDivision()
{ {
geom::Vector3f a {1.0, 2.0, 3.0}; geom::Vector3F a {1.0, 2.0, 3.0};
geom::Vector3f b {3.0, 6.0, 9.0}; geom::Vector3F b {3.0, 6.0, 9.0};
SCTEST_CHECK_EQ(b / 3.0, a); SCTEST_CHECK_EQ(b / 3.0, a);
@ -150,8 +150,8 @@ Vector3FloatTests_ScalarDivision()
static bool static bool
Vector3FloatTests_DotProduct() Vector3FloatTests_DotProduct()
{ {
geom::Vector3f a {1.0, 2.0, 3.0}; geom::Vector3F a {1.0, 2.0, 3.0};
geom::Vector3f b {4.0, 5.0, 6.0}; geom::Vector3F b {4.0, 5.0, 6.0};
SCTEST_CHECK_FEQ(a * b, (float)32.0); SCTEST_CHECK_FEQ(a * b, (float)32.0);
@ -162,14 +162,14 @@ static bool
Vector3FloatTests_UnitVector() Vector3FloatTests_UnitVector()
{ {
// Test values randomly generated and calculated with numpy. // Test values randomly generated and calculated with numpy.
geom::Vector3f vec3 {5.320264018493507, 5.6541812891273935, 1.9233435162644652}; geom::Vector3F vec3 {5.320264018493507, 5.6541812891273935, 1.9233435162644652};
geom::Vector3f unit {0.6651669556972103, 0.7069150218815566, 0.24046636539587804}; geom::Vector3F unit {0.6651669556972103, 0.7069150218815566, 0.24046636539587804};
geom::Vector3f unit2; geom::Vector3F unit2;
SCTEST_CHECK_EQ(vec3.unitVector(), unit); SCTEST_CHECK_EQ(vec3.UnitVector(), unit);
SCTEST_CHECK_FALSE(vec3.isUnitVector()); SCTEST_CHECK_FALSE(vec3.IsUnitVector());
SCTEST_CHECK(unit.isUnitVector()); SCTEST_CHECK(unit.IsUnitVector());
SCTEST_CHECK(unit2.isUnitVector()); SCTEST_CHECK(unit2.IsUnitVector());
return true; return true;
} }
@ -177,13 +177,13 @@ Vector3FloatTests_UnitVector()
static bool static bool
Vector3FloatTests_Angle() Vector3FloatTests_Angle()
{ {
geom::Vector3f a {0.3977933061361172, 8.053980094436525, 8.1287759943773}; geom::Vector3F a {0.3977933061361172, 8.053980094436525, 8.1287759943773};
geom::Vector3f b {9.817895298608196, 4.034166890407462, 4.37628316513266}; geom::Vector3F b {9.817895298608196, 4.034166890407462, 4.37628316513266};
geom::Vector3f c {7.35, 0.221, 5.188}; geom::Vector3F c {7.35, 0.221, 5.188};
geom::Vector3f d {2.751, 8.259, 3.985}; geom::Vector3F d {2.751, 8.259, 3.985};
SCTEST_CHECK_FEQ(a.angle(b), (float)0.9914540426033251); SCTEST_CHECK_FEQ(a.Angle(b), (float)0.9914540426033251);
if (!scmp::WithinTolerance(c.angle(d), (float)1.052, (float)0.001)) { if (!scmp::WithinTolerance(c.Angle(d), (float)1.052, (float)0.001)) {
return false; return false;
} }
@ -194,26 +194,26 @@ Vector3FloatTests_Angle()
static bool static bool
Vector3FloatTests_ParallelOrthogonalVectors() Vector3FloatTests_ParallelOrthogonalVectors()
{ {
geom::Vector3f a {-2.029, 9.97, 4.172}; geom::Vector3F a {-2.029, 9.97, 4.172};
geom::Vector3f b {-9.231, -6.639, -7.245}; geom::Vector3F b {-9.231, -6.639, -7.245};
geom::Vector3f c {-2.328, -7.284, -1.214}; geom::Vector3F c {-2.328, -7.284, -1.214};
geom::Vector3f d {-1.821, 1.072, -2.94}; geom::Vector3F d {-1.821, 1.072, -2.94};
geom::Vector3f e {-2.0, 1.0, 3.0}; geom::Vector3F e {-2.0, 1.0, 3.0};
geom::Vector3f f {-6.0, 3.0, 9.0}; geom::Vector3F f {-6.0, 3.0, 9.0};
geom::Vector3f zeroVector {0.0, 0.0, 0.0}; geom::Vector3F zeroVector {0.0, 0.0, 0.0};
SCTEST_CHECK_FALSE(a.isParallel(b)); SCTEST_CHECK_FALSE(a.IsParallel(b));
SCTEST_CHECK_FALSE(a.isOrthogonal(b)); SCTEST_CHECK_FALSE(a.IsOrthogonal(b));
SCTEST_CHECK_FALSE(c.isParallel(d)); SCTEST_CHECK_FALSE(c.IsParallel(d));
SCTEST_CHECK(c.isOrthogonal(d)); SCTEST_CHECK(c.IsOrthogonal(d));
SCTEST_CHECK(e.isParallel(f)); SCTEST_CHECK(e.IsParallel(f));
SCTEST_CHECK_FALSE(e.isOrthogonal(f)); SCTEST_CHECK_FALSE(e.IsOrthogonal(f));
SCTEST_CHECK(zeroVector.isZero()); SCTEST_CHECK(zeroVector.IsZero());
SCTEST_CHECK(c.isParallel(zeroVector)); SCTEST_CHECK(c.IsParallel(zeroVector));
SCTEST_CHECK(c.isOrthogonal(zeroVector)); SCTEST_CHECK(c.IsOrthogonal(zeroVector));
return true; return true;
} }
@ -222,13 +222,13 @@ Vector3FloatTests_ParallelOrthogonalVectors()
static bool static bool
Vector3FloatTests_Projections() Vector3FloatTests_Projections()
{ {
geom::Vector3f a {4.866769214609107, 6.2356222686140566, 9.140878417029711}; geom::Vector3F a {4.866769214609107, 6.2356222686140566, 9.140878417029711};
geom::Vector3f b {6.135533104801077, 8.757851406697895, 0.6738031370548048}; geom::Vector3F b {6.135533104801077, 8.757851406697895, 0.6738031370548048};
geom::Vector3f c {4.843812341655318, 6.9140509888133055, 0.5319465962229454}; geom::Vector3F c {4.843812341655318, 6.9140509888133055, 0.5319465962229454};
geom::Vector3f d {0.02295687295378901, -0.6784287201992489, 8.608931820806765}; geom::Vector3F d {0.02295687295378901, -0.6784287201992489, 8.608931820806765};
SCTEST_CHECK_EQ(a.projectParallel(b), c); SCTEST_CHECK_EQ(a.ProjectParallel(b), c);
SCTEST_CHECK_EQ(a.projectOrthogonal(b), d); SCTEST_CHECK_EQ(a.ProjectOrthogonal(b), d);
return true; return true;
} }
@ -237,12 +237,12 @@ Vector3FloatTests_Projections()
static bool static bool
Vector3FloatTests_CrossProduct() Vector3FloatTests_CrossProduct()
{ {
geom::Vector3f a {8.462, 7.893, -8.187}; geom::Vector3F a {8.462, 7.893, -8.187};
geom::Vector3f b {6.984, -5.975, 4.778}; geom::Vector3F b {6.984, -5.975, 4.778};
geom::Vector3f c {-11.2046, -97.6094, -105.685}; geom::Vector3F c {-11.2046, -97.6094, -105.685};
c.setEpsilon(0.001); c.SetEpsilon(0.001);
SCTEST_CHECK_EQ(c, a.cross(b)); SCTEST_CHECK_EQ(c, a.Cross(b));
return true; return true;
} }
@ -251,10 +251,10 @@ Vector3FloatTests_CrossProduct()
static bool static bool
Vector3DoubleTests_Magnitude() Vector3DoubleTests_Magnitude()
{ {
geom::Vector3d v3d{1.0, -2.0, 3.0}; geom::Vector3D v3d{1.0, -2.0, 3.0};
const double expected = 3.74165738677394; const double expected = 3.74165738677394;
SCTEST_CHECK_DEQ(v3d.magnitude(), expected); SCTEST_CHECK_DEQ(v3d.Magnitude(), expected);
return true; return true;
} }
@ -262,9 +262,9 @@ Vector3DoubleTests_Magnitude()
static bool static bool
Vector3DoubleTests_Equality() Vector3DoubleTests_Equality()
{ {
geom::Vector3d a {1.0, 2.0, 3.0}; geom::Vector3D a {1.0, 2.0, 3.0};
geom::Vector3d b {1.0, 2.0, 3.0}; geom::Vector3D b {1.0, 2.0, 3.0};
geom::Vector3d c {1.0, 2.0, 1.0}; geom::Vector3D c {1.0, 2.0, 1.0};
SCTEST_CHECK_EQ(a, b); SCTEST_CHECK_EQ(a, b);
SCTEST_CHECK_EQ(b, a); SCTEST_CHECK_EQ(b, a);
@ -278,9 +278,9 @@ Vector3DoubleTests_Equality()
static bool static bool
Vector3DoubleTests_Addition() Vector3DoubleTests_Addition()
{ {
geom::Vector3d a {1.0, 2.0, 3.0}; geom::Vector3D a {1.0, 2.0, 3.0};
geom::Vector3d b {4.0, 5.0, 6.0}; geom::Vector3D b {4.0, 5.0, 6.0};
geom::Vector3d expected {5.0, 7.0, 9.0}; geom::Vector3D expected {5.0, 7.0, 9.0};
SCTEST_CHECK_EQ(a+b, expected); SCTEST_CHECK_EQ(a+b, expected);
@ -291,9 +291,9 @@ Vector3DoubleTests_Addition()
static bool static bool
Vector3DoubleTests_Subtraction() Vector3DoubleTests_Subtraction()
{ {
geom::Vector3d a {1.0, 2.0, 3.0}; geom::Vector3D a {1.0, 2.0, 3.0};
geom::Vector3d b {4.0, 5.0, 6.0}; geom::Vector3D b {4.0, 5.0, 6.0};
geom::Vector3d c {5.0, 7.0, 9.0}; geom::Vector3D c {5.0, 7.0, 9.0};
SCTEST_CHECK_EQ(c-b, a); SCTEST_CHECK_EQ(c-b, a);
@ -304,8 +304,8 @@ Vector3DoubleTests_Subtraction()
static bool static bool
Vector3DoubleTests_ScalarMultiplication() Vector3DoubleTests_ScalarMultiplication()
{ {
geom::Vector3d a {1.0, 2.0, 3.0}; geom::Vector3D a {1.0, 2.0, 3.0};
geom::Vector3d expected {3.0, 6.0, 9.0}; geom::Vector3D expected {3.0, 6.0, 9.0};
SCTEST_CHECK_EQ(a * 3.0, expected); SCTEST_CHECK_EQ(a * 3.0, expected);
@ -316,8 +316,8 @@ Vector3DoubleTests_ScalarMultiplication()
static bool static bool
Vector3DoubleTests_ScalarDivision() Vector3DoubleTests_ScalarDivision()
{ {
geom::Vector3d a {1.0, 2.0, 3.0}; geom::Vector3D a {1.0, 2.0, 3.0};
geom::Vector3d b {3.0, 6.0, 9.0}; geom::Vector3D b {3.0, 6.0, 9.0};
SCTEST_CHECK_EQ(b / 3.0, a); SCTEST_CHECK_EQ(b / 3.0, a);
@ -328,8 +328,8 @@ Vector3DoubleTests_ScalarDivision()
static bool static bool
Vector3DoubleTests_DotProduct() Vector3DoubleTests_DotProduct()
{ {
geom::Vector3d a {1.0, 2.0, 3.0}; geom::Vector3D a {1.0, 2.0, 3.0};
geom::Vector3d b {4.0, 5.0, 6.0}; geom::Vector3D b {4.0, 5.0, 6.0};
SCTEST_CHECK_DEQ(a * b, 32.0); SCTEST_CHECK_DEQ(a * b, 32.0);
@ -341,14 +341,14 @@ static bool
Vector3DoubleTests_UnitVector() Vector3DoubleTests_UnitVector()
{ {
// Test values randomly generated and calculated with numpy. // Test values randomly generated and calculated with numpy.
geom::Vector3d vec3 {5.320264018493507, 5.6541812891273935, 1.9233435162644652}; geom::Vector3D vec3 {5.320264018493507, 5.6541812891273935, 1.9233435162644652};
geom::Vector3d unit {0.6651669556972103, 0.7069150218815566, 0.24046636539587804}; geom::Vector3D unit {0.6651669556972103, 0.7069150218815566, 0.24046636539587804};
geom::Vector3d unit2; geom::Vector3D unit2;
SCTEST_CHECK_EQ(vec3.unitVector(), unit); SCTEST_CHECK_EQ(vec3.UnitVector(), unit);
SCTEST_CHECK_FALSE(vec3.isUnitVector()); SCTEST_CHECK_FALSE(vec3.IsUnitVector());
SCTEST_CHECK(unit.isUnitVector()); SCTEST_CHECK(unit.IsUnitVector());
SCTEST_CHECK(unit2.isUnitVector()); SCTEST_CHECK(unit2.IsUnitVector());
return true; return true;
} }
@ -357,13 +357,13 @@ Vector3DoubleTests_UnitVector()
static bool static bool
Vector3DoubleTests_Angle() Vector3DoubleTests_Angle()
{ {
geom::Vector3d a {0.3977933061361172, 8.053980094436525, 8.1287759943773}; geom::Vector3D a {0.3977933061361172, 8.053980094436525, 8.1287759943773};
geom::Vector3d b {9.817895298608196, 4.034166890407462, 4.37628316513266}; geom::Vector3D b {9.817895298608196, 4.034166890407462, 4.37628316513266};
geom::Vector3d c {7.35, 0.221, 5.188}; geom::Vector3D c {7.35, 0.221, 5.188};
geom::Vector3d d {2.751, 8.259, 3.985}; geom::Vector3D d {2.751, 8.259, 3.985};
SCTEST_CHECK_DEQ(a.angle(b), 0.9914540426033251); SCTEST_CHECK_DEQ(a.Angle(b), 0.9914540426033251);
if (!scmp::WithinTolerance(c.angle(d), (double)1.052, (double)0.001)) { if (!scmp::WithinTolerance(c.Angle(d), (double)1.052, (double)0.001)) {
return false; return false;
} }
@ -374,26 +374,26 @@ Vector3DoubleTests_Angle()
static bool static bool
Vector3DoubleTests_ParallelOrthogonalVectors() Vector3DoubleTests_ParallelOrthogonalVectors()
{ {
geom::Vector3d a {-2.029, 9.97, 4.172}; geom::Vector3D a {-2.029, 9.97, 4.172};
geom::Vector3d b {-9.231, -6.639, -7.245}; geom::Vector3D b {-9.231, -6.639, -7.245};
geom::Vector3d c {-2.328, -7.284, -1.214}; geom::Vector3D c {-2.328, -7.284, -1.214};
geom::Vector3d d {-1.821, 1.072, -2.94}; geom::Vector3D d {-1.821, 1.072, -2.94};
geom::Vector3d e {-2.0, 1.0, 3.0}; geom::Vector3D e {-2.0, 1.0, 3.0};
geom::Vector3d f {-6.0, 3.0, 9.0}; geom::Vector3D f {-6.0, 3.0, 9.0};
geom::Vector3d zeroVector {0.0, 0.0, 0.0}; geom::Vector3D zeroVector {0.0, 0.0, 0.0};
SCTEST_CHECK_FALSE(a.isParallel(b)); SCTEST_CHECK_FALSE(a.IsParallel(b));
SCTEST_CHECK_FALSE(a.isOrthogonal(b)); SCTEST_CHECK_FALSE(a.IsOrthogonal(b));
SCTEST_CHECK_FALSE(c.isParallel(d)); SCTEST_CHECK_FALSE(c.IsParallel(d));
SCTEST_CHECK(c.isOrthogonal(d)); SCTEST_CHECK(c.IsOrthogonal(d));
SCTEST_CHECK(e.isParallel(f)); SCTEST_CHECK(e.IsParallel(f));
SCTEST_CHECK_FALSE(e.isOrthogonal(f)); SCTEST_CHECK_FALSE(e.IsOrthogonal(f));
SCTEST_CHECK(zeroVector.isZero()); SCTEST_CHECK(zeroVector.IsZero());
SCTEST_CHECK(c.isParallel(zeroVector)); SCTEST_CHECK(c.IsParallel(zeroVector));
SCTEST_CHECK(c.isOrthogonal(zeroVector)); SCTEST_CHECK(c.IsOrthogonal(zeroVector));
return true; return true;
} }
@ -402,13 +402,13 @@ Vector3DoubleTests_ParallelOrthogonalVectors()
static bool static bool
Vector3DoubleTests_Projections() Vector3DoubleTests_Projections()
{ {
geom::Vector3d a {4.866769214609107, 6.2356222686140566, 9.140878417029711}; geom::Vector3D a {4.866769214609107, 6.2356222686140566, 9.140878417029711};
geom::Vector3d b {6.135533104801077, 8.757851406697895, 0.6738031370548048}; geom::Vector3D b {6.135533104801077, 8.757851406697895, 0.6738031370548048};
geom::Vector3d c {4.843812341655318, 6.9140509888133055, 0.5319465962229454}; geom::Vector3D c {4.843812341655318, 6.9140509888133055, 0.5319465962229454};
geom::Vector3d d {0.02295687295378901, -0.6784287201992489, 8.608931820806765}; geom::Vector3D d {0.02295687295378901, -0.6784287201992489, 8.608931820806765};
SCTEST_CHECK_EQ(a.projectParallel(b), c); SCTEST_CHECK_EQ(a.ProjectParallel(b), c);
SCTEST_CHECK_EQ(a.projectOrthogonal(b), d); SCTEST_CHECK_EQ(a.ProjectOrthogonal(b), d);
return true; return true;
} }
@ -417,12 +417,12 @@ Vector3DoubleTests_Projections()
static bool static bool
Vector3DoubleTests_CrossProduct() Vector3DoubleTests_CrossProduct()
{ {
geom::Vector3d a {8.462, 7.893, -8.187}; geom::Vector3D a {8.462, 7.893, -8.187};
geom::Vector3d b {6.984, -5.975, 4.778}; geom::Vector3D b {6.984, -5.975, 4.778};
geom::Vector3d c {-11.2046, -97.6094, -105.685}; geom::Vector3D c {-11.2046, -97.6094, -105.685};
c.setEpsilon(0.001); // double trouble c.SetEpsilon(0.001); // double trouble
SCTEST_CHECK_EQ(c, a.cross(b)); SCTEST_CHECK_EQ(c, a.Cross(b));
return true; return true;
} }