92 lines
2.2 KiB
C++
92 lines
2.2 KiB
C++
|
|
/// \cond
|
|
// C headers
|
|
// C++ headers
|
|
#include <chrono>
|
|
#include <iostream>
|
|
#include <sstream>
|
|
#include <utility>
|
|
|
|
// 3rd party headers
|
|
/// \endcond
|
|
|
|
// qtrocket headers
|
|
#include "Propagator.h"
|
|
|
|
#include "sim/RK4Solver.h"
|
|
#include "utils/Logger.h"
|
|
|
|
namespace sim
|
|
{
|
|
|
|
Propagator::Propagator(std::shared_ptr<model::Propagatable> r)
|
|
: linearIntegrator(),
|
|
object(r),
|
|
saveStates(true),
|
|
timeStep(0.01)
|
|
{
|
|
// Linear velocity and acceleration
|
|
std::function<std::pair<Vector3, Vector3>(Vector3&, Vector3&)> linearODEs = [this](Vector3& state, Vector3& rate) -> std::pair<Vector3, Vector3>
|
|
{
|
|
Vector3 dPosition;
|
|
Vector3 dVelocity;
|
|
// dx/dt
|
|
dPosition = rate;
|
|
|
|
// dvx/dt
|
|
dVelocity = object->getForces(currentTime) / object->getMass(currentTime);
|
|
|
|
return std::make_pair(dPosition, dVelocity);
|
|
};
|
|
|
|
linearIntegrator.reset(new RK4Solver<Vector3>(linearODEs));
|
|
linearIntegrator->setTimeStep(timeStep);
|
|
|
|
saveStates = true;
|
|
}
|
|
|
|
Propagator::~Propagator()
|
|
{
|
|
}
|
|
|
|
void Propagator::runUntilTerminate()
|
|
{
|
|
std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
|
|
std::chrono::steady_clock::time_point endTime;
|
|
|
|
Vector3 currentPosition;
|
|
Vector3 currentVelocity;
|
|
Vector3 nextPosition;
|
|
Vector3 nextVelocity;
|
|
while(true)
|
|
{
|
|
currentPosition = object->getCurrentState().position;
|
|
currentVelocity = object->getCurrentState().velocity;
|
|
|
|
std::tie(nextPosition, nextVelocity) = linearIntegrator->step(currentPosition, currentVelocity);
|
|
|
|
StateData nextState;
|
|
nextState.position = nextPosition;
|
|
nextState.velocity = nextVelocity;
|
|
object->setCurrentState(nextState);
|
|
|
|
if(saveStates)
|
|
{
|
|
object->appendState(currentTime, nextState);
|
|
}
|
|
if(object->terminateCondition(currentTime))
|
|
break;
|
|
|
|
currentTime += timeStep;
|
|
}
|
|
endTime = std::chrono::steady_clock::now();
|
|
|
|
std::stringstream duration;
|
|
duration << "runUntilTerminate time (microseconds): ";
|
|
duration << std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTime).count();
|
|
utils::Logger::getInstance()->debug(duration.str());
|
|
|
|
}
|
|
|
|
} // namespace sim
|