Skip to content

Commit

Permalink
[wpimath] Make Rotation2d constexpr using gcem
Browse files Browse the repository at this point in the history
  • Loading branch information
prateekma committed Oct 5, 2020
1 parent bf26656 commit 97207f1
Show file tree
Hide file tree
Showing 67 changed files with 6,755 additions and 26 deletions.
21 changes: 20 additions & 1 deletion ThirdPartyNotices.txt
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ Team 254 Library wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineP
Portable File Dialogs wpigui/src/main/native/include/portable-file-dialogs.h
Drake wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp

GCE-Math wpimath/src/main/native/include/gcem/

==============================================================================
Google Test License
Expand Down Expand Up @@ -845,3 +845,22 @@ 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.

================
GCE-Math Library
================
Copyright (C) 2016-2020 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.
1 change: 1 addition & 0 deletions wpimath/.styleguide
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ generatedFileExclude {
src/main/native/cpp/drake/
src/main/native/eigeninclude/
src/main/native/include/drake/
src/main/native/include/gcem/
src/main/native/include/units/base\.h$
src/main/native/include/units/units\.h$
src/main/native/include/unsupported/
Expand Down
22 changes: 0 additions & 22 deletions wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,28 +15,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);
}
Expand Down
26 changes: 23 additions & 3 deletions wpimath/src/main/native/include/frc/geometry/Rotation2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@

#pragma once

#include <wpi/math>

#include "gcem/gcem.h"
#include "units/angle.h"

namespace wpi {
Expand All @@ -31,14 +34,21 @@ class Rotation2d {
*
* @param value The value of the angle in radians.
*/
Rotation2d(units::radian_t value); // NOLINT(runtime/explicit)
constexpr Rotation2d(units::radian_t value)
: m_value(value),
m_cos(gcem::cos(value.to<double>())),
m_sin(gcem::sin(value.to<double>())) {} // NOLINT(runtime/explicit)

/**
* Constructs a Rotation2d with the given degree value.
*
* @param value The value of the angle in degrees.
*/
Rotation2d(units::degree_t value); // NOLINT(runtime/explicit)
constexpr Rotation2d(units::degree_t value)
: m_value(value),
m_cos(gcem::cos(value.to<double>() * wpi::math::pi / 180.0)),
m_sin(gcem::sin(value.to<double>() * wpi::math::pi / 180.0)) {
} // NOLINT(runtime/explicit)

/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
Expand All @@ -47,7 +57,17 @@ class 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) {
double magnitude = gcem::sqrt(gcem::pow(x, 2) + gcem::pow(y, 2));
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(gcem::atan2(m_sin, m_cos));
}

/**
* Adds two rotations together, with the result being bounded between -pi and
Expand Down
97 changes: 97 additions & 0 deletions wpimath/src/main/native/include/gcem/gcem.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,97 @@
/*################################################################################
##
## Copyright (C) 2016-2020 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.
##
################################################################################*/

#pragma once

#include "gcem/gcem_incl/gcem_options.hpp"

namespace gcem {
#include "gcem/gcem_incl/quadrature/gauss_legendre_50.hpp"

#include "gcem/gcem_incl/is_inf.hpp"
#include "gcem/gcem_incl/is_nan.hpp"
#include "gcem/gcem_incl/is_finite.hpp"

#include "gcem/gcem_incl/abs.hpp"
#include "gcem/gcem_incl/ceil.hpp"
#include "gcem/gcem_incl/floor.hpp"
#include "gcem/gcem_incl/trunc.hpp"
#include "gcem/gcem_incl/is_odd.hpp"
#include "gcem/gcem_incl/is_even.hpp"
#include "gcem/gcem_incl/max.hpp"
#include "gcem/gcem_incl/min.hpp"
#include "gcem/gcem_incl/sqrt.hpp"

#include "gcem/gcem_incl/signbit.hpp"
#include "gcem/gcem_incl/copysign.hpp"
#include "gcem/gcem_incl/neg_zero.hpp"
#include "gcem/gcem_incl/sgn.hpp"

#include "gcem/gcem_incl/find_exponent.hpp"
#include "gcem/gcem_incl/find_fraction.hpp"
#include "gcem/gcem_incl/find_whole.hpp"
#include "gcem/gcem_incl/mantissa.hpp"
#include "gcem/gcem_incl/round.hpp"
#include "gcem/gcem_incl/fmod.hpp"

#include "gcem/gcem_incl/pow_integral.hpp"
#include "gcem/gcem_incl/exp.hpp"
#include "gcem/gcem_incl/expm1.hpp"
#include "gcem/gcem_incl/log.hpp"
#include "gcem/gcem_incl/log1p.hpp"
#include "gcem/gcem_incl/log2.hpp"
#include "gcem/gcem_incl/pow.hpp"

#include "gcem/gcem_incl/gcd.hpp"
#include "gcem/gcem_incl/lcm.hpp"

#include "gcem/gcem_incl/tan.hpp"
#include "gcem/gcem_incl/cos.hpp"
#include "gcem/gcem_incl/sin.hpp"

#include "gcem/gcem_incl/atan.hpp"
#include "gcem/gcem_incl/atan2.hpp"
#include "gcem/gcem_incl/acos.hpp"
#include "gcem/gcem_incl/asin.hpp"

#include "gcem/gcem_incl/tanh.hpp"
#include "gcem/gcem_incl/cosh.hpp"
#include "gcem/gcem_incl/sinh.hpp"

#include "gcem/gcem_incl/atanh.hpp"
#include "gcem/gcem_incl/acosh.hpp"
#include "gcem/gcem_incl/asinh.hpp"

#include "gcem/gcem_incl/binomial_coef.hpp"
#include "gcem/gcem_incl/lgamma.hpp"
#include "gcem/gcem_incl/tgamma.hpp"
#include "gcem/gcem_incl/factorial.hpp"
#include "gcem/gcem_incl/lbeta.hpp"
#include "gcem/gcem_incl/beta.hpp"
#include "gcem/gcem_incl/lmgamma.hpp"
#include "gcem/gcem_incl/log_binomial_coef.hpp"

#include "gcem/gcem_incl/erf.hpp"
#include "gcem/gcem_incl/erf_inv.hpp"
#include "gcem/gcem_incl/incomplete_beta.hpp"
#include "gcem/gcem_incl/incomplete_beta_inv.hpp"
#include "gcem/gcem_incl/incomplete_gamma.hpp"
#include "gcem/gcem_incl/incomplete_gamma_inv.hpp"
} // namespace gcem
45 changes: 45 additions & 0 deletions wpimath/src/main/native/include/gcem/gcem_incl/abs.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
/*################################################################################
##
## Copyright (C) 2016-2020 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/

#ifndef _gcem_abs_HPP
#define _gcem_abs_HPP

/**
* Compile-time absolute value function
*
* @param x a real-valued input.
* @return the absolute value of \c x, \f$ |x| \f$.
*/

template<typename T>
constexpr
T
abs(const T x)
noexcept
{
return( // deal with signed-zeros
x == T(0) ? \
T(0) :
// else
x < T(0) ? \
- x : x );
}

#endif
84 changes: 84 additions & 0 deletions wpimath/src/main/native/include/gcem/gcem_incl/acos.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
/*################################################################################
##
## Copyright (C) 2016-2020 Keith O'Hara
##
## This file is part of the GCE-Math C++ library.
##
## Licensed under the Apache License, Version 2.0 (the "License");
## you may not use this file except in compliance with the License.
## You may obtain a copy of the License at
##
## http://www.apache.org/licenses/LICENSE-2.0
##
## Unless required by applicable law or agreed to in writing, software
## distributed under the License is distributed on an "AS IS" BASIS,
## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
## See the License for the specific language governing permissions and
## limitations under the License.
##
################################################################################*/

/*
* compile-time arccosine function
*/

#ifndef _gcem_acos_HPP
#define _gcem_acos_HPP

namespace internal
{

template<typename T>
constexpr
T
acos_compute(const T x)
noexcept
{
return( // only defined on [-1,1]
abs(x) > T(1) ? \
GCLIM<T>::quiet_NaN() :
// indistinguishable from one or zero
GCLIM<T>::epsilon() > abs(x - T(1)) ? \
T(0) :
GCLIM<T>::epsilon() > abs(x) ? \
T(GCEM_HALF_PI) :
// else
atan( sqrt(T(1) - x*x)/x ) );
}

template<typename T>
constexpr
T
acos_check(const T x)
noexcept
{
return( // NaN check
is_nan(x) ? \
GCLIM<T>::quiet_NaN() :
//
x > T(0) ? \
// if
acos_compute(x) :
// else
T(GCEM_PI) - acos_compute(-x) );
}

}

/**
* Compile-time arccosine function
*
* @param x a real-valued input, where \f$ x \in [-1,1] \f$.
* @return the inverse cosine function using \f[ \text{acos}(x) = \text{atan} \left( \frac{\sqrt{1-x^2}}{x} \right) \f]
*/

template<typename T>
constexpr
return_t<T>
acos(const T x)
noexcept
{
return internal::acos_check( static_cast<return_t<T>>(x) );
}

#endif
Loading

0 comments on commit 97207f1

Please sign in to comment.