C++ StateType::begin方法代码示例

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];

//.........这里部分代码省略.........