Document and refactor geom code.
- Doxygenate headers. - Rename to bring methods and functions in line with everything else.
This commit is contained in:
parent
4b1007123a
commit
6a421d6adf
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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,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>
|
||||
///
|
||||
|
|
|
@ -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 &);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
//
|
||||
|
|
|
@ -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; }
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -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});
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -20,8 +20,8 @@ using namespace scmp;
|
|||
bool
|
||||
SimpleAngularOrientationFloat()
|
||||
{
|
||||
filter::Madgwickf mflt;
|
||||
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
||||
filter::Madgwickf mflt;
|
||||
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);
|
||||
|
@ -47,8 +47,8 @@ SimpleAngularOrientationFloat()
|
|||
bool
|
||||
SimpleAngularOrientationFloatDefaultDT()
|
||||
{
|
||||
filter::Madgwickf mflt;
|
||||
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
||||
filter::Madgwickf mflt;
|
||||
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);
|
||||
|
@ -75,8 +75,8 @@ SimpleAngularOrientationFloatDefaultDT()
|
|||
bool
|
||||
VerifyUpdateWithZeroDeltaTFails()
|
||||
{
|
||||
filter::Madgwickf mflt;
|
||||
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
||||
filter::Madgwickf mflt;
|
||||
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);
|
||||
|
||||
|
@ -100,8 +100,8 @@ VerifyUpdateWithZeroDeltaTFails()
|
|||
bool
|
||||
SimpleAngularOrientationDouble()
|
||||
{
|
||||
filter::Madgwickd mflt;
|
||||
const geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
||||
filter::Madgwickd mflt;
|
||||
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});
|
||||
filter::Madgwickd mflt(initialFrame);
|
||||
const geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
||||
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::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;
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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.
|
||||
double angle = M_PI / 2; // 90º rotation
|
||||
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};
|
||||
double w = M_PI;
|
||||
geom::Quaterniond p = geom::quaterniond(v, w);
|
||||
geom::Vector3D v {1.0, 1.0, 1.0};
|
||||
double w = M_PI;
|
||||
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};
|
||||
float angle = M_PI / 2;
|
||||
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};
|
||||
float w = M_PI;
|
||||
geom::Quaternionf p = geom::quaternionf(v, w);
|
||||
geom::Vector3F v {1.0, 1.0, 1.0};
|
||||
float w = M_PI;
|
||||
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};
|
||||
double w = 4.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",
|
||||
|
|
|
@ -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);
|
||||
|
|
248
test/vector.cc
248
test/vector.cc
|
@ -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,8 +37,8 @@ using namespace std;
|
|||
static bool
|
||||
Vector3Miscellaneous_ExtractionOperator3d()
|
||||
{
|
||||
geom::Vector3d vec {1.0, 2.0, 3.0};
|
||||
stringstream vecBuffer;
|
||||
geom::Vector3D vec {1.0, 2.0, 3.0};
|
||||
stringstream vecBuffer;
|
||||
|
||||
vecBuffer << vec;
|
||||
SCTEST_CHECK_EQ(vecBuffer.str(), "<1, 2, 3>");
|
||||
|
@ -49,8 +49,8 @@ Vector3Miscellaneous_ExtractionOperator3d()
|
|||
static bool
|
||||
Vector3Miscellaneous_ExtractionOperator3f()
|
||||
{
|
||||
geom::Vector3f vec {1.0, 2.0, 3.0};
|
||||
stringstream vecBuffer;
|
||||
geom::Vector3F vec {1.0, 2.0, 3.0};
|
||||
stringstream vecBuffer;
|
||||
|
||||
vecBuffer << vec;
|
||||
SCTEST_CHECK_EQ(vecBuffer.str(), "<1, 2, 3>");
|
||||
|
@ -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};
|
||||
const float expected = 3.74165738677394;
|
||||
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};
|
||||
const double expected = 3.74165738677394;
|
||||
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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue