Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Increase constexpr support in geometry data types (#4231)

Browse Source 
This uses std::is_constant_evaluated() to conditionally use the gcem library for constexpr calculations.
This commit is contained in:
David K Turner
2022-10-31 11:17:00 -05:00
committed by GitHub
parent 1c3c86e9f1
commit 3a5a376465
94 changed files with 6919 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
ThirdPartyNotices.txt
View File

@@ -45,6 +45,7 @@ Drake wpimath/src/main/native/thirdparty/drake/
wpimath/src/test/native/cpp/drake/
wpimath/src/test/native/include/drake/
V8 export-template wpiutil/src/main/native/include/wpi/SymbolExports.h
GCEM wpimath/src/main/native/thirdparty/gcem/include/
==============================================================================
Google Test License
@@ -1224,3 +1225,20 @@ DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
============
GCEM License
============
Copyright 2022 - ktholer (https://github.com/kthohr/gcem)
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

46
upstream_utils/update_gcem.py Normal file
View File

@@ -0,0 +1,46 @@
#!/usr/bin/env python3
import os
import shutil
from upstream_utils import (
get_repo_root,
clone_repo,
comment_out_invalid_includes,
walk_cwd_and_copy_if,
git_am,
)
def main():
upstream_root = clone_repo("https://github.com/kthohr/gcem.git", "v1.16.0")
wpilib_root = get_repo_root()
wpimath = os.path.join(wpilib_root, "wpimath")
# Apply patches to upstream Git repo
os.chdir(upstream_root)
for f in []:
git_am(os.path.join(wpilib_root, "upstream_utils/gcem_patches", f))
# Delete old install
for d in [
"src/main/native/thirdparty/gcem/include",
]:
shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True)
# Copy gcem include files into allwpilib
include_files = walk_cwd_and_copy_if(
lambda dp, f: dp.endswith("include")
or dp.endswith("gcem_incl")
or dp.endswith("quadrature"),
os.path.join(wpimath, "src/main/native/thirdparty/gcem"),
)
for f in include_files:
comment_out_invalid_includes(
f, [os.path.join(wpimath, "src/main/native/thirdparty/gcem/include")]
)
if __name__ == "__main__":
main()

2
wpimath/.styleguide
View File

@@ -1,5 +1,6 @@
cppHeaderFileInclude {
\.h$
\.hpp$
\.inc$
\.inl$
}
@@ -38,6 +39,7 @@ includeOtherLibs {
}
includeProject {
^gcem/
^drake/
^Eigen/
^units/

4
wpimath/CMakeLists.txt
View File

@@ -122,6 +122,10 @@ install(DIRECTORY src/main/native/thirdparty/drake/include/ DESTINATION "${inclu
target_include_directories(wpimath SYSTEM PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/drake/include>)
install(DIRECTORY src/main/native/thirdparty/gcem/include/ DESTINATION "${include_dest}/wpimath")
target_include_directories(wpimath SYSTEM PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/gcem/include>)
target_include_directories(wpimath PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
$<INSTALL_INTERFACE:${include_dest}/wpimath>)

9
wpimath/build.gradle
View File

@@ -19,7 +19,8 @@ ext {
}
exportedHeaders {
srcDirs 'src/main/native/thirdparty/drake/include',
'src/main/native/thirdparty/eigen/include'
'src/main/native/thirdparty/eigen/include',
'src/main/native/thirdparty/gcem/include'
}
}
}
@@ -35,6 +36,9 @@ cppHeadersZip {
from('src/main/native/thirdparty/eigen/include') {
into '/'
}
from('src/main/native/thirdparty/gcem/include') {
into '/'
}
}
model {
@@ -44,7 +48,8 @@ model {
it.exportedHeaders {
srcDirs 'src/main/native/include',
'src/main/native/thirdparty/drake/include',
'src/main/native/thirdparty/eigen/include'
'src/main/native/thirdparty/eigen/include',
'src/main/native/thirdparty/gcem/include'
}
}
}

23
wpimath/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -10,16 +10,6 @@
using namespace frc;
Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation(x, y), m_rotation(std::move(rotation)) {}
Pose2d Pose2d::operator+(const Transform2d& other) const {
return TransformBy(other);
}
Transform2d Pose2d::operator-(const Pose2d& other) const {
const auto pose = this->RelativeTo(other);
return Transform2d{pose.Translation(), pose.Rotation()};
@@ -33,19 +23,6 @@ bool Pose2d::operator!=(const Pose2d& other) const {
return !operator==(other);
}
Pose2d Pose2d::operator*(double scalar) const {
return Pose2d{m_translation * scalar, m_rotation * scalar};
}
Pose2d Pose2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
Pose2d Pose2d::TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
m_rotation + other.Rotation()};
}
Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
const Transform2d transform{other, *this};
return {transform.Translation(), transform.Rotation()};

55
wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
View File

@@ -12,61 +12,6 @@
using namespace frc;
Rotation2d::Rotation2d(units::radian_t value)
: m_value(value),
m_cos(units::math::cos(value)),
m_sin(units::math::sin(value)) {}
Rotation2d::Rotation2d(units::degree_t value)
: m_value(value),
m_cos(units::math::cos(value)),
m_sin(units::math::sin(value)) {}
Rotation2d::Rotation2d(double x, double y) {
const auto magnitude = std::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
}
m_value = units::radian_t{std::atan2(m_sin, m_cos)};
}
Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
return RotateBy(other);
}
Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
return *this + -other;
}
Rotation2d Rotation2d::operator-() const {
return Rotation2d{-m_value};
}
Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d{m_value * scalar};
}
Rotation2d Rotation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Rotation2d::operator==(const Rotation2d& other) const {
return std::hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
}
bool Rotation2d::operator!=(const Rotation2d& other) const {
return !operator==(other);
}
Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
json = wpi::json{{"radians", rotation.Radians().value()}};
}

10
wpimath/src/main/native/cpp/geometry/Transform2d.cpp
View File

@@ -18,16 +18,6 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
m_rotation = final.Rotation() - initial.Rotation();
}
Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
Transform2d Transform2d::operator+(const Transform2d& other) const {
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
}

35
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -10,12 +10,6 @@
using namespace frc;
Translation2d::Translation2d(units::meter_t x, units::meter_t y)
: m_x(x), m_y(y) {}
Translation2d::Translation2d(units::meter_t distance, const Rotation2d& angle)
: m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
units::meter_t Translation2d::Distance(const Translation2d& other) const {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}
@@ -24,35 +18,6 @@ units::meter_t Translation2d::Norm() const {
return units::math::hypot(m_x, m_y);
}
Rotation2d Translation2d::Angle() const {
return Rotation2d{m_x.value(), m_y.value()};
}
Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),
m_x * other.Sin() + m_y * other.Cos()};
}
Translation2d Translation2d::operator+(const Translation2d& other) const {
return {X() + other.X(), Y() + other.Y()};
}
Translation2d Translation2d::operator-(const Translation2d& other) const {
return *this + -other;
}
Translation2d Translation2d::operator-() const {
return {-m_x, -m_y};
}
Translation2d Translation2d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
Translation2d Translation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Translation2d::operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;

30
wpimath/src/main/native/cpp/geometry/Translation3d.cpp
View File

@@ -4,14 +4,8 @@
#include "frc/geometry/Translation3d.h"
#include "units/math.h"
using namespace frc;
Translation3d::Translation3d(units::meter_t x, units::meter_t y,
units::meter_t z)
: m_x(x), m_y(y), m_z(z) {}
Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
@@ -36,30 +30,6 @@ Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
units::meter_t{qprime.Z()}};
}
Translation2d Translation3d::ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
Translation3d Translation3d::operator+(const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
}
Translation3d Translation3d::operator-(const Translation3d& other) const {
return *this + -other;
}
Translation3d Translation3d::operator-() const {
return {-m_x, -m_y, -m_z};
}
Translation3d Translation3d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
Translation3d Translation3d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Translation3d::operator==(const Translation3d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m &&

22
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -32,7 +32,7 @@ class WPILIB_DLLEXPORT Pose2d {
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose2d(Translation2d translation, Rotation2d rotation);
constexpr Pose2d(Translation2d translation, Rotation2d rotation);
/**
* Constructs a pose with x and y translations instead of a separate
@@ -42,7 +42,7 @@ class WPILIB_DLLEXPORT Pose2d {
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
/**
* Transforms the pose by the given transformation and returns the new
@@ -58,7 +58,7 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The transformed pose.
*/
Pose2d operator+(const Transform2d& other) const;
constexpr Pose2d operator+(const Transform2d& other) const;
/**
* Returns the Transform2d that maps the one pose to another.
@@ -89,28 +89,28 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return Reference to the translational component of the pose.
*/
const Translation2d& Translation() const { return m_translation; }
constexpr const Translation2d& Translation() const { return m_translation; }
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
units::meter_t X() const { return m_translation.X(); }
constexpr units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the underlying rotation.
*
* @return Reference to the rotational component of the pose.
*/
const Rotation2d& Rotation() const { return m_rotation; }
constexpr const Rotation2d& Rotation() const { return m_rotation; }
/**
* Multiplies the current pose by a scalar.
@@ -119,7 +119,7 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The new scaled Pose2d.
*/
Pose2d operator*(double scalar) const;
constexpr Pose2d operator*(double scalar) const;
/**
* Divides the current pose by a scalar.
@@ -128,7 +128,7 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The new scaled Pose2d.
*/
Pose2d operator/(double scalar) const;
constexpr Pose2d operator/(double scalar) const;
/**
* Transforms the pose by the given transformation and returns the new pose.
@@ -138,7 +138,7 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The transformed pose.
*/
Pose2d TransformBy(const Transform2d& other) const;
constexpr Pose2d TransformBy(const Transform2d& other) const;
/**
* Returns the other pose relative to the current pose.
@@ -199,3 +199,5 @@ WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Pose2d& pose);
} // namespace frc
#include "Pose2d.inc"

39
wpimath/src/main/native/include/frc/geometry/Pose2d.inc Normal file
View File

@@ -0,0 +1,39 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <utility>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "units/length.h"
namespace frc {
constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
Rotation2d rotation)
: m_translation(x, y), m_rotation(std::move(rotation)) {}
constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
return TransformBy(other);
}
constexpr Pose2d Pose2d::operator*(double scalar) const {
return Pose2d{m_translation * scalar, m_rotation * scalar};
}
constexpr Pose2d Pose2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
m_rotation + other.Rotation()};
}
} // namespace frc

34
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -30,14 +30,14 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @param value The value of the angle in radians.
*/
Rotation2d(units::radian_t value); // NOLINT
constexpr Rotation2d(units::radian_t value); // NOLINT
/**
* Constructs a Rotation2d with the given degree value.
*
* @param value The value of the angle in degrees.
*/
Rotation2d(units::degree_t value); // NOLINT
constexpr Rotation2d(units::degree_t value); // NOLINT
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -46,7 +46,7 @@ class WPILIB_DLLEXPORT Rotation2d {
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
*/
Rotation2d(double x, double y);
constexpr Rotation2d(double x, double y);
/**
* Adds two rotations together, with the result being bounded between -pi and
@@ -59,7 +59,7 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The sum of the two rotations.
*/
Rotation2d operator+(const Rotation2d& other) const;
constexpr Rotation2d operator+(const Rotation2d& other) const;
/**
* Subtracts the new rotation from the current rotation and returns the new
@@ -72,7 +72,7 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The difference between the two rotations.
*/
Rotation2d operator-(const Rotation2d& other) const;
constexpr Rotation2d operator-(const Rotation2d& other) const;
/**
* Takes the inverse of the current rotation. This is simply the negative of
@@ -80,7 +80,7 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The inverse of the current rotation.
*/
Rotation2d operator-() const;
constexpr Rotation2d operator-() const;
/**
* Multiplies the current rotation by a scalar.
@@ -89,7 +89,7 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new scaled Rotation2d.
*/
Rotation2d operator*(double scalar) const;
constexpr Rotation2d operator*(double scalar) const;
/**
* Divides the current rotation by a scalar.
@@ -98,7 +98,7 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new scaled Rotation2d.
*/
Rotation2d operator/(double scalar) const;
constexpr Rotation2d operator/(double scalar) const;
/**
* Checks equality between this Rotation2d and another object.
@@ -106,7 +106,7 @@ class WPILIB_DLLEXPORT Rotation2d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Rotation2d& other) const;
constexpr bool operator==(const Rotation2d& other) const;
/**
* Checks inequality between this Rotation2d and another object.
@@ -114,7 +114,7 @@ class WPILIB_DLLEXPORT Rotation2d {
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Rotation2d& other) const;
constexpr bool operator!=(const Rotation2d& other) const;
/**
* Adds the new rotation to the current rotation using a rotation matrix.
@@ -129,42 +129,42 @@ class WPILIB_DLLEXPORT Rotation2d {
*
* @return The new rotated Rotation2d.
*/
Rotation2d RotateBy(const Rotation2d& other) const;
constexpr Rotation2d RotateBy(const Rotation2d& other) const;
/**
* Returns the radian value of the rotation.
*
* @return The radian value of the rotation.
*/
units::radian_t Radians() const { return m_value; }
constexpr units::radian_t Radians() const { return m_value; }
/**
* Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
*/
units::degree_t Degrees() const { return m_value; }
constexpr units::degree_t Degrees() const { return m_value; }
/**
* Returns the cosine of the rotation.
*
* @return The cosine of the rotation.
*/
double Cos() const { return m_cos; }
constexpr double Cos() const { return m_cos; }
/**
* Returns the sine of the rotation.
*
* @return The sine of the rotation.
*/
double Sin() const { return m_sin; }
constexpr double Sin() const { return m_sin; }
/**
* Returns the tangent of the rotation.
*
* @return The tangent of the rotation.
*/
double Tan() const { return m_sin / m_cos; }
constexpr double Tan() const { return Sin() / Cos(); }
private:
units::radian_t m_value = 0_rad;
@@ -179,3 +179,5 @@ WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Rotation2d& rotation);
} // namespace frc
#include "Rotation2d.inc"

74
wpimath/src/main/native/include/frc/geometry/Rotation2d.inc Normal file
View File

@@ -0,0 +1,74 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <type_traits>
#include "frc/geometry/Rotation2d.h"
#include "gcem.hpp"
#include "units/angle.h"
namespace frc {
constexpr Rotation2d::Rotation2d(units::radian_t value)
: m_value(value),
m_cos(std::is_constant_evaluated() ? gcem::cos(value.to<double>())
: std::cos(value.to<double>())),
m_sin(std::is_constant_evaluated() ? gcem::sin(value.to<double>())
: std::sin(value.to<double>())) {}
constexpr Rotation2d::Rotation2d(units::degree_t value)
: Rotation2d(units::radian_t{value}) {}
constexpr Rotation2d::Rotation2d(double x, double y) {
const auto magnitude = gcem::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
}
m_value =
units::radian_t{std::is_constant_evaluated() ? gcem::atan2(m_sin, m_cos)
: std::atan2(m_sin, m_cos)};
}
constexpr Rotation2d Rotation2d::operator-() const {
return Rotation2d{-m_value};
}
constexpr Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d(m_value * scalar);
}
constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
return RotateBy(other);
}
constexpr Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
return *this + -other;
}
constexpr Rotation2d Rotation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
constexpr bool Rotation2d::operator==(const Rotation2d& other) const {
return (std::is_constant_evaluated()
? gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin())
: std::hypot(Cos() - other.Cos(), Sin() - other.Sin())) < 1E-9;
}
constexpr bool Rotation2d::operator!=(const Rotation2d& other) const {
return !operator==(other);
}
constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
} // namespace frc

20
wpimath/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -31,7 +31,7 @@ class WPILIB_DLLEXPORT Transform2d {
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform2d(Translation2d translation, Rotation2d rotation);
constexpr Transform2d(Translation2d translation, Rotation2d rotation);
/**
* Constructs the identity transform -- maps an initial pose to itself.
@@ -43,35 +43,35 @@ class WPILIB_DLLEXPORT Transform2d {
*
* @return Reference to the translational component of the transform.
*/
const Translation2d& Translation() const { return m_translation; }
constexpr const Translation2d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
constexpr units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation2d& Rotation() const { return m_rotation; }
constexpr const Rotation2d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform2d Inverse() const;
constexpr Transform2d Inverse() const;
/**
* Multiplies the transform by the scalar.
@@ -79,7 +79,7 @@ class WPILIB_DLLEXPORT Transform2d {
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
Transform2d operator*(double scalar) const {
constexpr Transform2d operator*(double scalar) const {
return Transform2d(m_translation * scalar, m_rotation * scalar);
}
@@ -89,7 +89,9 @@ class WPILIB_DLLEXPORT Transform2d {
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
Transform2d operator/(double scalar) const { return *this * (1.0 / scalar); }
constexpr Transform2d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Composes two transformations.
@@ -120,3 +122,5 @@ class WPILIB_DLLEXPORT Transform2d {
Rotation2d m_rotation;
};
} // namespace frc
#include "Transform2d.inc"

26
wpimath/src/main/native/include/frc/geometry/Transform2d.inc Normal file
View File

@@ -0,0 +1,26 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <utility>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
namespace frc {
constexpr Transform2d::Transform2d(Translation2d translation,
Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
constexpr Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
} // namespace frc

24
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -37,7 +37,7 @@ class WPILIB_DLLEXPORT Translation2d {
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
Translation2d(units::meter_t x, units::meter_t y);
constexpr Translation2d(units::meter_t x, units::meter_t y);
/**
* Constructs a Translation2d with the provided distance and angle. This is
@@ -46,7 +46,7 @@ class WPILIB_DLLEXPORT Translation2d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
Translation2d(units::meter_t distance, const Rotation2d& angle);
constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
/**
* Calculates the distance between two translations in 2D space.
@@ -64,14 +64,14 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The X component of the translation.
*/
units::meter_t X() const { return m_x; }
constexpr units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
units::meter_t Y() const { return m_y; }
constexpr units::meter_t Y() const { return m_y; }
/**
* Returns the norm, or distance from the origin to the translation.
@@ -85,7 +85,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The angle of the translation
*/
Rotation2d Angle() const;
constexpr Rotation2d Angle() const;
/**
* Applies a rotation to the translation in 2D space.
@@ -105,7 +105,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The new rotated translation.
*/
Translation2d RotateBy(const Rotation2d& other) const;
constexpr Translation2d RotateBy(const Rotation2d& other) const;
/**
* Returns the sum of two translations in 2D space.
@@ -117,7 +117,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The sum of the translations.
*/
Translation2d operator+(const Translation2d& other) const;
constexpr Translation2d operator+(const Translation2d& other) const;
/**
* Returns the difference between two translations.
@@ -129,7 +129,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The difference between the two translations.
*/
Translation2d operator-(const Translation2d& other) const;
constexpr Translation2d operator-(const Translation2d& other) const;
/**
* Returns the inverse of the current translation. This is equivalent to
@@ -138,7 +138,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The inverse of the current translation.
*/
Translation2d operator-() const;
constexpr Translation2d operator-() const;
/**
* Returns the translation multiplied by a scalar.
@@ -149,7 +149,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The scaled translation.
*/
Translation2d operator*(double scalar) const;
constexpr Translation2d operator*(double scalar) const;
/**
* Returns the translation divided by a scalar.
@@ -160,7 +160,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The scaled translation.
*/
Translation2d operator/(double scalar) const;
constexpr Translation2d operator/(double scalar) const;
/**
* Checks equality between this Translation2d and another object.
@@ -190,3 +190,5 @@ WPILIB_DLLEXPORT
void from_json(const wpi::json& json, Translation2d& state);
} // namespace frc
#include "Translation2d.inc"

50
wpimath/src/main/native/include/frc/geometry/Translation2d.inc Normal file
View File

@@ -0,0 +1,50 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/geometry/Translation2d.h"
#include "units/length.h"
namespace frc {
constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
: m_x(x), m_y(y) {}
constexpr Translation2d::Translation2d(units::meter_t distance,
const Rotation2d& angle)
: m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
constexpr Rotation2d Translation2d::Angle() const {
return Rotation2d{m_x.value(), m_y.value()};
}
constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),
m_x * other.Sin() + m_y * other.Cos()};
}
constexpr Translation2d Translation2d::operator+(
const Translation2d& other) const {
return {X() + other.X(), Y() + other.Y()};
}
constexpr Translation2d Translation2d::operator-(
const Translation2d& other) const {
return *this + -other;
}
constexpr Translation2d Translation2d::operator-() const {
return {-m_x, -m_y};
}
constexpr Translation2d Translation2d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
constexpr Translation2d Translation2d::operator/(double scalar) const {
return operator*(1.0 / scalar);
}
} // namespace frc

22
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -35,7 +35,7 @@ class WPILIB_DLLEXPORT Translation3d {
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
/**
* Constructs a Translation3d with the provided distance and angle. This is
@@ -63,21 +63,21 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The Z component of the translation.
*/
units::meter_t X() const { return m_x; }
constexpr units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
units::meter_t Y() const { return m_y; }
constexpr units::meter_t Y() const { return m_y; }
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t Z() const { return m_z; }
constexpr units::meter_t Z() const { return m_z; }
/**
* Returns the norm, or distance from the origin to the translation.
@@ -102,7 +102,7 @@ class WPILIB_DLLEXPORT Translation3d {
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.
*/
Translation2d ToTranslation2d() const;
constexpr Translation2d ToTranslation2d() const;
/**
* Returns the sum of two translations in 3D space.
@@ -114,7 +114,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The sum of the translations.
*/
Translation3d operator+(const Translation3d& other) const;
constexpr Translation3d operator+(const Translation3d& other) const;
/**
* Returns the difference between two translations.
@@ -126,7 +126,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The difference between the two translations.
*/
Translation3d operator-(const Translation3d& other) const;
constexpr Translation3d operator-(const Translation3d& other) const;
/**
* Returns the inverse of the current translation. This is equivalent to
@@ -134,7 +134,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The inverse of the current translation.
*/
Translation3d operator-() const;
constexpr Translation3d operator-() const;
/**
* Returns the translation multiplied by a scalar.
@@ -146,7 +146,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The scaled translation.
*/
Translation3d operator*(double scalar) const;
constexpr Translation3d operator*(double scalar) const;
/**
* Returns the translation divided by a scalar.
@@ -158,7 +158,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The scaled translation.
*/
Translation3d operator/(double scalar) const;
constexpr Translation3d operator/(double scalar) const;
/**
* Checks equality between this Translation3d and another object.
@@ -183,3 +183,5 @@ class WPILIB_DLLEXPORT Translation3d {
};
} // namespace frc
#include "Translation3d.inc"

44
wpimath/src/main/native/include/frc/geometry/Translation3d.inc Normal file
View File

@@ -0,0 +1,44 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Translation3d.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
units::meter_t z)
: m_x(x), m_y(y), m_z(z) {}
constexpr Translation2d Translation3d::ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
constexpr Translation3d Translation3d::operator+(
const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
}
constexpr Translation3d Translation3d::operator-(
const Translation3d& other) const {
return operator+(-other);
}
constexpr Translation3d Translation3d::operator-() const {
return {-m_x, -m_y, -m_z};
}
constexpr Translation3d Translation3d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
constexpr Translation3d Translation3d::operator/(double scalar) const {
return operator*(1.0 / scalar);
}
} // namespace frc

2
wpimath/src/main/native/include/frc/geometry/Twist2d.h
View File

@@ -60,7 +60,7 @@ struct WPILIB_DLLEXPORT Twist2d {
* @param factor The factor by which to scale.
* @return The scaled Twist2d.
*/
Twist2d operator*(double factor) const {
constexpr Twist2d operator*(double factor) const {
return Twist2d{dx * factor, dy * factor, dtheta * factor};
}
};

2
wpimath/src/main/native/include/frc/geometry/Twist3d.h
View File

@@ -79,7 +79,7 @@ struct WPILIB_DLLEXPORT Twist3d {
* @param factor The factor by which to scale.
* @return The scaled Twist3d.
*/
Twist3d operator*(double factor) const {
constexpr Twist3d operator*(double factor) const {
return Twist3d{dx * factor, dy * factor, dz * factor,
rx * factor, ry * factor, rz * factor};
}

104
wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp vendored Normal file
View File

@@ -0,0 +1,104 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_HPP
#define _gcem_HPP
#include "gcem_incl/gcem_options.hpp"
namespace gcem
{
#include "gcem_incl/quadrature/gauss_legendre_50.hpp"
#include "gcem_incl/is_inf.hpp"
#include "gcem_incl/is_nan.hpp"
#include "gcem_incl/is_finite.hpp"
#include "gcem_incl/signbit.hpp"
#include "gcem_incl/copysign.hpp"
#include "gcem_incl/neg_zero.hpp"
#include "gcem_incl/sgn.hpp"
#include "gcem_incl/abs.hpp"
#include "gcem_incl/ceil.hpp"
#include "gcem_incl/floor.hpp"
#include "gcem_incl/trunc.hpp"
#include "gcem_incl/is_odd.hpp"
#include "gcem_incl/is_even.hpp"
#include "gcem_incl/max.hpp"
#include "gcem_incl/min.hpp"
#include "gcem_incl/sqrt.hpp"
#include "gcem_incl/inv_sqrt.hpp"
#include "gcem_incl/hypot.hpp"
#include "gcem_incl/find_exponent.hpp"
#include "gcem_incl/find_fraction.hpp"
#include "gcem_incl/find_whole.hpp"
#include "gcem_incl/mantissa.hpp"
#include "gcem_incl/round.hpp"
#include "gcem_incl/fmod.hpp"
#include "gcem_incl/pow_integral.hpp"
#include "gcem_incl/exp.hpp"
#include "gcem_incl/expm1.hpp"
#include "gcem_incl/log.hpp"
#include "gcem_incl/log1p.hpp"
#include "gcem_incl/log2.hpp"
#include "gcem_incl/log10.hpp"
#include "gcem_incl/pow.hpp"
#include "gcem_incl/gcd.hpp"
#include "gcem_incl/lcm.hpp"
#include "gcem_incl/tan.hpp"
#include "gcem_incl/cos.hpp"
#include "gcem_incl/sin.hpp"
#include "gcem_incl/atan.hpp"
#include "gcem_incl/atan2.hpp"
#include "gcem_incl/acos.hpp"
#include "gcem_incl/asin.hpp"
#include "gcem_incl/tanh.hpp"
#include "gcem_incl/cosh.hpp"
#include "gcem_incl/sinh.hpp"
#include "gcem_incl/atanh.hpp"
#include "gcem_incl/acosh.hpp"
#include "gcem_incl/asinh.hpp"
#include "gcem_incl/binomial_coef.hpp"
#include "gcem_incl/lgamma.hpp"
#include "gcem_incl/tgamma.hpp"
#include "gcem_incl/factorial.hpp"
#include "gcem_incl/lbeta.hpp"
#include "gcem_incl/beta.hpp"
#include "gcem_incl/lmgamma.hpp"
#include "gcem_incl/log_binomial_coef.hpp"
#include "gcem_incl/erf.hpp"
#include "gcem_incl/erf_inv.hpp"
#include "gcem_incl/incomplete_beta.hpp"
#include "gcem_incl/incomplete_beta_inv.hpp"
#include "gcem_incl/incomplete_gamma.hpp"
#include "gcem_incl/incomplete_gamma_inv.hpp"
}
#endif

45
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp vendored Normal file
View File

@@ -0,0 +1,45 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_abs_HPP
#define _gcem_abs_HPP
/**
* Compile-time absolute value function
*
* @param x a real-valued input.
* @return the absolute value of \c x, \f$ |x| \f$.
*/
template<typename T>
constexpr
T
abs(const T x)
noexcept
{
return( // deal with signed-zeros
x == T(0) ? \
T(0) :
// else
x < T(0) ? \
- x : x );
}
#endif

84
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp vendored Normal file
View File

@@ -0,0 +1,84 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time arccosine function
*/
#ifndef _gcem_acos_HPP
#define _gcem_acos_HPP
namespace internal
{
template<typename T>
constexpr
T
acos_compute(const T x)
noexcept
{
return( // only defined on [-1,1]
abs(x) > T(1) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from one or zero
GCLIM<T>::min() > abs(x - T(1)) ? \
T(0) :
GCLIM<T>::min() > abs(x) ? \
T(GCEM_HALF_PI) :
// else
atan( sqrt(T(1) - x*x)/x ) );
}
template<typename T>
constexpr
T
acos_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
//
x > T(0) ? \
// if
acos_compute(x) :
// else
T(GCEM_PI) - acos_compute(-x) );
}
}
/**
* Compile-time arccosine function
*
* @param x a real-valued input, where \f$ x \in [-1,1] \f$.
* @return the inverse cosine function using \f[ \text{acos}(x) = \text{atan} \left( \frac{\sqrt{1-x^2}}{x} \right) \f]
*/
template<typename T>
constexpr
return_t<T>
acos(const T x)
noexcept
{
return internal::acos_check( static_cast<return_t<T>>(x) );
}
#endif

68
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp vendored Normal file
View File

@@ -0,0 +1,68 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time inverse hyperbolic cosine function
*/
#ifndef _gcem_acosh_HPP
#define _gcem_acosh_HPP
namespace internal
{
template<typename T>
constexpr
T
acosh_compute(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// function defined for x >= 1
x < T(1) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from 1
GCLIM<T>::min() > abs(x - T(1)) ? \
T(0) :
// else
log( x + sqrt(x*x - T(1)) ) );
}
}
/**
* Compile-time inverse hyperbolic cosine function
*
* @param x a real-valued input.
* @return the inverse hyperbolic cosine function using \f[ \text{acosh}(x) = \ln \left( x + \sqrt{x^2 - 1} \right) \f]
*/
template<typename T>
constexpr
return_t<T>
acosh(const T x)
noexcept
{
return internal::acosh_compute( static_cast<return_t<T>>(x) );
}
#endif

82
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp vendored Normal file
View File

@@ -0,0 +1,82 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time arcsine function
*/
#ifndef _gcem_asin_HPP
#define _gcem_asin_HPP
namespace internal
{
template<typename T>
constexpr
T
asin_compute(const T x)
noexcept
{
return( // only defined on [-1,1]
x > T(1) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from one or zero
GCLIM<T>::min() > abs(x - T(1)) ? \
T(GCEM_HALF_PI) :
GCLIM<T>::min() > abs(x) ? \
T(0) :
// else
atan( x/sqrt(T(1) - x*x) ) );
}
template<typename T>
constexpr
T
asin_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
//
x < T(0) ? \
- asin_compute(-x) :
asin_compute(x) );
}
}
/**
* Compile-time arcsine function
*
* @param x a real-valued input, where \f$ x \in [-1,1] \f$.
* @return the inverse sine function using \f[ \text{asin}(x) = \text{atan} \left( \frac{x}{\sqrt{1-x^2}} \right) \f]
*/
template<typename T>
constexpr
return_t<T>
asin(const T x)
noexcept
{
return internal::asin_check( static_cast<return_t<T>>(x) );
}
#endif

66
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp vendored Normal file
View File

@@ -0,0 +1,66 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time inverse hyperbolic sine function
*/
#ifndef _gcem_asinh_HPP
#define _gcem_asinh_HPP
namespace internal
{
template<typename T>
constexpr
T
asinh_compute(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(0) :
// else
log( x + sqrt(x*x + T(1)) ) );
}
}
/**
* Compile-time inverse hyperbolic sine function
*
* @param x a real-valued input.
* @return the inverse hyperbolic sine function using \f[ \text{asinh}(x) = \ln \left( x + \sqrt{x^2 + 1} \right) \f]
*/
template<typename T>
constexpr
return_t<T>
asinh(const T x)
noexcept
{
return internal::asinh_compute( static_cast<return_t<T>>(x) );
}
#endif

155
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp vendored Normal file
View File

@@ -0,0 +1,155 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time arctangent function
*/
// see
// http://functions.wolfram.com/ElementaryFunctions/ArcTan/10/0001/
// http://functions.wolfram.com/ElementaryFunctions/ArcTan/06/01/06/01/0002/
#ifndef _gcem_atan_HPP
#define _gcem_atan_HPP
namespace internal
{
// Series
template<typename T>
constexpr
T
atan_series_order_calc(const T x, const T x_pow, const uint_t order)
noexcept
{
return( T(1)/( T((order-1)*4 - 1) * x_pow ) \
- T(1)/( T((order-1)*4 + 1) * x_pow*x) );
}
template<typename T>
constexpr
T
atan_series_order(const T x, const T x_pow, const uint_t order, const uint_t max_order)
noexcept
{
return( order == 1 ? \
GCEM_HALF_PI - T(1)/x + atan_series_order(x*x,pow(x,3),order+1,max_order) :
// NOTE: x changes to x*x for order > 1
order < max_order ? \
atan_series_order_calc(x,x_pow,order) \
+ atan_series_order(x,x_pow*x*x,order+1,max_order) :
// order == max_order
atan_series_order_calc(x,x_pow,order) );
}
template<typename T>
constexpr
T
atan_series_main(const T x)
noexcept
{
return( x < T(3) ? atan_series_order(x,x,1U,10U) : // O(1/x^39)
x < T(4) ? atan_series_order(x,x,1U,9U) : // O(1/x^35)
x < T(5) ? atan_series_order(x,x,1U,8U) : // O(1/x^31)
x < T(7) ? atan_series_order(x,x,1U,7U) : // O(1/x^27)
x < T(11) ? atan_series_order(x,x,1U,6U) : // O(1/x^23)
x < T(25) ? atan_series_order(x,x,1U,5U) : // O(1/x^19)
x < T(100) ? atan_series_order(x,x,1U,4U) : // O(1/x^15)
x < T(1000) ? atan_series_order(x,x,1U,3U) : // O(1/x^11)
atan_series_order(x,x,1U,2U) ); // O(1/x^7)
}
// CF
template<typename T>
constexpr
T
atan_cf_recur(const T xx, const uint_t depth, const uint_t max_depth)
noexcept
{
return( depth < max_depth ? \
// if
T(2*depth - 1) + depth*depth*xx/atan_cf_recur(xx,depth+1,max_depth) :
// else
T(2*depth - 1) );
}
template<typename T>
constexpr
T
atan_cf_main(const T x)
noexcept
{
return( x < T(0.5) ? x/atan_cf_recur(x*x,1U, 15U ) :
x < T(1) ? x/atan_cf_recur(x*x,1U, 25U ) :
x < T(1.5) ? x/atan_cf_recur(x*x,1U, 35U ) :
x < T(2) ? x/atan_cf_recur(x*x,1U, 45U ) :
x/atan_cf_recur(x*x,1U, 52U ) );
}
//
template<typename T>
constexpr
T
atan_begin(const T x)
noexcept
{
return( x > T(2.5) ? atan_series_main(x) : atan_cf_main(x) );
}
template<typename T>
constexpr
T
atan_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(0) :
// negative or positive
x < T(0) ? \
- atan_begin(-x) :
atan_begin( x) );
}
}
/**
* Compile-time arctangent function
*
* @param x a real-valued input.
* @return the inverse tangent function using \f[ \text{atan}(x) = \dfrac{x}{1 + \dfrac{x^2}{3 + \dfrac{4x^2}{5 + \dfrac{9x^2}{7 + \ddots}}}} \f]
*/
template<typename T>
constexpr
return_t<T>
atan(const T x)
noexcept
{
return internal::atan_check( static_cast<return_t<T>>(x) );
}
#endif

88
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp vendored Normal file
View File

@@ -0,0 +1,88 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time two-argument arctangent function
*/
#ifndef _gcem_atan2_HPP
#define _gcem_atan2_HPP
namespace internal
{
template<typename T>
constexpr
T
atan2_compute(const T y, const T x)
noexcept
{
return( // NaN check
any_nan(y,x) ? \
GCLIM<T>::quiet_NaN() :
//
GCLIM<T>::min() > abs(x) ? \
//
GCLIM<T>::min() > abs(y) ? \
neg_zero(y) ? \
neg_zero(x) ? - T(GCEM_PI) : - T(0) :
neg_zero(x) ? T(GCEM_PI) : T(0) :
y > T(0) ? \
T(GCEM_HALF_PI) : - T(GCEM_HALF_PI) :
//
x < T(0) ? \
y < T(0) ? \
atan(y/x) - T(GCEM_PI) :
atan(y/x) + T(GCEM_PI) :
//
atan(y/x) );
}
template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
constexpr
TC
atan2_type_check(const T1 y, const T2 x)
noexcept
{
return atan2_compute(static_cast<TC>(x),static_cast<TC>(y));
}
}
/**
* Compile-time two-argument arctangent function
*
* @param y a real-valued input.
* @param x a real-valued input.
* @return \f[ \text{atan2}(y,x) = \begin{cases} \text{atan}(y/x) & \text{ if } x > 0 \\ \text{atan}(y/x) + \pi & \text{ if } x < 0 \text{ and } y \geq 0 \\ \text{atan}(y/x) - \pi & \text{ if } x < 0 \text{ and } y < 0 \\ + \pi/2 & \text{ if } x = 0 \text{ and } y > 0 \\ - \pi/2 & \text{ if } x = 0 \text{ and } y < 0 \end{cases} \f]
* The function is undefined at the origin, however the following conventions are used.
* \f[ \text{atan2}(y,x) = \begin{cases} +0 & \text{ if } x = +0 \text{ and } y = +0 \\ -0 & \text{ if } x = +0 \text{ and } y = -0 \\ +\pi & \text{ if } x = -0 \text{ and } y = +0 \\ - \pi & \text{ if } x = -0 \text{ and } y = -0 \end{cases} \f]
*/
template<typename T1, typename T2>
constexpr
common_return_t<T1,T2>
atan2(const T1 y, const T2 x)
noexcept
{
return internal::atan2_type_check(x,y);
}
#endif

79
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp vendored Normal file
View File

@@ -0,0 +1,79 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time inverse hyperbolic tangent function
*/
#ifndef _gcem_atanh_HPP
#define _gcem_atanh_HPP
namespace internal
{
template<typename T>
constexpr
T
atanh_compute(const T x)
noexcept
{
return( log( (T(1) + x)/(T(1) - x) ) / T(2) );
}
template<typename T>
constexpr
T
atanh_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// function is defined for |x| < 1
T(1) < abs(x) ? \
GCLIM<T>::quiet_NaN() :
GCLIM<T>::min() > (T(1) - abs(x)) ? \
sgn(x)*GCLIM<T>::infinity() :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(0) :
// else
atanh_compute(x) );
}
}
/**
* Compile-time inverse hyperbolic tangent function
*
* @param x a real-valued input.
* @return the inverse hyperbolic tangent function using \f[ \text{atanh}(x) = \frac{1}{2} \ln \left( \frac{1+x}{1-x} \right) \f]
*/
template<typename T>
constexpr
return_t<T>
atanh(const T x)
noexcept
{
return internal::atanh_check( static_cast<return_t<T>>(x) );
}
#endif

42
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp vendored Normal file
View File

@@ -0,0 +1,42 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_beta_HPP
#define _gcem_beta_HPP
/**
* Compile-time beta function
*
* @param a a real-valued input.
* @param b a real-valued input.
* @return the beta function using \f[ \text{B}(\alpha,\beta) := \int_0^1 t^{\alpha - 1} (1-t)^{\beta - 1} dt = \frac{\Gamma(\alpha)\Gamma(\beta)}{\Gamma(\alpha + \beta)} \f]
* where \f$ \Gamma \f$ denotes the gamma function.
*/
template<typename T1, typename T2>
constexpr
common_return_t<T1,T2>
beta(const T1 a, const T2 b)
noexcept
{
return exp( lbeta(a,b) );
}
#endif

91
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp vendored Normal file
View File

@@ -0,0 +1,91 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_binomial_coef_HPP
#define _gcem_binomial_coef_HPP
namespace internal
{
template<typename T>
constexpr
T
binomial_coef_recur(const T n, const T k)
noexcept
{
return( // edge cases
(k == T(0) || n == k) ? T(1) : // deals with 0 choose 0 case
n == T(0) ? T(0) :
// else
binomial_coef_recur(n-1,k-1) + binomial_coef_recur(n-1,k) );
}
template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
constexpr
T
binomial_coef_check(const T n, const T k)
noexcept
{
return binomial_coef_recur(n,k);
}
template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
constexpr
T
binomial_coef_check(const T n, const T k)
noexcept
{
return( // NaN check; removed due to MSVC problems; template not being ignored in <int> cases
// (is_nan(n) || is_nan(k)) ? GCLIM<T>::quiet_NaN() :
//
static_cast<T>(binomial_coef_recur(static_cast<ullint_t>(n),static_cast<ullint_t>(k))) );
}
template<typename T1, typename T2, typename TC = common_t<T1,T2>>
constexpr
TC
binomial_coef_type_check(const T1 n, const T2 k)
noexcept
{
return binomial_coef_check(static_cast<TC>(n),static_cast<TC>(k));
}
}
/**
* Compile-time binomial coefficient
*
* @param n integral-valued input.
* @param k integral-valued input.
* @return computes the Binomial coefficient
* \f[ \binom{n}{k} = \frac{n!}{k!(n-k)!} \f]
* also known as '\c n choose \c k '.
*/
template<typename T1, typename T2>
constexpr
common_t<T1,T2>
binomial_coef(const T1 n, const T2 k)
noexcept
{
return internal::binomial_coef_type_check(n,k);
}
#endif

130
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp vendored Normal file
View File

@@ -0,0 +1,130 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_ceil_HPP
#define _gcem_ceil_HPP
namespace internal
{
template<typename T>
constexpr
int
ceil_resid(const T x, const T x_whole)
noexcept
{
return( (x > T(0)) && (x > x_whole) );
}
template<typename T>
constexpr
T
ceil_int(const T x, const T x_whole)
noexcept
{
return( x_whole + static_cast<T>(ceil_resid(x,x_whole)) );
}
template<typename T>
constexpr
T
ceil_check_internal(const T x)
noexcept
{
return x;
}
template<>
constexpr
float
ceil_check_internal<float>(const float x)
noexcept
{
return( abs(x) >= 8388608.f ? \
// if
x : \
// else
ceil_int(x, float(static_cast<int>(x))) );
}
template<>
constexpr
double
ceil_check_internal<double>(const double x)
noexcept
{
return( abs(x) >= 4503599627370496. ? \
// if
x : \
// else
ceil_int(x, double(static_cast<llint_t>(x))) );
}
template<>
constexpr
long double
ceil_check_internal<long double>(const long double x)
noexcept
{
return( abs(x) >= 9223372036854775808.l ? \
// if
x : \
// else
ceil_int(x, ((long double)static_cast<ullint_t>(abs(x))) * sgn(x)) );
}
template<typename T>
constexpr
T
ceil_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// +/- infinite
!is_finite(x) ? \
x :
// signed-zero cases
GCLIM<T>::min() > abs(x) ? \
x :
// else
ceil_check_internal(x) );
}
}
/**
* Compile-time ceil function
*
* @param x a real-valued input.
* @return computes the ceiling-value of the input.
*/
template<typename T>
constexpr
return_t<T>
ceil(const T x)
noexcept
{
return internal::ceil_check( static_cast<return_t<T>>(x) );
}
#endif

41
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp vendored Normal file
View File

@@ -0,0 +1,41 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_copysign_HPP
#define _gcem_copysign_HPP
/**
* Compile-time copy sign function
*
* @param x a real-valued input
* @param y a real-valued input
* @return replace the signbit of \c x with the signbit of \c y.
*/
template <typename T1, typename T2>
constexpr
T1
copysign(const T1 x, const T2 y)
noexcept
{
return( signbit(x) != signbit(y) ? -x : x );
}
#endif

83
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp vendored Normal file
View File

@@ -0,0 +1,83 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time cosine function using tan(x/2)
*/
#ifndef _gcem_cos_HPP
#define _gcem_cos_HPP
namespace internal
{
template<typename T>
constexpr
T
cos_compute(const T x)
noexcept
{
return( T(1) - x*x)/(T(1) + x*x );
}
template<typename T>
constexpr
T
cos_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from 0
GCLIM<T>::min() > abs(x) ?
T(1) :
// special cases: pi/2 and pi
GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
T(0) :
GCLIM<T>::min() > abs(x + T(GCEM_HALF_PI)) ? \
T(0) :
GCLIM<T>::min() > abs(x - T(GCEM_PI)) ? \
- T(1) :
GCLIM<T>::min() > abs(x + T(GCEM_PI)) ? \
- T(1) :
// else
cos_compute( tan(x/T(2)) ) );
}
}
/**
* Compile-time cosine function
*
* @param x a real-valued input.
* @return the cosine function using \f[ \cos(x) = \frac{1-\tan^2(x/2)}{1+\tan^2(x/2)} \f]
*/
template<typename T>
constexpr
return_t<T>
cos(const T x)
noexcept
{
return internal::cos_check( static_cast<return_t<T>>(x) );
}
#endif

65
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp vendored Normal file
View File

@@ -0,0 +1,65 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time hyperbolic cosine function
*/
#ifndef _gcem_cosh_HPP
#define _gcem_cosh_HPP
namespace internal
{
template<typename T>
constexpr
T
cosh_compute(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(1) :
// else
(exp(x) + exp(-x)) / T(2) );
}
}
/**
* Compile-time hyperbolic cosine function
*
* @param x a real-valued input.
* @return the hyperbolic cosine function using \f[ \cosh(x) = \frac{\exp(x) + \exp(-x)}{2} \f]
*/
template<typename T>
constexpr
return_t<T>
cosh(const T x)
noexcept
{
return internal::cosh_compute( static_cast<return_t<T>>(x) );
}
#endif

143
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp vendored Normal file
View File

@@ -0,0 +1,143 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time error function
*/
#ifndef _gcem_erf_HPP
#define _gcem_erf_HPP
namespace internal
{
// see
// http://functions.wolfram.com/GammaBetaErf/Erf/10/01/0007/
template<typename T>
constexpr
T
erf_cf_large_recur(const T x, const int depth)
noexcept
{
return( depth < GCEM_ERF_MAX_ITER ? \
// if
x + 2*depth/erf_cf_large_recur(x,depth+1) :
// else
x );
}
template<typename T>
constexpr
T
erf_cf_large_main(const T x)
noexcept
{
return( T(1) - T(2) * ( exp(-x*x) / T(GCEM_SQRT_PI) ) \
/ erf_cf_large_recur(T(2)*x,1) );
}
// see
// http://functions.wolfram.com/GammaBetaErf/Erf/10/01/0005/
template<typename T>
constexpr
T
erf_cf_small_recur(const T xx, const int depth)
noexcept
{
return( depth < GCEM_ERF_MAX_ITER ? \
// if
(2*depth - T(1)) - 2*xx \
+ 4*depth*xx / erf_cf_small_recur(xx,depth+1) :
// else
(2*depth - T(1)) - 2*xx );
}
template<typename T>
constexpr
T
erf_cf_small_main(const T x)
noexcept
{
return( T(2) * x * ( exp(-x*x) / T(GCEM_SQRT_PI) ) \
/ erf_cf_small_recur(x*x,1) );
}
//
template<typename T>
constexpr
T
erf_begin(const T x)
noexcept
{
return( x > T(2.1) ? \
// if
erf_cf_large_main(x) :
// else
erf_cf_small_main(x) );
}
template<typename T>
constexpr
T
erf_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// +/-Inf
is_posinf(x) ? \
T(1) :
is_neginf(x) ? \
- T(1) :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(0) :
// else
x < T(0) ? \
- erf_begin(-x) :
erf_begin( x) );
}
}
/**
* Compile-time Gaussian error function
*
* @param x a real-valued input.
* @return computes the Gaussian error function
* \f[ \text{erf}(x) = \frac{2}{\sqrt{\pi}} \int_0^x \exp( - t^2) dt \f]
* using a continued fraction representation:
* \f[ \text{erf}(x) = \frac{2x}{\sqrt{\pi}} \exp(-x^2) \dfrac{1}{1 - 2x^2 + \dfrac{4x^2}{3 - 2x^2 + \dfrac{8x^2}{5 - 2x^2 + \dfrac{12x^2}{7 - 2x^2 + \ddots}}}} \f]
*/
template<typename T>
constexpr
return_t<T>
erf(const T x)
noexcept
{
return internal::erf_check( static_cast<return_t<T>>(x) );
}
#endif

264
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp vendored Normal file
View File

@@ -0,0 +1,264 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time inverse error function
*
* Initial approximation based on:
* 'Approximating the erfinv function' by Mike Giles
*/
#ifndef _gcem_erf_inv_HPP
#define _gcem_erf_inv_HPP
namespace internal
{
template<typename T>
constexpr T erf_inv_decision(const T value, const T p, const T direc, const int iter_count) noexcept;
//
// initial value
// two cases: (1) a < 5; and (2) otherwise
template<typename T>
constexpr
T
erf_inv_initial_val_coef_2(const T a, const T p_term, const int order)
noexcept
{
return( order == 1 ? T(-0.000200214257L) :
order == 2 ? T( 0.000100950558L) + a*p_term :
order == 3 ? T( 0.00134934322L) + a*p_term :
order == 4 ? T(-0.003673428440L) + a*p_term :
order == 5 ? T( 0.005739507730L) + a*p_term :
order == 6 ? T(-0.00762246130L) + a*p_term :
order == 7 ? T( 0.009438870470L) + a*p_term :
order == 8 ? T( 1.001674060000L) + a*p_term :
order == 9 ? T( 2.83297682000L) + a*p_term :
p_term );
}
template<typename T>
constexpr
T
erf_inv_initial_val_case_2(const T a, const T p_term, const int order)
noexcept
{
return( order == 9 ? \
// if
erf_inv_initial_val_coef_2(a,p_term,order) :
// else
erf_inv_initial_val_case_2(a,erf_inv_initial_val_coef_2(a,p_term,order),order+1) );
}
template<typename T>
constexpr
T
erf_inv_initial_val_coef_1(const T a, const T p_term, const int order)
noexcept
{
return( order == 1 ? T( 2.81022636e-08L) :
order == 2 ? T( 3.43273939e-07L) + a*p_term :
order == 3 ? T(-3.5233877e-06L) + a*p_term :
order == 4 ? T(-4.39150654e-06L) + a*p_term :
order == 5 ? T( 0.00021858087L) + a*p_term :
order == 6 ? T(-0.00125372503L) + a*p_term :
order == 7 ? T(-0.004177681640L) + a*p_term :
order == 8 ? T( 0.24664072700L) + a*p_term :
order == 9 ? T( 1.50140941000L) + a*p_term :
p_term );
}
template<typename T>
constexpr
T
erf_inv_initial_val_case_1(const T a, const T p_term, const int order)
noexcept
{
return( order == 9 ? \
// if
erf_inv_initial_val_coef_1(a,p_term,order) :
// else
erf_inv_initial_val_case_1(a,erf_inv_initial_val_coef_1(a,p_term,order),order+1) );
}
template<typename T>
constexpr
T
erf_inv_initial_val_int(const T a)
noexcept
{
return( a < T(5) ? \
// if
erf_inv_initial_val_case_1(a-T(2.5),T(0),1) :
// else
erf_inv_initial_val_case_2(sqrt(a)-T(3),T(0),1) );
}
template<typename T>
constexpr
T
erf_inv_initial_val(const T x)
noexcept
{
return x*erf_inv_initial_val_int( -log( (T(1) - x)*(T(1) + x) ) );
}
//
// Halley recursion
template<typename T>
constexpr
T
erf_inv_err_val(const T value, const T p)
noexcept
{ // err_val = f(x)
return( erf(value) - p );
}
template<typename T>
constexpr
T
erf_inv_deriv_1(const T value)
noexcept
{ // derivative of the error function w.r.t. x
return( exp( -value*value ) );
}
template<typename T>
constexpr
T
erf_inv_deriv_2(const T value, const T deriv_1)
noexcept
{ // second derivative of the error function w.r.t. x
return( deriv_1*( -T(2)*value ) );
}
template<typename T>
constexpr
T
erf_inv_ratio_val_1(const T value, const T p, const T deriv_1)
noexcept
{
return( erf_inv_err_val(value,p) / deriv_1 );
}
template<typename T>
constexpr
T
erf_inv_ratio_val_2(const T value, const T deriv_1)
noexcept
{
return( erf_inv_deriv_2(value,deriv_1) / deriv_1 );
}
template<typename T>
constexpr
T
erf_inv_halley(const T ratio_val_1, const T ratio_val_2)
noexcept
{
return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
}
template<typename T>
constexpr
T
erf_inv_recur(const T value, const T p, const T deriv_1, const int iter_count)
noexcept
{
return erf_inv_decision( value, p,
erf_inv_halley(erf_inv_ratio_val_1(value,p,deriv_1),
erf_inv_ratio_val_2(value,deriv_1)),
iter_count );
}
template<typename T>
constexpr
T
erf_inv_decision(const T value, const T p, const T direc, const int iter_count)
noexcept
{
return( iter_count < GCEM_ERF_INV_MAX_ITER ? \
// if
erf_inv_recur(value-direc,p, erf_inv_deriv_1(value), iter_count+1) :
// else
value - direc );
}
template<typename T>
constexpr
T
erf_inv_recur_begin(const T initial_val, const T p)
noexcept
{
return erf_inv_recur(initial_val,p,erf_inv_deriv_1(initial_val),1);
}
template<typename T>
constexpr
T
erf_inv_begin(const T p)
noexcept
{
return( // NaN check
is_nan(p) ? \
GCLIM<T>::quiet_NaN() :
// bad values
abs(p) > T(1) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from 1
GCLIM<T>::min() > abs(T(1) - p) ? \
GCLIM<T>::infinity() :
// indistinguishable from - 1
GCLIM<T>::min() > abs(T(1) + p) ? \
- GCLIM<T>::infinity() :
// else
erf_inv_recur_begin(erf_inv_initial_val(p),p) );
}
}
/**
* Compile-time inverse Gaussian error function
*
* @param p a real-valued input with values in the unit-interval.
* @return Computes the inverse Gaussian error function, a value \f$ x \f$ such that
* \f[ f(x) := \text{erf}(x) - p \f]
* is equal to zero, for a given \c p.
* GCE-Math finds this root using Halley's method:
* \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
* where
* \f[ \frac{\partial}{\partial x} \text{erf}(x) = \exp(-x^2), \ \ \frac{\partial^2}{\partial x^2} \text{erf}(x) = -2x\exp(-x^2) \f]
*/
template<typename T>
constexpr
return_t<T>
erf_inv(const T p)
noexcept
{
return internal::erf_inv_begin( static_cast<return_t<T>>(p) );
}
#endif

106
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp vendored Normal file
View File

@@ -0,0 +1,106 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time exponential function
*/
#ifndef _gcem_exp_HPP
#define _gcem_exp_HPP
namespace internal
{
template<typename T>
constexpr
T
exp_cf_recur(const T x, const int depth)
noexcept
{
return( depth < GCEM_EXP_MAX_ITER_SMALL ? \
// if
depth == 1 ? \
T(1) - x/exp_cf_recur(x,depth+1) :
T(1) + x/T(depth - 1) - x/depth/exp_cf_recur(x,depth+1) :
// else
T(1) );
}
template<typename T>
constexpr
T
exp_cf(const T x)
noexcept
{
return( T(1)/exp_cf_recur(x,1) );
}
template<typename T>
constexpr
T
exp_split(const T x)
noexcept
{
return( static_cast<T>(pow_integral(GCEM_E,find_whole(x))) * exp_cf(find_fraction(x)) );
}
template<typename T>
constexpr
T
exp_check(const T x)
noexcept
{
return( is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
//
is_neginf(x) ? \
T(0) :
//
GCLIM<T>::min() > abs(x) ? \
T(1) :
//
is_posinf(x) ? \
GCLIM<T>::infinity() :
//
abs(x) < T(2) ? \
exp_cf(x) : \
exp_split(x) );
}
}
/**
* Compile-time exponential function
*
* @param x a real-valued input.
* @return \f$ \exp(x) \f$ using \f[ \exp(x) = \dfrac{1}{1-\dfrac{x}{1+x-\dfrac{\frac{1}{2}x}{1 + \frac{1}{2}x - \dfrac{\frac{1}{3}x}{1 + \frac{1}{3}x - \ddots}}}} \f]
* The continued fraction argument is split into two parts: \f$ x = n + r \f$, where \f$ n \f$ is an integer and \f$ r \in [-0.5,0.5] \f$.
*/
template<typename T>
constexpr
return_t<T>
exp(const T x)
noexcept
{
return internal::exp_check( static_cast<return_t<T>>(x) );
}
#endif

76
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp vendored Normal file
View File

@@ -0,0 +1,76 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time exponential function
*/
#ifndef _gcem_expm1_HPP
#define _gcem_expm1_HPP
namespace internal
{
template<typename T>
constexpr
T
expm1_compute(const T x)
noexcept
{
// return x * ( T(1) + x * ( T(1)/T(2) + x * ( T(1)/T(6) + x * ( T(1)/T(24) + x/T(120) ) ) ) ); // O(x^6)
return x + x * ( x/T(2) + x * ( x/T(6) + x * ( x/T(24) + x*x/T(120) ) ) ); // O(x^6)
}
template<typename T>
constexpr
T
expm1_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
//
abs(x) > T(1e-04) ? \
// if
exp(x) - T(1) :
// else
expm1_compute(x) );
}
}
/**
* Compile-time exponential-minus-1 function
*
* @param x a real-valued input.
* @return \f$ \exp(x) - 1 \f$ using \f[ \exp(x) = \sum_{k=0}^\infty \dfrac{x^k}{k!} \f]
*/
template<typename T>
constexpr
return_t<T>
expm1(const T x)
noexcept
{
return internal::expm1_check( static_cast<return_t<T>>(x) );
}
#endif

98
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp vendored Normal file
View File

@@ -0,0 +1,98 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time factorial function
*/
#ifndef _gcem_factorial_HPP
#define _gcem_factorial_HPP
namespace internal
{
// T should be int, long int, unsigned int, etc.
template<typename T>
constexpr
T
factorial_table(const T x)
noexcept
{ // table for x! when x = {2,...,16}
return( x == T(2) ? T(2) : x == T(3) ? T(6) :
x == T(4) ? T(24) : x == T(5) ? T(120) :
x == T(6) ? T(720) : x == T(7) ? T(5040) :
x == T(8) ? T(40320) : x == T(9) ? T(362880) :
//
x == T(10) ? T(3628800) :
x == T(11) ? T(39916800) :
x == T(12) ? T(479001600) :
x == T(13) ? T(6227020800) :
x == T(14) ? T(87178291200) :
x == T(15) ? T(1307674368000) :
T(20922789888000) );
}
template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
constexpr
T
factorial_recur(const T x)
noexcept
{
return( x == T(0) ? T(1) :
x == T(1) ? x :
//
x < T(17) ? \
// if
factorial_table(x) :
// else
x*factorial_recur(x-1) );
}
template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
constexpr
T
factorial_recur(const T x)
noexcept
{
return tgamma(x + 1);
}
}
/**
* Compile-time factorial function
*
* @param x a real-valued input.
* @return Computes the factorial value \f$ x! \f$.
* When \c x is an integral type (\c int, <tt>long int</tt>, etc.), a simple recursion method is used, along with table values.
* When \c x is real-valued, <tt>factorial(x) = tgamma(x+1)</tt>.
*/
template<typename T>
constexpr
T
factorial(const T x)
noexcept
{
return internal::factorial_recur(x);
}
#endif

47
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp vendored Normal file
View File

@@ -0,0 +1,47 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time find_exponent function
*/
#ifndef _gcem_find_exponent_HPP
#define _gcem_find_exponent_HPP
namespace internal
{
template<typename T>
constexpr
llint_t
find_exponent(const T x, const llint_t exponent)
noexcept
{
return( x < T(1) ? \
find_exponent(x*T(10),exponent - llint_t(1)) :
x > T(10) ? \
find_exponent(x/T(10),exponent + llint_t(1)) :
// else
exponent );
}
}
#endif

46
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp vendored Normal file
View File

@@ -0,0 +1,46 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* find the fraction part of x = n + r, where -0.5 <= r <= 0.5
*/
#ifndef _gcem_find_fraction_HPP
#define _gcem_find_fraction_HPP
namespace internal
{
template<typename T>
constexpr
T
find_fraction(const T x)
noexcept
{
return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
// if
x - internal::floor_check(x) - sgn(x) :
//else
x - internal::floor_check(x) );
}
}
#endif

46
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp vendored Normal file
View File

@@ -0,0 +1,46 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* find the whole number part of x = n + r, where -0.5 <= r <= 0.5
*/
#ifndef _gcem_find_whole_HPP
#define _gcem_find_whole_HPP
namespace internal
{
template<typename T>
constexpr
llint_t
find_whole(const T x)
noexcept
{
return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
// if
static_cast<llint_t>(internal::floor_check(x) + sgn(x)) :
// else
static_cast<llint_t>(internal::floor_check(x)) );
}
}
#endif

130
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp vendored Normal file
View File

@@ -0,0 +1,130 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_floor_HPP
#define _gcem_floor_HPP
namespace internal
{
template<typename T>
constexpr
int
floor_resid(const T x, const T x_whole)
noexcept
{
return( (x < T(0)) && (x < x_whole) );
}
template<typename T>
constexpr
T
floor_int(const T x, const T x_whole)
noexcept
{
return( x_whole - static_cast<T>(floor_resid(x,x_whole)) );
}
template<typename T>
constexpr
T
floor_check_internal(const T x)
noexcept
{
return x;
}
template<>
constexpr
float
floor_check_internal<float>(const float x)
noexcept
{
return( abs(x) >= 8388608.f ? \
// if
x : \
// else
floor_int(x, float(static_cast<int>(x))) );
}
template<>
constexpr
double
floor_check_internal<double>(const double x)
noexcept
{
return( abs(x) >= 4503599627370496. ? \
// if
x : \
// else
floor_int(x, double(static_cast<llint_t>(x))) );
}
template<>
constexpr
long double
floor_check_internal<long double>(const long double x)
noexcept
{
return( abs(x) >= 9223372036854775808.l ? \
// if
x : \
// else
floor_int(x, ((long double)static_cast<ullint_t>(abs(x))) * sgn(x)) );
}
template<typename T>
constexpr
T
floor_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// +/- infinite
!is_finite(x) ? \
x :
// signed-zero cases
GCLIM<T>::min() > abs(x) ? \
x :
// else
floor_check_internal(x) );
}
}
/**
* Compile-time floor function
*
* @param x a real-valued input.
* @return computes the floor-value of the input.
*/
template<typename T>
constexpr
return_t<T>
floor(const T x)
noexcept
{
return internal::floor_check( static_cast<return_t<T>>(x) );
}
#endif

70
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp vendored Normal file
View File

@@ -0,0 +1,70 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_fmod_HPP
#define _gcem_fmod_HPP
namespace internal
{
template<typename T>
constexpr
T
fmod_check(const T x, const T y)
noexcept
{
return( // NaN check
any_nan(x, y) ? \
GCLIM<T>::quiet_NaN() :
// +/- infinite
!all_finite(x, y) ? \
GCLIM<T>::quiet_NaN() :
// else
x - trunc(x/y)*y );
}
template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
constexpr
TC
fmod_type_check(const T1 x, const T2 y)
noexcept
{
return fmod_check(static_cast<TC>(x),static_cast<TC>(y));
}
}
/**
* Compile-time remainder of division function
* @param x a real-valued input.
* @param y a real-valued input.
* @return computes the floating-point remainder of \f$ x / y \f$ (rounded towards zero) using \f[ \text{fmod}(x,y) = x - \text{trunc}(x/y) \times y \f]
*/
template<typename T1, typename T2>
constexpr
common_return_t<T1,T2>
fmod(const T1 x, const T2 y)
noexcept
{
return internal::fmod_type_check(x,y);
}
#endif

82
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp vendored Normal file
View File

@@ -0,0 +1,82 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_gcd_HPP
#define _gcem_gcd_HPP
namespace internal
{
template<typename T>
constexpr
T
gcd_recur(const T a, const T b)
noexcept
{
return( b == T(0) ? a : gcd_recur(b, a % b) );
}
template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
constexpr
T
gcd_int_check(const T a, const T b)
noexcept
{
return gcd_recur(a,b);
}
template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
constexpr
T
gcd_int_check(const T a, const T b)
noexcept
{
return gcd_recur( static_cast<ullint_t>(a), static_cast<ullint_t>(b) );
}
template<typename T1, typename T2, typename TC = common_t<T1,T2>>
constexpr
TC
gcd_type_check(const T1 a, const T2 b)
noexcept
{
return gcd_int_check( static_cast<TC>(abs(a)), static_cast<TC>(abs(b)) );
}
}
/**
* Compile-time greatest common divisor (GCD) function
*
* @param a integral-valued input.
* @param b integral-valued input.
* @return the greatest common divisor between integers \c a and \c b using a Euclidean algorithm.
*/
template<typename T1, typename T2>
constexpr
common_t<T1,T2>
gcd(const T1 a, const T2 b)
noexcept
{
return internal::gcd_type_check(a,b);
}
#endif

213
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp vendored Normal file
View File

@@ -0,0 +1,213 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#include <cstddef> // size_t
#include <limits>
#include <type_traits>
// undef some functions from math.h
// see issue #29
#ifdef abs
#undef abs
#endif
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
#ifdef round
#undef round
#endif
#ifdef signbit
#undef signbit
#endif
//
// version
#ifndef GCEM_VERSION_MAJOR
#define GCEM_VERSION_MAJOR 1
#endif
#ifndef GCEM_VERSION_MINOR
#define GCEM_VERSION_MINOR 16
#endif
#ifndef GCEM_VERSION_PATCH
#define GCEM_VERSION_PATCH 0
#endif
//
// types
namespace gcem
{
using uint_t = unsigned int;
using ullint_t = unsigned long long int;
using llint_t = long long int;
template<class T>
using GCLIM = std::numeric_limits<T>;
template<typename T>
using return_t = typename std::conditional<std::is_integral<T>::value,double,T>::type;
template<typename ...T>
using common_t = typename std::common_type<T...>::type;
template<typename ...T>
using common_return_t = return_t<common_t<T...>>;
}
//
// constants
#ifndef GCEM_LOG_2
#define GCEM_LOG_2 0.6931471805599453094172321214581765680755L
#endif
#ifndef GCEM_LOG_10
#define GCEM_LOG_10 2.3025850929940456840179914546843642076011L
#endif
#ifndef GCEM_PI
#define GCEM_PI 3.1415926535897932384626433832795028841972L
#endif
#ifndef GCEM_LOG_PI
#define GCEM_LOG_PI 1.1447298858494001741434273513530587116473L
#endif
#ifndef GCEM_LOG_2PI
#define GCEM_LOG_2PI 1.8378770664093454835606594728112352797228L
#endif
#ifndef GCEM_LOG_SQRT_2PI
#define GCEM_LOG_SQRT_2PI 0.9189385332046727417803297364056176398614L
#endif
#ifndef GCEM_SQRT_2
#define GCEM_SQRT_2 1.4142135623730950488016887242096980785697L
#endif
#ifndef GCEM_HALF_PI
#define GCEM_HALF_PI 1.5707963267948966192313216916397514420986L
#endif
#ifndef GCEM_SQRT_PI
#define GCEM_SQRT_PI 1.7724538509055160272981674833411451827975L
#endif
#ifndef GCEM_SQRT_HALF_PI
#define GCEM_SQRT_HALF_PI 1.2533141373155002512078826424055226265035L
#endif
#ifndef GCEM_E
#define GCEM_E 2.7182818284590452353602874713526624977572L
#endif
//
// convergence settings
#ifndef GCEM_ERF_MAX_ITER
#define GCEM_ERF_MAX_ITER 60
#endif
#ifndef GCEM_ERF_INV_MAX_ITER
#define GCEM_ERF_INV_MAX_ITER 60
#endif
#ifndef GCEM_EXP_MAX_ITER_SMALL
#define GCEM_EXP_MAX_ITER_SMALL 25
#endif
// #ifndef GCEM_LOG_TOL
// #define GCEM_LOG_TOL 1E-14
// #endif
#ifndef GCEM_LOG_MAX_ITER_SMALL
#define GCEM_LOG_MAX_ITER_SMALL 25
#endif
#ifndef GCEM_LOG_MAX_ITER_BIG
#define GCEM_LOG_MAX_ITER_BIG 255
#endif
#ifndef GCEM_INCML_BETA_TOL
#define GCEM_INCML_BETA_TOL 1E-15
#endif
#ifndef GCEM_INCML_BETA_MAX_ITER
#define GCEM_INCML_BETA_MAX_ITER 205
#endif
#ifndef GCEM_INCML_BETA_INV_MAX_ITER
#define GCEM_INCML_BETA_INV_MAX_ITER 35
#endif
#ifndef GCEM_INCML_GAMMA_MAX_ITER
#define GCEM_INCML_GAMMA_MAX_ITER 55
#endif
#ifndef GCEM_INCML_GAMMA_INV_MAX_ITER
#define GCEM_INCML_GAMMA_INV_MAX_ITER 35
#endif
#ifndef GCEM_SQRT_MAX_ITER
#define GCEM_SQRT_MAX_ITER 100
#endif
#ifndef GCEM_INV_SQRT_MAX_ITER
#define GCEM_INV_SQRT_MAX_ITER 100
#endif
#ifndef GCEM_TAN_MAX_ITER
#define GCEM_TAN_MAX_ITER 35
#endif
#ifndef GCEM_TANH_MAX_ITER
#define GCEM_TANH_MAX_ITER 35
#endif
//
// Macros
#ifdef _MSC_VER
#ifndef GCEM_SIGNBIT
#define GCEM_SIGNBIT(x) _signbit(x)
#endif
#ifndef GCEM_COPYSIGN
#define GCEM_COPYSIGN(x,y) _copysign(x,y)
#endif
#else
#ifndef GCEM_SIGNBIT
#define GCEM_SIGNBIT(x) __builtin_signbit(x)
#endif
#ifndef GCEM_COPYSIGN
#define GCEM_COPYSIGN(x,y) __builtin_copysign(x,y)
#endif
#endif

90
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp vendored Normal file
View File

@@ -0,0 +1,90 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time Pythagorean addition function
*/
// see: https://en.wikipedia.org/wiki/Pythagorean_addition
#ifndef _gcem_hypot_HPP
#define _gcem_hypot_HPP
namespace internal
{
template<typename T>
constexpr
T
hypot_compute(const T x, const T ydx)
noexcept
{
return abs(x) * sqrt( T(1) + (ydx * ydx) );
}
template<typename T>
constexpr
T
hypot_vals_check(const T x, const T y)
noexcept
{
return( any_nan(x, y) ? \
GCLIM<T>::quiet_NaN() :
//
any_inf(x,y) ? \
GCLIM<T>::infinity() :
// indistinguishable from zero or one
GCLIM<T>::min() > abs(x) ? \
abs(y) :
GCLIM<T>::min() > abs(y) ? \
abs(x) :
// else
hypot_compute(x, y/x) );
}
template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
constexpr
TC
hypot_type_check(const T1 x, const T2 y)
noexcept
{
return hypot_vals_check(static_cast<TC>(x),static_cast<TC>(y));
}
}
/**
* Compile-time Pythagorean addition function
*
* @param x a real-valued input.
* @param y a real-valued input.
* @return Computes \f$ x \oplus y = \sqrt{x^2 + y^2} \f$.
*/
template<typename T1, typename T2>
constexpr
common_return_t<T1,T2>
hypot(const T1 x, const T2 y)
noexcept
{
return internal::hypot_type_check(x,y);
}
#endif

194
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp vendored Normal file
View File

@@ -0,0 +1,194 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time incomplete beta function
*
* see eq. 18.5.17a in the Handbook of Continued Fractions for Special Functions
*/
#ifndef _gcem_incomplete_beta_HPP
#define _gcem_incomplete_beta_HPP
namespace internal
{
template<typename T>
constexpr T incomplete_beta_cf(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth) noexcept;
//
// coefficients; see eq. 18.5.17b
template<typename T>
constexpr
T
incomplete_beta_coef_even(const T a, const T b, const T z, const int k)
noexcept
{
return( -z*(a + k)*(a + b + k)/( (a + 2*k)*(a + 2*k + T(1)) ) );
}
template<typename T>
constexpr
T
incomplete_beta_coef_odd(const T a, const T b, const T z, const int k)
noexcept
{
return( z*k*(b - k)/((a + 2*k - T(1))*(a + 2*k)) );
}
template<typename T>
constexpr
T
incomplete_beta_coef(const T a, const T b, const T z, const int depth)
noexcept
{
return( !is_odd(depth) ? incomplete_beta_coef_even(a,b,z,depth/2) :
incomplete_beta_coef_odd(a,b,z,(depth+1)/2) );
}
//
// update formulae for the modified Lentz method
template<typename T>
constexpr
T
incomplete_beta_c_update(const T a, const T b, const T z, const T c_j, const int depth)
noexcept
{
return( T(1) + incomplete_beta_coef(a,b,z,depth)/c_j );
}
template<typename T>
constexpr
T
incomplete_beta_d_update(const T a, const T b, const T z, const T d_j, const int depth)
noexcept
{
return( T(1) / (T(1) + incomplete_beta_coef(a,b,z,depth)*d_j) );
}
//
// convergence-type condition
template<typename T>
constexpr
T
incomplete_beta_decision(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth)
noexcept
{
return( // tolerance check
abs(c_j*d_j - T(1)) < GCEM_INCML_BETA_TOL ? f_j*c_j*d_j :
// max_iter check
depth < GCEM_INCML_BETA_MAX_ITER ? \
// if
incomplete_beta_cf(a,b,z,c_j,d_j,f_j*c_j*d_j,depth+1) :
// else
f_j*c_j*d_j );
}
template<typename T>
constexpr
T
incomplete_beta_cf(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth)
noexcept
{
return incomplete_beta_decision(a,b,z,
incomplete_beta_c_update(a,b,z,c_j,depth),
incomplete_beta_d_update(a,b,z,d_j,depth),
f_j,depth);
}
//
// x^a (1-x)^{b} / (a beta(a,b)) * cf
template<typename T>
constexpr
T
incomplete_beta_begin(const T a, const T b, const T z)
noexcept
{
return ( (exp(a*log(z) + b*log(T(1)-z) - lbeta(a,b)) / a) * \
incomplete_beta_cf(a,b,z,T(1),
incomplete_beta_d_update(a,b,z,T(1),0),
incomplete_beta_d_update(a,b,z,T(1),0),1)
);
}
template<typename T>
constexpr
T
incomplete_beta_check(const T a, const T b, const T z)
noexcept
{
return( // NaN check
any_nan(a, b, z) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero
GCLIM<T>::min() > z ? \
T(0) :
// parameter check for performance
(a + T(1))/(a + b + T(2)) > z ? \
incomplete_beta_begin(a,b,z) :
T(1) - incomplete_beta_begin(b,a,T(1) - z) );
}
template<typename T1, typename T2, typename T3, typename TC = common_return_t<T1,T2,T3>>
constexpr
TC
incomplete_beta_type_check(const T1 a, const T2 b, const T3 p)
noexcept
{
return incomplete_beta_check(static_cast<TC>(a),
static_cast<TC>(b),
static_cast<TC>(p));
}
}
/**
* Compile-time regularized incomplete beta function
*
* @param a a real-valued, non-negative input.
* @param b a real-valued, non-negative input.
* @param z a real-valued, non-negative input.
*
* @return computes the regularized incomplete beta function,
* \f[ \frac{\text{B}(z;\alpha,\beta)}{\text{B}(\alpha,\beta)} = \frac{1}{\text{B}(\alpha,\beta)}\int_0^z t^{a-1} (1-t)^{\beta-1} dt \f]
* using a continued fraction representation, found in the Handbook of Continued Fractions for Special Functions, and a modified Lentz method.
* \f[ \frac{\text{B}(z;\alpha,\beta)}{\text{B}(\alpha,\beta)} = \frac{z^{\alpha} (1-t)^{\beta}}{\alpha \text{B}(\alpha,\beta)} \dfrac{a_1}{1 + \dfrac{a_2}{1 + \dfrac{a_3}{1 + \dfrac{a_4}{1 + \ddots}}}} \f]
* where \f$ a_1 = 1 \f$ and
* \f[ a_{2m+2} = - \frac{(\alpha + m)(\alpha + \beta + m)}{(\alpha + 2m)(\alpha + 2m + 1)}, \ m \geq 0 \f]
* \f[ a_{2m+1} = \frac{m(\beta - m)}{(\alpha + 2m - 1)(\alpha + 2m)}, \ m \geq 1 \f]
* The Lentz method works as follows: let \f$ f_j \f$ denote the value of the continued fraction up to the first \f$ j \f$ terms; \f$ f_j \f$ is updated as follows:
* \f[ c_j = 1 + a_j / c_{j-1}, \ \ d_j = 1 / (1 + a_j d_{j-1}) \f]
* \f[ f_j = c_j d_j f_{j-1} \f]
*/
template<typename T1, typename T2, typename T3>
constexpr
common_return_t<T1,T2,T3>
incomplete_beta(const T1 a, const T2 b, const T3 z)
noexcept
{
return internal::incomplete_beta_type_check(a,b,z);
}
#endif

352
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp vendored Normal file
View File

@@ -0,0 +1,352 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* inverse of the incomplete beta function
*/
#ifndef _gcem_incomplete_beta_inv_HPP
#define _gcem_incomplete_beta_inv_HPP
namespace internal
{
template<typename T>
constexpr T incomplete_beta_inv_decision(const T value, const T alpha_par, const T beta_par, const T p,
const T direc, const T lb_val, const int iter_count) noexcept;
//
// initial value for Halley
//
// a,b > 1 case
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_1_tval(const T p)
noexcept
{ // a > 1.0
return( p > T(0.5) ? \
// if
sqrt(-T(2)*log(T(1) - p)) :
// else
sqrt(-T(2)*log(p)) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_1_int_begin(const T t_val)
noexcept
{ // internal for a > 1.0
return( t_val - ( T(2.515517) + T(0.802853)*t_val + T(0.010328)*t_val*t_val ) \
/ ( T(1) + T(1.432788)*t_val + T(0.189269)*t_val*t_val + T(0.001308)*t_val*t_val*t_val ) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_1_int_ab1(const T alpha_par, const T beta_par)
noexcept
{
return( T(1)/(2*alpha_par - T(1)) + T(1)/(2*beta_par - T(1)) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_1_int_ab2(const T alpha_par, const T beta_par)
noexcept
{
return( T(1)/(2*beta_par - T(1)) - T(1)/(2*alpha_par - T(1)) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_1_int_h(const T ab_term_1)
noexcept
{
return( T(2) / ab_term_1 );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_1_int_w(const T value, const T ab_term_2, const T h_term)
noexcept
{
// return( value * sqrt(h_term + lambda)/h_term - ab_term_2*(lambda + 5.0/6.0 -2.0/(3.0*h_term)) );
return( value * sqrt(h_term + (value*value - T(3))/T(6))/h_term \
- ab_term_2*((value*value - T(3))/T(6) + T(5)/T(6) - T(2)/(T(3)*h_term)) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_1_int_end(const T alpha_par, const T beta_par, const T w_term)
noexcept
{
return( alpha_par / (alpha_par + beta_par*exp(2*w_term)) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_1(const T alpha_par, const T beta_par, const T t_val, const T sgn_term)
noexcept
{ // a > 1.0
return incomplete_beta_inv_initial_val_1_int_end( alpha_par, beta_par,
incomplete_beta_inv_initial_val_1_int_w(
sgn_term*incomplete_beta_inv_initial_val_1_int_begin(t_val),
incomplete_beta_inv_initial_val_1_int_ab2(alpha_par,beta_par),
incomplete_beta_inv_initial_val_1_int_h(
incomplete_beta_inv_initial_val_1_int_ab1(alpha_par,beta_par)
)
)
);
}
//
// a,b else
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_2_s1(const T alpha_par, const T beta_par)
noexcept
{
return( pow(alpha_par/(alpha_par+beta_par),alpha_par) / alpha_par );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_2_s2(const T alpha_par, const T beta_par)
noexcept
{
return( pow(beta_par/(alpha_par+beta_par),beta_par) / beta_par );
}
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val_2(const T alpha_par, const T beta_par, const T p, const T s_1, const T s_2)
noexcept
{
return( p <= s_1/(s_1 + s_2) ? pow(p*(s_1+s_2)*alpha_par,T(1)/alpha_par) :
T(1) - pow(p*(s_1+s_2)*beta_par,T(1)/beta_par) );
}
// initial value
template<typename T>
constexpr
T
incomplete_beta_inv_initial_val(const T alpha_par, const T beta_par, const T p)
noexcept
{
return( (alpha_par > T(1) && beta_par > T(1)) ?
// if
incomplete_beta_inv_initial_val_1(alpha_par,beta_par,
incomplete_beta_inv_initial_val_1_tval(p),
p < T(0.5) ? T(1) : T(-1) ) :
// else
p > T(0.5) ?
// if
T(1) - incomplete_beta_inv_initial_val_2(beta_par,alpha_par,T(1) - p,
incomplete_beta_inv_initial_val_2_s1(beta_par,alpha_par),
incomplete_beta_inv_initial_val_2_s2(beta_par,alpha_par)) :
// else
incomplete_beta_inv_initial_val_2(alpha_par,beta_par,p,
incomplete_beta_inv_initial_val_2_s1(alpha_par,beta_par),
incomplete_beta_inv_initial_val_2_s2(alpha_par,beta_par))
);
}
//
// Halley recursion
template<typename T>
constexpr
T
incomplete_beta_inv_err_val(const T value, const T alpha_par, const T beta_par, const T p)
noexcept
{ // err_val = f(x)
return( incomplete_beta(alpha_par,beta_par,value) - p );
}
template<typename T>
constexpr
T
incomplete_beta_inv_deriv_1(const T value, const T alpha_par, const T beta_par, const T lb_val)
noexcept
{ // derivative of the incomplete beta function w.r.t. x
return( // indistinguishable from zero or one
GCLIM<T>::min() > abs(value) ? \
T(0) :
GCLIM<T>::min() > abs(T(1) - value) ? \
T(0) :
// else
exp( (alpha_par - T(1))*log(value) + (beta_par - T(1))*log(T(1) - value) - lb_val ) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_deriv_2(const T value, const T alpha_par, const T beta_par, const T deriv_1)
noexcept
{ // second derivative of the incomplete beta function w.r.t. x
return( deriv_1*((alpha_par - T(1))/value - (beta_par - T(1))/(T(1) - value)) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_ratio_val_1(const T value, const T alpha_par, const T beta_par, const T p, const T deriv_1)
noexcept
{
return( incomplete_beta_inv_err_val(value,alpha_par,beta_par,p) / deriv_1 );
}
template<typename T>
constexpr
T
incomplete_beta_inv_ratio_val_2(const T value, const T alpha_par, const T beta_par, const T deriv_1)
noexcept
{
return( incomplete_beta_inv_deriv_2(value,alpha_par,beta_par,deriv_1) / deriv_1 );
}
template<typename T>
constexpr
T
incomplete_beta_inv_halley(const T ratio_val_1, const T ratio_val_2)
noexcept
{
return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_recur(const T value, const T alpha_par, const T beta_par, const T p, const T deriv_1,
const T lb_val, const int iter_count)
noexcept
{
return( // derivative = 0
GCLIM<T>::min() > abs(deriv_1) ? \
incomplete_beta_inv_decision( value, alpha_par, beta_par, p, T(0), lb_val,
GCEM_INCML_BETA_INV_MAX_ITER+1) :
// else
incomplete_beta_inv_decision( value, alpha_par, beta_par, p,
incomplete_beta_inv_halley(
incomplete_beta_inv_ratio_val_1(value,alpha_par,beta_par,p,deriv_1),
incomplete_beta_inv_ratio_val_2(value,alpha_par,beta_par,deriv_1)
), lb_val, iter_count) );
}
template<typename T>
constexpr
T
incomplete_beta_inv_decision(const T value, const T alpha_par, const T beta_par, const T p, const T direc,
const T lb_val, const int iter_count)
noexcept
{
return( iter_count <= GCEM_INCML_BETA_INV_MAX_ITER ?
// if
incomplete_beta_inv_recur(value-direc,alpha_par,beta_par,p,
incomplete_beta_inv_deriv_1(value,alpha_par,beta_par,lb_val),
lb_val, iter_count+1) :
// else
value - direc );
}
template<typename T>
constexpr
T
incomplete_beta_inv_begin(const T initial_val, const T alpha_par, const T beta_par, const T p, const T lb_val)
noexcept
{
return incomplete_beta_inv_recur(initial_val,alpha_par,beta_par,p,
incomplete_beta_inv_deriv_1(initial_val,alpha_par,beta_par,lb_val),
lb_val,1);
}
template<typename T>
constexpr
T
incomplete_beta_inv_check(const T alpha_par, const T beta_par, const T p)
noexcept
{
return( // NaN check
any_nan(alpha_par, beta_par, p) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero or one
GCLIM<T>::min() > p ? \
T(0) :
GCLIM<T>::min() > abs(T(1) - p) ? \
T(1) :
// else
incomplete_beta_inv_begin(incomplete_beta_inv_initial_val(alpha_par,beta_par,p),
alpha_par,beta_par,p,lbeta(alpha_par,beta_par)) );
}
template<typename T1, typename T2, typename T3, typename TC = common_t<T1,T2,T3>>
constexpr
TC
incomplete_beta_inv_type_check(const T1 a, const T2 b, const T3 p)
noexcept
{
return incomplete_beta_inv_check(static_cast<TC>(a),
static_cast<TC>(b),
static_cast<TC>(p));
}
}
/**
* Compile-time inverse incomplete beta function
*
* @param a a real-valued, non-negative input.
* @param b a real-valued, non-negative input.
* @param p a real-valued input with values in the unit-interval.
*
* @return Computes the inverse incomplete beta function, a value \f$ x \f$ such that
* \f[ f(x) := \frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)} - p \f]
* equal to zero, for a given \c p.
* GCE-Math finds this root using Halley's method:
* \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
* where
* \f[ \frac{\partial}{\partial x} \left(\frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)}\right) = \frac{1}{\text{B}(\alpha,\beta)} x^{\alpha-1} (1-x)^{\beta-1} \f]
* \f[ \frac{\partial^2}{\partial x^2} \left(\frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)}\right) = \frac{1}{\text{B}(\alpha,\beta)} x^{\alpha-1} (1-x)^{\beta-1} \left( \frac{\alpha-1}{x} - \frac{\beta-1}{1 - x} \right) \f]
*/
template<typename T1, typename T2, typename T3>
constexpr
common_t<T1,T2,T3>
incomplete_beta_inv(const T1 a, const T2 b, const T3 p)
noexcept
{
return internal::incomplete_beta_inv_type_check(a,b,p);
}
#endif

247
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp vendored Normal file
View File

@@ -0,0 +1,247 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time (regularized) incomplete gamma function
*/
#ifndef _gcem_incomplete_gamma_HPP
#define _gcem_incomplete_gamma_HPP
namespace internal
{
// 50 point Gauss-Legendre quadrature
template<typename T>
constexpr
T
incomplete_gamma_quad_inp_vals(const T lb, const T ub, const int counter)
noexcept
{
return (ub-lb) * gauss_legendre_50_points[counter] / T(2) + (ub + lb) / T(2);
}
template<typename T>
constexpr
T
incomplete_gamma_quad_weight_vals(const T lb, const T ub, const int counter)
noexcept
{
return (ub-lb) * gauss_legendre_50_weights[counter] / T(2);
}
template<typename T>
constexpr
T
incomplete_gamma_quad_fn(const T x, const T a, const T lg_term)
noexcept
{
return exp( -x + (a-T(1))*log(x) - lg_term );
}
template<typename T>
constexpr
T
incomplete_gamma_quad_recur(const T lb, const T ub, const T a, const T lg_term, const int counter)
noexcept
{
return( counter < 49 ? \
// if
incomplete_gamma_quad_fn(incomplete_gamma_quad_inp_vals(lb,ub,counter),a,lg_term) \
* incomplete_gamma_quad_weight_vals(lb,ub,counter) \
+ incomplete_gamma_quad_recur(lb,ub,a,lg_term,counter+1) :
// else
incomplete_gamma_quad_fn(incomplete_gamma_quad_inp_vals(lb,ub,counter),a,lg_term) \
* incomplete_gamma_quad_weight_vals(lb,ub,counter) );
}
template<typename T>
constexpr
T
incomplete_gamma_quad_lb(const T a, const T z)
noexcept
{
return( a > T(1000) ? max(T(0),min(z,a) - 11*sqrt(a)) : // break integration into ranges
a > T(800) ? max(T(0),min(z,a) - 11*sqrt(a)) :
a > T(500) ? max(T(0),min(z,a) - 10*sqrt(a)) :
a > T(300) ? max(T(0),min(z,a) - 10*sqrt(a)) :
a > T(100) ? max(T(0),min(z,a) - 9*sqrt(a)) :
a > T(90) ? max(T(0),min(z,a) - 9*sqrt(a)) :
a > T(70) ? max(T(0),min(z,a) - 8*sqrt(a)) :
a > T(50) ? max(T(0),min(z,a) - 7*sqrt(a)) :
a > T(40) ? max(T(0),min(z,a) - 6*sqrt(a)) :
a > T(30) ? max(T(0),min(z,a) - 5*sqrt(a)) :
// else
max(T(0),min(z,a)-4*sqrt(a)) );
}
template<typename T>
constexpr
T
incomplete_gamma_quad_ub(const T a, const T z)
noexcept
{
return( a > T(1000) ? min(z, a + 10*sqrt(a)) :
a > T(800) ? min(z, a + 10*sqrt(a)) :
a > T(500) ? min(z, a + 9*sqrt(a)) :
a > T(300) ? min(z, a + 9*sqrt(a)) :
a > T(100) ? min(z, a + 8*sqrt(a)) :
a > T(90) ? min(z, a + 8*sqrt(a)) :
a > T(70) ? min(z, a + 7*sqrt(a)) :
a > T(50) ? min(z, a + 6*sqrt(a)) :
// else
min(z, a + 5*sqrt(a)) );
}
template<typename T>
constexpr
T
incomplete_gamma_quad(const T a, const T z)
noexcept
{
return incomplete_gamma_quad_recur(incomplete_gamma_quad_lb(a,z), incomplete_gamma_quad_ub(a,z), a,lgamma(a),0);
}
// reverse cf expansion
// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/
template<typename T>
constexpr
T
incomplete_gamma_cf_2_recur(const T a, const T z, const int depth)
noexcept
{
return( depth < 100 ? \
// if
(1 + (depth-1)*2 - a + z) + depth*(a - depth)/incomplete_gamma_cf_2_recur(a,z,depth+1) :
// else
(1 + (depth-1)*2 - a + z) );
}
template<typename T>
constexpr
T
incomplete_gamma_cf_2(const T a, const T z)
noexcept
{ // lower (regularized) incomplete gamma function
return( T(1.0) - exp(a*log(z) - z - lgamma(a)) / incomplete_gamma_cf_2_recur(a,z,1) );
}
// cf expansion
// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/
template<typename T>
constexpr
T
incomplete_gamma_cf_1_coef(const T a, const T z, const int depth)
noexcept
{
return( is_odd(depth) ? - (a - 1 + T(depth+1)/T(2)) * z : T(depth)/T(2) * z );
}
template<typename T>
constexpr
T
incomplete_gamma_cf_1_recur(const T a, const T z, const int depth)
noexcept
{
return( depth < GCEM_INCML_GAMMA_MAX_ITER ? \
// if
(a + depth - 1) + incomplete_gamma_cf_1_coef(a,z,depth)/incomplete_gamma_cf_1_recur(a,z,depth+1) :
// else
(a + depth - 1) );
}
template<typename T>
constexpr
T
incomplete_gamma_cf_1(const T a, const T z)
noexcept
{ // lower (regularized) incomplete gamma function
return( exp(a*log(z) - z - lgamma(a)) / incomplete_gamma_cf_1_recur(a,z,1) );
}
//
template<typename T>
constexpr
T
incomplete_gamma_check(const T a, const T z)
noexcept
{
return( // NaN check
any_nan(a, z) ? \
GCLIM<T>::quiet_NaN() :
//
a < T(0) ? \
GCLIM<T>::quiet_NaN() :
//
GCLIM<T>::min() > z ? \
T(0) :
//
GCLIM<T>::min() > a ? \
T(1) :
// cf or quadrature
(a < T(10)) && (z - a < T(10)) ?
incomplete_gamma_cf_1(a,z) :
(a < T(10)) || (z/a > T(3)) ?
incomplete_gamma_cf_2(a,z) :
// else
incomplete_gamma_quad(a,z) );
}
template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
constexpr
TC
incomplete_gamma_type_check(const T1 a, const T2 p)
noexcept
{
return incomplete_gamma_check(static_cast<TC>(a),
static_cast<TC>(p));
}
}
/**
* Compile-time regularized lower incomplete gamma function
*
* @param a a real-valued, non-negative input.
* @param x a real-valued, non-negative input.
*
* @return the regularized lower incomplete gamma function evaluated at (\c a, \c x),
* \f[ \frac{\gamma(a,x)}{\Gamma(a)} = \frac{1}{\Gamma(a)} \int_0^x t^{a-1} \exp(-t) dt \f]
* When \c a is not too large, the value is computed using the continued fraction representation of the upper incomplete gamma function, \f$ \Gamma(a,x) \f$, using
* \f[ \Gamma(a,x) = \Gamma(a) - \dfrac{x^a\exp(-x)}{a - \dfrac{ax}{a + 1 + \dfrac{x}{a + 2 - \dfrac{(a+1)x}{a + 3 + \dfrac{2x}{a + 4 - \ddots}}}}} \f]
* where \f$ \gamma(a,x) \f$ and \f$ \Gamma(a,x) \f$ are connected via
* \f[ \frac{\gamma(a,x)}{\Gamma(a)} + \frac{\Gamma(a,x)}{\Gamma(a)} = 1 \f]
* When \f$ a > 10 \f$, a 50-point Gauss-Legendre quadrature scheme is employed.
*/
template<typename T1, typename T2>
constexpr
common_return_t<T1,T2>
incomplete_gamma(const T1 a, const T2 x)
noexcept
{
return internal::incomplete_gamma_type_check(a,x);
}
#endif

271
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp vendored Normal file
View File

@@ -0,0 +1,271 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* inverse of the incomplete gamma function
*/
#ifndef _gcem_incomplete_gamma_inv_HPP
#define _gcem_incomplete_gamma_inv_HPP
namespace internal
{
template<typename T>
constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, const T direc, const T lg_val, const int iter_count) noexcept;
//
// initial value for Halley
template<typename T>
constexpr
T
incomplete_gamma_inv_t_val_1(const T p)
noexcept
{ // a > 1.0
return( p > T(0.5) ? sqrt(-2*log(T(1) - p)) : sqrt(-2*log(p)) );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_t_val_2(const T a)
noexcept
{ // a <= 1.0
return( T(1) - T(0.253) * a - T(0.12) * a*a );
}
//
template<typename T>
constexpr
T
incomplete_gamma_inv_initial_val_1_int_begin(const T t_val)
noexcept
{ // internal for a > 1.0
return( t_val - (T(2.515517L) + T(0.802853L)*t_val + T(0.010328L)*t_val*t_val) \
/ (T(1) + T(1.432788L)*t_val + T(0.189269L)*t_val*t_val + T(0.001308L)*t_val*t_val*t_val) );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_initial_val_1_int_end(const T value_inp, const T a)
noexcept
{ // internal for a > 1.0
return max( T(1E-04), a*pow(T(1) - T(1)/(9*a) - value_inp/(3*sqrt(a)), 3) );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_initial_val_1(const T a, const T t_val, const T sgn_term)
noexcept
{ // a > 1.0
return incomplete_gamma_inv_initial_val_1_int_end(sgn_term*incomplete_gamma_inv_initial_val_1_int_begin(t_val), a);
}
template<typename T>
constexpr
T
incomplete_gamma_inv_initial_val_2(const T a, const T p, const T t_val)
noexcept
{ // a <= 1.0
return( p < t_val ? \
// if
pow(p/t_val,T(1)/a) :
// else
T(1) - log(T(1) - (p - t_val)/(T(1) - t_val)) );
}
// initial value
template<typename T>
constexpr
T
incomplete_gamma_inv_initial_val(const T a, const T p)
noexcept
{
return( a > T(1) ? \
// if
incomplete_gamma_inv_initial_val_1(a,
incomplete_gamma_inv_t_val_1(p),
p > T(0.5) ? T(-1) : T(1)) :
// else
incomplete_gamma_inv_initial_val_2(a,p,
incomplete_gamma_inv_t_val_2(a)));
}
//
// Halley recursion
template<typename T>
constexpr
T
incomplete_gamma_inv_err_val(const T value, const T a, const T p)
noexcept
{ // err_val = f(x)
return( incomplete_gamma(a,value) - p );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_deriv_1(const T value, const T a, const T lg_val)
noexcept
{ // derivative of the incomplete gamma function w.r.t. x
return( exp( - value + (a - T(1))*log(value) - lg_val ) );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_deriv_2(const T value, const T a, const T deriv_1)
noexcept
{ // second derivative of the incomplete gamma function w.r.t. x
return( deriv_1*((a - T(1))/value - T(1)) );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_ratio_val_1(const T value, const T a, const T p, const T deriv_1)
noexcept
{
return( incomplete_gamma_inv_err_val(value,a,p) / deriv_1 );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_ratio_val_2(const T value, const T a, const T deriv_1)
noexcept
{
return( incomplete_gamma_inv_deriv_2(value,a,deriv_1) / deriv_1 );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_halley(const T ratio_val_1, const T ratio_val_2)
noexcept
{
return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_recur(const T value, const T a, const T p, const T deriv_1, const T lg_val, const int iter_count)
noexcept
{
return incomplete_gamma_inv_decision(value, a, p,
incomplete_gamma_inv_halley(incomplete_gamma_inv_ratio_val_1(value,a,p,deriv_1),
incomplete_gamma_inv_ratio_val_2(value,a,deriv_1)),
lg_val, iter_count);
}
template<typename T>
constexpr
T
incomplete_gamma_inv_decision(const T value, const T a, const T p, const T direc, const T lg_val, const int iter_count)
noexcept
{
// return( abs(direc) > GCEM_INCML_GAMMA_INV_TOL ? incomplete_gamma_inv_recur(value - direc, a, p, incomplete_gamma_inv_deriv_1(value,a,lg_val), lg_val) : value - direc );
return( iter_count <= GCEM_INCML_GAMMA_INV_MAX_ITER ? \
// if
incomplete_gamma_inv_recur(value-direc,a,p,
incomplete_gamma_inv_deriv_1(value,a,lg_val),
lg_val,iter_count+1) :
// else
value - direc );
}
template<typename T>
constexpr
T
incomplete_gamma_inv_begin(const T initial_val, const T a, const T p, const T lg_val)
noexcept
{
return incomplete_gamma_inv_recur(initial_val,a,p,
incomplete_gamma_inv_deriv_1(initial_val,a,lg_val), lg_val,1);
}
template<typename T>
constexpr
T
incomplete_gamma_inv_check(const T a, const T p)
noexcept
{
return( // NaN check
any_nan(a, p) ? \
GCLIM<T>::quiet_NaN() :
//
GCLIM<T>::min() > p ? \
T(0) :
p > T(1) ? \
GCLIM<T>::quiet_NaN() :
GCLIM<T>::min() > abs(T(1) - p) ? \
GCLIM<T>::infinity() :
//
GCLIM<T>::min() > a ? \
T(0) :
// else
incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a,p),a,p,lgamma(a)) );
}
template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
constexpr
TC
incomplete_gamma_inv_type_check(const T1 a, const T2 p)
noexcept
{
return incomplete_gamma_inv_check(static_cast<TC>(a),
static_cast<TC>(p));
}
}
/**
* Compile-time inverse incomplete gamma function
*
* @param a a real-valued, non-negative input.
* @param p a real-valued input with values in the unit-interval.
*
* @return Computes the inverse incomplete gamma function, a value \f$ x \f$ such that
* \f[ f(x) := \frac{\gamma(a,x)}{\Gamma(a)} - p \f]
* equal to zero, for a given \c p.
* GCE-Math finds this root using Halley's method:
* \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
* where
* \f[ \frac{\partial}{\partial x} \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} \exp(-x) \f]
* \f[ \frac{\partial^2}{\partial x^2} \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} \exp(-x) \left( \frac{a-1}{x} - 1 \right) \f]
*/
template<typename T1, typename T2>
constexpr
common_return_t<T1,T2>
incomplete_gamma_inv(const T1 a, const T2 p)
noexcept
{
return internal::incomplete_gamma_inv_type_check(a,p);
}
#endif

88
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp vendored Normal file
View File

@@ -0,0 +1,88 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time inverse-square-root function
*/
#ifndef _gcem_inv_sqrt_HPP
#define _gcem_inv_sqrt_HPP
namespace internal
{
template<typename T>
constexpr
T
inv_sqrt_recur(const T x, const T xn, const int count)
noexcept
{
return( abs( xn - T(1)/(x*xn) ) / (T(1) + xn) < GCLIM<T>::min() ? \
// if
xn :
count < GCEM_INV_SQRT_MAX_ITER ? \
// else
inv_sqrt_recur(x, T(0.5)*(xn + T(1)/(x*xn)), count+1) :
xn );
}
template<typename T>
constexpr
T
inv_sqrt_check(const T x)
noexcept
{
return( is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
//
x < T(0) ? \
GCLIM<T>::quiet_NaN() :
//
is_posinf(x) ? \
T(0) :
// indistinguishable from zero or one
GCLIM<T>::min() > abs(x) ? \
GCLIM<T>::infinity() :
GCLIM<T>::min() > abs(T(1) - x) ? \
x :
// else
inv_sqrt_recur(x, x/T(2), 0) );
}
}
/**
* Compile-time inverse-square-root function
*
* @param x a real-valued input.
* @return Computes \f$ 1 / \sqrt{x} \f$ using a Newton-Raphson approach.
*/
template<typename T>
constexpr
return_t<T>
inv_sqrt(const T x)
noexcept
{
return internal::inv_sqrt_check( static_cast<return_t<T>>(x) );
}
#endif

41
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp vendored Normal file
View File

@@ -0,0 +1,41 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time check if integer is even
*/
#ifndef _gcem_is_even_HPP
#define _gcem_is_even_HPP
namespace internal
{
constexpr
bool
is_even(const llint_t x)
noexcept
{
return !is_odd(x);
}
}
#endif

78
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp vendored Normal file
View File

@@ -0,0 +1,78 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time check if a float is not NaN-valued or +/-Inf
*/
#ifndef _gcem_is_finite_HPP
#define _gcem_is_finite_HPP
namespace internal
{
template<typename T>
constexpr
bool
is_finite(const T x)
noexcept
{
return (!is_nan(x)) && (!is_inf(x));
}
template<typename T1, typename T2>
constexpr
bool
any_finite(const T1 x, const T2 y)
noexcept
{
return( is_finite(x) || is_finite(y) );
}
template<typename T1, typename T2>
constexpr
bool
all_finite(const T1 x, const T2 y)
noexcept
{
return( is_finite(x) && is_finite(y) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
any_finite(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_finite(x) || is_finite(y) || is_finite(z) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
all_finite(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_finite(x) && is_finite(y) && is_finite(z) );
}
}
#endif

172
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp vendored Normal file
View File

@@ -0,0 +1,172 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time check if a float is +/-Inf
*/
#ifndef _gcem_is_inf_HPP
#define _gcem_is_inf_HPP
namespace internal
{
template<typename T>
constexpr
bool
is_neginf(const T x)
noexcept
{
return x == - GCLIM<T>::infinity();
}
template<typename T1, typename T2>
constexpr
bool
any_neginf(const T1 x, const T2 y)
noexcept
{
return( is_neginf(x) || is_neginf(y) );
}
template<typename T1, typename T2>
constexpr
bool
all_neginf(const T1 x, const T2 y)
noexcept
{
return( is_neginf(x) && is_neginf(y) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
any_neginf(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_neginf(x) || is_neginf(y) || is_neginf(z) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
all_neginf(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_neginf(x) && is_neginf(y) && is_neginf(z) );
}
//
template<typename T>
constexpr
bool
is_posinf(const T x)
noexcept
{
return x == GCLIM<T>::infinity();
}
template<typename T1, typename T2>
constexpr
bool
any_posinf(const T1 x, const T2 y)
noexcept
{
return( is_posinf(x) || is_posinf(y) );
}
template<typename T1, typename T2>
constexpr
bool
all_posinf(const T1 x, const T2 y)
noexcept
{
return( is_posinf(x) && is_posinf(y) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
any_posinf(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_posinf(x) || is_posinf(y) || is_posinf(z) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
all_posinf(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_posinf(x) && is_posinf(y) && is_posinf(z) );
}
//
template<typename T>
constexpr
bool
is_inf(const T x)
noexcept
{
return( is_neginf(x) || is_posinf(x) );
}
template<typename T1, typename T2>
constexpr
bool
any_inf(const T1 x, const T2 y)
noexcept
{
return( is_inf(x) || is_inf(y) );
}
template<typename T1, typename T2>
constexpr
bool
all_inf(const T1 x, const T2 y)
noexcept
{
return( is_inf(x) && is_inf(y) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
any_inf(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_inf(x) || is_inf(y) || is_inf(z) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
all_inf(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_inf(x) && is_inf(y) && is_inf(z) );
}
}
#endif

80
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp vendored Normal file
View File

@@ -0,0 +1,80 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time check if a float is NaN-valued
*/
#ifndef _gcem_is_nan_HPP
#define _gcem_is_nan_HPP
namespace internal
{
// future: consider using __builtin_isnan(__x)
template<typename T>
constexpr
bool
is_nan(const T x)
noexcept
{
return x != x;
}
template<typename T1, typename T2>
constexpr
bool
any_nan(const T1 x, const T2 y)
noexcept
{
return( is_nan(x) || is_nan(y) );
}
template<typename T1, typename T2>
constexpr
bool
all_nan(const T1 x, const T2 y)
noexcept
{
return( is_nan(x) && is_nan(y) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
any_nan(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_nan(x) || is_nan(y) || is_nan(z) );
}
template<typename T1, typename T2, typename T3>
constexpr
bool
all_nan(const T1 x, const T2 y, const T3 z)
noexcept
{
return( is_nan(x) && is_nan(y) && is_nan(z) );
}
}
#endif

42
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp vendored Normal file
View File

@@ -0,0 +1,42 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time check if integer is odd
*/
#ifndef _gcem_is_odd_HPP
#define _gcem_is_odd_HPP
namespace internal
{
constexpr
bool
is_odd(const llint_t x)
noexcept
{
// return( x % llint_t(2) == llint_t(0) ? false : true );
return (x & 1U) != 0;
}
}
#endif

42
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp vendored Normal file
View File

@@ -0,0 +1,42 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_lbeta_HPP
#define _gcem_lbeta_HPP
/**
* Compile-time log-beta function
*
* @param a a real-valued input.
* @param b a real-valued input.
* @return the log-beta function using \f[ \ln \text{B}(\alpha,\beta) := \ln \int_0^1 t^{\alpha - 1} (1-t)^{\beta - 1} dt = \ln \Gamma(\alpha) + \ln \Gamma(\beta) - \ln \Gamma(\alpha + \beta) \f]
* where \f$ \Gamma \f$ denotes the gamma function.
*/
template<typename T1, typename T2>
constexpr
common_return_t<T1,T2>
lbeta(const T1 a, const T2 b)
noexcept
{
return( (lgamma(a) + lgamma(b)) - lgamma(a+b) );
}
#endif

65
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp vendored Normal file
View File

@@ -0,0 +1,65 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_lcm_HPP
#define _gcem_lcm_HPP
namespace internal
{
template<typename T>
constexpr
T
lcm_compute(const T a, const T b)
noexcept
{
return abs(a * (b / gcd(a,b)));
}
template<typename T1, typename T2, typename TC = common_t<T1,T2>>
constexpr
TC
lcm_type_check(const T1 a, const T2 b)
noexcept
{
return lcm_compute(static_cast<TC>(a),static_cast<TC>(b));
}
}
/**
* Compile-time least common multiple (LCM) function
*
* @param a integral-valued input.
* @param b integral-valued input.
* @return the least common multiple between integers \c a and \c b using the representation \f[ \text{lcm}(a,b) = \dfrac{| a b |}{\text{gcd}(a,b)} \f]
* where \f$ \text{gcd}(a,b) \f$ denotes the greatest common divisor between \f$ a \f$ and \f$ b \f$.
*/
template<typename T1, typename T2>
constexpr
common_t<T1,T2>
lcm(const T1 a, const T2 b)
noexcept
{
return internal::lcm_type_check(a,b);
}
#endif

135
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp vendored Normal file
View File

@@ -0,0 +1,135 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time log-gamma function
*
* for coefficient values, see:
* http://my.fit.edu/~gabdo/gamma.txt
*/
#ifndef _gcem_lgamma_HPP
#define _gcem_lgamma_HPP
namespace internal
{
// P. Godfrey's coefficients:
//
// 0.99999999999999709182
// 57.156235665862923517
// -59.597960355475491248
// 14.136097974741747174
// -0.49191381609762019978
// .33994649984811888699e-4
// .46523628927048575665e-4
// -.98374475304879564677e-4
// .15808870322491248884e-3
// -.21026444172410488319e-3
// .21743961811521264320e-3
// -.16431810653676389022e-3
// .84418223983852743293e-4
// -.26190838401581408670e-4
// .36899182659531622704e-5
constexpr
long double
lgamma_coef_term(const long double x)
noexcept
{
return( 0.99999999999999709182L + 57.156235665862923517L / (x+1) \
- 59.597960355475491248L / (x+2) + 14.136097974741747174L / (x+3) \
- 0.49191381609762019978L / (x+4) + .33994649984811888699e-4L / (x+5) \
+ .46523628927048575665e-4L / (x+6) - .98374475304879564677e-4L / (x+7) \
+ .15808870322491248884e-3L / (x+8) - .21026444172410488319e-3L / (x+9) \
+ .21743961811521264320e-3L / (x+10) - .16431810653676389022e-3L / (x+11) \
+ .84418223983852743293e-4L / (x+12) - .26190838401581408670e-4L / (x+13) \
+ .36899182659531622704e-5L / (x+14) );
}
template<typename T>
constexpr
T
lgamma_term_2(const T x)
noexcept
{ //
return( T(GCEM_LOG_SQRT_2PI) + log(T(lgamma_coef_term(x))) );
}
template<typename T>
constexpr
T
lgamma_term_1(const T x)
noexcept
{ // note: 607/128 + 0.5 = 5.2421875
return( (x + T(0.5))*log(x + T(5.2421875L)) - (x + T(5.2421875L)) );
}
template<typename T>
constexpr
T
lgamma_begin(const T x)
noexcept
{ // returns lngamma(x+1)
return( lgamma_term_1(x) + lgamma_term_2(x) );
}
template<typename T>
constexpr
T
lgamma_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from one or <= zero
GCLIM<T>::min() > abs(x - T(1)) ? \
T(0) :
GCLIM<T>::min() > x ? \
GCLIM<T>::infinity() :
// else
lgamma_begin(x - T(1)) );
}
}
/**
* Compile-time log-gamma function
*
* @param x a real-valued input.
* @return computes the log-gamma function
* \f[ \ln \Gamma(x) = \ln \int_0^\infty y^{x-1} \exp(-y) dy \f]
* using a polynomial form:
* \f[ \Gamma(x+1) \approx (x+g+0.5)^{x+0.5} \exp(-x-g-0.5) \sqrt{2 \pi} \left[ c_0 + \frac{c_1}{x+1} + \frac{c_2}{x+2} + \cdots + \frac{c_n}{x+n} \right] \f]
* where the value \f$ g \f$ and the coefficients \f$ (c_0, c_1, \ldots, c_n) \f$
* are taken from Paul Godfrey, whose note can be found here: http://my.fit.edu/~gabdo/gamma.txt
*/
template<typename T>
constexpr
return_t<T>
lgamma(const T x)
noexcept
{
return internal::lgamma_check( static_cast<return_t<T>>(x) );
}
#endif

73
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp vendored Normal file
View File

@@ -0,0 +1,73 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* log multivariate gamma function
*/
#ifndef _gcem_lmgamma_HPP
#define _gcem_lmgamma_HPP
namespace internal
{
// see https://en.wikipedia.org/wiki/Multivariate_gamma_function
template<typename T1, typename T2>
constexpr
T1
lmgamma_recur(const T1 a, const T2 p)
noexcept
{
return( // NaN check
is_nan(a) ? \
GCLIM<T1>::quiet_NaN() :
//
p == T2(1) ? \
lgamma(a) :
p < T2(1) ? \
GCLIM<T1>::quiet_NaN() :
// else
T1(GCEM_LOG_PI) * (p - T1(1))/T1(2) \
+ lgamma(a) + lmgamma_recur(a - T1(0.5),p-T2(1)) );
}
}
/**
* Compile-time log multivariate gamma function
*
* @param a a real-valued input.
* @param p integral-valued input.
* @return computes log-multivariate gamma function via recursion
* \f[ \Gamma_p(a) = \pi^{(p-1)/2} \Gamma(a) \Gamma_{p-1}(a-0.5) \f]
* where \f$ \Gamma_1(a) = \Gamma(a) \f$.
*/
template<typename T1, typename T2>
constexpr
return_t<T1>
lmgamma(const T1 a, const T2 p)
noexcept
{
return internal::lmgamma_recur(static_cast<return_t<T1>>(a),p);
}
#endif

162
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp vendored Normal file
View File

@@ -0,0 +1,162 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time natural logarithm function
*/
#ifndef _gcem_log_HPP
#define _gcem_log_HPP
namespace internal
{
// continued fraction seems to be a better approximation for small x
// see http://functions.wolfram.com/ElementaryFunctions/Log/10/0005/
template<typename T>
constexpr
T
log_cf_main(const T xx, const int depth)
noexcept
{
return( depth < GCEM_LOG_MAX_ITER_SMALL ? \
// if
T(2*depth - 1) - T(depth*depth)*xx/log_cf_main(xx,depth+1) :
// else
T(2*depth - 1) );
}
template<typename T>
constexpr
T
log_cf_begin(const T x)
noexcept
{
return( T(2)*x/log_cf_main(x*x,1) );
}
template<typename T>
constexpr
T
log_main(const T x)
noexcept
{
return( log_cf_begin((x - T(1))/(x + T(1))) );
}
constexpr
long double
log_mantissa_integer(const int x)
noexcept
{
return( x == 2 ? 0.6931471805599453094172321214581765680755L :
x == 3 ? 1.0986122886681096913952452369225257046475L :
x == 4 ? 1.3862943611198906188344642429163531361510L :
x == 5 ? 1.6094379124341003746007593332261876395256L :
x == 6 ? 1.7917594692280550008124773583807022727230L :
x == 7 ? 1.9459101490553133051053527434431797296371L :
x == 8 ? 2.0794415416798359282516963643745297042265L :
x == 9 ? 2.1972245773362193827904904738450514092950L :
x == 10 ? 2.3025850929940456840179914546843642076011L :
0.0L );
}
template<typename T>
constexpr
T
log_mantissa(const T x)
noexcept
{ // divide by the integer part of x, which will be in [1,10], then adjust using tables
return( log_main(x/T(static_cast<int>(x))) + T(log_mantissa_integer(static_cast<int>(x))) );
}
template<typename T>
constexpr
T
log_breakup(const T x)
noexcept
{ // x = a*b, where b = 10^c
return( log_mantissa(mantissa(x)) + T(GCEM_LOG_10)*T(find_exponent(x,0)) );
}
template<typename T>
constexpr
T
log_check(const T x)
noexcept
{
return( is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// x < 0
x < T(0) ? \
GCLIM<T>::quiet_NaN() :
// x ~= 0
GCLIM<T>::min() > x ? \
- GCLIM<T>::infinity() :
// indistinguishable from 1
GCLIM<T>::min() > abs(x - T(1)) ? \
T(0) :
//
x == GCLIM<T>::infinity() ? \
GCLIM<T>::infinity() :
// else
(x < T(0.5) || x > T(1.5)) ?
// if
log_breakup(x) :
// else
log_main(x) );
}
template<typename T>
constexpr
return_t<T>
log_integral_check(const T x)
noexcept
{
return( std::is_integral<T>::value ? \
x == T(0) ? \
- GCLIM<return_t<T>>::infinity() :
x > T(1) ? \
log_check( static_cast<return_t<T>>(x) ) :
static_cast<return_t<T>>(0) :
log_check( static_cast<return_t<T>>(x) ) );
}
}
/**
* Compile-time natural logarithm function
*
* @param x a real-valued input.
* @return \f$ \log_e(x) \f$ using \f[ \log\left(\frac{1+x}{1-x}\right) = \dfrac{2x}{1-\dfrac{x^2}{3-\dfrac{4x^2}{5 - \dfrac{9x^3}{7 - \ddots}}}}, \ \ x \in [-1,1] \f]
* The continued fraction argument is split into two parts: \f$ x = a \times 10^c \f$, where \f$ c \f$ is an integer.
*/
template<typename T>
constexpr
return_t<T>
log(const T x)
noexcept
{
return internal::log_integral_check( x );
}
#endif

59
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp vendored Normal file
View File

@@ -0,0 +1,59 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time common logarithm function
*/
#ifndef _gcem_log10_HPP
#define _gcem_log10_HPP
namespace internal
{
template<typename T>
constexpr
return_t<T>
log10_check(const T x)
noexcept
{
// log_10(x) = ln(x) / ln(10)
return return_t<T>(log(x) / GCEM_LOG_10);
}
}
/**
* Compile-time common logarithm function
*
* @param x a real-valued input.
* @return \f$ \log_{10}(x) \f$ using \f[ \log_{10}(x) = \frac{\log_e(x)}{\log_e(10)} \f]
*/
template<typename T>
constexpr
return_t<T>
log10(const T x)
noexcept
{
return internal::log10_check( x );
}
#endif

80
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp vendored Normal file
View File

@@ -0,0 +1,80 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time natural logarithm(x+1) function
*/
#ifndef _gcem_log1p_HPP
#define _gcem_log1p_HPP
namespace internal
{
// see:
// http://functions.wolfram.com/ElementaryFunctions/Log/06/01/04/01/0003/
template<typename T>
constexpr
T
log1p_compute(const T x)
noexcept
{
// return x * ( T(1) + x * ( -T(1)/T(2) + x * ( T(1)/T(3) + x * ( -T(1)/T(4) + x/T(5) ) ) ) ); // O(x^6)
return x + x * ( - x/T(2) + x * ( x/T(3) + x * ( -x/T(4) + x*x/T(5) ) ) ); // O(x^6)
}
template<typename T>
constexpr
T
log1p_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
//
abs(x) > T(1e-04) ? \
// if
log(T(1) + x) :
// else
log1p_compute(x) );
}
}
/**
* Compile-time natural-logarithm-plus-1 function
*
* @param x a real-valued input.
* @return \f$ \log_e(x+1) \f$ using \f[ \log(x+1) = \sum_{k=1}^\infty \dfrac{(-1)^{k-1}x^k}{k}, \ \ |x| < 1 \f]
*/
template<typename T>
constexpr
return_t<T>
log1p(const T x)
noexcept
{
return internal::log1p_check( static_cast<return_t<T>>(x) );
}
#endif

59
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp vendored Normal file
View File

@@ -0,0 +1,59 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time binary logarithm function
*/
#ifndef _gcem_log2_HPP
#define _gcem_log2_HPP
namespace internal
{
template<typename T>
constexpr
return_t<T>
log2_check(const T x)
noexcept
{
// log_2(x) = ln(x) / ln(2)
return return_t<T>(log(x) / GCEM_LOG_2);
}
}
/**
* Compile-time binary logarithm function
*
* @param x a real-valued input.
* @return \f$ \log_2(x) \f$ using \f[ \log_{2}(x) = \frac{\log_e(x)}{\log_e(2)} \f]
*/
template<typename T>
constexpr
return_t<T>
log2(const T x)
noexcept
{
return internal::log2_check( x );
}
#endif

65
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp vendored Normal file
View File

@@ -0,0 +1,65 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_log_binomial_coef_HPP
#define _gcem_log_binomial_coef_HPP
namespace internal
{
template<typename T>
constexpr
T
log_binomial_coef_compute(const T n, const T k)
noexcept
{
return( lgamma(n+1) - (lgamma(k+1) + lgamma(n-k+1)) );
}
template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
constexpr
TC
log_binomial_coef_type_check(const T1 n, const T2 k)
noexcept
{
return log_binomial_coef_compute(static_cast<TC>(n),static_cast<TC>(k));
}
}
/**
* Compile-time log binomial coefficient
*
* @param n integral-valued input.
* @param k integral-valued input.
* @return computes the log Binomial coefficient
* \f[ \ln \frac{n!}{k!(n-k)!} = \ln \Gamma(n+1) - [ \ln \Gamma(k+1) + \ln \Gamma(n-k+1) ] \f]
*/
template<typename T1, typename T2>
constexpr
common_return_t<T1,T2>
log_binomial_coef(const T1 n, const T2 k)
noexcept
{
return internal::log_binomial_coef_type_check(n,k);
}
#endif

47
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp vendored Normal file
View File

@@ -0,0 +1,47 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time mantissa function
*/
#ifndef _gcem_mantissa_HPP
#define _gcem_mantissa_HPP
namespace internal
{
template<typename T>
constexpr
T
mantissa(const T x)
noexcept
{
return( x < T(1) ? \
mantissa(x*T(10)) :
x > T(10) ? \
mantissa(x/T(10)) :
// else
x );
}
}
#endif

41
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp vendored Normal file
View File

@@ -0,0 +1,41 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_max_HPP
#define _gcem_max_HPP
/**
* Compile-time pairwise maximum function
*
* @param x a real-valued input.
* @param y a real-valued input.
* @return Computes the maximum between \c x and \c y, where \c x and \c y have the same type (e.g., \c int, \c double, etc.)
*/
template<typename T1, typename T2>
constexpr
common_t<T1,T2>
max(const T1 x, const T2 y)
noexcept
{
return( y < x ? x : y );
}
#endif

41
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp vendored Normal file
View File

@@ -0,0 +1,41 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_min_HPP
#define _gcem_min_HPP
/**
* Compile-time pairwise minimum function
*
* @param x a real-valued input.
* @param y a real-valued input.
* @return Computes the minimum between \c x and \c y, where \c x and \c y have the same type (e.g., \c int, \c double, etc.)
*/
template<typename T1, typename T2>
constexpr
common_t<T1,T2>
min(const T1 x, const T2 y)
noexcept
{
return( y > x ? x : y );
}
#endif

37
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp vendored Normal file
View File

@@ -0,0 +1,37 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* extract signbit for signed zeros
*/
namespace internal
{
template<typename T>
constexpr
bool
neg_zero(const T x)
noexcept
{
return( (x == T(0.0)) && (copysign(T(1.0), x) == T(-1.0)) );
}
}

82
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp vendored Normal file
View File

@@ -0,0 +1,82 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time power function
*/
#ifndef _gcem_pow_HPP
#define _gcem_pow_HPP
namespace internal
{
template<typename T>
constexpr
T
pow_dbl(const T base, const T exp_term)
noexcept
{
return exp(exp_term*log(base));
}
template<typename T1, typename T2, typename TC = common_t<T1,T2>,
typename std::enable_if<!std::is_integral<T2>::value>::type* = nullptr>
constexpr
TC
pow_check(const T1 base, const T2 exp_term)
noexcept
{
return( base < T1(0) ? \
GCLIM<TC>::quiet_NaN() :
//
pow_dbl(static_cast<TC>(base),static_cast<TC>(exp_term)) );
}
template<typename T1, typename T2, typename TC = common_t<T1,T2>,
typename std::enable_if<std::is_integral<T2>::value>::type* = nullptr>
constexpr
TC
pow_check(const T1 base, const T2 exp_term)
noexcept
{
return pow_integral(base,exp_term);
}
}
/**
* Compile-time power function
*
* @param base a real-valued input.
* @param exp_term a real-valued input.
* @return Computes \c base raised to the power \c exp_term. In the case where \c exp_term is integral-valued, recursion by squaring is used, otherwise \f$ \text{base}^{\text{exp\_term}} = e^{\text{exp\_term} \log(\text{base})} \f$
*/
template<typename T1, typename T2>
constexpr
common_t<T1,T2>
pow(const T1 base, const T2 exp_term)
noexcept
{
return internal::pow_check(base,exp_term);
}
#endif

128
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp vendored Normal file
View File

@@ -0,0 +1,128 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time power function
*/
#ifndef _gcem_pow_integral_HPP
#define _gcem_pow_integral_HPP
namespace internal
{
template<typename T1, typename T2>
constexpr T1 pow_integral_compute(const T1 base, const T2 exp_term) noexcept;
// integral-valued powers using method described in
// https://en.wikipedia.org/wiki/Exponentiation_by_squaring
template<typename T1, typename T2>
constexpr
T1
pow_integral_compute_recur(const T1 base, const T1 val, const T2 exp_term)
noexcept
{
return( exp_term > T2(1) ? \
(is_odd(exp_term) ? \
pow_integral_compute_recur(base*base,val*base,exp_term/2) :
pow_integral_compute_recur(base*base,val,exp_term/2)) :
(exp_term == T2(1) ? val*base : val) );
}
template<typename T1, typename T2, typename std::enable_if<std::is_signed<T2>::value>::type* = nullptr>
constexpr
T1
pow_integral_sgn_check(const T1 base, const T2 exp_term)
noexcept
{
return( exp_term < T2(0) ? \
//
T1(1) / pow_integral_compute(base, - exp_term) :
//
pow_integral_compute_recur(base,T1(1),exp_term) );
}
template<typename T1, typename T2, typename std::enable_if<!std::is_signed<T2>::value>::type* = nullptr>
constexpr
T1
pow_integral_sgn_check(const T1 base, const T2 exp_term)
noexcept
{
return( pow_integral_compute_recur(base,T1(1),exp_term) );
}
template<typename T1, typename T2>
constexpr
T1
pow_integral_compute(const T1 base, const T2 exp_term)
noexcept
{
return( exp_term == T2(3) ? \
base*base*base :
exp_term == T2(2) ? \
base*base :
exp_term == T2(1) ? \
base :
exp_term == T2(0) ? \
T1(1) :
// check for overflow
exp_term == GCLIM<T2>::min() ? \
T1(0) :
exp_term == GCLIM<T2>::max() ? \
GCLIM<T1>::infinity() :
// else
pow_integral_sgn_check(base,exp_term) );
}
template<typename T1, typename T2, typename std::enable_if<std::is_integral<T2>::value>::type* = nullptr>
constexpr
T1
pow_integral_type_check(const T1 base, const T2 exp_term)
noexcept
{
return pow_integral_compute(base,exp_term);
}
template<typename T1, typename T2, typename std::enable_if<!std::is_integral<T2>::value>::type* = nullptr>
constexpr
T1
pow_integral_type_check(const T1 base, const T2 exp_term)
noexcept
{
// return GCLIM<return_t<T1>>::quiet_NaN();
return pow_integral_compute(base,static_cast<llint_t>(exp_term));
}
//
// main function
template<typename T1, typename T2>
constexpr
T1
pow_integral(const T1 base, const T2 exp_term)
noexcept
{
return internal::pow_integral_type_check(base,exp_term);
}
}
#endif

91
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp vendored Normal file
View File

@@ -0,0 +1,91 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* Gauss-Legendre quadrature: 30 points
*/
static const long double gauss_legendre_30_points[30] = \
{
-0.05147184255531769583302521316672L,
0.05147184255531769583302521316672L,
-0.15386991360858354696379467274326L,
0.15386991360858354696379467274326L,
-0.25463692616788984643980512981781L,
0.25463692616788984643980512981781L,
-0.35270472553087811347103720708937L,
0.35270472553087811347103720708937L,
-0.44703376953808917678060990032285L,
0.44703376953808917678060990032285L,
-0.53662414814201989926416979331107L,
0.53662414814201989926416979331107L,
-0.62052618298924286114047755643119L,
0.62052618298924286114047755643119L,
-0.69785049479331579693229238802664L,
0.69785049479331579693229238802664L,
-0.76777743210482619491797734097450L,
0.76777743210482619491797734097450L,
-0.82956576238276839744289811973250L,
0.82956576238276839744289811973250L,
-0.88256053579205268154311646253023L,
0.88256053579205268154311646253023L,
-0.92620004742927432587932427708047L,
0.92620004742927432587932427708047L,
-0.96002186496830751221687102558180L,
0.96002186496830751221687102558180L,
-0.98366812327974720997003258160566L,
0.98366812327974720997003258160566L,
-0.99689348407464954027163005091870L,
0.99689348407464954027163005091870L\
};
static const long double gauss_legendre_30_weights[30] = \
{
0.10285265289355884034128563670542L,
0.10285265289355884034128563670542L,
0.10176238974840550459642895216855L,
0.10176238974840550459642895216855L,
0.09959342058679526706278028210357L,
0.09959342058679526706278028210357L,
0.09636873717464425963946862635181L,
0.09636873717464425963946862635181L,
0.09212252223778612871763270708762L,
0.09212252223778612871763270708762L,
0.08689978720108297980238753071513L,
0.08689978720108297980238753071513L,
0.08075589522942021535469493846053L,
0.08075589522942021535469493846053L,
0.07375597473770520626824385002219L,
0.07375597473770520626824385002219L,
0.06597422988218049512812851511596L,
0.06597422988218049512812851511596L,
0.05749315621761906648172168940206L,
0.05749315621761906648172168940206L,
0.04840267283059405290293814042281L,
0.04840267283059405290293814042281L,
0.03879919256962704959680193644635L,
0.03879919256962704959680193644635L,
0.02878470788332336934971917961129L,
0.02878470788332336934971917961129L,
0.01846646831109095914230213191205L,
0.01846646831109095914230213191205L,
0.00796819249616660561546588347467L,
0.00796819249616660561546588347467L\
};

131
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp vendored Normal file
View File

@@ -0,0 +1,131 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* Gauss-Legendre quadrature: 50 points
*/
static const long double gauss_legendre_50_points[50] = \
{
-0.03109833832718887611232898966595L,
0.03109833832718887611232898966595L,
-0.09317470156008614085445037763960L,
0.09317470156008614085445037763960L,
-0.15489058999814590207162862094111L,
0.15489058999814590207162862094111L,
-0.21600723687604175684728453261710L,
0.21600723687604175684728453261710L,
-0.27628819377953199032764527852113L,
0.27628819377953199032764527852113L,
-0.33550024541943735683698825729107L,
0.33550024541943735683698825729107L,
-0.39341431189756512739422925382382L,
0.39341431189756512739422925382382L,
-0.44980633497403878914713146777838L,
0.44980633497403878914713146777838L,
-0.50445814490746420165145913184914L,
0.50445814490746420165145913184914L,
-0.55715830451465005431552290962580L,
0.55715830451465005431552290962580L,
-0.60770292718495023918038179639183L,
0.60770292718495023918038179639183L,
-0.65589646568543936078162486400368L,
0.65589646568543936078162486400368L,
-0.70155246870682225108954625788366L,
0.70155246870682225108954625788366L,
-0.74449430222606853826053625268219L,
0.74449430222606853826053625268219L,
-0.78455583290039926390530519634099L,
0.78455583290039926390530519634099L,
-0.82158207085933594835625411087394L,
0.82158207085933594835625411087394L,
-0.85542976942994608461136264393476L,
0.85542976942994608461136264393476L,
-0.88596797952361304863754098246675L,
0.88596797952361304863754098246675L,
-0.91307855665579189308973564277166L,
0.91307855665579189308973564277166L,
-0.93665661894487793378087494727250L,
0.93665661894487793378087494727250L,
-0.95661095524280794299774564415662L,
0.95661095524280794299774564415662L,
-0.97286438510669207371334410460625L,
0.97286438510669207371334410460625L,
-0.98535408404800588230900962563249L,
0.98535408404800588230900962563249L,
-0.99403196943209071258510820042069L,
0.99403196943209071258510820042069L,
-0.99886640442007105018545944497422L,
0.99886640442007105018545944497422L\
};
static const long double gauss_legendre_50_weights[50] = \
{
0.06217661665534726232103310736061L,
0.06217661665534726232103310736061L,
0.06193606742068324338408750978083L,
0.06193606742068324338408750978083L,
0.06145589959031666375640678608392L,
0.06145589959031666375640678608392L,
0.06073797084177021603175001538481L,
0.06073797084177021603175001538481L,
0.05978505870426545750957640531259L,
0.05978505870426545750957640531259L,
0.05860084981322244583512243663085L,
0.05860084981322244583512243663085L,
0.05718992564772838372302931506599L,
0.05718992564772838372302931506599L,
0.05555774480621251762356742561227L,
0.05555774480621251762356742561227L,
0.05371062188899624652345879725566L,
0.05371062188899624652345879725566L,
0.05165570306958113848990529584010L,
0.05165570306958113848990529584010L,
0.04940093844946631492124358075143L,
0.04940093844946631492124358075143L,
0.04695505130394843296563301363499L,
0.04695505130394843296563301363499L,
0.04432750433880327549202228683039L,
0.04432750433880327549202228683039L,
0.04152846309014769742241197896407L,
0.04152846309014769742241197896407L,
0.03856875661258767524477015023639L,
0.03856875661258767524477015023639L,
0.03545983561514615416073461100098L,
0.03545983561514615416073461100098L,
0.03221372822357801664816582732300L,
0.03221372822357801664816582732300L,
0.02884299358053519802990637311323L,
0.02884299358053519802990637311323L,
0.02536067357001239044019487838544L,
0.02536067357001239044019487838544L,
0.02178024317012479298159206906269L,
0.02178024317012479298159206906269L,
0.01811556071348939035125994342235L,
0.01811556071348939035125994342235L,
0.01438082276148557441937890892732L,
0.01438082276148557441937890892732L,
0.01059054838365096926356968149924L,
0.01059054838365096926356968149924L,
0.00675979919574540150277887817799L,
0.00675979919574540150277887817799L,
0.00290862255315514095840072434286L,
0.00290862255315514095840072434286L\
};

125
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp vendored Normal file
View File

@@ -0,0 +1,125 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_round_HPP
#define _gcem_round_HPP
namespace internal
{
template<typename T>
constexpr
T
round_int(const T x)
noexcept
{
return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
// if
internal::floor_check(x) + sgn(x) : \
// else
internal::floor_check(x) );
}
template<typename T>
constexpr
T
round_check_internal(const T x)
noexcept
{
return x;
}
template<>
constexpr
float
round_check_internal<float>(const float x)
noexcept
{
return( abs(x) >= 8388608.f ? \
// if
x : \
//else
round_int(x) );
}
template<>
constexpr
double
round_check_internal<double>(const double x)
noexcept
{
return( abs(x) >= 4503599627370496. ? \
// if
x : \
// else
round_int(x) );
}
template<>
constexpr
long double
round_check_internal<long double>(const long double x)
noexcept
{
return( abs(x) >= 9223372036854775808.l ? \
// if
x : \
// else
round_int(x) );
}
template<typename T>
constexpr
T
round_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// +/- infinite
!is_finite(x) ? \
x :
// signed-zero cases
GCLIM<T>::min() > abs(x) ? \
x :
// else
sgn(x) * round_check_internal(abs(x)) );
}
}
/**
* Compile-time round function
*
* @param x a real-valued input.
* @return computes the rounding value of the input.
*/
template<typename T>
constexpr
return_t<T>
round(const T x)
noexcept
{
return internal::round_check( static_cast<return_t<T>>(x) );
}
#endif

45
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp vendored Normal file
View File

@@ -0,0 +1,45 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_sgn_HPP
#define _gcem_sgn_HPP
/**
* Compile-time sign function
*
* @param x a real-valued input
* @return a value \f$ y \f$ such that \f[ y = \begin{cases} 1 \ &\text{ if } x > 0 \\ 0 \ &\text{ if } x = 0 \\ -1 \ &\text{ if } x < 0 \end{cases} \f]
*/
template<typename T>
constexpr
int
sgn(const T x)
noexcept
{
return( // positive
x > T(0) ? 1 :
// negative
x < T(0) ? -1 :
// else
0 );
}
#endif

44
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp vendored Normal file
View File

@@ -0,0 +1,44 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_signbit_HPP
#define _gcem_signbit_HPP
/**
* Compile-time sign bit detection function
*
* @param x a real-valued input
* @return return true if \c x is negative, otherwise return false.
*/
template <typename T>
constexpr
bool
signbit(const T x)
noexcept
{
#ifdef _MSC_VER
return( (x == T(0)) ? (_fpclass(x) == _FPCLASS_NZ) : (x < T(0)) );
#else
return GCEM_SIGNBIT(x);
#endif
}
#endif

85
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp vendored Normal file
View File

@@ -0,0 +1,85 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time sine function using tan(x/2)
*
* see eq. 5.4.8 in Numerical Recipes
*/
#ifndef _gcem_sin_HPP
#define _gcem_sin_HPP
namespace internal
{
template<typename T>
constexpr
T
sin_compute(const T x)
noexcept
{
return T(2)*x/(T(1) + x*x);
}
template<typename T>
constexpr
T
sin_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(0) :
// special cases: pi/2 and pi
GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
T(1) :
GCLIM<T>::min() > abs(x + T(GCEM_HALF_PI)) ? \
- T(1) :
GCLIM<T>::min() > abs(x - T(GCEM_PI)) ? \
T(0) :
GCLIM<T>::min() > abs(x + T(GCEM_PI)) ? \
- T(0) :
// else
sin_compute( tan(x/T(2)) ) );
}
}
/**
* Compile-time sine function
*
* @param x a real-valued input.
* @return the sine function using \f[ \sin(x) = \frac{2\tan(x/2)}{1+\tan^2(x/2)} \f]
*/
template<typename T>
constexpr
return_t<T>
sin(const T x)
noexcept
{
return internal::sin_check( static_cast<return_t<T>>(x) );
}
#endif

65
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp vendored Normal file
View File

@@ -0,0 +1,65 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time hyperbolic sine function
*/
#ifndef _gcem_sinh_HPP
#define _gcem_sinh_HPP
namespace internal
{
template<typename T>
constexpr
T
sinh_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(0) :
// else
(exp(x) - exp(-x))/T(2) );
}
}
/**
* Compile-time hyperbolic sine function
*
* @param x a real-valued input.
* @return the hyperbolic sine function using \f[ \sinh(x) = \frac{\exp(x) - \exp(-x)}{2} \f]
*/
template<typename T>
constexpr
return_t<T>
sinh(const T x)
noexcept
{
return internal::sinh_check( static_cast<return_t<T>>(x) );
}
#endif

90
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp vendored Normal file
View File

@@ -0,0 +1,90 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time square-root function
*/
#ifndef _gcem_sqrt_HPP
#define _gcem_sqrt_HPP
namespace internal
{
template<typename T>
constexpr
T
sqrt_recur(const T x, const T xn, const int count)
noexcept
{
return( abs(xn - x/xn) / (T(1) + xn) < GCLIM<T>::min() ? \
// if
xn :
count < GCEM_SQRT_MAX_ITER ? \
// else
sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) :
xn );
}
template<typename T>
constexpr
T
sqrt_check(const T x, const T m_val)
noexcept
{
return( is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
//
x < T(0) ? \
GCLIM<T>::quiet_NaN() :
//
is_posinf(x) ? \
x :
// indistinguishable from zero or one
GCLIM<T>::min() > abs(x) ? \
T(0) :
GCLIM<T>::min() > abs(T(1) - x) ? \
x :
// else
x > T(4) ? \
sqrt_check(x/T(4), T(2)*m_val) :
m_val * sqrt_recur(x, x/T(2), 0) );
}
}
/**
* Compile-time square-root function
*
* @param x a real-valued input.
* @return Computes \f$ \sqrt{x} \f$ using a Newton-Raphson approach.
*/
template<typename T>
constexpr
return_t<T>
sqrt(const T x)
noexcept
{
return internal::sqrt_check( static_cast<return_t<T>>(x), return_t<T>(1) );
}
#endif

140
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp vendored Normal file
View File

@@ -0,0 +1,140 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time tangent function
*/
#ifndef _gcem_tan_HPP
#define _gcem_tan_HPP
namespace internal
{
template<typename T>
constexpr
T
tan_series_exp_long(const T z)
noexcept
{ // this is based on a fourth-order expansion of tan(z) using Bernoulli numbers
return( - 1/z + (z/3 + (pow_integral(z,3)/45 + (2*pow_integral(z,5)/945 + pow_integral(z,7)/4725))) );
}
template<typename T>
constexpr
T
tan_series_exp(const T x)
noexcept
{
return( GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
// the value tan(pi/2) is somewhat of a convention;
// technically the function is not defined at EXACTLY pi/2,
// but this is floating point pi/2
T(1.633124e+16) :
// otherwise we use an expansion around pi/2
tan_series_exp_long(x - T(GCEM_HALF_PI))
);
}
template<typename T>
constexpr
T
tan_cf_recur(const T xx, const int depth, const int max_depth)
noexcept
{
return( depth < max_depth ? \
// if
T(2*depth - 1) - xx/tan_cf_recur(xx,depth+1,max_depth) :
// else
T(2*depth - 1) );
}
template<typename T>
constexpr
T
tan_cf_main(const T x)
noexcept
{
return( (x > T(1.55) && x < T(1.60)) ? \
tan_series_exp(x) : // deals with a singularity at tan(pi/2)
//
x > T(1.4) ? \
x/tan_cf_recur(x*x,1,45) :
x > T(1) ? \
x/tan_cf_recur(x*x,1,35) :
// else
x/tan_cf_recur(x*x,1,25) );
}
template<typename T>
constexpr
T
tan_begin(const T x, const int count = 0)
noexcept
{ // tan(x) = tan(x + pi)
return( x > T(GCEM_PI) ? \
// if
count > 1 ? GCLIM<T>::quiet_NaN() : // protect against undefined behavior
tan_begin( x - T(GCEM_PI) * internal::floor_check(x/T(GCEM_PI)), count+1 ) :
// else
tan_cf_main(x) );
}
template<typename T>
constexpr
T
tan_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(0) :
// else
x < T(0) ? \
- tan_begin(-x) :
tan_begin( x) );
}
}
/**
* Compile-time tangent function
*
* @param x a real-valued input.
* @return the tangent function using
* \f[ \tan(x) = \dfrac{x}{1 - \dfrac{x^2}{3 - \dfrac{x^2}{5 - \ddots}}} \f]
* To deal with a singularity at \f$ \pi / 2 \f$, the following expansion is employed:
* \f[ \tan(x) = - \frac{1}{x-\pi/2} - \sum_{k=1}^\infty \frac{(-1)^k 2^{2k} B_{2k}}{(2k)!} (x - \pi/2)^{2k - 1} \f]
* where \f$ B_n \f$ is the n-th Bernoulli number.
*/
template<typename T>
constexpr
return_t<T>
tan(const T x)
noexcept
{
return internal::tan_check( static_cast<return_t<T>>(x) );
}
#endif

89
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp vendored Normal file
View File

@@ -0,0 +1,89 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* compile-time hyperbolic tangent function
*/
#ifndef _gcem_tanh_HPP
#define _gcem_tanh_HPP
namespace internal
{
template<typename T>
constexpr
T
tanh_cf(const T xx, const int depth)
noexcept
{
return( depth < GCEM_TANH_MAX_ITER ? \
// if
(2*depth - 1) + xx/tanh_cf(xx,depth+1) :
// else
T(2*depth - 1) );
}
template<typename T>
constexpr
T
tanh_begin(const T x)
noexcept
{
return( x/tanh_cf(x*x,1) );
}
template<typename T>
constexpr
T
tanh_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from zero
GCLIM<T>::min() > abs(x) ? \
T(0) :
// else
x < T(0) ? \
- tanh_begin(-x) :
tanh_begin( x) );
}
}
/**
* Compile-time hyperbolic tangent function
*
* @param x a real-valued input.
* @return the hyperbolic tangent function using \f[ \tanh(x) = \dfrac{x}{1 + \dfrac{x^2}{3 + \dfrac{x^2}{5 + \dfrac{x^2}{7 + \ddots}}}} \f]
*/
template<typename T>
constexpr
return_t<T>
tanh(const T x)
noexcept
{
return internal::tanh_check( static_cast<return_t<T>>(x) );
}
#endif

80
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp vendored Normal file
View File

@@ -0,0 +1,80 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
/*
* the ('true') gamma function
*/
#ifndef _gcem_tgamma_HPP
#define _gcem_tgamma_HPP
namespace internal
{
template<typename T>
constexpr
T
tgamma_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from one or zero
GCLIM<T>::min() > abs(x - T(1)) ? \
T(1) :
GCLIM<T>::min() > abs(x) ? \
GCLIM<T>::infinity() :
// negative numbers
x < T(0) ? \
// check for integer
GCLIM<T>::min() > abs(x - find_whole(x)) ? \
GCLIM<T>::quiet_NaN() :
// else
tgamma_check(x+T(1)) / x :
// else
exp(lgamma(x)) );
}
}
/**
* Compile-time gamma function
*
* @param x a real-valued input.
* @return computes the `true' gamma function
* \f[ \Gamma(x) = \int_0^\infty y^{x-1} \exp(-y) dy \f]
* using a polynomial form:
* \f[ \Gamma(x+1) \approx (x+g+0.5)^{x+0.5} \exp(-x-g-0.5) \sqrt{2 \pi} \left[ c_0 + \frac{c_1}{x+1} + \frac{c_2}{x+2} + \cdots + \frac{c_n}{x+n} \right] \f]
* where the value \f$ g \f$ and the coefficients \f$ (c_0, c_1, \ldots, c_n) \f$
* are taken from Paul Godfrey, whose note can be found here: http://my.fit.edu/~gabdo/gamma.txt
*/
template<typename T>
constexpr
return_t<T>
tgamma(const T x)
noexcept
{
return internal::tgamma_check( static_cast<return_t<T>>(x) );
}
#endif

121
wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp vendored Normal file
View File

@@ -0,0 +1,121 @@
/*################################################################################
##
## Copyright (C) 2016-2022 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/
#ifndef _gcem_trunc_HPP
#define _gcem_trunc_HPP
namespace internal
{
template<typename T>
constexpr
T
trunc_int(const T x)
noexcept
{
return( T(static_cast<llint_t>(x)) );
}
template<typename T>
constexpr
T
trunc_check_internal(const T x)
noexcept
{
return x;
}
template<>
constexpr
float
trunc_check_internal<float>(const float x)
noexcept
{
return( abs(x) >= 8388608.f ? \
// if
x : \
// else
trunc_int(x) );
}
template<>
constexpr
double
trunc_check_internal<double>(const double x)
noexcept
{
return( abs(x) >= 4503599627370496. ? \
// if
x : \
// else
trunc_int(x) );
}
template<>
constexpr
long double
trunc_check_internal<long double>(const long double x)
noexcept
{
return( abs(x) >= 9223372036854775808.l ? \
// if
x : \
// else
((long double)static_cast<ullint_t>(abs(x))) * sgn(x) );
}
template<typename T>
constexpr
T
trunc_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
// +/- infinite
!is_finite(x) ? \
x :
// signed-zero cases
GCLIM<T>::min() > abs(x) ? \
x :
// else
trunc_check_internal(x) );
}
}
/**
* Compile-time trunc function
*
* @param x a real-valued input.
* @return computes the trunc-value of the input, essentially returning the integer part of the input.
*/
template<typename T>
constexpr
return_t<T>
trunc(const T x)
noexcept
{
return internal::trunc_check( static_cast<return_t<T>>(x) );
}
#endif

19
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -53,3 +53,22 @@ TEST(Pose2dTest, Minus) {
EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
}
TEST(Pose2dTest, Constexpr) {
constexpr Pose2d defaultConstructed;
constexpr Pose2d translationRotation{Translation2d{0_m, 1_m},
Rotation2d{0_deg}};
constexpr Pose2d coordRotation{0_m, 0_m, Rotation2d{45_deg}};
constexpr auto added =
translationRotation + Transform2d{Translation2d{}, Rotation2d{45_deg}};
constexpr auto multiplied = coordRotation * 2;
static_assert(defaultConstructed.X() == 0_m);
static_assert(translationRotation.Y() == 1_m);
static_assert(coordRotation.Rotation().Degrees() == 45_deg);
static_assert(added.X() == 0_m);
static_assert(added.Y() == 1_m);
static_assert(added.Rotation().Degrees() == 45_deg);
static_assert(multiplied.Rotation().Degrees() == 90_deg);
}

19
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -63,3 +63,22 @@ TEST(Rotation2dTest, Inequality) {
const auto rot2 = Rotation2d{43.5_deg};
EXPECT_NE(rot1, rot2);
}
TEST(Rotation2dTest, Constexpr) {
constexpr Rotation2d defaultCtor;
constexpr Rotation2d radianCtor{5_rad};
constexpr Rotation2d degreeCtor{270_deg};
constexpr Rotation2d rotation45{45_deg};
constexpr Rotation2d cartesianCtor{3.5, -3.5};
constexpr auto negated = -radianCtor;
constexpr auto multiplied = radianCtor * 2;
constexpr auto subtracted = cartesianCtor - degreeCtor;
static_assert(defaultCtor.Radians() == 0_rad);
static_assert(degreeCtor.Degrees() == 270_deg);
static_assert(negated.Radians() == (-5_rad));
static_assert(multiplied.Radians() == 10_rad);
static_assert(subtracted == rotation45);
static_assert(radianCtor != degreeCtor);
}

18
wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
View File

@@ -40,3 +40,21 @@ TEST(Transform2dTest, Composition) {
EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Degrees().value(),
transformedCombined.Rotation().Degrees().value());
}
TEST(Transform2dTest, Constexpr) {
constexpr Transform2d defaultCtor;
constexpr Transform2d translationRotationCtor{Translation2d{},
Rotation2d{10_deg}};
constexpr auto multiplied = translationRotationCtor * 5;
constexpr auto divided = translationRotationCtor / 2;
static_assert(defaultCtor.Translation().X() == 0_m);
static_assert(translationRotationCtor.X() == 0_m);
static_assert(translationRotationCtor.Y() == 0_m);
static_assert(multiplied.Rotation().Degrees() == 50_deg);
static_assert(translationRotationCtor.Inverse().Rotation().Degrees() ==
(-10_deg));
static_assert(translationRotationCtor.Inverse().X() == 0_m);
static_assert(translationRotationCtor.Inverse().Y() == 0_m);
static_assert(divided.Rotation().Degrees() == 5_deg);
}

18
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -93,3 +93,21 @@ TEST(Translation2dTest, PolarConstructor) {
EXPECT_DOUBLE_EQ(1.0, two.X().value());
EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
}
TEST(Translation2dTest, Constexpr) {
constexpr Translation2d defaultCtor;
constexpr Translation2d componentCtor{1_m, 2_m};
constexpr auto added = defaultCtor + componentCtor;
constexpr auto subtracted = defaultCtor - componentCtor;
constexpr auto negated = -componentCtor;
constexpr auto multiplied = componentCtor * 2;
constexpr auto divided = componentCtor / 2;
static_assert(defaultCtor.X() == 0_m);
static_assert(componentCtor.Y() == 2_m);
static_assert(added.X() == 1_m);
static_assert(subtracted.Y() == (-2_m));
static_assert(negated.X() == (-1_m));
static_assert(multiplied.X() == 2_m);
static_assert(divided.Y() == 1_m);
}

21
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

@@ -126,3 +126,24 @@ TEST(Translation3dTest, PolarConstructor) {
EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
}
TEST(Translation3dTest, Constexpr) {
constexpr Translation3d defaultCtor;
constexpr Translation3d componentCtor{1_m, 2_m, 3_m};
constexpr auto added = defaultCtor + componentCtor;
constexpr auto subtracted = defaultCtor - componentCtor;
constexpr auto negated = -componentCtor;
constexpr auto multiplied = componentCtor * 2;
constexpr auto divided = componentCtor / 2;
constexpr Translation2d projected = componentCtor.ToTranslation2d();
static_assert(defaultCtor.X() == 0_m);
static_assert(componentCtor.Y() == 2_m);
static_assert(added.Z() == 3_m);
static_assert(subtracted.X() == (-1_m));
static_assert(negated.Y() == (-2_m));
static_assert(multiplied.Z() == 6_m);
static_assert(divided.Y() == 1_m);
static_assert(projected.X() == 1_m);
static_assert(projected.Y() == 2_m);
}

10
wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
View File

@@ -64,3 +64,13 @@ TEST(Twist2dTest, Pose2dLog) {
const auto reapplied = start.Exp(twist);
EXPECT_EQ(end, reapplied);
}
TEST(Twist2dTest, Constexpr) {
constexpr Twist2d defaultCtor;
constexpr Twist2d componentCtor{1_m, 2_m, 3_rad};
constexpr auto multiplied = componentCtor * 2;
static_assert(defaultCtor.dx == 0_m);
static_assert(componentCtor.dy == 2_m);
static_assert(multiplied.dtheta == 6_rad);
}

14
wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
View File

@@ -115,3 +115,17 @@ TEST(Twist3dTest, Pose3dLogZ) {
const auto reapplied = start.Exp(twist);
EXPECT_EQ(end, reapplied);
}
TEST(Twist3dTest, Constexpr) {
constexpr Twist3d defaultCtor;
constexpr Twist3d componentCtor{1_m, 2_m, 3_m, 4_rad, 5_rad, 6_rad};
constexpr auto multiplied = componentCtor * 2;
static_assert(defaultCtor.dx == 0_m);
static_assert(componentCtor.dy == 2_m);
static_assert(componentCtor.dz == 3_m);
static_assert(multiplied.dx == 2_m);
static_assert(multiplied.rx == 8_rad);
static_assert(multiplied.ry == 10_rad);
static_assert(multiplied.rz == 12_rad);
}