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

Restructure project, start importing sc3 code.

Browse Source
This commit is contained in:
Kyle Isom
2023-10-18 23:44:05 -07:00
parent 3122ed6ac7
commit 5f3dc6e9f6
46 changed files with 2300 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
include/scmp/geom/coord2d.h Executable file
View File

@@ -0,0 +1,108 @@
/// coord2d.h defines 2D point and polar coordinate systems.
//
// Project: scccl
// File: include/math/coord2d.h
// Author: Kyle Isom
// Date: 2017-06-05
// Namespace: math::geom
//
// coord2d.h defines 2D coordinate classes and functions.
//
// 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_GEOM_COORD2D_H
#define SCMATH_GEOM_COORD2D_H
#include <cmath>
#include <ostream>
#include <vector>
namespace scmath {
namespace geom {
class Point2D;
class Polar2D;
// Point2D is a logical grouping of a set of 2D cartesian coordinates.
class Point2D {
public:
int x, y;
// A Point2D can be initialised by setting its members to 0, by providing the
// x and y coordiantes, or through translation from a polar coordinate.
Point2D() : x(0), y(0) {}
Point2D(int _x, int _y) : x(_x), y(_y) {}
Point2D(const Polar2D&);
std::string ToString(void);
void ToPolar(Polar2D&);
// Rotate rotates the point by theta radians. Alternatively, a rotation
// can use this point as the centre, with a polar coordinate and a rotation
// amount (in radians). The latter is used to specify a central point
// of rotation with vertices specified as polar coordinates from the centre.
// Both forms take a reference to a Point2D to store the rotated point.
void Rotate(Point2D& rotated, double theta);
std::vector<Point2D> Rotate(std::vector<Polar2D>, double);
// Translate adds this point to the first argument, storing the result in the
// second argument.
void Translate(const Point2D& other, Point2D& translated);
// Distance returns the distance from this point to another.
int Distance(const Point2D& other);
Point2D operator+(const Point2D &rhs) const { return Point2D(x + rhs.x, y + rhs.y); }
Point2D operator-(const Point2D &rhs) const { return Point2D(x - rhs.x, y - rhs.y); }
Point2D operator*(const int k) const { return Point2D(x * k, y * k); }
bool operator==(const Point2D& rhs) const;
bool operator!=(const Point2D& rhs) const { return !(*this == rhs); }
friend std::ostream& operator<<(std::ostream& outs, const Point2D& pt);
};
// A Polar2D is a 2D polar coordinate, specified in terms of the radius from
// some origin and the angle from the positive X axis of a cartesian coordinate
// system.
class Polar2D {
public:
double r, theta;
// A Polar2D can be initialised as a zeroised polar coordinate, by specifying
// the radius and angle directly, or via conversion from a Point2D.
Polar2D() : r(0.0), theta(0.0) {}
Polar2D(double _r, double _theta) : r(_r), theta(_theta) {}
Polar2D(const Point2D&);
std::string ToString();
void ToPoint(Point2D&);
// Rotate rotates the polar coordinate by the number of radians, storing the result
// in the Polar2D argument.
void Rotate(Polar2D&, double);
// RotateAround rotates this point about by theta radians, storing the rotated point
// in result.
void RotateAround(const Point2D& other, Point2D& result, double tjeta);
bool operator==(const Polar2D&) const;
bool operator!=(const Polar2D& rhs) const { return !(*this == rhs); }
friend std::ostream& operator<<(std::ostream&, const Polar2D&);
};
} // end namespace geom
} // end namespace math
#endif

88
include/scmp/geom/orientation.h Normal file
View File

@@ -0,0 +1,88 @@
/**
* 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.
*/
#ifndef SCMATH_GEOM_ORIENTATION_H
#define SCMATH_GEOM_ORIENTATION_H
namespace scmath {
namespace geom {
/// \defgroup basis Basis vector indices.
/// The following constants are provided as a convenience for indexing two-
/// and three-dimensional vectors.
/// \ingroup basis
/// Convenience constant for the x index.
constexpr uint8_t Basis_x = 0;
/// \ingroup basis
/// Convenience constant for the y index.
constexpr uint8_t Basis_y = 1;
/// \ingroup basis
/// Convenience constant for the z index.
constexpr uint8_t Basis_z = 2;
/// @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 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},
};
/// 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);
/// 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);
/// 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);
/// 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 math
#endif // __WRMATH_ORIENTATION_H

520
include/scmp/geom/quaternion.h Normal file
View File

@@ -0,0 +1,520 @@
/// quaternion.h contains an implementation of quaternions suitable
/// for navigation in R3.
#ifndef SCMATH_QUATERNION_H
#define SCMATH_QUATERNION_H
#include <cassert>
#include <cmath>
#include <initializer_list>
#include <iostream>
#include <ostream>
#include <scccl/math/math.h>
#include <scccl/math/geom/vector.h>
/// math contains the shimmering clarity math library.
namespace scmath {
/// geom contains geometric classes and functions.
namespace geom {
/// @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.
///
/// For information on the underlying vector type, see the documentation for
/// wr::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.
template<typename T>
class Quaternion {
public:
/// The default Quaternion constructor returns an identity quaternion.
Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0)
{
scmath::DefaultEpsilon(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.
///
/// @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();
scmath::DefaultEpsilon(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.
///
/// @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();
scmath::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps);
}
/// A Quaternion may be constructed with an initializer list of
/// type T, which must have exactly N elements.
///
/// @param ilst An initial set of values in the form <w, x, y, z>.
Quaternion(std::initializer_list<T> ilst)
{
auto it = ilst.begin();
this->v = Vector<T, 3>{it[1], it[2], it[3]};
this->w = it[0];
this->constrainAngle();
scmath::DefaultEpsilon(this->eps);
v.setEpsilon(this->eps);
}
/// Set the comparison tolerance for this quaternion.
///
/// @param epsilon A tolerance value.
void
setEpsilon(T epsilon)
{
this->eps = epsilon;
this->v.setEpsilon(epsilon);
}
/// Return the axis of rotation of this quaternion.
///
/// @return The axis of rotation of this quaternion.
Vector<T, 3>
axis() const
{
return this->v;
}
/// Return the angle of rotation of this quaternion.
///
/// @return the angle of rotation of this quaternion.
T
angle() const
{
return this->w;
}
/// Compute the dot product of two quaternions.
///
/// \param other Another quaternion.
/// \return The dot product between the two quaternions.
T
dot(const Quaternion<T> &other) const
{
double innerProduct = this->v[0] * other.v[0];
innerProduct += (this->v[1] * other.v[1]);
innerProduct += (this->v[2] * other.v[2]);
innerProduct += (this->w * other.w);
return innerProduct;
}
/// Compute the norm of a quaternion. Treating the Quaternion as a
/// Vector<T, 4>, it's the same as computing the magnitude.
///
/// @return A non-negative real number.
T
norm() const
{
T n = 0;
n += (this->v[0] * this->v[0]);
n += (this->v[1] * this->v[1]);
n += (this->v[2] * this->v[2]);
n += (this->w * this->w);
return std::sqrt(n);
}
/// Return the unit quaternion.
///
/// \return The unit quaternion.
Quaternion
unitQuaternion()
{
return *this / this->norm();
}
/// Compute the conjugate of a quaternion.
///
/// @return The conjugate of this quaternion.
Quaternion
conjugate() const
{
return Quaternion(Vector<T, 4>{this->w, -this->v[0], -this->v[1], -this->v[2]});
}
/// Compute the inverse of a quaternion.
///
/// @return The inverse of this quaternion.
Quaternion
inverse() const
{
T _norm = this->norm();
return this->conjugate() / (_norm * _norm);
}
/// Determine whether this is an identity quaternion.
///
/// \return true if this is an identity quaternion.
bool
isIdentity() const {
return this->v.isZero() &&
scmath::WithinTolerance(this->w, (T)1.0, this->eps);
}
/// Determine whether this is a unit quaternion.
///
/// @return true if this is a unit quaternion.
bool
isUnitQuaternion() const
{
return scmath::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.
///
/// @return A vector representation of the quaternion.
Vector<T, 4>
asVector() const
{
return Vector<T, 4>{this->w, this->v[0], this->v[1], this->v[2]};
}
/// Rotate vector vr about this quaternion.
///
/// @param vr The vector to be rotated.
/// @return The rotated vector.
Vector<T, 3>
rotate(Vector<T, 3> vr) const
{
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.
///
/// @return A vector<T, 3> containing <yaw, pitch, roll>
Vector<T, 3>
euler() const
{
T yaw, pitch, roll;
T a = this->w, a2 = a * a;
T b = this->v[0], b2 = b * b;
T c = this->v[1], c2 = c * c;
T d = this->v[2], d2 = d * d;
yaw = std::atan2(2 * ((a * b) + (c * d)), a2 - b2 - c2 + d2);
pitch = std::asin(2 * ((b * d) - (a * c)));
roll = std::atan2(2 * ((a * d) + (b * c)), a2 + b2 - c2 - d2);
return Vector<T, 3>{yaw, pitch, roll};
}
/// Perform quaternion addition with another quaternion.
///
/// @param other The quaternion to be added with this one.
/// @return The result of adding the two quaternions together.
Quaternion
operator+(const Quaternion<T> &other) const
{
return Quaternion(this->v + other.v, this->w + other.w);
}
/// Perform quaternion subtraction with another quaternion.
///
/// @param other The quaternion to be subtracted from this one.
/// @return The result of subtracting the other quaternion from this one.
Quaternion
operator-(const Quaternion<T> &other) const
{
return Quaternion(this->v - other.v, this->w - other.w);
}
/// Perform scalar multiplication.
///
/// @param k The scaling value.
/// @return A scaled quaternion.
Quaternion
operator*(const T k) const
{
return Quaternion(this->v * k, this->w * k);
}
/// Perform scalar division.
///
/// @param k The scalar divisor.
/// @return A scaled quaternion.
Quaternion
operator/(const T k) const
{
return Quaternion(this->v / k, this->w / k);
}
/// 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).
///
/// @param vector The vector to multiply with this quaternion.
/// @return The Hamilton product of the quaternion and vector.
Quaternion
operator*(const Vector<T, 3> &vector) const
{
return Quaternion(vector * this->w + this->v.cross(vector),
(T) 0.0);
}
/// Perform quaternion Hamilton multiplication.
///
/// @param other The other quaternion to multiply with this one.
/// @result The Hamilton product of the two quaternions.
Quaternion
operator*(const Quaternion<T> &other) const
{
T angle = (this->w * other.w) -
(this->v * other.v);
Vector<T, 3> axis = (other.v * this->w) +
(this->v * other.w) +
(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.
bool
operator==(const Quaternion<T> &other) const
{
return (this->v == other.v) &&
(scmath::WithinTolerance(this->w, other.w, this->eps));
}
/// Perform quaternion inequality checking.
///
/// @param other The quaternion to check inequality against.
/// @return True if the two quaternions are unequal within their tolerance.
bool
operator!=(const Quaternion<T> &other) const
{
return !(*this == other);
}
/// Support stream output of a quaternion in the form `a + <i, j, k>`.
/// \todo improve the formatting.
///
/// @param outs An output stream
/// @param q A quaternion
/// @return The output stream
friend std::ostream &
operator<<(std::ostream &outs, const Quaternion<T> &q)
{
outs << q.w << " + " << q.v;
return outs;
}
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
T eps;
void
constrainAngle()
{
if (this->w < 0.0) {
this->w = std::fmod(this->w, this->minRotation);
}
else {
this->w = std::fmod(this->w, this->maxRotation);
}
}
};
///
/// \defgroup quaternion_aliases Quaternion type aliases.
/// Type aliases are provided for float and double quaternions.
///
/// \ingroup quaternion_aliases
/// Type alias for a float Quaternion.
typedef Quaternion<float> Quaternionf;
/// \ingroup quaternion_aliases
/// 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).
///
/// @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).
///
/// @param axis The axis of rotation.
/// @param angle The angle of rotation.
/// @return A quaternion.
/// @relatesalso Quaternion
Quaterniond quaterniond(Vector3d axis, double angle);
/// Return a double quaternion scaled appropriately from a vector and angle,
/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
///
/// @param axis The axis of rotation.
/// @param angle The angle of rotation.
/// @return A quaternion.
/// @relatesalso Quaternion
template <typename T>
Quaternion<T>
quaternion(Vector<T, 3> axis, T angle)
{
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.
///
/// @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.
///
/// @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);
/// 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 t The fraction of the distance between the two quaternions to
/// interpolate.
/// \return A Quaternion representing the linear interpolation of the
/// two quaternions.
template <typename T>
Quaternion<T>
LERP(Quaternion<T> p, Quaternion<T> q, T t)
{
return (p + (q - p) * t).unitQuaternion();
}
/// ShortestSLERP computes the shortest distance spherical linear
/// interpolation between two quaternions at some fraction of the
/// distance between them.
///
/// \tparam T
/// \param p The starting quaternion.
/// \param q The ending quaternion.Short
/// \param t The fraction of the distance between the two quaternions
/// to interpolate.
/// \return A Quaternion representing the shortest path between two
/// quaternions.
template <typename T>
Quaternion<T>
ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t)
{
assert(p.isUnitQuaternion());
assert(q.isUnitQuaternion());
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.
if (dp > 0.99999) {
return LERP(p, q * sign, t);
}
return (p * std::sin((1.0 - t) * omega) / sin_omega) +
(q * sign * std::sin(omega*t) / sin_omega);
}
/// 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();
} // namespace geom
} // namespace wr
#endif // WRMATH_QUATERNION_H

422
include/scmp/geom/vector.h Normal file
View File

@@ -0,0 +1,422 @@
//
// 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 math::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
#include <array>
#include <cassert>
#include <cmath>
#include <initializer_list>
#include <ostream>
#include <iostream>
#include <scccl/math/math.h>
// This implementation is essentially a C++ translation of a Python library
// I wrote for Coursera's "Linear Algebra for Machine Learning" course. Many
// of the test vectors come from quiz questions in the class.
namespace scmath {
namespace geom {
/// @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.
///
/// 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.
Vector()
{
T unitLength = (T)1.0 / std::sqrt(N);
for (size_t i = 0; i < N; i++) {
this->arr[i] = unitLength;
}
scmath::DefaultEpsilon(this->epsilon);
}
/// If given an initializer_list, the vector is created with
/// those values. There must be exactly N elements in the list.
/// @param ilst An intializer list with N elements of type T.
Vector(std::initializer_list<T> ilst)
{
assert(ilst.size() == N);
scmath::DefaultEpsilon(this->epsilon);
std::copy(ilst.begin(), ilst.end(), this->arr.begin());
}
/// Compute the length of the vector.
/// @return The length of the vector.
T magnitude() const {
T result = 0;
for (size_t i = 0; i < N; i++) {
result += (this->arr[i] * this->arr[i]);
}
return std::sqrt(result);
}
/// 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
/// another.
void
setEpsilon(T eps)
{
this->epsilon = eps;
}
/// Determine whether this is a zero vector.
/// @return true if the vector is zero.
bool
isZero() const
{
for (size_t i = 0; i < N; i++) {
if (!scmath::WithinTolerance(this->arr[i], (T)0.0, this->epsilon)) {
return false;
}
}
return true;
}
/// Obtain the unit vector for this vector.
/// @return The unit vector
Vector
unitVector() const
{
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.
bool
isUnitVector() const
{
return scmath::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.
T
angle(const Vector<T, N> &other) const
{
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);
}
/// 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
{
if (this->isZero() || other.isZero()) {
return true;
}
T angle = this->angle(other);
if (scmath::WithinTolerance(angle, (T)0.0, this->epsilon)) {
return true;
}
return false;
}
/// 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
{
if (this->isZero() || other.isZero()) {
return true;
}
return scmath::WithinTolerance(*this * other, (T)0.0, this->epsilon);
}
/// 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
{
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
/// the basis vector.
Vector
projectOrthogonal(const Vector<T, N> &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.
Vector
cross(const Vector<T, N> &other) const
{
assert(N == 3);
return Vector<T, N> {
(this->arr[1] * other.arr[2]) - (other.arr[1] * this->arr[2]),
-((this->arr[0] * other.arr[2]) - (other.arr[0] * this->arr[2])),
(this->arr[0] * other.arr[1]) - (other.arr[0] * this->arr[1])
};
}
/// 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
/// other vector.
Vector
operator+(const Vector<T, N> &other) const
{
Vector<T, N> vec;
for (size_t i = 0; i < N; i++) {
vec.arr[i] = this->arr[i] + other.arr[i];
}
return vec;
}
/// 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
/// other vector from this one.
Vector
operator-(const Vector<T, N> &other) const
{
Vector<T, N> vec;
for (size_t i = 0; i < N; i++) {
vec.arr[i] = this->arr[i] - other.arr[i];
}
return vec;
}
/// 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.
Vector
operator*(const T k) const
{
Vector<T, N> vec;
for (size_t i = 0; i < N; i++) {
vec.arr[i] = this->arr[i] * k;
}
return vec;
}
/// 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.
Vector
operator/(const T k) const
{
Vector<T, N> vec;
for (size_t i = 0; i < N; i++) {
vec.arr[i] = this->arr[i] / k;
}
return vec;
}
/// 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
{
T result = 0;
for (size_t i = 0; i < N; i++) {
result += (this->arr[i] * other.arr[i]);
}
return result;
}
/// Compare two vectors for equality.
/// @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
{
for (size_t i = 0; i<N; i++) {
if (!scmath::WithinTolerance(this->arr[i], other.arr[i], this->epsilon)) {
return false;
}
}
return true;
}
/// Compare two vectors for inequality.
/// @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
{
return !(*this == other);
}
/// Support array indexing into vector.
///
/// Note that the values of the vector cannot be modified. Instead,
/// it's required to do something like the following:
///
/// ```
/// 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.
const T&
operator[](size_t i) const
{
return this->arr[i];
}
/// Support outputting vectors 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)
{
outs << "<";
for (size_t i = 0; i < N; i++) {
outs << vec.arr[i];
if (i < (N-1)) {
outs << ", ";
}
}
outs << ">";
return outs;
}
private:
static const size_t dim = N;
T epsilon;
std::array<T, N> arr;
};
///
/// \defgroup vector_aliases Vector type aliases.
///
/// \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.
/// \ingroup vector_aliases
/// @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;
/// \ingroup vector_aliases
/// @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;
/// \ingroup vector_aliases
/// @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;
} // namespace geom
} // namespace math
#endif // SCMATH_VECTORS_H_H

77
include/scmp/math.h Normal file
View File

@@ -0,0 +1,77 @@
/// math.h provides certain useful mathematical functions.
#ifndef SCCCL_MATH_H
#define SCCCL_MATH_H
#include <cmath>
#include <vector>
namespace scmath {
// MAX_RADIAN is a precomputed 2 * M_PI, and MIN_RADIAN is -2 * M_PI.
constexpr double MAX_RADIAN = 2 * M_PI;
constexpr double MIN_RADIAN = -2 * M_PI;
constexpr double POS_HALF_RADIAN = M_PI / 2;
constexpr double NEG_HALF_RADIAN = -(M_PI / 2);
/// Roll m die of n sides, returning a vector of the dice.
std::vector<int> Die(int m, int n);
/// Roll m die of n sides, returning the total of the die.
int DieTotal(int m, int n);
/// Roll m die of n sides, and take the total of the top k die.
int BestDie(int k, int m, int n);
/// Convert radians to degrees.
/// @param rads the angle in radians
/// @return the angle in degrees.
float RadiansToDegreesF(float rads);
/// Convert radians to degrees.
/// @param rads the angle in radians
/// @return the angle in degrees.
double RadiansToDegreesD(double rads);
/// Convert degrees to radians.
/// @param degrees the angle in degrees
/// @return the angle in radians.
float DegreesToRadiansF(float degrees);
/// Convert degrees to radians.
/// @param degrees the angle in degrees
/// @return the angle in radians.
double DegreesToRadiansD(double degrees);
/// RotateRadians rotates theta0 by theta1 radians, wrapping the result to
/// MIN_RADIAN <= result <= MAX_RADIAN.
double RotateRadians(double theta0, double theta1);
/// Get the default epsilon value.
/// @param epsilon The variable to store the epsilon value in.
void DefaultEpsilon(double &epsilon);
/// Get the default epsilon value.
/// @param epsilon The variable to store the epsilon value in.
void DefaultEpsilon(float &epsilon);
/// Return whether the two values of type T are equal to within some tolerance.
/// @tparam T The type of value
/// @param a A value of type T used as the left-hand side of an equality check.
/// @param b A value of type T used as the right-hand side of an equality check.
/// @param epsilon The tolerance value.
/// @return Whether the two values are "close enough" to be considered equal.
template <typename T>
static T
WithinTolerance(T a, T b, T epsilon)
{
return std::abs(a - b) < epsilon;
}
} // namespace math
#endif //SCCCL_MATH_H

21
include/scmp/motion2d.h Normal file
View File

@@ -0,0 +1,21 @@
//
// Created by Kyle Isom on 2/21/20.
//
#ifndef SCCCL_MOTION2D_H
#define SCCCL_MOTION2D_H
#include <scccl/math/geom/vector.h>
namespace scphys {
namespace basic {
scmath::geom::Vector2d Acceleration(double speed, double heading);
} // namespace basic
} // namespace phsyics
#endif //SCCCL_MOTION2D_H