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

Add 'ControlCore/' from commit '0a57e6d99abadc3257c6b1fdf5880b993e0d0fcb'

Browse Source 
git-subtree-dir: ControlCore
git-subtree-mainline: 2b5f5a58ac608aca3aad452a87f6cb27f428cbde
git-subtree-split: 0a57e6d99abadc3257c6b1fdf5880b993e0d0fcb
This commit is contained in:
Pascal Serrarens 2024-12-28 11:01:12 +01:00
commit 07f78105aa
61 changed files with 11751 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ControlCore/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
build
.vscode

61
ControlCore/CMakeLists.txt Normal file
View File

@ -0,0 +1,61 @@
cmake_minimum_required(VERSION 3.13) # CMake version check
if(ESP_PLATFORM)
idf_component_register(
SRC_DIRS "."
INCLUDE_DIRS "."
)
else()
project(ControlCore)
add_subdirectory(LinearAlgebra)
set(CMAKE_CXX_STANDARD 11) # Enable c++11 standard
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
add_compile_definitions(GTEST)
include(FetchContent)
FetchContent_Declare(
googletest
DOWNLOAD_EXTRACT_TIMESTAMP ON
URL https://github.com/google/googletest/archive/refs/heads/main.zip
)
# For Windows: Prevent overriding the parent project's compiler/linker settings
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
FetchContent_MakeAvailable(googletest)
include_directories(
.
LinearAlgebra
)
add_library(ControlCore STATIC
"Thing.cpp"
"LowLevelMessages.cpp"
"Messages.cpp"
"Participant.cpp"
"float16.cpp"
)
enable_testing()
file(GLOB_RECURSE test_srcs test/*_test.cc)
add_executable(
ControlCoreTest
${test_srcs}
)
target_link_libraries(
ControlCoreTest
gtest_main
ControlCore
)
if(MSVC)
target_compile_options(ControlCoreTest PRIVATE /W4 /WX)
else()
target_compile_options(ControlCoreTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
endif()
include(GoogleTest)
gtest_discover_tests(ControlCoreTest)
endif()

2
ControlCore/LinearAlgebra/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
build
.vscode

32
ControlCore/LinearAlgebra/.gitlab-ci.yml Normal file
View File

@ -0,0 +1,32 @@
# You can override the included template(s) by including variable overrides
# SAST customization: https://docs.gitlab.com/ee/user/application_security/sast/#customizing-the-sast-settings
# Secret Detection customization: https://docs.gitlab.com/ee/user/application_security/secret_detection/#customizing-settings
# Dependency Scanning customization: https://docs.gitlab.com/ee/user/application_security/dependency_scanning/#customizing-the-dependency-scanning-settings
# Note that environment variables can be set in several places
# See https://docs.gitlab.com/ee/ci/variables/#cicd-variable-precedence
#
# Specify the docker image to use (only used if using docker runners)
# See http://doc.gitlab.com/ee/ci/docker/using_docker_images.html)
default:
image: rikorose/gcc-cmake
stages:
- test
unit-test-job:
stage: test
script:
- mkdir build
- cd build
- cmake ..
- cmake --build .
- export GTEST_OUTPUT="xml:report.xml"
- ls -la
- "./LinearAlgebraTest"
artifacts:
when: always
reports:
junit: build/report.xml
sast:
stage: test
include:
- template: Security/SAST.gitlab-ci.yml

375
ControlCore/LinearAlgebra/Angle.cpp Normal file
View File

@ -0,0 +1,375 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Angle.h"
#include "FloatSingle.h"
#include <math.h>
const float Rad2Deg = 57.29578F;
const float Deg2Rad = 0.0174532924F;
/*
float Angle::Normalize(float angle) {
if (!isfinite(angle))
return angle;
while (angle <= -180)
angle += 360;
while (angle > 180)
angle -= 360;
return angle;
}
*/
//----------------------
template <typename T> AngleOf<T>::AngleOf() : value(0) {}
template <typename T> const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
// template <typename T>
// const AngleOf<T> AngleOf<T>::deg90 = AngleOf<T>::Degrees(90);
// template <typename T>
// const AngleOf<T> AngleOf<T>::deg180 = AngleOf<T>::Degrees(180);
//===== AngleSingle, AngleOf<float>
template <> AngleOf<float> AngleOf<float>::Degrees(float degrees) {
if (isfinite(degrees)) {
while (degrees < -180)
degrees += 360;
while (degrees >= 180)
degrees -= 360;
}
return Binary(degrees);
}
template <> AngleOf<float> AngleOf<float>::Radians(float radians) {
if (isfinite(radians)) {
while (radians <= -pi)
radians += 2 * pi;
while (radians > pi)
radians -= 2 * pi;
}
return Binary(radians * Rad2Deg);
}
template <> float AngleOf<float>::InDegrees() const { return this->value; }
template <> float AngleOf<float>::InRadians() const {
return this->value * Deg2Rad;
}
//===== Angle16, AngleOf<signed short>
template <>
AngleOf<signed short> AngleOf<signed short>::Degrees(float degrees) {
// map float [-180..180) to integer [-32768..32767]
signed short value = (signed short)roundf(degrees / 360.0F * 65536.0F);
return Binary(value);
}
template <>
AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
if (!isfinite(radians))
return AngleOf<signed short>::zero;
// map float [-PI..PI) to integer [-32768..32767]
signed short value = (signed short)roundf(radians / pi * 32768.0F);
return Binary(value);
}
template <> float AngleOf<signed short>::InDegrees() const {
float degrees = this->value / 65536.0f * 360.0f;
return degrees;
}
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) {
// 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) {
if (!isfinite(radians))
return AngleOf<signed char>::zero;
// map float [-pi..pi) to integer [-128..127)
signed char value = (signed char)roundf(radians / pi * 128.0f);
return Binary(value);
}
template <> float AngleOf<signed char>::InDegrees() const {
float degrees = this->value / 256.0f * 360.0f;
return degrees;
}
template <> float AngleOf<signed char>::InRadians() const {
float radians = this->value / 128.0f * pi;
return radians;
}
//===== Generic
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) {
this->value = rawValue;
}
template <typename T>
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 {
return this->value > angle.value;
}
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 {
return this->value < angle.value;
}
template <typename T> bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
return this->value <= angle.value;
}
template <typename T>
signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> angle) {
if (angle.value < 0)
return -1;
if (angle.value > 0)
return 1;
return 0;
}
template <typename T>
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 {
AngleOf<T> angle = Binary(-this->value);
return angle;
}
template <>
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> r = Binary(this->value - angle.value);
return r;
}
template <>
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> r = Binary(this->value + angle.value);
return r;
}
template <>
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) {
this->value += angle.value;
return *this;
}
// This defintion is not matching the declaration in the header file somehow
// template <typename T>
// AngleOf<T> operator*(const AngleOf<T> &angle, float factor) {
// return AngleOf::Degrees((float)angle.InDegrees() * factor);
// }
// This defintion is not matching the declaration in the header file somehow
// template <typename T>
// AngleOf<T> operator*(float factor, const AngleOf<T> &angle) {
// return AngleOf::Degrees((float)factor * angle.InDegrees());
// }
template <typename T> void AngleOf<T>::Normalize() {
float angleValue = this->InDegrees();
if (!isfinite(angleValue))
return;
while (angleValue <= -180)
angleValue += 360;
while (angleValue > 180)
angleValue -= 360;
*this = AngleOf::Degrees(angleValue);
}
template <typename T> AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
float angleValue = angle.InDegrees();
if (!isfinite(angleValue))
return angle;
while (angleValue <= -180)
angleValue += 360;
while (angleValue > 180)
angleValue -= 360;
return AngleOf::Degrees(angleValue);
}
template <typename T>
AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
float r = Float::Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees());
return AngleOf<T>::Degrees(r);
}
template <typename T>
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
float maxDegrees) {
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));
if (Sign(d) < 0)
d = -d;
return fromAngle + d;
}
template <typename T> float AngleOf<T>::Cos(AngleOf<T> angle) {
return cosf(angle.InRadians());
}
template <typename T> float AngleOf<T>::Sin(AngleOf<T> angle) {
return sinf(angle.InRadians());
}
template <typename T> float AngleOf<T>::Tan(AngleOf<T> angle) {
return tanf(angle.InRadians());
}
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) {
return AngleOf<T>::Radians(asinf(f));
}
template <typename T> AngleOf<T> AngleOf<T>::Atan(float f) {
return AngleOf<T>::Radians(atanf(f));
}
template <typename T>
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Atan2(float y, float x) {
return AngleOf<T>::Radians(atan2f(y, x));
}
// template <>
// float AngleOf<float>::CosineRuleSide(float a, float b, AngleOf<float> gamma)
// {
// float a2 = a * a;
// float b2 = b * b;
// float d =
// a2 + b2 -
// 2 * a * b * Cos(gamma); // cosf(gamma *
// Passer::LinearAlgebra::Deg2Rad);
// // Catch edge cases where float inacuracies lead tot nans
// if (d < 0)
// return 0.0f;
// float c = sqrtf(d);
// return c;
// }
template <typename T>
float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
float a2 = a * a;
float b2 = b * b;
float d =
a2 + b2 -
2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
// Catch edge cases where float inacuracies lead tot nans
if (d < 0)
return 0;
float c = sqrtf(d);
return c;
}
// template <>
// AngleOf<float> AngleOf<float>::CosineRuleAngle(float a, float b, float c) {
// float a2 = a * a;
// float b2 = b * b;
// float c2 = c * c;
// float d = (a2 + b2 - c2) / (2 * a * b);
// // Catch edge cases where float inacuracies lead tot nans
// if (d >= 1)
// return 0.0f;
// if (d <= -1)
// return 180.0f;
// float gamma = acosf(d) * Rad2Deg;
// return gamma;
// }
template <typename T>
AngleOf<T> AngleOf<T>::CosineRuleAngle(float a, float b, float c) {
float a2 = a * a;
float b2 = b * b;
float c2 = c * c;
float d = (a2 + b2 - c2) / (2 * a * b);
// Catch edge cases where float inacuracies lead tot nans
if (d >= 1)
return AngleOf<T>();
if (d <= -1)
return AngleOf<T>::Degrees(180);
// float gamma = acosf(d) * Rad2Deg;
AngleOf<T> gamma = Acos(d);
return gamma;
}
// template <>
// AngleOf<float> AngleOf<float>::SineRuleAngle(float a,
// AngleOf<float> beta,
// float b) {
// float deg2rad = Deg2Rad;
// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
// return alpha;
// }
template <typename T>
AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
// float deg2rad = Deg2Rad;
// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
AngleOf<T> alpha = Asin(a * Sin(beta) / b);
return alpha;
}
template class AngleOf<float>;
template class AngleOf<signed char>;
template class AngleOf<signed short>;

226
ControlCore/LinearAlgebra/Angle.h Normal file
View File

@ -0,0 +1,226 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef ANGLE_H
#define ANGLE_H
namespace Passer {
namespace LinearAlgebra {
static float pi = 3.1415927410125732421875F;
static float Rad2Deg = 360.0f / (pi * 2);
static float Deg2Rad = (pi * 2) / 360.0f;
/// @brief An angle in various representations.
/// @tparam T The internal type used for the representation of the angle.
/// 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:
/// @brief Create a new angle with a zero value
AngleOf<T>();
/// @brief An zero value angle
const static AngleOf<T> zero;
// const static AngleOf<T> deg90;
// const static AngleOf<T> deg180;
/// @brief Creates an angle in degrees
/// @param degrees the angle in degrees
/// @return The angle value
static AngleOf<T> Degrees(float degrees);
/// @brief Short-hand Deg alias for the Degrees function
constexpr static auto Deg = Degrees;
/// @brief Creates an angle in radians
/// @param radians the angle in radians
/// @return The angle value
static AngleOf<T> Radians(float radians);
/// @brief Short-hand Rad alias for the Radians function
constexpr static auto Rad = Radians;
/// @brief Creates an angle from a raw value
/// @param rawValue the raw value to use for the angle
/// @return The the angle
static AngleOf<T> Binary(T rawValue);
/// @brief Get the angle value in degrees
/// @return The angle value in degrees
float InDegrees() const;
/// @brief Get the angle value in radians
/// @return The angle value in radians
float InRadians() const;
/// @brief Get the raw value for the angle
/// @return The raw value
T GetBinary() const;
/// @brief Set the raw value of the angle
/// @param rawValue The raw value
void SetBinary(T rawValue);
/// @brief Tests whether this angle is equal to the given angle
/// @param angle The angle to compare to
/// @return True when the angles are equal, False otherwise
/// @note The equality is determine within the limits of precision of the raw
/// type T
bool operator==(const AngleOf<T> angle) const;
/// @brief Tests if this angle is greater than the given angle
/// @param angle The given angle
/// @return True when this angle is greater than the given angle, False
/// otherwise
bool operator>(AngleOf<T> angle) const;
/// @brief Tests if this angle is greater than or equal to the given angle
/// @param angle The given angle
/// @return True when this angle is greater than or equal to the given angle.
/// False otherwise.
bool operator>=(AngleOf<T> angle) const;
/// @brief Tests if this angle is less than the given angle
/// @param angle The given angle
/// @return True when this angle is less than the given angle. False
/// otherwise.
bool operator<(AngleOf<T> angle) const;
/// @brief Tests if this angle is less than or equal to the given angle
/// @param angle The given angle
/// @return True when this angle is less than or equal to the given angle.
/// False otherwise.
bool operator<=(AngleOf<T> angle) const;
/// @brief Returns the sign of the angle
/// @param angle The angle
/// @return -1 when the angle is negative, 1 when it is positive and 0
/// otherwise.
static signed int Sign(AngleOf<T> angle);
/// @brief Returns the magnitude of the angle
/// @param angle The angle
/// @return The positive magitude of the angle.
/// Negative values are negated to get a positive result
static AngleOf<T> Abs(AngleOf<T> angle);
/// @brief Negate the angle
/// @return The negated angle
AngleOf<T> operator-() const;
/// @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;
/// @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;
/// @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);
/// @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) {
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) {
return AngleOf::Degrees((float)factor * angle.InDegrees());
}
/// @brief Normalizes the angle to (-180..180] or (-PI..PI]
/// Should not be needed but available in case it is.
void Normalize();
/// @brief Normalizes the angle to (-180..180] or (-PI..PI]
/// @param angle The angle to normalize
/// @return The normalized angle;
static AngleOf<T> Normalize(AngleOf<T> angle);
/// @brief Clamps the angle value between the two given angles
/// @param angle The angle to clamp
/// @param min The minimum angle
/// @param max The maximum angle
/// @return The clamped value
/// @remark When the min value is greater than the max value, angle is
/// returned unclamped.
static AngleOf<T> Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max);
// static AngleOf<T> Difference(AngleOf<T> a, AngleOf<T> b) {
// AngleOf<T> r = Normalize(b.InDegrees() - a.InDegrees());
// return r;
// };
/// @brief Rotates an angle towards another angle with a max distance
/// @param fromAngle The angle to start from
/// @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,
float maxAngle);
/// @brief Calculates the cosine of an angle
/// @param angle The given angle
/// @return The cosine of the angle
static float Cos(AngleOf<T> angle);
/// @brief Calculates the sine of an angle
/// @param angle The given angle
/// @return The sine of the angle
static float Sin(AngleOf<T> angle);
/// @brief Calculates the tangent of an angle
/// @param angle The given angle
/// @return The tangent of the angle
static float Tan(AngleOf<T> angle);
/// @brief Calculates the arc cosine angle
/// @param f The value
/// @return The arc cosine for the given value
static AngleOf<T> Acos(float f);
/// @brief Calculates the arc sine angle
/// @param f The value
/// @return The arc sine for the given value
static AngleOf<T> Asin(float f);
/// @brief Calculates the arc tangent angle
/// @param f The value
/// @return The arc tangent for the given value
static AngleOf<T> Atan(float f);
/// @brief Calculates the tangent for the given values
/// @param y The vertical value
/// @param x The horizontal value
/// @return The tanget for the given values
/// Uses the y and x signs to compute the quadrant
static AngleOf<T> Atan2(float y, float x);
/// @brief Computes the length of a side using the rule of cosines
/// @param a The length of side A
/// @param b The length of side B
/// @param gamma The angle of the corner opposing side C
/// @return The length of side C
static float CosineRuleSide(float a, float b, AngleOf<T> gamma);
/// @brief Computes the angle of a corner using the rule of cosines
/// @param a The length of side A
/// @param b The length of side B
/// @param c The length of side C
/// @return The angle of the corner opposing side C
static AngleOf<T> CosineRuleAngle(float a, float b, float c);
/// @brief Computes the angle of a corner using the rule of sines
/// @param a The length of side A
/// @param beta the angle of the corner opposing side B
/// @param c The length of side C
/// @return The angle of the corner opposing side A
static AngleOf<T> SineRuleAngle(float a, AngleOf<T> beta, float c);
private:
T value;
AngleOf<T>(T value);
};
// using Angle = AngleOf<float>;
using AngleSingle = AngleOf<float>;
using Angle16 = AngleOf<signed short>;
using Angle8 = AngleOf<signed char>;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

53
ControlCore/LinearAlgebra/AngleAxis.cpp Normal file
View File

@ -0,0 +1,53 @@
/*
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "AngleAxis.h"
template <typename T>
AngleAxisOf<T>::AngleAxisOf() {
this->angle = 0.0F;
this->axis = DirectionOf<T>();
}
template <typename T>
AngleAxisOf<T>::AngleAxisOf(float angle, DirectionOf<T> axis) {
this->angle = angle;
this->axis = axis;
}
template <typename T>
AngleAxisOf<T>::AngleAxisOf(float angle, Vector3 axis) {
this->angle = angle;
this->axis = DirectionOf<T>::FromVector3(axis);
}
template <typename T>
AngleAxisOf<T>::AngleAxisOf(Quaternion q) {
float angle;
Vector3 axis;
q.ToAngleAxis(&angle, &axis);
this->angle = angle;
this->axis = DirectionOf<T>::FromVector3(axis);
}
template <typename T>
const AngleAxisOf<T> AngleAxisOf<T>::zero =
AngleAxisOf<T>(0.0, DirectionOf<T>(AngleOf<T>(), AngleOf<T>()));
template <typename T>
Quaternion AngleAxisOf<T>::ToQuaternion() {
Vector3 axisVector = this->axis.ToVector3();
Quaternion q = Quaternion::AngleAxis(this->angle, axisVector);
return q;
}
template <typename T>
DirectionOf<T> AngleAxisOf<T>::GetSwing() {
return this->axis;
}
template class AngleAxisOf<float>;
template class AngleAxisOf<signed short>;
*/

52
ControlCore/LinearAlgebra/AngleUsing.h Normal file
View File

@ -0,0 +1,52 @@
/*
#ifndef DISCRETEANGLE_H
#define DISCRETEANGLE_H
#include "Angle.h"
#include "Range.h"
namespace Passer {
namespace LinearAlgebra {
// A fixed angle between (-180..180]
template <typename T>
class AngleUsing {
public:
AngleUsing(T sourceValue) { this->value = sourceValue; }
AngleUsing(float f);
float ToFloat() const;
inline T GetValue() const { return this->value; }
AngleUsing<T> operator+(const AngleUsing<T> a) {
AngleUsing<T> r = AngleUsing((float)this->value + a.value);
return r;
}
inline AngleUsing<T> operator+=(const AngleUsing<T> a) {
return this->value + a.value;
}
inline AngleUsing<T> operator-(const AngleUsing<T> a) {
return this->value - a.value;
}
inline AngleUsing<T> operator-() {
this->value = -this->value;
return *this;
}
inline bool operator==(const AngleUsing<T> a) {
return this->value == a.value;
}
// protected:
T value;
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif
*/

64
ControlCore/LinearAlgebra/CMakeLists.txt Normal file
View File

@ -0,0 +1,64 @@
cmake_minimum_required(VERSION 3.13) # CMake version check
if(ESP_PLATFORM)
idf_component_register(
SRC_DIRS "."
INCLUDE_DIRS "."
)
else()
project(LinearAlgebra)
set(CMAKE_CXX_STANDARD 11) # Enable c++11 standard
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
add_compile_definitions(GTEST)
include(FetchContent)
FetchContent_Declare(
googletest
DOWNLOAD_EXTRACT_TIMESTAMP
URL https://github.com/google/googletest/archive/refs/heads/main.zip
)
# For Windows: Prevent overriding the parent project's compiler/linker settings
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
FetchContent_MakeAvailable(googletest)
include_directories(.)
add_library(LinearAlgebra STATIC
"FloatSingle.cpp"
"Angle.cpp"
"Vector2.cpp"
"Vector3.cpp"
"Quaternion.cpp"
"Polar.cpp"
"Spherical.cpp"
"Matrix.cpp"
# "Axis.cpp"
# "AngleAxis.cpp"
"SwingTwist.cpp"
"Direction.cpp"
)
enable_testing()
file(GLOB_RECURSE test_srcs test/*_test.cc)
add_executable(
LinearAlgebraTest
${test_srcs}
)
target_link_libraries(
LinearAlgebraTest
gtest_main
LinearAlgebra
)
if(MSVC)
target_compile_options(LinearAlgebraTest PRIVATE /W4 /WX)
else()
target_compile_options(LinearAlgebraTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
endif()
include(GoogleTest)
gtest_discover_tests(LinearAlgebraTest)
endif()

111
ControlCore/LinearAlgebra/Direction.cpp Normal file
View File

@ -0,0 +1,111 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Direction.h"
#include "Quaternion.h"
#include "Vector3.h"
#include <math.h>
template <typename T>
DirectionOf<T>::DirectionOf() {
this->horizontal = AngleOf<T>();
this->vertical = AngleOf<T>();
}
template <typename T>
DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
this->horizontal = horizontal;
this->vertical = vertical;
Normalize();
};
template <typename T>
const DirectionOf<T> DirectionOf<T>::forward =
DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::back =
DirectionOf<T>(AngleOf<T>::Degrees(180), AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::up =
DirectionOf<T>(AngleOf<T>(), AngleOf<T>::Degrees(90));
template <typename T>
const DirectionOf<T> DirectionOf<T>::down =
DirectionOf<T>(AngleOf<T>(), AngleOf<T>::Degrees(-90));
template <typename T>
const DirectionOf<T> DirectionOf<T>::left =
DirectionOf<T>(AngleOf<T>::Degrees(-90), AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::right =
DirectionOf<T>(AngleOf<T>::Degrees(90), AngleOf<T>());
template <typename T>
Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
this->horizontal.InDegrees(), 0);
Vector3 v = q * Vector3::forward;
return v;
}
template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 v) {
DirectionOf<T> d;
d.horizontal = AngleOf<T>::Atan2(
v.Right(),
v.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
d.vertical =
AngleOf<T>::Degrees(-90) -
AngleOf<T>::Acos(
v.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
d.Normalize();
return d;
}
template <typename T>
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> Passer::LinearAlgebra::DirectionOf<T>::Radians(float horizontal,
float vertical) {
return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
AngleOf<T>::Radians(vertical));
}
template <typename T>
bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
const DirectionOf<T> d) const {
return (this->horizontal == d.horizontal) && (this->vertical == d.vertical);
}
template <typename T>
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>
Vector3 DirectionOf<T>::ToVector3() {
Vector3 v = Quaternion::Euler(-this->vertical.InDegrees(),
this->horizontal.InDegrees(), 0) *
Vector3::forward;
return v;
}
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);
this->vertical = AngleOf<T>::Degrees(180) - this->vertical;
}
}
template class DirectionOf<float>;
template class DirectionOf<signed short>;

57
ControlCore/LinearAlgebra/Direction.h Normal file
View File

@ -0,0 +1,57 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef DIRECTION_H
#define DIRECTION_H
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
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;
DirectionOf<T>();
DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
Vector3 ToVector3() const;
static DirectionOf<T> FromVector3(Vector3 v);
static DirectionOf<T> Degrees(float horizontal, float vertical);
static DirectionOf<T> Radians(float horizontal, float vertical);
const static DirectionOf forward;
const static DirectionOf back;
const static DirectionOf up;
const static DirectionOf down;
const static DirectionOf left;
const static DirectionOf right;
bool operator==(const DirectionOf<T> d) const;
DirectionOf<T> operator-() const;
Vector3 ToVector3();
protected:
void Normalize();
};
using DirectionSingle = DirectionOf<float>;
using Direction16 = DirectionOf<signed short>;
using Direction = DirectionOf<float>;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

122
ControlCore/LinearAlgebra/DoxyGen/DoxyWarnLogfile.txt Normal file
View File

@ -0,0 +1,122 @@
warning: source 'images' is not a readable file or directory... skipping.
d:/PlatformIO/linear-algebra/Quaternion.cpp:100: warning: no uniquely matching class member found for
Quaternion Quaternion::operator*(const Quaternion &r2) const
Possible candidates:
'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 120 of file d:/PlatformIO/linear-algebra/Angle.h
'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 127 of file d:/PlatformIO/linear-algebra/Angle.h
'Vector3 Passer::LinearAlgebra::MatrixOf< T >::operator*(const Vector3 v) const' at line 64 of file d:/PlatformIO/linear-algebra/Matrix.h
'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 100 of file d:/PlatformIO/linear-algebra/Polar.h
'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 103 of file d:/PlatformIO/linear-algebra/Polar.h
'Vector3 Passer::LinearAlgebra::Quaternion::operator*(const Vector3 &vector) const' at line 98 of file d:/PlatformIO/linear-algebra/Quaternion.h
'Quaternion Passer::LinearAlgebra::Quaternion::operator*(const Quaternion &rotation) const' at line 106 of file d:/PlatformIO/linear-algebra/Quaternion.h
'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 109 of file d:/PlatformIO/linear-algebra/Spherical.h
'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 112 of file d:/PlatformIO/linear-algebra/Spherical.h
'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 45 of file d:/PlatformIO/linear-algebra/SwingTwist.h
'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 53 of file d:/PlatformIO/linear-algebra/SwingTwist.h
'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 143 of file d:/PlatformIO/linear-algebra/Vector2.h
'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 146 of file d:/PlatformIO/linear-algebra/Vector2.h
'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 151 of file d:/PlatformIO/linear-algebra/Vector3.h
'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 154 of file d:/PlatformIO/linear-algebra/Vector3.h
d:/PlatformIO/linear-algebra/Quaternion.cpp:108: warning: no uniquely matching class member found for
Vector3 Quaternion::operator*(const Vector3 &p) const
Possible candidates:
'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 120 of file d:/PlatformIO/linear-algebra/Angle.h
'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 127 of file d:/PlatformIO/linear-algebra/Angle.h
'Vector3 Passer::LinearAlgebra::MatrixOf< T >::operator*(const Vector3 v) const' at line 64 of file d:/PlatformIO/linear-algebra/Matrix.h
'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 100 of file d:/PlatformIO/linear-algebra/Polar.h
'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 103 of file d:/PlatformIO/linear-algebra/Polar.h
'Vector3 Passer::LinearAlgebra::Quaternion::operator*(const Vector3 &vector) const' at line 98 of file d:/PlatformIO/linear-algebra/Quaternion.h
'Quaternion Passer::LinearAlgebra::Quaternion::operator*(const Quaternion &rotation) const' at line 106 of file d:/PlatformIO/linear-algebra/Quaternion.h
'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 109 of file d:/PlatformIO/linear-algebra/Spherical.h
'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 112 of file d:/PlatformIO/linear-algebra/Spherical.h
'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 45 of file d:/PlatformIO/linear-algebra/SwingTwist.h
'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 53 of file d:/PlatformIO/linear-algebra/SwingTwist.h
'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 143 of file d:/PlatformIO/linear-algebra/Vector2.h
'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 146 of file d:/PlatformIO/linear-algebra/Vector2.h
'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 151 of file d:/PlatformIO/linear-algebra/Vector3.h
'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 154 of file d:/PlatformIO/linear-algebra/Vector3.h
d:/PlatformIO/linear-algebra/Quaternion.cpp:152: warning: no uniquely matching class member found for
Quaternion Quaternion::LookRotation(const Vector3 &forward, const Vector3 &up)
Possible candidates:
'static Quaternion Passer::LinearAlgebra::Quaternion::LookRotation(const Vector3 &forward, const Vector3 &upwards)' at line 132 of file d:/PlatformIO/linear-algebra/Quaternion.h
'static Quaternion Passer::LinearAlgebra::Quaternion::LookRotation(const Vector3 &forward)' at line 143 of file d:/PlatformIO/linear-algebra/Quaternion.h
d:/PlatformIO/linear-algebra/Quaternion.cpp:330: warning: no uniquely matching class member found for
Quaternion Quaternion::Euler(Vector3 euler)
Possible candidates:
'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(float x, float y, float z)' at line 218 of file d:/PlatformIO/linear-algebra/Quaternion.h
'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(Vector3 eulerAngles)' at line 225 of file d:/PlatformIO/linear-algebra/Quaternion.h
d:/PlatformIO/linear-algebra/Quaternion.cpp:362: warning: no uniquely matching class member found for
Quaternion Quaternion::EulerXYZ(Vector3 euler)
Possible candidates:
'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(float x, float y, float z)' at line 235 of file d:/PlatformIO/linear-algebra/Quaternion.h
'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(Vector3 eulerAngles)' at line 242 of file d:/PlatformIO/linear-algebra/Quaternion.h
d:/PlatformIO/linear-algebra/Spherical.cpp:137: warning: no uniquely matching class member found for
template < T >
SphericalOf< T > SphericalOf::operator-(const SphericalOf< T > &s2) const
Possible candidates:
'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-() const' at line 102 of file d:/PlatformIO/linear-algebra/Angle.h
'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-(const AngleOf< T > &angle) const' at line 106 of file d:/PlatformIO/linear-algebra/Angle.h
'DirectionOf< T > Passer::LinearAlgebra::DirectionOf< T >::operator-() const' at line 41 of file d:/PlatformIO/linear-algebra/Direction.h
'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-() const' at line 82 of file d:/PlatformIO/linear-algebra/Polar.h
'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-(const PolarOf &v) const' at line 87 of file d:/PlatformIO/linear-algebra/Polar.h
'RangeUsing< T > Passer::LinearAlgebra::RangeUsing< T >::operator-(RangeUsing< T > a)' at line 38 of file d:/PlatformIO/linear-algebra/Range.h
'RangeUsing< T > Passer::LinearAlgebra::RangeUsing< T >::operator-()' at line 40 of file d:/PlatformIO/linear-algebra/Range.h
'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-() const' at line 91 of file d:/PlatformIO/linear-algebra/Spherical.h
'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-(const SphericalOf< T > &v) const' at line 96 of file d:/PlatformIO/linear-algebra/Spherical.h
'Vector2 Passer::LinearAlgebra::Vector2::operator-()' at line 118 of file d:/PlatformIO/linear-algebra/Vector2.h
'Vector2 Passer::LinearAlgebra::Vector2::operator-(const Vector2 &v) const' at line 123 of file d:/PlatformIO/linear-algebra/Vector2.h
'Vector3 Passer::LinearAlgebra::Vector3::operator-() const' at line 126 of file d:/PlatformIO/linear-algebra/Vector3.h
'Vector3 Passer::LinearAlgebra::Vector3::operator-(const Vector3 &v) const' at line 131 of file d:/PlatformIO/linear-algebra/Vector3.h
d:/PlatformIO/linear-algebra/Vector2.cpp:20: warning: no uniquely matching class member found for
Vector2::Vector2(float _x, float _y)
Possible candidates:
'Passer::LinearAlgebra::Vector2::Vector2()' at line 45 of file d:/PlatformIO/linear-algebra/Vector2.h
'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 49 of file d:/PlatformIO/linear-algebra/Vector2.h
'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 53 of file d:/PlatformIO/linear-algebra/Vector2.h
'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 56 of file d:/PlatformIO/linear-algebra/Vector2.h
d:/PlatformIO/linear-algebra/Vector2.cpp:32: warning: no uniquely matching class member found for
Vector2::Vector2(Polar p)
Possible candidates:
'Passer::LinearAlgebra::Vector2::Vector2()' at line 45 of file d:/PlatformIO/linear-algebra/Vector2.h
'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 49 of file d:/PlatformIO/linear-algebra/Vector2.h
'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 53 of file d:/PlatformIO/linear-algebra/Vector2.h
'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 56 of file d:/PlatformIO/linear-algebra/Vector2.h
d:/PlatformIO/linear-algebra/Vector3.cpp:33: warning: no uniquely matching class member found for
Vector3::Vector3(SphericalOf< float > s)
Possible candidates:
'Passer::LinearAlgebra::Vector3::Vector3()' at line 49 of file d:/PlatformIO/linear-algebra/Vector3.h
'Passer::LinearAlgebra::Vector3::Vector3(float right, float up, float forward)' at line 54 of file d:/PlatformIO/linear-algebra/Vector3.h
'Passer::LinearAlgebra::Vector3::Vector3(Vector2 v)' at line 57 of file d:/PlatformIO/linear-algebra/Vector3.h
'Passer::LinearAlgebra::Vector3::Vector3(SphericalOf< float > v)' at line 61 of file d:/PlatformIO/linear-algebra/Vector3.h
d:/PlatformIO/linear-algebra/Matrix.h:12: warning: Member MatrixOf(unsigned int rows, unsigned int cols) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:13: warning: Member MatrixOf(unsigned int rows, unsigned int cols, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:17: warning: Member MatrixOf(Vector3 v) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:59: warning: Member Multiply(const MatrixOf< T > *m, MatrixOf< T > *r) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:64: warning: Member operator*(const Vector3 v) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:66: warning: Member Get(unsigned int rowIx, unsigned int colIx) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:71: warning: Member Set(unsigned int rowIx, unsigned int colIx, T value) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:77: warning: Member Set(const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:84: warning: Member SetRow(unsigned int rowIx, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:91: warning: Member SetCol(unsigned int colIx, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:98: warning: Member CopyFrom(const MatrixOf< T > *m) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:108: warning: Member RowCount() const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:109: warning: Member ColCount() const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:57: warning: Member Multiply(const MatrixOf< T > *m1, const MatrixOf< T > *m2, MatrixOf< T > *r) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Matrix.h:63: warning: Member Multiply(const MatrixOf< T > *m, Vector3 v) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
d:/PlatformIO/linear-algebra/Vector2.h:124: warning: Member operator-=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
d:/PlatformIO/linear-algebra/Vector2.h:129: warning: Member operator+=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
d:/PlatformIO/linear-algebra/Vector2.h:150: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
d:/PlatformIO/linear-algebra/Vector2.h:161: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
d:/PlatformIO/linear-algebra/Vector2.h:146: warning: Member operator*(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented.
d:/PlatformIO/linear-algebra/Vector2.h:158: warning: Member operator/(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:84: warning: Member Forward() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:85: warning: Member Up() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:86: warning: Member Right() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:132: warning: Member operator-=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:137: warning: Member operator+=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:158: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:170: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:154: warning: Member operator*(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:166: warning: Member operator/(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
t f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
d:/PlatformIO/linear-algebra/Vector3.h:166: warning: Member operator/(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.

2684
ControlCore/LinearAlgebra/DoxyGen/Doxyfile Normal file
View File

File diff suppressed because it is too large Load Diff

226
ControlCore/LinearAlgebra/DoxyGen/DoxygenLayout.xml Normal file
View File

@ -0,0 +1,226 @@
<doxygenlayout version="1.0">
<!-- Generated by doxygen 1.8.18 -->
<!-- Navigation index tabs for HTML output -->
<navindex>
<tab type="mainpage" visible="yes" title=""/>
<tab type="pages" visible="yes" title="" intro=""/>
<tab type="modules" visible="yes" title="" intro=""/>
<tab type="namespaces" visible="yes" title="">
<tab type="namespacelist" visible="yes" title="" intro=""/>
<tab type="namespacemembers" visible="yes" title="" intro=""/>
</tab>
<tab type="interfaces" visible="yes" title="">
<tab type="interfacelist" visible="yes" title="" intro=""/>
<tab type="interfaceindex" visible="$ALPHABETICAL_INDEX" title=""/>
<tab type="interfacehierarchy" visible="yes" title="" intro=""/>
</tab>
<tab type="classes" visible="yes" title="">
<tab type="classlist" visible="yes" title="" intro=""/>
<tab type="classindex" visible="$ALPHABETICAL_INDEX" title=""/>
<tab type="hierarchy" visible="yes" title="" intro=""/>
<tab type="classmembers" visible="yes" title="" intro=""/>
</tab>
<tab type="structs" visible="yes" title="">
<tab type="structlist" visible="yes" title="" intro=""/>
<tab type="structindex" visible="$ALPHABETICAL_INDEX" title=""/>
</tab>
<tab type="exceptions" visible="yes" title="">
<tab type="exceptionlist" visible="yes" title="" intro=""/>
<tab type="exceptionindex" visible="$ALPHABETICAL_INDEX" title=""/>
<tab type="exceptionhierarchy" visible="yes" title="" intro=""/>
</tab>
<tab type="files" visible="yes" title="">
<tab type="filelist" visible="yes" title="" intro=""/>
<tab type="globals" visible="yes" title="" intro=""/>
</tab>
<tab type="examples" visible="yes" title="" intro=""/>
</navindex>
<!-- Layout definition for a class page -->
<class>
<briefdescription visible="no"/>
<detaileddescription title=""/>
<includes visible="$SHOW_INCLUDE_FILES"/>
<inheritancegraph visible="$CLASS_GRAPH"/>
<collaborationgraph visible="$COLLABORATION_GRAPH"/>
<memberdecl>
<nestedclasses visible="yes" title=""/>
<publictypes title=""/>
<services title=""/>
<interfaces title=""/>
<publicslots title=""/>
<signals title=""/>
<publicmethods title=""/>
<publicstaticmethods title=""/>
<publicattributes title=""/>
<publicstaticattributes title=""/>
<protectedtypes title=""/>
<protectedslots title=""/>
<protectedmethods title=""/>
<protectedstaticmethods title=""/>
<protectedattributes title=""/>
<protectedstaticattributes title=""/>
<packagetypes title=""/>
<packagemethods title=""/>
<packagestaticmethods title=""/>
<packageattributes title=""/>
<packagestaticattributes title=""/>
<properties title=""/>
<events title=""/>
<privatetypes title=""/>
<privateslots title=""/>
<privatemethods title=""/>
<privatestaticmethods title=""/>
<privateattributes title=""/>
<privatestaticattributes title=""/>
<friends title=""/>
<related title="" subtitle=""/>
<membergroups visible="yes"/>
</memberdecl>
<memberdef>
<inlineclasses title=""/>
<typedefs title=""/>
<enums title=""/>
<services title=""/>
<interfaces title=""/>
<constructors title=""/>
<functions title=""/>
<related title=""/>
<variables title=""/>
<properties title=""/>
<events title=""/>
</memberdef>
<allmemberslink visible="yes"/>
<usedfiles visible="$SHOW_USED_FILES"/>
<authorsection visible="yes"/>
</class>
<!-- Layout definition for a namespace page -->
<namespace>
<briefdescription visible="yes"/>
<memberdecl>
<nestednamespaces visible="yes" title=""/>
<constantgroups visible="yes" title=""/>
<interfaces visible="yes" title=""/>
<classes visible="yes" title=""/>
<structs visible="yes" title=""/>
<exceptions visible="yes" title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<functions title=""/>
<variables title=""/>
<membergroups visible="yes"/>
</memberdecl>
<detaileddescription title=""/>
<memberdef>
<inlineclasses title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<functions title=""/>
<variables title=""/>
</memberdef>
<authorsection visible="yes"/>
</namespace>
<!-- Layout definition for a file page -->
<file>
<briefdescription visible="yes"/>
<includes visible="$SHOW_INCLUDE_FILES"/>
<includegraph visible="$INCLUDE_GRAPH"/>
<includedbygraph visible="$INCLUDED_BY_GRAPH"/>
<sourcelink visible="yes"/>
<memberdecl>
<interfaces visible="yes" title=""/>
<classes visible="yes" title=""/>
<structs visible="yes" title=""/>
<exceptions visible="yes" title=""/>
<namespaces visible="yes" title=""/>
<constantgroups visible="yes" title=""/>
<defines title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<functions title=""/>
<variables title=""/>
<membergroups visible="yes"/>
</memberdecl>
<detaileddescription title=""/>
<memberdef>
<inlineclasses title=""/>
<defines title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<functions title=""/>
<variables title=""/>
</memberdef>
<authorsection/>
</file>
<!-- Layout definition for a group page -->
<group>
<briefdescription visible="yes"/>
<groupgraph visible="$GROUP_GRAPHS"/>
<memberdecl>
<nestedgroups visible="yes" title=""/>
<dirs visible="yes" title=""/>
<files visible="yes" title=""/>
<namespaces visible="yes" title=""/>
<classes visible="yes" title=""/>
<defines title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<enumvalues title=""/>
<functions title=""/>
<variables title=""/>
<signals title=""/>
<publicslots title=""/>
<protectedslots title=""/>
<privateslots title=""/>
<events title=""/>
<properties title=""/>
<friends title=""/>
<membergroups visible="yes"/>
</memberdecl>
<detaileddescription title=""/>
<memberdef>
<pagedocs/>
<inlineclasses title=""/>
<defines title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<enumvalues title=""/>
<functions title=""/>
<variables title=""/>
<signals title=""/>
<publicslots title=""/>
<protectedslots title=""/>
<privateslots title=""/>
<events title=""/>
<properties title=""/>
<friends title=""/>
</memberdef>
<authorsection visible="yes"/>
</group>
<!-- Layout definition for a directory page -->
<directory>
<briefdescription visible="yes"/>
<directorygraph visible="yes"/>
<memberdecl>
<dirs visible="yes"/>
<files visible="yes"/>
</memberdecl>
<detaileddescription title=""/>
</directory>
</doxygenlayout>

12
ControlCore/LinearAlgebra/DoxyGen/custom_doxygen.css Normal file
View File

@ -0,0 +1,12 @@
/* Custom PasserVR CSS for DoxyGen */
a {
color: #e77505;
}
.contents a:visited {
color: #e77505;
}
a:hover {
color: #10659C;
}

19
ControlCore/LinearAlgebra/FloatSingle.cpp Normal file
View File

@ -0,0 +1,19 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "FloatSingle.h"
#include <math.h>
const float Float::epsilon = 1e-05f;
const float Float::sqrEpsilon = 1e-10f;
float Float::Clamp(float f, float min, float max) {
if (max < min)
return f;
if (f < min)
return min;
if (f > max)
return max;
return f;
}

23
ControlCore/LinearAlgebra/FloatSingle.h Normal file
View File

@ -0,0 +1,23 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef FLOAT_H
#define FLOAT_H
namespace Passer {
namespace LinearAlgebra {
class Float {
public:
static const float epsilon;
static const float sqrEpsilon;
static float Clamp(float f, float min, float max);
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

373
ControlCore/LinearAlgebra/LICENSE Normal file
View File

@ -0,0 +1,373 @@
Mozilla Public License Version 2.0
==================================
1. Definitions
--------------
1.1. "Contributor"
means each individual or legal entity that creates, contributes to
the creation of, or owns Covered Software.
1.2. "Contributor Version"
means the combination of the Contributions of others (if any) used
by a Contributor and that particular Contributor's Contribution.
1.3. "Contribution"
means Covered Software of a particular Contributor.
1.4. "Covered Software"
means Source Code Form to which the initial Contributor has attached
the notice in Exhibit A, the Executable Form of such Source Code
Form, and Modifications of such Source Code Form, in each case
including portions thereof.
1.5. "Incompatible With Secondary Licenses"
means
(a) that the initial Contributor has attached the notice described
in Exhibit B to the Covered Software; or
(b) that the Covered Software was made available under the terms of
version 1.1 or earlier of the License, but not also under the
terms of a Secondary License.
1.6. "Executable Form"
means any form of the work other than Source Code Form.
1.7. "Larger Work"
means a work that combines Covered Software with other material, in
a separate file or files, that is not Covered Software.
1.8. "License"
means this document.
1.9. "Licensable"
means having the right to grant, to the maximum extent possible,
whether at the time of the initial grant or subsequently, any and
all of the rights conveyed by this License.
1.10. "Modifications"
means any of the following:
(a) any file in Source Code Form that results from an addition to,
deletion from, or modification of the contents of Covered
Software; or
(b) any new file in Source Code Form that contains any Covered
Software.
1.11. "Patent Claims" of a Contributor
means any patent claim(s), including without limitation, method,
process, and apparatus claims, in any patent Licensable by such
Contributor that would be infringed, but for the grant of the
License, by the making, using, selling, offering for sale, having
made, import, or transfer of either its Contributions or its
Contributor Version.
1.12. "Secondary License"
means either the GNU General Public License, Version 2.0, the GNU
Lesser General Public License, Version 2.1, the GNU Affero General
Public License, Version 3.0, or any later versions of those
licenses.
1.13. "Source Code Form"
means the form of the work preferred for making modifications.
1.14. "You" (or "Your")
means an individual or a legal entity exercising rights under this
License. For legal entities, "You" includes any entity that
controls, is controlled by, or is under common control with You. For
purposes of this definition, "control" means (a) the power, direct
or indirect, to cause the direction or management of such entity,
whether by contract or otherwise, or (b) ownership of more than
fifty percent (50%) of the outstanding shares or beneficial
ownership of such entity.
2. License Grants and Conditions
--------------------------------
2.1. Grants
Each Contributor hereby grants You a world-wide, royalty-free,
non-exclusive license:
(a) under intellectual property rights (other than patent or trademark)
Licensable by such Contributor to use, reproduce, make available,
modify, display, perform, distribute, and otherwise exploit its
Contributions, either on an unmodified basis, with Modifications, or
as part of a Larger Work; and
(b) under Patent Claims of such Contributor to make, use, sell, offer
for sale, have made, import, and otherwise transfer either its
Contributions or its Contributor Version.
2.2. Effective Date
The licenses granted in Section 2.1 with respect to any Contribution
become effective for each Contribution on the date the Contributor first
distributes such Contribution.
2.3. Limitations on Grant Scope
The licenses granted in this Section 2 are the only rights granted under
this License. No additional rights or licenses will be implied from the
distribution or licensing of Covered Software under this License.
Notwithstanding Section 2.1(b) above, no patent license is granted by a
Contributor:
(a) for any code that a Contributor has removed from Covered Software;
or
(b) for infringements caused by: (i) Your and any other third party's
modifications of Covered Software, or (ii) the combination of its
Contributions with other software (except as part of its Contributor
Version); or
(c) under Patent Claims infringed by Covered Software in the absence of
its Contributions.
This License does not grant any rights in the trademarks, service marks,
or logos of any Contributor (except as may be necessary to comply with
the notice requirements in Section 3.4).
2.4. Subsequent Licenses
No Contributor makes additional grants as a result of Your choice to
distribute the Covered Software under a subsequent version of this
License (see Section 10.2) or under the terms of a Secondary License (if
permitted under the terms of Section 3.3).
2.5. Representation
Each Contributor represents that the Contributor believes its
Contributions are its original creation(s) or it has sufficient rights
to grant the rights to its Contributions conveyed by this License.
2.6. Fair Use
This License is not intended to limit any rights You have under
applicable copyright doctrines of fair use, fair dealing, or other
equivalents.
2.7. Conditions
Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
in Section 2.1.
3. Responsibilities
-------------------
3.1. Distribution of Source Form
All distribution of Covered Software in Source Code Form, including any
Modifications that You create or to which You contribute, must be under
the terms of this License. You must inform recipients that the Source
Code Form of the Covered Software is governed by the terms of this
License, and how they can obtain a copy of this License. You may not
attempt to alter or restrict the recipients' rights in the Source Code
Form.
3.2. Distribution of Executable Form
If You distribute Covered Software in Executable Form then:
(a) such Covered Software must also be made available in Source Code
Form, as described in Section 3.1, and You must inform recipients of
the Executable Form how they can obtain a copy of such Source Code
Form by reasonable means in a timely manner, at a charge no more
than the cost of distribution to the recipient; and
(b) You may distribute such Executable Form under the terms of this
License, or sublicense it under different terms, provided that the
license for the Executable Form does not attempt to limit or alter
the recipients' rights in the Source Code Form under this License.
3.3. Distribution of a Larger Work
You may create and distribute a Larger Work under terms of Your choice,
provided that You also comply with the requirements of this License for
the Covered Software. If the Larger Work is a combination of Covered
Software with a work governed by one or more Secondary Licenses, and the
Covered Software is not Incompatible With Secondary Licenses, this
License permits You to additionally distribute such Covered Software
under the terms of such Secondary License(s), so that the recipient of
the Larger Work may, at their option, further distribute the Covered
Software under the terms of either this License or such Secondary
License(s).
3.4. Notices
You may not remove or alter the substance of any license notices
(including copyright notices, patent notices, disclaimers of warranty,
or limitations of liability) contained within the Source Code Form of
the Covered Software, except that You may alter any license notices to
the extent required to remedy known factual inaccuracies.
3.5. Application of Additional Terms
You may choose to offer, and to charge a fee for, warranty, support,
indemnity or liability obligations to one or more recipients of Covered
Software. However, You may do so only on Your own behalf, and not on
behalf of any Contributor. You must make it absolutely clear that any
such warranty, support, indemnity, or liability obligation is offered by
You alone, and You hereby agree to indemnify every Contributor for any
liability incurred by such Contributor as a result of warranty, support,
indemnity or liability terms You offer. You may include additional
disclaimers of warranty and limitations of liability specific to any
jurisdiction.
4. Inability to Comply Due to Statute or Regulation
---------------------------------------------------
If it is impossible for You to comply with any of the terms of this
License with respect to some or all of the Covered Software due to
statute, judicial order, or regulation then You must: (a) comply with
the terms of this License to the maximum extent possible; and (b)
describe the limitations and the code they affect. Such description must
be placed in a text file included with all distributions of the Covered
Software under this License. Except to the extent prohibited by statute
or regulation, such description must be sufficiently detailed for a
recipient of ordinary skill to be able to understand it.
5. Termination
--------------
5.1. The rights granted under this License will terminate automatically
if You fail to comply with any of its terms. However, if You become
compliant, then the rights granted under this License from a particular
Contributor are reinstated (a) provisionally, unless and until such
Contributor explicitly and finally terminates Your grants, and (b) on an
ongoing basis, if such Contributor fails to notify You of the
non-compliance by some reasonable means prior to 60 days after You have
come back into compliance. Moreover, Your grants from a particular
Contributor are reinstated on an ongoing basis if such Contributor
notifies You of the non-compliance by some reasonable means, this is the
first time You have received notice of non-compliance with this License
from such Contributor, and You become compliant prior to 30 days after
Your receipt of the notice.
5.2. If You initiate litigation against any entity by asserting a patent
infringement claim (excluding declaratory judgment actions,
counter-claims, and cross-claims) alleging that a Contributor Version
directly or indirectly infringes any patent, then the rights granted to
You by any and all Contributors for the Covered Software under Section
2.1 of this License shall terminate.
5.3. In the event of termination under Sections 5.1 or 5.2 above, all
end user license agreements (excluding distributors and resellers) which
have been validly granted by You or Your distributors under this License
prior to termination shall survive termination.
************************************************************************
* *
* 6. Disclaimer of Warranty *
* ------------------------- *
* *
* Covered Software is provided under this License on an "as is" *
* basis, without warranty of any kind, either expressed, implied, or *
* statutory, including, without limitation, warranties that the *
* Covered Software is free of defects, merchantable, fit for a *
* particular purpose or non-infringing. The entire risk as to the *
* quality and performance of the Covered Software is with You. *
* Should any Covered Software prove defective in any respect, You *
* (not any Contributor) assume the cost of any necessary servicing, *
* repair, or correction. This disclaimer of warranty constitutes an *
* essential part of this License. No use of any Covered Software is *
* authorized under this License except under this disclaimer. *
* *
************************************************************************
************************************************************************
* *
* 7. Limitation of Liability *
* -------------------------- *
* *
* Under no circumstances and under no legal theory, whether tort *
* (including negligence), contract, or otherwise, shall any *
* Contributor, or anyone who distributes Covered Software as *
* permitted above, be liable to You for any direct, indirect, *
* special, incidental, or consequential damages of any character *
* including, without limitation, damages for lost profits, loss of *
* goodwill, work stoppage, computer failure or malfunction, or any *
* and all other commercial damages or losses, even if such party *
* shall have been informed of the possibility of such damages. This *
* limitation of liability shall not apply to liability for death or *
* personal injury resulting from such party's negligence to the *
* extent applicable law prohibits such limitation. Some *
* jurisdictions do not allow the exclusion or limitation of *
* incidental or consequential damages, so this exclusion and *
* limitation may not apply to You. *
* *
************************************************************************
8. Litigation
-------------
Any litigation relating to this License may be brought only in the
courts of a jurisdiction where the defendant maintains its principal
place of business and such litigation shall be governed by laws of that
jurisdiction, without reference to its conflict-of-law provisions.
Nothing in this Section shall prevent a party's ability to bring
cross-claims or counter-claims.
9. Miscellaneous
----------------
This License represents the complete agreement concerning the subject
matter hereof. If any provision of this License is held to be
unenforceable, such provision shall be reformed only to the extent
necessary to make it enforceable. Any law or regulation which provides
that the language of a contract shall be construed against the drafter
shall not be used to construe this License against a Contributor.
10. Versions of the License
---------------------------
10.1. New Versions
Mozilla Foundation is the license steward. Except as provided in Section
10.3, no one other than the license steward has the right to modify or
publish new versions of this License. Each version will be given a
distinguishing version number.
10.2. Effect of New Versions
You may distribute the Covered Software under the terms of the version
of the License under which You originally received the Covered Software,
or under the terms of any subsequent version published by the license
steward.
10.3. Modified Versions
If you create software not governed by this License, and you want to
create a new license for such software, you may create and use a
modified version of this License if you rename the license and remove
any references to the name of the license steward (except to note that
such modified license differs from this License).
10.4. Distributing Source Code Form that is Incompatible With Secondary
Licenses
If You choose to distribute Source Code Form that is Incompatible With
Secondary Licenses under the terms of this version of the License, the
notice described in Exhibit B of this License must be attached.
Exhibit A - Source Code Form License Notice
-------------------------------------------
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.
If it is not possible or desirable to put the notice in a particular
file, then You may include the notice in a location (such as a LICENSE
file in a relevant directory) where a recipient would be likely to look
for such a notice.
You may add additional accurate notices of copyright ownership.
Exhibit B - "Incompatible With Secondary Licenses" Notice
---------------------------------------------------------
This Source Code Form is "Incompatible With Secondary Licenses", as
defined by the Mozilla Public License, v. 2.0.

62
ControlCore/LinearAlgebra/Matrix.cpp Normal file
View File

@ -0,0 +1,62 @@
#include "Matrix.h"
template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
if (rows <= 0 || cols <= 0) {
this->rows = 0;
this->cols = 0;
this->data = nullptr;
return;
}
this->rows = rows;
this->cols = cols;
unsigned int matrixSize = this->cols * this->rows;
this->data = new float[matrixSize]{0.0f};
}
template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
Set(0, 0, v.Right());
Set(1, 0, v.Up());
Set(2, 0, v.Forward());
}
template <>
void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
const MatrixOf<float> *m2, MatrixOf<float> *r) {
for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) {
for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) {
unsigned int rDataIx = colIx2 * m2->cols + rowIx1;
r->data[rDataIx] = 0.0F;
for (unsigned int kIx = 0; kIx < m2->rows; kIx++) {
unsigned int dataIx1 = rowIx1 * m1->cols + kIx;
unsigned int dataIx2 = kIx * m2->cols + colIx2;
r->data[rDataIx] += m1->data[dataIx1] * m2->data[dataIx2];
}
}
}
}
template <>
Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
MatrixOf<float> v_m = MatrixOf<float>(v);
MatrixOf<float> r_m = MatrixOf<float>(3, 1);
Multiply(m, &v_m, &r_m);
Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
return r;
}
template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
float *vData = new float[3]{v.x, v.y, v.z};
MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
float *rData = new float[3]{};
MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
Multiply(this, &v_m, &r_m);
Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
delete[] vData;
delete[] rData;
return r;
}

121
ControlCore/LinearAlgebra/Matrix.h Normal file
View File

@ -0,0 +1,121 @@
#ifndef MATRIX_H
#define MATRIX_H
#include "Vector3.h"
namespace Passer {
namespace LinearAlgebra {
/// @brief Single precision float matrix
template <typename T> class MatrixOf {
public:
MatrixOf(unsigned int rows, unsigned int cols);
MatrixOf(unsigned int rows, unsigned int cols, const T *source)
: MatrixOf(rows, cols) {
Set(source);
}
MatrixOf(Vector3 v); // creates a 3,1 matrix
~MatrixOf() {
if (this->data == nullptr)
return;
delete[] this->data;
}
/// @brief Transpose with result in matrix m
/// @param r The matrix in which the transposed matrix is stored
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
// We cannot check the data size directly, but the row*col should be equal
unsigned int matrixSize = this->cols * this->rows;
unsigned int resultSize = r->rows * r->cols;
if (matrixSize != resultSize) {
// Return a null matrix;
// We dont set data to nullptr because it is allocated memory
// Instead we write all zeros
for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++)
r->data[dataIx] = 0.0f;
r->rows = 0;
r->cols = 0;
return;
}
r->cols = this->rows;
r->rows = this->cols;
for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
unsigned int rowIx = rDataIx / this->rows;
unsigned int colIx = rDataIx % this->rows;
unsigned int mDataIx = this->cols * colIx + rowIx;
r->data[rDataIx] = this->data[mDataIx];
}
}
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);
Vector3 operator*(const Vector3 v) const;
T Get(unsigned int rowIx, unsigned int colIx) const {
unsigned int dataIx = rowIx * this->cols + colIx;
return this->data[dataIx];
}
void Set(unsigned int rowIx, unsigned int colIx, T value) {
unsigned int dataIx = rowIx * this->cols + colIx;
this->data[dataIx] = value;
}
// This function does not check on source size!
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) {
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) {
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) {
unsigned int thisMatrixSize = this->cols * this->rows;
unsigned int mMatrixSize = m->cols * m->rows;
if (mMatrixSize != thisMatrixSize)
return;
for (unsigned int dataIx = 0; dataIx < thisMatrixSize; dataIx++)
this->data[dataIx] = m->data[dataIx];
}
unsigned int RowCount() const { return rows; }
unsigned int ColCount() const { return cols; }
private:
unsigned int rows;
unsigned int cols;
T *data;
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

165
ControlCore/LinearAlgebra/Polar.cpp Normal file
View File

@ -0,0 +1,165 @@
#include <math.h>
#include "Polar.h"
#include "Vector2.h"
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) {
// distance should always be 0 or greater
if (distance < 0.0f) {
this->distance = -distance;
this->angle = AngleOf<T>::Normalize(angle - AngleOf<T>::Degrees(180));
} else {
this->distance = distance;
if (this->distance == 0.0f)
// angle is always 0 if distance is 0
this->angle = AngleOf<T>();
else
this->angle = AngleOf<T>::Normalize(angle);
}
}
template <typename T>
PolarOf<T> PolarOf<T>::Degrees(float distance, float degrees) {
AngleOf<T> angle = AngleOf<T>::Degrees(degrees);
PolarOf<T> r = PolarOf<T>(distance, angle);
return r;
}
template <typename T>
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) {
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() *
Passer::LinearAlgebra::Deg2Rad);
AngleOf<T> angle = v.direction.horizontal;
PolarOf<T> p = PolarOf(distance, angle);
return p;
}
template <typename T>
const PolarOf<T> PolarOf<T>::zero = PolarOf(0.0f, AngleOf<T>());
template <typename T>
const PolarOf<T> PolarOf<T>::forward = PolarOf(1.0f, AngleOf<T>());
template <typename T>
const PolarOf<T> PolarOf<T>::back = PolarOf(1.0, AngleOf<T>::Degrees(180));
template <typename T>
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 {
return (this->distance == v.distance &&
this->angle.InDegrees() == v.angle.InDegrees());
}
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 {
PolarOf<T> r = PolarOf(1, this->angle);
return r;
}
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 {
PolarOf<T> r = -v;
return *this + r;
}
template <typename T> PolarOf<T> PolarOf<T>::operator-=(const PolarOf &v) {
*this = *this - v;
return *this;
// angle = AngleOf<T>::Normalize(newAngle);
// distance = newDistance;
}
// Polar::Polar(Vector2 v) {
// float signY = (v.y >= 0) - (v.y < 0);
// angle = atan2(v.y, signY * sqrt(v.y * v.y + v.x * v.x)) * Angle::Rad2Deg;
// distance = v.magnitude();
// }
// const Polar Polar::zero = Polar(0, 0);
// float Polar::Distance(const Polar &v1, const Polar &v2) {
// float d =
// Angle::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
// return d;
// }
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)
return v;
float deltaAngle = AngleOf<T>::Normalize(v.angle - this->angle).InDegrees();
float rotation =
deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;
if (rotation == 180.0f && v.distance > 0.0f) {
// angle is too small, take this angle and add the distances
return PolarOf(this->distance + v.distance, this->angle);
}
float newDistance = AngleOf<T>::CosineRuleSide(v.distance, this->distance,
AngleOf<T>::Degrees(rotation));
float angle =
AngleSingle::CosineRuleAngle(newDistance, this->distance, v.distance)
.InDegrees();
float newAngle = deltaAngle < 0.0f ? this->angle.InDegrees() - angle
: this->angle.InDegrees() + angle;
AngleOf<T> newAngleA = AngleOf<T>::Normalize(AngleOf<T>::Degrees(newAngle));
PolarOf vector = PolarOf(newDistance, newAngleA);
return vector;
}
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) {
this->distance *= f;
return *this;
}
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 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) {
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>;

160
ControlCore/LinearAlgebra/Polar.h Normal file
View File

@ -0,0 +1,160 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef POLAR_H
#define POLAR_H
#include "Angle.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector2;
template <typename T> class SphericalOf;
template <typename T> class PolarOf {
public:
/// @brief The distance in meters
/// @remark The distance shall never be negative
float distance;
/// @brief The angle in degrees clockwise rotation
/// @remark The angle shall be between -180 .. 180
AngleOf<T> angle;
/// @brief A new vector with polar coordinates with zero degrees and
/// distance
PolarOf();
/// @brief A new vector with polar coordinates
/// @param distance The distance in meters
/// @param angle The angle in degrees, clockwise rotation
/// @note The distance is automatically converted to a positive value.
/// @note The angle is automatically normalized to -180 .. 180
PolarOf(float distance, AngleOf<T> angle);
/// @brief Create polar vector without using AngleOf type. All given angles
/// are in degrees
/// @param distance The distance in meters
/// @param degrees The angle in degrees
/// @return The polar vector
static PolarOf<T> Degrees(float distance, float degrees);
/// @brief Short-hand Deg alias for the Degrees function
constexpr static auto Deg = Degrees;
/// @brief Create polar vector without using AngleOf type. All given angles
/// are in radians.
/// @param distance The distance in meters
/// @param radians The angle in radians
/// @return The polar vector
static PolarOf<T> Radians(float distance, float radians);
/// @brief Short-hand Rad alias for the Radians function
constexpr static auto Rad = Radians;
/// @brief Convert a vector from 2D carthesian coordinates to polar
/// coordinates
/// @param v The vector to convert
static PolarOf<T> FromVector2(Vector2 v);
/// @brief Convert a vector from spherical coordinates to polar coordinates
/// @param s The vector to convert
/// @note The resulting vector will be projected on the horizontal plane
static PolarOf<T> FromSpherical(SphericalOf<T> v);
/// @brief A polar vector with zero degrees and distance
const static PolarOf zero;
/// @brief A normalized forward-oriented vector
const static PolarOf forward;
/// @brief A normalized back-oriented vector
const static PolarOf back;
/// @brief A normalized right-oriented vector
const static PolarOf right;
/// @brief A normalized left-oriented vector
const static PolarOf left;
/// @brief Equality test to another vector
/// @param v The vector to check against
/// @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;
/// @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; }
/// @brief The vector length
/// @return The vector length
inline float magnitude() const { return this->distance; }
/// @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);
/// @brief Convert the vector to a length of a
/// @return The vector normalized to a length of 1
PolarOf normalized() const;
/// @brief Negate the vector
/// @return The negated vector
/// This will rotate the vector by 180 degrees. Distance will stay the same.
PolarOf operator-() const;
/// @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);
/// @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);
/// @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) {
return PolarOf(v.distance * f, v.angle);
}
friend PolarOf operator*(float f, const PolarOf &v) {
return PolarOf(f * v.distance, v.angle);
}
PolarOf operator*=(float f);
/// @brief Scale the vector uniformly down
/// @param f The scaling factor
/// @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) {
return PolarOf(v.distance / f, v.angle);
}
friend PolarOf operator/(float f, const PolarOf &v) {
return PolarOf(f / v.distance, v.angle);
}
PolarOf operator/=(float f);
/// @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 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);
};
using PolarSingle = PolarOf<float>;
using Polar16 = PolarOf<signed short>;
// using Polar = PolarSingle;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#include "Spherical.h"
#include "Vector2.h"
#endif

418
ControlCore/LinearAlgebra/Quaternion.cpp Normal file
View File

@ -0,0 +1,418 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Quaternion.h"
#include <float.h>
#include <math.h>
#include "Angle.h"
#include "Vector3.h"
void CopyQuat(const Quat& q1, Quat& q2) {
q2.x = q1.x;
q2.y = q1.y;
q2.z = q1.z;
q2.w = q1.w;
}
const float Deg2Rad = 0.0174532924F;
const float Rad2Deg = 57.29578F;
Quaternion::Quaternion() {
x = 0;
y = 0;
z = 0;
w = 1;
}
Quaternion::Quaternion(float _x, float _y, float _z, float _w) {
x = _x;
y = _y;
z = _z;
w = _w;
}
Quaternion::Quaternion(Quat q) {
x = q.x;
y = q.y;
z = q.z;
w = q.w;
}
Quaternion::~Quaternion() {}
const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1);
Vector3 Quaternion::xyz() const {
return Vector3(x, y, z);
}
float Quaternion::GetLength() const {
return sqrtf(x * x + y * y + z * z + w * w);
}
float Quaternion::GetLengthSquared() const {
return x * x + y * y + z * z + w * w;
}
float Quaternion::GetLengthSquared(const Quaternion& q) {
return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
}
void Quaternion::Normalize() {
float length = GetLength();
x /= length;
y /= length;
z /= length;
w /= length;
}
Quaternion Quaternion::Normalize(const Quaternion& q) {
Quaternion result;
float length = q.GetLength();
result = Quaternion(q.x / length, q.y / length, q.z / length, q.w / length);
return result;
};
float Quaternion::Dot(Quaternion a, Quaternion b) {
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}
Vector3 Quaternion::ToAngles(const Quaternion& q1) {
float test = q1.x * q1.y + q1.z * q1.w;
if (test > 0.499f) { // singularity at north pole
return Vector3(0, 2 * (float)atan2(q1.x, q1.w) * Rad2Deg, 90);
} else if (test < -0.499f) { // singularity at south pole
return Vector3(0, -2 * (float)atan2(q1.x, q1.w) * Rad2Deg, -90);
} else {
float sqx = q1.x * q1.x;
float sqy = q1.y * q1.y;
float sqz = q1.z * q1.z;
return Vector3(
atan2f(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz) *
Rad2Deg,
atan2f(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz) *
Rad2Deg,
asinf(2 * test) * Rad2Deg);
}
}
Quaternion Quaternion::operator*(const Quaternion& r2) const {
return Quaternion(
this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,
-this->x * r2.z + this->y * r2.w + this->z * r2.x + this->w * r2.y,
this->x * r2.y - this->y * r2.x + this->z * r2.w + this->w * r2.z,
-this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w);
};
Vector3 Quaternion::operator*(const Vector3& p) const {
float num = this->x * 2;
float num2 = this->y * 2;
float num3 = this->z * 2;
float num4 = this->x * num;
float num5 = this->y * num2;
float num6 = this->z * num3;
float num7 = this->x * num2;
float num8 = this->x * num3;
float num9 = this->y * num3;
float num10 = this->w * num;
float num11 = this->w * num2;
float num12 = this->w * num3;
float px = p.Right();
float py = p.Up();
float pz = p.Forward();
// Vector3 result = Vector3::zero;
// result.x =
float rx =
(1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz;
// result.y =
float ry =
(num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz;
// result.z =
float rz =
(num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz;
Vector3 result = Vector3(rx, ry, rz);
return result;
}
bool Quaternion::operator==(const Quaternion& q) const {
return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w);
}
Quaternion Quaternion::Inverse(Quaternion r) {
float n = sqrtf(r.x * r.x + r.y * r.y + r.z * r.z + r.w * r.w);
return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
}
Quaternion Quaternion::LookRotation(const Vector3& forward) {
Vector3 up = Vector3(0, 1, 0);
return LookRotation(forward, up);
}
Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
Vector3 nForward = Vector3::Normalize(forward);
Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
Vector3 nUp = Vector3::Cross(nForward, nRight);
float m00 = nRight.Right(); // x;
float m01 = nRight.Up(); // y;
float m02 = nRight.Forward(); // z;
float m10 = nUp.Right(); // x;
float m11 = nUp.Up(); // y;
float m12 = nUp.Forward(); // z;
float m20 = nForward.Right(); // x;
float m21 = nForward.Up(); // y;
float m22 = nForward.Forward(); // z;
float num8 = (m00 + m11) + m22;
Quaternion quaternion = Quaternion();
if (num8 > 0) {
float num = sqrtf(num8 + 1);
quaternion.w = num * 0.5f;
num = 0.5f / num;
quaternion.x = (m12 - m21) * num;
quaternion.y = (m20 - m02) * num;
quaternion.z = (m01 - m10) * num;
return quaternion;
}
if ((m00 >= m11) && (m00 >= m22)) {
float num7 = sqrtf(((1 + m00) - m11) - m22);
float num4 = 0.5F / num7;
quaternion.x = 0.5f * num7;
quaternion.y = (m01 + m10) * num4;
quaternion.z = (m02 + m20) * num4;
quaternion.w = (m12 - m21) * num4;
return quaternion;
}
if (m11 > m22) {
float num6 = sqrtf(((1 + m11) - m00) - m22);
float num3 = 0.5F / num6;
quaternion.x = (m10 + m01) * num3;
quaternion.y = 0.5F * num6;
quaternion.z = (m21 + m12) * num3;
quaternion.w = (m20 - m02) * num3;
return quaternion;
}
float num5 = sqrtf(((1 + m22) - m00) - m11);
float num2 = 0.5F / num5;
quaternion.x = (m20 + m02) * num2;
quaternion.y = (m21 + m12) * num2;
quaternion.z = 0.5F * num5;
quaternion.w = (m01 - m10) * num2;
return quaternion;
}
Quaternion Quaternion::FromToRotation(Vector3 fromDirection,
Vector3 toDirection) {
Vector3 axis = Vector3::Cross(fromDirection, toDirection);
axis = Vector3::Normalize(axis);
AngleOf<float> angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
Quaternion rotation = Quaternion::AngleAxis(angle.InDegrees(), axis);
return rotation;
}
Quaternion Quaternion::RotateTowards(const Quaternion& from,
const Quaternion& to,
float maxDegreesDelta) {
float num = Quaternion::Angle(from, to);
if (num == 0) {
return to;
}
float t = (float)fmin(1, maxDegreesDelta / num);
return SlerpUnclamped(from, to, t);
}
Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) {
if (Vector3::SqrMagnitude(axis) == 0.0f)
return Quaternion();
Quaternion result = Quaternion();
float radians = angle * Deg2Rad;
radians *= 0.5f;
Vector3 axis2 = axis * (float)sin(radians);
result.x = axis2.Right(); // x;
result.y = axis2.Up(); // y;
result.z = axis2.Forward(); // z;
result.w = (float)cos(radians);
return Quaternion::Normalize(result);
}
float Quaternion::Angle(Quaternion a, Quaternion b) {
float f = Quaternion::Dot(a, b);
return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
}
void Quaternion::ToAngleAxis(float* angle, Vector3* axis) {
Quaternion::ToAxisAngleRad(*this, axis, angle);
*angle *= Rad2Deg;
}
void Quaternion::ToAxisAngleRad(const Quaternion& q,
Vector3* const axis,
float* angle) {
Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q;
*angle = 2.0f * acosf(q1.w); // angle
float den = sqrtf(1.0F - q1.w * q1.w);
if (den > 0.0001f) {
*axis = Vector3::Normalize(q1.xyz() / den);
} else {
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
*axis = Vector3(1, 0, 0);
}
}
Quaternion Quaternion::SlerpUnclamped(const Quaternion& a,
const Quaternion& b,
float t) {
// if either input is zero, return the other.
if (Quaternion::GetLengthSquared(a) == 0.0f) {
if (Quaternion::GetLengthSquared(b) == 0.0f) {
return Quaternion();
}
return b;
} else if (Quaternion::GetLengthSquared(b) == 0.0f) {
return a;
}
const Vector3 axyz = a.xyz();
const Vector3 bxyz = b.xyz();
float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz);
Quaternion b2 = b;
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
// angle = 0.0f, so just return one input.
return a;
} else if (cosHalfAngle < 0.0f) {
b2.x = -b.x;
b2.y = -b.y;
b2.z = -b.z;
b2.w = -b.w;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f) {
// do proper slerp for big angles
float halfAngle = acosf(cosHalfAngle);
float sinHalfAngle = sinf(halfAngle);
float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
blendB = sinf(halfAngle * t) * oneOverSinHalfAngle;
} else {
// do lerp if angle is really small.
blendA = 1.0f - t;
blendB = t;
}
Vector3 v = axyz * blendA + b2.xyz() * blendB;
Quaternion result =
Quaternion(v.Right(), v.Up(), v.Forward(), blendA * a.w + blendB * b2.w);
if (result.GetLengthSquared() > 0.0f)
return Quaternion::Normalize(result);
else
return Quaternion();
}
Quaternion Quaternion::Slerp(const Quaternion& a,
const Quaternion& b,
float t) {
if (t > 1)
t = 1;
if (t < 0)
t = 0;
return Quaternion::SlerpUnclamped(a, b, t);
}
Quaternion Quaternion::Euler(float x, float y, float z) {
return Quaternion::Euler(Vector3(x, y, z));
}
Quaternion Quaternion::Euler(Vector3 euler) {
return Quaternion::FromEulerRad(euler * Deg2Rad);
}
Quaternion Quaternion::FromEulerRad(Vector3 euler) {
float yaw = euler.Right();
float pitch = euler.Up();
float roll = euler.Forward();
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)sin((float)rollOver2);
float cosRollOver2 = (float)cos((float)rollOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)sin((float)pitchOver2);
float cosPitchOver2 = (float)cos((float)pitchOver2);
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)sin((float)yawOver2);
float cosYawOver2 = (float)cos((float)yawOver2);
Quaternion result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
Quaternion Quaternion::EulerXYZ(float x, float y, float z) {
return Quaternion::EulerXYZ(Vector3(x, y, z));
}
Quaternion Quaternion::EulerXYZ(Vector3 euler) {
return Quaternion::FromEulerRadXYZ(euler * Deg2Rad);
}
Quaternion Quaternion::FromEulerRadXYZ(Vector3 euler) {
float yaw = euler.Right(); // x;
float pitch = euler.Up(); // y;
float roll = euler.Forward(); // z;
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)sin((float)rollOver2);
float cosRollOver2 = (float)cos((float)rollOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)sin((float)pitchOver2);
float cosPitchOver2 = (float)cos((float)pitchOver2);
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)sin((float)yawOver2);
float cosYawOver2 = (float)cos((float)yawOver2);
Quaternion result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 -
cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 +
sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) {
Quaternion secondaryRotation = GetRotationAround(axis, rotation);
float rotationAngle;
Vector3 rotationAxis;
secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis);
// Do the axis point in opposite directions?
if (Vector3::Dot(axis, rotationAxis) < 0)
rotationAngle = -rotationAngle;
return rotationAngle;
}
Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) {
Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis
Vector3 p = Vector3::Project(
ra, axis); // return projection ra on to axis (parallel component)
Quaternion twist = Quaternion(p.Right(), p.Up(), p.Forward(), rotation.w);
twist = Quaternion::Normalize(twist);
return twist;
}
void Quaternion::GetSwingTwist(Vector3 axis,
Quaternion rotation,
Quaternion* swing,
Quaternion* twist) {
*twist = GetRotationAround(axis, rotation);
*swing = rotation * Quaternion::Inverse(*twist);
}

295
ControlCore/LinearAlgebra/Quaternion.h Normal file
View File

@ -0,0 +1,295 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef QUATERNION_H
#define QUATERNION_H
#include "Vector3.h"
namespace Passer {
namespace LinearAlgebra {
extern "C" {
/// <summary>
/// A quaternion
/// </summary>
/// This is a C-style implementation
typedef struct Quat {
/// <summary>
/// The x component
/// </summary>
float x;
/// <summary>
/// The y component
/// </summary>
float y;
/// <summary>
/// The z component
/// </summary>
float z;
/// <summary>
/// The w component
/// </summary>
float w;
} Quat;
}
/// <summary>
/// A quaternion
/// </summary>
struct Quaternion : Quat {
public:
/// <summary>
/// Create a new identity quaternion
/// </summary>
Quaternion();
/// <summary>
/// create a new quaternion with the given values
/// </summary>
/// <param name="_x">x component</param>
/// <param name="_y">y component</param>
/// <param name="_z">z component</param>
/// <param name="_w">w component</param>
Quaternion(float _x, float _y, float _z, float _w);
/// <summary>
/// Create a quaternion from C-style Quat
/// </summary>
/// <param name="q"></param>
Quaternion(Quat q);
/// <summary>
/// Quaternion destructor
/// </summary>
~Quaternion();
/// <summary>
/// An identity quaternion
/// </summary>
const static Quaternion identity;
/// <summary>
/// Convert to unit quaternion
/// </summary>
/// This will preserve the orientation,
/// but ensures that it is a unit quaternion.
void Normalize();
/// <summary>
/// Convert to unity quaternion
/// </summary>
/// <param name="q">The quaternion to convert</param>
/// <returns>A unit quaternion</returns>
/// This will preserve the orientation,
/// but ensures that it is a unit quaternion.
static Quaternion Normalize(const Quaternion& q);
/// <summary>
/// Convert to euler angles
/// </summary>
/// <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);
/// <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;
/// <summary>
/// Multiply this quaternion with another quaternion
/// </summary>
/// <param name="rotation">The quaternion to multiply with</param>
/// <returns>The resulting rotation</returns>
/// The result will be this quaternion rotated according to
/// the give rotation.
Quaternion operator*(const Quaternion& rotation) const;
/// <summary>
/// Check the equality of two quaternions
/// </summary>
/// <param name="quaternion">The quaternion to compare to</param>
/// <returns>True when the components of the quaternions are
/// identical</returns> Note that this does not compare the rotations
/// 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;
/// <summary>
/// The inverse of quaterion
/// </summary>
/// <param name="quaternion">The quaternion for which the inverse is
/// needed</param> <returns>The inverted quaternion</returns>
static Quaternion Inverse(Quaternion quaternion);
/// <summary>
/// A rotation which looks in the given direction
/// </summary>
/// <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);
/// <summary>
/// Creates a quaternion with the given forward direction with up =
/// Vector3::up
/// </summary>
/// <param name="forward">The look direction</param>
/// <returns>The rotation for this direction</returns>
/// 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);
/// <summary>
/// Calculat the rotation from on vector to another
/// </summary>
/// <param name="fromDirection">The from direction</param>
/// <param name="toDirection">The to direction</param>
/// <returns>The rotation from the first to the second vector</returns>
static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);
/// <summary>
/// Rotate form one orientation to anther with a maximum amount of degrees
/// </summary>
/// <param name="from">The from rotation</param>
/// <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,
float maxDegreesDelta);
/// <summary>
/// Convert an angle/axis representation to a quaternion
/// </summary>
/// <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);
/// <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);
/// <summary>
/// Get the angle between two orientations
/// </summary>
/// <param name="orientation1">The first orientation</param>
/// <param name="orientation2">The second orientation</param>
/// <returns>The smallest angle in degrees between the two
/// orientations</returns>
static float Angle(Quaternion orientation1, Quaternion orientation2);
/// <summary>
/// Sherical lerp between two rotations
/// </summary>
/// <param name="rotation1">The first rotation</param>
/// <param name="rotation2">The second rotation</param>
/// <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);
/// <summary>
/// Unclamped sherical lerp between two rotations
/// </summary>
/// <param name="rotation1">The first rotation</param>
/// <param name="rotation2">The second rotation</param>
/// <param name="factor">The factor</param>
/// <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);
/// <summary>
/// Create a rotation from euler angles
/// </summary>
/// <param name="x">The angle around the right axis</param>
/// <param name="y">The angle around the upward axis</param>
/// <param name="z">The angle around the forward axis</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order Z, X, Y.
static Quaternion Euler(float x, float y, float z);
/// <summary>
/// Create a rotation from a vector containing euler angles
/// </summary>
/// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order Z, X, Y.
static Quaternion Euler(Vector3 eulerAngles);
/// <summary>
/// Create a rotation from euler angles
/// </summary>
/// <param name="x">The angle around the right axis</param>
/// <param name="y">The angle around the upward axis</param>
/// <param name="z">The angle around the forward axis</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order X, Y, Z.
static Quaternion EulerXYZ(float x, float y, float z);
/// <summary>
/// Create a rotation from a vector containing euler angles
/// </summary>
/// <param name="eulerAngles">Vector with the euler angles</param>
/// <returns>The resulting quaternion</returns>
/// Rotation are appied in the order X, Y, Z.
static Quaternion EulerXYZ(Vector3 eulerAngles);
/// <summary>
/// Returns the angle of around the give axis for a rotation
/// </summary>
/// <param name="axis">The axis around which the angle should be
/// computed</param> <param name="rotation">The source rotation</param>
/// <returns>The signed angle around the axis</returns>
static float GetAngleAround(Vector3 axis, Quaternion rotation);
/// <summary>
/// Returns the rotation limited around the given axis
/// </summary>
/// <param name="axis">The axis which which the rotation should be
/// limited</param> <param name="rotation">The source rotation</param>
/// <returns>The rotation around the given axis</returns>
static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
/// <summary>
/// Swing-twist decomposition of a rotation
/// </summary>
/// <param name="axis">The base direction for the decomposition</param>
/// <param name="rotation">The source rotation</param>
/// <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);
/// <summary>
/// Calculate the dot product of two quaternions
/// </summary>
/// <param name="rotation1">The first rotation</param>
/// <param name="rotation2">The second rotation</param>
/// <returns></returns>
static float Dot(Quaternion rotation1, Quaternion rotation2);
private:
float GetLength() const;
float GetLengthSquared() const;
static float GetLengthSquared(const Quaternion& q);
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
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

12
ControlCore/LinearAlgebra/README.md Normal file
View File

@ -0,0 +1,12 @@
\mainpage Vector Algebra
Vector algebra library
Main components
---------------
* [Vector3](struct_passer_1_1_linear_algebra_1_1_vector3.html)
* [Quaternion](struct_passer_1_1_linear_algebra_1_1_quaternion.html)
* [Vector2](https://serrarens.nl/apis/LinearAlgebra/struct_vector2.html)
* [Polar](https://passervr.com/apis/LinearAlgebra/struct_polar.html)

294
ControlCore/LinearAlgebra/Spherical.cpp Normal file
View File

@ -0,0 +1,294 @@
#include "Spherical.h"
#include "Angle.h"
#include "Quaternion.h"
#include <math.h>
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,
AngleOf<T> vertical) {
if (distance < 0) {
this->distance = -distance;
this->direction = -DirectionOf<T>(horizontal, vertical);
} else {
this->distance = distance;
this->direction = DirectionOf<T>(horizontal, vertical);
}
}
template <typename T>
SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
if (distance < 0) {
this->distance = -distance;
this->direction = -direction;
} else {
this->distance = distance;
this->direction = direction;
}
}
template <typename T>
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);
SphericalOf<T> r = SphericalOf<T>(distance, horizontalAngle, verticalAngle);
return r;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::Radians(float distance, float horizontal,
float vertical) {
return SphericalOf<T>(distance, AngleOf<T>::Radians(horizontal),
AngleOf<T>::Radians(vertical));
}
template <typename T>
SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
AngleOf<T> horizontal = polar.angle;
AngleOf<T> vertical = AngleOf<T>();
SphericalOf<T> r = SphericalOf(polar.distance, horizontal, vertical);
return r;
}
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>());
} else {
AngleOf<T> verticalAngle =
AngleOf<T>::Radians((pi / 2 - acosf(v.Up() / distance)));
AngleOf<T> horizontalAngle =
AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
return SphericalOf(distance, horizontalAngle, verticalAngle);
}
}
/**
* @brief Converts spherical coordinates to a 3D vector.
*
* This function converts the spherical coordinates represented by the
* SphericalOf object to a 3D vector (Vector3). The conversion is based
* on the distance and direction (vertical and horizontal angles) of the
* spherical coordinates.
*
* @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 {
float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
float horizontalRad = this->direction.horizontal.InRadians();
float cosVertical = cosf(verticalRad);
float sinVertical = sinf(verticalRad);
float cosHorizontal = cosf(horizontalRad);
float sinHorizontal = sinf(horizontalRad);
float x = this->distance * sinVertical * sinHorizontal;
float y = this->distance * cosVertical;
float z = this->distance * sinVertical * cosHorizontal;
Vector3 v = Vector3(x, y, z);
return v;
}
template <typename T>
const SphericalOf<T> SphericalOf<T>::zero =
SphericalOf<T>(0.0f, AngleOf<T>(), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::forward =
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::back =
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(180), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::right =
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(90), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::left =
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(-90), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::up =
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(90));
template <typename T>
const SphericalOf<T> SphericalOf<T>::down =
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(-90));
template <typename T>
SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
SphericalOf<T> v = SphericalOf<T>(distance, this->direction);
return SphericalOf<T>();
}
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));
return v;
}
template <typename T>
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();
Vector3 v = v1 - v2;
SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
return r;
}
template <typename T>
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 {
// let's do it the easy way...
Vector3 v1 = this->ToVector3();
Vector3 v2 = s2.ToVector3();
Vector3 v = v1 + v2;
SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
return r;
/*
// This is the hard way...
if (v2.distance <= 0)
return Spherical(this->distance, this->horizontalAngle,
this->verticalAngle);
if (this->distance <= 0)
return v2;
float deltaHorizontalAngle =
(float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle);
float horizontalRotation = deltaHorizontalAngle < 0
? 180 + deltaHorizontalAngle
: 180 - deltaHorizontalAngle;
float deltaVerticalAngle =
Angle::Normalize(v2.verticalAngle - this->verticalAngle);
float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle
: 180 - deltaVerticalAngle;
if (horizontalRotation == 180 && verticalRotation == 180)
// angle is too small, take this angle and add the distances
return Spherical(this->distance + v2.distance, this->horizontalAngle,
this->verticalAngle);
Angle rotation = AngleBetween(*this, v2);
float newDistance =
Angle::CosineRuleSide(v2.distance, this->distance, rotation);
float angle =
Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
// Now we have to project the angle to the horizontal and vertical planes...
// The axis for the angle is the cross product of the two spherical vectors
// (which function we do not have either...)
float horizontalAngle = 0;
float verticalAngle = 0;
float newHorizontalAngle =
deltaHorizontalAngle < 0
? Angle::Normalize(this->horizontalAngle - horizontalAngle)
: Angle::Normalize(this->horizontalAngle + horizontalAngle);
float newVerticalAngle =
deltaVerticalAngle < 0
? Angle::Normalize(this->verticalAngle - verticalAngle)
: Angle::Normalize(this->verticalAngle + verticalAngle);
Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
*/
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T> &v) {
*this = *this + v;
return *this;
}
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) {
this->distance /= f;
return *this;
}
#include "FloatSingle.h"
#include "Vector3.h"
const float epsilon = 1E-05f;
template <typename T>
float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
const SphericalOf<T> &v2) {
// SphericalOf<T> difference = v1 - v2;
// return difference.distance;
Vector3 vec1 = v1.ToVector3();
Vector3 vec2 = v2.ToVector3();
float distance = Vector3::Distance(vec1, vec2);
return distance;
}
template <typename T>
AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
const SphericalOf &v2) {
// float denominator = v1.distance * v2.distance;
// if (denominator < epsilon)
// return 0.0f;
Vector3 v1_3 = v1.ToVector3();
Vector3 v2_3 = v2.ToVector3();
// float dot = Vector3::Dot(v1_3, v2_3);
// float fraction = dot / denominator;
// if (isnan(fraction))
// return fraction; // short cut to returning NaN universally
// float cdot = Float::Clamp(fraction, -1.0, 1.0);
// float r = ((float)acos(cdot)) * Rad2Deg;
AngleSingle r = Vector3::Angle(v1_3, v2_3);
return AngleOf<T>::Degrees(r.InDegrees());
}
template <typename T>
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();
AngleSingle r = Vector3::SignedAngle(v1_vector, v2_vector, axis_vector);
return AngleOf<T>::Degrees(r.InDegrees());
}
template <typename T>
SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T> &v,
AngleOf<T> horizontalAngle,
AngleOf<T> verticalAngle) {
SphericalOf<T> r =
SphericalOf(v.distance, v.direction.horizontal + horizontalAngle,
v.direction.vertical + verticalAngle);
return r;
}
template <typename T>
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,
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>;

188
ControlCore/LinearAlgebra/Spherical.h Normal file
View File

@ -0,0 +1,188 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef SPHERICAL_H
#define SPHERICAL_H
#include "Direction.h"
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
template <typename T> class PolarOf;
template <typename T> class SphericalOf {
public:
/// @brief The distance in meters
/// @remark The distance should never be negative
float distance;
/// @brief The angle in the horizontal plane in degrees, clockwise rotation
/// @details The angle is automatically normalized to -180 .. 180
// AngleOf<T> horizontal;
/// @brief The angle in the vertical plane in degrees. Positive is upward.
/// @details The angle is automatically normalized to -180 .. 180
// AngleOf<T> vertical;
DirectionOf<T> direction;
SphericalOf<T>();
SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
SphericalOf<T>(float distance, DirectionOf<T> direction);
/// @brief Create spherical vector without using AngleOf type. All given
/// angles are in degrees
/// @param distance The distance in meters
/// @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,
float vertical);
/// @brief Short-hand Deg alias for the Degrees function
constexpr static auto Deg = Degrees;
/// @brief Create sperical vector without using the AngleOf type. All given
/// angles are in radians
/// @param distance The distance in meters
/// @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,
float vertical);
// Short-hand Rad alias for the Radians function
constexpr static auto Rad = Radians;
/// @brief Create a Spherical coordinate from a Polar coordinate
/// @param v The polar coordinate
/// @return The spherical coordinate with the vertical angle set to zero.
static SphericalOf<T> FromPolar(PolarOf<T> v);
/// @brief Create a Spherical coordinate from a Vector3 coordinate
/// @param v The vector coordinate
/// @return The spherical coordinate
static SphericalOf<T> FromVector3(Vector3 v);
/// @brief Convert the spherical coordinate to a Vector3 coordinate
/// @return The vector coordinate
Vector3 ToVector3() const;
/// @brief A spherical vector with zero degree angles and distance
const static SphericalOf<T> zero;
/// @brief A normalized forward-oriented vector
const static SphericalOf<T> forward;
/// @brief A normalized back-oriented vector
const static SphericalOf<T> back;
/// @brief A normalized right-oriented vector
const static SphericalOf<T> right;
/// @brief A normalized left-oriented vector
const static SphericalOf<T> left;
/// @brief A normalized up-oriented vector
const static SphericalOf<T> up;
/// @brief A normalized down-oriented vector
const static SphericalOf<T> down;
/// @brief Update the distance component of the spherical coordinate
/// @param distance The new distance
/// @return The updated coordinate
SphericalOf<T> WithDistance(float distance);
/// @brief Negate the vector
/// @return The negated vector
/// This will rotate the vector by 180 degrees horizontally and
/// vertically. Distance will stay the same.
SphericalOf<T> operator-() const;
/// @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);
/// @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);
/// @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) {
return SphericalOf<T>(v.distance * f, v.direction);
}
friend SphericalOf<T> operator*(float f, const SphericalOf<T> &v) {
return SphericalOf<T>(f * v.distance, v.direction);
}
SphericalOf<T> operator*=(float f);
/// @brief Scale the vector uniformly down
/// @param f The scaling factor
/// @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) {
return SphericalOf<T>(v.distance / f, v.direction);
}
friend SphericalOf<T> operator/(float f, const SphericalOf<T> &v) {
return SphericalOf<T>(f / v.distance, v.direction);
}
SphericalOf<T> operator/=(float f);
/// @brief Calculate the distance between two spherical coordinates
/// @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);
/// @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);
/// @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);
/// @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,
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,
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,
AngleOf<T> angle);
};
/// @brief Shorthand notation for a spherical vector using single precision
/// floats for the angles This is the fastest implementation on devices with
/// floating point harware
using SphericalSingle = SphericalOf<float>;
/// @brief Shorthand notation for a spherical vector using signed 16-bit words
/// for the angles
/// @note This is the fastest implementation on devices without floating point
/// hardware
using Spherical16 = SphericalOf<signed short>;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#include "Polar.h"
#include "Vector3.h"
#endif

168
ControlCore/LinearAlgebra/SwingTwist.cpp Normal file
View File

@ -0,0 +1,168 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "SwingTwist.h"
template <typename T>
SwingTwistOf<T>::SwingTwistOf() {
this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
this->twist = AngleOf<T>();
}
template <typename T>
SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist) {
// Normalize angles
AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
if (swing.vertical > deg90 || swing.vertical < -deg90) {
swing.horizontal += deg180;
swing.vertical = deg180 - swing.vertical;
twist += deg180;
}
this->swing = swing;
this->twist = twist;
}
template <typename T>
SwingTwistOf<T>::SwingTwistOf(AngleOf<T> horizontal,
AngleOf<T> vertical,
AngleOf<T> twist) {
// Normalize angles
AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
if (vertical > deg90 || vertical < -deg90) {
horizontal += deg180;
vertical = deg180 - vertical;
twist += deg180;
}
this->swing = DirectionOf<T>(horizontal, vertical);
this->twist = twist;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::Degrees(float horizontal,
float vertical,
float twist) {
SwingTwistOf<T> orientation = SwingTwistOf<T>(AngleOf<T>::Degrees(horizontal),
-AngleOf<T>::Degrees(vertical),
AngleOf<T>::Degrees(twist));
// DirectionOf<T> swing = DirectionOf<T>::Degrees(horizontal, vertical);
// AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
// SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
return orientation;
}
template <typename T>
Quaternion SwingTwistOf<T>::ToQuaternion() const {
Quaternion q = Quaternion::Euler(-this->swing.vertical.InDegrees(),
this->swing.horizontal.InDegrees(),
this->twist.InDegrees());
return q;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::FromQuaternion(Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q);
SwingTwistOf<T> r =
SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward());
r.Normalize();
return r;
}
template <typename T>
SphericalOf<T> SwingTwistOf<T>::ToAngleAxis() const {
Quaternion q = this->ToQuaternion();
float angle;
Vector3 axis;
q.ToAngleAxis(&angle, &axis);
DirectionOf<T> direction = DirectionOf<T>::FromVector3(axis);
SphericalOf<T> aa = SphericalOf<T>(angle, direction);
return aa;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::FromAngleAxis(SphericalOf<T> aa) {
Vector3 vectorAxis = aa.direction.ToVector3();
Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis);
return SwingTwistOf<T>();
}
template <typename T>
bool SwingTwistOf<T>::operator==(const SwingTwistOf<T> s) const {
return (this->swing == s.swing) && (this->twist == s.twist);
}
template <typename T>
const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();
template <typename T>
SphericalOf<T> SwingTwistOf<T>::operator*(const SphericalOf<T>& vector) const {
SphericalOf<T> v = SphericalOf<T>(
vector.distance, vector.direction.horizontal + this->swing.horizontal,
vector.direction.vertical + this->swing.vertical);
return v;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*(
const SwingTwistOf<T>& rotation) const {
SwingTwistOf<T> r =
SwingTwistOf(this->swing.horizontal + rotation.swing.horizontal,
this->swing.vertical + rotation.swing.vertical,
this->twist + rotation.twist);
return r;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*=(const SwingTwistOf<T>& rotation) {
this->swing.horizontal += rotation.swing.horizontal;
this->swing.vertical += rotation.swing.vertical;
this->twist += rotation.twist;
return *this;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::Inverse(SwingTwistOf<T> rotation) {
SwingTwistOf<T> r = SwingTwistOf<T>(
-rotation.swing.horizontal, -rotation.swing.vertical, -rotation.twist);
return r;
}
template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::AngleAxis(float angle,
const DirectionOf<T>& axis) {
Vector3 axis_vector = axis.ToVector3();
Quaternion q = Quaternion::AngleAxis(angle, axis_vector);
SwingTwistOf<T> r = SwingTwistOf<T>::FromQuaternion(q);
return r;
}
template <typename T>
AngleOf<T> SwingTwistOf<T>::Angle(const SwingTwistOf<T>& r1,
const SwingTwistOf<T>& r2) {
Quaternion q1 = r1.ToQuaternion();
Quaternion q2 = r2.ToQuaternion();
float angle = Quaternion::Angle(q1, q2);
return AngleOf<T>::Degrees(angle);
}
template <typename T>
void SwingTwistOf<T>::Normalize() {
AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
if (this->swing.vertical > deg90 || this->swing.vertical < -deg90) {
this->swing.horizontal += deg180;
this->swing.vertical = deg180 - this->swing.vertical;
this->twist += deg180;
}
}
template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>;

78
ControlCore/LinearAlgebra/SwingTwist.h Normal file
View File

@ -0,0 +1,78 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef SWINGTWIST_H
#define SWINGTWIST_H
#include "Angle.h"
#include "Direction.h"
#include "Quaternion.h"
#include "Spherical.h"
namespace Passer {
namespace LinearAlgebra {
template <typename T>
class SwingTwistOf {
public:
DirectionOf<T> swing;
AngleOf<T> twist;
SwingTwistOf<T>();
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,
float twist = 0);
Quaternion ToQuaternion() const;
static SwingTwistOf<T> FromQuaternion(Quaternion q);
SphericalOf<T> ToAngleAxis() const;
static SwingTwistOf<T> FromAngleAxis(SphericalOf<T> aa);
const static SwingTwistOf<T> identity;
bool operator==(const SwingTwistOf<T> d) const;
/// <summary>
/// Rotate a vector using this rotation
/// </summary>
/// <param name="vector">The vector to rotate</param>
/// <returns>The rotated vector</returns>
SphericalOf<T> operator*(const SphericalOf<T>& vector) const;
/// <summary>
/// Multiply this rotation with another rotation
/// </summary>
/// <param name="rotation">The swing/twist rotation to multiply with</param>
/// <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);
static SwingTwistOf<T> Inverse(SwingTwistOf<T> rotation);
/// <summary>
/// Convert an angle/axis representation to a swingt
/// </summary>
/// <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 AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);
void Normalize();
};
using SwingTwistSingle = SwingTwistOf<float>;
using SwingTwist16 = SwingTwistOf<signed short>;
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif

175
ControlCore/LinearAlgebra/Vector2.cpp Normal file
View File

@ -0,0 +1,175 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Vector2.h"
#include "Angle.h"
#include "FloatSingle.h"
#include "Vector3.h"
// #if defined(AVR)
// #include <Arduino.h>
// #else
#include <math.h>
// #endif
Vector2::Vector2() {
x = 0;
y = 0;
}
Vector2::Vector2(float _x, float _y) {
x = _x;
y = _y;
}
// Vector2::Vector2(Vec2 v) {
// x = v.x;
// y = v.y;
// }
Vector2::Vector2(Vector3 v) {
x = v.Right(); // x;
y = v.Forward(); // z;
}
Vector2::Vector2(PolarSingle p) {
float horizontalRad = p.angle.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
float cosHorizontal = cosf(horizontalRad);
float sinHorizontal = sinf(horizontalRad);
x = p.distance * sinHorizontal;
y = p.distance * cosHorizontal;
}
Vector2::~Vector2() {}
const Vector2 Vector2::zero = Vector2(0, 0);
const Vector2 Vector2::one = Vector2(1, 1);
const Vector2 Vector2::right = Vector2(1, 0);
const Vector2 Vector2::left = Vector2(-1, 0);
const Vector2 Vector2::up = Vector2(0, 1);
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) {
return (this->x == v.x && this->y == v.y);
}
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); }
Vector2 Vector2::Normalize(const Vector2 &v) {
float num = Vector2::Magnitude(v);
Vector2 result = Vector2::zero;
if (num > Float::epsilon) {
result = v / num;
}
return result;
}
Vector2 Vector2::normalized() const {
float num = this->magnitude();
Vector2 result = Vector2::zero;
if (num > Float::epsilon) {
result = ((Vector2) * this) / num;
}
return result;
}
Vector2 Vector2::operator-() { return Vector2(-this->x, -this->y); }
Vector2 Vector2::operator-(const Vector2 &v) const {
return Vector2(this->x - v.x, this->y - v.y);
}
Vector2 Vector2::operator-=(const Vector2 &v) {
this->x -= v.x;
this->y -= v.y;
return *this;
}
Vector2 Vector2::operator+(const Vector2 &v) const {
return Vector2(this->x + v.x, this->y + v.y);
}
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) {
return Vector2(v1.x * v2.x, v1.y * v2.y);
}
// Vector2 Passer::LinearAlgebra::operator*(const Vector2 &v, float f) {
// return Vector2(v.x * f, v.y * f);
// }
// Vector2 Passer::LinearAlgebra::operator*(float f, const Vector2 &v) {
// return Vector2(v.x * f, v.y * f);
// }
Vector2 Vector2::operator*=(float f) {
this->x *= f;
this->y *= f;
return *this;
}
// Vector2 Passer::LinearAlgebra::operator/(const Vector2 &v, float f) {
// return Vector2(v.x / f, v.y / f);
// }
// Vector2 Passer::LinearAlgebra::operator/(float f, const Vector2 &v) {
// return Vector2(v.x / f, v.y / f);
// }
Vector2 Vector2::operator/=(float f) {
this->x /= f;
this->y /= f;
return *this;
}
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) {
return Magnitude(v1 - 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 sqrMagFrom = v1.sqrMagnitude();
float sqrMagTo = v2.sqrMagnitude();
if (sqrMagFrom == 0 || sqrMagTo == 0)
return 0;
if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
#if defined(AVR)
return NAN;
#else
return nanf("");
#endif
float angleFrom = atan2f(v1.y, v1.x);
float angleTo = atan2f(v2.y, v2.x);
return -(angleTo - angleFrom) * Passer::LinearAlgebra::Rad2Deg;
}
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);
#else
float sinValue = (float)sinf(angleRad);
float cosValue = (float)cosf(angleRad);
#endif
float tx = v.x;
float ty = v.y;
Vector2 r = Vector2((cosValue * tx) - (sinValue * ty),
(sinValue * tx) + (cosValue * ty));
return r;
}
Vector2 Vector2::Lerp(const Vector2 &v1, const Vector2 &v2, float f) {
Vector2 v = v1 + (v2 - v1) * f;
return v;
}

211
ControlCore/LinearAlgebra/Vector2.h Normal file
View File

@ -0,0 +1,211 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef VECTOR2_H
#define VECTOR2_H
#include "Angle.h"
extern "C" {
/// <summary>
/// 2-dimensional Vector representation
/// </summary>
/// This is a C-style implementation
/// This uses the right-handed coordinate system.
typedef struct Vec2 {
/// <summary>
/// The right axis of the vector
/// </summary>
float x;
/// <summary>
/// The upward/forward axis of the vector
/// </summary>
float y;
} Vec2;
}
namespace Passer {
namespace LinearAlgebra {
struct Vector3;
template <typename T> class PolarOf;
// using Polar = PolarOf<float>
/// @brief A 2=dimensional vector
/// @remark This uses the right=handed carthesian coordinate system.
/// @note This implementation intentionally avoids the use of x and y
struct Vector2 : Vec2 {
friend struct Vec2;
public:
/// @brief A new 2-dimensional zero vector
Vector2();
/// @brief A new 2-dimensional vector
/// @param right The distance in the right direction in meters
/// @param forward The distance in the forward direction in meters
Vector2(float right, float forward);
/// @brief Convert a Vector3 to a Vector2
/// @param v The 3D vector
/// @note This will project the vector to the horizontal plane
Vector2(Vector3 v);
/// @brief Convert a Polar vector to a 2-dimensional vector
/// @param v The vector in polar coordinates
Vector2(PolarOf<float> v);
/// @brief Vector2 destructor
~Vector2();
/// @brief A vector with zero for all axis
const static Vector2 zero;
/// @brief A vector with one for all axis
const static Vector2 one;
/// @brief A normalized forward-oriented vector
const static Vector2 forward;
/// @brief A normalized back-oriented vector
const static Vector2 back;
/// @brief A normalized right-oriented vector
const static Vector2 right;
/// @brief A normalized left-oriented vector
const static Vector2 left;
/// @brief A normalized up-oriented vector
/// @note This is a convenience function which is equal to Vector2::forward
const static Vector2 up;
/// @brief A normalized down-oriented vector
/// @note This is a convenience function which is equal to Vector2::down
const static Vector2 down;
/// @brief Check if this vector to the given vector
/// @param v The vector to check against
/// @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);
/// @brief The vector length
/// @param v The vector for which you need the length
/// @return The vector length
static float Magnitude(const Vector2 &v);
/// @brief The vector length
/// @return The vector length
float magnitude() const;
/// @brief The squared vector length
/// @param v The vector for which you need the squared length
/// @return The squared vector length
/// @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);
/// @brief The squared vector length
/// @return The squared vector length
/// @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.
float sqrMagnitude() const;
/// @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);
/// @brief Convert the vector to a length 1
/// @return The vector normalized to a length of 1
Vector2 normalized() const;
/// @brief Negate the vector such that it points in the opposite direction
/// @return The negated vector
Vector2 operator-();
/// @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);
/// @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);
/// @brief Scale the vector using another vector
/// @param v1 The vector to scale
/// @param v2 A vector with the scaling factors
/// @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);
/// @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) {
return Vector2(v.x * f, v.y * f);
}
friend Vector2 operator*(float f, const Vector2 &v) {
return Vector2(v.x * f, v.y * f);
// return Vector2(f * v.x, f * v.y);
}
Vector2 operator*=(float f);
/// @brief Scale the vector uniformly down
/// @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) {
return Vector2(v.x / f, v.y / f);
}
friend Vector2 operator/(float f, const Vector2 &v) {
return Vector2(f / v.x, f / v.y);
}
Vector2 operator/=(float f);
/// @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 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);
/// @brief The angle between two vectors
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The angle between the two vectors
/// @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);
/// @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);
/// @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, Passer::LinearAlgebra::AngleSingle a);
/// @brief Lerp (linear interpolation) between two vectors
/// @param v1 The starting vector
/// @param v2 The end vector
/// @param f The interpolation distance
/// @return The lerped vector
/// @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);
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#include "Polar.h"
#endif

221
ControlCore/LinearAlgebra/Vector3.cpp Normal file
View File

@ -0,0 +1,221 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Vector3.h"
#include "Angle.h"
#include "Spherical.h"
#include <math.h>
const float Deg2Rad = 0.0174532924F;
const float Rad2Deg = 57.29578F;
const float epsilon = 1E-05f;
Vector3::Vector3() {
this->x = 0;
this->y = 0;
this->z = 0;
}
Vector3::Vector3(float right, float up, float forward) {
this->x = right;
this->y = up;
this->z = forward;
}
Vector3::Vector3(Vector2 v) {
this->x = v.x;
this->y = 0.0f;
this->z = v.y;
}
Vector3::Vector3(SphericalOf<float> s) {
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);
float sinHorizontal = sinf(horizontalRad);
x = s.distance * sinVertical * sinHorizontal;
y = s.distance * cosVertical;
z = s.distance * sinVertical * cosHorizontal;
// Vector3 v = Vector3(s.distance * sinVertical * sinHorizontal,
// s.distance * cosVertical,
// );
// return v;
}
Vector3::~Vector3() {}
const Vector3 Vector3::zero = Vector3(0, 0, 0);
const Vector3 Vector3::one = Vector3(1, 1, 1);
const Vector3 Vector3::right = Vector3(1, 0, 0);
const Vector3 Vector3::left = Vector3(-1, 0, 0);
const Vector3 Vector3::up = Vector3(0, 1, 0);
const Vector3 Vector3::down = Vector3(0, -1, 0);
const Vector3 Vector3::forward = Vector3(0, 0, 1);
const Vector3 Vector3::back = Vector3(0, 0, -1);
// inline float Vector3::Forward() { return z; }
// inline float Vector3::Up() { return y; }
// inline float Vector3::Right() { return x; }
// Vector3 Vector3::FromHorizontal(const Vector2 &v) {
// return Vector3(v.x, 0, v.y);
// }
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::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); }
Vector3 Vector3::Normalize(const Vector3 &v) {
float num = Vector3::Magnitude(v);
Vector3 result = Vector3::zero;
if (num > epsilon) {
result = v / num;
}
return result;
}
Vector3 Vector3::normalized() const {
float num = this->magnitude();
Vector3 result = Vector3::zero;
if (num > epsilon) {
result = ((Vector3) * this) / num;
}
return result;
}
Vector3 Vector3::operator-() const {
return Vector3(-this->x, -this->y, -this->z);
}
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) {
this->x -= v.x;
this->y -= v.y;
this->z -= v.z;
return *this;
}
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) {
this->x += v.x;
this->y += v.y;
this->z += v.z;
return *this;
}
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) {
// return Vector3(v.x * f, v.y * f, v.z * f);
// }
// Vector3 Passer::LinearAlgebra::operator*(float f, const Vector3 &v) {
// return Vector3(v.x * f, v.y * f, v.z * f);
// }
Vector3 Vector3::operator*=(float f) {
this->x *= f;
this->y *= f;
this->z *= f;
return *this;
}
// Vector3 Passer::LinearAlgebra::operator/(const Vector3 &v, float f) {
// return Vector3(v.x / f, v.y / f, v.z / f);
// }
// Vector3 Passer::LinearAlgebra::operator/(float f, const Vector3 &v) {
// return Vector3(v.x / f, v.y / f, v.z / f);
// }
Vector3 Vector3::operator/=(float f) {
this->x /= f;
this->y /= f;
this->z /= f;
return *this;
}
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 {
return (this->x == v.x && this->y == v.y && this->z == v.z);
}
float Vector3::Distance(const Vector3 &v1, const Vector3 &v2) {
return Magnitude(v1 - 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) {
float sqrMagnitude = Dot(n, n);
if (sqrMagnitude < epsilon)
return Vector3::zero;
else {
float dot = Dot(v, n);
Vector3 r = n * dot / sqrMagnitude;
return r;
}
}
Vector3 Vector3::ProjectOnPlane(const Vector3 &v, const Vector3 &n) {
Vector3 r = v - Project(v, n);
return r;
}
float clamp(float x, float lower, float upper) {
float lowerClamp = fmaxf(x, lower);
float upperClamp = fminf(upper, lowerClamp);
return upperClamp;
}
AngleOf<float> Vector3::Angle(const Vector3 &v1, const Vector3 &v2) {
float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude());
if (denominator < epsilon)
return AngleOf<float>();
float dot = Vector3::Dot(v1, v2);
float fraction = dot / denominator;
if (isnan(fraction))
return AngleOf<float>::Degrees(
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) {
// angle in [0,180]
AngleOf<float> angle = Vector3::Angle(v1, v2);
Vector3 cross = Vector3::Cross(v1, v2);
float b = Vector3::Dot(axis, cross);
float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F);
// angle in [-179,180]
AngleOf<float> signed_angle = angle * signd;
return AngleOf<float>(signed_angle);
}
Vector3 Vector3::Lerp(const Vector3 &v1, const Vector3 &v2, float f) {
Vector3 v = v1 + (v2 - v1) * f;
return v;
}

234
ControlCore/LinearAlgebra/Vector3.h Normal file
View File

@ -0,0 +1,234 @@
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#ifndef VECTOR3_H
#define VECTOR3_H
#include "Vector2.h"
namespace Passer {
namespace LinearAlgebra {
// struct Spherical;
template <typename T> class SphericalOf;
extern "C" {
/// <summary>
/// 3-dimensional Vector representation
/// </summary>
/// This is a C-style implementation
/// This uses the right-handed coordinate system.
typedef struct Vec3 {
protected:
/// <summary>
/// The right axis of the vector
/// </summary>
float x;
/// <summary>
/// The upward axis of the vector
/// </summary>
float y;
/// <summary>
/// The forward axis of the vector
/// </summary>
float z;
} Vec3;
}
/// @brief A 3-dimensional vector
/// @remark This uses a right-handed carthesian coordinate system.
/// @note This implementation intentionally avoids the use of x, y and z values.
struct Vector3 : Vec3 {
friend struct Vec3;
public:
/// @brief A new 3-dimensional zero vector
Vector3();
/// @brief A new 3-dimensional vector
/// @param right The distance in the right direction in meters
/// @param up The distance in the upward direction in meters
/// @param forward The distance in the forward direction in meters
Vector3(float right, float up, float forward);
/// @brief Convert a 2-dimenstional vector to a 3-dimensional vector
/// @param v The vector to convert
Vector3(Vector2 v);
/// @brief Convert vector in spherical coordinates to 3d carthesian
/// coordinates
/// @param v The vector to convert
Vector3(SphericalOf<float> v);
/// @brief Vector3 destructor
~Vector3();
/// @brief A vector with zero for all axis
const static Vector3 zero;
/// @brief A vector with one for all axis
const static Vector3 one;
/// @brief A normalized forward-oriented vector
const static Vector3 forward;
/// @brief A normalized back-oriented vector
const static Vector3 back;
/// @brief A normalized right-oriented vector
const static Vector3 right;
/// @brief A normalized left-oriented vector
const static Vector3 left;
/// @brief A normalized up-oriented vector
const static Vector3 up;
/// @brief A normalized down-oriented vector
const static Vector3 down;
// Access functions which are intended to replace the use of XYZ
inline float Forward() const { return z; };
inline float Up() const { return y; };
inline float Right() const { return x; };
/// @brief Check if this vector to the given vector
/// @param v The vector to check against
/// @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;
/// @brief The vector length
/// @param v The vector for which you need the length
/// @return The vector length
static float Magnitude(const Vector3 &v);
/// @brief The vector length
/// @return The vector length
float magnitude() const;
/// @brief The squared vector length
/// @param v The vector for which you need the length
/// @return The squared vector length
/// @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);
/// @brief The squared vector length
/// @return The squared vector length
/// @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.
float sqrMagnitude() const;
/// @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);
/// @brief Convert the vector to a length of 1
/// @return The vector normalized to a length of 1
Vector3 normalized() const;
/// @brief Negate te vector such that it points in the opposite direction
/// @return The negated vector
Vector3 operator-() const;
/// @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);
/// @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);
/// @brief Scale the vector using another vector
/// @param v1 The vector to scale
/// @param v2 A vector with the scaling factors
/// @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);
/// @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) {
return Vector3(v.x * f, v.y * f, v.z * f);
}
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);
}
Vector3 operator*=(float f);
/// @brief Scale the vector uniformly down
/// @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) {
return Vector3(v.x / f, v.y / f, v.z / f);
}
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);
}
Vector3 operator/=(float f);
/// @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 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);
/// @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);
/// @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);
/// @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);
/// @brief The angle between two vectors
/// @param v1 The first vector
/// @param v2 The second vector
/// @return The angle between the two vectors
/// @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);
/// @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);
/// @brief Lerp (linear interpolation) between two vectors
/// @param v1 The starting vector
/// @param v2 The ending vector
/// @param f The interpolation distance
/// @return The lerped vector
/// @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);
};
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#include "Spherical.h"
#endif

1
ControlCore/LinearAlgebra/component.mk Normal file
View File

@ -0,0 +1 @@
COMPONENT_ADD_INCLUDEDIRS = .

0
ControlCore/LinearAlgebra/test/.gitkeep Normal file
View File

239
ControlCore/LinearAlgebra/test/Angle16_test.cc Normal file
View File

@ -0,0 +1,239 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Angle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle16, Construct) {
float angle = 0.0F;
Angle16 a = Angle16::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = -180.0F;
a = Angle16::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 270.0F;
a = Angle16::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
}
TEST(Angle16, Negate) {
float angle = 0;
Angle16 a = Angle16::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 90.0F;
a = Angle16::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}
TEST(Angle16, Subtract) {
Angle16 a = Angle16::Degrees(0);
Angle16 b = Angle16::Degrees(45.0F);
Angle16 r = a - b;
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}
TEST(Angle16, Add) {
Angle16 a = Angle16::Degrees(-45);
Angle16 b = Angle16::Degrees(45.0F);
Angle16 r = a + b;
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}
TEST(Angle16, Compare) {
Angle16 a = Angle16::Degrees(45);
bool r = false;
r = a > Angle16::Degrees(0);
EXPECT_TRUE(r) << "45 > 0";
r = a > Angle16::Degrees(90);
EXPECT_FALSE(r) << "45 > 90";
r = a > Angle16::Degrees(-90);
EXPECT_TRUE(r) << "45 > -90";
}
TEST(Angle16, Normalize) {
Angle16 r = Angle16();
r = Angle16::Normalize(Angle16::Degrees(90.0f));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
r = Angle16::Normalize(Angle16::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
r = Angle16::Normalize(Angle16::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
r = Angle16::Normalize(Angle16::Degrees(270 + 360));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
r = Angle16::Normalize(Angle16::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
r = Angle16::Normalize(Angle16::Degrees(-270 - 360));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
r = Angle16::Normalize(Angle16::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
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";
r = Angle16::Normalize(Angle16::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(Angle16, Clamp) {
Angle16 r = Angle16();
// Clamp(1, 0, 2) will fail because Angle16 does not have enough resolution
// for this. Instead we use Clamp(10, 0, 20) etc.
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
Angle16::Degrees(20));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 10 0 20";
r = Angle16::Clamp(Angle16::Degrees(-10), Angle16::Degrees(0),
Angle16::Degrees(20));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -10 0 20";
r = Angle16::Clamp(Angle16::Degrees(30), Angle16::Degrees(0),
Angle16::Degrees(20));
EXPECT_NEAR(r.InDegrees(), 20, 1.0e-2) << "Clamp 30 0 20";
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
Angle16::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 10 0 0";
r = Angle16::Clamp(Angle16::Degrees(0), Angle16::Degrees(0),
Angle16::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
r = Angle16::Clamp(Angle16::Degrees(0), Angle16::Degrees(10),
Angle16::Degrees(-10));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
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));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 1 0 INFINITY";
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(-FLOAT_INFINITY),
Angle16::Degrees(10));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 1 -INFINITY 1";
}
}
// TEST(Angle16, Difference) {
// Angle16 r = 0;
// r = Angle16::Difference(0, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
// r = Angle16::Difference(0, -90);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
// r = Angle16::Difference(0, 270);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
// r = Angle16::Difference(0, -270);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
// r = Angle16::Difference(90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
// r = Angle16::Difference(-90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
// r = Angle16::Difference(0, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
// r = Angle16::Difference(90, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
// if (std::numeric_limits<float>::is_iec559) {
// r = Angle16::Difference(0, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
// r = Angle16::Difference(0, -INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
// r = Angle16::Difference(-INFINITY, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
// INFINITY";
// }
// }
TEST(Angle16, MoveTowards) {
Angle16 r = Angle16();
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 30);
EXPECT_NEAR(r.InDegrees(), 30, 1.0e-2) << "MoveTowards 0 90 30";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 90);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), 180);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -180";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 270);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -90);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -180);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -270);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
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) {
// infinites are not supported
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90),
FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = Angle16::MoveTowards(Angle16::Degrees(0),
Angle16::Degrees(FLOAT_INFINITY), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90),
-FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
<< "MoveTowards 0 -90 -FLOAT_INFINITY";
r = Angle16::MoveTowards(Angle16::Degrees(0),
Angle16::Degrees(-FLOAT_INFINITY), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif

239
ControlCore/LinearAlgebra/test/Angle8_test.cc Normal file
View File

@ -0,0 +1,239 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Angle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle8, Construct) {
float angle = 0.0F;
Angle8 a = Angle8::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = -180.0F;
a = Angle8::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 270.0F;
a = Angle8::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
}
TEST(Angle8, Negate) {
float angle = 0;
Angle8 a = Angle8::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 90.0F;
a = Angle8::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}
TEST(Angle8, Add) {
Angle8 a = Angle8::Degrees(-45);
Angle8 b = Angle8::Degrees(45.0F);
Angle8 r = a + b;
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}
TEST(Angle8, Subtract) {
Angle8 a = Angle8::Degrees(0);
Angle8 b = Angle8::Degrees(45.0F);
Angle8 r = a - b;
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}
TEST(Angle8, Compare) {
Angle8 a = Angle8::Degrees(45);
bool r = false;
r = a > Angle8::Degrees(0);
EXPECT_TRUE(r) << "45 > 0";
r = a > Angle8::Degrees(90);
EXPECT_FALSE(r) << "45 > 90";
r = a > Angle8::Degrees(-90);
EXPECT_TRUE(r) << "45 > -90";
}
TEST(Angle8, Normalize) {
Angle8 r = Angle8();
r = Angle8::Normalize(Angle8::Degrees(90.0f));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
r = Angle8::Normalize(Angle8::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
r = Angle8::Normalize(Angle8::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
r = Angle8::Normalize(Angle8::Degrees(270 + 360));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
r = Angle8::Normalize(Angle8::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
r = Angle8::Normalize(Angle8::Degrees(-270 - 360));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
r = Angle8::Normalize(Angle8::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
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";
r = Angle8::Normalize(Angle8::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(Angle8, Clamp) {
Angle8 r = Angle8();
// Clamp(1, 0, 2) will fail because Angle8 does not have enough resolution for
// this. Instead we use Clamp(10, 0, 20) etc.
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
Angle8::Degrees(20));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 10 0 20";
r = Angle8::Clamp(Angle8::Degrees(-10), Angle8::Degrees(0),
Angle8::Degrees(20));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -10 0 20";
r = Angle8::Clamp(Angle8::Degrees(30), Angle8::Degrees(0),
Angle8::Degrees(20));
EXPECT_NEAR(r.InDegrees(), 20, 1.0e-0) << "Clamp 30 0 20";
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
Angle8::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 10 0 0";
r = Angle8::Clamp(Angle8::Degrees(0), Angle8::Degrees(0), Angle8::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
r = Angle8::Clamp(Angle8::Degrees(0), Angle8::Degrees(10),
Angle8::Degrees(-10));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
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));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 1 0 INFINITY";
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(-FLOAT_INFINITY),
Angle8::Degrees(10));
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 1 -INFINITY 1";
}
}
// TEST(Angle8, Difference) {
// Angle8 r = 0;
// r = Angle8::Difference(0, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
// r = Angle8::Difference(0, -90);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
// r = Angle8::Difference(0, 270);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
// r = Angle8::Difference(0, -270);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
// r = Angle8::Difference(90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
// r = Angle8::Difference(-90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
// r = Angle8::Difference(0, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
// r = Angle8::Difference(90, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
// if (std::numeric_limits<float>::is_iec559) {
// r = Angle8::Difference(0, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
// r = Angle8::Difference(0, -INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
// r = Angle8::Difference(-INFINITY, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
// INFINITY";
// }
// }
TEST(Angle8, MoveTowards) {
Angle8 r = Angle8();
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 30);
EXPECT_NEAR(r.InDegrees(), 30, 1.0e-0) << "MoveTowards 0 90 30";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 90);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), 180);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -180";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 270);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -90);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -180);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -270);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
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) {
// infinites are not supported
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90),
FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(FLOAT_INFINITY),
30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90),
-FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
<< "MoveTowards 0 -90 -FLOAT_INFINITY";
r = Angle8::MoveTowards(Angle8::Degrees(0),
Angle8::Degrees(-FLOAT_INFINITY), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif

247
ControlCore/LinearAlgebra/test/AngleSingle_test.cc Normal file
View File

@ -0,0 +1,247 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Angle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(AngleSingle, Construct) {
float angle = 0.0F;
AngleSingle a = AngleSingle::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = -180.0F;
a = AngleSingle::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 270.0F;
a = AngleSingle::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
}
TEST(AngleSingle, Negate) {
float angle = 0;
AngleSingle a = AngleSingle::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 90.0F;
a = AngleSingle::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}
TEST(AngleSingle, Add) {
AngleSingle a = AngleSingle::Degrees(-45);
AngleSingle b = AngleSingle::Degrees(45.0F);
AngleSingle r = a + b;
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}
TEST(AngleSingle, Subtract) {
AngleSingle a = AngleSingle::Degrees(0);
AngleSingle b = AngleSingle::Degrees(45.0F);
AngleSingle r = a - b;
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}
TEST(AngleSingle, Compare) {
AngleSingle a = AngleSingle::Degrees(45);
bool r = false;
r = a > AngleSingle::Degrees(0);
EXPECT_TRUE(r) << "45 > 0";
r = a > AngleSingle::Degrees(90);
EXPECT_FALSE(r) << "45 > 90";
r = a > AngleSingle::Degrees(-90);
EXPECT_TRUE(r) << "45 > -90";
}
TEST(AngleSingle, Normalize) {
AngleSingle r = AngleSingle();
r = AngleSingle::Normalize(AngleSingle::Degrees(90.0f));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
r = AngleSingle::Normalize(AngleSingle::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
r = AngleSingle::Normalize(AngleSingle::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
r = AngleSingle::Normalize(AngleSingle::Degrees(270 + 360));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
r = AngleSingle::Normalize(AngleSingle::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
r = AngleSingle::Normalize(AngleSingle::Degrees(-270 - 360));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
r = AngleSingle::Normalize(AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
if (std::numeric_limits<float>::is_iec559) {
r = AngleSingle::Normalize(AngleSingle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
r = AngleSingle::Normalize(AngleSingle::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(AngleSingle, Clamp) {
AngleSingle r = AngleSingle();
r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 2";
r = AngleSingle::Clamp(AngleSingle::Degrees(-1), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -1 0 2";
r = AngleSingle::Clamp(AngleSingle::Degrees(3), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 2) << "Clamp 3 0 2";
r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 1 0 0";
r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(1),
AngleSingle::Degrees(-1));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 1 -1";
if (std::numeric_limits<float>::is_iec559) {
r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 INFINITY";
r = AngleSingle::Clamp(AngleSingle::Degrees(1),
AngleSingle::Degrees(-FLOAT_INFINITY),
AngleSingle::Degrees(1));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 -INFINITY 1";
}
}
// TEST(AngleSingle, Difference) {
// AngleSingle r = 0;
// r = AngleSingle::Difference(0, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
// r = AngleSingle::Difference(0, -90);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
// r = AngleSingle::Difference(0, 270);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
// r = AngleSingle::Difference(0, -270);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
// r = AngleSingle::Difference(90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
// r = AngleSingle::Difference(-90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
// r = AngleSingle::Difference(0, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
// r = AngleSingle::Difference(90, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
// if (std::numeric_limits<float>::is_iec559) {
// r = AngleSingle::Difference(0, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
// r = AngleSingle::Difference(0, -INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
// r = AngleSingle::Difference(-INFINITY, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
// INFINITY";
// }
// }
TEST(AngleSingle, MoveTowards) {
AngleSingle r = AngleSingle();
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 90 30";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 90);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 180);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 180";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 270);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -90);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -180);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -270);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), 0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
0);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
if (std::numeric_limits<float>::is_iec559) {
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(90), FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(FLOAT_INFINITY), 30);
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-90), -FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -FLOAT_INFINITY";
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
AngleSingle::Degrees(-FLOAT_INFINITY), -30);
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif

56
ControlCore/LinearAlgebra/test/Direction_test.cc Normal file
View File

@ -0,0 +1,56 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Direction.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Direction16, Compare) {
Direction16 d = Direction16::Degrees(45, 135);
bool r;
r = (d == Direction16(Angle16::Degrees(45), Angle16::Degrees(135)));
EXPECT_TRUE(r) << "45,135 == 45, 135";
r = (d ==
Direction16(Angle16::Degrees(45 + 360), Angle16::Degrees(135 - 360)));
EXPECT_TRUE(r) << "45+360, 135-360 == 45, 135";
}
TEST(Direction16, Inverse) {
Direction16 d;
Direction16 r;
d = Direction16::Degrees(45, 135);
r = -d;
EXPECT_EQ(r, Direction16::Degrees(-135, -135)) << "-(45, 135)";
d = Direction16::Degrees(-45, -135);
r = -d;
EXPECT_EQ(r, Direction16::Degrees(135, 135)) << "-(-45, -135)";
d = Direction16::Degrees(0, 0);
r = -d;
EXPECT_EQ(r, Direction16::Degrees(180, 0)) << "-(0, 0)";
d = Direction16::Degrees(0, 45);
r = -d;
EXPECT_EQ(r, Direction16::Degrees(180, -45)) << "-(0, 45)";
}
TEST(Direction16, Equality) {
Direction16 d;
d = Direction16::Degrees(135, 45);
EXPECT_EQ(d, Direction16::Degrees(135, 45)) << "(135, 45) == (135, 45)";
EXPECT_EQ(d, Direction16::Degrees(135 + 360, 45))
<< "(135, 45) == (135 + 360, 45) ";
EXPECT_EQ(d, Direction16::Degrees(135 - 360, 45))
<< "(135, 135) == (135 - 360, 45) ";
d = Direction16::Degrees(0, 45 + 180);
EXPECT_EQ(d, Direction16::Degrees(180, -45)) << "(0, 45+180) == (180, -45)";
}
#endif

82
ControlCore/LinearAlgebra/test/DiscreteAngle_test.cc Normal file
View File

@ -0,0 +1,82 @@
/*
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Angle.h"
// #include "Angle16.h"
// #include "Angle8.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle8, Construct) {
float angle = 0.0F;
Angle8 a = Angle8::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = -180.0F;
a = Angle8::Degrees(angle);
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
}
TEST(Angle8, Negate) {
float angle = 0;
Angle8 a = Angle8::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 90.0F;
a = Angle8::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}
TEST(Angle8, Add) {
Angle8 a = Angle8::Degrees(-45);
Angle8 b = Angle8::Degrees(45.0F);
Angle8 r = a + b;
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}
TEST(Angle8, Subtract) {
Angle8 a = Angle8::Degrees(0);
Angle8 b = Angle8::Degrees(45.0F);
Angle8 r = a - b;
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}
TEST(Angle16, Construct) {
Angle16 a = Angle16::Degrees(0.0F);
EXPECT_FLOAT_EQ(a.InDegrees(), 0);
}
TEST(Angle16, Negate) {
float angle = 0;
Angle16 a = Angle16::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
angle = 90.0F;
a = Angle16::Degrees(angle);
a = -a;
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
}
TEST(Angle16, Subtract) {
Angle16 a = Angle16::Degrees(0);
Angle16 b = Angle16::Degrees(45.0F);
Angle16 r = a - b;
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
}
TEST(Angle16, Add) {
Angle16 a = Angle16::Degrees(-45);
Angle16 b = Angle16::Degrees(45.0F);
Angle16 r = a + b;
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
}
#endif
*/

41
ControlCore/LinearAlgebra/test/FloatSingle_test.cc Normal file
View File

@ -0,0 +1,41 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "FloatSingle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(FloatC, Clamp) {
float r = 0;
r = Float::Clamp(1, 0, 2);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 2";
r = Float::Clamp(-1, 0, 2);
EXPECT_FLOAT_EQ(r, 0) << "Clamp -1 0 2";
r = Float::Clamp(3, 0, 2);
EXPECT_FLOAT_EQ(r, 2) << "Clamp 3 0 2";
r = Float::Clamp(1, 0, 0);
EXPECT_FLOAT_EQ(r, 0) << "Clamp 1 0 0";
r = Float::Clamp(0, 0, 0);
EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 0 0";
r = Float::Clamp(0, 1, -1);
EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 1 -1";
if (std::numeric_limits<float>::is_iec559) {
r = Float::Clamp(1, 0, FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 INFINITY";
r = Float::Clamp(1, -FLOAT_INFINITY, 1);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 -INFINITY 1";
}
}
#endif

135
ControlCore/LinearAlgebra/test/Matrix_test.cc Normal file
View File

@ -0,0 +1,135 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Matrix.h"
TEST(MatrixSingle, Init) {
// zero
MatrixOf<float> m0 = MatrixOf<float>(0, 0);
// one
float data1[] = {1.0F};
MatrixOf<float> m1 = MatrixOf<float>(1, 1, data1);
// two
float data2[] = {1.0F, 2.0F, 3.0F, 4.0F};
MatrixOf<float> m2 = MatrixOf<float>(2, 2, data2);
// negative
// MatrixOf<float> m_1 = MatrixOf<float>(-1, -1);
// parameters are unsigned
}
TEST(MatrixSingle, Transpose) {
float data1[] = {1.0F};
MatrixOf<float> m = MatrixOf<float>(1, 1, data1);
MatrixOf<float> r = MatrixOf<float>(1, 1);
m.Transpose(&r);
// 2 x 2
float data3[] = {1.0F, 2.0F, 3.0F, 4.0F};
MatrixOf<float> m22 = MatrixOf<float>(2, 2, data3);
EXPECT_EQ(m22.RowCount(), 2);
EXPECT_EQ(m22.ColCount(), 2);
float data4[] = {0.0F, 0.0F, 0.0F, 0.0F};
MatrixOf<float> r22 = MatrixOf<float>(2, 2, data4);
EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2);
m22.Transpose(&r22);
EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2);
EXPECT_FLOAT_EQ(r22.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r22.Get(0, 1), 3.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 0), 2.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 1), 4.0F);
// 1 x 2
float data12[] = {1.0F, 2.0F};
MatrixOf<float> m12 = MatrixOf<float>(1, 2, data12);
EXPECT_EQ(m12.RowCount(), 1);
EXPECT_EQ(m12.ColCount(), 2);
float data21[] = {0.0F, 0.0F};
MatrixOf<float> r21 = MatrixOf<float>(2, 1, data21);
EXPECT_EQ(r21.RowCount(), 2);
EXPECT_EQ(r21.ColCount(), 1);
m12.Transpose(&r21);
EXPECT_EQ(r21.RowCount(), 2);
EXPECT_EQ(r21.ColCount(), 1);
EXPECT_FLOAT_EQ(r21.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r21.Get(1, 0), 2.0F);
// changing dimensions, same size is okay
MatrixOf<float> r12 = MatrixOf<float>(1, 2, data21);
EXPECT_EQ(r12.RowCount(), 1);
EXPECT_EQ(r12.ColCount(), 2);
m12.Transpose(&r12);
EXPECT_EQ(r12.RowCount(), 2);
EXPECT_EQ(r12.ColCount(), 1);
EXPECT_FLOAT_EQ(r12.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r12.Get(0, 1), 2.0F);
}
TEST(MatrixSingle, Multiply) {
float m12data[] = {1.0F, 2.0F};
MatrixOf<float> m12 = MatrixOf<float>(1, 2, m12data);
EXPECT_EQ(m12.RowCount(), 1);
EXPECT_EQ(m12.ColCount(), 2);
EXPECT_FLOAT_EQ(m12.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(m12.Get(0, 1), 2.0F);
float m21data[] = {3.0F, 4.0F};
MatrixOf<float> m21 = MatrixOf<float>(2, 1, m21data);
EXPECT_EQ(m21.RowCount(), 2);
EXPECT_EQ(m21.ColCount(), 1);
EXPECT_FLOAT_EQ(m21.Get(0, 0), 3.0F);
EXPECT_FLOAT_EQ(m21.Get(1, 0), 4.0F);
float r11data[] = {0.0F};
MatrixOf<float> r11 = MatrixOf<float>(1, 1, r11data);
EXPECT_EQ(r11.RowCount(), 1);
EXPECT_EQ(r11.ColCount(), 1);
MatrixOf<float>::Multiply(&m12, &m21, &r11);
EXPECT_EQ(r11.RowCount(), 1);
EXPECT_EQ(r11.ColCount(), 1);
EXPECT_FLOAT_EQ(r11.Get(0, 0), 11.0F);
float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F};
MatrixOf<float> r22 = MatrixOf<float>(2, 2, r22data);
MatrixOf<float>::Multiply(&m21, &m12, &r22);
EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2);
EXPECT_FLOAT_EQ(r22.Get(0, 0), 3.0F);
EXPECT_FLOAT_EQ(r22.Get(0, 1), 4.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 0), 6.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 1), 8.0F);
}
TEST(MatrixSingle, Multiply_Vector3) {
Vector3 v = Vector3(1.0, 2.0, 3.0);
Vector3 r = Vector3::zero;
// float m13data[] = {3.0, 4.0, 5.0};
// MatrixOf<float> m13 = MatrixOf<float>(1, 3, m13data);
// Vector3 r = MatrixOf<float>::Multiply(&m13, v);
float m33data[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
MatrixOf<float> m33 = MatrixOf<float>(3, 3, m33data);
r = MatrixOf<float>::Multiply(&m33, v);
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(1.0f, 2.0f, 3.0f)), 0);
}
#endif

232
ControlCore/LinearAlgebra/test/Polar_test.cc Normal file
View File

@ -0,0 +1,232 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Polar.h"
#include "Spherical.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Polar, FromVector2) {
Vector2 v = Vector2(0, 1);
PolarSingle p = PolarSingle::FromVector2(v);
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance 0 1";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "s.angle 0 0 1";
v = Vector2(1, 0);
p = PolarSingle::FromVector2(v);
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance 1 0";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 90.0F) << "s.angle 1 0";
v = Vector2(-1, 1);
p = PolarSingle::FromVector2(v);
EXPECT_FLOAT_EQ(p.distance, sqrt(2.0F)) << "p.distance -1 1";
EXPECT_NEAR(p.angle.InDegrees(), -45.0F, 1.0e-05) << "s.angle -1 1";
}
TEST(Polar, FromSpherical) {
SphericalSingle s;
PolarSingle p;
s = SphericalSingle(1, DirectionSingle::forward);
p = PolarSingle::FromSpherical(s);
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 0 0)";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(1 0 0)";
s = SphericalSingle(1, AngleSingle::Degrees(45), AngleSingle::Degrees(0));
p = PolarSingle::FromSpherical(s);
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 45 0)";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 45.0F)
<< "p.angle FromSpherical(1 45 0)";
s = SphericalSingle(1, AngleSingle::Degrees(-45), AngleSingle::Degrees(0));
p = PolarSingle::FromSpherical(s);
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 -45 0)";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), -45.0F)
<< "p.angle FromSpherical(1 -45 0)";
s = SphericalSingle(0, AngleSingle::Degrees(0), AngleSingle::Degrees(0));
p = PolarSingle::FromSpherical(s);
EXPECT_FLOAT_EQ(p.distance, 0.0F) << "p.distance FromSpherical(0 0 0)";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(0 0 0)";
s = SphericalSingle(-1, AngleSingle::Degrees(0), AngleSingle::Degrees(0));
p = PolarSingle::FromSpherical(s);
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(-1 0 0)";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), -180.0F)
<< "p.angle FromSpherical(-1 0 0)";
s = SphericalSingle(0, AngleSingle::Degrees(0), AngleSingle::Degrees(90));
p = PolarSingle::FromSpherical(s);
EXPECT_FLOAT_EQ(p.distance, 0.0F) << "p.distance FromSpherical(0 0 90)";
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(0 0 90)";
}
TEST(Polar, Negation) {
PolarSingle v = PolarSingle(2, AngleSingle::Degrees(45));
PolarSingle r = PolarSingle::zero;
r = -v;
EXPECT_FLOAT_EQ(r.distance, 2);
EXPECT_FLOAT_EQ(r.angle.InDegrees(), -135);
EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(-135)))
<< "Negate(2 45)";
v = PolarSingle::Deg(2, -45);
r = -v;
EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(135)))
<< "Negate(2 -45)";
v = PolarSingle::Degrees(2, 0);
r = -v;
EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(180))) << "Negate(2 0)";
v = PolarSingle(0, AngleSingle::Degrees(0));
r = -v;
EXPECT_FLOAT_EQ(r.distance, 0.0f);
EXPECT_FLOAT_EQ(r.angle.InDegrees(), 0.0f);
EXPECT_TRUE(r == PolarSingle(0, AngleSingle::Degrees(0))) << "Negate(0 0)";
}
TEST(Polar, Subtraction) {
PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
PolarSingle r = PolarSingle::zero;
r = v1 - v2;
// don't know what to expect yet
v2 = PolarSingle::zero;
r = v1 - v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Subtraction(0 0)";
}
TEST(Polar, Addition) {
PolarSingle v1 = PolarSingle(1, AngleSingle::Degrees(45));
PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
PolarSingle r = PolarSingle::zero;
r = v1 - v2;
// don't know what to expect yet
v2 = PolarSingle::zero;
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
r = v1;
r += v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
v2 = PolarSingle(1, AngleSingle::Degrees(-45));
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(0 0 0)";
EXPECT_FLOAT_EQ(r.angle.InDegrees(), 0) << "Addition(0 0 0)";
}
TEST(Polar, Scale_Multiply) {
PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
PolarSingle r = PolarSingle::zero;
r = v1 * 2.0f;
EXPECT_FLOAT_EQ(r.distance, v1.distance * 2) << "ScaleMult(4 45, 2)";
EXPECT_FLOAT_EQ(r.angle.InDegrees(), v1.angle.InDegrees())
<< "ScaleMult(4 45, 2)";
}
TEST(Polar, Scale_Divide) {
PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
PolarSingle r = PolarSingle::zero;
r = v1 / 2.0f;
EXPECT_FLOAT_EQ(r.distance, v1.distance / 2) << "ScaleDiv(4 45, 2)";
EXPECT_FLOAT_EQ(r.angle.InDegrees(), v1.angle.InDegrees())
<< "ScaleDiv(4 45, 2)";
}
TEST(Polar, Distance) {
PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
float d = 0;
d = PolarSingle::Distance(v1, v2);
// don't know what to expect yet
v2 = PolarSingle::zero;
d = PolarSingle::Distance(v1, v2);
EXPECT_FLOAT_EQ(d, v1.distance) << "Distance(4 45, zero)";
}
TEST(Polar, Rotate) {
PolarSingle v = PolarSingle(4, AngleSingle::Degrees(45));
PolarSingle r = PolarSingle::zero;
r = PolarSingle::Rotate(v, AngleSingle::Degrees(45));
EXPECT_FLOAT_EQ(r.distance, v.distance) << "Rotate(4 45, 45)";
EXPECT_FLOAT_EQ(r.angle.InDegrees(), 90.0f) << "Rotate(4 45, 45)";
}
// Performance Test
TEST(PolarOfTest, PerformanceTest) {
const int numIterations = 1000000; // Number of instances to test
std::vector<PolarOf<float>> polarObjects;
// Measure time for creating a large number of PolarOf objects
auto start = std::chrono::high_resolution_clock::now();
for (int i = 0; i < numIterations; ++i) {
float distance =
static_cast<float>(rand() % 100); // Random distance from 0 to 100
AngleOf<float> angle = AngleOf<float>::Degrees(
static_cast<float>(rand() % 360)); // Random angle from 0 to 360 degrees
PolarOf<float> p = PolarOf<float>(distance, angle);
polarObjects.emplace_back(p); // Create and store the object
}
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> duration = end - start;
std::cout << "Time to construct " << numIterations
<< " PolarOf objects: " << duration.count() << " seconds."
<< std::endl;
// Test completion with a message
ASSERT_GE(duration.count(), 0); // Ensure duration is non-negative
// Assert that the duration is less than or equal to 1 second
ASSERT_LE(duration.count(), 1.0)
<< "Performance test failed: Construction took longer than 1 second.";
}
// Edge Case 1: Testing with distance = 0 and angle = 45
TEST(PolarOfTest, TestDistanceZero) {
PolarOf<float> p1(0.0f, AngleOf<float>::Degrees(45.0f));
EXPECT_EQ(p1.distance, 0.0f); // Ensure distance is 0
EXPECT_EQ(p1.angle.InDegrees(), 0.0f); // Ensure angle is 0 when distance is 0
}
// Edge Case 2: Testing with negative distance, angle should be adjusted
TEST(PolarOfTest, TestNegativeDistance) {
PolarOf<float> p2(-10.0f, AngleOf<float>::Degrees(90.0f));
EXPECT_EQ(p2.distance, 10.0f); // Ensure distance is positive
EXPECT_NEAR(p2.angle.InDegrees(), -90.0f,
0.0001f); // Ensure angle is normalized to 270 degrees (180 + 90)
}
// Edge Case 3: Testing with positive distance and angle = 180
TEST(PolarOfTest, TestPositiveDistance) {
PolarOf<float> p3(100.0f, AngleOf<float>::Degrees(180.0f));
EXPECT_EQ(p3.distance, 100.0f); // Ensure distance is correct
EXPECT_NEAR(p3.angle.InDegrees(), -180.0f,
0.0001f); // Ensure angle is correct
}
#endif

189
ControlCore/LinearAlgebra/test/Quaternion_test.cc Normal file
View File

@ -0,0 +1,189 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Quaternion.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Quaternion, Normalize) {
bool r = false;
Quaternion q1 = Quaternion(0, 0, 0, 1);
Quaternion q = Quaternion::identity;
q = q1;
q.Normalize();
r = q == q1;
EXPECT_TRUE(r) << "q.Normalzed 0 0 0 1";
q = Quaternion::Normalize(q1);
r = q == q1;
EXPECT_TRUE(r) << "Quaternion::Normalize 0 0 0 1";
}
TEST(Quaternion, ToAngles) {
bool r = false;
Quaternion q1 = Quaternion(0, 0, 0, 1);
Vector3 v = Vector3::zero;
v = Quaternion::ToAngles(q1);
r = v == Vector3(0, 0, 0);
EXPECT_TRUE(r) << "Quaternion::ToAngles 0 0 0 1";
q1 = Quaternion(1, 0, 0, 0);
v = Quaternion::ToAngles(q1);
r = v == Vector3(180, 0, 0);
EXPECT_TRUE(r) << "Quaternion::ToAngles 1 0 0 0";
}
TEST(Quaternion, Multiplication) {
bool r = false;
Quaternion q1 = Quaternion(0, 0, 0, 1);
Quaternion q2 = Quaternion(1, 0, 0, 0);
Quaternion q = Quaternion::identity;
q = q1 * q2;
r = q == Quaternion(1, 0, 0, 0);
EXPECT_TRUE(r) << "0 0 0 1 * 1 0 0 0";
}
TEST(Quaternion, MultiplicationVector) {
bool r = false;
Quaternion q1 = Quaternion(0, 0, 0, 1);
Vector3 v1 = Vector3(0, 1, 0);
Vector3 v = Vector3::zero;
v = q1 * v1;
r = v == Vector3(0, 1, 0);
EXPECT_TRUE(r) << "0 0 0 1 * Vector 0 1 0";
q1 = Quaternion(1, 0, 0, 0);
v = q1 * v1;
r = v == Vector3(0, -1, 0);
EXPECT_TRUE(r) << "1 0 0 0 * Vector 0 1 0";
}
TEST(Quaternion, Equality) {
bool r = false;
Quaternion q1 = Quaternion(0, 0, 0, 1);
Quaternion q2 = Quaternion(1, 0, 0, 0);
r = q1 == q2;
EXPECT_FALSE(r) << " 0 0 0 1 == 1 0 0 0";
q2 = Quaternion(0, 0, 0, 1);
r = q1 == q2;
EXPECT_TRUE(r) << "0 0 0 1 == 0 0 0 1";
}
TEST(Quaternion, Inverse) {
}
TEST(Quaternion, LookRotation) {
}
TEST(Quaternion, FromToRotation) {
}
TEST(Quaternion, RotateTowards) {
}
TEST(Quaternion, AngleAxis) {
}
TEST(Quaternion, Angle) {
}
TEST(Quaternion, Slerp) {
}
TEST(Quaternion, SlerpUnclamped) {
}
TEST(Quaternion, Euler) {
bool r = false;
Vector3 v1 = Vector3(0, 0, 0);
Quaternion q = Quaternion::identity;
q = Quaternion::Euler(v1);
r = q == Quaternion::identity;
EXPECT_TRUE(r) << "Euler Vector 0 0 0";
q = Quaternion::Euler(0, 0, 0);
r = q == Quaternion::identity;
EXPECT_TRUE(r) << "Euler 0 0 0";
v1 = Vector3(90, 90, -90);
q = Quaternion::Euler(v1);
r = q == Quaternion(0, 0.707106709F, -0.707106709F, 0);
EXPECT_TRUE(r) << "Euler Vector 90 90 -90";
q = Quaternion::Euler(90, 90, -90);
r = q == Quaternion(0, 0.707106709F, -0.707106709F, 0);
EXPECT_TRUE(r) << "Euler 90 90 -90";
}
TEST(Quaternion, GetAngleAround) {
bool r = false;
Vector3 v1 = Vector3(0, 1, 0);
Quaternion q1 = Quaternion(0, 0, 0, 1);
float f;
f = Quaternion::GetAngleAround(v1, q1);
EXPECT_FLOAT_EQ(f, 0) << "GetAngleAround 0 1 0 , 0 0 0 1";
q1 = Quaternion(0, 0.707106709F, -0.707106709F, 0);
f = Quaternion::GetAngleAround(v1, q1);
EXPECT_FLOAT_EQ(f, 180) << "GetAngleAround 0 1 0 , 0 0.7 -0.7 0";
v1 = Vector3(0, 0, 0);
f = Quaternion::GetAngleAround(v1, q1);
r = isnan(f);
EXPECT_TRUE(r) << "GetAngleAround 0 0 0 , 0 0.7 -0.7 0";
}
TEST(Quaternion, GetRotationAround) {
bool r = false;
Vector3 v1 = Vector3(0, 1, 0);
Quaternion q1 = Quaternion(0, 0, 0, 1);
Quaternion q = Quaternion::identity;
q = Quaternion::GetRotationAround(v1, q1);
r = q == Quaternion::identity;
EXPECT_TRUE(r) << "GetRotationAround 0 1 0 , 0 0 0 1";
q1 = Quaternion(0, 0.707106709F, -0.707106709F, 0);
q = Quaternion::GetRotationAround(v1, q1);
r = q == Quaternion(0, 1, 0, 0);
EXPECT_TRUE(r) << "GetRotationAround 0 1 0 , 0 0.7 -0.7 0";
v1 = Vector3(0, 0, 0);
q = Quaternion::GetRotationAround(v1, q1);
r = isnan(q.x) && isnan(q.y) && isnan(q.z) && isnan(q.w);
EXPECT_TRUE(r) << "GetRotationAround 0 0 0 , 0 0.7 -0.7 0";
}
TEST(Quaternion, GetSwingTwist) {
}
TEST(Quaternion, Dot) {
}
#endif

222
ControlCore/LinearAlgebra/test/Spherical16_test.cc Normal file
View File

@ -0,0 +1,222 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Spherical.h"
#include "Vector3.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Spherical16, FromVector3) {
Vector3 v = Vector3(0, 0, 1);
Spherical16 s = Spherical16::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
EXPECT_FLOAT_EQ((float)s.direction.horizontal.InDegrees(), 0.0F)
<< "s.hor 0 0 1";
EXPECT_FLOAT_EQ((float)s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert 0 0 1";
v = Vector3(0, 1, 0);
s = Spherical16::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0";
v = Vector3(1, 0, 0);
s = Spherical16::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
}
TEST(Spherical16, Vector3) {
Vector3 v = Vector3(1, 2, 3);
Spherical16 rd = Spherical16::FromVector3(v);
Vector3 rv = rd.ToVector3();
EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
v = Vector3(1, 2, -3);
rd = Spherical16::FromVector3(v);
rv = rd.ToVector3();
EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
}
// TEST(Spherical16, FromPolar) {
// Polar p = Polar(1, 0);
// Spherical16 s = Spherical16::FromPolar(p);
// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 0.0F) << "s.hor Polar(1 0)";
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 0)";
// p = Polar(1, 45);
// s = Spherical16::FromPolar(p);
// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 45.0F) << "s.hor Polar(1 45)";
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 45)";
// p = Polar(1, -45);
// s = Spherical16::FromPolar(p);
// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), -45.0F) << "s.hor Polar(1 -45)";
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 -45)";
// p = Polar(0, 0);
// s = Spherical16::FromPolar(p);
// EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 0.0F) << "s.hor Polar(0 0)";
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(0 0)";
// p = Polar(-1, 0);
// s = Spherical16::FromPolar(p);
// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), -180.0F) << "s.hor Polar(-1 0)";
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(-1 0)";
// }
TEST(Spherical16, Incident1) {
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
Spherical16 s = Spherical16::FromVector3(v);
Spherical16 sr =
Spherical16(2.49F, Angle16::Degrees(98.18f), Angle16::Degrees(24.4F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-02);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-02);
Vector3 r =
Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
.ToVector3();
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0";
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
}
TEST(Spherical16, Incident2) {
Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
Spherical16 s = Spherical16::FromVector3(v);
Spherical16 sr = Spherical16(1.4142135623F, Angle16::Degrees(45.0f),
Angle16::Degrees(0.0F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
Vector3 r =
Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
.ToVector3();
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(0.0f, 1.0f, 1.0f);
s = Spherical16::FromVector3(v);
sr = Spherical16(1.4142135623F, Angle16::Degrees(0), Angle16::Degrees(45));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
r = Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
.ToVector3();
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(1.0f, 1.0f, 1.0f);
s = Spherical16::FromVector3(v);
r = Spherical16(s.distance, s.direction.horizontal, s.direction.vertical)
.ToVector3();
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02);
EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-04);
// s = Spherical16(10, 45, 45);
// r = s.ToVector3();
// EXPECT_NEAR(r.x, 5, 1.0e-06);
// EXPECT_NEAR(r.y, 7.07, 1.0e-06);
// EXPECT_NEAR(r.z, 5, 1.0e-06);
}
TEST(Spherical16, Addition) {
Spherical16 v1 = Spherical16(1, Angle16::Degrees(45), Angle16::Degrees(0));
Spherical16 v2 = Spherical16::zero;
Spherical16 r = Spherical16::zero;
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
r = v1;
r += v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
v2 = Spherical16(1, Angle16::Degrees(-45), Angle16::Degrees(0));
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";
v2 = Spherical16(1, Angle16::Degrees(0), Angle16::Degrees(90));
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)";
}
TEST(Spherical16, AdditionPerformance) {
const int numIterations = 1000000; // Number of additions to test
std::vector<Spherical16> sphericalObjects;
// Populate the vector with random SphericalOf objects
for (int i = 0; i < numIterations; ++i) {
float distance = (float)(rand() % 100);
float horizontal = (float)(rand() % 180);
float vertical = (float)(rand() % 360);
Spherical16 s = Spherical16::Deg(distance, horizontal, vertical);
sphericalObjects.push_back(s);
}
// Measure the time to perform multiple additions
auto start = std::chrono::high_resolution_clock::now();
Spherical16 result = Spherical16::zero; // Start with a
// zero-initialized object
for (int i = 0; i < numIterations - 1; ++i) {
result = result + sphericalObjects[i]; // Add objects
// together
}
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> duration = end - start;
std::cout << "Time to perform " << numIterations - 1
<< " additions: " << duration.count() << " seconds." << std::endl;
// Assert that the time taken is less than
// 1 second (or any other performance
// requirement)
ASSERT_LE(duration.count(), 2.0) << "Performance test failed: "
"Additions took longer than 1 "
"second.";
}
#endif

213
ControlCore/LinearAlgebra/test/SphericalSingle_test.cc Normal file
View File

@ -0,0 +1,213 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Spherical.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(SphericalSingle, FromVector3) {
Vector3 v = Vector3(0, 0, 1);
SphericalSingle s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 0 1";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 0 0 1";
v = Vector3(0, 1, 0);
s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0";
v = Vector3(1, 0, 0);
s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
}
TEST(SphericalSingle, FromPolar) {
PolarSingle p = PolarSingle(1, AngleSingle::Degrees(0));
SphericalSingle s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
<< "s.hor Polar(1 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 0)";
p = PolarSingle(1, AngleSingle::Degrees(45));
s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 45.0F)
<< "s.hor Polar(1 45)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 45)";
p = PolarSingle(1, AngleSingle::Degrees(-45));
s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -45.0F)
<< "s.hor Polar(1 -45)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 -45)";
p = PolarSingle(0, AngleSingle::Degrees(0));
s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
<< "s.hor Polar(0 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(0 0)";
p = PolarSingle(-1, AngleSingle::Degrees(0));
s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -180.0F)
<< "s.hor Polar(-1 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(-1 0)";
}
TEST(SphericalSingle, Incident1) {
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
SphericalSingle s = SphericalSingle ::FromVector3(v);
SphericalSingle sr = SphericalSingle(2.49F, AngleSingle::Degrees(98.18f),
AngleSingle::Degrees(24.4F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-02);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-02);
Vector3 r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0";
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
}
TEST(SphericalSingle, Incident2) {
Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
SphericalSingle s = SphericalSingle ::FromVector3(v);
SphericalSingle sr = SphericalSingle(
1.4142135623F, AngleSingle::Degrees(45.0f), AngleSingle::Degrees(0.0F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
Vector3 r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(0.0f, 1.0f, 1.0f);
s = SphericalSingle ::FromVector3(v);
sr = SphericalSingle(1.4142135623F, AngleSingle::Degrees(0.0f),
AngleSingle::Degrees(45.0F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(1.0f, 1.0f, 1.0f);
s = SphericalSingle ::FromVector3(v);
r = Vector3(s);
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02);
EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
// s = SphericalSingle(10, 45, 45);
// r = s.ToVector3();
// EXPECT_NEAR(r.x, 5, 1.0e-06);
// EXPECT_NEAR(r.y, 7.07, 1.0e-06);
// EXPECT_NEAR(r.z, 5, 1.0e-06);
}
TEST(SphericalSingle, Addition) {
SphericalSingle v1 =
SphericalSingle(1, AngleSingle::Degrees(45), AngleSingle::Degrees(0));
SphericalSingle v2 = SphericalSingle ::zero;
SphericalSingle r = SphericalSingle ::zero;
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
r = v1;
r += v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
v2 = SphericalSingle(1, AngleSingle::Degrees(-45), AngleSingle::Degrees(0));
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";
v2 = SphericalSingle(1, AngleSingle::Degrees(0), AngleSingle::Degrees(90));
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)";
}
TEST(SphericalSingle, AdditionPerformance) {
const int numIterations = 1000000; // Number of additions to test
std::vector<SphericalSingle> sphericalObjects;
// Populate the vector with random SphericalOf objects
for (int i = 0; i < numIterations; ++i) {
float distance = (float)(rand() % 100);
float horizontal = (float)(rand() % 180);
float vertical = (float)(rand() % 360);
SphericalSingle s = SphericalSingle::Deg(distance, horizontal, vertical);
sphericalObjects.push_back(s);
}
// Measure the time to perform multiple additions
auto start = std::chrono::high_resolution_clock::now();
SphericalSingle result = SphericalSingle::zero; // Start with a
// zero-initialized object
for (int i = 0; i < numIterations - 1; ++i) {
result = result + sphericalObjects[i]; // Add objects
// together
}
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> duration = end - start;
std::cout << "Time to perform " << numIterations - 1
<< " additions: " << duration.count() << " seconds." << std::endl;
// Assert that the time taken is less than
// 1 second (or any other performance
// requirement)
ASSERT_LE(duration.count(), 1.0) << "Performance test failed: "
"Additions took longer than 1 "
"second.";
}
#endif

131
ControlCore/LinearAlgebra/test/SwingTwistSingle_test.cc Normal file
View File

@ -0,0 +1,131 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "SwingTwist.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(SwingTwistSingle, Quaternion) {
Quaternion q;
SwingTwistSingle s;
Quaternion rq;
q = Quaternion::identity;
s = SwingTwistSingle::FromQuaternion(q);
rq = s.ToQuaternion();
EXPECT_EQ(q, rq) << " 0 0 0 1 <-> SwingTwist";
q = Quaternion::Euler(90, 0, 0);
s = SwingTwistSingle::FromQuaternion(q);
rq = s.ToQuaternion();
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 90 0 0 <-> SwingTwist";
q = Quaternion::Euler(0, 90, 0);
s = SwingTwistSingle::FromQuaternion(q);
rq = s.ToQuaternion();
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
q = Quaternion::Euler(0, 0, 90);
s = SwingTwistSingle::FromQuaternion(q);
rq = s.ToQuaternion();
EXPECT_EQ(q, rq) << " Euler 0 0 90 <-> SwingTwist";
q = Quaternion::Euler(0, 180, 0); // ==> spherical S(180 0)T0
s = SwingTwistSingle::FromQuaternion(q);
rq = s.ToQuaternion();
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
q = Quaternion::Euler(0, 135, 0); // ==> spherical S(180 45)T0
s = SwingTwistSingle::FromQuaternion(q);
rq = s.ToQuaternion();
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
}
TEST(SwingTwistSingle, AngleAxis) {
SwingTwistSingle s;
SwingTwistSingle r;
s = SwingTwistSingle::AngleAxis(0, DirectionSingle::up);
EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 up";
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::up);
s = SwingTwistSingle::Degrees(90, 0, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "90 up";
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::up);
s = SwingTwistSingle::Degrees(180, 0, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "180 up";
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::up);
s = SwingTwistSingle::Degrees(-90, 0, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "270 up";
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::right);
s = SwingTwistSingle::Degrees(0, 90, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "90 right";
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::right);
s = SwingTwistSingle::Degrees(0, 180, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "180 right";
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::right);
s = SwingTwistSingle::Degrees(0, -90, 0);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "270 right";
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::forward);
s = SwingTwistSingle::Degrees(0, 0, 90);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "90 up";
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::forward);
s = SwingTwistSingle::Degrees(0, 0, 180);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "180 up";
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::forward);
s = SwingTwistSingle::Degrees(0, 0, -90);
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
<< "270 up";
auto r16 = SwingTwist16::AngleAxis(13, Direction16::down);
auto s16 = SwingTwist16::Degrees(-13, 0, 0);
EXPECT_LT(SwingTwist16::Angle(r16, s16), Angle16::Degrees(10e-2f))
<< "270 up";
}
TEST(SwingTwistSingle, Normalize) {
SwingTwistSingle s;
s = SwingTwistSingle::Degrees(0, 0, 0);
EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 0 0 Normalized";
s = SwingTwistSingle::Degrees(0, 180, 0);
EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 180)) << "0 180 0 Normalized";
s = SwingTwistSingle::Degrees(0, 180, 180);
EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 0)) << "0 180 180 Normalized";
s = SwingTwistSingle::Degrees(270, 90, 0);
EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 90, 0)) << "270 90 0 Normalized";
s = SwingTwistSingle::Degrees(270, 270, 0);
EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, -90, 0))
<< "270 270 0 Normalized";
s = SwingTwistSingle::Degrees(270, 225, 0);
EXPECT_EQ(s, SwingTwistSingle::Degrees(90, -45, -180))
<< "270 225 0 Normalized";
s = SwingTwistSingle::Degrees(270, 0, 225);
EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 0, -135))
<< "270 0 225 Normalized";
}
#endif

499
ControlCore/LinearAlgebra/test/Vector2_test.cc Normal file
View File

@ -0,0 +1,499 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Vector2.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Vector2, FromPolar) {
Vector2 v;
PolarSingle p;
Vector2 r;
v = Vector2(0, 1);
p = PolarSingle::FromVector2(v);
r = Vector2(p);
EXPECT_FLOAT_EQ(r.x, 0.0F) << "FromPolar(0 1)";
EXPECT_FLOAT_EQ(r.y, 1.0F) << "FromPolar(0 1)";
v = Vector2(1, 0);
p = PolarSingle::FromVector2(v);
r = Vector2(p);
EXPECT_FLOAT_EQ(r.x, 1.0F) << "FromPolar(1 0)";
EXPECT_NEAR(r.y, 0.0F, 1.0e-07) << "FromPolar(1 0)";
v = Vector2(0, 0);
p = PolarSingle::FromVector2(v);
r = Vector2(p);
EXPECT_FLOAT_EQ(r.x, 0.0F) << "FromPolar(0 0)";
EXPECT_FLOAT_EQ(r.y, 0.0F) << "FromPolar(0 0)";
}
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, SqrMagnitude) {
Vector2 v = Vector2(1, 2);
float m = 0;
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, 5) << "v.sqrMagnitude 1 2";
m = Vector2::SqrMagnitude(v);
EXPECT_FLOAT_EQ(m, 5) << "Vector2::SqrMagnitude 1 2";
v = Vector2(-1, -2);
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, 5) << "v.sqrMagnitude -1 -2";
v = Vector2(0, 0);
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, 0) << "v.sqrMagnitude 0 0 ";
if (std::numeric_limits<float>::is_iec559) {
v = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.sqrMagnitude INFINITY INFINITY ";
v = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.sqrMagnitude -INFINITY -INFINITY ";
}
}
TEST(Vector2, Normalize) {
bool r = false;
Vector2 v1 = Vector2(0, 2);
Vector2 v = Vector2::zero;
v = v1.normalized();
EXPECT_TRUE(v == Vector2(0, 1)) << "v.normalized 0 2";
v = Vector2::Normalize(v1);
EXPECT_TRUE(v == Vector2(0, 1)) << "Vector3::Normalize 0 2";
v1 = Vector2(0, -2);
v = v1.normalized();
EXPECT_TRUE(v == Vector2(0, -1)) << "v.normalized 0 -2";
v1 = Vector2(0, 0);
v = v1.normalized();
EXPECT_TRUE(v == Vector2(0, 0)) << "v.normalized 0 0";
if (std::numeric_limits<float>::is_iec559) {
v1 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
v = v1.normalized();
r = isnan(v.x) && isnan(v.y);
EXPECT_TRUE(r) << "v.normalized INFINITY INFINITY";
v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
v = v1.normalized();
r = isnan(v.x) && isnan(v.y);
EXPECT_TRUE(r) << "v.normalized -INFINITY -INFINITY";
}
}
TEST(Vector2, Negate) {
Vector2 v1 = Vector2(4, 5);
Vector2 v = Vector2::zero;
v = -v1;
EXPECT_TRUE(v == Vector2(-4, -5)) << "- 4 5";
v1 = Vector2(-4, -5);
v = -v1;
EXPECT_TRUE(v == Vector2(4, 5)) << "- -4 -5";
v1 = Vector2(0, 0);
v = -v1;
EXPECT_TRUE(v == Vector2(0, 0)) << "- 0 0";
if (std::numeric_limits<float>::is_iec559) {
v1 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
v = -v1;
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
<< "- INFINITY INFINITY";
v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
v = -v1;
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
<< "- -INFINITY -INFINITY";
}
}
TEST(Vector2, Subtract) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
Vector2 v = Vector2::zero;
v = v1 - v2;
EXPECT_TRUE(v == Vector2(3, 3)) << "4 5 - 1 2";
v2 = Vector2(-1, -2);
v = v1 - v2;
EXPECT_TRUE(v == Vector2(5, 7)) << "4 5 - -1 -2";
v2 = Vector2(4, 5);
v = v1 - v2;
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 - 4 5";
v = v1;
v -= v2;
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 - 4 5";
v2 = Vector2(0, 0);
v = v1 - v2;
EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 - 0 0";
v -= v2;
EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 - 0 0";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
v = v1 - v2;
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
<< "4 5 - INFINITY INFINITY";
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
v = v1 - v2;
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 - -INFINITY -INFINITY";
}
}
TEST(Vector2, Addition) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
Vector2 v = Vector2::zero;
v = v1 + v2;
EXPECT_TRUE(v == Vector2(5, 7)) << "4 5 + 1 2";
v2 = Vector2(-1, -2);
v = v1 + v2;
EXPECT_TRUE(v == Vector2(3, 3)) << "4 5 + -1 -2";
v = v1;
v += v2;
EXPECT_TRUE(v == Vector2(3, 3)) << "4 5 + -1 -2";
v2 = Vector2(0, 0);
v = v1 + v2;
EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 + 0 0";
v += v2;
EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 + 0 0";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
v = v1 + v2;
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 + INFINITY INFINITY";
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
v = v1 + v2;
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
<< "4 5 + -INFINITY -INFINITY";
}
}
TEST(Vector2, Scale) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
Vector2 v = Vector2::zero;
v = Vector2::Scale(v1, v2);
EXPECT_TRUE(v == Vector2(4, 10)) << "Scale 4 5 , 1 2";
v2 = Vector2(-1, -2);
v = Vector2::Scale(v1, v2);
EXPECT_TRUE(v == Vector2(-4, -10)) << "Scale 4 5 , -1 -2";
v2 = Vector2(0, 0);
v = Vector2::Scale(v1, v2);
EXPECT_TRUE(v == Vector2(0, 0)) << "Scale 4 5 , 0 0";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
v = Vector2::Scale(v1, v2);
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 + INFINITY INFINITY";
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
v = Vector2::Scale(v1, v2);
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
<< "4 5 + -INFINITY -INFINITY";
}
}
TEST(Vector2, Multiply) {
Vector2 v1 = Vector2(4, 5);
float f = 3;
Vector2 v = Vector2::zero;
v = v1 * f;
EXPECT_TRUE(v == Vector2(12, 15)) << "4 5 * 3";
f = -3;
v = v1 * f;
EXPECT_TRUE(v == Vector2(-12, -15)) << "4 5 * -3";
f = 0;
v = v1 * f;
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 * 0";
if (std::numeric_limits<float>::is_iec559) {
f = FLOAT_INFINITY;
v = v1 * f;
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 * INFINITY";
f = -FLOAT_INFINITY;
v = v1 * f;
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
<< "4 5 * -INFINITY";
}
}
TEST(Vector2, Divide) {
Vector2 v1 = Vector2(4, 5);
float f = 2;
Vector2 v = Vector2::zero;
v = v1 / f;
EXPECT_TRUE(v == Vector2(2, 2.5F)) << "4 5 / 3";
f = -2;
v = v1 / f;
EXPECT_TRUE(v == Vector2(-2, -2.5F)) << "4 5 / -3";
if (std::numeric_limits<float>::is_iec559) {
f = 0;
v = v1 / f;
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY)) << "4 5 / 0";
f = FLOAT_INFINITY;
v = v1 / f;
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 / INFINITY";
f = -FLOAT_INFINITY;
v = v1 / f;
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 / -INFINITY";
}
}
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);
float f = 0;
f = Vector2::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, 4.24264F) << "Distance(4 5, 1 2)";
v2 = Vector2(-1, -2);
f = Vector2::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, 8.602325F) << "Distance(4 5, -1 -2)";
v2 = Vector2(0, 0);
f = Vector2::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, 6.403124F) << "Distance(4 5, 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector2::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Distance(4 5, INFINITY INFINITY)";
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector2::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Distance(4 5, -INFINITY -INFINITY)";
}
}
TEST(Vector2, Angle) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
float f = 0;
bool r = false;
f = Vector2::Angle(v1, v2);
EXPECT_FLOAT_EQ(f, 12.09476F) << "Angle(4 5, 1 2)";
v2 = Vector2(-1, -2);
f = Vector2::Angle(v1, v2);
EXPECT_FLOAT_EQ(f, 167.9052F) << "Angle(4 5, -1 -2)";
v2 = Vector2(0, 0);
f = Vector2::Angle(v1, v2);
EXPECT_FLOAT_EQ(f, 0) << "Angle(4 5, 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector2::Angle(v1, v2);
r = isnan(f);
EXPECT_TRUE(r) << "Angle(4 5, INFINITY INFINITY)";
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector2::Angle(v1, v2);
r = isnan(f);
EXPECT_TRUE(r) << "Angle(4 5, -INFINITY -INFINITY)";
}
}
TEST(Vector2, SignedAngle) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
float f = 0;
bool r = false;
f = Vector2::SignedAngle(v1, v2);
EXPECT_FLOAT_EQ(f, -12.09476F) << "SignedAngle(4 5, 1 2)";
v2 = Vector2(-1, -2);
f = Vector2::SignedAngle(v1, v2);
EXPECT_FLOAT_EQ(f, 167.9052F) << "SignedAngle(4 5, -1 -2)";
v2 = Vector2(0, 0);
f = Vector2::SignedAngle(v1, v2);
EXPECT_FLOAT_EQ(f, 0) << "SignedAngle(4 5, 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector2::SignedAngle(v1, v2);
r = isnan(f);
EXPECT_TRUE(r) << "SignedAngle(4 5, INFINITY INFINITY)";
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector2::SignedAngle(v1, v2);
r = isnan(f);
EXPECT_TRUE(r) << "SignedAngle(4 5, -INFINITY -INFINITY)";
}
v1 = Vector2(0, 1);
v2 = Vector2(1, 0);
f = Vector2::SignedAngle(v1, v2);
EXPECT_FLOAT_EQ(f, 90.0F) << "SignedAngle(0 1, 1 0)";
v1 = Vector2(0, 1);
v2 = Vector2(0, -1);
f = Vector2::SignedAngle(v1, v2);
EXPECT_FLOAT_EQ(f, 180.0F) << "SignedAngle(0 1, 1 0)";
}
TEST(Vector2, Rotate) {
Vector2 v1 = Vector2(1, 2);
Vector2 r = Vector2(0, 0);
r = Vector2::Rotate(v1, AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(Vector2::Distance(r, v1), 0);
r = Vector2::Rotate(v1, AngleSingle::Degrees(180));
EXPECT_NEAR(Vector2::Distance(r, Vector2(-1, -2)), 0, 1.0e-06);
r = Vector2::Rotate(v1, AngleSingle::Degrees(-90));
EXPECT_NEAR(Vector2::Distance(r, Vector2(2, -1)), 0, 1.0e-06);
r = Vector2::Rotate(v1, AngleSingle::Degrees(270));
EXPECT_NEAR(Vector2::Distance(r, Vector2(2, -1)), 0, 1.0e-06);
}
TEST(Vector2, Lerp) {
Vector2 v1 = Vector2(4, 5);
Vector2 v2 = Vector2(1, 2);
Vector2 r = Vector2(0, 0);
r = Vector2::Lerp(v1, v2, 0);
EXPECT_FLOAT_EQ(Vector2::Distance(r, v1), 0);
r = Vector2::Lerp(v1, v2, 1);
EXPECT_FLOAT_EQ(Vector2::Distance(r, v2), 0);
r = Vector2::Lerp(v1, v2, 0.5f);
EXPECT_FLOAT_EQ(Vector2::Distance(r, Vector2(2.5f, 3.5f)), 0);
r = Vector2::Lerp(v1, v2, -1);
EXPECT_FLOAT_EQ(Vector2::Distance(r, Vector2(7.0f, 8.0f)), 0);
r = Vector2::Lerp(v1, v2, 2);
EXPECT_FLOAT_EQ(Vector2::Distance(r, Vector2(-2.0, -1.0f)), 0);
}
#endif

583
ControlCore/LinearAlgebra/test/Vector3_test.cc Normal file
View File

@ -0,0 +1,583 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Vector3.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Vector3, FromSpherical) {
Vector3 v = Vector3(0, 0, 1);
SphericalOf<float> s = SphericalOf<float>::FromVector3(v);
Vector3 r = Vector3(s);
EXPECT_FLOAT_EQ(r.Right(), 0.0F) << "toVector3.x 0 0 1";
EXPECT_NEAR(r.Up(), 0.0F, 1.0e-06) << "toVector3.y 0 0 1";
EXPECT_FLOAT_EQ(r.Forward(), 1.0F) << "toVector3.z 0 0 1";
v = Vector3(0, 1, 0);
s = SphericalOf<float>::FromVector3(v);
r = Vector3(s);
EXPECT_FLOAT_EQ(r.Right(), 0.0F) << "toVector3.x 0 1 0";
EXPECT_FLOAT_EQ(r.Up(), 1.0F) << "toVector3.y 0 1 0";
EXPECT_NEAR(r.Forward(), 0.0F, 1.0e-06) << "toVector3.z 0 1 0";
v = Vector3(1, 0, 0);
s = SphericalOf<float>::FromVector3(v);
r = Vector3(s);
EXPECT_FLOAT_EQ(r.Right(), 1.0F) << "toVector3.x 1 0 0";
EXPECT_NEAR(r.Up(), 0.0F, 1.0e-06) << "toVector3.y 1 0 0";
EXPECT_NEAR(r.Forward(), 0.0F, 1.0e-06) << "toVector3.z 1 0 0";
}
TEST(Vector3, Magnitude) {
Vector3 v = Vector3(1, 2, 3);
float m = 0;
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 3.741657F) << "v.magnitude 1 2 3";
m = Vector3::Magnitude(v);
EXPECT_FLOAT_EQ(m, 3.741657F) << "Vector3::Magnitude 1 2 3";
v = Vector3(-1, -2, -3);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 3.741657F) << "v.magnitude -1 -2 -3";
v = Vector3(0, 0, 0);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, 0) << "v.magnitude 0 0 0 ";
if (std::numeric_limits<float>::is_iec559) {
v = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
<< "v.magnitude INFINITY INFINITY INFINITY ";
v = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
m = v.magnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
<< "v.magnitude -INFINITY -INFINITY -INFINITY ";
}
}
TEST(Vector3, SqrMagnitude) {
Vector3 v = Vector3(1, 2, 3);
float m = 0;
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, 14) << "v.sqrMagnitude 1 2 3";
m = Vector3::SqrMagnitude(v);
EXPECT_FLOAT_EQ(m, 14) << "Vector3::SqrMagnitude 1 2 3";
v = Vector3(-1, -2, -3);
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, 14) << "v.sqrMagnitude -1 -2 -3";
v = Vector3(0, 0, 0);
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, 0) << "v.sqrMagnitude 0 0 0 ";
if (std::numeric_limits<float>::is_iec559) {
v = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
<< "v.sqrMagnitude INFINITY INFINITY INFINITY ";
v = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
m = v.sqrMagnitude();
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
<< "v.sqrMagnitude -INFINITY -INFINITY -INFINITY ";
}
}
TEST(Vector3, Normalize) {
bool r = false;
Vector3 v1 = Vector3(0, 2, 0);
Vector3 v = Vector3::zero;
v = v1.normalized();
EXPECT_TRUE(v == Vector3(0, 1, 0)) << "v.normalized 0 2 0";
v = Vector3::Normalize(v1);
EXPECT_TRUE(v == Vector3(0, 1, 0)) << "Vector3::Normalize 0 2 0";
v1 = Vector3(0, -2, 0);
v = v1.normalized();
EXPECT_TRUE(v == Vector3(0, -1, 0)) << "v.normalized 0 -2 0";
v1 = Vector3(0, 0, 0);
v = v1.normalized();
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "v.normalized 0 0 0";
if (std::numeric_limits<float>::is_iec559) {
v1 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
v = v1.normalized();
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
EXPECT_TRUE(r) << "v.normalized INFINITY INFINITY INFINITY";
v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
v = v1.normalized();
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
EXPECT_TRUE(r) << "v.normalized -INFINITY -INFINITY -INFINITY";
}
}
TEST(Vector3, Negate) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v = Vector3::zero;
v = -v1;
EXPECT_TRUE(v == Vector3(-4, -5, -6)) << "- 4 5 6";
v1 = Vector3(-4, -5, -6);
v = -v1;
EXPECT_TRUE(v == Vector3(4, 5, 6)) << "- -4 -5 -6";
v1 = Vector3(0, 0, 0);
v = -v1;
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "- 0 0 0";
if (std::numeric_limits<float>::is_iec559) {
v1 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
v = -v1;
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
<< "- INFINITY INFINITY INFINITY";
v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
v = -v1;
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
<< "- -INFINITY -INFINITY -INFINITY";
}
}
TEST(Vector3, Subtract) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 v = Vector3::zero;
v = v1 - v2;
EXPECT_TRUE(v == Vector3(3, 3, 3)) << "4 5 6 - 1 2 3";
v2 = Vector3(-1, -2, -3);
v = v1 - v2;
EXPECT_TRUE(v == Vector3(5, 7, 9)) << "4 5 6 - -1 -2 -3";
v2 = Vector3(4, 5, 6);
v = v1 - v2;
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 - 4 5 6";
v2 = Vector3(0, 0, 0);
v = v1 - v2;
EXPECT_TRUE(v == Vector3(4, 5, 6)) << "4 5 6 - 0 0 0";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
v = v1 - v2;
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
<< "4 5 6 - INFINITY INFINITY INFINITY";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
v = v1 - v2;
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 6 - -INFINITY -INFINITY -INFINITY";
}
}
TEST(Vector3, Addition) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 v = Vector3::zero;
v = v1 + v2;
EXPECT_TRUE(v == Vector3(5, 7, 9)) << "4 5 6 + 1 2 3";
v2 = Vector3(-1, -2, -3);
v = v1 + v2;
EXPECT_TRUE(v == Vector3(3, 3, 3)) << "4 5 6 + -1 -2 -3";
v2 = Vector3(0, 0, 0);
v = v1 + v2;
EXPECT_TRUE(v == Vector3(4, 5, 6)) << "4 5 6 + 0 0 0";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
v = v1 + v2;
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 6 + INFINITY INFINITY INFINITY";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
v = v1 + v2;
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
<< "4 5 6 + -INFINITY -INFINITY -INFINITY";
}
}
TEST(Vector3, Scale) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 v = Vector3::zero;
v = Vector3::Scale(v1, v2);
EXPECT_TRUE(v == Vector3(4, 10, 18)) << "Scale 4 5 6 , 1 2 3";
v2 = Vector3(-1, -2, -3);
v = Vector3::Scale(v1, v2);
EXPECT_TRUE(v == Vector3(-4, -10, -18)) << "Scale 4 5 6 , -1 -2 -3";
v2 = Vector3(0, 0, 0);
v = Vector3::Scale(v1, v2);
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "Scale 4 5 6 , 0 0 0";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
v = Vector3::Scale(v1, v2);
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 6 + INFINITY INFINITY INFINITY";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
v = Vector3::Scale(v1, v2);
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
<< "4 5 6 + -INFINITY -INFINITY -INFINITY";
}
}
TEST(Vector3, Multiply) {
Vector3 v1 = Vector3(4, 5, 6);
float f = 3;
Vector3 v = Vector3::zero;
v = v1 * f;
EXPECT_TRUE(v == Vector3(12, 15, 18)) << "4 5 6 * 3";
f = -3;
v = v1 * f;
EXPECT_TRUE(v == Vector3(-12, -15, -18)) << "4 5 6 * -3";
f = 0;
v = v1 * f;
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 * 0";
if (std::numeric_limits<float>::is_iec559) {
f = FLOAT_INFINITY;
v = v1 * f;
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 6 * INFINITY";
f = -FLOAT_INFINITY;
v = v1 * f;
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
<< "4 5 6 * -INFINITY";
}
}
TEST(Vector3, Divide) {
Vector3 v1 = Vector3(4, 5, 6);
float f = 2;
Vector3 v = Vector3::zero;
v = v1 / f;
EXPECT_TRUE(v == Vector3(2, 2.5F, 3)) << "4 5 6 / 3";
f = -2;
v = v1 / f;
EXPECT_TRUE(v == Vector3(-2, -2.5F, -3)) << "4 5 6 / -3";
if (std::numeric_limits<float>::is_iec559) {
f = 0;
v = v1 / f;
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
<< "4 5 6 / 0";
f = FLOAT_INFINITY;
v = v1 / f;
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 / INFINITY";
f = -FLOAT_INFINITY;
v = v1 / f;
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 / -INFINITY";
}
}
TEST(Vector3, Dot) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
float f = 0;
f = Vector3::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, 32) << "Dot(4 5 6, 1 2 3)";
v2 = Vector3(-1, -2, -3);
f = Vector3::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, -32) << "Dot(4 5 6, -1 -2 -3)";
v2 = Vector3(0, 0, 0);
f = Vector3::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, 0) << "Dot(4 5 6, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector3::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
<< "Dot(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector3::Dot(v1, v2);
EXPECT_FLOAT_EQ(f, -FLOAT_INFINITY)
<< "Dot(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}
TEST(Vector3, Equality) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
bool r = false;
r = v1 == v2;
EXPECT_FALSE(r) << "4 5 6 == 1 2 3";
v2 = Vector3(4, 5, 6);
r = v1 == v2;
EXPECT_TRUE(r) << "4 5 6 == 1 2 3";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
r = v1 == v2;
EXPECT_FALSE(r) << "4 5 6 == INFINITY INFINITY INFINITY";
v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
r = v1 == v2;
EXPECT_FALSE(r)
<< "-INFINITY -INFINITY -INFINITY == INFINITY INFINITY INFINITY";
}
}
TEST(Vector3, Distance) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
float f = 0;
f = Vector3::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, 5.19615221F) << "Distance(4 5 6, 1 2 3)";
v2 = Vector3(-1, -2, -3);
f = Vector3::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, 12.4498997F) << "Distance(4 5 6, -1 -2 -3)";
v2 = Vector3(0, 0, 0);
f = Vector3::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, 8.77496433F) << "Distance(4 5 6, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector3::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
<< "Distance(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector3::Distance(v1, v2);
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
<< "Distance(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}
TEST(Vector3, Cross) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 v = Vector3::zero;
bool r = false;
v = Vector3::Cross(v1, v2);
r = v == Vector3(3, -6, 3);
EXPECT_TRUE(r) << "Cross(4 5 6, 1 2 3)";
v2 = Vector3(-1, -2, -3);
v = Vector3::Cross(v1, v2);
r = v == Vector3(-3, 6, -3);
EXPECT_TRUE(r) << "Cross(4 5 6, -1 -2 -3)";
v2 = Vector3(0, 0, 0);
v = Vector3::Cross(v1, v2);
r = v == Vector3(0, 0, 0);
EXPECT_TRUE(r) << "Cross(4 5 6, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
v = Vector3::Cross(v1, v2);
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
EXPECT_TRUE(r) << "Cross(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
v = Vector3::Cross(v1, v2);
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
EXPECT_TRUE(r) << "Cross(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}
TEST(Vector3, Project) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 v = Vector3::zero;
bool r = false;
v = Vector3::Project(v1, v2);
r = v == Vector3(2.28571439F, 4.57142878F, 6.85714293F);
EXPECT_TRUE(r) << "Project(4 5 6, 1 2 3)";
v2 = Vector3(-1, -2, -3);
v = Vector3::Project(v1, v2);
r = v == Vector3(2.28571439F, 4.57142878F, 6.85714293F);
EXPECT_TRUE(r) << "Project(4 5 6, -1 -2 -3)";
v2 = Vector3(0, 0, 0);
v = Vector3::Project(v1, v2);
r = v == Vector3(0, 0, 0);
EXPECT_TRUE(r) << "Project(4 5 6, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
v = Vector3::Project(v1, v2);
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
EXPECT_TRUE(r) << "Project(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
v = Vector3::Project(v1, v2);
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
EXPECT_TRUE(r) << "Project(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}
TEST(Vector3, ProjectOnPlane) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 v = Vector3::zero;
bool r = false;
v = Vector3::ProjectOnPlane(v1, v2);
r = v == Vector3(1.71428561F, 0.428571224F, -0.857142925F);
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, 1 2 3)";
v2 = Vector3(-1, -2, -3);
v = Vector3::ProjectOnPlane(v1, v2);
r = v == Vector3(1.71428561F, 0.428571224F, -0.857142925F);
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, -1 -2 -3)";
v2 = Vector3(0, 0, 0);
v = Vector3::ProjectOnPlane(v1, v2);
r = v == Vector3(4, 5, 6);
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
v = Vector3::ProjectOnPlane(v1, v2);
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
v = Vector3::ProjectOnPlane(v1, v2);
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}
TEST(Vector3, Angle) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
AngleOf<float> f = AngleOf<float>::Degrees(0);
bool r = false;
f = Vector3::Angle(v1, v2);
EXPECT_FLOAT_EQ(f.InDegrees(), 12.9331388F) << "Angle(4 5 6, 1 2 3)";
v2 = Vector3(-1, -2, -3);
f = Vector3::Angle(v1, v2);
EXPECT_FLOAT_EQ(f.InDegrees(), 167.066864F) << "Angle(4 5 6, -1 -2 -3)";
v2 = Vector3(0, 0, 0);
f = Vector3::Angle(v1, v2);
EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "Angle(4 5 6, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector3::Angle(v1, v2);
r = isnan(f.InDegrees());
EXPECT_TRUE(r) << "Angle(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector3::Angle(v1, v2);
r = isnan(f.InDegrees());
EXPECT_TRUE(r) << "Angle(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}
TEST(Vector3, SignedAngle) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 v3 = Vector3(7, 8, -9);
AngleOf<float> f = AngleOf<float>::Degrees(0);
bool r = false;
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f.InDegrees(), -12.9331388F)
<< "SignedAngle(4 5 6, 1 2 3, 7 8 -9)";
v2 = Vector3(-1, -2, -3);
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f.InDegrees(), 167.066864F)
<< "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)";
v2 = Vector3(0, 0, 0);
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )";
v2 = Vector3(1, 2, 3);
v3 = Vector3(-7, -8, 9);
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f.InDegrees(), 12.9331388F)
<< "SignedAngle(4 5 6, 1 2 3, -7 -8 9)";
v3 = Vector3(0, 0, 0);
f = Vector3::SignedAngle(v1, v2, v3);
EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)";
if (std::numeric_limits<float>::is_iec559) {
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
f = Vector3::SignedAngle(v1, v2, v3);
r = isnan(f.InDegrees());
EXPECT_TRUE(r) << "SignedAngle(4 5 6, INFINITY INFINITY INFINITY)";
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
f = Vector3::SignedAngle(v1, v2, v3);
r = isnan(f.InDegrees());
EXPECT_TRUE(r) << "SignedAngle(4 5 6, -INFINITY -INFINITY -INFINITY)";
}
}
TEST(Vector3, Lerp) {
Vector3 v1 = Vector3(4, 5, 6);
Vector3 v2 = Vector3(1, 2, 3);
Vector3 r = Vector3(0, 0, 0);
r = Vector3::Lerp(v1, v2, 0);
EXPECT_FLOAT_EQ(Vector3::Distance(r, v1), 0);
r = Vector3::Lerp(v1, v2, 1);
EXPECT_FLOAT_EQ(Vector3::Distance(r, v2), 0);
r = Vector3::Lerp(v1, v2, 0.5f);
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(2.5f, 3.5f, 4.5f)), 0);
r = Vector3::Lerp(v1, v2, -1);
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(7.0f, 8.0f, 9.0f)), 0);
r = Vector3::Lerp(v1, v2, 2);
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(-2.0, -1.0f, 0.0f)), 0);
}
#endif

88
ControlCore/LowLevelMessages.cpp Normal file
View File

@ -0,0 +1,88 @@
#include "LowLevelMessages.h"
#include "float16.h"
void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
const float angle) {
Angle8 packedAngle2 = Angle8::Degrees(angle);
buffer[(*ix)++] = packedAngle2.GetBinary();
}
Angle8 LowLevelMessages::ReceiveAngle8(const unsigned char *buffer,
unsigned char *startIndex) {
unsigned char binary = buffer[(*startIndex)++];
Angle8 angle = Angle8::Binary(binary);
return angle;
}
void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
float value) {
float16 value16 = float16(value);
short binary = value16.getBinary();
buffer[(*ix)++] = (binary >> 8) & 0xFF;
buffer[(*ix)++] = binary & 0xFF;
}
float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
unsigned char *startIndex) {
unsigned char ix = *startIndex;
unsigned short value = buffer[ix++] << 8 | buffer[ix++];
float16 f = float16();
f.setBinary(value);
*startIndex = ix;
return (float)f.toFloat();
}
void LowLevelMessages::SendSpherical16(unsigned char *buffer, unsigned char *ix,
Spherical16 s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
Spherical16 LowLevelMessages::ReceiveSpherical16(const unsigned char *buffer,
unsigned char *startIndex) {
float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
Spherical16 s = Spherical16(distance, horizontal, vertical);
return s;
}
void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
unsigned char *ix,
SwingTwist16 rotation) {
Quaternion q = rotation.ToQuaternion();
unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128);
unsigned char qz = (char)(q.z * 127 + 128);
unsigned char qw = (char)(q.w * 255);
if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
// Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
buffer[(*ix)++] = qx;
buffer[(*ix)++] = qy;
buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw;
}
SwingTwist16 LowLevelMessages::ReceiveQuat32(const unsigned char *buffer,
unsigned char *ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qw = buffer[(*ix)++] / 255.0F;
Quaternion q = Quaternion(qx, qy, qz, qw);
SwingTwist16 s = SwingTwist16::FromQuaternion(q);
return s;
}

32
ControlCore/LowLevelMessages.h Normal file
View File

@ -0,0 +1,32 @@
#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"
namespace Passer {
namespace Control {
class LowLevelMessages {
public:
static void SendAngle8(unsigned char *buffer, unsigned char *ix,
const float angle);
static Angle8 ReceiveAngle8(const unsigned char *buffer,
unsigned char *startIndex);
static void SendFloat16(unsigned char *buffer, unsigned char *ix,
float value);
static float ReceiveFloat16(const unsigned char *buffer,
unsigned char *startIndex);
static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
Spherical16 s);
static Spherical16 ReceiveSpherical16(const unsigned char *buffer,
unsigned char *startIndex);
static void SendQuat32(unsigned char *buffer, unsigned char *ix,
SwingTwist16 q);
static SwingTwist16 ReceiveQuat32(const unsigned char *buffer,
unsigned char *ix);
};
} // namespace Control
} // namespace Passer
using namespace Passer::Control;

300
ControlCore/Messages.cpp Normal file
View File

@ -0,0 +1,300 @@
#include "Messages.h"
#include "LowLevelMessages.h"
#include "Participant.h"
#include "string.h"
#pragma region IMessage
IMessage::IMessage() {}
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }
// void IMessage::Deserialize(unsigned char *buffer) {}
// bool IMessage::SendMsg(Participant *client, IMessage msg) {
// // return SendMsg(client, client.buffer, );nameLength
// return client->SendBuffer(msg.Serialize(client->buffer));
// }
unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
return nullptr;
}
bool IMessage::Publish(Participant *participant) {
return participant->PublishBuffer(Serialize(participant->buffer));
}
bool IMessage::SendTo(Participant *participant) {
return participant->SendBuffer(Serialize(participant->buffer));
}
// IMessage
#pragma endregion
#pragma region Client
ClientMsg::ClientMsg(unsigned char networkId) { this->networkId = networkId; }
unsigned char ClientMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
return ix;
}
// bool ClientMsg::Send(Participant *participant, unsigned char networkId) {
// ClientMsg msg = ClientMsg()
// }
// Client Msg
#pragma endregion
#pragma region Network Id
NetworkIdMsg::NetworkIdMsg(unsigned char *buffer) {
this->networkId = buffer[1];
}
// void NetworkIdMsg::Deserialize(unsigned char *buffer) {
// this->networkId = buffer[1];
// }
NetworkIdMsg NetworkIdMsg::Receive(unsigned char *buffer,
unsigned char bufferSize) {
NetworkIdMsg msg = NetworkIdMsg(buffer);
return msg;
}
// Network Id
#pragma endregion
#pragma region Investigate
InvestigateMsg::InvestigateMsg(unsigned char *buffer) {
unsigned ix = 1; // first byte is msgId
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
}
InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
this->networkId = networkId;
this->thingId = thingId;
}
unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
return ix;
}
// bool InvestigateMsg::Send(Participant *participant, unsigned char networkId,
// unsigned char thingId) {
// InvestigateMsg msg = InvestigateMsg(networkId, thingId);
// return msg.Send(participant);
// }
// Investigate
#pragma endregion
#pragma region Thing
ThingMsg::ThingMsg(const unsigned char *buffer) {
unsigned char ix = 1; // first byte is msg id
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->thingType = buffer[ix++];
this->parentId = buffer[ix++];
}
ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
unsigned char thingType, unsigned char parentId) {
this->networkId = networkId;
this->thingId = thingId;
this->thingType = thingType;
this->parentId = parentId;
}
unsigned char ThingMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->thingType;
buffer[ix++] = this->parentId;
return ix;
}
// bool ThingMsg::Send(Participant *participant, unsigned char networkId,
// unsigned char thingId, unsigned char thingType,
// unsigned char parentId) {
// ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
// return msg.Send(participant);
// }
// Thing
#pragma endregion
#pragma region Name
NameMsg::NameMsg(unsigned char networkId, unsigned char thingId,
const char *name, unsigned char nameLength) {
this->networkId = networkId;
this->thingId = thingId;
this->name = name;
this->nameLength = nameLength;
}
unsigned char NameMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->nameLength;
for (int nameIx = 0; nameIx < this->nameLength; nameIx++)
buffer[ix++] = this->name[nameIx];
return ix;
}
// bool NameMsg::Send(Participant *participant, Thing *thing,
// unsigned char nameLength) {
// if (thing->name == nullptr)
// return true; // nothing sent, but still a success!
// if (strlen(thing->name) == 0)
// return true;
// NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name,
// nameLength); return msg.Send(participant);
// }
// Name
#pragma endregion
#pragma region ModelUrl
ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
unsigned char urlLength, const char *url,
float scale) {
this->networkId = networkId;
this->thingId = thingId;
this->urlLength = urlLength;
this->url = url;
this->scale = scale;
}
unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
LowLevelMessages::SendFloat16(buffer, &ix, this->scale);
buffer[ix++] = this->urlLength;
for (int urlIx = 0; urlIx < this->urlLength; urlIx++)
buffer[ix++] = url[urlIx];
return ix;
}
// Model Url
#pragma endregion
#pragma region PoseMsg
PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation, Spherical16 linearVelocity,
Spherical16 angularVelocity) {
this->networkId = networkId;
this->thingId = thingId;
this->poseType = poseType;
this->position = position;
this->orientation = orientation;
this->linearVelocity = linearVelocity;
this->angularVelocity = angularVelocity;
}
PoseMsg::PoseMsg(const unsigned char *buffer) {
unsigned char ix = 1; // First byte is msg id
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->poseType = buffer[ix++];
this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
}
unsigned char PoseMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg::id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->poseType;
if ((this->poseType & Pose_Position) != 0)
LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
if ((this->poseType & Pose_Orientation) != 0)
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
if ((this->poseType & Pose_LinearVelocity) != 0)
LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
if ((this->poseType & Pose_AngularVelocity) != 0)
LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
return ix;
}
// Pose
#pragma endregion
#pragma region CustomMsg
CustomMsg::CustomMsg(unsigned char *buffer) {
unsigned char ix = 1;
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->data =
buffer + ix; // This is only valid because the code ensures the the msg
// lifetime is shorter than the buffer lifetime...
}
CustomMsg::CustomMsg(unsigned char networkId, Thing *thing) {
this->networkId = networkId;
this->thingId = thing->id;
this->thing = thing;
}
unsigned char CustomMsg::Serialize(unsigned char *buffer) {
unsigned char ix = this->length;
this->thing->SendBytes(buffer, &ix);
if (ix <= this->length) // in this case, no data is actually sent
return 0;
buffer[0] = this->id;
buffer[1] = this->networkId;
buffer[2] = this->thingId;
return ix;
}
CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
CustomMsg msg = CustomMsg(buffer);
return msg;
}
// CustomMsg
#pragma endregion
#pragma region DestroyMsg
DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
this->networkId = networkId;
this->thingId = thing->id;
}
unsigned char DestroyMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
return ix;
}
// DestroyMsg
#pragma endregion

168
ControlCore/Messages.h Normal file
View File

@ -0,0 +1,168 @@
#pragma once
#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"
#include "Thing.h"
#include "float16.h"
namespace Passer {
namespace Control {
class Participant;
class IMessage {
public:
IMessage();
virtual unsigned char Serialize(unsigned char *buffer);
static unsigned char *ReceiveMsg(unsigned char packetSize);
bool Publish(Participant *participant);
bool SendTo(Participant *participant);
};
class ClientMsg : public IMessage {
public:
static const unsigned char id = 0xA0;
unsigned char networkId;
ClientMsg(unsigned char networkId);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class NetworkIdMsg : public IMessage {
public:
static const unsigned char id = 0xA1;
static const unsigned char length = 2;
unsigned char networkId;
NetworkIdMsg(unsigned char *buffer);
static NetworkIdMsg Receive(unsigned char *buffer, unsigned char bufferSize);
};
class InvestigateMsg : public IMessage {
public:
static const unsigned char id = 0x81;
static const unsigned char length = 3;
unsigned char networkId;
unsigned char thingId;
InvestigateMsg(unsigned char *buffer);
InvestigateMsg(unsigned char networkId, unsigned char thingId);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class ThingMsg : public IMessage {
public:
static const unsigned char id = 0x80;
static const unsigned char length = 5;
unsigned char networkId;
unsigned char thingId;
unsigned char thingType;
unsigned char parentId;
ThingMsg(const unsigned char *buffer);
ThingMsg(unsigned char networkId, unsigned char thingId,
unsigned char thingType, unsigned char parentId);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class NameMsg : public IMessage {
public:
static const unsigned char id = 0x91;
static const unsigned char length = 4;
unsigned char networkId;
unsigned char thingId;
unsigned char nameLength;
const char *name;
NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
unsigned char nameLength);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class ModelUrlMsg : public IMessage {
public:
static const unsigned char id = 0x90;
unsigned char networkId;
unsigned char thingId;
float scale;
unsigned char urlLength;
const char *url;
ModelUrlMsg(unsigned char networkId, unsigned char thingId,
unsigned char urlLegth, const char *url, float scale = 1);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class PoseMsg : public IMessage {
public:
static const unsigned char id = 0x10;
unsigned char length = 4 + 4 + 4;
unsigned char networkId;
unsigned char thingId;
unsigned char poseType;
static const unsigned char Pose_Position = 0x01;
static const unsigned char Pose_Orientation = 0x02;
static const unsigned char Pose_LinearVelocity = 0x04; // For future use
static const unsigned char Pose_AngularVelocity = 0x08; // For future use
Spherical16 position;
SwingTwist16 orientation;
Spherical16 linearVelocity;
Spherical16 angularVelocity;
PoseMsg(unsigned char networkId, unsigned char thingId,
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(),
Spherical16 angularVelocity = Spherical16());
PoseMsg(const unsigned char *buffer);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class CustomMsg : public IMessage {
public:
static const unsigned char id = 0xB1;
static const unsigned length = 3;
unsigned char networkId;
unsigned char thingId;
Thing *thing;
unsigned char dataSize;
unsigned char *data;
CustomMsg(unsigned char *buffer);
CustomMsg(unsigned char networkId, Thing *thing);
virtual unsigned char Serialize(unsigned char *buffer) override;
static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
};
class DestroyMsg : public IMessage {
public:
static const unsigned char id = 0x20;
static const unsigned length = 3;
unsigned char networkId;
unsigned char thingId;
DestroyMsg(unsigned char networkId, Thing *thing);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
} // namespace Control
} // namespace Passer
using namespace Passer::Control;

40
ControlCore/Participant.cpp Normal file
View File

@ -0,0 +1,40 @@
#include "Participant.h"
bool Participant::SendBuffer(unsigned char bufferSize) { return false; }
bool Participant::PublishBuffer(unsigned char bufferSize) { return false; }
void Participant::ReceiveData(unsigned char bufferSize) {
unsigned char msgId = this->buffer[0];
switch (msgId) {
case NetworkIdMsg::id: {
// NetworkIdMsg msg = NetworkIdMsg::Receive(this->buffer, bufferSize);
NetworkIdMsg msg = NetworkIdMsg(this->buffer);
ProcessNetworkIdMsg(msg);
} break;
case InvestigateMsg::id: {
InvestigateMsg msg = InvestigateMsg(this->buffer);
ProcessInvestigateMsg(msg);
} break;
case ThingMsg::id: {
ThingMsg msg = ThingMsg(this->buffer);
ProcessThingMsg(msg);
} break;
case PoseMsg::id: {
PoseMsg msg = PoseMsg(this->buffer);
ProcessPoseMsg(msg);
} break;
case CustomMsg::id: {
CustomMsg msg = CustomMsg(this->buffer);
ProcessCustomMsg(msg);
} break;
};
}
void Participant::ProcessNetworkIdMsg(NetworkIdMsg msg) {}
void Participant::ProcessInvestigateMsg(InvestigateMsg msg) {}
void Participant::ProcessThingMsg(ThingMsg msg) {}
void Participant::ProcessCustomMsg(CustomMsg msg) {}

27
ControlCore/Participant.h Normal file
View File

@ -0,0 +1,27 @@
#pragma once
#include "Messages.h"
namespace Passer {
namespace Control {
class Participant {
public:
unsigned char buffer[1024];
virtual bool SendBuffer(unsigned char bufferSize);
virtual bool PublishBuffer(unsigned char bufferSize);
void ReceiveData(unsigned char bufferSize);
protected:
virtual void ProcessNetworkIdMsg(NetworkIdMsg msg);
virtual void ProcessInvestigateMsg(InvestigateMsg msg);
virtual void ProcessThingMsg(ThingMsg msg);
virtual void ProcessPoseMsg(PoseMsg msg);
virtual void ProcessCustomMsg(CustomMsg msg);
};
} // namespace Control
} // namespace Passer
using namespace Passer::Control;

195
ControlCore/Thing.cpp Normal file
View File

@ -0,0 +1,195 @@
#include "Thing.h"
#include "Participant.h"
#include <iostream>
#include <string.h>
Thing::Thing(unsigned char networkId, unsigned char thingType) {
this->type = thingType;
this->networkId = networkId;
this->Init();
int thingId = Thing::Add(this);
if (thingId < 0) {
std::cout << "ERROR: Thing store is full\n";
this->id = 0; // what to do when we cannot store any more things?
} else
this->id = thingId;
this->linearVelocity = Spherical16::zero;
this->angularVelocity = Spherical16::zero;
}
void Thing::Terminate() { Thing::Remove(this); }
void Thing::Init() {}
Thing *Thing::FindThing(const char *name) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing *child = this->children[childIx];
if (child == nullptr || child->name == nullptr)
continue;
if (strcmp(child->name, name) == 0)
return child;
Thing *foundChild = child->FindThing(name);
if (foundChild != nullptr)
return foundChild;
}
return nullptr;
}
void Thing::SetParent(Thing *parent) {
if (parent == nullptr) {
Thing *parentThing = this->parent;
if (parentThing != nullptr)
parentThing->RemoveChild(this);
this->parent = nullptr;
} else
parent->AddChild(this);
}
void Thing::SetParent(Thing *root, const char *name) {
Thing *thing = root->FindThing(name);
if (thing != nullptr)
this->SetParent(thing);
}
Thing *Thing::GetParent() { return this->parent; }
void Thing::AddChild(Thing *child) {
unsigned char newChildCount = this->childCount + 1;
Thing **newChildren = new Thing *[newChildCount];
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
newChildren[childIx] = this->children[childIx];
if (this->children[childIx] == child) {
// child is already present, stop copying do not update the children
delete[] newChildren;
return;
}
}
newChildren[this->childCount] = child;
child->parent = this;
if (this->children != nullptr)
delete[] this->children;
this->children = newChildren;
this->childCount = newChildCount;
}
Thing *Thing::RemoveChild(Thing *child) {
unsigned char newChildCount = this->childCount - 1;
Thing **newChildren = new Thing *[newChildCount];
unsigned char newChildIx = 0;
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
if (this->children[childIx] != child) {
if (newChildIx == newChildCount) { // We did not find the child
// stop copying and return nothing
delete[] newChildren;
return nullptr;
} else
newChildren[newChildIx++] = this->children[childIx];
}
}
child->parent = nullptr;
delete[] this->children;
this->children = newChildren;
this->childCount = newChildCount;
return child;
}
Thing *Passer::Control::Thing::GetChild(unsigned char id, bool recursive) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing *child = this->children[childIx];
if (child == nullptr)
continue;
if (child->id == id)
return child;
if (recursive) {
Thing *foundChild = child->GetChild(id, recursive);
if (foundChild != nullptr)
return foundChild;
}
}
return nullptr;
}
Thing *Passer::Control::Thing::GetChildByIndex(unsigned char ix) {
return this->children[ix];
}
void Thing::SetModel(const char *url) { this->modelUrl = url; }
void Thing::SetPosition(Spherical16 position) {
this->position = position;
this->positionUpdated = true;
}
Spherical16 Thing::GetPosition() { return this->position; }
void Thing::SetOrientation(SwingTwist16 orientation) {
this->orientation = orientation;
this->orientationUpdated = true;
}
SwingTwist16 Thing::GetOrientation() { return this->orientation; }
Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }
// All things
Thing *Thing::allThings[THING_STORE_SIZE] = {nullptr};
Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
Thing *thing = allThings[ix];
if (thing == nullptr)
continue;
if (thing->networkId == networkId && thing->id == thingId)
return thing;
}
return nullptr;
}
int Thing::Add(Thing *newThing) {
// Exclude 0 because that value is reserved for 'no thing'
for (uint16_t ix = 1; ix < THING_STORE_SIZE; ix++) {
Thing *thing = allThings[ix];
if (thing == nullptr) {
allThings[ix] = newThing;
// std::cout << " Add new thing " << (int)ix << "\n";
return ix;
}
}
return -1;
}
void Thing::Remove(Thing *thing) {
// std::cout << " remove " << (int)thing->id << "\n";
allThings[thing->id] = nullptr;
}
void Thing::UpdateAll(unsigned long currentTimeMs) {
// Not very efficient, but it works for now.
for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
Thing *thing = allThings[ix];
if (thing != nullptr &&
thing->parent == nullptr) { // update all root things
// std::cout << " update " << (int)ix << " thingid " << (int)thing->id
// << "\n";
thing->Update(currentTimeMs);
}
}
}

133
ControlCore/Thing.h Normal file
View File

@ -0,0 +1,133 @@
#pragma once
#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"
#include <iostream>
namespace Passer {
namespace Control {
#define THING_STORE_SIZE 256
// IMPORTANT: values higher than 256 will need to change the Thing::id type
// to 16-bit or higher, breaking the networking protocol!
class Thing {
public:
// Participant *client;
unsigned char networkId = 0;
/// @char The id of the thing
unsigned char id = 0;
Thing *FindThing(const char *name);
// Thing *FindChild(unsigned char id);
/// @brief Sets the parent Thing
/// @param parent The Thing which should become the parnet
/// @remark This is equivalent to calling parent->AddChild(this);
virtual void SetParent(Thing *parent);
void SetParent(Thing *root, const char *name);
/// @brief Gets the parent Thing
/// @return The parent Thing
Thing *GetParent();
/// @brief Add a child Thing to this Thing
/// @param child The Thing which should become a child
/// @remark When the Thing is already a child, it will not be added again
virtual void AddChild(Thing *child);
Thing *RemoveChild(Thing *child);
unsigned char childCount = 0;
Thing *GetChild(unsigned char id, bool recursive = false);
Thing *GetChildByIndex(unsigned char ix);
protected:
Thing *parent = nullptr;
Thing **children = nullptr;
public:
/// @brief The type of Thing
unsigned char type = 0;
const char *name = nullptr;
const char *modelUrl = nullptr;
float modelScale = 1;
// protected Sensor sensor;
/// @brief Basic Thing types
enum class Type {
Undetermined,
// Sensor,
Switch,
DistanceSensor,
DirectionalSensor,
TemperatureSensor,
// Motor,
ControlledMotor,
UncontrolledMotor,
Servo,
// Other
Roboid,
Humanoid,
ExternalSensor,
};
void SetPosition(Spherical16 position);
Spherical16 GetPosition();
void SetOrientation(SwingTwist16 orientation);
SwingTwist16 GetOrientation();
float scale = 1; // assuming uniform scale
bool positionUpdated = false;
bool orientationUpdated = false;
protected:
/// @brief The position in local space
/// @remark When this Thing has a parent, the position is relative to the
/// parent's position and orientation
Spherical16 position;
/// @brief The orientation in local space
/// @remark When this Thing has a parent, the orientation is relative to the
/// parent's orientation
SwingTwist16 orientation;
public:
Spherical16 linearVelocity;
Spherical16 angularVelocity;
virtual Spherical16 GetLinearVelocity();
virtual Spherical16 GetAngularVelocity();
public:
Thing(unsigned char networkId = 0,
unsigned char thingType = (unsigned char)Type::Undetermined);
/// @brief Terminated thins are no longer updated
void Terminate();
/// @brief Sets the location from where the 3D model of this Thing can be
/// loaded from
/// @param url The url of the model
/// @remark Although the roboid implementation is not dependent on the model,
/// the only official supported model format is .obj
void SetModel(const char *url);
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds
virtual void Update(unsigned long currentTimeMs) {};
virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
virtual void ProcessBytes(unsigned char *bytes) {};
protected:
virtual void Init();
//------------ All things
public:
static Thing *Get(unsigned char networkId, unsigned char thingId);
static int Add(Thing *thing);
static void Remove(Thing *thing);
static void UpdateAll(unsigned long currentTimeMs);
private:
static Thing *allThings[];
};
} // namespace Control
} // namespace Passer
using namespace Passer::Control;

243
ControlCore/float16.cpp Normal file
View File

@ -0,0 +1,243 @@
//
// FILE: float16.cpp
// AUTHOR: Rob Tillaart
// VERSION: 0.1.8
// PURPOSE: library for Float16s for Arduino
// URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format
#include "float16.h"
// #include <limits>
#include <math.h>
// CONSTRUCTOR
float16::float16(float f) { _value = f32tof16(f); }
// PRINTING
// size_t float16::printTo(Print& p) const
// {
// double d = this->f16tof32(_value);
// return p.print(d, _decimals);
// }
float float16::toFloat() const { return f16tof32(_value); }
//////////////////////////////////////////////////////////
//
// EQUALITIES
//
bool float16::operator==(const float16 &f) { return (_value == f._value); }
bool float16::operator!=(const float16 &f) { return (_value != f._value); }
bool float16::operator>(const float16 &f) {
if ((_value & 0x8000) && (f._value & 0x8000))
return _value < f._value;
if (_value & 0x8000)
return false;
if (f._value & 0x8000)
return true;
return _value > f._value;
}
bool float16::operator>=(const float16 &f) {
if ((_value & 0x8000) && (f._value & 0x8000))
return _value <= f._value;
if (_value & 0x8000)
return false;
if (f._value & 0x8000)
return true;
return _value >= f._value;
}
bool float16::operator<(const float16 &f) {
if ((_value & 0x8000) && (f._value & 0x8000))
return _value > f._value;
if (_value & 0x8000)
return true;
if (f._value & 0x8000)
return false;
return _value < f._value;
}
bool float16::operator<=(const float16 &f) {
if ((_value & 0x8000) && (f._value & 0x8000))
return _value >= f._value;
if (_value & 0x8000)
return true;
if (f._value & 0x8000)
return false;
return _value <= f._value;
}
//////////////////////////////////////////////////////////
//
// NEGATION
//
float16 float16::operator-() {
float16 f16;
f16.setBinary(_value ^ 0x8000);
return f16;
}
//////////////////////////////////////////////////////////
//
// MATH
//
float16 float16::operator+(const float16 &f) {
return float16(this->toFloat() + f.toFloat());
}
float16 float16::operator-(const float16 &f) {
return float16(this->toFloat() - f.toFloat());
}
float16 float16::operator*(const float16 &f) {
return float16(this->toFloat() * f.toFloat());
}
float16 float16::operator/(const float16 &f) {
return float16(this->toFloat() / f.toFloat());
}
float16 &float16::operator+=(const float16 &f) {
*this = this->toFloat() + f.toFloat();
return *this;
}
float16 &float16::operator-=(const float16 &f) {
*this = this->toFloat() - f.toFloat();
return *this;
}
float16 &float16::operator*=(const float16 &f) {
*this = this->toFloat() * f.toFloat();
return *this;
}
float16 &float16::operator/=(const float16 &f) {
*this = this->toFloat() / f.toFloat();
return *this;
}
//////////////////////////////////////////////////////////
//
// MATH HELPER FUNCTIONS
//
int float16::sign() {
if (_value & 0x8000)
return -1;
if (_value & 0xFFFF)
return 1;
return 0;
}
bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); }
bool float16::isNaN() {
if ((_value & 0x7C00) != 0x7C00)
return false;
if ((_value & 0x03FF) == 0x0000)
return false;
return true;
}
bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); }
//////////////////////////////////////////////////////////
//
// CORE CONVERSION
//
float float16::f16tof32(uint16_t _value) const {
uint16_t sgn, man;
int exp;
float f;
sgn = (_value & 0x8000) > 0;
exp = (_value & 0x7C00) >> 10;
man = (_value & 0x03FF);
// ZERO
if ((_value & 0x7FFF) == 0) {
return sgn ? -0.0f : 0.0f;
}
// NAN & INF
if (exp == 0x001F) {
if (man == 0)
return sgn ? -INFINITY : INFINITY;
else
return NAN;
}
// SUBNORMAL/NORMAL
if (exp == 0)
f = 0;
else
f = 1;
// PROCESS MANTISSE
for (int i = 9; i >= 0; i--) {
f *= 2;
if (man & (1 << i))
f = f + 1;
}
f = f * powf(2.0f, (float)(exp - 25));
if (exp == 0) {
f = f * powf(2.0f, -13); // 5.96046447754e-8;
}
return sgn ? -f : f;
}
uint16_t float16::f32tof16(float f) const {
uint32_t t = *(uint32_t *)&f;
// man bits = 10; but we keep 11 for rounding
uint16_t man = (t & 0x007FFFFF) >> 12;
int16_t exp = (t & 0x7F800000) >> 23;
bool sgn = (t & 0x80000000);
// handle 0
if ((t & 0x7FFFFFFF) == 0) {
return sgn ? 0x8000 : 0x0000;
}
// denormalized float32 does not fit in float16
if (exp == 0x00) {
return sgn ? 0x8000 : 0x0000;
}
// handle infinity & NAN
if (exp == 0x00FF) {
if (man)
return 0xFE00; // NAN
return sgn ? 0xFC00 : 0x7C00; // -INF : INF
}
// normal numbers
exp = exp - 127 + 15;
// overflow does not fit => INF
if (exp > 30) {
return sgn ? 0xFC00 : 0x7C00; // -INF : INF
}
// subnormal numbers
if (exp < -38) {
return sgn ? 0x8000 : 0x0000; // -0 or 0 ? just 0 ?
}
if (exp <= 0) // subnormal
{
man >>= (exp + 14);
// rounding
man++;
man >>= 1;
if (sgn)
return 0x8000 | man;
return man;
}
// normal
// TODO rounding
exp <<= 10;
man++;
man >>= 1;
if (sgn)
return 0x8000 | exp | man;
return exp | man;
}
// -- END OF FILE --

75
ControlCore/float16.h Normal file
View File

@ -0,0 +1,75 @@
#pragma once
//
// FILE: float16.h
// AUTHOR: Rob Tillaart
// VERSION: 0.1.8
// PURPOSE: Arduino library to implement float16 data type.
// half-precision floating point format,
// used for efficient storage and transport.
// URL: https://github.com/RobTillaart/float16
// #include "Arduino.h"
#include <stdint.h>
#define FLOAT16_LIB_VERSION (F("0.1.8"))
typedef uint16_t __fp16;
class float16 {
public:
// Constructors
float16(void) { _value = 0x0000; };
float16(float f);
float16(const float16 &f) { _value = f._value; };
// Conversion
float toFloat(void) const;
// access the 2 byte representation.
uint16_t getBinary() { return _value; };
void setBinary(uint16_t u) { _value = u; };
// Printable
// size_t printTo(Print &p) const;
void setDecimals(uint8_t d) { _decimals = d; };
uint8_t getDecimals() { return _decimals; };
// equalities
bool operator==(const float16 &f);
bool operator!=(const float16 &f);
bool operator>(const float16 &f);
bool operator>=(const float16 &f);
bool operator<(const float16 &f);
bool operator<=(const float16 &f);
// negation
float16 operator-();
// basic math
float16 operator+(const float16 &f);
float16 operator-(const float16 &f);
float16 operator*(const float16 &f);
float16 operator/(const float16 &f);
float16 &operator+=(const float16 &f);
float16 &operator-=(const float16 &f);
float16 &operator*=(const float16 &f);
float16 &operator/=(const float16 &f);
// math helper functions
int sign(); // 1 = positive 0 = zero -1 = negative.
bool isZero();
bool isNaN();
bool isInf();
// CORE CONVERSION
// should be private but for testing...
float f16tof32(uint16_t) const;
uint16_t f32tof16(float) const;
private:
uint8_t _decimals = 4;
__fp16 _value;
};
// -- END OF FILE --

36
ControlCore/test/CMakeLists.txt Normal file
View File

@ -0,0 +1,36 @@
cmake_minimum_required(VERSION 3.13) # CMake version check
Project(ControlCoreTest)
set(CMAKE_CXX_STANDARD 11) # Enable c++11 standard
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
add_compile_definitions(GTEST)
include(FetchContent)
FetchContent_Declare(
googletest
DOWNLOAD_EXTRACT_TIMESTAMP ON
URL https://github.com/google/googletest/archive/refs/heads/main.zip
)
# For Windows: Prevent overriding the parent project's compiler/linker settings
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
FetchContent_MakeAvailable(googletest)
include_directories(
.
..
)
enable_testing()
add_executable(
ControlCoreTest
"dummy_test.cc"
)
target_link_libraries(
ControlCoreTest
gtest_main
)
include(GoogleTest)
gtest_discover_tests(ControlCoreTest)

9
ControlCore/test/dummy_test.cc Normal file
View File

@ -0,0 +1,9 @@
#if GTEST
// #include <gmock/gmock.h>
// not supported using Visual Studio 2022 compiler...
#include <gtest/gtest.h>
TEST(Dummy, Dummytest) {}
#endif