LinearAlgebra/LinearAlgebra-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: LinearAlgebra:Experimental
Branches Tags
LinearAlgebra:main
LinearAlgebra:ESP-IDF
LinearAlgebra:Experimental
LinearAlgebra:DiscreteAngle
LinearAlgebra:merged-includes
LinearAlgebra:0.3-beta
LinearAlgebra:0.2.0
LinearAlgebra:0.1.0
..
compare: LinearAlgebra:main
Branches Tags
LinearAlgebra:ESP-IDF
LinearAlgebra:main
LinearAlgebra:Experimental
LinearAlgebra:DiscreteAngle
LinearAlgebra:merged-includes
LinearAlgebra:0.3-beta
LinearAlgebra:0.2.0
LinearAlgebra:0.1.0

1 Commits
Experiment ... main

Author SHA1 Message Date
Pascal Serrarens
54634f0582 Backporting some style changes from Python 2025-03-31 08:45:39 +02:00
21 changed files with 427 additions and 527 deletions
Show Stats Expand all files Collapse all files

129
Angle.cpp
View File

@ -3,15 +3,15 @@
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Angle.h"
#include <math.h>
#include "FloatSingle.h"
#include <math.h>
namespace LinearAlgebra {
const float Rad2Deg = 57.29578F;
const float Deg2Rad = 0.0174532924F;
//===== AngleSingle, AngleOf<float>
template <>
AngleOf<float> AngleOf<float>::Degrees(float degrees) {
template <> AngleOf<float> Passer::LinearAlgebra::AngleOf<float>::Degrees(float degrees) {
if (isfinite(degrees)) {
while (degrees < -180)
degrees += 360;
@ -22,8 +22,7 @@ AngleOf<float> AngleOf<float>::Degrees(float degrees) {
return AngleOf<float>(degrees);
}
template <>
AngleOf<float> AngleOf<float>::Radians(float radians) {
template <> AngleOf<float> AngleOf<float>::Radians(float radians) {
if (isfinite(radians)) {
while (radians <= -pi)
radians += 2 * pi;
@ -34,13 +33,9 @@ AngleOf<float> AngleOf<float>::Radians(float radians) {
return Binary(radians * Rad2Deg);
}
template <>
float AngleOf<float>::InDegrees() const {
return this->value;
}
template <> float AngleOf<float>::InDegrees() const { return this->value; }
template <>
float AngleOf<float>::InRadians() const {
template <> float AngleOf<float>::InRadians() const {
return this->value * Deg2Rad;
}
@ -63,29 +58,25 @@ AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
return Binary(value);
}
template <>
float AngleOf<signed short>::InDegrees() const {
template <> float AngleOf<signed short>::InDegrees() const {
float degrees = this->value / 65536.0f * 360.0f;
return degrees;
}
template <>
float AngleOf<signed short>::InRadians() const {
template <> float AngleOf<signed short>::InRadians() const {
float radians = this->value / 65536.0f * (2 * pi);
return radians;
}
//===== Angle8, AngleOf<signed char>
template <>
AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
template <> AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
// map float [-180..180) to integer [-128..127)
signed char value = (signed char)roundf(degrees / 360.0F * 256.0F);
return Binary(value);
}
template <>
AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
template <> AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
if (!isfinite(radians))
return AngleOf<signed char>::zero;
@ -94,42 +85,32 @@ AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
return Binary(value);
}
template <>
float AngleOf<signed char>::InDegrees() const {
template <> float AngleOf<signed char>::InDegrees() const {
float degrees = this->value / 256.0f * 360.0f;
return degrees;
}
template <>
float AngleOf<signed char>::InRadians() const {
template <> float AngleOf<signed char>::InRadians() const {
float radians = this->value / 128.0f * pi;
return radians;
}
//===== Generic
template <typename T>
AngleOf<T>::AngleOf() : value(0) {}
template <typename T> AngleOf<T>::AngleOf() : value(0) {}
template <typename T>
AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
template <typename T> AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
template <typename T>
const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
template <typename T> const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
template <typename T>
AngleOf<T> AngleOf<T>::Binary(T rawValue) {
template <typename T> AngleOf<T> AngleOf<T>::Binary(T rawValue) {
AngleOf<T> angle = AngleOf<T>();
angle.SetBinary(rawValue);
return angle;
}
template <typename T>
T AngleOf<T>::GetBinary() const {
return this->value;
}
template <typename T>
void AngleOf<T>::SetBinary(T rawValue) {
template <typename T> T AngleOf<T>::GetBinary() const { return this->value; }
template <typename T> void AngleOf<T>::SetBinary(T rawValue) {
this->value = rawValue;
}
@ -138,28 +119,24 @@ bool AngleOf<T>::operator==(const AngleOf<T> angle) const {
return this->value == angle.value;
}
template <typename T>
bool AngleOf<T>::operator>(AngleOf<T> angle) const {
template <typename T> bool AngleOf<T>::operator>(AngleOf<T> angle) const {
return this->value > angle.value;
}
template <typename T>
bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
template <typename T> bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
return this->value >= angle.value;
}
template <typename T>
bool AngleOf<T>::operator<(AngleOf<T> angle) const {
template <typename T> bool AngleOf<T>::operator<(AngleOf<T> angle) const {
return this->value < angle.value;
}
template <typename T>
bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
template <typename T> bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
return this->value <= angle.value;
}
template <typename T>
signed int AngleOf<T>::Sign(AngleOf<T> angle) {
signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> angle) {
if (angle.value < 0)
return -1;
if (angle.value > 0)
@ -168,52 +145,51 @@ signed int AngleOf<T>::Sign(AngleOf<T> angle) {
}
template <typename T>
AngleOf<T> AngleOf<T>::Abs(AngleOf<T> angle) {
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Abs(AngleOf<T> angle) {
if (Sign(angle) < 0)
return -angle;
else
return angle;
}
template <typename T>
AngleOf<T> AngleOf<T>::operator-() const {
template <typename T> AngleOf<T> AngleOf<T>::operator-() const {
AngleOf<T> angle = Binary(-this->value);
return angle;
}
template <>
AngleOf<float> AngleOf<float>::operator-(const AngleOf<float>& angle) const {
AngleOf<float> AngleOf<float>::operator-(const AngleOf<float> &angle) const {
AngleOf<float> r = Binary(this->value - angle.value);
r = Normalize(r);
return r;
}
template <typename T>
AngleOf<T> AngleOf<T>::operator-(const AngleOf<T>& angle) const {
AngleOf<T> AngleOf<T>::operator-(const AngleOf<T> &angle) const {
AngleOf<T> r = Binary(this->value - angle.value);
return r;
}
template <>
AngleOf<float> AngleOf<float>::operator+(const AngleOf<float>& angle) const {
AngleOf<float> AngleOf<float>::operator+(const AngleOf<float> &angle) const {
AngleOf<float> r = Binary(this->value + angle.value);
r = Normalize(r);
return r;
}
template <typename T>
AngleOf<T> AngleOf<T>::operator+(const AngleOf<T>& angle) const {
AngleOf<T> AngleOf<T>::operator+(const AngleOf<T> &angle) const {
AngleOf<T> r = Binary(this->value + angle.value);
return r;
}
template <>
AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float>& angle) {
AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float> &angle) {
this->value += angle.value;
this->Normalize();
return *this;
}
template <typename T>
AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& angle) {
AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T> &angle) {
this->value += angle.value;
return *this;
}
@ -230,8 +206,7 @@ AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& angle) {
// return AngleOf::Degrees((float)factor * angle.InDegrees());
// }
template <typename T>
void AngleOf<T>::Normalize() {
template <typename T> void AngleOf<T>::Normalize() {
float angleValue = this->InDegrees();
if (!isfinite(angleValue))
return;
@ -243,8 +218,7 @@ void AngleOf<T>::Normalize() {
*this = AngleOf::Degrees(angleValue);
}
template <typename T>
AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
template <typename T> AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
float angleValue = angle.InDegrees();
if (!isfinite(angleValue))
return angle;
@ -263,10 +237,9 @@ AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
}
template <typename T>
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
AngleOf<T> toAngle,
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
float maxDegrees) {
maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
AngleOf<T> d = toAngle - fromAngle;
float dDegrees = Abs(d).InDegrees();
d = AngleOf<T>::Degrees(Float::Clamp(dDegrees, 0, maxDegrees));
@ -276,34 +249,28 @@ AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
return fromAngle + d;
}
template <typename T>
float AngleOf<T>::Cos(AngleOf<T> angle) {
template <typename T> float AngleOf<T>::Cos(AngleOf<T> angle) {
return cosf(angle.InRadians());
}
template <typename T>
float AngleOf<T>::Sin(AngleOf<T> angle) {
template <typename T> float AngleOf<T>::Sin(AngleOf<T> angle) {
return sinf(angle.InRadians());
}
template <typename T>
float AngleOf<T>::Tan(AngleOf<T> angle) {
template <typename T> float AngleOf<T>::Tan(AngleOf<T> angle) {
return tanf(angle.InRadians());
}
template <typename T>
AngleOf<T> AngleOf<T>::Acos(float f) {
template <typename T> AngleOf<T> AngleOf<T>::Acos(float f) {
return AngleOf<T>::Radians(acosf(f));
}
template <typename T>
AngleOf<T> AngleOf<T>::Asin(float f) {
template <typename T> AngleOf<T> AngleOf<T>::Asin(float f) {
return AngleOf<T>::Radians(asinf(f));
}
template <typename T>
AngleOf<T> AngleOf<T>::Atan(float f) {
template <typename T> AngleOf<T> AngleOf<T>::Atan(float f) {
return AngleOf<T>::Radians(atanf(f));
}
template <typename T>
AngleOf<T> AngleOf<T>::Atan2(float y, float x) {
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Atan2(float y, float x) {
return AngleOf<T>::Radians(atan2f(y, x));
}
@ -330,7 +297,7 @@ float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
float b2 = b * b;
float d =
a2 + b2 -
2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
// Catch edge cases where float inacuracies lead tot nans
if (d < 0)
return 0;
@ -387,8 +354,6 @@ AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
return alpha;
}
template class AngleOf<float>;
template class AngleOf<signed char>;
template class AngleOf<signed short>;
} // namespace LinearAlgebra
template class Passer::LinearAlgebra::AngleOf<float>;
template class Passer::LinearAlgebra::AngleOf<signed char>;
template class Passer::LinearAlgebra::AngleOf<signed short>;

31
Angle.h
View File

@ -5,6 +5,7 @@
#ifndef ANGLE_H
#define ANGLE_H
namespace Passer {
namespace LinearAlgebra {
static float pi = 3.1415927410125732421875F;
@ -17,9 +18,8 @@ static float Deg2Rad = (pi * 2) / 360.0f;
/// The angle is internally limited to (-180..180] degrees or (-PI...PI]
/// radians. When an angle exceeds this range, it is normalized to a value
/// within the range.
template <typename T>
class AngleOf {
public:
template <typename T> class AngleOf {
public:
/// @brief Create a new angle with a zero value
AngleOf<T>();
@ -100,28 +100,28 @@ class AngleOf {
/// @brief Substract another angle from this angle
/// @param angle The angle to subtract from this angle
/// @return The result of the subtraction
AngleOf<T> operator-(const AngleOf<T>& angle) const;
AngleOf<T> operator-(const AngleOf<T> &angle) const;
/// @brief Add another angle from this angle
/// @param angle The angle to add to this angle
/// @return The result of the addition
AngleOf<T> operator+(const AngleOf<T>& angle) const;
AngleOf<T> operator+(const AngleOf<T> &angle) const;
/// @brief Add another angle to this angle
/// @param angle The angle to add to this angle
/// @return The result of the addition
AngleOf<T> operator+=(const AngleOf<T>& angle);
AngleOf<T> operator+=(const AngleOf<T> &angle);
/// @brief Mutliplies the angle
/// @param angle The angle to multiply
/// @param factor The factor by which the angle is multiplied
/// @return The multiplied angle
friend AngleOf<T> operator*(const AngleOf<T>& angle, float factor) {
friend AngleOf<T> operator*(const AngleOf<T> &angle, float factor) {
return AngleOf::Degrees((float)angle.InDegrees() * factor);
}
/// @brief Multiplies the angle
/// @param factor The factor by which the angle is multiplies
/// @param angle The angle to multiply
/// @return The multiplied angle
friend AngleOf<T> operator*(float factor, const AngleOf<T>& angle) {
friend AngleOf<T> operator*(float factor, const AngleOf<T> &angle) {
return AngleOf::Degrees((float)factor * angle.InDegrees());
}
@ -150,8 +150,7 @@ class AngleOf {
/// @param toAngle The angle to rotate towards
/// @param maxAngle The maximum angle to rotate
/// @return The rotated angle
static AngleOf<T> MoveTowards(AngleOf<T> fromAngle,
AngleOf<T> toAngle,
static AngleOf<T> MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
float maxAngle);
/// @brief Calculates the cosine of an angle
@ -206,7 +205,7 @@ class AngleOf {
/// @return The angle of the corner opposing side A
static AngleOf<T> SineRuleAngle(float a, AngleOf<T> beta, float c);
private:
private:
T value;
AngleOf<T>(T rawValue);
@ -216,12 +215,8 @@ using AngleSingle = AngleOf<float>;
using Angle16 = AngleOf<signed short>;
using Angle8 = AngleOf<signed char>;
#if defined(ARDUINO)
using Angle = Angle16;
#else
using Angle = AngleSingle;
#endif
} // namespace LinearAlgebra
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

31
Direction.cpp
View File

@ -9,8 +9,7 @@
#include <math.h>
template <typename T>
DirectionOf<T>::DirectionOf() {
template <typename T> DirectionOf<T>::DirectionOf() {
this->horizontal = AngleOf<T>();
this->vertical = AngleOf<T>();
}
@ -42,7 +41,7 @@ const DirectionOf<T> DirectionOf<T>::right =
DirectionOf<T>(AngleOf<T>::Degrees(90), AngleOf<T>());
template <typename T>
Vector3 DirectionOf<T>::ToVector3() const {
Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
this->horizontal.InDegrees(), 0);
Vector3 v = q * Vector3::forward;
@ -50,47 +49,49 @@ Vector3 DirectionOf<T>::ToVector3() const {
}
template <typename T>
DirectionOf<T> DirectionOf<T>::FromVector3(Vector3 vector) {
DirectionOf<T>
Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 vector) {
DirectionOf<T> d;
d.horizontal = AngleOf<T>::Atan2(
vector.Right(),
vector
.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
vector.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
d.vertical =
AngleOf<T>::Degrees(-90) -
AngleOf<T>::Acos(
vector.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
vector.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
d.Normalize();
return d;
}
template <typename T>
DirectionOf<T> DirectionOf<T>::Degrees(float horizontal, float vertical) {
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
float vertical) {
return DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
AngleOf<T>::Degrees(vertical));
}
template <typename T>
DirectionOf<T> DirectionOf<T>::Radians(float horizontal, float vertical) {
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Radians(float horizontal,
float vertical) {
return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
AngleOf<T>::Radians(vertical));
}
template <typename T>
bool DirectionOf<T>::operator==(const DirectionOf<T> direction) const {
bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
const DirectionOf<T> direction) const {
return (this->horizontal == direction.horizontal) &&
(this->vertical == direction.vertical);
}
template <typename T>
DirectionOf<T> DirectionOf<T>::operator-() const {
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
DirectionOf<T> r = DirectionOf<T>(this->horizontal + AngleOf<T>::Degrees(180),
-this->vertical);
return r;
}
template <typename T>
void DirectionOf<T>::Normalize() {
template <typename T> void DirectionOf<T>::Normalize() {
if (this->vertical > AngleOf<T>::Degrees(90) ||
this->vertical < AngleOf<T>::Degrees(-90)) {
this->horizontal += AngleOf<T>::Degrees(180);
@ -98,5 +99,5 @@ void DirectionOf<T>::Normalize() {
}
}
template class DirectionOf<float>;
template class DirectionOf<signed short>;
template class Passer::LinearAlgebra::DirectionOf<float>;
template class Passer::LinearAlgebra::DirectionOf<signed short>;

30
Direction.h
View File

@ -7,6 +7,7 @@
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
@ -21,14 +22,8 @@ struct Vector3;
/// rotation has been applied.
/// The angles are automatically normalized to stay within the abovenmentioned
/// ranges.
template <typename T>
class DirectionOf {
public:
/// @brief horizontal angle, range= (-180..180]
AngleOf<T> horizontal;
/// @brief vertical angle, range in degrees = (-90..90]
AngleOf<T> vertical;
template <typename T> class DirectionOf {
public:
/// @brief Create a new direction with zero angles
DirectionOf<T>();
/// @brief Create a new direction
@ -36,6 +31,11 @@ class DirectionOf {
/// @param vertical The vertical angle.
DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
/// @brief horizontal angle, range= (-180..180]
AngleOf<T> horizontal;
/// @brief vertical angle, range in degrees = (-90..90]
AngleOf<T> vertical;
/// @brief Convert the direction into a carthesian vector
/// @return The carthesian vector corresponding to this direction.
Vector3 ToVector3() const;
@ -83,7 +83,7 @@ class DirectionOf {
/// @return The reversed direction.
DirectionOf<T> operator-() const;
protected:
protected:
/// @brief Normalize this vector to the specified ranges
void Normalize();
};
@ -91,14 +91,8 @@ class DirectionOf {
using DirectionSingle = DirectionOf<float>;
using Direction16 = DirectionOf<signed short>;
#if defined(ARDUINO)
using Direction = Direction16;
#else
using Direction = DirectionSingle;
#endif
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

9
FloatSingle.h
View File

@ -5,18 +5,19 @@
#ifndef FLOAT_H
#define FLOAT_H
namespace Passer {
namespace LinearAlgebra {
class Float {
public:
public:
static const float epsilon;
static const float sqrEpsilon;
static float Clamp(float f, float min, float max);
};
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

38
Matrix.h
View File

@ -3,18 +3,18 @@
#include "Vector3.h"
namespace Passer {
namespace LinearAlgebra {
/// @brief Single precision float matrix
template <typename T>
class MatrixOf {
public:
template <typename T> class MatrixOf {
public:
MatrixOf(unsigned int rows, unsigned int cols);
MatrixOf(unsigned int rows, unsigned int cols, const T* source)
MatrixOf(unsigned int rows, unsigned int cols, const T *source)
: MatrixOf(rows, cols) {
Set(source);
}
MatrixOf(Vector3 v); // creates a 3,1 matrix
MatrixOf(Vector3 v); // creates a 3,1 matrix
~MatrixOf() {
if (this->data == nullptr)
@ -25,7 +25,7 @@ class MatrixOf {
/// @brief Transpose with result in matrix m
/// @param r The matrix in which the transposed matrix is stored
void Transpose(MatrixOf<T>* r) const {
void Transpose(MatrixOf<T> *r) const {
// Check dimensions first
// We dont care about the rows and cols (we overwrite them)
// but the data size should be equal to avoid problems
@ -54,14 +54,13 @@ class MatrixOf {
}
}
static void Multiply(const MatrixOf<T>* m1,
const MatrixOf<T>* m2,
MatrixOf<T>* r);
void Multiply(const MatrixOf<T>* m, MatrixOf<T>* r) const {
static void Multiply(const MatrixOf<T> *m1, const MatrixOf<T> *m2,
MatrixOf<T> *r);
void Multiply(const MatrixOf<T> *m, MatrixOf<T> *r) const {
Multiply(this, m, r);
}
static Vector3 Multiply(const MatrixOf<T>* m, Vector3 v);
static Vector3 Multiply(const MatrixOf<T> *m, Vector3 v);
Vector3 operator*(const Vector3 v) const;
T Get(unsigned int rowIx, unsigned int colIx) const {
@ -75,28 +74,28 @@ class MatrixOf {
}
// This function does not check on source size!
void Set(const T* source) {
void Set(const T *source) {
unsigned int matrixSize = this->cols * this->rows;
for (unsigned int dataIx = 0; dataIx < matrixSize; dataIx++)
this->data[dataIx] = source[dataIx];
}
// This function does not check on source size!
void SetRow(unsigned int rowIx, const T* source) {
void SetRow(unsigned int rowIx, const T *source) {
unsigned int dataIx = rowIx * this->cols;
for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx++, sourceIx++)
this->data[dataIx] = source[sourceIx];
}
// This function does not check on source size!
void SetCol(unsigned int colIx, const T* source) {
void SetCol(unsigned int colIx, const T *source) {
unsigned int dataIx = colIx;
for (unsigned int sourceIx = 0; sourceIx < this->cols;
dataIx += this->cols, sourceIx++)
this->data[dataIx] = source[sourceIx];
}
void CopyFrom(const MatrixOf<T>* m) {
void CopyFrom(const MatrixOf<T> *m) {
unsigned int thisMatrixSize = this->cols * this->rows;
unsigned int mMatrixSize = m->cols * m->rows;
if (mMatrixSize != thisMatrixSize)
@ -109,13 +108,14 @@ class MatrixOf {
unsigned int RowCount() const { return rows; }
unsigned int ColCount() const { return cols; }
private:
private:
unsigned int rows;
unsigned int cols;
T* data;
T *data;
};
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

54
Polar.cpp
View File

@ -3,13 +3,11 @@
#include "Polar.h"
#include "Vector2.h"
template <typename T>
PolarOf<T>::PolarOf() {
template <typename T> PolarOf<T>::PolarOf() {
this->distance = 0.0f;
this->angle = AngleOf<T>();
}
template <typename T>
PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
template <typename T> PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
// distance should always be 0 or greater
if (distance < 0.0f) {
this->distance = -distance;
@ -36,18 +34,16 @@ PolarOf<T> PolarOf<T>::Radians(float distance, float radians) {
return PolarOf<T>(distance, AngleOf<T>::Radians(radians));
}
template <typename T>
PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
template <typename T> PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
float distance = v.magnitude();
AngleOf<T> angle =
AngleOf<T>::Degrees(Vector2::SignedAngle(Vector2::forward, v));
PolarOf<T> p = PolarOf(distance, angle);
return p;
}
template <typename T>
PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
float distance =
v.distance * cosf(v.direction.vertical.InDegrees() * Deg2Rad);
template <typename T> PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
float distance = v.distance * cosf(v.direction.vertical.InDegrees() *
Passer::LinearAlgebra::Deg2Rad);
AngleOf<T> angle = v.direction.horizontal;
PolarOf<T> p = PolarOf(distance, angle);
return p;
@ -64,37 +60,31 @@ const PolarOf<T> PolarOf<T>::right = PolarOf(1.0, AngleOf<T>::Degrees(90));
template <typename T>
const PolarOf<T> PolarOf<T>::left = PolarOf(1.0, AngleOf<T>::Degrees(-90));
template <typename T>
bool PolarOf<T>::operator==(const PolarOf& v) const {
template <typename T> bool PolarOf<T>::operator==(const PolarOf &v) const {
return (this->distance == v.distance &&
this->angle.InDegrees() == v.angle.InDegrees());
}
template <typename T>
PolarOf<T> PolarOf<T>::Normalize(const PolarOf& v) {
template <typename T> PolarOf<T> PolarOf<T>::Normalize(const PolarOf &v) {
PolarOf<T> r = PolarOf(1, v.angle);
return r;
}
template <typename T>
PolarOf<T> PolarOf<T>::normalized() const {
template <typename T> PolarOf<T> PolarOf<T>::normalized() const {
PolarOf<T> r = PolarOf(1, this->angle);
return r;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator-() const {
template <typename T> PolarOf<T> PolarOf<T>::operator-() const {
PolarOf<T> v =
PolarOf(this->distance, this->angle + AngleOf<T>::Degrees(180));
return v;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator-(const PolarOf& v) const {
template <typename T> PolarOf<T> PolarOf<T>::operator-(const PolarOf &v) const {
PolarOf<T> r = -v;
return *this + r;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
template <typename T> PolarOf<T> PolarOf<T>::operator-=(const PolarOf &v) {
*this = *this - v;
return *this;
// angle = AngleOf<T>::Normalize(newAngle);
@ -115,8 +105,7 @@ PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
// return d;
// }
template <typename T>
PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
template <typename T> PolarOf<T> PolarOf<T>::operator+(const PolarOf &v) const {
if (v.distance == 0)
return PolarOf(this->distance, this->angle);
if (this->distance == 0.0f)
@ -144,36 +133,33 @@ PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
PolarOf vector = PolarOf(newDistance, newAngleA);
return vector;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator+=(const PolarOf& v) {
template <typename T> PolarOf<T> PolarOf<T>::operator+=(const PolarOf &v) {
*this = *this + v;
return *this;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator*=(float f) {
template <typename T> PolarOf<T> PolarOf<T>::operator*=(float f) {
this->distance *= f;
return *this;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator/=(float f) {
template <typename T> PolarOf<T> PolarOf<T>::operator/=(float f) {
this->distance /= f;
return *this;
}
template <typename T>
float PolarOf<T>::Distance(const PolarOf& v1, const PolarOf& v2) {
float PolarOf<T>::Distance(const PolarOf &v1, const PolarOf &v2) {
float d =
AngleOf<T>::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
return d;
}
template <typename T>
PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
PolarOf<T> PolarOf<T>::Rotate(const PolarOf &v, AngleOf<T> angle) {
AngleOf<T> a = AngleOf<T>::Normalize(v.angle + angle);
PolarOf<T> r = PolarOf(v.distance, a);
return r;
}
template class PolarOf<float>;
template class PolarOf<signed short>;
template class Passer::LinearAlgebra::PolarOf<float>;
template class Passer::LinearAlgebra::PolarOf<signed short>;

40
Polar.h
View File

@ -7,17 +7,16 @@
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector2;
template <typename T>
class SphericalOf;
template <typename T> class SphericalOf;
/// @brief A polar vector using an angle in various representations
/// @tparam T The implementation type used for the representation of the angle
template <typename T>
class PolarOf {
public:
template <typename T> class PolarOf {
public:
/// @brief The distance in meters
/// @remark The distance shall never be negative
float distance;
@ -77,12 +76,12 @@ class PolarOf {
/// @return true: if it is identical to the given vector
/// @note This uses float comparison to check equality which may have
/// strange effects. Equality on floats should be avoided.
bool operator==(const PolarOf& v) const;
bool operator==(const PolarOf &v) const;
/// @brief The vector length
/// @param v The vector for which you need the length
/// @return The vector length;
inline static float Magnitude(const PolarOf& v) { return v.distance; }
inline static float Magnitude(const PolarOf &v) { return v.distance; }
/// @brief The vector length
/// @return The vector length
inline float magnitude() const { return this->distance; }
@ -90,7 +89,7 @@ class PolarOf {
/// @brief Convert the vector to a length of 1
/// @param v The vector to convert
/// @return The vector normalized to a length of 1
static PolarOf Normalize(const PolarOf& v);
static PolarOf Normalize(const PolarOf &v);
/// @brief Convert the vector to a length of a
/// @return The vector normalized to a length of 1
PolarOf normalized() const;
@ -103,23 +102,23 @@ class PolarOf {
/// @brief Subtract a polar vector from this vector
/// @param v The vector to subtract
/// @return The result of the subtraction
PolarOf operator-(const PolarOf& v) const;
PolarOf operator-=(const PolarOf& v);
PolarOf operator-(const PolarOf &v) const;
PolarOf operator-=(const PolarOf &v);
/// @brief Add a polar vector to this vector
/// @param v The vector to add
/// @return The result of the addition
PolarOf operator+(const PolarOf& v) const;
PolarOf operator+=(const PolarOf& v);
PolarOf operator+(const PolarOf &v) const;
PolarOf operator+=(const PolarOf &v);
/// @brief Scale the vector uniformly up
/// @param f The scaling factor
/// @return The scaled vector
/// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected.
friend PolarOf operator*(const PolarOf& v, float f) {
friend PolarOf operator*(const PolarOf &v, float f) {
return PolarOf(v.distance * f, v.angle);
}
friend PolarOf operator*(float f, const PolarOf& v) {
friend PolarOf operator*(float f, const PolarOf &v) {
return PolarOf(f * v.distance, v.angle);
}
PolarOf operator*=(float f);
@ -128,10 +127,10 @@ class PolarOf {
/// @return The scaled factor
/// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected.
friend PolarOf operator/(const PolarOf& v, float f) {
friend PolarOf operator/(const PolarOf &v, float f) {
return PolarOf(v.distance / f, v.angle);
}
friend PolarOf operator/(float f, const PolarOf& v) {
friend PolarOf operator/(float f, const PolarOf &v) {
return PolarOf(f / v.distance, v.angle);
}
PolarOf operator/=(float f);
@ -140,21 +139,22 @@ class PolarOf {
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The distance between the two vectors
static float Distance(const PolarOf& v1, const PolarOf& v2);
static float Distance(const PolarOf &v1, const PolarOf &v2);
/// @brief Rotate a vector
/// @param v The vector to rotate
/// @param a The angle in degreesto rotate
/// @return The rotated vector
static PolarOf Rotate(const PolarOf& v, AngleOf<T> a);
static PolarOf Rotate(const PolarOf &v, AngleOf<T> a);
};
using PolarSingle = PolarOf<float>;
using Polar16 = PolarOf<signed short>;
// using Polar = PolarSingle;
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#include "Spherical.h"
#include "Vector2.h"

53
Quaternion.h
View File

@ -32,13 +32,14 @@ typedef struct Quat {
} Quat;
}
namespace Passer {
namespace LinearAlgebra {
/// <summary>
/// A quaternion
/// </summary>
struct Quaternion : Quat {
public:
public:
/// <summary>
/// Create a new identity quaternion
/// </summary>
@ -79,7 +80,7 @@ struct Quaternion : Quat {
/// <returns>A unit quaternion</returns>
/// This will preserve the orientation,
/// but ensures that it is a unit quaternion.
static Quaternion Normalize(const Quaternion& q);
static Quaternion Normalize(const Quaternion &q);
/// <summary>
/// Convert to euler angles
@ -87,14 +88,14 @@ struct Quaternion : Quat {
/// <param name="q">The quaternion to convert</param>
/// <returns>A vector containing euler angles</returns>
/// The euler angles performed in the order: Z, X, Y
static Vector3 ToAngles(const Quaternion& q);
static Vector3 ToAngles(const Quaternion &q);
/// <summary>
/// Rotate a vector using this quaterion
/// </summary>
/// <param name="vector">The vector to rotate</param>
/// <returns>The rotated vector</returns>
Vector3 operator*(const Vector3& vector) const;
Vector3 operator*(const Vector3 &vector) const;
/// <summary>
/// Multiply this quaternion with another quaternion
/// </summary>
@ -102,7 +103,7 @@ struct Quaternion : Quat {
/// <returns>The resulting rotation</returns>
/// The result will be this quaternion rotated according to
/// the give rotation.
Quaternion operator*(const Quaternion& rotation) const;
Quaternion operator*(const Quaternion &rotation) const;
/// <summary>
/// Check the equality of two quaternions
@ -113,7 +114,7 @@ struct Quaternion : Quat {
/// themselves. Two quaternions with the same rotational effect may have
/// different components. Use Quaternion::Angle to check if the rotations are
/// the same.
bool operator==(const Quaternion& quaternion) const;
bool operator==(const Quaternion &quaternion) const;
/// <summary>
/// The inverse of quaterion
@ -128,8 +129,8 @@ struct Quaternion : Quat {
/// <param name="forward">The look direction</param>
/// <param name="upwards">The up direction</param>
/// <returns>The look rotation</returns>
static Quaternion LookRotation(const Vector3& forward,
const Vector3& upwards);
static Quaternion LookRotation(const Vector3 &forward,
const Vector3 &upwards);
/// <summary>
/// Creates a quaternion with the given forward direction with up =
/// Vector3::up
@ -139,7 +140,7 @@ struct Quaternion : Quat {
/// For the rotation, Vector::up is used for the up direction.
/// Note: if the forward direction == Vector3::up, the result is
/// Quaternion::identity
static Quaternion LookRotation(const Vector3& forward);
static Quaternion LookRotation(const Vector3 &forward);
/// <summary>
/// Calculat the rotation from on vector to another
@ -156,8 +157,7 @@ struct Quaternion : Quat {
/// <param name="to">The destination rotation</param>
/// <param name="maxDegreesDelta">The maximum amount of degrees to
/// rotate</param> <returns>The possibly limited rotation</returns>
static Quaternion RotateTowards(const Quaternion& from,
const Quaternion& to,
static Quaternion RotateTowards(const Quaternion &from, const Quaternion &to,
float maxDegreesDelta);
/// <summary>
@ -166,13 +166,13 @@ struct Quaternion : Quat {
/// <param name="angle">The angle</param>
/// <param name="axis">The axis</param>
/// <returns>The resulting quaternion</returns>
static Quaternion AngleAxis(float angle, const Vector3& axis);
static Quaternion AngleAxis(float angle, const Vector3 &axis);
/// <summary>
/// Convert this quaternion to angle/axis representation
/// </summary>
/// <param name="angle">A pointer to the angle for the result</param>
/// <param name="axis">A pointer to the axis for the result</param>
void ToAngleAxis(float* angle, Vector3* axis);
void ToAngleAxis(float *angle, Vector3 *axis);
/// <summary>
/// Get the angle between two orientations
@ -190,9 +190,8 @@ struct Quaternion : Quat {
/// <param name="factor">The factor between 0 and 1.</param>
/// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2.
static Quaternion Slerp(const Quaternion& rotation1,
const Quaternion& rotation2,
float factor);
static Quaternion Slerp(const Quaternion &rotation1,
const Quaternion &rotation2, float factor);
/// <summary>
/// Unclamped sherical lerp between two rotations
/// </summary>
@ -202,9 +201,8 @@ struct Quaternion : Quat {
/// <returns>The resulting rotation</returns>
/// A factor 0 returns rotation1, factor1 returns rotation2.
/// Values outside the 0..1 range will result in extrapolated rotations
static Quaternion SlerpUnclamped(const Quaternion& rotation1,
const Quaternion& rotation2,
float factor);
static Quaternion SlerpUnclamped(const Quaternion &rotation1,
const Quaternion &rotation2, float factor);
/// <summary>
/// Create a rotation from euler angles
@ -262,10 +260,8 @@ struct Quaternion : Quat {
/// <param name="swing">A pointer to the quaternion for the swing
/// result</param> <param name="twist">A pointer to the quaternion for the
/// twist result</param>
static void GetSwingTwist(Vector3 axis,
Quaternion rotation,
Quaternion* swing,
Quaternion* twist);
static void GetSwingTwist(Vector3 axis, Quaternion rotation,
Quaternion *swing, Quaternion *twist);
/// <summary>
/// Calculate the dot product of two quaternions
@ -275,19 +271,20 @@ struct Quaternion : Quat {
/// <returns></returns>
static float Dot(Quaternion rotation1, Quaternion rotation2);
private:
private:
float GetLength() const;
float GetLengthSquared() const;
static float GetLengthSquared(const Quaternion& q);
static float GetLengthSquared(const Quaternion &q);
void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
void ToAxisAngleRad(const Quaternion &q, Vector3 *const axis, float *angle);
static Quaternion FromEulerRad(Vector3 euler);
static Quaternion FromEulerRadXYZ(Vector3 euler);
Vector3 xyz() const;
};
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

59
Spherical.cpp
View File

@ -5,15 +5,13 @@
#include <math.h>
template <typename T>
SphericalOf<T>::SphericalOf() {
template <typename T> SphericalOf<T>::SphericalOf() {
this->distance = 0.0f;
this->direction = DirectionOf<T>();
}
template <typename T>
SphericalOf<T>::SphericalOf(float distance,
AngleOf<T> horizontal,
SphericalOf<T>::SphericalOf(float distance, AngleOf<T> horizontal,
AngleOf<T> vertical) {
if (distance < 0) {
this->distance = -distance;
@ -36,8 +34,7 @@ SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
}
template <typename T>
SphericalOf<T> SphericalOf<T>::Degrees(float distance,
float horizontal,
SphericalOf<T> SphericalOf<T>::Degrees(float distance, float horizontal,
float vertical) {
AngleOf<T> horizontalAngle = AngleOf<T>::Degrees(horizontal);
AngleOf<T> verticalAngle = AngleOf<T>::Degrees(vertical);
@ -46,8 +43,7 @@ SphericalOf<T> SphericalOf<T>::Degrees(float distance,
}
template <typename T>
SphericalOf<T> SphericalOf<T>::Radians(float distance,
float horizontal,
SphericalOf<T> SphericalOf<T>::Radians(float distance, float horizontal,
float vertical) {
return SphericalOf<T>(distance, AngleOf<T>::Radians(horizontal),
AngleOf<T>::Radians(vertical));
@ -61,8 +57,7 @@ SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
return r;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
template <typename T> SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
float distance = v.magnitude();
if (distance == 0.0f) {
return SphericalOf(distance, AngleOf<T>(), AngleOf<T>());
@ -86,8 +81,7 @@ SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
* @tparam T The type of the distance and direction values.
* @return Vector3 The 3D vector representation of the spherical coordinates.
*/
template <typename T>
Vector3 SphericalOf<T>::ToVector3() const {
template <typename T> Vector3 SphericalOf<T>::ToVector3() const {
float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
float horizontalRad = this->direction.horizontal.InRadians();
@ -132,8 +126,7 @@ SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
return v;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator-() const {
template <typename T> SphericalOf<T> SphericalOf<T>::operator-() const {
SphericalOf<T> v = SphericalOf<T>(
this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
this->direction.vertical + AngleOf<T>::Degrees(180));
@ -141,7 +134,7 @@ SphericalOf<T> SphericalOf<T>::operator-() const {
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T> &s2) const {
// let's do it the easy way...
Vector3 v1 = this->ToVector3();
Vector3 v2 = s2.ToVector3();
@ -150,13 +143,13 @@ SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
return r;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T>& v) {
SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T> &v) {
*this = *this - v;
return *this;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T> &s2) const {
// let's do it the easy way...
Vector3 v1 = this->ToVector3();
Vector3 v2 = s2.ToVector3();
@ -211,19 +204,17 @@ SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
*/
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T> &v) {
*this = *this + v;
return *this;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator*=(float f) {
template <typename T> SphericalOf<T> SphericalOf<T>::operator*=(float f) {
this->distance *= f;
return *this;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator/=(float f) {
template <typename T> SphericalOf<T> SphericalOf<T>::operator/=(float f) {
this->distance /= f;
return *this;
}
@ -234,8 +225,8 @@ SphericalOf<T> SphericalOf<T>::operator/=(float f) {
const float epsilon = 1E-05f;
template <typename T>
float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2) {
float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
const SphericalOf<T> &v2) {
// SphericalOf<T> difference = v1 - v2;
// return difference.distance;
Vector3 vec1 = v1.ToVector3();
@ -245,8 +236,8 @@ float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
}
template <typename T>
AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
const SphericalOf& v2) {
AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
const SphericalOf &v2) {
// float denominator = v1.distance * v2.distance;
// if (denominator < epsilon)
// return 0.0f;
@ -265,9 +256,9 @@ AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
}
template <typename T>
AngleOf<T> SphericalOf<T>::SignedAngleBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2,
const SphericalOf<T>& axis) {
AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
const SphericalOf<T> &v1, const SphericalOf<T> &v2,
const SphericalOf<T> &axis) {
Vector3 v1_vector = v1.ToVector3();
Vector3 v2_vector = v2.ToVector3();
Vector3 axis_vector = axis.ToVector3();
@ -276,7 +267,7 @@ AngleOf<T> SphericalOf<T>::SignedAngleBetween(const SphericalOf<T>& v1,
}
template <typename T>
SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T> &v,
AngleOf<T> horizontalAngle,
AngleOf<T> verticalAngle) {
SphericalOf<T> r =
@ -285,19 +276,19 @@ SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
return r;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T> &v,
AngleOf<T> a) {
SphericalOf<T> r =
SphericalOf(v.distance, v.direction.horizontal + a, v.direction.vertical);
return r;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T> &v,
AngleOf<T> a) {
SphericalOf<T> r =
SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a);
return r;
}
template class SphericalOf<float>;
template class SphericalOf<signed short>;
template class Passer::LinearAlgebra::SphericalOf<float>;
template class Passer::LinearAlgebra::SphericalOf<signed short>;

63
Spherical.h
View File

@ -7,17 +7,16 @@
#include "Direction.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
template <typename T>
class PolarOf;
template <typename T> class PolarOf;
/// @brief A spherical vector using angles in various representations
/// @tparam T The implementation type used for the representations of the agles
template <typename T>
class SphericalOf {
public:
template <typename T> class SphericalOf {
public:
/// @brief The distance in meters
/// @remark The distance should never be negative
float distance;
@ -39,8 +38,7 @@ class SphericalOf {
/// @param horizontal The horizontal angle in degrees
/// @param vertical The vertical angle in degrees
/// @return The spherical vector
static SphericalOf<T> Degrees(float distance,
float horizontal,
static SphericalOf<T> Degrees(float distance, float horizontal,
float vertical);
/// @brief Short-hand Deg alias for the Degrees function
constexpr static auto Deg = Degrees;
@ -50,8 +48,7 @@ class SphericalOf {
/// @param horizontal The horizontal angle in radians
/// @param vertical The vertical angle in radians
/// @return The spherical vectpr
static SphericalOf<T> Radians(float distance,
float horizontal,
static SphericalOf<T> Radians(float distance, float horizontal,
float vertical);
// Short-hand Rad alias for the Radians function
constexpr static auto Rad = Radians;
@ -98,23 +95,23 @@ class SphericalOf {
/// @brief Subtract a spherical vector from this vector
/// @param v The vector to subtract
/// @return The result of the subtraction
SphericalOf<T> operator-(const SphericalOf<T>& v) const;
SphericalOf<T> operator-=(const SphericalOf<T>& v);
SphericalOf<T> operator-(const SphericalOf<T> &v) const;
SphericalOf<T> operator-=(const SphericalOf<T> &v);
/// @brief Add a spherical vector to this vector
/// @param v The vector to add
/// @return The result of the addition
SphericalOf<T> operator+(const SphericalOf<T>& v) const;
SphericalOf<T> operator+=(const SphericalOf<T>& v);
SphericalOf<T> operator+(const SphericalOf<T> &v) const;
SphericalOf<T> operator+=(const SphericalOf<T> &v);
/// @brief Scale the vector uniformly up
/// @param f The scaling factor
/// @return The scaled vector
/// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected.
friend SphericalOf<T> operator*(const SphericalOf<T>& v, float f) {
friend SphericalOf<T> operator*(const SphericalOf<T> &v, float f) {
return SphericalOf<T>(v.distance * f, v.direction);
}
friend SphericalOf<T> operator*(float f, const SphericalOf<T>& v) {
friend SphericalOf<T> operator*(float f, const SphericalOf<T> &v) {
return SphericalOf<T>(f * v.distance, v.direction);
}
SphericalOf<T> operator*=(float f);
@ -123,10 +120,10 @@ class SphericalOf {
/// @return The scaled factor
/// @remark This operation will scale the distance of the vector. The angle
/// will be unaffected.
friend SphericalOf<T> operator/(const SphericalOf<T>& v, float f) {
friend SphericalOf<T> operator/(const SphericalOf<T> &v, float f) {
return SphericalOf<T>(v.distance / f, v.direction);
}
friend SphericalOf<T> operator/(float f, const SphericalOf<T>& v) {
friend SphericalOf<T> operator/(float f, const SphericalOf<T> &v) {
return SphericalOf<T>(f / v.distance, v.direction);
}
SphericalOf<T> operator/=(float f);
@ -135,42 +132,41 @@ class SphericalOf {
/// @param v1 The first coordinate
/// @param v2 The second coordinate
/// @return The distance between the coordinates in meters
static float DistanceBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2);
static float DistanceBetween(const SphericalOf<T> &v1,
const SphericalOf<T> &v2);
/// @brief Calculate the unsigned angle between two spherical vectors
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The unsigned angle between the vectors
static AngleOf<T> AngleBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2);
static AngleOf<T> AngleBetween(const SphericalOf<T> &v1,
const SphericalOf<T> &v2);
/// @brief Calculate the signed angle between two spherical vectors
/// @param v1 The first vector
/// @param v2 The second vector
/// @param axis The axis are which the angle is calculated
/// @return The signed angle between the vectors
static AngleOf<T> SignedAngleBetween(const SphericalOf<T>& v1,
const SphericalOf<T>& v2,
const SphericalOf<T>& axis);
static AngleOf<T> SignedAngleBetween(const SphericalOf<T> &v1,
const SphericalOf<T> &v2,
const SphericalOf<T> &axis);
/// @brief Rotate a spherical vector
/// @param v The vector to rotate
/// @param horizontalAngle The horizontal rotation angle in local space
/// @param verticalAngle The vertical rotation angle in local space
/// @return The rotated vector
static SphericalOf<T> Rotate(const SphericalOf& v,
AngleOf<T> horizontalAngle,
static SphericalOf<T> Rotate(const SphericalOf &v, AngleOf<T> horizontalAngle,
AngleOf<T> verticalAngle);
/// @brief Rotate a spherical vector horizontally
/// @param v The vector to rotate
/// @param angle The horizontal rotation angle in local space
/// @return The rotated vector
static SphericalOf<T> RotateHorizontal(const SphericalOf<T>& v,
static SphericalOf<T> RotateHorizontal(const SphericalOf<T> &v,
AngleOf<T> angle);
/// @brief Rotate a spherical vector vertically
/// @param v The vector to rotate
/// @param angle The vertical rotation angle in local space
/// @return The rotated vector
static SphericalOf<T> RotateVertical(const SphericalOf<T>& v,
static SphericalOf<T> RotateVertical(const SphericalOf<T> &v,
AngleOf<T> angle);
};
@ -184,14 +180,9 @@ using SphericalSingle = SphericalOf<float>;
/// hardware
using Spherical16 = SphericalOf<signed short>;
#if defined(ARDUINO)
using Spherical = Spherical16;
#else
using Spherical = SphericalSingle;
#endif
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#include "Polar.h"
#include "Vector3.h"

4
SwingTwist.cpp
View File

@ -164,5 +164,5 @@ void SwingTwistOf<T>::Normalize() {
}
}
template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>;
template class Passer::LinearAlgebra::SwingTwistOf<float>;
template class Passer::LinearAlgebra::SwingTwistOf<signed short>;

30
SwingTwist.h
View File

@ -10,14 +10,14 @@
#include "Quaternion.h"
#include "Spherical.h"
namespace Passer {
namespace LinearAlgebra {
/// @brief An orientation using swing and twist angles in various
/// representations
/// @tparam T The implmentation type used for the representation of the angles
template <typename T>
class SwingTwistOf {
public:
template <typename T> class SwingTwistOf {
public:
DirectionOf<T> swing;
AngleOf<T> twist;
@ -25,8 +25,7 @@ class SwingTwistOf {
SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
static SwingTwistOf<T> Degrees(float horizontal,
float vertical = 0,
static SwingTwistOf<T> Degrees(float horizontal, float vertical = 0,
float twist = 0);
Quaternion ToQuaternion() const;
@ -44,7 +43,7 @@ class SwingTwistOf {
/// </summary>
/// <param name="vector">The vector to rotate</param>
/// <returns>The rotated vector</returns>
SphericalOf<T> operator*(const SphericalOf<T>& vector) const;
SphericalOf<T> operator*(const SphericalOf<T> &vector) const;
/// <summary>
/// Multiply this rotation with another rotation
/// </summary>
@ -52,8 +51,8 @@ class SwingTwistOf {
/// <returns>The resulting swing/twist rotation</returns>
/// The result will be this rotation rotated according to
/// the give rotation.
SwingTwistOf<T> operator*(const SwingTwistOf<T>& rotation) const;
SwingTwistOf<T> operator*=(const SwingTwistOf<T>& rotation);
SwingTwistOf<T> operator*(const SwingTwistOf<T> &rotation) const;
SwingTwistOf<T> operator*=(const SwingTwistOf<T> &rotation);
static SwingTwistOf<T> Inverse(SwingTwistOf<T> rotation);
@ -63,9 +62,9 @@ class SwingTwistOf {
/// <param name="angle">The angle</param>
/// <param name="axis">The axis</param>
/// <returns>The resulting quaternion</returns>
static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis);
static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T> &axis);
static AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);
static AngleOf<T> Angle(const SwingTwistOf<T> &r1, const SwingTwistOf<T> &r2);
void Normalize();
};
@ -73,13 +72,8 @@ class SwingTwistOf {
using SwingTwistSingle = SwingTwistOf<float>;
using SwingTwist16 = SwingTwistOf<signed short>;
#if defined(ARDUINO)
using SwingTwist = SwingTwist16;
#else
using SwingTwist = SwingTwistSingle;
#endif
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

57
Vector2.cpp
View File

@ -26,11 +26,11 @@ Vector2::Vector2(float _x, float _y) {
// y = v.y;
// }
Vector2::Vector2(Vector3 v) {
x = v.Right(); // x;
y = v.Forward(); // z;
x = v.Right(); // x;
y = v.Forward(); // z;
}
Vector2::Vector2(PolarSingle p) {
float horizontalRad = p.angle.InDegrees() * Deg2Rad;
float horizontalRad = p.angle.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
float cosHorizontal = cosf(horizontalRad);
float sinHorizontal = sinf(horizontalRad);
@ -49,24 +49,18 @@ const Vector2 Vector2::down = Vector2(0, -1);
const Vector2 Vector2::forward = Vector2(0, 1);
const Vector2 Vector2::back = Vector2(0, -1);
bool Vector2::operator==(const Vector2& v) {
bool Vector2::operator==(const Vector2 &v) {
return (this->x == v.x && this->y == v.y);
}
float Vector2::Magnitude(const Vector2& v) {
float Vector2::Magnitude(const Vector2 &v) {
return sqrtf(v.x * v.x + v.y * v.y);
}
float Vector2::magnitude() const {
return (float)sqrtf(x * x + y * y);
}
float Vector2::SqrMagnitude(const Vector2& v) {
return v.x * v.x + v.y * v.y;
}
float Vector2::sqrMagnitude() const {
return (x * x + y * y);
}
float Vector2::magnitude() const { return (float)sqrtf(x * x + y * y); }
float Vector2::SqrMagnitude(const Vector2 &v) { return v.x * v.x + v.y * v.y; }
float Vector2::sqrMagnitude() const { return (x * x + y * y); }
Vector2 Vector2::Normalize(const Vector2& v) {
Vector2 Vector2::Normalize(const Vector2 &v) {
float num = Vector2::Magnitude(v);
Vector2 result = Vector2::zero;
if (num > Float::epsilon) {
@ -83,28 +77,26 @@ Vector2 Vector2::normalized() const {
return result;
}
Vector2 Vector2::operator-() {
return Vector2(-this->x, -this->y);
}
Vector2 Vector2::operator-() { return Vector2(-this->x, -this->y); }
Vector2 Vector2::operator-(const Vector2& v) const {
Vector2 Vector2::operator-(const Vector2 &v) const {
return Vector2(this->x - v.x, this->y - v.y);
}
Vector2 Vector2::operator-=(const Vector2& v) {
Vector2 Vector2::operator-=(const Vector2 &v) {
this->x -= v.x;
this->y -= v.y;
return *this;
}
Vector2 Vector2::operator+(const Vector2& v) const {
Vector2 Vector2::operator+(const Vector2 &v) const {
return Vector2(this->x + v.x, this->y + v.y);
}
Vector2 Vector2::operator+=(const Vector2& v) {
Vector2 Vector2::operator+=(const Vector2 &v) {
this->x += v.x;
this->y += v.y;
return *this;
}
Vector2 Vector2::Scale(const Vector2& v1, const Vector2& v2) {
Vector2 Vector2::Scale(const Vector2 &v1, const Vector2 &v2) {
return Vector2(v1.x * v2.x, v1.y * v2.y);
}
// Vector2 Passer::LinearAlgebra::operator*(const Vector2 &v, float f) {
@ -130,18 +122,18 @@ Vector2 Vector2::operator/=(float f) {
return *this;
}
float Vector2::Dot(const Vector2& v1, const Vector2& v2) {
float Vector2::Dot(const Vector2 &v1, const Vector2 &v2) {
return v1.x * v2.x + v1.y * v2.y;
}
float Vector2::Distance(const Vector2& v1, const Vector2& v2) {
float Vector2::Distance(const Vector2 &v1, const Vector2 &v2) {
return Magnitude(v1 - v2);
}
float Vector2::Angle(const Vector2& v1, const Vector2& v2) {
float Vector2::Angle(const Vector2 &v1, const Vector2 &v2) {
return (float)fabs(SignedAngle(v1, v2));
}
float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
float Vector2::SignedAngle(const Vector2 &v1, const Vector2 &v2) {
float sqrMagFrom = v1.sqrMagnitude();
float sqrMagTo = v2.sqrMagnitude();
@ -156,14 +148,15 @@ float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
float angleFrom = atan2f(v1.y, v1.x);
float angleTo = atan2f(v2.y, v2.x);
return -(angleTo - angleFrom) * Rad2Deg;
return -(angleTo - angleFrom) * Passer::LinearAlgebra::Rad2Deg;
}
Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
float angleRad = a.InDegrees() * Deg2Rad;
Vector2 Vector2::Rotate(const Vector2 &v,
Passer::LinearAlgebra::AngleSingle a) {
float angleRad = a.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
#if defined(AVR)
float sinValue = sin(angleRad);
float cosValue = cos(angleRad); // * Angle::Deg2Rad);
float cosValue = cos(angleRad); // * Angle::Deg2Rad);
#else
float sinValue = (float)sinf(angleRad);
float cosValue = (float)cosf(angleRad);
@ -176,7 +169,7 @@ Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
return r;
}
Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
Vector2 Vector2::Lerp(const Vector2 &v1, const Vector2 &v2, float f) {
Vector2 v = v1 + (v2 - v1) * f;
return v;
}

49
Vector2.h
View File

@ -26,11 +26,11 @@ typedef struct Vec2 {
} Vec2;
}
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
template <typename T>
class PolarOf;
template <typename T> class PolarOf;
/// @brief A 2-dimensional vector
/// @remark This uses the right=handed carthesian coordinate system.
@ -38,7 +38,7 @@ class PolarOf;
struct Vector2 : Vec2 {
friend struct Vec2;
public:
public:
/// @brief A new 2-dimensional zero vector
Vector2();
/// @brief A new 2-dimensional vector
@ -80,12 +80,12 @@ struct Vector2 : Vec2 {
/// @return true if it is identical to the given vector
/// @note This uses float comparison to check equality which may have strange
/// effects. Equality on floats should be avoided.
bool operator==(const Vector2& v);
bool operator==(const Vector2 &v);
/// @brief The vector length
/// @param v The vector for which you need the length
/// @return The vector length
static float Magnitude(const Vector2& v);
static float Magnitude(const Vector2 &v);
/// @brief The vector length
/// @return The vector length
float magnitude() const;
@ -95,7 +95,7 @@ struct Vector2 : Vec2 {
/// @remark The squared length is computationally simpler than the real
/// length. Think of Pythagoras A^2 + B^2 = C^2. This prevents the calculation
/// of the squared root of C.
static float SqrMagnitude(const Vector2& v);
static float SqrMagnitude(const Vector2 &v);
/// @brief The squared vector length
/// @return The squared vector length
/// @remark The squared length is computationally simpler than the real
@ -106,7 +106,7 @@ struct Vector2 : Vec2 {
/// @brief Convert the vector to a length of 1
/// @param v The vector to convert
/// @return The vector normalized to a length of 1
static Vector2 Normalize(const Vector2& v);
static Vector2 Normalize(const Vector2 &v);
/// @brief Convert the vector to a length 1
/// @return The vector normalized to a length of 1
Vector2 normalized() const;
@ -118,13 +118,13 @@ struct Vector2 : Vec2 {
/// @brief Subtract a vector from this vector
/// @param v The vector to subtract from this vector
/// @return The result of the subtraction
Vector2 operator-(const Vector2& v) const;
Vector2 operator-=(const Vector2& v);
Vector2 operator-(const Vector2 &v) const;
Vector2 operator-=(const Vector2 &v);
/// @brief Add a vector to this vector
/// @param v The vector to add to this vector
/// @return The result of the addition
Vector2 operator+(const Vector2& v) const;
Vector2 operator+=(const Vector2& v);
Vector2 operator+(const Vector2 &v) const;
Vector2 operator+=(const Vector2 &v);
/// @brief Scale the vector using another vector
/// @param v1 The vector to scale
@ -132,16 +132,16 @@ struct Vector2 : Vec2 {
/// @return The scaled vector
/// @remark Each component of the vector v1 will be multiplied with the
/// matching component from the scaling vector v2.
static Vector2 Scale(const Vector2& v1, const Vector2& v2);
static Vector2 Scale(const Vector2 &v1, const Vector2 &v2);
/// @brief Scale the vector uniformly up
/// @param f The scaling factor
/// @return The scaled vector
/// @remark Each component of the vector will be multipled with the same
/// factor f.
friend Vector2 operator*(const Vector2& v, float f) {
friend Vector2 operator*(const Vector2 &v, float f) {
return Vector2(v.x * f, v.y * f);
}
friend Vector2 operator*(float f, const Vector2& v) {
friend Vector2 operator*(float f, const Vector2 &v) {
return Vector2(v.x * f, v.y * f);
// return Vector2(f * v.x, f * v.y);
}
@ -150,10 +150,10 @@ struct Vector2 : Vec2 {
/// @param f The scaling factor
/// @return The scaled vector
/// @remark Each componet of the vector will be divided by the same factor.
friend Vector2 operator/(const Vector2& v, float f) {
friend Vector2 operator/(const Vector2 &v, float f) {
return Vector2(v.x / f, v.y / f);
}
friend Vector2 operator/(float f, const Vector2& v) {
friend Vector2 operator/(float f, const Vector2 &v) {
return Vector2(f / v.x, f / v.y);
}
Vector2 operator/=(float f);
@ -162,13 +162,13 @@ struct Vector2 : Vec2 {
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The dot product of the two vectors
static float Dot(const Vector2& v1, const Vector2& v2);
static float Dot(const Vector2 &v1, const Vector2 &v2);
/// @brief The distance between two vectors
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The distance between the two vectors
static float Distance(const Vector2& v1, const Vector2& v2);
static float Distance(const Vector2 &v1, const Vector2 &v2);
/// @brief The angle between two vectors
/// @param v1 The first vector
@ -177,18 +177,18 @@ struct Vector2 : Vec2 {
/// @remark This reterns an unsigned angle which is the shortest distance
/// between the two vectors. Use Vector2::SignedAngle if a signed angle is
/// needed.
static float Angle(const Vector2& v1, const Vector2& v2);
static float Angle(const Vector2 &v1, const Vector2 &v2);
/// @brief The signed angle between two vectors
/// @param v1 The starting vector
/// @param v2 The ending vector
/// @return The signed angle between the two vectors
static float SignedAngle(const Vector2& v1, const Vector2& v2);
static float SignedAngle(const Vector2 &v1, const Vector2 &v2);
/// @brief Rotate the vector
/// @param v The vector to rotate
/// @param a The angle in degrees to rotate
/// @return The rotated vector
static Vector2 Rotate(const Vector2& v, AngleSingle a);
static Vector2 Rotate(const Vector2 &v, Passer::LinearAlgebra::AngleSingle a);
/// @brief Lerp (linear interpolation) between two vectors
/// @param v1 The starting vector
@ -198,11 +198,12 @@ struct Vector2 : Vec2 {
/// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
/// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
/// between *v1* and *v2* etc.
static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f);
static Vector2 Lerp(const Vector2 &v1, const Vector2 &v2, float f);
};
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#include "Polar.h"

53
Vector3.cpp
View File

@ -31,8 +31,10 @@ Vector3::Vector3(Vector2 v) {
}
Vector3::Vector3(SphericalOf<float> s) {
float verticalRad = (90.0f - s.direction.vertical.InDegrees()) * Deg2Rad;
float horizontalRad = s.direction.horizontal.InDegrees() * Deg2Rad;
float verticalRad = (90.0f - s.direction.vertical.InDegrees()) *
Passer::LinearAlgebra::Deg2Rad;
float horizontalRad =
s.direction.horizontal.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
float cosVertical = cosf(verticalRad);
float sinVertical = sinf(verticalRad);
float cosHorizontal = cosf(horizontalRad);
@ -65,21 +67,17 @@ const Vector3 Vector3::back = Vector3(0, 0, -1);
// return Vector3(v.x, 0, v.y);
// }
float Vector3::Magnitude(const Vector3& v) {
float Vector3::Magnitude(const Vector3 &v) {
return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
}
float Vector3::magnitude() const {
return (float)sqrtf(x * x + y * y + z * z);
}
float Vector3::magnitude() const { return (float)sqrtf(x * x + y * y + z * z); }
float Vector3::SqrMagnitude(const Vector3& v) {
float Vector3::SqrMagnitude(const Vector3 &v) {
return v.x * v.x + v.y * v.y + v.z * v.z;
}
float Vector3::sqrMagnitude() const {
return (x * x + y * y + z * z);
}
float Vector3::sqrMagnitude() const { return (x * x + y * y + z * z); }
Vector3 Vector3::Normalize(const Vector3& v) {
Vector3 Vector3::Normalize(const Vector3 &v) {
float num = Vector3::Magnitude(v);
Vector3 result = Vector3::zero;
if (num > epsilon) {
@ -100,26 +98,26 @@ Vector3 Vector3::operator-() const {
return Vector3(-this->x, -this->y, -this->z);
}
Vector3 Vector3::operator-(const Vector3& v) const {
Vector3 Vector3::operator-(const Vector3 &v) const {
return Vector3(this->x - v.x, this->y - v.y, this->z - v.z);
}
Vector3 Vector3::operator-=(const Vector3& v) {
Vector3 Vector3::operator-=(const Vector3 &v) {
this->x -= v.x;
this->y -= v.y;
this->z -= v.z;
return *this;
}
Vector3 Vector3::operator+(const Vector3& v) const {
Vector3 Vector3::operator+(const Vector3 &v) const {
return Vector3(this->x + v.x, this->y + v.y, this->z + v.z);
}
Vector3 Vector3::operator+=(const Vector3& v) {
Vector3 Vector3::operator+=(const Vector3 &v) {
this->x += v.x;
this->y += v.y;
this->z += v.z;
return *this;
}
Vector3 Vector3::Scale(const Vector3& v1, const Vector3& v2) {
Vector3 Vector3::Scale(const Vector3 &v1, const Vector3 &v2) {
return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
}
// Vector3 Passer::LinearAlgebra::operator*(const Vector3 &v, float f) {
@ -147,24 +145,24 @@ Vector3 Vector3::operator/=(float f) {
return *this;
}
float Vector3::Dot(const Vector3& v1, const Vector3& v2) {
float Vector3::Dot(const Vector3 &v1, const Vector3 &v2) {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
bool Vector3::operator==(const Vector3& v) const {
bool Vector3::operator==(const Vector3 &v) const {
return (this->x == v.x && this->y == v.y && this->z == v.z);
}
float Vector3::Distance(const Vector3& v1, const Vector3& v2) {
float Vector3::Distance(const Vector3 &v1, const Vector3 &v2) {
return Magnitude(v1 - v2);
}
Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) {
Vector3 Vector3::Cross(const Vector3 &v1, const Vector3 &v2) {
return Vector3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z,
v1.x * v2.y - v1.y * v2.x);
}
Vector3 Vector3::Project(const Vector3& v, const Vector3& n) {
Vector3 Vector3::Project(const Vector3 &v, const Vector3 &n) {
float sqrMagnitude = Dot(n, n);
if (sqrMagnitude < epsilon)
return Vector3::zero;
@ -175,7 +173,7 @@ Vector3 Vector3::Project(const Vector3& v, const Vector3& n) {
}
}
Vector3 Vector3::ProjectOnPlane(const Vector3& v, const Vector3& n) {
Vector3 Vector3::ProjectOnPlane(const Vector3 &v, const Vector3 &n) {
Vector3 r = v - Project(v, n);
return r;
}
@ -186,7 +184,7 @@ float clamp(float x, float lower, float upper) {
return upperClamp;
}
AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
AngleOf<float> Vector3::Angle(const Vector3 &v1, const Vector3 &v2) {
float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude());
if (denominator < epsilon)
return AngleOf<float>();
@ -195,16 +193,15 @@ AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
float fraction = dot / denominator;
if (isnan(fraction))
return AngleOf<float>::Degrees(
fraction); // short cut to returning NaN universally
fraction); // short cut to returning NaN universally
float cdot = clamp(fraction, -1.0, 1.0);
float r = ((float)acos(cdot));
return AngleOf<float>::Radians(r);
}
AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
const Vector3& v2,
const Vector3& axis) {
AngleOf<float> Vector3::SignedAngle(const Vector3 &v1, const Vector3 &v2,
const Vector3 &axis) {
// angle in [0,180]
AngleOf<float> angle = Vector3::Angle(v1, v2);
@ -218,7 +215,7 @@ AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
return AngleOf<float>(signed_angle);
}
Vector3 Vector3::Lerp(const Vector3& v1, const Vector3& v2, float f) {
Vector3 Vector3::Lerp(const Vector3 &v1, const Vector3 &v2, float f) {
Vector3 v = v1 + (v2 - v1) * f;
return v;
}

58
Vector3.h
View File

@ -14,7 +14,7 @@ extern "C" {
/// This is a C-style implementation
/// This uses the right-handed coordinate system.
typedef struct Vec3 {
protected:
protected:
/// <summary>
/// The right axis of the vector
/// </summary>
@ -31,10 +31,10 @@ typedef struct Vec3 {
} Vec3;
}
namespace Passer {
namespace LinearAlgebra {
template <typename T>
class SphericalOf;
template <typename T> class SphericalOf;
/// @brief A 3-dimensional vector
/// @remark This uses a right-handed carthesian coordinate system.
@ -42,7 +42,7 @@ class SphericalOf;
struct Vector3 : Vec3 {
friend struct Vec3;
public:
public:
/// @brief A new 3-dimensional zero vector
Vector3();
/// @brief A new 3-dimensional vector
@ -88,12 +88,12 @@ struct Vector3 : Vec3 {
/// @return true if it is identical to the given vector
/// @note This uses float comparison to check equality which may have strange
/// effects. Equality on floats should be avoided.
bool operator==(const Vector3& v) const;
bool operator==(const Vector3 &v) const;
/// @brief The vector length
/// @param v The vector for which you need the length
/// @return The vector length
static float Magnitude(const Vector3& v);
static float Magnitude(const Vector3 &v);
/// @brief The vector length
/// @return The vector length
float magnitude() const;
@ -103,7 +103,7 @@ struct Vector3 : Vec3 {
/// @remark The squared length is computationally simpler than the real
/// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
/// calculation of the squared root of C.
static float SqrMagnitude(const Vector3& v);
static float SqrMagnitude(const Vector3 &v);
/// @brief The squared vector length
/// @return The squared vector length
/// @remark The squared length is computationally simpler than the real
@ -114,7 +114,7 @@ struct Vector3 : Vec3 {
/// @brief Convert the vector to a length of 1
/// @param v The vector to convert
/// @return The vector normalized to a length of 1
static Vector3 Normalize(const Vector3& v);
static Vector3 Normalize(const Vector3 &v);
/// @brief Convert the vector to a length of 1
/// @return The vector normalized to a length of 1
Vector3 normalized() const;
@ -126,13 +126,13 @@ struct Vector3 : Vec3 {
/// @brief Subtract a vector from this vector
/// @param v The vector to subtract from this vector
/// @return The result of this subtraction
Vector3 operator-(const Vector3& v) const;
Vector3 operator-=(const Vector3& v);
Vector3 operator-(const Vector3 &v) const;
Vector3 operator-=(const Vector3 &v);
/// @brief Add a vector to this vector
/// @param v The vector to add to this vector
/// @return The result of the addition
Vector3 operator+(const Vector3& v) const;
Vector3 operator+=(const Vector3& v);
Vector3 operator+(const Vector3 &v) const;
Vector3 operator+=(const Vector3 &v);
/// @brief Scale the vector using another vector
/// @param v1 The vector to scale
@ -140,16 +140,16 @@ struct Vector3 : Vec3 {
/// @return The scaled vector
/// @remark Each component of the vector v1 will be multiplied with the
/// matching component from the scaling vector v2.
static Vector3 Scale(const Vector3& v1, const Vector3& v2);
static Vector3 Scale(const Vector3 &v1, const Vector3 &v2);
/// @brief Scale the vector uniformly up
/// @param f The scaling factor
/// @return The scaled vector
/// @remark Each component of the vector will be multipled with the same
/// factor f.
friend Vector3 operator*(const Vector3& v, float f) {
friend Vector3 operator*(const Vector3 &v, float f) {
return Vector3(v.x * f, v.y * f, v.z * f);
}
friend Vector3 operator*(float f, const Vector3& v) {
friend Vector3 operator*(float f, const Vector3 &v) {
// return Vector3(f * v.x, f * v.y, f * v.z);
return Vector3(v.x * f, v.y * f, v.z * f);
}
@ -158,10 +158,10 @@ struct Vector3 : Vec3 {
/// @param f The scaling factor
/// @return The scaled vector
/// @remark Each componet of the vector will be divided by the same factor.
friend Vector3 operator/(const Vector3& v, float f) {
friend Vector3 operator/(const Vector3 &v, float f) {
return Vector3(v.x / f, v.y / f, v.z / f);
}
friend Vector3 operator/(float f, const Vector3& v) {
friend Vector3 operator/(float f, const Vector3 &v) {
// return Vector3(f / v.x, f / v.y, f / v.z);
return Vector3(v.x / f, v.y / f, v.z / f);
}
@ -171,31 +171,31 @@ struct Vector3 : Vec3 {
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The distance between the two vectors
static float Distance(const Vector3& v1, const Vector3& v2);
static float Distance(const Vector3 &v1, const Vector3 &v2);
/// @brief The dot product of two vectors
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The dot product of the two vectors
static float Dot(const Vector3& v1, const Vector3& v2);
static float Dot(const Vector3 &v1, const Vector3 &v2);
/// @brief The cross product of two vectors
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The cross product of the two vectors
static Vector3 Cross(const Vector3& v1, const Vector3& v2);
static Vector3 Cross(const Vector3 &v1, const Vector3 &v2);
/// @brief Project the vector on another vector
/// @param v The vector to project
/// @param n The normal vecto to project on
/// @return The projected vector
static Vector3 Project(const Vector3& v, const Vector3& n);
static Vector3 Project(const Vector3 &v, const Vector3 &n);
/// @brief Project the vector on a plane defined by a normal orthogonal to the
/// plane.
/// @param v The vector to project
/// @param n The normal of the plane to project on
/// @return Teh projected vector
static Vector3 ProjectOnPlane(const Vector3& v, const Vector3& n);
static Vector3 ProjectOnPlane(const Vector3 &v, const Vector3 &n);
/// @brief The angle between two vectors
/// @param v1 The first vector
@ -204,15 +204,14 @@ struct Vector3 : Vec3 {
/// @remark This reterns an unsigned angle which is the shortest distance
/// between the two vectors. Use Vector3::SignedAngle if a signed angle is
/// needed.
static AngleOf<float> Angle(const Vector3& v1, const Vector3& v2);
static AngleOf<float> Angle(const Vector3 &v1, const Vector3 &v2);
/// @brief The signed angle between two vectors
/// @param v1 The starting vector
/// @param v2 The ending vector
/// @param axis The axis to rotate around
/// @return The signed angle between the two vectors
static AngleOf<float> SignedAngle(const Vector3& v1,
const Vector3& v2,
const Vector3& axis);
static AngleOf<float> SignedAngle(const Vector3 &v1, const Vector3 &v2,
const Vector3 &axis);
/// @brief Lerp (linear interpolation) between two vectors
/// @param v1 The starting vector
@ -222,11 +221,12 @@ struct Vector3 : Vec3 {
/// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
/// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
/// between *v1* and *v2* etc.
static Vector3 Lerp(const Vector3& v1, const Vector3& v2, float f);
static Vector3 Lerp(const Vector3 &v1, const Vector3 &v2, float f);
};
} // namespace LinearAlgebra
using namespace LinearAlgebra;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#include "Spherical.h"

10
test/Angle16_test.cc
View File

@ -1,13 +1,11 @@
#if GTEST
#include "gtest/gtest.h"
#include <math.h>
#include <limits>
#include <math.h>
#include "Angle.h"
using namespace LinearAlgebra;
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle16, Construct) {
@ -88,7 +86,7 @@ TEST(Angle16, Normalize) {
r = Angle16::Normalize(Angle16::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
if (false) { // std::numeric_limits<float>::is_iec559) {
if (false) { // std::numeric_limits<float>::is_iec559) {
// Infinites are not supported
r = Angle16::Normalize(Angle16::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
@ -127,7 +125,7 @@ TEST(Angle16, Clamp) {
Angle16::Degrees(-10));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
if (false) { // std::numeric_limits<float>::is_iec559) {
if (false) { // std::numeric_limits<float>::is_iec559) {
// Infinites are not supported
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
Angle16::Degrees(FLOAT_INFINITY));
@ -218,7 +216,7 @@ TEST(Angle16, MoveTowards) {
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
if (false) { // std::numeric_limits<float>::is_iec559) {
if (false) { // std::numeric_limits<float>::is_iec559) {
// infinites are not supported
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90),
FLOAT_INFINITY);

10
test/Angle8_test.cc
View File

@ -1,13 +1,11 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include <math.h>
#include "Angle.h"
using namespace LinearAlgebra;
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle8, Construct) {
@ -88,7 +86,7 @@ TEST(Angle8, Normalize) {
r = Angle8::Normalize(Angle8::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
if (false) { // std::numeric_limits<float>::is_iec559) {
if (false) { // std::numeric_limits<float>::is_iec559) {
// Infinites are not supported
r = Angle8::Normalize(Angle8::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
@ -126,7 +124,7 @@ TEST(Angle8, Clamp) {
Angle8::Degrees(-10));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
if (false) { // std::numeric_limits<float>::is_iec559) {
if (false) { // std::numeric_limits<float>::is_iec559) {
// Infinites are not supported
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
Angle8::Degrees(FLOAT_INFINITY));
@ -217,7 +215,7 @@ TEST(Angle8, MoveTowards) {
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
if (false) { // std::numeric_limits<float>::is_iec559) {
if (false) { // std::numeric_limits<float>::is_iec559) {
// infinites are not supported
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90),
FLOAT_INFINITY);

4
test/AngleSingle_test.cc
View File

@ -1,13 +1,11 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include <math.h>
#include "Angle.h"
using namespace LinearAlgebra;
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(AngleSingle, Construct) {

142
test/Vector2_test.cc
View File

@ -34,32 +34,26 @@ TEST(Vector2, FromPolar) {
EXPECT_FLOAT_EQ(r.y, 0.0F) << "FromPolar(0 0)";
}
TEST(Vector2, Magnitude) {
Vector2 v = Vector2(1, 2);
float m = 0;
TEST(Vector2, Equality) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
bool r = false;
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude 1 2";
r = v1 == v2;
EXPECT_FALSE(r) << "4 5 == 1 2";
m = Vector2::Magnitude(v);
EXPECT_FLOAT_EQ(m, 2.236068F) << "Vector2::Magnitude 1 2";
v = Vector2(-1, -2);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude -1 -2";
v = Vector2(0, 0);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 0) << "v.magnitude 0 0 ";
v2 = Vector2(4, 5);
r = v1 == v2;
EXPECT_TRUE(r) << "4 5 == 1 2";
if (std::numeric_limits<float>::is_iec559) {
v = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude INFINITY INFINITY ";
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
r = v1 == v2;
EXPECT_FALSE(r) << "4 5 == INFINITY INFINITY";
v = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude -INFINITY -INFINITY ";
v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
r = v1 == v2;
EXPECT_FALSE(r) << "-INFINITY -INFINITY == INFINITY INFINITY";
}
}
@ -92,6 +86,35 @@ TEST(Vector2, SqrMagnitude) {
}
}
TEST(Vector2, Magnitude) {
Vector2 v = Vector2(1, 2);
float m = 0;
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude 1 2";
m = Vector2::Magnitude(v);
EXPECT_FLOAT_EQ(m, 2.236068F) << "Vector2::Magnitude 1 2";
v = Vector2(-1, -2);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude -1 -2";
v = Vector2(0, 0);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 0) << "v.magnitude 0 0 ";
if (std::numeric_limits<float>::is_iec559) {
v = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude INFINITY INFINITY ";
v = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude -INFINITY -INFINITY ";
}
}
TEST(Vector2, Normalize) {
bool r = false;
@ -311,56 +334,6 @@ TEST(Vector2, Divide) {
}
}
TEST(Vector2, Dot) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
float f = 0;
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, 14) << "Dot(4 5, 1 2)";
v2 = Vector2(-1, -2);
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, -14) << "Dot(4 5, -1 -2)";
v2 = Vector2(0, 0);
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, 0) << "Dot(4 5, 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Dot(4 5, INFINITY INFINITY)";
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, -FLOAT_INFINITY) << "Dot(4 5, -INFINITY -INFINITY)";
}
}
TEST(Vector2, Equality) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
bool r = false;
r = v1 == v2;
EXPECT_FALSE(r) << "4 5 == 1 2";
v2 = Vector2(4, 5);
r = v1 == v2;
EXPECT_TRUE(r) << "4 5 == 1 2";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
r = v1 == v2;
EXPECT_FALSE(r) << "4 5 == INFINITY INFINITY";
v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
r = v1 == v2;
EXPECT_FALSE(r) << "-INFINITY -INFINITY == INFINITY INFINITY";
}
}
TEST(Vector2, Distance) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
@ -388,6 +361,33 @@ TEST(Vector2, Distance) {
}
}
TEST(Vector2, Dot) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
float f = 0;
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, 14) << "Dot(4 5, 1 2)";
v2 = Vector2(-1, -2);
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, -14) << "Dot(4 5, -1 -2)";
v2 = Vector2(0, 0);
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, 0) << "Dot(4 5, 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Dot(4 5, INFINITY INFINITY)";
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector2::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, -FLOAT_INFINITY) << "Dot(4 5, -INFINITY -INFINITY)";
}
}
TEST(Vector2, Angle) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);