33 #include "autodiff/forward/dual.hpp"
34 #include "autodiff/forward/real.hpp"
36 #include <autodiff/forward/dual/dual.hpp>
43 template <
typename T >
55 double exp(
double x );
63 constexpr
double radToDeg(
const double alpha )
70 constexpr
double degToRad(
const double alpha )
79 return scalar >= 0 ? scalar : 0.0;
86 return scalar >= 0 ? 1 : 0;
91 template <
typename T >
92 constexpr T
sgn( T val )
94 return val / std::abs( val );
97 double makeReal(
const double& value );
98 double makeReal(
const std::complex< double >& value );
99 double makeReal(
const autodiff::real& value );
101 template <
typename T,
typename G >
102 double makeReal(
const autodiff::detail::Dual< T, G >& number )
104 return double( number );
107 template <
typename T,
int... Rest >
108 Eigen::Matrix< double, Rest... >
makeReal(
const Eigen::Matrix< T, Rest... > mat )
110 Eigen::Matrix< double, Rest... > out;
111 const size_t m =
static_cast< size_t >( mat.rows() );
112 const size_t n =
static_cast< size_t >( mat.cols() );
114 for (
size_t i = 0; i < m; i++ ) {
115 for (
size_t j = 0; j < n; j++ ) {
116 out( i, j ) =
makeReal( mat( i, j ) );
121 template <
typename T >
122 Eigen::VectorXd
makeReal( Eigen::Vector< T, Eigen::Dynamic > in )
125 int inSize = in.size();
126 Eigen::VectorXd out( inSize );
127 for (
int i = 0; i < inSize; i++ ) {
128 out( i ) = double( in( i ) );
136 template <
int nRows,
int nCols >
137 Eigen::Matrix< double, nRows, nCols >
macaulyMatrix(
const Eigen::Matrix< double, nRows, nCols >& mat )
139 Eigen::Matrix< double, nRows, nCols > positivePart = Eigen::Matrix< double, nRows, nCols >::Zero();
140 for (
int i = 0; i < nRows; i++ ) {
141 for (
int j = 0; j < nCols; j++ ) {
142 positivePart( i, j ) =
macauly( mat( i, j ) );
150 template <
typename functionType,
typename yType,
typename... Args >
151 yType
explicitEuler( yType yN,
const double dt, functionType fRate, Args&&... fRateArgs )
153 return yN + fRate( yN, fRateArgs... ) * dt;
161 template <
int ySize,
typename functionType,
typename... Args >
165 Args&&... fRateArgs )
167 Eigen::Matrix< double, ySize, ySize > fS = Eigen::Matrix< double, ySize, ySize >::Zero();
168 Eigen::Matrix< double, ySize, ySize > Iy = Eigen::Matrix< double, ySize, ySize >::Identity();
170 Eigen::VectorXd leftX( ySize );
171 Eigen::VectorXd rightX( ySize );
173 for (
size_t i = 0; i < ySize; i++ ) {
179 fS.col( i ) = 1. / ( 2. * h ) * ( fRate( rightX, fRateArgs... ) - fRate( leftX, fRateArgs... ) );
182 return yN + ( Iy - dt * fS ).inverse() * dt * fRate( yN, fRateArgs... );
188 template <
int nRows,
int nCols,
typename functionType >
189 Eigen::Matrix< double, nRows, nCols >
centralDiff( functionType f,
const Eigen::Matrix< double, nCols, 1 >& X )
191 Eigen::Matrix< double, nRows, nCols > dXdY = Eigen::Matrix< double, nRows, nCols >::Zero();
193 Eigen::VectorXd leftX( nCols );
194 Eigen::VectorXd rightX( nCols );
196 for (
size_t i = 0; i < nCols; i++ ) {
202 dXdY.col( i ) = 1. / ( 2. * h ) * ( f( rightX ) - f( leftX ) );
212 template <
typename functionType,
typename yType,
typename... Args >
215 yType fN = fRate( yN, fRateArgs... );
216 yType u = yN + fN * dt;
217 yType v = yN + fN * dt / 2.;
218 yType w = v + fRate( v, fRateArgs... ) * dt / 2.;
227 template <
int ySize,
typename functionType,
typename... Args >
229 Eigen::Matrix< double, ySize, 1 > yN,
233 Args&&... fRateArgs )
236 typedef Eigen::Matrix< double, ySize, 1 > ySized;
237 ySized fN = fRate( yN, fRateArgs... );
238 ySized u = yN + fN * dt;
239 ySized v = yN + fN * dt / 2.;
240 ySized w = v + fRate( v, fRateArgs... ) * dt / 2.;
241 ySized yNew = 2. * w - u;
244 const double AERR = 1.0;
245 const double aI = AERR / TOL;
246 const double rI = 1.0;
248 ySized ESTVec = ySized::Zero();
250 for (
int i = 0; i < ySize; i++ ) {
251 scaling = aI + rI * std::max( abs( yNew( i ) ), abs( yN( i ) ) );
252 ESTVec( i ) = abs( w( i ) - u( i ) ) / abs( scaling );
255 const double EST = ESTVec.maxCoeff();
256 const double tauNew = dt * std::min( 2., std::max( 0.2, 0.9 * std::sqrt( TOL / EST ) ) );
258 return std::make_tuple( yNew, tauNew );