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")
add_compile_options("-stdlib=libc++")
else ()
# nothing special for gcc at the moment
# nothing special for gcc At the moment
endif ()
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.
///
/// \param rads the angle in radians
/// \return the angle in degrees.
/// \param rads the Angle in radians
/// \return the Angle in degrees.
float RadiansToDegreesF(float rads);
/// \brief Convert radians to degrees.
///
/// \param rads the angle in radians
/// \return the angle in degrees.
/// \param rads the Angle in radians
/// \return the Angle in degrees.
double RadiansToDegreesD(double rads);
/// \brief Convert degrees to radians.
///
/// \param degrees the angle in degrees
/// \return the angle in radians.
/// \param degrees the Angle in degrees
/// \return the Angle in radians.
float DegreesToRadiansF(float degrees);
/// \brief Convert degrees to radians.
///
/// \param degrees the angle in degrees
/// \return the angle in radians.
/// \param degrees the Angle in degrees
/// \return the Angle in radians.
double DegreesToRadiansD(double degrees);
/// \brief RotateRadians rotates theta0 by theta1 radians, wrapping

2
include/scmp/Motion2D.h
View File

@ -12,7 +12,7 @@ namespace scmp {
namespace basic {
scmp::geom::Vector2d Acceleration(double speed, double heading);
scmp::geom::Vector2D Acceleration(double speed, double heading);
} // 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.
///
/// Madgwick is a novel Orientation filter applicable to IMUs
/// consisting of tri-axis gyroscopes and accelerometers, and MARG
/// sensor arrays that also include tri-axis magnetometers. The MARG
/// consisting of tri-Axis gyroscopes and accelerometers, and MARG
/// sensor arrays that also include tri-Axis magnetometers. The MARG
/// implementation incorporates magnetic distortionand gyroscope bias
/// drift compensation.
///
@ -51,24 +51,24 @@ namespace filter {
template <typename T>
class Madgwick {
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()
{};
/// \brief The Madgwick filter is initialised with a sensor frame.
///
/// \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()
{
if (!sf.isZero()) {
sensorFrame = scmp::geom::quaternion<T>(sf, 0.0);
if (!sf.IsZero()) {
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) :
deltaT(0.0), previousSensorFrame(), sensorFrame(sf)
{};
@ -91,7 +91,7 @@ public:
///
/// \param gyro A three-dimensional vector containing gyro readings
/// 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>
AngularRate(const scmp::geom::Vector<T, 3> &gyro) const
{
@ -166,7 +166,7 @@ public:
scmp::geom::Vector<T, 3>
Euler()
{
return this->sensorFrame.euler();
return this->sensorFrame.Euler();
}
/// \brief Set the default Δt.

2
include/scmp/geom.h
View File

@ -2,7 +2,7 @@
/// \file include/scmp/geom.h
/// \author K. Isom <kyle@imap.cc>
/// \date 2023-10-20
/// \brief 2D point and polar coordinate systems.
/// \brief Vector-based geometry code.
///
/// 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).
Point2D();
/// \brief Initialize a Point2D at (_x, _y).
/// \brief Initialize a Point2D At (_x, _y).
Point2D(int _x, int _y);
/// \brief Initialize a Point2D from a Polar2D coordinate.
@ -74,13 +74,13 @@ public:
/// \brief Rotate rotates the point by theta radians.
///
/// \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);
/// \brief Rotate this point around a series of vertices.
///
/// \param vertices A series of vertices to rotate this point around.
/// \param theta The angle to rotate by.
/// \param vertices A series of vertices to Rotate this point around.
/// \param theta The Angle to Rotate by.
/// \return A series of rotated points.
std::vector<Point2D> Rotate(std::vector<Polar2D> vertices, double theta);
@ -98,12 +98,12 @@ public:
};
// 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.
class Polar2D : public Vector<double, 2> {
public:
// 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(double _r, double _theta);
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
* vector with respect to a reference plane that is assumed to be the
* of the Earth.
*/
///
/// \file include/scmp/geom/Orientation.h
/// \author K. Isom <kyle@imap.cc>
/// \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 <scmp/geom/Vector.h>
#ifndef SCMATH_GEOM_ORIENTATION_H
#define SCMATH_GEOM_ORIENTATION_H
@ -19,71 +39,75 @@ namespace geom {
/// and three-dimensional vectors.
/// \ingroup basis
/// Convenience constant for the x index.
constexpr uint8_t Basis_x = 0;
/// \brief Convenience constant for the x index.
constexpr uint8_t BasisX = 0;
/// \ingroup basis
/// Convenience constant for the y index.
constexpr uint8_t Basis_y = 1;
/// \brief Convenience constant for the y index.
constexpr uint8_t BasisY = 1;
/// \ingroup basis
/// Convenience constant for the z index.
constexpr uint8_t Basis_z = 2;
/// \brief Convenience constant for the z index.
constexpr uint8_t BasisZ = 2;
/// @brief Basis2d provides basis vectors for Vector2ds.
static const Vector2d Basis2d[] = {
Vector2d{1, 0},
Vector2d{0, 1},
/// \brief Basis2D provides basis vectors for Vector2ds.
static const Vector2D Basis2D[] = {
Vector2D{1, 0},
Vector2D{0, 1},
};
/// @brief Basis2d provides basis vectors for Vector2fs.
static const Vector2f Basis2f[] = {
Vector2f{1, 0},
Vector2f{0, 1},
/// \brief Basis2D provides basis vectors for Vector2fs.
static const Vector2F Basis2F[] = {
Vector2F{1, 0},
Vector2F{0, 1},
};
/// @brief Basis2d provides basis vectors for Vector3ds.
static const Vector3d Basis3d[] = {
Vector3d{1, 0, 0},
Vector3d{0, 1, 0},
Vector3d{0, 0, 1},
/// \brief Basis2D provides basis vectors for Vector3ds.
static const Vector3D Basis3D[] = {
Vector3D{1, 0, 0},
Vector3D{0, 1, 0},
Vector3D{0, 0, 1},
};
/// @brief Basis2d provides basis vectors for Vector3fs.
static const Vector3f Basis3f[] = {
Vector3f{1, 0, 0},
Vector3f{0, 1, 0},
Vector3f{0, 0, 1},
/// \brief Basis2D provides basis vectors for Vector3fs.
static const Vector3F Basis3F[] = {
Vector3F{1, 0, 0},
Vector3F{0, 1, 0},
Vector3F{0, 0, 1},
};
/// Heading2f returns a compass heading for a Vector2f.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
float Heading2f(Vector2f vec);
/// \brief Compass heading for a Vector2F.
///
/// \param vec A vector Orientation.
/// \return The compass heading of the vector in radians.
float Heading2F(Vector2F vec);
/// Heading2d returns a compass heading for a Vector2d.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
double Heading2d(Vector2d vec);
/// \brief Compass heading for a Vector2D.
///
/// \param vec A vector Orientation.
/// \return The compass heading of the vector in radians.
double Heading2d(Vector2D vec);
/// Heading3f returns a compass heading for a Vector2f.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
float Heading3f(Vector3f vec);
/// \brief Compass heading for a Vector2F.
///
/// \param vec A vector Orientation.
/// \return The compass heading of the vector in radians.
float Heading3f(Vector3F vec);
/// Heading3d returns a compass heading for a Vector2f.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
double Heading3d(Vector3d vec);
/// Heading3d returns a compass heading for a Vector2F.
///
/// \param vec A vector Orientation.
/// \return The compass heading of the vector in radians.
double Heading3d(Vector3D vec);
} // namespace geom
} // namespace scmp
#endif // __WRMATH_ORIENTATION_H
#endif // SCMATH_GEOM_ORIENTATION_H

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

@ -1,7 +1,27 @@
/// quaternion.h contains an implementation of quaternions suitable
/// for navigation in R3.
#ifndef SCMATH_QUATERNION_H
#define SCMATH_QUATERNION_H
///
/// \file include/scmp/geom/Quaternion.h
/// \author K. Isom <kyle@imap.cc>
/// \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>
@ -13,77 +33,79 @@
#include <scmp/Math.h>
#include <scmp/geom/Vector.h>
/// math contains the shimmering clarity math library.
namespace scmp {
/// geom contains geometric classes and functions.
namespace geom {
/// @brief Quaternions provide a representation of Orientation and rotations
/// in three dimensions.
/// \brief Quaternions provide a representation of Orientation
/// and rotations in three dimensions.
///
/// Quaternions encode rotations in three-dimensional space. While technically
/// a quaternion is comprised of a real element and a complex vector<3>, for
/// the purposes of this library, it is modeled as a floating point 4D vector
/// of the form <w, x, y, z>, where x, y, and z represent an axis of rotation in
/// R3 and w the angle, in radians, of the rotation about that axis. Where Euler
/// angles are concerned, the ZYX (or yaw, pitch, roll) sequence is used.
/// Quaternions encode rotations in three-dimensional space. While
/// technically a MakeQuaternion is comprised of a real element and a
/// complex vector<3>, for the purposes of this library, it is modeled
/// as a floating point 4D vector of the form <w, x, y, z>, where x, y,
/// and z represent an Axis of rotation in R3 and w the Angle, in
/// 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
/// wr::geom::Vector.
/// For information on the underlying vector type, see the
/// documentation for scmp::geom::Vector.
///
/// The constructors are primarily intended for intended operations; in practice,
/// the quaternionf() and quaterniond() functions are more useful for constructing
/// quaternions from vectors and angles.
///
/// Like vectors, quaternions carry an internal tolerance value ε that is used for
/// 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.
/// Like vectors, quaternions carry an internal tolerance value ε that
/// is used for 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>
class Quaternion {
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)
{
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
/// and an angle of rotation. This doesn't do the angle transforms to simplify
/// internal operations.
/// \brief Construct a MakeQuaternion with an Axis and Angle of
/// rotation.
///
/// @param _axis A three-dimensional vector of the same type as the Quaternion.
/// @param _angle The angle of rotation about the axis of rotation.
/// A Quaternion may be initialised with a Vector<T, 3> Axis
/// 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)
{
this->constrainAngle();
scmp::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps);
v.SetEpsilon(this->eps);
};
/// 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) :
v(Vector<T, 3>{vector[1], vector[2], vector[3]}),
w(vector[0])
{
this->constrainAngle();
scmp::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps);
v.SetEpsilon(this->eps);
}
/// A Quaternion may be constructed with an initializer list of
/// type T, which must have exactly N elements.
/// \brief An initializer list containing values for w, x, y,
/// 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)
{
auto it = ilst.begin();
@ -93,47 +115,47 @@ public:
this->constrainAngle();
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
setEpsilon(T epsilon)
SetEpsilon(T 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>
axis() const
Axis() const
{
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
angle() const
Angle() const
{
return this->w;
}
/// Compute the dot product of two quaternions.
/// \brief Compute the Dot product of two quaternions.
///
/// \param other Another quaternion.
/// \return The dot product between the two quaternions.
/// \param other Another MakeQuaternion.
/// \return The Dot product between the two quaternions.
T
dot(const Quaternion<T> &other) const
Dot(const Quaternion<T> &other) const
{
double innerProduct = this->v[0] * other.v[0];
@ -144,12 +166,14 @@ public:
}
/// Compute the norm of a quaternion. Treating the Quaternion as a
/// Vector<T, 4>, it's the same as computing the magnitude.
/// \brief Compute the Norm of a MakeQuaternion.
///
/// @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
norm() const
Norm() const
{
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
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
conjugate() const
Conjugate() const
{
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
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
isIdentity() const {
return this->v.isZero() &&
IsIdentity() const {
return this->v.IsZero() &&
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
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
/// followed by the angle of rotation.
/// \brief Convert to Vector form.
///
/// @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>
asVector() const
AsVector() const
{
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.
/// @return The rotated vector.
/// \param vr The vector to be rotated.
/// \return The rotated vector.
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
/// <yaw, pitch, roll>. Users of this function should watch out
/// for gimbal lock.
/// \brief Return Euler angles for this MakeQuaternion.
///
/// @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>
euler() const
Euler() const
{
T yaw, pitch, roll;
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.
/// @return The result of adding the two quaternions together.
/// \param other The MakeQuaternion to be added with this one.
/// \return The result of adding the two quaternions together.
Quaternion
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.
/// @return The result of subtracting the other quaternion from this one.
/// \param other The MakeQuaternion to be subtracted from this one.
/// \return The result of subtracting the other MakeQuaternion from this one.
Quaternion
operator-(const Quaternion<T> &other) const
{
@ -279,10 +309,10 @@ public:
}
/// Perform scalar multiplication.
/// \brief Scalar multiplication.
///
/// @param k The scaling value.
/// @return A scaled quaternion.
/// \param k The scaling value.
/// \return A scaled MakeQuaternion.
Quaternion
operator*(const T k) const
{
@ -290,10 +320,10 @@ public:
}
/// Perform scalar division.
/// \brief Scalar division.
///
/// @param k The scalar divisor.
/// @return A scaled quaternion.
/// \param k The scalar divisor.
/// \return A scaled MakeQuaternion.
Quaternion
operator/(const T k) const
{
@ -301,23 +331,25 @@ public:
}
/// Perform quaternion Hamilton multiplication with a three-
/// dimensional vector; this is done by treating the vector
/// as a pure quaternion (e.g. with an angle of rotation of 0).
/// \brief Quaternion Hamilton multiplication with a three-
/// dimensional vector.
///
/// @param vector The vector to multiply with this quaternion.
/// @return The Hamilton product of the quaternion and vector.
/// This is done by treating the vector as a pure MakeQuaternion
/// (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
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);
}
/// 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.
Quaternion
operator*(const Quaternion<T> &other) const
@ -326,14 +358,15 @@ public:
(this->v * other.v);
Vector<T, 3> axis = (other.v * this->w) +
(this->v * other.w) +
(this->v.cross(other.v));
(this->v.Cross(other.v));
return Quaternion(axis, angle);
}
/// Perform quaternion equality checking.
/// @param other The quaternion to check equality against.
/// @return True if the two quaternions are equal within their tolerance.
/// \brief Quaternion equivalence.
///
/// \param other The MakeQuaternion to check equality against.
/// \return True if the two quaternions are equal within their tolerance.
bool
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.
/// @return True if the two quaternions are unequal within their tolerance.
/// \param other The MakeQuaternion to check inequality against.
/// \return True if the two quaternions are unequal within their tolerance.
bool
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.
///
/// @param outs An output stream
/// @param q A quaternion
/// @return The output stream
/// \param outs An output stream
/// \param q A MakeQuaternion
/// \return The output stream
friend std::ostream &
operator<<(std::ostream &outs, const Quaternion<T> &q)
{
@ -370,8 +405,8 @@ private:
static constexpr T minRotation = -4 * M_PI;
static constexpr T maxRotation = 4 * M_PI;
Vector<T, 3> v; // axis of rotation
T w; // angle of rotation
Vector<T, 3> v; // Axis of rotation
T w; // Angle of rotation
T eps;
void
@ -393,76 +428,93 @@ private:
///
/// \ingroup quaternion_aliases
/// Type alias for a float Quaternion.
/// \brief Type alias for a float Quaternion.
typedef Quaternion<float> Quaternionf;
/// \ingroup quaternion_aliases
/// Type alias for a double Quaternion.
/// \brief Type alias for a double Quaternion.
typedef Quaternion<double> Quaterniond;
/// Return a float quaternion scaled appropriately from a vector and angle,
/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
/// \brief Convenience Quaternion construction function.
///
/// @param axis The axis of rotation.
/// @param angle The angle of rotation.
/// @return A quaternion.
/// @relatesalso Quaternion
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).
/// Return a float 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 quaternion.
/// @relatesalso Quaternion
Quaterniond quaterniond(Vector3d axis, double angle);
/// \param axis The Axis of rotation.
/// \param angle The Angle of rotation.
/// \return A MakeQuaternion.
/// \relatesalso Quaternion
Quaternionf MakeQuaternion(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).
/// \brief Convience Quaternion construction function.
///
/// @param axis The axis of rotation.
/// @param angle The angle of rotation.
/// @return A quaternion.
/// @relatesalso Quaternion
/// 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
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>
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));
}
/// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll),
/// return a quaternion.
/// \brief COnstruct a Quaternion from Euler angles.
///
/// @param euler A vector Euler angle in ZYX sequence.
/// @return A Quaternion representation of the Orientation represented
/// 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.
/// 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
/// \param euler A vector Euler Angle in ZYX sequence.
/// \return A Quaternion representation of the Orientation represented
/// by the Euler angles.
/// @relatesalso Quaternion
Quaterniond quaterniond_from_euler(Vector3d euler);
/// \relatesalso Quaternion
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.
///
/// \tparam T
/// \param p The starting quaternion.
/// \param q The ending quaternion.
/// \param p The starting MakeQuaternion.
/// \param q The ending MakeQuaternion.
/// \param t The fraction of the distance between the two quaternions to
/// interpolate.
/// \return A Quaternion representing the linear interpolation of the
@ -471,17 +523,19 @@ template <typename T>
Quaternion<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
/// interpolation between two quaternions at some fraction of the
/// interpolation between two unit quaternions At some fraction of the
/// distance between them.
///
/// \tparam T
/// \param p The starting quaternion.
/// \param q The ending quaternion.Short
/// \param p The starting MakeQuaternion.
/// \param q The ending MakeQuaternion.Short
/// \param t The fraction of the distance between the two quaternions
/// to interpolate.
/// \return A Quaternion representing the shortest path between two
@ -490,10 +544,10 @@ template <typename T>
Quaternion<T>
ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t)
{
assert(p.isUnitQuaternion());
assert(q.isUnitQuaternion());
assert(p.IsUnitQuaternion());
assert(q.IsUnitQuaternion());
T dp = p.dot(q);
T dp = p.Dot(q);
T sign = dp < 0.0 ? -1.0 : 1.0;
T omega = std::acos(dp * sign);
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
/// class to verify correct operation. Note that if \#NDEBUG is defined, the
/// self test is disabled.
void Quaternion_SelfTest();
void QuaternionSelfTest();
} // namespace geom
} // 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/math/vectors.h
// Author: Kyle Isom
// Date: 2017-06-05
// Namespace: math::vectors.
//
// vectors.h defines the Vector2D class and associated functions in the
// namespace scmp::vectors.
//
// 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.
#ifndef SCMATH_VECTORS_H
#define SCMATH_VECTORS_H
///
/// \file include/scmp/geom/Vector.h
/// \author K. Isom <kyle@imap.cc>
/// \date 2017-06-05
/// \brief Linear algebraic vector class.
///
/// Copyright 2017 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_VECTORS_H
#define SCMATH_GEOM_VECTORS_H
#include <array>
@ -44,21 +43,20 @@ namespace scmp {
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
/// vectors. Once instantiated, they cannot be modified.
///
/// Note that while the class is templated, it's intended to be used with
/// floating-point types.
/// Note that while the class is templated, it's intended to be used
/// with floating-point types.
///
/// Vectors can be indexed like arrays, and they contain an epsilon value
/// that defines a tolerance for equality.
/// Vectors can be indexed like arrays, and they contain an epsilon
/// value that defines a tolerance for equality.
template<typename T, size_t N>
class Vector {
public:
/// The default constructor creates a unit vector for a given type
/// and size.
/// \brief Construct a unit vector of a given type and size.
Vector()
{
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
/// 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)
{
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.
///
/// \param index The index of the item to retrieve.
/// \return The value at the index.
T at(size_t index) const
/// \return The value At the index.
T At(size_t index) const
{
if (index > this->arr.size()) {
throw std::out_of_range("index " +
@ -105,7 +106,7 @@ public:
///
/// \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
void Set(size_t index, T value)
{
@ -122,9 +123,10 @@ public:
/// Compute the length of the vector.
/// @return The length of the vector.
T magnitude() const
/// \brief Compute the length of the vector.
///
/// \return The length of the vector.
T Magnitude() const
{
T result = 0;
@ -135,21 +137,25 @@ public:
}
/// Set the tolerance for equality checks. At a minimum, this allows
/// for systemic errors in floating math arithmetic.
/// @param eps is the maximum difference between this vector and
/// \brief Set equivalence tolerance.
///
/// 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.
void
setEpsilon(T eps)
SetEpsilon(T eps)
{
this->epsilon = eps;
}
/// Determine whether this is a zero vector.
/// @return true if the vector is zero.
/// \brief Determine whether this is a zero vector.
///
/// \return true if the vector is zero.
bool
isZero() const
IsZero() const
{
for (size_t i = 0; i < N; i++) {
if (!scmp::WithinTolerance(this->arr[i], (T) 0.0, this->epsilon)) {
@ -160,51 +166,55 @@ public:
}
/// Obtain the unit vector for this vector.
/// @return The unit vector
/// \brief Obtain the unit vector for this vector.
///
/// \return The unit 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.
/// @return true if the vector is a unit vector.
/// \brief Determine if this is a unit vector.
///
/// \return true if the vector is a unit vector.
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.
/// @param other Another vector.
/// @return The angle in radians between the two vectors.
/// \brief Compute the Angle between two vectors.
///
/// \param other Another vector.
/// \return The Angle in radians between the two vectors.
T
angle(const Vector<T, N> &other) const
Angle(const Vector<T, N> &other) const
{
Vector<T, N> unitA = this->unitVector();
Vector<T, N> unitB = other.unitVector();
Vector<T, N> unitA = this->UnitVector();
Vector<T, N> unitB = other.UnitVector();
// Can't compute angles with a zero vector.
assert(!this->isZero());
assert(!other.isZero());
return std::acos(unitA * unitB);
assert(!this->IsZero());
assert(!other.IsZero());
return static_cast<T>(std::acos(unitA * unitB));
}
/// Determine whether two vectors are parallel.
/// @param other Another vector
/// @return True if the angle between the vectors is zero.
/// \brief Determine whether two vectors are parallel.
///
/// \param other Another vector
/// \return True if the Angle between the vectors is zero.
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;
}
T angle = this->angle(other);
T angle = this->Angle(other);
if (scmp::WithinTolerance(angle, (T) 0.0, this->epsilon)) {
return true;
}
@ -213,14 +223,15 @@ public:
}
/// Determine if two vectors are orthogonal or perpendicular to each
/// other.
/// @param other Another vector
/// @return True if the two vectors are orthogonal.
/// \brief Determine if two vectors are orthogonal or
/// perpendicular to each other.
///
/// \param other Another vector
/// \return True if the two vectors are orthogonal.
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;
}
@ -228,40 +239,51 @@ public:
}
/// 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
/// \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
/// 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);
}
/// 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.
/// @return A vector that is the orthogonal projection of this onto
/// \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.
/// \return A vector that is the orthogonal projection of this onto
/// the basis 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;
}
/// Compute the cross product of two vectors. This is only defined
/// over three-dimensional vectors.
/// @param other Another 3D vector.
/// @return The cross product vector.
/// \brief Compute the cross product of two vectors.
///
/// This is only defined over three-dimensional vectors.
///
/// \throw std::out_of_range if this is not a three-dimensional vector.
///
/// \param other Another 3D vector.
/// \return The Cross product vector.
Vector
cross(const Vector<T, N> &other) const
Cross(const Vector<T, N> &other) const
{
assert(N == 3);
if (N != 3) {
throw std::out_of_range("Cross-product can only called on Vector<T, 3>.");
}
return Vector<T, N>{
(this->arr[1] * other.arr[2]) - (other.arr[1] * this->arr[2]),
-((this->arr[0] * other.arr[2]) - (other.arr[0] * this->arr[2])),
@ -270,9 +292,10 @@ public:
}
/// Perform vector addition with another vector.
/// @param other The vector to be added.
/// @return A new vector that is the result of adding this and the
/// \brief Vector addition.
///
/// \param other The vector to be added.
/// \return A new vector that is the result of adding this and the
/// other vector.
Vector
operator+(const Vector<T, N> &other) const
@ -287,9 +310,10 @@ public:
}
/// Perform vector subtraction with another vector.
/// @param other The vector to be subtracted from this vector.
/// @return A new vector that is the result of subtracting the
/// \brief Vector subtraction.
///
/// \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.
Vector
operator-(const Vector<T, N> &other) const
@ -304,9 +328,10 @@ public:
}
/// Perform scalar multiplication of this vector by some scale factor.
/// @param k The scaling value.
/// @return A new vector that is this vector scaled by k.
/// \brief Scalar multiplication.
///
/// \param k The scaling value.
/// \return A new vector that is this vector scaled by k.
Vector
operator*(const T k) const
{
@ -320,9 +345,10 @@ public:
}
/// Perform scalar division of this vector by some scale factor.
/// @param k The scaling value
/// @return A new vector that is this vector scaled by 1/k.
/// \brief Scalar division.
///
/// \param k The scaling value
/// \return A new vector that is this vector scaled by 1/k.
Vector
operator/(const T k) const
{
@ -336,9 +362,10 @@ public:
}
/// 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.
/// \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.
T
operator*(const Vector<T, N> &other) const
{
@ -352,9 +379,10 @@ public:
}
/// Compare two vectors for equality.
/// @param other The other vector.
/// @return Return true if all the components of both vectors are
/// \brief Vector equivalence
///
/// \param other The other vector.
/// \return Return true if all the components of both vectors are
/// within the tolerance value.
bool
operator==(const Vector<T, N> &other) const
@ -368,9 +396,10 @@ public:
}
/// Compare two vectors for inequality.
/// @param other The other vector.
/// @return Return true if any of the components of both vectors are
/// \brief Vector non-equivalence.
///
/// \param other The other vector.
/// \return Return true if any of the components of both vectors are
/// not within the tolerance value.
bool
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,
/// it's required to do something like the following:
/// Note that the values of the vector cannot be modified.
/// Instead, something like the following must be done:
///
/// ```
/// Vector3d a {1.0, 2.0, 3.0};
/// Vector3d b {a[0], a[1]*2.0, a[2]};
/// Vector3D a {1.0, 2.0, 3.0};
/// Vector3D b {a[0], a[1]*2.0, a[2]};
/// ```
///
/// @param i The component index.
/// @return The value of the vector component at i.
/// \param i The component index.
/// \return The value of the vector component At i.
const T &
operator[](size_t i) const
{
@ -398,10 +427,11 @@ public:
}
/// Support outputting vectors in the form "<i, j, ...>".
/// @param outs An output stream.
/// @param vec The vector to be formatted.
/// @return The output stream.
/// \brief Write a vector a stream in the form "<i, j, ...>".
///
/// \param outs An output stream.
/// \param vec The vector to be formatted.
/// \return The output stream.
friend std::ostream &
operator<<(std::ostream &outs, const Vector<T, N> &vec)
{
@ -429,36 +459,35 @@ private:
/// \ingroup vector_aliases
/// 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.
/// For example, a 2D float vector is Vector2f.
/// For example, a 2D float vector is Vector2F.
/// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional float vector.
typedef Vector<float, 2> Vector2f;
/// \brief Type alias for a two-dimensional float vector.
typedef Vector<float, 2> Vector2F;
/// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional float vector.
typedef Vector<float, 3> Vector3f;
/// \brief Type alias for a three-dimensional float vector.
typedef Vector<float, 3> Vector3F;
/// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional float vector.
typedef Vector<float, 4> Vector4f;
/// \brief Type alias for a four-dimensional float vector.
typedef Vector<float, 4> Vector4F;
/// \ingroup vector_aliases
/// @brief Type alias for a two-dimensional double vector.
typedef Vector<double, 2> Vector2d;
/// \brief Type alias for a two-dimensional double vector.
typedef Vector<double, 2> Vector2D;
/// \ingroup vector_aliases
/// @brief Type alias for a three-dimensional double vector.
typedef Vector<double, 3> Vector3d;
/// \brief Type alias for a three-dimensional double vector.
typedef Vector<double, 3> Vector3D;
/// \ingroup vector_aliases
/// @brief Type alias for a four-dimensional double vector.
typedef Vector<double, 4> Vector4d;
/// \brief Type alias for a four-dimensional double vector.
typedef Vector<double, 4> Vector4D;
} // namespace geom
} // namespace scmp
#endif // SCMATH_VECTORS_H_H
#endif // SCMATH_GEOM_VECTORS_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,
/// 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
/// arena's life, at which point Destroy should be called.
/// arena's life, At which point Destroy should be called.
class Arena {
public:
/// 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.
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.
/// \return True if the Buffer was resized.
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.
/// \return True if the Buffer was resized.
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 datalen The size of the data buffer.
/// \return True if the Buffer was resized.
bool
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.
/// \return True if the Buffer was resized.
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 count The number of bytes to remove.

2
include/scsl/Flags.h
View File

@ -282,7 +282,7 @@ public:
/// Return a particular argument.
///
/// \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
/// exception is thrown.
std::string Arg(size_t index);

14
include/scsl/StringUtil.h
View File

@ -39,27 +39,27 @@ namespace U {
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.
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.
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.
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.
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.
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.
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);
/// 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).
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];
};
/// 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.
///
/// \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 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.
/// \return If the tag is found, a cursor pointing to the next record is
/// 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 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.
/// \return If the tag is found, a cursor pointing to the record is
/// 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 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
/// of empty space; otherwise, nullptr is returned.
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");
// 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
//

4
include/sctest/SimpleSuite.h
View File

@ -59,14 +59,14 @@ public:
/// \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
/// predicate: if it returns false, tests automatically fail.
void Setup(std::function<bool(void)> setupFn) { fnSetup = setupFn; }
/// \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.
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 <iostream>
#include <vector>
#include <scmp/Math.h>
#include <scmp/geom/Coord2D.h>
#include <scmp/geom/Orientation.h>
#include <scmp/geom/Vector.h>
@ -61,21 +60,21 @@ Point2D::Point2D(const Polar2D &pol)
int
Point2D::X() const
{
return this->at(0);
return this->At(0);
}
void
Point2D::X(int _x)
{
this->Set(0, _x);
this->Set(BasisX, _x);
}
int
Point2D::Y() const
{
return this->at(1);
return this->At(1);
}
@ -165,7 +164,7 @@ Polar2D::Polar2D(const Point2D &pt)
double
Polar2D::R() const
{
return this->at(0);
return this->At(0);
}
@ -179,7 +178,7 @@ Polar2D::R(const double _r)
double
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 {
scmp::geom::Vector2d
scmp::geom::Vector2D
Acceleration(double speed, double heading)
{
auto dx = std::cos(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
Heading2f(Vector2f vec)
Heading2F(Vector2F vec)
{
return vec.angle(Basis2f[Basis_x]);
return vec.Angle(Basis2F[BasisX]);
}
float
Heading3f(Vector3f vec)
Heading3f(Vector3F vec)
{
Vector2f vec2f {vec[0], vec[1]};
return Heading2f(vec2f);
Vector2F vec2f {vec[0], vec[1]};
return Heading2F(vec2f);
}
double
Heading2d(Vector2d vec)
Heading2d(Vector2D vec)
{
return vec.angle(Basis2d[Basis_x]);
return vec.Angle(Basis2D[BasisX]);
}
double
Heading3d(Vector3d vec)
Heading3d(Vector3D vec)
{
Vector2d vec2d {vec[0], vec[1]};
Vector2D vec2d {vec[0], vec[1]};
return Heading2d(vec2d);
}

24
src/scmp/Quaternion.cc
View File

@ -8,23 +8,23 @@ namespace geom {
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));
}
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));
}
Quaternionf
quaternionf_from_euler(Vector3f euler)
QuaternionFromEuler(Vector3F euler)
{
float x, y, z, w;
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);
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_from_euler(Vector3d euler)
QuaternionFromEuler(Vector3D euler)
{
double x, y, z, w;
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);
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
Quaternion_SelfTest()
QuaternionSelfTest()
{
#ifndef NDEBUG
Vector3f v {1.0, 0.0, 0.0};
Vector3f yAxis {0.0, 1.0, 0.0};
Vector3F v {1.0, 0.0, 0.0};
Vector3F yAxis {0.0, 1.0, 0.0};
float angle = M_PI / 2;
Quaternionf p = quaternionf(yAxis, angle);
Quaternionf q;
Vector3f vr {0.0, 0.0, 1.0};
Vector3F vr {0.0, 0.0, 1.0};
assert(p.isUnitQuaternion());
std::cerr << p.rotate(v) << std::endl;

2
src/test/Assert.cc
View File

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

12
test/coord2d.cc
View File

@ -90,6 +90,18 @@ geomConversionIdentities()
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;
}

28
test/madgwick.cc
View File

@ -21,7 +21,7 @@ bool
SimpleAngularOrientationFloat()
{
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 float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -48,7 +48,7 @@ bool
SimpleAngularOrientationFloatDefaultDT()
{
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 float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -76,7 +76,7 @@ bool
VerifyUpdateWithZeroDeltaTFails()
{
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 float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -101,7 +101,7 @@ bool
SimpleAngularOrientationDouble()
{
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 double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
@ -126,9 +126,9 @@ SimpleAngularOrientationDouble()
bool
SimpleAngularOrientation2InitialVector3f()
{
const geom::Vector3f initialFrame{0, 0, 0};
const geom::Vector3F initialFrame{0, 0, 0};
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 float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -153,9 +153,9 @@ SimpleAngularOrientation2InitialVector3f()
bool
SimpleAngularOrientation2InitialQuaternionf()
{
const auto initialFrame = geom::quaternionf_from_euler({0, 0, 0});
const auto initialFrame = geom::QuaternionFromEuler({0, 0, 0});
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 float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
@ -180,9 +180,9 @@ SimpleAngularOrientation2InitialQuaternionf()
bool
SimpleAngularOrientation2InitialVector3d()
{
const geom::Vector3d initialFrame{0, 0, 0};
const geom::Vector3D initialFrame{0, 0, 0};
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 double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
@ -207,9 +207,9 @@ SimpleAngularOrientation2InitialVector3d()
bool
SimpleAngularOrientation2InitialQuaterniond()
{
const auto initialFrame = geom::quaterniond_from_euler({0, 0, 0});
const auto initialFrame = geom::QuaternionFromEuler({0, 0, 0});
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 double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
@ -266,9 +266,9 @@ main(int argc, char **argv)
SimpleAngularOrientation2InitialVector3f);
suite.AddTest("SimpleAngularOrientationDouble (inital vector3d)",
SimpleAngularOrientation2InitialVector3d);
suite.AddTest("SimpleAngularOrientationFloat (inital quaternionf)",
suite.AddTest("SimpleAngularOrientationFloat (inital MakeQuaternion)",
SimpleAngularOrientation2InitialQuaternionf);
suite.AddTest("SimpleAngularOrientationDouble (inital quaterniond)",
suite.AddTest("SimpleAngularOrientationDouble (inital MakeQuaternion)",
SimpleAngularOrientation2InitialQuaterniond);
delete flags;

10
test/orientation.cc
View File

@ -45,9 +45,9 @@ UnitConversions_RadiansToDegreesD()
bool
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;
}
@ -56,7 +56,7 @@ Orientation2f_Heading()
bool
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));
@ -67,7 +67,7 @@ Orientation3f_Heading()
bool
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);
}
@ -76,7 +76,7 @@ Orientation2d_Heading()
bool
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);
}

116
test/quaternion.cc
View File

@ -15,7 +15,7 @@ using namespace sctest;
static bool
Quaternion_SelfTest()
{
geom::Quaternion_SelfTest();
geom::QuaternionSelfTest();
return true;
}
@ -23,9 +23,9 @@ Quaternion_SelfTest()
static bool
Quaterniond_Addition()
{
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 expected(geom::Vector4d{5.0, 0.0, 0.0, 4.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 expected(geom::Vector4D{5.0, 0.0, 0.0, 4.0});
SCTEST_CHECK_EQ(p + q, expected);
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 q {2.0, -3.0, -4.0, -5.0};
SCTEST_CHECK_EQ(p.conjugate(), q);
SCTEST_CHECK_EQ(p.Conjugate(), q);
return true;
}
@ -49,8 +49,9 @@ Quaterniond_Conjugate()
static bool
Quaterniond_Euler()
{
geom::Quaterniond p = geom::quaterniond(geom::Vector3d{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI/4.0);
geom::Quaterniond q = geom::quaterniond_from_euler(p.euler());
geom::Quaterniond p = geom::MakeQuaternion(
geom::Vector3D{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI / 4.0);
geom::Quaterniond q = geom::QuaternionFromEuler(p.Euler());
SCTEST_CHECK_EQ(p, q);
@ -64,7 +65,7 @@ Quaterniond_Identity()
geom::Quaterniond p {3.0, 1.0, -2.0, 1.0};
geom::Quaterniond q;
SCTEST_CHECK(q.isIdentity());
SCTEST_CHECK(q.IsIdentity());
SCTEST_CHECK_EQ(p * q, p);
return true;
@ -77,7 +78,7 @@ Quaterniond_Inverse()
geom::Quaterniond p {2.0, 3.0, 4.0, 5.0};
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;
}
@ -89,7 +90,7 @@ Quaterniond_Norm()
geom::Quaterniond p {5.563199889674063, 0.9899139811480784, 9.387110042325054, 6.161341707794767};
double norm = 12.57016663729933;
SCTEST_CHECK_DEQ(p.norm(), norm);
SCTEST_CHECK_DEQ(p.Norm(), norm);
return true;
}
@ -111,27 +112,27 @@ Quaterniond_Product()
static bool
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
// If we assume a standard IMU frame of reference following the
// right hand rule:
// + The x axis points toward magnetic north
// + The y axis points toward magnentic west
// + The z axis points toward the sky
// + The x Axis points toward magnetic north
// + The y Axis points toward magnentic west
// + The z Axis points toward the sky
// 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 yAxis {0.0, 1.0, 0.0}; // a vector representing the y axis.
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.
double angle = M_PI / 2; // 90º rotation
// A quaternion representing a 90º rotation about the y axis.
geom::Quaterniond p = geom::quaterniond(yAxis, angle);
geom::Vector3d vr {0.0, 0.0, 1.0}; // expected rotated vector.
// A MakeQuaternion representing a 90º rotation about the y Axis.
geom::Quaterniond p = geom::MakeQuaternion(yAxis, angle);
geom::Vector3D vr {0.0, 0.0, 1.0}; // expected rotated vector.
// A rotation quaternion should be a unit quaternion.
SCTEST_CHECK(p.isUnitQuaternion());
SCTEST_CHECK_EQ(p.rotate(v), vr);
// A rotation quaternion should be a unit MakeQuaternion.
SCTEST_CHECK(p.IsUnitQuaternion());
SCTEST_CHECK_EQ(p.Rotate(v), vr);
return true;
}
@ -141,13 +142,13 @@ static bool
Quaterniond_ShortestSLERP()
{
// 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};
// 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};
// The halfway point should be oriented midway about the X axis. It turns
// out this is an identity quaternion.
// The halfway point should be oriented midway about the X Axis. It turns
// out this is an identity MakeQuaternion.
geom::Quaterniond r;
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};
SCTEST_CHECK(q.isUnitQuaternion());
SCTEST_CHECK(q.IsUnitQuaternion());
return true;
}
@ -194,9 +195,9 @@ Quaterniond_Unit()
static bool
Quaterniond_UtilityCreator()
{
geom::Vector3d v {1.0, 1.0, 1.0};
geom::Vector3D v {1.0, 1.0, 1.0};
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};
SCTEST_CHECK_EQ(p, q);
@ -226,7 +227,7 @@ Quaternionf_Conjugate()
geom::Quaternionf p {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;
}
@ -235,8 +236,9 @@ Quaternionf_Conjugate()
static bool
Quaternionf_Euler()
{
geom::Quaternionf p = geom::quaternionf(geom::Vector3f{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI/4.0);
geom::Quaternionf q = geom::quaternionf_from_euler(p.euler());
geom::Quaternionf p = geom::MakeQuaternion(
geom::Vector3F{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI / 4.0);
geom::Quaternionf q = geom::QuaternionFromEuler(p.Euler());
SCTEST_CHECK_EQ(p, q);
@ -262,7 +264,7 @@ Quaternionf_Inverse()
geom::Quaternionf p {2.0, 3.0, 4.0, 5.0};
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;
}
@ -274,7 +276,7 @@ Quaternionf_Norm()
geom::Quaternionf p {0.9899139811480784, 9.387110042325054, 6.161341707794767, 5.563199889674063};
float norm = 12.57016663729933;
SCTEST_CHECK_FEQ(p.norm(), norm);
SCTEST_CHECK_FEQ(p.Norm(), norm);
return true;
}
@ -296,15 +298,15 @@ Quaternionf_Product()
static bool
Quaternionf_Rotate()
{
geom::Vector3f v {1.0, 0.0, 0.0};
geom::Vector3f yAxis {0.0, 1.0, 0.0};
geom::Vector3F v {1.0, 0.0, 0.0};
geom::Vector3F yAxis {0.0, 1.0, 0.0};
float angle = M_PI / 2;
geom::Quaternionf p = geom::quaternionf(yAxis, angle);
geom::Vector3f vr {0.0, 0.0, 1.0};
geom::Quaternionf p = geom::MakeQuaternion(yAxis, angle);
geom::Vector3F vr {0.0, 0.0, 1.0};
SCTEST_CHECK(p.isUnitQuaternion());
SCTEST_CHECK_EQ(p.rotate(v), vr);
SCTEST_CHECK(p.IsUnitQuaternion());
SCTEST_CHECK_EQ(p.Rotate(v), vr);
return true;
}
@ -314,13 +316,13 @@ static bool
Quaternionf_ShortestSLERP()
{
// 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};
// 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};
// The halfway point should be oriented midway about the X axis. It turns
// out this is an identity quaternion.
// The halfway point should be oriented midway about the X Axis. It turns
// out this is an identity MakeQuaternion.
geom::Quaternionf r;
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};
SCTEST_CHECK(q.isUnitQuaternion());
SCTEST_CHECK(q.IsUnitQuaternion());
return true;
}
@ -367,9 +369,9 @@ Quaternionf_Unit()
static bool
Quaternionf_UtilityCreator()
{
geom::Vector3f v {1.0, 1.0, 1.0};
geom::Vector3F v {1.0, 1.0, 1.0};
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};
SCTEST_CHECK_EQ(p, q);
@ -381,15 +383,15 @@ Quaternionf_UtilityCreator()
static bool
QuaternionMiscellaneous_SanityChecks()
{
geom::Vector4d q {4.0, 1.0, 2.0, 3.0};
geom::Vector3d v {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};
double w = 4.0;
geom::Quaterniond p(q);
geom::Quaterniond u = p.unitQuaternion();
geom::Quaterniond u = p.UnitQuaternion();
SCTEST_CHECK_EQ(p.axis(), v);
SCTEST_CHECK_DEQ(p.angle(), w);
SCTEST_CHECK(u.isUnitQuaternion());
SCTEST_CHECK_EQ(p.Axis(), v);
SCTEST_CHECK_DEQ(p.Angle(), w);
SCTEST_CHECK(u.IsUnitQuaternion());
return true;
}
@ -417,10 +419,10 @@ static bool
QuaternionMiscellanous_InitializerConstructor()
{
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_FEQ(p.norm(), (float)2.0);
SCTEST_CHECK_FEQ(p.Norm(), (float)2.0);
return true;
}
@ -449,7 +451,7 @@ main(int argc, char *argv[])
suite.Silence();
}
suite.AddTest("Quaternion_SelfTest", Quaternion_SelfTest);
suite.AddTest("QuaternionSelfTest", Quaternion_SelfTest);
suite.AddTest("QuaternionMiscellanous_InitializerConstructor",
QuaternionMiscellanous_InitializerConstructor);
suite.AddTest("QuaternionMiscellaneous_SanityChecks",

2
test/tlv.cc
View File

@ -40,7 +40,7 @@ tlvTestSuite(Arena &backend)
sctest::Assert(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.
std::cout << "\tFindTag 1" << "\n";
cursor = TLV::FindTag(backend, cursor, rec4);

240
test/vector.cc
View File

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