void gram_schmidt( StateType &x , LyapType &lyap , size_t n )
{
if( !num_of_lyap ) return;
if( ptrdiff_t( ( num_of_lyap + 1 ) * n ) != std::distance( x.begin() , x.end() ) )
throw std::domain_error( "renormalization() : size of state does not match the number of lyapunov exponents." );
typedef typename StateType::value_type value_type;
typedef typename StateType::iterator iterator;
value_type norm[num_of_lyap];
value_type tmp[num_of_lyap];
iterator first = x.begin() + n;
iterator beg1 = first , end1 = first + n ;
std::fill( norm , norm+num_of_lyap , 0.0 );
// normalize first vector
norm[0] = sqrt( std::inner_product( beg1 , end1 , beg1 , 0.0 ) );
normalize( beg1 , end1 , norm[0] );
beg1 += n;
end1 += n;
for( size_t j=1 ; j<num_of_lyap ; ++j , beg1+=n , end1+=n )
{
for( size_t k=0 ; k<j ; ++k )
{
tmp[k] = std::inner_product( beg1 , end1 , first + k*n , 0.0 );
// clog << j << " " << k << " " << tmp[k] << "\n";
}
for( size_t k=0 ; k<j ; ++k )
substract_vector( beg1 , end1 , first + k*n , tmp[k] );
// normalize j-th vector
norm[j] = sqrt( std::inner_product( beg1 , end1 , beg1 , 0.0 ) );
// clog << j << " " << norm[j] << "\n";
normalize( beg1 , end1 , norm[j] );
}
for( size_t j=0 ; j<num_of_lyap ; j++ )
lyap[j] += log( norm[j] );
}
void
RigidBody::GetState(StateType& State)
{
// Copy the elements of the member variables that describe the state of
// the rigid body into the state vector.
StateType::iterator it = State.begin();
if (State.size() != m_CountStateVectors)
{
State.resize(m_CountStateVectors);
}
*it++ = m_Position;
*it++ = bnu::column(m_Rotate, 0);
*it++ = bnu::column(m_Rotate, 1);
*it++ = bnu::column(m_Rotate, 2);
*it++ = m_AngularMomentum;
*it = m_LinearMomentum;
}
void
RigidBody::GetStateDerivative(StateType& State)
{
//Work out the state derivatives and place them in State
StateType::iterator it = State.begin();
bnu::vector<double> AngularMomentum;
bnu::vector<double> Temp0;
bnu::vector<double> Temp1;
if (State.size() != m_CountStateVectors)
{
State.resize(m_CountStateVectors);
}
bnu::vector<double> LinearVelocity(m_LinearMomentum/m_Mass);
/* Inertia Tensor = R*IBodyInv*RTranspose*/
bnu::matrix<double> InertiaTensorInv((bnu::prod(m_Rotate, m_BodySpaceInertiaTensorInv)));
InertiaTensorInv = bnu::prod(InertiaTensorInv, bnu::trans(m_Rotate));
/* omega = IInv * angularmomentum*/
bnu::vector<double> Omega(bnu::prod(InertiaTensorInv, m_AngularMomentum));
*it++ = LinearVelocity;
/* work out the derivative of R(t) and copy the result into the state array*/
Temp0 = bnu::column(m_Rotate, 0);
Cross(m_AngularMomentum, Temp0, Temp1);
*it++ = Temp1;
Temp0 = bnu::column(m_Rotate, 1);
Cross(m_AngularMomentum, Temp0, Temp1);
*it++ = Temp1;
Temp0 = bnu::column(m_Rotate, 2);
Cross(m_AngularMomentum, Temp0, Temp1);
*it++ = Temp1;
/* copy force and torque into the array */
*it++ = m_Force;
*it++ = m_Torque;
}
inline int CashKarp54(const SystemType &dxdt, StateType &x, double &t,
double &h, double relTol, double absTol,
double maxStepSize) {
// Constants from Butcher tableau, see: http://en.wikipedia.org/wiki/Cash-Karp_method
// and http://en.wikipedia.org/wiki/Runge-Kutta_methods
const double c2 = 1.0 / 5.0;
const double c3 = 3.0 / 10.0;
const double c4 = 3.0 / 5.0;
const double c5 = 1.0;
const double c6 = 7.0 / 8.0;
const double b5th1 = 37.0 / 378.0;
const double b5th2 = 0.0;
const double b5th3 = 250.0 / 621.0;
const double b5th4 = 125.0 / 594.0;
const double b5th5 = 0.0;
const double b5th6 = 512.0 / 1771.0;
const double b4th1 = 2825.0 / 27648.0;
const double b4th2 = 0.0;
const double b4th3 = 18575.0 / 48384.0;
const double b4th4 = 13525.0 / 55296.0;
const double b4th5 = 277.0 / 14336.0;
const double b4th6 = 1.0 / 4.0;
const double bDiff1 = b5th1 - b4th1;
const double bDiff2 = b5th2 - b4th2;
const double bDiff3 = b5th3 - b4th3;
const double bDiff4 = b5th4 - b4th4;
const double bDiff5 = b5th5 - b4th5;
const double bDiff6 = b5th6 - b4th6;
const double a21 = 1.0 / 5.0;
const double a31 = 3.0 / 40.0;
const double a32 = 9.0 / 40.0;
const double a41 = 3.0 / 10.0;
const double a42 = -9.0 / 10.0;
const double a43 = 6.0 / 5.0;
const double a51 = -11.0 / 54.0;
const double a52 = 5.0 / 2.0;
const double a53 = -70.0 / 27.0;
const double a54 = 35.0 / 27.0;
const double a61 = 1631.0 / 55296.0;
const double a62 = 175.0 / 512.0;
const double a63 = 575.0 / 13824.0;
const double a64 = 44275.0 / 110592.0;
const double a65 = 253.0 / 4096.0;
const std::size_t stateSize = x.size();
StateType tempState; // used to store state for next k value and later used
// for error difference
StateType k1;
dxdt(x, k1, t); // fill k1
for (std::size_t i = 0; i < stateSize; ++i)
tempState[i] = x[i] + h * a21 * k1[i];
StateType k2;
dxdt(tempState, k2, t + c2 * h); // fill k2
for (std::size_t i = 0; i < stateSize; ++i)
tempState[i] = x[i] + h * (a31 * k1[i] + a32 * k2[i]);
StateType k3;
dxdt(tempState, k3, t + c3 * h); // fill k3
for (std::size_t i = 0; i < stateSize; ++i)
tempState[i] = x[i] + h * (a41 * k1[i] + a42 * k2[i] + a43 * k3[i]);
StateType k4;
dxdt(tempState, k4, t + c4 * h); // fill k4
for (std::size_t i = 0; i < stateSize; ++i)
tempState[i] =
x[i] +
h * (a51 * k1[i] + a52 * k2[i] + a53 * k3[i] + a54 * k4[i]);
StateType k5;
dxdt(tempState, k5, t + c5 * h); // fill k5
for (std::size_t i = 0; i < stateSize; ++i)
tempState[i] = x[i] +
h * (a61 * k1[i] + a62 * k2[i] + a63 * k3[i] +
a64 * k4[i] + a65 * k5[i]);
StateType k6;
dxdt(tempState, k6, t + c6 * h); // fill k6
StateType order5Solution;
for (std::size_t i = 0; i < stateSize; ++i)
order5Solution[i] =
h * (b5th1 * k1[i] + b5th2 * k2[i] + b5th3 * k3[i] +
b5th4 * k4[i] + b5th5 * k5[i] + b5th6 * k6[i]);
// difference between order 4 and 5, used for error check, reusing tempState
// variable
for (std::size_t i = 0; i < stateSize; ++i)
tempState[i] = h * (bDiff1 * k1[i] + bDiff2 * k2[i] + bDiff3 * k3[i] +
bDiff4 * k4[i] + bDiff5 * k5[i] + bDiff6 * k6[i]);
StateType potentialSolution;
for (std::size_t i = 0; i < stateSize; ++i)
potentialSolution[i] = x[i] + order5Solution[i];
// boost odeint syle error step sizing method
StateType errorValueList;
for (std::size_t i = 0; i < stateSize; ++i)
errorValueList[i] =
std::abs(tempState[i] / (absTol + relTol * (potentialSolution[i])));
double maxErrorValue =
*(std::max_element(errorValueList.begin(), errorValueList.end()));
// reject step and decrease step size
if (maxErrorValue > 1.0) {
h = h * std::max(0.9 * std::pow(maxErrorValue, -0.25), 0.2);
return 0;
}
// use the step
t += h;
for (std::size_t i = 0; i < stateSize; ++i)
x[i] = potentialSolution[i];
// if error is small enough then increase step size
if (maxErrorValue < 0.5) {
h = std::min(h * std::min(0.9 * std::pow(maxErrorValue, -0.20), 5.0),
maxStepSize);
}
return 1;
}