// #include <QuantumStateITPP.h>
template <class StateType> StateType StateTensor(const StateType& psi_1, const StateType& psi_2){
StateType tmp(psi_1.size()*psi_2.size());
for (int i =0; i<psi_1.size(); i++){
tmp.coefficients.set_subvector(i*psi_2.size(),psi_1.coefficients(i)*psi_2.coefficients);
}
return tmp;
}
void VarianceDiffusionStrategy<StateType>::diffuse(const ParticleList &_src, ParticleList &_dest)
{
if(_src.size() == 0) return;
if(_dest.size() != _src.size()) _dest = _src;
const unsigned numDofs = _src[0].state.size();
unsigned index = 0;
StateType noise = _dest[0].state;
if(mVariance.size() != noise.size())
{
TRACE("------------- variance has wrong size. (should not be possible) ---------- ABORTING");
return;
}
if(mVarianceFactor.size() != noise.size())
{
TRACE("----------resizing variance factors. (should not happen) -------------- ");
mVarianceFactor.resize(noise.size(), 1);
}
// iterate over all source particles
for(ParticleListConstIterator it = _src.begin(); it != _src.end(); it++)
{
// for every dof
for(unsigned i=0; i < numDofs; i++)
{
noise[i] = this->mNormDistGenerator() * mVariance[i] * mVarianceFactor[i];
}
_dest[index].state = noise;
_dest[index].state += _src[index].state;
index++;
}
}
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;
}