From 3a5a376465b4ff3f63d5c62ecba0f5cc9c8cf11e Mon Sep 17 00:00:00 2001 From: David K Turner Date: Mon, 31 Oct 2022 11:17:00 -0500 Subject: [PATCH] [wpimath] Increase constexpr support in geometry data types (#4231) This uses std::is_constant_evaluated() to conditionally use the gcem library for constexpr calculations. --- ThirdPartyNotices.txt | 18 + upstream_utils/update_gcem.py | 46 +++ wpimath/.styleguide | 2 + wpimath/CMakeLists.txt | 4 + wpimath/build.gradle | 9 +- .../src/main/native/cpp/geometry/Pose2d.cpp | 23 -- .../main/native/cpp/geometry/Rotation2d.cpp | 55 --- .../main/native/cpp/geometry/Transform2d.cpp | 10 - .../native/cpp/geometry/Translation2d.cpp | 35 -- .../native/cpp/geometry/Translation3d.cpp | 30 -- .../main/native/include/frc/geometry/Pose2d.h | 22 +- .../native/include/frc/geometry/Pose2d.inc | 39 ++ .../native/include/frc/geometry/Rotation2d.h | 34 +- .../include/frc/geometry/Rotation2d.inc | 74 ++++ .../native/include/frc/geometry/Transform2d.h | 20 +- .../include/frc/geometry/Transform2d.inc | 26 ++ .../include/frc/geometry/Translation2d.h | 24 +- .../include/frc/geometry/Translation2d.inc | 50 +++ .../include/frc/geometry/Translation3d.h | 22 +- .../include/frc/geometry/Translation3d.inc | 44 +++ .../native/include/frc/geometry/Twist2d.h | 2 +- .../native/include/frc/geometry/Twist3d.h | 2 +- .../native/thirdparty/gcem/include/gcem.hpp | 104 ++++++ .../thirdparty/gcem/include/gcem_incl/abs.hpp | 45 +++ .../gcem/include/gcem_incl/acos.hpp | 84 +++++ .../gcem/include/gcem_incl/acosh.hpp | 68 ++++ .../gcem/include/gcem_incl/asin.hpp | 82 ++++ .../gcem/include/gcem_incl/asinh.hpp | 66 ++++ .../gcem/include/gcem_incl/atan.hpp | 155 ++++++++ .../gcem/include/gcem_incl/atan2.hpp | 88 +++++ .../gcem/include/gcem_incl/atanh.hpp | 79 ++++ .../gcem/include/gcem_incl/beta.hpp | 42 +++ .../gcem/include/gcem_incl/binomial_coef.hpp | 91 +++++ .../gcem/include/gcem_incl/ceil.hpp | 130 +++++++ .../gcem/include/gcem_incl/copysign.hpp | 41 ++ .../thirdparty/gcem/include/gcem_incl/cos.hpp | 83 +++++ .../gcem/include/gcem_incl/cosh.hpp | 65 ++++ .../thirdparty/gcem/include/gcem_incl/erf.hpp | 143 +++++++ .../gcem/include/gcem_incl/erf_inv.hpp | 264 +++++++++++++ .../thirdparty/gcem/include/gcem_incl/exp.hpp | 106 ++++++ .../gcem/include/gcem_incl/expm1.hpp | 76 ++++ .../gcem/include/gcem_incl/factorial.hpp | 98 +++++ .../gcem/include/gcem_incl/find_exponent.hpp | 47 +++ .../gcem/include/gcem_incl/find_fraction.hpp | 46 +++ .../gcem/include/gcem_incl/find_whole.hpp | 46 +++ .../gcem/include/gcem_incl/floor.hpp | 130 +++++++ .../gcem/include/gcem_incl/fmod.hpp | 70 ++++ .../thirdparty/gcem/include/gcem_incl/gcd.hpp | 82 ++++ .../gcem/include/gcem_incl/gcem_options.hpp | 213 +++++++++++ .../gcem/include/gcem_incl/hypot.hpp | 90 +++++ .../include/gcem_incl/incomplete_beta.hpp | 194 ++++++++++ .../include/gcem_incl/incomplete_beta_inv.hpp | 352 ++++++++++++++++++ .../include/gcem_incl/incomplete_gamma.hpp | 247 ++++++++++++ .../gcem_incl/incomplete_gamma_inv.hpp | 271 ++++++++++++++ .../gcem/include/gcem_incl/inv_sqrt.hpp | 88 +++++ .../gcem/include/gcem_incl/is_even.hpp | 41 ++ .../gcem/include/gcem_incl/is_finite.hpp | 78 ++++ .../gcem/include/gcem_incl/is_inf.hpp | 172 +++++++++ .../gcem/include/gcem_incl/is_nan.hpp | 80 ++++ .../gcem/include/gcem_incl/is_odd.hpp | 42 +++ .../gcem/include/gcem_incl/lbeta.hpp | 42 +++ .../thirdparty/gcem/include/gcem_incl/lcm.hpp | 65 ++++ .../gcem/include/gcem_incl/lgamma.hpp | 135 +++++++ .../gcem/include/gcem_incl/lmgamma.hpp | 73 ++++ .../thirdparty/gcem/include/gcem_incl/log.hpp | 162 ++++++++ .../gcem/include/gcem_incl/log10.hpp | 59 +++ .../gcem/include/gcem_incl/log1p.hpp | 80 ++++ .../gcem/include/gcem_incl/log2.hpp | 59 +++ .../include/gcem_incl/log_binomial_coef.hpp | 65 ++++ .../gcem/include/gcem_incl/mantissa.hpp | 47 +++ .../thirdparty/gcem/include/gcem_incl/max.hpp | 41 ++ .../thirdparty/gcem/include/gcem_incl/min.hpp | 41 ++ .../gcem/include/gcem_incl/neg_zero.hpp | 37 ++ .../thirdparty/gcem/include/gcem_incl/pow.hpp | 82 ++++ .../gcem/include/gcem_incl/pow_integral.hpp | 128 +++++++ .../quadrature/gauss_legendre_30.hpp | 91 +++++ .../quadrature/gauss_legendre_50.hpp | 131 +++++++ .../gcem/include/gcem_incl/round.hpp | 125 +++++++ .../thirdparty/gcem/include/gcem_incl/sgn.hpp | 45 +++ .../gcem/include/gcem_incl/signbit.hpp | 44 +++ .../thirdparty/gcem/include/gcem_incl/sin.hpp | 85 +++++ .../gcem/include/gcem_incl/sinh.hpp | 65 ++++ .../gcem/include/gcem_incl/sqrt.hpp | 90 +++++ .../thirdparty/gcem/include/gcem_incl/tan.hpp | 140 +++++++ .../gcem/include/gcem_incl/tanh.hpp | 89 +++++ .../gcem/include/gcem_incl/tgamma.hpp | 80 ++++ .../gcem/include/gcem_incl/trunc.hpp | 121 ++++++ .../test/native/cpp/geometry/Pose2dTest.cpp | 19 + .../native/cpp/geometry/Rotation2dTest.cpp | 19 + .../native/cpp/geometry/Transform2dTest.cpp | 18 + .../native/cpp/geometry/Translation2dTest.cpp | 18 + .../native/cpp/geometry/Translation3dTest.cpp | 21 ++ .../test/native/cpp/geometry/Twist2dTest.cpp | 10 + .../test/native/cpp/geometry/Twist3dTest.cpp | 14 + 94 files changed, 6919 insertions(+), 212 deletions(-) create mode 100644 upstream_utils/update_gcem.py create mode 100644 wpimath/src/main/native/include/frc/geometry/Pose2d.inc create mode 100644 wpimath/src/main/native/include/frc/geometry/Rotation2d.inc create mode 100644 wpimath/src/main/native/include/frc/geometry/Transform2d.inc create mode 100644 wpimath/src/main/native/include/frc/geometry/Translation2d.inc create mode 100644 wpimath/src/main/native/include/frc/geometry/Translation3d.inc create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp create mode 100644 wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp diff --git a/ThirdPartyNotices.txt b/ThirdPartyNotices.txt index 6fe514b648..18163b2cbb 100644 --- a/ThirdPartyNotices.txt +++ b/ThirdPartyNotices.txt @@ -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. diff --git a/upstream_utils/update_gcem.py b/upstream_utils/update_gcem.py new file mode 100644 index 0000000000..ad1cb5a436 --- /dev/null +++ b/upstream_utils/update_gcem.py @@ -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() diff --git a/wpimath/.styleguide b/wpimath/.styleguide index 8c17f345d7..6ae8bbfc3f 100644 --- a/wpimath/.styleguide +++ b/wpimath/.styleguide @@ -1,5 +1,6 @@ cppHeaderFileInclude { \.h$ + \.hpp$ \.inc$ \.inl$ } @@ -38,6 +39,7 @@ includeOtherLibs { } includeProject { + ^gcem/ ^drake/ ^Eigen/ ^units/ diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index e11353a2c9..16ae1be44d 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -122,6 +122,10 @@ install(DIRECTORY src/main/native/thirdparty/drake/include/ DESTINATION "${inclu target_include_directories(wpimath SYSTEM PUBLIC $) +install(DIRECTORY src/main/native/thirdparty/gcem/include/ DESTINATION "${include_dest}/wpimath") +target_include_directories(wpimath SYSTEM PUBLIC + $) + target_include_directories(wpimath PUBLIC $ $) diff --git a/wpimath/build.gradle b/wpimath/build.gradle index 0b268dfcf3..9e7c3a8d08 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -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' } } } diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index 3a6f530510..3e7c0c0bd9 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -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()}; diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index fa438f73f2..921e1f81a0 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -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()}}; } diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp index 43c3488175..dd3019d276 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp @@ -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)}; } diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index 37b8822725..a44414c2b9 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -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; diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index 9a25254652..ff3dedc9de 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -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 && diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 8c92fcf95e..04fbec5b65 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -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" diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc new file mode 100644 index 0000000000..feeadfa92b --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc @@ -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 + +#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 diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 4f9ecab470..3a4ea4d70e 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -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" diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc new file mode 100644 index 0000000000..ac3f1f5e59 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc @@ -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 + +#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()) + : std::cos(value.to())), + m_sin(std::is_constant_evaluated() ? gcem::sin(value.to()) + : std::sin(value.to())) {} + +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 diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 956db1490f..7a87315616 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -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" diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc new file mode 100644 index 0000000000..f851a05207 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc @@ -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 + +#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 diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 7ee99aa264..dd1a8d29b2 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -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" diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc new file mode 100644 index 0000000000..3be897fad6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc @@ -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 diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 385a300c21..36dc258835 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -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" diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc new file mode 100644 index 0000000000..8ab6e94c60 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc @@ -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 diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h index a5fcbe76c4..690686a44e 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h @@ -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}; } }; diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h index f862d497ec..465b553f17 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h @@ -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}; } diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp new file mode 100644 index 0000000000..cb28ff0c45 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp @@ -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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp new file mode 100644 index 0000000000..0a3ada698a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp @@ -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 +constexpr +T +abs(const T x) +noexcept +{ + return( // deal with signed-zeros + x == T(0) ? \ + T(0) : + // else + x < T(0) ? \ + - x : x ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp new file mode 100644 index 0000000000..9d3bc07964 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp @@ -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 +constexpr +T +acos_compute(const T x) +noexcept +{ + return( // only defined on [-1,1] + abs(x) > T(1) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from one or zero + GCLIM::min() > abs(x - T(1)) ? \ + T(0) : + GCLIM::min() > abs(x) ? \ + T(GCEM_HALF_PI) : + // else + atan( sqrt(T(1) - x*x)/x ) ); +} + +template +constexpr +T +acos_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::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 +constexpr +return_t +acos(const T x) +noexcept +{ + return internal::acos_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp new file mode 100644 index 0000000000..79579d7136 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp @@ -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 +constexpr +T +acosh_compute(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // function defined for x >= 1 + x < T(1) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from 1 + GCLIM::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 +constexpr +return_t +acosh(const T x) +noexcept +{ + return internal::acosh_compute( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp new file mode 100644 index 0000000000..210d9fc902 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp @@ -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 +constexpr +T +asin_compute(const T x) +noexcept +{ + return( // only defined on [-1,1] + x > T(1) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from one or zero + GCLIM::min() > abs(x - T(1)) ? \ + T(GCEM_HALF_PI) : + GCLIM::min() > abs(x) ? \ + T(0) : + // else + atan( x/sqrt(T(1) - x*x) ) ); +} + +template +constexpr +T +asin_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::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 +constexpr +return_t +asin(const T x) +noexcept +{ + return internal::asin_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp new file mode 100644 index 0000000000..dfad57e5c6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp @@ -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 +constexpr +T +asinh_compute(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from zero + GCLIM::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 +constexpr +return_t +asinh(const T x) +noexcept +{ + return internal::asinh_compute( static_cast>(x) ); +} + + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp new file mode 100644 index 0000000000..9aea85b708 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp @@ -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 +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 +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 +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 +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 +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 +constexpr +T +atan_begin(const T x) +noexcept +{ + return( x > T(2.5) ? atan_series_main(x) : atan_cf_main(x) ); +} + +template +constexpr +T +atan_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from zero + GCLIM::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 +constexpr +return_t +atan(const T x) +noexcept +{ + return internal::atan_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp new file mode 100644 index 0000000000..97c8d6a96f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp @@ -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 +constexpr +T +atan2_compute(const T y, const T x) +noexcept +{ + return( // NaN check + any_nan(y,x) ? \ + GCLIM::quiet_NaN() : + // + GCLIM::min() > abs(x) ? \ + // + GCLIM::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> +constexpr +TC +atan2_type_check(const T1 y, const T2 x) +noexcept +{ + return atan2_compute(static_cast(x),static_cast(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 +constexpr +common_return_t +atan2(const T1 y, const T2 x) +noexcept +{ + return internal::atan2_type_check(x,y); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp new file mode 100644 index 0000000000..2e1cb7631a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp @@ -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 +constexpr +T +atanh_compute(const T x) +noexcept +{ + return( log( (T(1) + x)/(T(1) - x) ) / T(2) ); +} + +template +constexpr +T +atanh_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // function is defined for |x| < 1 + T(1) < abs(x) ? \ + GCLIM::quiet_NaN() : + GCLIM::min() > (T(1) - abs(x)) ? \ + sgn(x)*GCLIM::infinity() : + // indistinguishable from zero + GCLIM::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 +constexpr +return_t +atanh(const T x) +noexcept +{ + return internal::atanh_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp new file mode 100644 index 0000000000..e888e4729f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp @@ -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 +constexpr +common_return_t +beta(const T1 a, const T2 b) +noexcept +{ + return exp( lbeta(a,b) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp new file mode 100644 index 0000000000..fb050a2cca --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp @@ -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 +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::value>::type* = nullptr> +constexpr +T +binomial_coef_check(const T n, const T k) +noexcept +{ + return binomial_coef_recur(n,k); +} + +template::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 cases + // (is_nan(n) || is_nan(k)) ? GCLIM::quiet_NaN() : + // + static_cast(binomial_coef_recur(static_cast(n),static_cast(k))) ); +} + +template> +constexpr +TC +binomial_coef_type_check(const T1 n, const T2 k) +noexcept +{ + return binomial_coef_check(static_cast(n),static_cast(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 +constexpr +common_t +binomial_coef(const T1 n, const T2 k) +noexcept +{ + return internal::binomial_coef_type_check(n,k); +} + +#endif \ No newline at end of file diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp new file mode 100644 index 0000000000..e8570ab3c0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp @@ -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 +constexpr +int +ceil_resid(const T x, const T x_whole) +noexcept +{ + return( (x > T(0)) && (x > x_whole) ); +} + +template +constexpr +T +ceil_int(const T x, const T x_whole) +noexcept +{ + return( x_whole + static_cast(ceil_resid(x,x_whole)) ); +} + +template +constexpr +T +ceil_check_internal(const T x) +noexcept +{ + return x; +} + +template<> +constexpr +float +ceil_check_internal(const float x) +noexcept +{ + return( abs(x) >= 8388608.f ? \ + // if + x : \ + // else + ceil_int(x, float(static_cast(x))) ); +} + +template<> +constexpr +double +ceil_check_internal(const double x) +noexcept +{ + return( abs(x) >= 4503599627370496. ? \ + // if + x : \ + // else + ceil_int(x, double(static_cast(x))) ); +} + +template<> +constexpr +long double +ceil_check_internal(const long double x) +noexcept +{ + return( abs(x) >= 9223372036854775808.l ? \ + // if + x : \ + // else + ceil_int(x, ((long double)static_cast(abs(x))) * sgn(x)) ); +} + +template +constexpr +T +ceil_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // +/- infinite + !is_finite(x) ? \ + x : + // signed-zero cases + GCLIM::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 +constexpr +return_t +ceil(const T x) +noexcept +{ + return internal::ceil_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp new file mode 100644 index 0000000000..a3bab7455d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp @@ -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 +constexpr +T1 +copysign(const T1 x, const T2 y) +noexcept +{ + return( signbit(x) != signbit(y) ? -x : x ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp new file mode 100644 index 0000000000..0e98012228 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp @@ -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 +constexpr +T +cos_compute(const T x) +noexcept +{ + return( T(1) - x*x)/(T(1) + x*x ); +} + +template +constexpr +T +cos_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from 0 + GCLIM::min() > abs(x) ? + T(1) : + // special cases: pi/2 and pi + GCLIM::min() > abs(x - T(GCEM_HALF_PI)) ? \ + T(0) : + GCLIM::min() > abs(x + T(GCEM_HALF_PI)) ? \ + T(0) : + GCLIM::min() > abs(x - T(GCEM_PI)) ? \ + - T(1) : + GCLIM::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 +constexpr +return_t +cos(const T x) +noexcept +{ + return internal::cos_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp new file mode 100644 index 0000000000..b3cebb8813 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp @@ -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 +constexpr +T +cosh_compute(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from zero + GCLIM::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 +constexpr +return_t +cosh(const T x) +noexcept +{ + return internal::cosh_compute( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp new file mode 100644 index 0000000000..afec09e915 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp @@ -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 +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 +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 +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 +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 +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 +constexpr +T +erf_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // +/-Inf + is_posinf(x) ? \ + T(1) : + is_neginf(x) ? \ + - T(1) : + // indistinguishable from zero + GCLIM::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 +constexpr +return_t +erf(const T x) +noexcept +{ + return internal::erf_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp new file mode 100644 index 0000000000..a93f8db7c5 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp @@ -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 +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 +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 +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 +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 +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 +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 +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 +constexpr +T +erf_inv_err_val(const T value, const T p) +noexcept +{ // err_val = f(x) + return( erf(value) - p ); +} + +template +constexpr +T +erf_inv_deriv_1(const T value) +noexcept +{ // derivative of the error function w.r.t. x + return( exp( -value*value ) ); +} + +template +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 +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 +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 +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 +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 +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 +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 +constexpr +T +erf_inv_begin(const T p) +noexcept +{ + return( // NaN check + is_nan(p) ? \ + GCLIM::quiet_NaN() : + // bad values + abs(p) > T(1) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from 1 + GCLIM::min() > abs(T(1) - p) ? \ + GCLIM::infinity() : + // indistinguishable from - 1 + GCLIM::min() > abs(T(1) + p) ? \ + - GCLIM::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 +constexpr +return_t +erf_inv(const T p) +noexcept +{ + return internal::erf_inv_begin( static_cast>(p) ); +} + + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp new file mode 100644 index 0000000000..0c6682962f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp @@ -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 +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 +constexpr +T +exp_cf(const T x) +noexcept +{ + return( T(1)/exp_cf_recur(x,1) ); +} + +template +constexpr +T +exp_split(const T x) +noexcept +{ + return( static_cast(pow_integral(GCEM_E,find_whole(x))) * exp_cf(find_fraction(x)) ); +} + +template +constexpr +T +exp_check(const T x) +noexcept +{ + return( is_nan(x) ? \ + GCLIM::quiet_NaN() : + // + is_neginf(x) ? \ + T(0) : + // + GCLIM::min() > abs(x) ? \ + T(1) : + // + is_posinf(x) ? \ + GCLIM::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 +constexpr +return_t +exp(const T x) +noexcept +{ + return internal::exp_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp new file mode 100644 index 0000000000..11b2eb9f06 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp @@ -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 +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 +constexpr +T +expm1_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::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 +constexpr +return_t +expm1(const T x) +noexcept +{ + return internal::expm1_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp new file mode 100644 index 0000000000..539c3f302d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp @@ -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 +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::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::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, long int, etc.), a simple recursion method is used, along with table values. + * When \c x is real-valued, factorial(x) = tgamma(x+1). + */ + +template +constexpr +T +factorial(const T x) +noexcept +{ + return internal::factorial_recur(x); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp new file mode 100644 index 0000000000..710adcedc1 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp @@ -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 +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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp new file mode 100644 index 0000000000..d9769e6853 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp @@ -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 +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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp new file mode 100644 index 0000000000..bd5e0b9ce3 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp @@ -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 +constexpr +llint_t +find_whole(const T x) +noexcept +{ + return( abs(x - internal::floor_check(x)) >= T(0.5) ? \ + // if + static_cast(internal::floor_check(x) + sgn(x)) : + // else + static_cast(internal::floor_check(x)) ); +} + +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp new file mode 100644 index 0000000000..c60ff6ace8 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp @@ -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 +constexpr +int +floor_resid(const T x, const T x_whole) +noexcept +{ + return( (x < T(0)) && (x < x_whole) ); +} + +template +constexpr +T +floor_int(const T x, const T x_whole) +noexcept +{ + return( x_whole - static_cast(floor_resid(x,x_whole)) ); +} + +template +constexpr +T +floor_check_internal(const T x) +noexcept +{ + return x; +} + +template<> +constexpr +float +floor_check_internal(const float x) +noexcept +{ + return( abs(x) >= 8388608.f ? \ + // if + x : \ + // else + floor_int(x, float(static_cast(x))) ); +} + +template<> +constexpr +double +floor_check_internal(const double x) +noexcept +{ + return( abs(x) >= 4503599627370496. ? \ + // if + x : \ + // else + floor_int(x, double(static_cast(x))) ); +} + +template<> +constexpr +long double +floor_check_internal(const long double x) +noexcept +{ + return( abs(x) >= 9223372036854775808.l ? \ + // if + x : \ + // else + floor_int(x, ((long double)static_cast(abs(x))) * sgn(x)) ); +} + +template +constexpr +T +floor_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // +/- infinite + !is_finite(x) ? \ + x : + // signed-zero cases + GCLIM::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 +constexpr +return_t +floor(const T x) +noexcept +{ + return internal::floor_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp new file mode 100644 index 0000000000..c804ce63e4 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp @@ -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 +constexpr +T +fmod_check(const T x, const T y) +noexcept +{ + return( // NaN check + any_nan(x, y) ? \ + GCLIM::quiet_NaN() : + // +/- infinite + !all_finite(x, y) ? \ + GCLIM::quiet_NaN() : + // else + x - trunc(x/y)*y ); +} + +template> +constexpr +TC +fmod_type_check(const T1 x, const T2 y) +noexcept +{ + return fmod_check(static_cast(x),static_cast(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 +constexpr +common_return_t +fmod(const T1 x, const T2 y) +noexcept +{ + return internal::fmod_type_check(x,y); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp new file mode 100644 index 0000000000..4a10bbe264 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp @@ -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 +constexpr +T +gcd_recur(const T a, const T b) +noexcept +{ + return( b == T(0) ? a : gcd_recur(b, a % b) ); +} + +template::value>::type* = nullptr> +constexpr +T +gcd_int_check(const T a, const T b) +noexcept +{ + return gcd_recur(a,b); +} + +template::value>::type* = nullptr> +constexpr +T +gcd_int_check(const T a, const T b) +noexcept +{ + return gcd_recur( static_cast(a), static_cast(b) ); +} + +template> +constexpr +TC +gcd_type_check(const T1 a, const T2 b) +noexcept +{ + return gcd_int_check( static_cast(abs(a)), static_cast(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 +constexpr +common_t +gcd(const T1 a, const T2 b) +noexcept +{ + return internal::gcd_type_check(a,b); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp new file mode 100644 index 0000000000..cd2747cf1c --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp @@ -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 // size_t +#include +#include + +// 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 + using GCLIM = std::numeric_limits; + + template + using return_t = typename std::conditional::value,double,T>::type; + + template + using common_t = typename std::common_type::type; + + template + using common_return_t = return_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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp new file mode 100644 index 0000000000..5a805ed76b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp @@ -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 +constexpr +T +hypot_compute(const T x, const T ydx) +noexcept +{ + return abs(x) * sqrt( T(1) + (ydx * ydx) ); +} + +template +constexpr +T +hypot_vals_check(const T x, const T y) +noexcept +{ + return( any_nan(x, y) ? \ + GCLIM::quiet_NaN() : + // + any_inf(x,y) ? \ + GCLIM::infinity() : + // indistinguishable from zero or one + GCLIM::min() > abs(x) ? \ + abs(y) : + GCLIM::min() > abs(y) ? \ + abs(x) : + // else + hypot_compute(x, y/x) ); +} + +template> +constexpr +TC +hypot_type_check(const T1 x, const T2 y) +noexcept +{ + return hypot_vals_check(static_cast(x),static_cast(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 +constexpr +common_return_t +hypot(const T1 x, const T2 y) +noexcept +{ + return internal::hypot_type_check(x,y); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp new file mode 100644 index 0000000000..5645dbef53 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp @@ -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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +constexpr +T +incomplete_beta_check(const T a, const T b, const T z) +noexcept +{ + return( // NaN check + any_nan(a, b, z) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from zero + GCLIM::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> +constexpr +TC +incomplete_beta_type_check(const T1 a, const T2 b, const T3 p) +noexcept +{ + return incomplete_beta_check(static_cast(a), + static_cast(b), + static_cast(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 +constexpr +common_return_t +incomplete_beta(const T1 a, const T2 b, const T3 z) +noexcept +{ + return internal::incomplete_beta_type_check(a,b,z); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp new file mode 100644 index 0000000000..f7fdfa0960 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp @@ -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 +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 +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 +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 +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 +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 +constexpr +T +incomplete_beta_inv_initial_val_1_int_h(const T ab_term_1) +noexcept +{ + return( T(2) / ab_term_1 ); +} + +template +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 +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 +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 +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 +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 +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 +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 +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 +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::min() > abs(value) ? \ + T(0) : + GCLIM::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 +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 +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 +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 +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 +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::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 +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 +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 +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::quiet_NaN() : + // indistinguishable from zero or one + GCLIM::min() > p ? \ + T(0) : + GCLIM::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> +constexpr +TC +incomplete_beta_inv_type_check(const T1 a, const T2 b, const T3 p) +noexcept +{ + return incomplete_beta_inv_check(static_cast(a), + static_cast(b), + static_cast(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 +constexpr +common_t +incomplete_beta_inv(const T1 a, const T2 b, const T3 p) +noexcept +{ + return internal::incomplete_beta_inv_type_check(a,b,p); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp new file mode 100644 index 0000000000..38734a52a2 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp @@ -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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +constexpr +T +incomplete_gamma_check(const T a, const T z) +noexcept +{ + return( // NaN check + any_nan(a, z) ? \ + GCLIM::quiet_NaN() : + // + a < T(0) ? \ + GCLIM::quiet_NaN() : + // + GCLIM::min() > z ? \ + T(0) : + // + GCLIM::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> +constexpr +TC +incomplete_gamma_type_check(const T1 a, const T2 p) +noexcept +{ + return incomplete_gamma_check(static_cast(a), + static_cast(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 +constexpr +common_return_t +incomplete_gamma(const T1 a, const T2 x) +noexcept +{ + return internal::incomplete_gamma_type_check(a,x); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp new file mode 100644 index 0000000000..1e57fc14fc --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp @@ -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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +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 +constexpr +T +incomplete_gamma_inv_check(const T a, const T p) +noexcept +{ + return( // NaN check + any_nan(a, p) ? \ + GCLIM::quiet_NaN() : + // + GCLIM::min() > p ? \ + T(0) : + p > T(1) ? \ + GCLIM::quiet_NaN() : + GCLIM::min() > abs(T(1) - p) ? \ + GCLIM::infinity() : + // + GCLIM::min() > a ? \ + T(0) : + // else + incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a,p),a,p,lgamma(a)) ); +} + +template> +constexpr +TC +incomplete_gamma_inv_type_check(const T1 a, const T2 p) +noexcept +{ + return incomplete_gamma_inv_check(static_cast(a), + static_cast(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 +constexpr +common_return_t +incomplete_gamma_inv(const T1 a, const T2 p) +noexcept +{ + return internal::incomplete_gamma_inv_type_check(a,p); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp new file mode 100644 index 0000000000..0200f115dc --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp @@ -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 +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::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 +constexpr +T +inv_sqrt_check(const T x) +noexcept +{ + return( is_nan(x) ? \ + GCLIM::quiet_NaN() : + // + x < T(0) ? \ + GCLIM::quiet_NaN() : + // + is_posinf(x) ? \ + T(0) : + // indistinguishable from zero or one + GCLIM::min() > abs(x) ? \ + GCLIM::infinity() : + GCLIM::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 +constexpr +return_t +inv_sqrt(const T x) +noexcept +{ + return internal::inv_sqrt_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp new file mode 100644 index 0000000000..fa925bb958 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp @@ -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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp new file mode 100644 index 0000000000..25f2e3c240 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp @@ -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 +constexpr +bool +is_finite(const T x) +noexcept +{ + return (!is_nan(x)) && (!is_inf(x)); +} + +template +constexpr +bool +any_finite(const T1 x, const T2 y) +noexcept +{ + return( is_finite(x) || is_finite(y) ); +} + +template +constexpr +bool +all_finite(const T1 x, const T2 y) +noexcept +{ + return( is_finite(x) && is_finite(y) ); +} + +template +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 +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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp new file mode 100644 index 0000000000..627c5099ec --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp @@ -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 +constexpr +bool +is_neginf(const T x) +noexcept +{ + return x == - GCLIM::infinity(); +} + +template +constexpr +bool +any_neginf(const T1 x, const T2 y) +noexcept +{ + return( is_neginf(x) || is_neginf(y) ); +} + +template +constexpr +bool +all_neginf(const T1 x, const T2 y) +noexcept +{ + return( is_neginf(x) && is_neginf(y) ); +} + +template +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 +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 +constexpr +bool +is_posinf(const T x) +noexcept +{ + return x == GCLIM::infinity(); +} + +template +constexpr +bool +any_posinf(const T1 x, const T2 y) +noexcept +{ + return( is_posinf(x) || is_posinf(y) ); +} + +template +constexpr +bool +all_posinf(const T1 x, const T2 y) +noexcept +{ + return( is_posinf(x) && is_posinf(y) ); +} + +template +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 +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 +constexpr +bool +is_inf(const T x) +noexcept +{ + return( is_neginf(x) || is_posinf(x) ); +} + +template +constexpr +bool +any_inf(const T1 x, const T2 y) +noexcept +{ + return( is_inf(x) || is_inf(y) ); +} + +template +constexpr +bool +all_inf(const T1 x, const T2 y) +noexcept +{ + return( is_inf(x) && is_inf(y) ); +} + +template +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 +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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp new file mode 100644 index 0000000000..a7a1af3389 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp @@ -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 +constexpr +bool +is_nan(const T x) +noexcept +{ + return x != x; +} + +template +constexpr +bool +any_nan(const T1 x, const T2 y) +noexcept +{ + return( is_nan(x) || is_nan(y) ); +} + +template +constexpr +bool +all_nan(const T1 x, const T2 y) +noexcept +{ + return( is_nan(x) && is_nan(y) ); +} + +template +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 +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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp new file mode 100644 index 0000000000..e6da720977 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp @@ -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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp new file mode 100644 index 0000000000..2213849c16 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp @@ -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 +constexpr +common_return_t +lbeta(const T1 a, const T2 b) +noexcept +{ + return( (lgamma(a) + lgamma(b)) - lgamma(a+b) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp new file mode 100644 index 0000000000..b0d8fb4b72 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp @@ -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 +constexpr +T +lcm_compute(const T a, const T b) +noexcept +{ + return abs(a * (b / gcd(a,b))); +} + +template> +constexpr +TC +lcm_type_check(const T1 a, const T2 b) +noexcept +{ + return lcm_compute(static_cast(a),static_cast(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 +constexpr +common_t +lcm(const T1 a, const T2 b) +noexcept +{ + return internal::lcm_type_check(a,b); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp new file mode 100644 index 0000000000..5d78eb31b0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp @@ -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 +constexpr +T +lgamma_term_2(const T x) +noexcept +{ // + return( T(GCEM_LOG_SQRT_2PI) + log(T(lgamma_coef_term(x))) ); +} + +template +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 +constexpr +T +lgamma_begin(const T x) +noexcept +{ // returns lngamma(x+1) + return( lgamma_term_1(x) + lgamma_term_2(x) ); +} + +template +constexpr +T +lgamma_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from one or <= zero + GCLIM::min() > abs(x - T(1)) ? \ + T(0) : + GCLIM::min() > x ? \ + GCLIM::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 +constexpr +return_t +lgamma(const T x) +noexcept +{ + return internal::lgamma_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp new file mode 100644 index 0000000000..76bf833d06 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp @@ -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 +constexpr +T1 +lmgamma_recur(const T1 a, const T2 p) +noexcept +{ + return( // NaN check + is_nan(a) ? \ + GCLIM::quiet_NaN() : + // + p == T2(1) ? \ + lgamma(a) : + p < T2(1) ? \ + GCLIM::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 +constexpr +return_t +lmgamma(const T1 a, const T2 p) +noexcept +{ + return internal::lmgamma_recur(static_cast>(a),p); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp new file mode 100644 index 0000000000..0d83e97940 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp @@ -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 +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 +constexpr +T +log_cf_begin(const T x) +noexcept +{ + return( T(2)*x/log_cf_main(x*x,1) ); +} + +template +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 +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(x))) + T(log_mantissa_integer(static_cast(x))) ); +} + +template +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 +constexpr +T +log_check(const T x) +noexcept +{ + return( is_nan(x) ? \ + GCLIM::quiet_NaN() : + // x < 0 + x < T(0) ? \ + GCLIM::quiet_NaN() : + // x ~= 0 + GCLIM::min() > x ? \ + - GCLIM::infinity() : + // indistinguishable from 1 + GCLIM::min() > abs(x - T(1)) ? \ + T(0) : + // + x == GCLIM::infinity() ? \ + GCLIM::infinity() : + // else + (x < T(0.5) || x > T(1.5)) ? + // if + log_breakup(x) : + // else + log_main(x) ); +} + +template +constexpr +return_t +log_integral_check(const T x) +noexcept +{ + return( std::is_integral::value ? \ + x == T(0) ? \ + - GCLIM>::infinity() : + x > T(1) ? \ + log_check( static_cast>(x) ) : + static_cast>(0) : + log_check( static_cast>(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 +constexpr +return_t +log(const T x) +noexcept +{ + return internal::log_integral_check( x ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp new file mode 100644 index 0000000000..4a3c37df40 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp @@ -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 +constexpr +return_t +log10_check(const T x) +noexcept +{ + // log_10(x) = ln(x) / ln(10) + return return_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 +constexpr +return_t +log10(const T x) +noexcept +{ + return internal::log10_check( x ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp new file mode 100644 index 0000000000..3883b2298b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp @@ -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 +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 +constexpr +T +log1p_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::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 +constexpr +return_t +log1p(const T x) +noexcept +{ + return internal::log1p_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp new file mode 100644 index 0000000000..56b7f8e71f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp @@ -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 +constexpr +return_t +log2_check(const T x) +noexcept +{ + // log_2(x) = ln(x) / ln(2) + return return_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 +constexpr +return_t +log2(const T x) +noexcept +{ + return internal::log2_check( x ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp new file mode 100644 index 0000000000..7aa9a2b4df --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp @@ -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 +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> +constexpr +TC +log_binomial_coef_type_check(const T1 n, const T2 k) +noexcept +{ + return log_binomial_coef_compute(static_cast(n),static_cast(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 +constexpr +common_return_t +log_binomial_coef(const T1 n, const T2 k) +noexcept +{ + return internal::log_binomial_coef_type_check(n,k); +} + +#endif \ No newline at end of file diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp new file mode 100644 index 0000000000..df6152b222 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp @@ -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 +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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp new file mode 100644 index 0000000000..4aed84f167 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp @@ -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 +constexpr +common_t +max(const T1 x, const T2 y) +noexcept +{ + return( y < x ? x : y ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp new file mode 100644 index 0000000000..d593dbc911 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp @@ -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 +constexpr +common_t +min(const T1 x, const T2 y) +noexcept +{ + return( y > x ? x : y ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp new file mode 100644 index 0000000000..db33f87c9b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp @@ -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 +constexpr +bool +neg_zero(const T x) +noexcept +{ + return( (x == T(0.0)) && (copysign(T(1.0), x) == T(-1.0)) ); +} + +} diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp new file mode 100644 index 0000000000..166a8c1f96 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp @@ -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 +constexpr +T +pow_dbl(const T base, const T exp_term) +noexcept +{ + return exp(exp_term*log(base)); +} + +template, + typename std::enable_if::value>::type* = nullptr> +constexpr +TC +pow_check(const T1 base, const T2 exp_term) +noexcept +{ + return( base < T1(0) ? \ + GCLIM::quiet_NaN() : + // + pow_dbl(static_cast(base),static_cast(exp_term)) ); +} + +template, + typename std::enable_if::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 +constexpr +common_t +pow(const T1 base, const T2 exp_term) +noexcept +{ + return internal::pow_check(base,exp_term); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp new file mode 100644 index 0000000000..3a902ca2f2 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp @@ -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 +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 +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::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::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 +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::min() ? \ + T1(0) : + exp_term == GCLIM::max() ? \ + GCLIM::infinity() : + // else + pow_integral_sgn_check(base,exp_term) ); +} + +template::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::value>::type* = nullptr> +constexpr +T1 +pow_integral_type_check(const T1 base, const T2 exp_term) +noexcept +{ + // return GCLIM>::quiet_NaN(); + return pow_integral_compute(base,static_cast(exp_term)); +} + +// +// main function + +template +constexpr +T1 +pow_integral(const T1 base, const T2 exp_term) +noexcept +{ + return internal::pow_integral_type_check(base,exp_term); +} + +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp new file mode 100644 index 0000000000..e609b89c53 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp @@ -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\ +}; diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp new file mode 100644 index 0000000000..44281f9080 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp @@ -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\ +}; diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp new file mode 100644 index 0000000000..43d7a5e09d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp @@ -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 +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 +constexpr +T +round_check_internal(const T x) +noexcept +{ + return x; +} + +template<> +constexpr +float +round_check_internal(const float x) +noexcept +{ + return( abs(x) >= 8388608.f ? \ + // if + x : \ + //else + round_int(x) ); +} + +template<> +constexpr +double +round_check_internal(const double x) +noexcept +{ + return( abs(x) >= 4503599627370496. ? \ + // if + x : \ + // else + round_int(x) ); +} + +template<> +constexpr +long double +round_check_internal(const long double x) +noexcept +{ + return( abs(x) >= 9223372036854775808.l ? \ + // if + x : \ + // else + round_int(x) ); +} + +template +constexpr +T +round_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // +/- infinite + !is_finite(x) ? \ + x : + // signed-zero cases + GCLIM::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 +constexpr +return_t +round(const T x) +noexcept +{ + return internal::round_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp new file mode 100644 index 0000000000..605a35abda --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp @@ -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 +constexpr +int +sgn(const T x) +noexcept +{ + return( // positive + x > T(0) ? 1 : + // negative + x < T(0) ? -1 : + // else + 0 ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp new file mode 100644 index 0000000000..e207a5a1e1 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp @@ -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 +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 diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp new file mode 100644 index 0000000000..128cd321c7 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp @@ -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 +constexpr +T +sin_compute(const T x) +noexcept +{ + return T(2)*x/(T(1) + x*x); +} + +template +constexpr +T +sin_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from zero + GCLIM::min() > abs(x) ? \ + T(0) : + // special cases: pi/2 and pi + GCLIM::min() > abs(x - T(GCEM_HALF_PI)) ? \ + T(1) : + GCLIM::min() > abs(x + T(GCEM_HALF_PI)) ? \ + - T(1) : + GCLIM::min() > abs(x - T(GCEM_PI)) ? \ + T(0) : + GCLIM::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 +constexpr +return_t +sin(const T x) +noexcept +{ + return internal::sin_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp new file mode 100644 index 0000000000..5355301d8c --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp @@ -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 +constexpr +T +sinh_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from zero + GCLIM::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 +constexpr +return_t +sinh(const T x) +noexcept +{ + return internal::sinh_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp new file mode 100644 index 0000000000..0fd559d052 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp @@ -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 +constexpr +T +sqrt_recur(const T x, const T xn, const int count) +noexcept +{ + return( abs(xn - x/xn) / (T(1) + xn) < GCLIM::min() ? \ + // if + xn : + count < GCEM_SQRT_MAX_ITER ? \ + // else + sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) : + xn ); +} + +template +constexpr +T +sqrt_check(const T x, const T m_val) +noexcept +{ + return( is_nan(x) ? \ + GCLIM::quiet_NaN() : + // + x < T(0) ? \ + GCLIM::quiet_NaN() : + // + is_posinf(x) ? \ + x : + // indistinguishable from zero or one + GCLIM::min() > abs(x) ? \ + T(0) : + GCLIM::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 +constexpr +return_t +sqrt(const T x) +noexcept +{ + return internal::sqrt_check( static_cast>(x), return_t(1) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp new file mode 100644 index 0000000000..e53f5c8f57 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp @@ -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 +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 +constexpr +T +tan_series_exp(const T x) +noexcept +{ + return( GCLIM::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 +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 +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 +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::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 +constexpr +T +tan_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from zero + GCLIM::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 +constexpr +return_t +tan(const T x) +noexcept +{ + return internal::tan_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp new file mode 100644 index 0000000000..109d751a82 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp @@ -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 +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 +constexpr +T +tanh_begin(const T x) +noexcept +{ + return( x/tanh_cf(x*x,1) ); +} + +template +constexpr +T +tanh_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from zero + GCLIM::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 +constexpr +return_t +tanh(const T x) +noexcept +{ + return internal::tanh_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp new file mode 100644 index 0000000000..5a9ae97012 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp @@ -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 +constexpr +T +tgamma_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // indistinguishable from one or zero + GCLIM::min() > abs(x - T(1)) ? \ + T(1) : + GCLIM::min() > abs(x) ? \ + GCLIM::infinity() : + // negative numbers + x < T(0) ? \ + // check for integer + GCLIM::min() > abs(x - find_whole(x)) ? \ + GCLIM::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 +constexpr +return_t +tgamma(const T x) +noexcept +{ + return internal::tgamma_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp new file mode 100644 index 0000000000..4e19ef9946 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp @@ -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 +constexpr +T +trunc_int(const T x) +noexcept +{ + return( T(static_cast(x)) ); +} + +template +constexpr +T +trunc_check_internal(const T x) +noexcept +{ + return x; +} + +template<> +constexpr +float +trunc_check_internal(const float x) +noexcept +{ + return( abs(x) >= 8388608.f ? \ + // if + x : \ + // else + trunc_int(x) ); +} + +template<> +constexpr +double +trunc_check_internal(const double x) +noexcept +{ + return( abs(x) >= 4503599627370496. ? \ + // if + x : \ + // else + trunc_int(x) ); +} + +template<> +constexpr +long double +trunc_check_internal(const long double x) +noexcept +{ + return( abs(x) >= 9223372036854775808.l ? \ + // if + x : \ + // else + ((long double)static_cast(abs(x))) * sgn(x) ); +} + +template +constexpr +T +trunc_check(const T x) +noexcept +{ + return( // NaN check + is_nan(x) ? \ + GCLIM::quiet_NaN() : + // +/- infinite + !is_finite(x) ? \ + x : + // signed-zero cases + GCLIM::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 +constexpr +return_t +trunc(const T x) +noexcept +{ + return internal::trunc_check( static_cast>(x) ); +} + +#endif diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index 4c3396166a..5ce6819167 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -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); +} diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 8f9e14c56b..3fdd37a3ff 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -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); +} diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp index 161378c713..2dd619d182 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -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); +} diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 76efd4942c..5493c43c19 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -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); +} diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp index 76e001fa89..58c611ed40 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp @@ -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); +} diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp index 5e5c4609aa..33970cdeec 100644 --- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp @@ -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); +} diff --git a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp index 559b7225cc..0d8a8f42a1 100644 --- a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp @@ -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); +}