int main()
{
int cnt = 0;
bool expected = false;
try
{
std::cerr<<std::endl<< "Point 1 - code should show points 1..3, no gaps."<<std::endl;
int i = 0;
OTL_ASSERT(i < 1); // 0 < 1
cnt++;
std::cerr<<std::endl<< "Point 2"<<std::endl;
i = 1;
expected = true;
cnt++;
OTL_ASSERT(i < 1); // 1 < 1
cnt = 1000;
}
catch(my_base_exc_class& p)
{
std::cerr<<std::endl<< "Point 3: "<<(expected?"expected error":"**UNEXPECTED** error")<<std::endl;
// intercept OTL 'ex' exceptions
std::cerr<<p.what()<<std::endl; // print out error message
if (expected) cnt++; else cnt = 1000;
}
return (cnt == 3 ? 0 /* EXIT_SUCCESS */ : 1 /* EXIT_FAILURE */ );
}
DepartureNode MGADSMTrajectory::GetDeparture() const
{
OTL_ASSERT(!m_nodes.empty(), "Can't set Departure node. Node not found.");
auto depNode = dynamic_cast<DepartureNode*>(m_nodes.front().get());
OTL_ASSERT(depNode, "Failed to convert node to Departure node.");
return *depNode;
}
InsertionNode MGADSMTrajectory::GetInsertion(int n) const
{
int nodeIndex = GetInsertionIndex(n);
OTL_ASSERT(nodeIndex >= 0, "Can't get Flyby node. Node not found.");
auto insNode = dynamic_cast<InsertionNode*>(m_nodes[nodeIndex].get());
OTL_ASSERT(insNode, "Failed to convert node to Insertion node.");
return *insNode;
}
RendezvousNode MGADSMTrajectory::GetRendezvous(int n) const
{
int nodeIndex = GetRendezvousIndex(n);
OTL_ASSERT(nodeIndex >= 0, "Can't get Rendezvous node. Node not found.");
auto renNode = dynamic_cast<RendezvousNode*>(m_nodes[nodeIndex].get());
OTL_ASSERT(renNode, "Failed to convert node to Rendezvous node.");
return *renNode;
}
FlybyNode MGADSMTrajectory::GetFlyby(int n) const
{
int nodeIndex = GetFlybyIndex(n);
OTL_ASSERT(nodeIndex >= 0, "Can't get Flyby node. Node not found.");
auto flyNode = dynamic_cast<FlybyNode*>(m_nodes[nodeIndex].get());
OTL_ASSERT(flyNode, "Failed to convert node to Flyby node.");
return *flyNode;
}
DSMNode MGADSMTrajectory::GetDSM(int n) const
{
int nodeIndex = GetDSMIndex(n);
OTL_ASSERT(nodeIndex >= 0, "Can't get DSM node. Node not found.");
auto dsmNode = dynamic_cast<DSMNode*>(m_nodes[nodeIndex].get());
OTL_ASSERT(dsmNode, "Failed to convert node to DSM node.");
return *dsmNode;
}
EulerAngles::EulerAngles(const Vector3d& angles, int a1, int a2, int a3) :
angles(angles),
a1(a1),
a2(a2),
a3(a3)
{
OTL_ASSERT(a1 >= 1 && a1 <= 3, "Euler axis must be 1, 2, or 3");
OTL_ASSERT(a2 >= 1 && a2 <= 3, "Euler axis must be 1, 2, or 3");
OTL_ASSERT(a3 >= 1 && a3 <= 3, "Euler axis must be 1, 2, or 3");
}
void MGADSMTrajectory::SetNode(int n, const TrajectoryNodePtr& node)
{
OTL_ASSERT(node, "Can't set node. Invalid node");
OTL_ASSERT(n >= 0 && n < static_cast<int>(m_nodes.size()), "Can't set node. Index out of bounds.");
OTL_ASSERT(m_nodes[n]->GetType() == node->GetType(), "Can't set node. Type mismatch.");
m_nodes[n] = node;
m_legsInitialized = false;
}
void MGADSMTrajectory::SetNodes(int n, const std::vector<TrajectoryNodePtr>& nodes)
{
OTL_ASSERT(n >= 0, "Invalid node index");
for (std::size_t i = 0; i < nodes.size(); ++i)
{
SetNode(n + i, nodes[i]);
}
}
void MGADSMTrajectory::AddNode(const TrajectoryNodePtr& node)
{
OTL_ASSERT(node, "Can't add node. Invalid node.");
if (m_nodes.empty() && node->GetType() != TrajectoryNode::Type::Departure)
{
OTL_ASSERT(false, "Can't add node. The first node must be of type Departure.");
}
if (!m_nodes.empty() && node->GetType() == TrajectoryNode::Type::Departure)
{
OTL_ASSERT(false, "Can't add node. Only one Departure node allowed.");
}
m_nodes.push_back(node);
m_numNodes = m_nodes.size();
m_legsInitialized = false;
}
void MGADSMTrajectory::SetLambertType(LambertType type)
{
switch (type)
{
case LambertType::MultiRev:
m_lambert = std::unique_ptr<ILambertAlgorithm>(new LambertExponentialSinusoid());
break;
case LambertType::Invalid:
default:
OTL_ASSERT(false, "Can't set Lambert algorithm. Uknown or invalid type.");
break;
}
}
void MGADSMTrajectory::SetPropagateType(PropagatorType type)
{
switch (type)
{
case PropagatorType::Keplerian:
//m_propagator = std::unique_ptr<otl::IPropagator>(new KeplerianPropagator());
break;
case PropagatorType::Invalid:
default:
OTL_ASSERT(false, "Can't set Propagate algorithm. Uknown or invalid type.");
break;
}
}
void MGADSMTrajectory::SetFlybyType(FlybyType type)
{
switch (type)
{
case FlybyType::Unpowered:
m_flyby = std::unique_ptr<IFlybyAlgorithm>(new UnpoweredFlyby());
break;
case FlybyType::Invalid:
default:
OTL_ASSERT(false, "Can't set Flyby algorithm. Uknown or invalid type.");
break;
}
}
void MGADSMTrajectory::SetStateVector(const std::vector<double>& states)
{
if (!m_legsInitialized)
{
CalculateLegs();
}
if (states.size() != m_states.size())
{
OTL_ASSERT(false, "Can't set state vector. Dimension mismatch.");
}
else
{
// UpdateNodeStates(states); [TODO]
m_states = states;
}
}
int MGADSMTrajectory::GetInsertionIndex(int insertion) const
{
OTL_ASSERT(insertion >= 0, "Invalid Insertion node index");
int nodeIndex = -1;
int numInsertions = 0;
for (std::size_t i = 0; i < m_nodes.size(); ++i)
{
if (m_nodes[i]->GetType() == TrajectoryNode::Type::Insertion)
{
numInsertions++;
}
if (insertion == numInsertions)
{
nodeIndex = i;
break;
}
}
return nodeIndex;
}
int MGADSMTrajectory::GetRendezvousIndex(int rendezvous) const
{
OTL_ASSERT(rendezvous >= 0, "Invalid Rendezvous node index");
int nodeIndex = -1;
int numRendezvous = 0;
for (std::size_t i = 0; i < m_nodes.size(); ++i)
{
if (m_nodes[i]->GetType() == TrajectoryNode::Type::Rendezvous)
{
numRendezvous++;
}
if (rendezvous == numRendezvous)
{
nodeIndex = i;
break;
}
}
return nodeIndex;
}
int MGADSMTrajectory::GetFlybyIndex(int flyby) const
{
OTL_ASSERT(flyby >= 0, "Invalid Flyby node index");
int nodeIndex = -1;
int numFlybys = 0;
for (std::size_t i = 0; i < m_nodes.size(); ++i)
{
if (m_nodes[i]->GetType() == TrajectoryNode::Type::Flyby)
{
numFlybys++;
}
if (flyby == numFlybys)
{
nodeIndex = i;
break;
}
}
return nodeIndex;
}
int MGADSMTrajectory::GetDSMIndex(int dsm) const
{
OTL_ASSERT(dsm >= 0, "Invalid DSM node index");
int nodeIndex = -1;
int numDSMs = 0;
for (std::size_t i = 0; i < m_nodes.size(); ++i)
{
if (m_nodes[i]->GetType() == TrajectoryNode::Type::DSM)
{
numDSMs++;
}
if (dsm == numDSMs)
{
nodeIndex = i;
break;
}
}
return nodeIndex;
}
Quaterniond Rotation::GetQuaternion() const
{
OTL_ASSERT(m_type == RotationType::Quaternion, "Invalid rotation type");
return Quaterniond(m_matrix);
}
EulerAngles Rotation::GetEulerAngles() const
{
OTL_ASSERT(m_type == RotationType::Euler, "Invalid rotation type");
return EulerAngles(m_matrix.eulerAngles(1, 2, 3), 1, 2, 3);
}
Matrix3d Rotation::GetRotationMatrix() const
{
OTL_ASSERT(m_type == RotationType::Matrix, "Invalid rotation type");
return m_matrix;
}
const std::vector<double>& MGADSMTrajectory::Evaluate(const std::vector<double>& states)
{
OTL_ASSERT((int)states.size() == m_numStates, "Invalid state vector. Dimension mismatch");
Evaluate(states, m_deltaVs);
return m_deltaVs;
}
void MGADSMTrajectory::Evaluate(const std::vector<double>& states, std::vector<double>& deltaVs)
{
OTL_ASSERT((int)states.size() == m_numStates, "Invalid state vector. Dimension mismatch");
CalculateTrajectory(states, deltaVs);
}
void MGADSMTrajectory::CalculateLegs()
{
// Detect a valid node configuration
if (m_nodes.size() <= 1)
{
OTL_ASSERT(false, "Invalid trajectory. Trajectory must consist of at least two nodes.");
}
if (m_nodes.front()->GetType() != TrajectoryNode::Type::Departure)
{
OTL_ASSERT(false, "Invalid trajectory. Trajectory must start with a Departure node.");
}
if (m_nodes.back()->GetType() != TrajectoryNode::Type::Flyby &&
m_nodes.back()->GetType() != TrajectoryNode::Type::Rendezvous &&
m_nodes.back()->GetType() != TrajectoryNode::Type::Insertion)
{
OTL_ASSERT(false, "Invalid trajectory. Trajectory must end with a Flyby, Rendezvous, or Insertion node.");
}
m_numStates = 0;
m_legs.clear();
TrajectoryLeg leg;
for (int i = 0; i < m_numNodes; ++i)
{
// Departure
if (m_nodes[i]->GetType() == TrajectoryNode::Type::Departure)
{
auto depNode = dynamic_cast<DepartureNode*>(m_nodes[i].get());
OTL_ASSERT(depNode, "Failed to convert node to Departure node.");
leg.departure = true;
leg.initialPlanet = Planet(depNode->orbitalBody);
AddState(depNode->epoch.GetMJD2000());
// Only if followed by a DSM
if (m_nodes[i+1]->GetType() == TrajectoryNode::Type::DSM)
{
AddState(depNode->deltaV.x());
AddState(depNode->deltaV.y());
AddState(depNode->deltaV.z());
}
}
// DSM
else if (m_nodes[i]->GetType() == TrajectoryNode::Type::DSM)
{
auto dsmNode = dynamic_cast<DSMNode*>(m_nodes[i].get());
OTL_ASSERT(dsmNode, "Failed to convert node to DSM node.");
leg.numDSM++;
AddState(dsmNode->alpha);
// Only for more than one DSM
if (leg.numDSM > 1)
{
AddState(dsmNode->deltaV.x());
AddState(dsmNode->deltaV.y());
AddState(dsmNode->deltaV.z());
}
}
// Flyby
else if (m_nodes[i]->GetType() == TrajectoryNode::Type::Flyby)
{
auto flyNode = dynamic_cast<FlybyNode*>(m_nodes[i].get());
OTL_ASSERT(flyNode, "Failed to convert node to Flyby node.");
leg.flyby = true;
leg.finalPlanet = Planet(flyNode->orbitalBody);
leg.timeofFlightIndex = m_numStates;
AddState(flyNode->timeOfFlight * MATH_DAY_TO_SEC);
AddState(flyNode->altitude);
AddState(flyNode->bInclinationAngle);
}
// Rendezvous
else if (m_nodes[i]->GetType() == TrajectoryNode::Type::Rendezvous)
{
auto renNode = dynamic_cast<RendezvousNode*>(m_nodes[i].get());
OTL_ASSERT(renNode, "Failed to convert node to Rendezvous node.");
leg.rendezvous = true;
leg.finalPlanet = Planet(renNode->orbitalBody);
leg.timeofFlightIndex = m_numStates;
AddState(renNode->timeOfFlight * MATH_DAY_TO_SEC);
}
// Insertion
else if (m_nodes[i]->GetType() == TrajectoryNode::Type::Insertion)
{
auto insNode = dynamic_cast<InsertionNode*>(m_nodes[i].get());
OTL_ASSERT(insNode, "Failed to convert node to Insertion node.");
leg.insertion = true;
leg.insertionOrbit = insNode->orbitalElements;
leg.finalPlanet = Planet(insNode->orbitalBody);
leg.timeofFlightIndex = m_numStates;
AddState(insNode->timeOfFlight * MATH_DAY_TO_SEC);
AddState(insNode->timeOfOrbit * MATH_DAY_TO_SEC);
if (m_nodes[i+1]->GetType() == TrajectoryNode::Type::DSM)
{
AddState(insNode->deltaV.x());
AddState(insNode->deltaV.y());
AddState(insNode->deltaV.z());
}
}
// Finished a leg, save it and start a new one
if (m_nodes[i]->GetType() == TrajectoryNode::Type::Flyby ||
m_nodes[i]->GetType() == TrajectoryNode::Type::Rendezvous ||
m_nodes[i]->GetType() == TrajectoryNode::Type::Insertion)
{
m_legs.push_back(leg);
leg = TrajectoryLeg();
// Set the initial planet of the new leg to the final planet of the previous leg
leg.initialPlanet = m_legs.back().finalPlanet;
}
}
m_legsInitialized = true;
}
