Document and refactor geom code, round 2.
- Doxygenate headers. - Rename to bring methods and functions in line with everything else.
This commit is contained in:
parent
6a421d6adf
commit
0c7fa41cc8
|
@ -174,7 +174,7 @@ public:
|
||||||
/// \note This must be explicitly called before calling any
|
/// \note This must be explicitly called before calling any
|
||||||
/// method which uses the filter's internal Δt.
|
/// method which uses the filter's internal Δt.
|
||||||
///
|
///
|
||||||
/// \param The time delta to use when no time delta is
|
/// \param newDeltaT The time delta to use when no time delta is
|
||||||
/// provided.
|
/// provided.
|
||||||
void
|
void
|
||||||
DeltaT(T newDeltaT)
|
DeltaT(T newDeltaT)
|
||||||
|
|
|
@ -39,8 +39,7 @@ class Point2D;
|
||||||
|
|
||||||
class Polar2D;
|
class Polar2D;
|
||||||
|
|
||||||
/// \brief Point2D is a logical grouping of a set of 2D cartesian
|
/// \brief Point2D is a cartesian (X,Y) pairing.
|
||||||
/// coordinates.
|
|
||||||
class Point2D : public Vector<int, 2> {
|
class Point2D : public Vector<int, 2> {
|
||||||
public:
|
public:
|
||||||
/// \brief A Point2D defaults to (0,0).
|
/// \brief A Point2D defaults to (0,0).
|
||||||
|
@ -97,37 +96,63 @@ public:
|
||||||
friend std::ostream &operator<<(std::ostream &outs, const Point2D &pt);
|
friend std::ostream &operator<<(std::ostream &outs, const Point2D &pt);
|
||||||
};
|
};
|
||||||
|
|
||||||
// A Polar2D is a 2D polar coordinate, specified in terms of the radius from
|
/// \brief Polar2D is a pairing of a radius r and angle θ from some
|
||||||
// some origin and the Angle from the positive X Axis of a cartesian coordinate
|
/// reference point; in this library, it is assumed to be the
|
||||||
// system.
|
/// Cartesian origin (0, 0).
|
||||||
class Polar2D : public Vector<double, 2> {
|
class Polar2D : public Vector<double, 2> {
|
||||||
public:
|
public:
|
||||||
// A Polar2D can be initialised as a zeroised polar coordinate, by specifying
|
/// A Polar2D can be initialised as a zeroised polar coordinate, by specifying
|
||||||
// the radius and Angle directly, or via conversion from a Point2D.
|
/// the radius and Angle directly, or via conversion from a Point2D.
|
||||||
Polar2D();
|
|
||||||
Polar2D(double _r, double _theta);
|
|
||||||
Polar2D(const Point2D &);
|
|
||||||
|
|
||||||
|
/// \brief Construct a zero polar coordinate.
|
||||||
|
Polar2D();
|
||||||
|
|
||||||
|
/// \brief Construct a polar coordinate from a radius and
|
||||||
|
/// angle.
|
||||||
|
///
|
||||||
|
/// \param _r A radius
|
||||||
|
/// \param _theta An angle
|
||||||
|
Polar2D(double _r, double _theta);
|
||||||
|
|
||||||
|
/// \brief Construct a polar coordinate from a point.
|
||||||
|
///
|
||||||
|
/// This construct uses the origin (0,0) as the reference point.
|
||||||
|
///
|
||||||
|
/// \param point A 2D Cartesian point.
|
||||||
|
Polar2D(const Point2D& point);
|
||||||
|
|
||||||
|
/// \brief Return the radius component of this coordinate.
|
||||||
double R() const;
|
double R() const;
|
||||||
|
|
||||||
|
/// \brief Set the radius component of this coordinate.
|
||||||
void R(const double _r);
|
void R(const double _r);
|
||||||
|
|
||||||
|
/// \brief Return the angle component of this coordinate.
|
||||||
double Theta() const;
|
double Theta() const;
|
||||||
|
|
||||||
|
/// \brief Set the angle component of this coordinate.
|
||||||
void Theta(const double _theta);
|
void Theta(const double _theta);
|
||||||
|
|
||||||
|
/// \brief Return the coordinate in string form.
|
||||||
std::string ToString();
|
std::string ToString();
|
||||||
void ToPoint(Point2D &);
|
|
||||||
|
|
||||||
// Rotate rotates the polar coordinate by the number of radians, storing the result
|
/// \brief Construct a Point2D representing this Polar2D.
|
||||||
// in the Polar2D argument.
|
void ToPoint(Point2D &point);
|
||||||
void Rotate(Polar2D &, double);
|
|
||||||
|
|
||||||
// RotateAround rotates this point about by theta radians, storing the rotated point
|
/// \brief Rotate polar coordinate by some angle.
|
||||||
// in result.
|
///
|
||||||
void RotateAround(const Point2D &other, Point2D &result, double tjeta);
|
/// \param rotated The rotated Polar2D will be stored in this
|
||||||
|
/// coordinate.
|
||||||
|
/// \param delta The angle to rotate by.
|
||||||
|
void Rotate(Polar2D &rotated, double delta);
|
||||||
|
|
||||||
|
/// \brief Rotate this polar coordinate around a 2D point.
|
||||||
|
///
|
||||||
|
/// \param other The reference point.
|
||||||
|
/// \param result The point where the result will stored.
|
||||||
|
/// \param delta The angle to rotate by.
|
||||||
|
void RotateAround(const Point2D &other, Point2D &result, double delta);
|
||||||
|
|
||||||
bool operator==(const Polar2D &) const;
|
|
||||||
bool operator!=(const Polar2D &rhs) const
|
|
||||||
{ return !(*this == rhs); }
|
|
||||||
friend std::ostream &operator<<(std::ostream &, const Polar2D &);
|
friend std::ostream &operator<<(std::ostream &, const Polar2D &);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -266,8 +266,8 @@ public:
|
||||||
/// Return the Euler angles for this MakeQuaternion as a vector of
|
/// Return the Euler angles for this MakeQuaternion as a vector of
|
||||||
/// <yaw, pitch, roll>.
|
/// <yaw, pitch, roll>.
|
||||||
///
|
///
|
||||||
/// \warn Users of this function should watch out for gimbal
|
/// \warning Users of this function should watch out for gimbal
|
||||||
/// lock.
|
/// lock.
|
||||||
///
|
///
|
||||||
/// \return A vector<T, 3> containing <yaw, pitch, roll>
|
/// \return A vector<T, 3> containing <yaw, pitch, roll>
|
||||||
Vector<T, 3>
|
Vector<T, 3>
|
||||||
|
@ -492,7 +492,7 @@ MakeQuaternion(Vector<T, 3> axis, T angle)
|
||||||
/// \return A Quaternion representation of the Orientation represented
|
/// \return A Quaternion representation of the Orientation represented
|
||||||
/// by the Euler angles.
|
/// by the Euler angles.
|
||||||
/// \relatesalso Quaternion
|
/// \relatesalso Quaternion
|
||||||
Quaternionf QuaternionFromEuler(Vector3F euler);
|
Quaternionf FloatQuaternionFromEuler(Vector3F euler);
|
||||||
|
|
||||||
|
|
||||||
/// \brief COnstruct a Quaternion from Euler angles.
|
/// \brief COnstruct a Quaternion from Euler angles.
|
||||||
|
@ -504,7 +504,7 @@ Quaternionf QuaternionFromEuler(Vector3F euler);
|
||||||
/// \return A Quaternion representation of the Orientation represented
|
/// \return A Quaternion representation of the Orientation represented
|
||||||
/// by the Euler angles.
|
/// by the Euler angles.
|
||||||
/// \relatesalso Quaternion
|
/// \relatesalso Quaternion
|
||||||
Quaterniond QuaternionFromEuler(Vector3D euler);
|
Quaterniond DoubleQuaternionFromEuler(Vector3D euler);
|
||||||
|
|
||||||
|
|
||||||
/// \brief Linear interpolation for two Quaternions.
|
/// \brief Linear interpolation for two Quaternions.
|
||||||
|
|
|
@ -1,5 +1,5 @@
|
||||||
///
|
///
|
||||||
/// \file Flag.h
|
/// \file include/scsl/Flags.h
|
||||||
/// \author K. Isom <kyle@imap.cc>
|
/// \author K. Isom <kyle@imap.cc>
|
||||||
/// \date 2023-10-12
|
/// \date 2023-10-12
|
||||||
/// \brief Flag declares a command-line flag parser.
|
/// \brief Flag declares a command-line flag parser.
|
||||||
|
|
|
@ -89,7 +89,7 @@ std::vector<std::string> SplitKeyValuePair(std::string line, char delimiter);
|
||||||
/// \param maxCount The maximum number of parts to split. If 0, there is no
|
/// \param maxCount The maximum number of parts to split. If 0, there is no
|
||||||
/// limit to the number of parts.
|
/// limit to the number of parts.
|
||||||
/// \return A vector containing all the parts of the string.
|
/// \return A vector containing all the parts of the string.
|
||||||
std::vector<std::string> SplitN(std::string, std::string delimiter, size_t maxCount=0);
|
std::vector<std::string> SplitN(std::string s, std::string delimiter, size_t maxCount=0);
|
||||||
|
|
||||||
/// WrapText is a very simple wrapping function that breaks the line into
|
/// WrapText is a very simple wrapping function that breaks the line into
|
||||||
/// lines of At most lineLength characters. It does this by breaking the
|
/// lines of At most lineLength characters. It does this by breaking the
|
||||||
|
|
|
@ -155,9 +155,9 @@ Polar2D::Polar2D() : Vector<double, 2>{0.0, 0.0} {};
|
||||||
Polar2D::Polar2D(double _r, double _theta) : Vector<double, 2>{_r, _theta}
|
Polar2D::Polar2D(double _r, double _theta) : Vector<double, 2>{_r, _theta}
|
||||||
{}
|
{}
|
||||||
|
|
||||||
Polar2D::Polar2D(const Point2D &pt)
|
Polar2D::Polar2D(const Point2D &point)
|
||||||
: Vector<double, 2>{std::sqrt((pt.X() * pt.X()) + (pt.Y() * pt.Y())),
|
: Vector<double, 2>{std::sqrt((point.X() * point.X()) + (point.Y() * point.Y())),
|
||||||
std::atan2(pt.Y(), pt.X())}
|
std::atan2(point.Y(), point.X())}
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
|
||||||
|
@ -190,10 +190,10 @@ Polar2D::Theta(const double _theta)
|
||||||
|
|
||||||
|
|
||||||
void
|
void
|
||||||
Polar2D::ToPoint(Point2D &pt)
|
Polar2D::ToPoint(Point2D &point)
|
||||||
{
|
{
|
||||||
pt.Y(std::rint(std::sin(this->Theta()) * this->R()));
|
point.Y(std::rint(std::sin(this->Theta()) * this->R()));
|
||||||
pt.X(std::rint(std::cos(this->Theta()) * this->R()));
|
point.X(std::rint(std::cos(this->Theta()) * this->R()));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -206,22 +206,10 @@ Polar2D::ToString()
|
||||||
|
|
||||||
|
|
||||||
void
|
void
|
||||||
Polar2D::Rotate(Polar2D &rot, double delta)
|
Polar2D::Rotate(Polar2D &rotated, double delta)
|
||||||
{
|
{
|
||||||
rot.R(this->R());
|
rotated.R(this->R());
|
||||||
rot.Theta(RotateRadians(this->Theta(), delta));
|
rotated.Theta(RotateRadians(this->Theta(), delta));
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
bool
|
|
||||||
Polar2D::operator==(const Polar2D &rhs) const
|
|
||||||
{
|
|
||||||
static double eps = 0.0;
|
|
||||||
if (eps == 0.0) {
|
|
||||||
scmp::DefaultEpsilon(eps);
|
|
||||||
}
|
|
||||||
return scmp::WithinTolerance(this->R(), rhs.R(), eps) &&
|
|
||||||
scmp::WithinTolerance(this->Theta(), rhs.Theta(), eps);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -24,7 +24,7 @@ MakeQuaternion(Vector3D axis, double angle)
|
||||||
|
|
||||||
|
|
||||||
Quaternionf
|
Quaternionf
|
||||||
QuaternionFromEuler(Vector3F euler)
|
FloatQuaternionFromEuler(Vector3F euler)
|
||||||
{
|
{
|
||||||
float x, y, z, w;
|
float x, y, z, w;
|
||||||
euler = euler / 2.0;
|
euler = euler / 2.0;
|
||||||
|
@ -46,7 +46,7 @@ QuaternionFromEuler(Vector3F euler)
|
||||||
|
|
||||||
|
|
||||||
Quaterniond
|
Quaterniond
|
||||||
QuaternionFromEuler(Vector3D euler)
|
DoubleQuaternionFromEuler(Vector3D euler)
|
||||||
{
|
{
|
||||||
double x, y, z, w;
|
double x, y, z, w;
|
||||||
euler = euler / 2.0;
|
euler = euler / 2.0;
|
||||||
|
|
|
@ -1,5 +1,5 @@
|
||||||
///
|
///
|
||||||
/// \file Flag.cc
|
/// \file src/sl/Flags.cc
|
||||||
/// \author K. Isom <kyle@imap.cc>
|
/// \author K. Isom <kyle@imap.cc>
|
||||||
/// \date 2023-10-12
|
/// \date 2023-10-12
|
||||||
/// \brief Flag defines a command-line flag parser.
|
/// \brief Flag defines a command-line flag parser.
|
||||||
|
|
|
@ -153,7 +153,7 @@ SimpleAngularOrientation2InitialVector3f()
|
||||||
bool
|
bool
|
||||||
SimpleAngularOrientation2InitialQuaternionf()
|
SimpleAngularOrientation2InitialQuaternionf()
|
||||||
{
|
{
|
||||||
const auto initialFrame = geom::QuaternionFromEuler({0, 0, 0});
|
const auto initialFrame = geom::FloatQuaternionFromEuler({0, 0, 0});
|
||||||
filter::Madgwickf mflt(initialFrame);
|
filter::Madgwickf mflt(initialFrame);
|
||||||
const geom::Vector3F gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
const geom::Vector3F gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
||||||
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
|
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
|
||||||
|
@ -207,7 +207,7 @@ SimpleAngularOrientation2InitialVector3d()
|
||||||
bool
|
bool
|
||||||
SimpleAngularOrientation2InitialQuaterniond()
|
SimpleAngularOrientation2InitialQuaterniond()
|
||||||
{
|
{
|
||||||
const auto initialFrame = geom::QuaternionFromEuler({0, 0, 0});
|
const auto initialFrame = geom::DoubleQuaternionFromEuler({0, 0, 0});
|
||||||
filter::Madgwickd mflt(initialFrame);
|
filter::Madgwickd mflt(initialFrame);
|
||||||
const geom::Vector3D gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
const geom::Vector3D gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
||||||
const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
|
const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
|
||||||
|
|
|
@ -51,7 +51,7 @@ Quaterniond_Euler()
|
||||||
{
|
{
|
||||||
geom::Quaterniond p = geom::MakeQuaternion(
|
geom::Quaterniond p = geom::MakeQuaternion(
|
||||||
geom::Vector3D{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI / 4.0);
|
geom::Vector3D{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI / 4.0);
|
||||||
geom::Quaterniond q = geom::QuaternionFromEuler(p.Euler());
|
geom::Quaterniond q = geom::DoubleQuaternionFromEuler(p.Euler());
|
||||||
|
|
||||||
SCTEST_CHECK_EQ(p, q);
|
SCTEST_CHECK_EQ(p, q);
|
||||||
|
|
||||||
|
@ -238,7 +238,7 @@ Quaternionf_Euler()
|
||||||
{
|
{
|
||||||
geom::Quaternionf p = geom::MakeQuaternion(
|
geom::Quaternionf p = geom::MakeQuaternion(
|
||||||
geom::Vector3F{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI / 4.0);
|
geom::Vector3F{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI / 4.0);
|
||||||
geom::Quaternionf q = geom::QuaternionFromEuler(p.Euler());
|
geom::Quaternionf q = geom::FloatQuaternionFromEuler(p.Euler());
|
||||||
|
|
||||||
SCTEST_CHECK_EQ(p, q);
|
SCTEST_CHECK_EQ(p, q);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue