diff --git a/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile.cpp b/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile.cpp index f585be59c..13c322692 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile.cpp @@ -90,14 +90,17 @@ State Profile::getStateAt ( throw ostk::core::error::runtime::Undefined("Profile") ; } - const Transform transform = transformProvider_.getTransformAt(anInstant) ; + const Transform T_REF_B = transformProvider_.getTransformAt(anInstant) ; + + const Quaternion q_REF_B = T_REF_B.getOrientation() ; + const Vector3d w_REF_B_in_REF = T_REF_B.getAngularVelocity() ; - const Vector3d position = transform.applyToPosition({ 0.0, 0.0, 0.0 }) ; - const Vector3d velocity = transform.applyToVelocity({ 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 }) ; - const Quaternion attitude = transform.getOrientation().toConjugate() ; - const Vector3d angularVelocity = transform.getAngularVelocity() ; // [TBC] + const Vector3d x_REF = T_REF_B.applyToPosition({ 0.0, 0.0, 0.0 }) ; + const Vector3d v_REF_in_REF = T_REF_B.applyToVelocity({ 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 }) ; + const Quaternion q_B_REF = q_REF_B.toConjugate() ; + const Vector3d w_B_REF_in_B = q_B_REF * (w_REF_B_in_REF * -1.0) ; - return { anInstant, position, velocity, attitude, angularVelocity, frameSPtr_ } ; + return { anInstant, x_REF, v_REF_in_REF, q_B_REF, w_B_REF_in_B, frameSPtr_ } ; } diff --git a/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.cpp b/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.cpp index d08c07be3..ce9a141cf 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.cpp @@ -207,14 +207,34 @@ State State::inFrame ( throw ostk::core::error::runtime::Undefined("State") ; } + const Vector3d& x_OLD = position_ ; + const Vector3d& v_OLD_in_OLD = velocity_ ; + const Quaternion& q_B_OLD = attitude_ ; + const Vector3d& w_B_OLD_in_B = angularVelocity_ ; + const Transform transform_NEW_OLD = frameSPtr_->getTransformTo(aFrameSPtr, instant_) ; - const Vector3d position = transform_NEW_OLD.applyToPosition(position_) ; - const Vector3d velocity = transform_NEW_OLD.applyToVelocity(position_, velocity_) ; - const Quaternion attitude = attitude_ * transform_NEW_OLD.getOrientation().toConjugate() ; - const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // transform_NEW_OLD.getAngularVelocity() [TBI] ; + const Quaternion q_NEW_OLD = transform_NEW_OLD.getOrientation() ; + const Vector3d w_NEW_OLD_in_NEW = transform_NEW_OLD.getAngularVelocity() ; + + // x_NEW = T_NEW_OLD(x_OLD) + + const Vector3d x_NEW = transform_NEW_OLD.applyToPosition(x_OLD) ; + + // v_NEW = T_NEW_OLD(v_OLD) + + const Vector3d v_NEW_in_NEW = transform_NEW_OLD.applyToVelocity(x_OLD, v_OLD_in_OLD) ; + + // q_B_NEW = q_B_OLD * q_OLD_NEW + + const Quaternion q_B_NEW = q_B_OLD * q_NEW_OLD.toConjugate() ; + + // w_B_NEW_in_B = w_B_OLD_in_B + w_OLD_NEW_in_B + // = w_B_OLD_in_B - q_B_NEW * w_NEW_OLD_in_NEW + + const Vector3d w_B_NEW_in_B = w_B_OLD_in_B - q_B_NEW * w_NEW_OLD_in_NEW ; - return { instant_, position, velocity, attitude, angularVelocity, aFrameSPtr } ; + return { instant_, x_NEW, v_NEW_in_NEW, q_B_NEW, w_B_NEW_in_B, aFrameSPtr } ; } diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.cpp index 30bef35a4..171ce13d7 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -371,8 +372,10 @@ Shared Orbit::getOrbitalFrame ( using ostk::math::obj::Vector3d ; using ostk::math::geom::d3::trf::rot::Quaternion ; + using ostk::math::geom::d3::trf::rot::RotationVector ; using ostk::math::geom::d3::trf::rot::RotationMatrix ; + using ostk::physics::time::Duration ; using ostk::physics::coord::spherical::LLA ; using FrameManager = ostk::physics::coord::frame::Manager ; using DynamicProvider = ostk::physics::coord::frame::provider::Dynamic ; @@ -390,6 +393,42 @@ Shared Orbit::getOrbitalFrame ( return FrameManager::Get().accessFrameWithName(frameName) ; } + const auto generateDynamicProvider = [this] (const auto& anAttitudeGenerator, const Shared& aReferenceFrame) -> auto + { + + const Shared dynamicProviderSPtr = std::make_shared + ( + [this, anAttitudeGenerator, aReferenceFrame] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead + { + + const State state = this->getStateAt(anInstant).inFrame(aReferenceFrame) ; + + const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ; + const Vector3d v_GCRF_in_GCRF = state.accessVelocity().accessCoordinates() ; + + const Vector3d x_VVLH_GCRF_in_GCRF = -x_GCRF ; // [m] + const Vector3d v_VVLH_GCRF_in_GCRF = -v_GCRF_in_GCRF ; // [m/s] + + const Quaternion q_VVLH_GCRF = anAttitudeGenerator(state) ; + + const Duration delta = Duration::Seconds(1.0) ; // TBM This should be a parameter + + const Quaternion q_VVLH_next_GCRF = anAttitudeGenerator(this->getStateAt(anInstant + delta).inFrame(Frame::GCRF())) ; + + const Quaternion q_VVLH_next_VVLH = q_VVLH_next_GCRF * q_VVLH_GCRF.toConjugate() ; + const RotationVector rv_VVLH_next_VVLH = RotationVector::Quaternion(q_VVLH_next_VVLH) ; + + const Vector3d w_VVLH_GCRF_in_VVLH = rv_VVLH_next_VVLH.getAxis() * (rv_VVLH_next_VVLH.getAngle().inRadians() / delta.inSeconds()) ; // [rad/s] + + return { anInstant, x_VVLH_GCRF_in_GCRF, v_VVLH_GCRF_in_GCRF, q_VVLH_GCRF, w_VVLH_GCRF_in_VVLH, Transform::Type::Passive } ; + + } + ) ; + + return dynamicProviderSPtr ; + + } ; + Shared orbitalFrameSPtr = nullptr ; switch (aFrameType) @@ -398,32 +437,28 @@ Shared Orbit::getOrbitalFrame ( case Orbit::FrameType::NED: { - const Shared dynamicProviderSPtr = std::make_shared - ( - [this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead - { + const auto calculateAttitude = [this] (const State& aState) -> Quaternion + { - // Get state in central body centered, central body fixed frame + // Get state in central body centered, central body fixed frame - const State state = this->getStateAt(anInstant).inFrame(celestialObjectSPtr_->accessFrame()) ; + const State state = this->getStateAt(aState.getInstant()).inFrame(celestialObjectSPtr_->accessFrame()) ; - // Express the state position in geodetic coordinates + // Express the state position in geodetic coordinates - const LLA lla = LLA::Cartesian(state.accessPosition().accessCoordinates(), celestialObjectSPtr_->getEquatorialRadius(), celestialObjectSPtr_->getFlattening()) ; + const LLA lla = LLA::Cartesian(state.accessPosition().accessCoordinates(), celestialObjectSPtr_->getEquatorialRadius(), celestialObjectSPtr_->getFlattening()) ; - // Compute the NED frame to central body centered, central body fixed frame transform at position + // Compute the NED frame to central body centered, central body fixed frame transform at position - const Transform transform = ostk::physics::coord::frame::utilities::NorthEastDownTransformAt(lla, celestialObjectSPtr_->getEquatorialRadius(), celestialObjectSPtr_->getFlattening()) ; // [TBM] This should be optimized: LLA <> ECEF calculation done twice + const Transform transform = ostk::physics::coord::frame::utilities::NorthEastDownTransformAt(lla, celestialObjectSPtr_->getEquatorialRadius(), celestialObjectSPtr_->getFlattening()) ; // [TBM] This should be optimized: LLA <> ECEF calculation done twice - const Vector3d velocity = - state.accessVelocity().accessCoordinates() ; // [TBM] Check if derivation frame is correct - const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity + const Quaternion q_NED_GCRF = transform.getOrientation() ; - return { anInstant, transform.getTranslation(), velocity, transform.getOrientation(), angularVelocity, Transform::Type::Passive } ; + return q_NED_GCRF ; - } - ) ; + } ; - orbitalFrameSPtr = Frame::Construct(frameName, false, celestialObjectSPtr_->accessFrame(), dynamicProviderSPtr) ; + orbitalFrameSPtr = Frame::Construct(frameName, false, celestialObjectSPtr_->accessFrame(), generateDynamicProvider(calculateAttitude, celestialObjectSPtr_->accessFrame())) ; break ; @@ -436,31 +471,23 @@ Shared Orbit::getOrbitalFrame ( // Z axis along orbital momentum // Y axis toward velocity vector - const Shared dynamicProviderSPtr = std::make_shared - ( - [this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead - { - - const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ; - - const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ; - const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ; + const auto calculateAttitude = [] (const State& aState) -> Quaternion + { - const Vector3d xAxis = x_GCRF.normalized() ; - const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ; - const Vector3d yAxis = zAxis.cross(xAxis) ; + const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ; + const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ; - const Quaternion q_LVLH_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; + const Vector3d xAxis = x_GCRF.normalized() ; + const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ; + const Vector3d yAxis = zAxis.cross(xAxis) ; - const Vector3d velocity = -v_GCRF ; - const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity + const Quaternion q_LVLH_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; - return { anInstant, -x_GCRF, velocity, q_LVLH_GCRF, angularVelocity, Transform::Type::Passive } ; + return q_LVLH_GCRF ; - } - ) ; + } ; - orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), dynamicProviderSPtr) ; + orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ; break ; @@ -473,31 +500,23 @@ Shared Orbit::getOrbitalFrame ( // Y axis along negative orbital momentum // X axis toward velocity vector - const Shared dynamicProviderSPtr = std::make_shared - ( - [this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead - { - - const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ; - - const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ; - const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ; + const auto calculateAttitude = [] (const State& aState) -> Quaternion + { - const Vector3d zAxis = -x_GCRF.normalized() ; - const Vector3d yAxis = -x_GCRF.cross(v_GCRF).normalized() ; - const Vector3d xAxis = yAxis.cross(zAxis) ; + const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ; + const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ; - const Quaternion q_VVLH_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; + const Vector3d zAxis = -x_GCRF.normalized() ; + const Vector3d yAxis = -x_GCRF.cross(v_GCRF).normalized() ; + const Vector3d xAxis = yAxis.cross(zAxis) ; - const Vector3d velocity = -v_GCRF ; - const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity + const Quaternion q_VVLH_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; - return { anInstant, -x_GCRF, velocity, q_VVLH_GCRF, angularVelocity, Transform::Type::Passive } ; + return q_VVLH_GCRF ; - } - ) ; + } ; - orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), dynamicProviderSPtr) ; + orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ; break ; @@ -509,31 +528,23 @@ Shared Orbit::getOrbitalFrame ( // X axis along position vector // Z axis along orbital momentum - const Shared dynamicProviderSPtr = std::make_shared - ( - [this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead - { - - const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ; + const auto calculateAttitude = [] (const State& aState) -> Quaternion + { - const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ; - const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ; + const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ; + const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ; - const Vector3d xAxis = x_GCRF.normalized() ; - const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ; - const Vector3d yAxis = zAxis.cross(xAxis) ; + const Vector3d xAxis = x_GCRF.normalized() ; + const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ; + const Vector3d yAxis = zAxis.cross(xAxis) ; - const Quaternion q_QSW_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; + const Quaternion q_QSW_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; - const Vector3d velocity = -v_GCRF ; - const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity + return q_QSW_GCRF ; - return { anInstant, -x_GCRF, velocity, q_QSW_GCRF, angularVelocity, Transform::Type::Passive } ; + } ; - } - ) ; - - orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), dynamicProviderSPtr) ; + orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ; break ; @@ -545,31 +556,23 @@ Shared Orbit::getOrbitalFrame ( // X axis along velocity vector // Z axis along orbital momentum - const Shared dynamicProviderSPtr = std::make_shared - ( - [this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead - { - - const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ; - - const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ; - const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ; + const auto calculateAttitude = [] (const State& aState) -> Quaternion + { - const Vector3d xAxis = v_GCRF.normalized() ; - const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ; - const Vector3d yAxis = zAxis.cross(xAxis) ; + const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ; + const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ; - const Quaternion q_TNW_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; + const Vector3d xAxis = v_GCRF.normalized() ; + const Vector3d zAxis = x_GCRF.cross(v_GCRF).normalized() ; + const Vector3d yAxis = zAxis.cross(xAxis) ; - const Vector3d velocity = -v_GCRF ; - const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity + const Quaternion q_TNW_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; - return { anInstant, -x_GCRF, velocity, q_TNW_GCRF, angularVelocity, Transform::Type::Passive } ; + return q_TNW_GCRF ; - } - ) ; + } ; - orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), dynamicProviderSPtr) ; + orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ; break ; @@ -581,31 +584,23 @@ Shared Orbit::getOrbitalFrame ( // X axis along velocity vector // Y axis along orbital momentum - const Shared dynamicProviderSPtr = std::make_shared - ( - [this] (const Instant& anInstant) -> Transform // [TBI] Use shared_from_this instead - { - - const State state = this->getStateAt(anInstant).inFrame(Frame::GCRF()) ; - - const Vector3d x_GCRF = state.accessPosition().accessCoordinates() ; - const Vector3d v_GCRF = state.accessVelocity().accessCoordinates() ; + const auto calculateAttitude = [] (const State& aState) -> Quaternion + { - const Vector3d xAxis = v_GCRF.normalized() ; - const Vector3d yAxis = x_GCRF.cross(v_GCRF).normalized() ; - const Vector3d zAxis = xAxis.cross(yAxis) ; + const Vector3d x_GCRF = aState.accessPosition().accessCoordinates() ; + const Vector3d v_GCRF = aState.accessVelocity().accessCoordinates() ; - const Quaternion q_VNC_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; + const Vector3d xAxis = v_GCRF.normalized() ; + const Vector3d yAxis = x_GCRF.cross(v_GCRF).normalized() ; + const Vector3d zAxis = xAxis.cross(yAxis) ; - const Vector3d velocity = -v_GCRF ; - const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; // [TBI] Use orbital angular velocity + const Quaternion q_VNC_GCRF = Quaternion::RotationMatrix(RotationMatrix::Rows(xAxis, yAxis, zAxis)).rectify() ; - return { anInstant, -x_GCRF, velocity, q_VNC_GCRF, angularVelocity, Transform::Type::Passive } ; + return q_VNC_GCRF ; - } - ) ; + } ; - orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), dynamicProviderSPtr) ; + orbitalFrameSPtr = Frame::Construct(frameName, false, Frame::GCRF(), generateDynamicProvider(calculateAttitude, Frame::GCRF())) ; break ; diff --git a/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp index e46ab4840..612ac2eb4 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp @@ -337,7 +337,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, InertialPointing) const Real positionTolerance_m = 1e-3 ; const Real velocityTolerance_meterPerSec = 1e-6 ; const Real angularTolerance_asec = 0.0 ; - const Real angularVelocityTolerance_radPerSec = 0.0 ; + const Real angularVelocityTolerance_radPerSec = 1e-10 ; // Reference data setup @@ -373,7 +373,104 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, InertialPointing) } -TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing) +// TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing_LVLH) +// { + +// using ostk::core::types::Shared ; +// using ostk::core::types::Real ; +// using ostk::core::types::String ; +// using ostk::core::ctnr::Array ; +// using ostk::core::ctnr::Table ; +// using ostk::core::fs::Path ; +// using ostk::core::fs::File ; + +// using ostk::math::obj::Vector3d ; +// using ostk::math::geom::d3::trf::rot::Quaternion ; + +// using ostk::physics::units::Length ; +// using ostk::physics::units::Angle ; +// using ostk::physics::units::Derived ; +// using ostk::physics::time::Scale ; +// using ostk::physics::time::Instant ; +// using ostk::physics::time::Duration ; +// using ostk::physics::time::Interval ; +// using ostk::physics::time::DateTime ; +// using ostk::physics::coord::Frame ; +// using ostk::physics::Environment ; +// using ostk::physics::env::obj::celest::Earth ; + +// using ostk::astro::trajectory::Orbit ; +// using ostk::astro::trajectory::orbit::models::Kepler ; +// using ostk::astro::trajectory::orbit::models::kepler::COE ; +// using ostk::astro::flight::Profile ; +// using ostk::astro::flight::profile::State ; + +// // LVLH #1 + +// { + +// const Environment environment = Environment::Default() ; + +// const Length semiMajorAxis = Length::Kilometers(7000.0) ; +// const Real eccentricity = 0.0 ; +// const Angle inclination = Angle::Degrees(0.0) ; +// const Angle raan = Angle::Degrees(0.0) ; +// const Angle aop = Angle::Degrees(0.0) ; +// const Angle trueAnomaly = Angle::Degrees(0.0) ; + +// const COE coe = { semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly } ; + +// const Instant epoch = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; +// const Derived gravitationalParameter = Earth::GravitationalParameter ; +// const Length equatorialRadius = Earth::EquatorialRadius ; +// const Real J2 = Earth::J2 ; + +// const Kepler keplerianModel = { coe, epoch, gravitationalParameter, equatorialRadius, J2, Kepler::PerturbationType::None } ; + +// const Orbit orbit = { keplerianModel, environment.accessCelestialObjectWithName("Earth") } ; + +// const Profile profile = Profile::NadirPointing(orbit, Orbit::FrameType::LVLH) ; + +// const Real positionTolerance_m = 1e-3 ; +// const Real velocityTolerance_meterPerSec = 1e-6 ; +// const Real angularTolerance_asec = 0.0 ; +// const Real angularVelocityTolerance_radPerSec = 1e-10 ; + +// // Reference data setup + +// const File referenceDataFile = File::Path(Path::Parse("/app/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/NadirPointing/VVLH/Satellite_1 t_UTC x_GCRF v_GCRF q_B_GCRF w_B_GCRF_in_GCRF.csv")) ; + +// const Table referenceData = Table::Load(referenceDataFile, Table::Format::CSV, true) ; + +// for (const auto& referenceRow : referenceData) +// { + +// const Instant instant_ref = Instant::DateTime(DateTime::Parse(referenceRow["Time (UTCG)"].accessString()), Scale::UTC) ; + +// const Vector3d x_BODY_GCRF_ref = { referenceRow["x (m)"].accessReal(), referenceRow["y (m)"].accessReal(), referenceRow["z (m)"].accessReal() } ; +// const Vector3d v_BODY_GCRF_in_GCRF_ref = { referenceRow["vx (m/sec)"].accessReal(), referenceRow["vy (m/sec)"].accessReal(), referenceRow["vz (m/sec)"].accessReal() } ; +// const Quaternion q_BODY_GCRF_ref = Quaternion::XYZS(referenceRow["q1"].accessReal(), referenceRow["q2"].accessReal(), referenceRow["q3"].accessReal(), referenceRow["q4"].accessReal()).normalize() ; +// const Vector3d w_BODY_GCRF_in_BODY_ref = { referenceRow["wx (rad/sec)"].accessReal(), referenceRow["wy (rad/sec)"].accessReal(), referenceRow["wz (rad/sec)"].accessReal() } ; + +// const State state = profile.getStateAt(instant_ref) ; + +// const Vector3d x_BODY_GCRF = state.getPosition() ; +// const Vector3d v_BODY_GCRF_in_GCRF = state.getVelocity() ; +// const Quaternion q_BODY_GCRF = state.getAttitude() ; +// const Vector3d w_BODY_GCRF_in_BODY = state.getAngularVelocity() ; + +// ASSERT_TRUE(x_BODY_GCRF.isNear(x_BODY_GCRF_ref, positionTolerance_m)) << String::Format("@ {}: {} - {} = {} [m]", instant_ref.toString(), x_BODY_GCRF.toString(), x_BODY_GCRF_ref.toString(), (x_BODY_GCRF - x_BODY_GCRF_ref).norm()) ; +// ASSERT_TRUE(v_BODY_GCRF_in_GCRF.isNear(v_BODY_GCRF_in_GCRF_ref, velocityTolerance_meterPerSec)) << String::Format("@ {}: {} - {} = {} [m/s]", instant_ref.toString(), v_BODY_GCRF_in_GCRF.toString(), v_BODY_GCRF_in_GCRF_ref.toString(), (v_BODY_GCRF_in_GCRF - v_BODY_GCRF_in_GCRF_ref).norm()) ; +// ASSERT_TRUE(q_BODY_GCRF.isNear(q_BODY_GCRF_ref, Angle::Arcseconds(angularTolerance_asec))) << String::Format("@ {}: {} / {} = {} [asec]", instant_ref.toString(), q_BODY_GCRF_ref.toString(), q_BODY_GCRF.toString(), q_BODY_GCRF.angularDifferenceWith(q_BODY_GCRF_ref).inArcseconds().toString()) ; +// ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ; + +// } + +// } + +// } + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing_VVLH) { using ostk::core::types::Shared ; @@ -434,7 +531,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing) const Real positionTolerance_m = 1e-3 ; const Real velocityTolerance_meterPerSec = 1e-6 ; const Real angularTolerance_asec = 0.0 ; - const Real angularVelocityTolerance_radPerSec = 0.0 ; + const Real angularVelocityTolerance_radPerSec = 1e-10 ; // Reference data setup @@ -462,7 +559,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing) ASSERT_TRUE(x_BODY_GCRF.isNear(x_BODY_GCRF_ref, positionTolerance_m)) << String::Format("@ {}: {} - {} = {} [m]", instant_ref.toString(), x_BODY_GCRF.toString(), x_BODY_GCRF_ref.toString(), (x_BODY_GCRF - x_BODY_GCRF_ref).norm()) ; ASSERT_TRUE(v_BODY_GCRF_in_GCRF.isNear(v_BODY_GCRF_in_GCRF_ref, velocityTolerance_meterPerSec)) << String::Format("@ {}: {} - {} = {} [m/s]", instant_ref.toString(), v_BODY_GCRF_in_GCRF.toString(), v_BODY_GCRF_in_GCRF_ref.toString(), (v_BODY_GCRF_in_GCRF - v_BODY_GCRF_in_GCRF_ref).norm()) ; ASSERT_TRUE(q_BODY_GCRF.isNear(q_BODY_GCRF_ref, Angle::Arcseconds(angularTolerance_asec))) << String::Format("@ {}: {} / {} = {} [asec]", instant_ref.toString(), q_BODY_GCRF_ref.toString(), q_BODY_GCRF.toString(), q_BODY_GCRF.angularDifferenceWith(q_BODY_GCRF_ref).inArcseconds().toString()) ; - // ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ; + ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ; } @@ -497,7 +594,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing) const Real positionTolerance_m = 1e-3 ; const Real velocityTolerance_meterPerSec = 1e-6 ; const Real angularTolerance_asec = 0.0 ; - const Real angularVelocityTolerance_radPerSec = 0.0 ; + const Real angularVelocityTolerance_radPerSec = 1e-10 ; // Reference data setup @@ -525,7 +622,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing) ASSERT_TRUE(x_BODY_GCRF.isNear(x_BODY_GCRF_ref, positionTolerance_m)) << String::Format("@ {}: {} - {} = {} [m]", instant_ref.toString(), x_BODY_GCRF.toString(), x_BODY_GCRF_ref.toString(), (x_BODY_GCRF - x_BODY_GCRF_ref).norm()) ; ASSERT_TRUE(v_BODY_GCRF_in_GCRF.isNear(v_BODY_GCRF_in_GCRF_ref, velocityTolerance_meterPerSec)) << String::Format("@ {}: {} - {} = {} [m/s]", instant_ref.toString(), v_BODY_GCRF_in_GCRF.toString(), v_BODY_GCRF_in_GCRF_ref.toString(), (v_BODY_GCRF_in_GCRF - v_BODY_GCRF_in_GCRF_ref).norm()) ; ASSERT_TRUE(q_BODY_GCRF.isNear(q_BODY_GCRF_ref, Angle::Arcseconds(angularTolerance_asec))) << String::Format("@ {}: {} / {} = {} [asec]", instant_ref.toString(), q_BODY_GCRF_ref.toString(), q_BODY_GCRF.toString(), q_BODY_GCRF.angularDifferenceWith(q_BODY_GCRF_ref).inArcseconds().toString()) ; - // ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ; + ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ; } @@ -560,7 +657,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing) const Real positionTolerance_m = 1e-3 ; const Real velocityTolerance_meterPerSec = 1e-6 ; const Real angularTolerance_asec = 0.0 ; - const Real angularVelocityTolerance_radPerSec = 0.0 ; + const Real angularVelocityTolerance_radPerSec = 1e-7 ; // Reference data setup @@ -588,20 +685,12 @@ TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing) ASSERT_TRUE(x_BODY_GCRF.isNear(x_BODY_GCRF_ref, positionTolerance_m)) << String::Format("@ {}: {} - {} = {} [m]", instant_ref.toString(), x_BODY_GCRF.toString(), x_BODY_GCRF_ref.toString(), (x_BODY_GCRF - x_BODY_GCRF_ref).norm()) ; ASSERT_TRUE(v_BODY_GCRF_in_GCRF.isNear(v_BODY_GCRF_in_GCRF_ref, velocityTolerance_meterPerSec)) << String::Format("@ {}: {} - {} = {} [m/s]", instant_ref.toString(), v_BODY_GCRF_in_GCRF.toString(), v_BODY_GCRF_in_GCRF_ref.toString(), (v_BODY_GCRF_in_GCRF - v_BODY_GCRF_in_GCRF_ref).norm()) ; ASSERT_TRUE(q_BODY_GCRF.isNear(q_BODY_GCRF_ref, Angle::Arcseconds(angularTolerance_asec))) << String::Format("@ {}: {} / {} = {} [asec]", instant_ref.toString(), q_BODY_GCRF_ref.toString(), q_BODY_GCRF.toString(), q_BODY_GCRF.angularDifferenceWith(q_BODY_GCRF_ref).inArcseconds().toString()) ; - // ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ; + ASSERT_TRUE(w_BODY_GCRF_in_BODY.isNear(w_BODY_GCRF_in_BODY_ref, angularVelocityTolerance_radPerSec)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant_ref.toString(), w_BODY_GCRF_in_BODY_ref.toString(), w_BODY_GCRF_in_BODY.toString(), (w_BODY_GCRF_in_BODY - w_BODY_GCRF_in_BODY_ref).norm()) ; } } - // VVLHGD - - // { - - // [TBI] - - // } - } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.test.cpp index 07613dafa..2ad70b01d 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/State.test.cpp @@ -7,12 +7,461 @@ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +#include #include #include //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, Constructor) +{ + using ostk::core::types::Shared ; + + using ostk::math::obj::Vector3d ; + using ostk::math::geom::d3::trf::rot::Quaternion ; + + using ostk::physics::time::Scale ; + using ostk::physics::time::Instant ; + using ostk::physics::time::DateTime ; + using ostk::physics::coord::Frame ; + + using ostk::astro::flight::profile::State ; + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + EXPECT_NO_THROW(State state(instant, position, velocity, attitude, angularVelocity, referenceFrame) ;) ; + + } + +} + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, EqualToOperator) +{ + + using ostk::core::types::Shared ; + + using ostk::math::obj::Vector3d ; + using ostk::math::geom::d3::trf::rot::Quaternion ; + + using ostk::physics::time::Scale ; + using ostk::physics::time::Instant ; + using ostk::physics::time::DateTime ; + using ostk::physics::coord::Frame ; + + using ostk::astro::flight::profile::State ; + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_TRUE(state == state) ; + + } + + { + + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const Instant instant_1 = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + + const State state_1 = { instant_1, position, velocity, attitude, angularVelocity, referenceFrame } ; + + const Instant instant_2 = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 1), Scale::UTC) ; + + const State state_2 = { instant_2, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_FALSE(state_1 == state_2) ; + + } + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_FALSE(State::Undefined() == state) ; + EXPECT_FALSE(state == State::Undefined()) ; + EXPECT_FALSE(State::Undefined() == State::Undefined()) ; + + } + +} + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, NotEqualToOperator) +{ + + using ostk::core::types::Shared ; + + using ostk::math::obj::Vector3d ; + using ostk::math::geom::d3::trf::rot::Quaternion ; + + using ostk::physics::time::Scale ; + using ostk::physics::time::Instant ; + using ostk::physics::time::DateTime ; + using ostk::physics::coord::Frame ; + + using ostk::astro::flight::profile::State ; + + { + + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const Instant instant_1 = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + + const State state_1 = { instant_1, position, velocity, attitude, angularVelocity, referenceFrame } ; + + const Instant instant_2 = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 1), Scale::UTC) ; + + const State state_2 = { instant_2, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_TRUE(state_1 != state_2) ; + + } + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_FALSE(state != state) ; + + } + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_TRUE(State::Undefined() != state) ; + EXPECT_TRUE(state != State::Undefined()) ; + EXPECT_TRUE(State::Undefined() != State::Undefined()) ; + + } + +} + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, StreamOperator) +{ + + using ostk::core::types::Shared ; + + using ostk::math::obj::Vector3d ; + using ostk::math::geom::d3::trf::rot::Quaternion ; + + using ostk::physics::time::Scale ; + using ostk::physics::time::Instant ; + using ostk::physics::time::DateTime ; + using ostk::physics::coord::Frame ; + + using ostk::astro::flight::profile::State ; + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + testing::internal::CaptureStdout() ; + + EXPECT_NO_THROW(std::cout << state << std::endl) ; + + EXPECT_FALSE(testing::internal::GetCapturedStdout().empty()) ; + + } + + { + + testing::internal::CaptureStdout() ; + + EXPECT_NO_THROW(std::cout << State::Undefined() << std::endl) ; + + EXPECT_FALSE(testing::internal::GetCapturedStdout().empty()) ; + + } + +} + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, IsDefined) +{ + + using ostk::core::types::Shared ; + + using ostk::math::obj::Vector3d ; + using ostk::math::geom::d3::trf::rot::Quaternion ; + + using ostk::physics::time::Scale ; + using ostk::physics::time::Instant ; + using ostk::physics::time::DateTime ; + using ostk::physics::coord::Frame ; + + using ostk::astro::flight::profile::State ; + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_TRUE(state.isDefined()) ; + + } + + { + + const Instant instant = Instant::Undefined() ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_FALSE(state.isDefined()) ; + + } + + { + + EXPECT_FALSE(State::Undefined().isDefined()) ; + + } + +} + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, Accessors) +{ + + using ostk::core::types::Shared ; + + using ostk::math::obj::Vector3d ; + using ostk::math::geom::d3::trf::rot::Quaternion ; + + using ostk::physics::time::Scale ; + using ostk::physics::time::Instant ; + using ostk::physics::time::DateTime ; + using ostk::physics::coord::Frame ; + + using ostk::astro::flight::profile::State ; + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_EQ(instant, state.accessInstant()) ; + EXPECT_EQ(position, state.accessPosition()) ; + EXPECT_EQ(velocity, state.accessVelocity()) ; + EXPECT_EQ(attitude, state.accessAttitude()) ; + EXPECT_EQ(angularVelocity, state.accessAngularVelocity()) ; + + } + + { + + EXPECT_ANY_THROW(State::Undefined().accessInstant()) ; + EXPECT_ANY_THROW(State::Undefined().accessPosition()) ; + EXPECT_ANY_THROW(State::Undefined().accessVelocity()) ; + EXPECT_ANY_THROW(State::Undefined().accessAttitude()) ; + EXPECT_ANY_THROW(State::Undefined().accessAngularVelocity()) ; + + } + +} + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, Getters) +{ + + using ostk::core::types::Shared ; + + using ostk::math::obj::Vector3d ; + using ostk::math::geom::d3::trf::rot::Quaternion ; + + using ostk::physics::time::Scale ; + using ostk::physics::time::Instant ; + using ostk::physics::time::DateTime ; + using ostk::physics::coord::Frame ; + + using ostk::astro::flight::profile::State ; + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_EQ(instant, state.getInstant()) ; + EXPECT_EQ(position, state.getPosition()) ; + EXPECT_EQ(velocity, state.getVelocity()) ; + EXPECT_EQ(attitude, state.getAttitude()) ; + EXPECT_EQ(angularVelocity, state.getAngularVelocity()) ; + EXPECT_EQ(referenceFrame, state.getFrame()) ; + + } + + { + + EXPECT_ANY_THROW(State::Undefined().getInstant()) ; + EXPECT_ANY_THROW(State::Undefined().getPosition()) ; + EXPECT_ANY_THROW(State::Undefined().getVelocity()) ; + EXPECT_ANY_THROW(State::Undefined().getAttitude()) ; + EXPECT_ANY_THROW(State::Undefined().getAngularVelocity()) ; + EXPECT_ANY_THROW(State::Undefined().getFrame()) ; + + } + +} + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, InFrame) +{ + + using ostk::core::types::Shared ; + + using ostk::math::obj::Vector3d ; + using ostk::math::geom::d3::trf::rot::Quaternion ; + using ostk::math::geom::d3::trf::rot::RotationVector ; + + using ostk::physics::units::Angle ; + using ostk::physics::time::Scale ; + using ostk::physics::time::Instant ; + using ostk::physics::time::DateTime ; + using ostk::physics::coord::Frame ; + + using ostk::astro::flight::profile::State ; + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state_GCRF_1 = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + const State state_GCRF_2 = state_GCRF_1.inFrame(Frame::GCRF()) ; + + EXPECT_EQ(state_GCRF_2, state_GCRF_1) ; + + } + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::RotationVector(RotationVector({ 1.0, 0.0, 0.0 }, Angle::Degrees(90.0))) ; // x_B = x_GCRF, y_B = z_GCRF, z_B = -y_GCRF + const Vector3d angularVelocity = { 0.0, 0.0, 0.0 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state_GCRF_1 = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + const State state_GCRF_2 = state_GCRF_1.inFrame(Frame::ITRF()) ; + + EXPECT_EQ(state_GCRF_2.getInstant(), state_GCRF_1.getInstant()) ; + + EXPECT_TRUE(state_GCRF_2.getPosition().isNear({ 3.130432245445, -1.782920894026, 5.601927082917 }, 1e-12)) ; + EXPECT_TRUE(state_GCRF_2.getVelocity().isNear({ 7.449331963058, -9.290756194490, 1.213098202596 }, 1e-12)) ; + EXPECT_TRUE(state_GCRF_2.getAttitude().isNear(Quaternion::XYZS(0.452292767431, -0.543517630607, -0.542758030465, 0.453247788869).normalize(), Angle::Arcseconds(1.0))) ; + EXPECT_TRUE(state_GCRF_2.getAngularVelocity().isNear({ 0.0, -0.0000729, 0.0 }, 1e-5)) ; + + EXPECT_EQ(state_GCRF_2.getFrame(), Frame::ITRF()) ; + + } + + { + + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ; + const Vector3d position = { 1.2, 3.4, 5.6 } ; + const Vector3d velocity = { 7.8, 9.0, 1.2 } ; + const Quaternion attitude = Quaternion::XYZS(0.0, 0.0, 0.0, 1.0) ; + const Vector3d angularVelocity = { 3.4, 5.6, 7.8 } ; + const Shared referenceFrame = Frame::GCRF() ; + + const State state = { instant, position, velocity, attitude, angularVelocity, referenceFrame } ; + + EXPECT_ANY_THROW(State::Undefined().inFrame(referenceFrame)) ; + EXPECT_ANY_THROW(state.inFrame(Frame::Undefined())) ; + + } + +} + +TEST (OpenSpaceToolkit_Astrodynamics_Flight_Profile_State, Undefined) +{ + + using ostk::astro::flight::profile::State ; + + { + + EXPECT_NO_THROW(State::Undefined()) ; + + } + +} //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.test.cpp index 4fcf5b5b4..3610aa5de 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit.test.cpp @@ -720,11 +720,11 @@ TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit, GetOrbitalFrame) const Quaternion q_ITRF_NED = nedOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getOrientation() ; const Vector3d w_ITRF_NED_in_ITRF = nedOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getAngularVelocity() ; - EXPECT_TRUE(x_NED_ITRF.isNear(x_NED_ITRF_ref, 1e-1)) << String::Format("{} - {} ? {} [m]", x_NED_ITRF_ref.toString(), x_NED_ITRF.toString(), (x_NED_ITRF - x_NED_ITRF_ref).norm()) ; - EXPECT_TRUE(v_NED_ITRF_in_ITRF.isNear(v_NED_ITRF_in_ITRF_ref, 1e-4)) << String::Format("{} - {} ? {} [m/s]", v_NED_ITRF_in_ITRF_ref.toString(), v_NED_ITRF_in_ITRF.toString(), (v_NED_ITRF_in_ITRF - v_NED_ITRF_in_ITRF_ref).norm()) ; + ASSERT_TRUE(x_NED_ITRF.isNear(x_NED_ITRF_ref, 1e-1)) << String::Format("@ {}: {} - {} = {} [m]", instant.toString(), x_NED_ITRF_ref.toString(), x_NED_ITRF.toString(), (x_NED_ITRF - x_NED_ITRF_ref).norm()) ; + ASSERT_TRUE(v_NED_ITRF_in_ITRF.isNear(v_NED_ITRF_in_ITRF_ref, 1e-4)) << String::Format("@ {}: {} - {} = {} [m/s]", instant.toString(), v_NED_ITRF_in_ITRF_ref.toString(), v_NED_ITRF_in_ITRF.toString(), (v_NED_ITRF_in_ITRF - v_NED_ITRF_in_ITRF_ref).norm()) ; - EXPECT_TRUE(q_ITRF_NED.isNear(q_ITRF_NED_ref, Angle::Arcseconds(1.0))) << String::Format("{} / {} ? {} [asec]", q_ITRF_NED_ref.toString(), q_ITRF_NED.toString(), q_ITRF_NED.angularDifferenceWith(q_ITRF_NED_ref).inArcseconds().toString()) ; - // EXPECT_TRUE(w_ITRF_NED_in_ITRF.isNear(w_ITRF_NED_in_ITRF_ref, 1e-12)) << String::Format("{} - {} ? {} [rad/s]", w_ITRF_NED_in_ITRF_ref.toString(), w_ITRF_NED_in_ITRF.toString(), (w_ITRF_NED_in_ITRF - w_ITRF_NED_in_ITRF_ref).norm()) ; + ASSERT_TRUE(q_ITRF_NED.isNear(q_ITRF_NED_ref, Angle::Arcseconds(1.0))) << String::Format("@ {}: {} / {} = {} [asec]", instant.toString(), q_ITRF_NED_ref.toString(), q_ITRF_NED.toString(), q_ITRF_NED.angularDifferenceWith(q_ITRF_NED_ref).inArcseconds().toString()) ; + ASSERT_TRUE(w_ITRF_NED_in_ITRF.isNear(w_ITRF_NED_in_ITRF_ref, 1e-8)) << String::Format("@ {}: {} - {} = {} [rad/s]", instant.toString(), w_ITRF_NED_in_ITRF_ref.toString(), w_ITRF_NED_in_ITRF.toString(), (w_ITRF_NED_in_ITRF - w_ITRF_NED_in_ITRF_ref).norm()) ; } @@ -790,11 +790,11 @@ TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit, GetOrbitalFrame) // // const Quaternion q_ITRF_NED = lvlhOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getOrientation() ; // // const Vector3d w_ITRF_NED_in_ITRF = lvlhOrbitalFrameSPtr->getTransformTo(Frame::ITRF(), instant).getAngularVelocity() ; - // // EXPECT_TRUE(x_NED_ITRF.isNear(x_NED_ITRF_ref, 1e-1)) << String::Format("{} - {} ? {} [m]", x_NED_ITRF_ref.toString(), x_NED_ITRF.toString(), (x_NED_ITRF - x_NED_ITRF_ref).norm()) ; - // // EXPECT_TRUE(v_NED_ITRF_in_ITRF.isNear(v_NED_ITRF_in_ITRF_ref, 1e-4)) << String::Format("{} - {} ? {} [m/s]", v_NED_ITRF_in_ITRF_ref.toString(), v_NED_ITRF_in_ITRF.toString(), (v_NED_ITRF_in_ITRF - v_NED_ITRF_in_ITRF_ref).norm()) ; + // // EXPECT_TRUE(x_NED_ITRF.isNear(x_NED_ITRF_ref, 1e-1)) << String::Format("{} - {} = {} [m]", x_NED_ITRF_ref.toString(), x_NED_ITRF.toString(), (x_NED_ITRF - x_NED_ITRF_ref).norm()) ; + // // EXPECT_TRUE(v_NED_ITRF_in_ITRF.isNear(v_NED_ITRF_in_ITRF_ref, 1e-4)) << String::Format("{} - {} = {} [m/s]", v_NED_ITRF_in_ITRF_ref.toString(), v_NED_ITRF_in_ITRF.toString(), (v_NED_ITRF_in_ITRF - v_NED_ITRF_in_ITRF_ref).norm()) ; - // // EXPECT_TRUE(q_ITRF_NED.isNear(q_ITRF_NED_ref, Angle::Arcseconds(1.0))) << String::Format("{} / {} ? {} [asec]", q_ITRF_NED_ref.toString(), q_ITRF_NED.toString(), q_ITRF_NED.angularDifferenceWith(q_ITRF_NED_ref).inArcseconds().toString()) ; - // // // EXPECT_TRUE(w_ITRF_NED_in_ITRF.isNear(w_ITRF_NED_in_ITRF_ref, 1e-12)) << String::Format("{} - {} ? {} [rad/s]", w_ITRF_NED_in_ITRF_ref.toString(), w_ITRF_NED_in_ITRF.toString(), (w_ITRF_NED_in_ITRF - w_ITRF_NED_in_ITRF_ref).norm()) ; + // // EXPECT_TRUE(q_ITRF_NED.isNear(q_ITRF_NED_ref, Angle::Arcseconds(1.0))) << String::Format("{} / {} = {} [asec]", q_ITRF_NED_ref.toString(), q_ITRF_NED.toString(), q_ITRF_NED.angularDifferenceWith(q_ITRF_NED_ref).inArcseconds().toString()) ; + // // // EXPECT_TRUE(w_ITRF_NED_in_ITRF.isNear(w_ITRF_NED_in_ITRF_ref, 1e-12)) << String::Format("{} - {} = {} [rad/s]", w_ITRF_NED_in_ITRF_ref.toString(), w_ITRF_NED_in_ITRF.toString(), (w_ITRF_NED_in_ITRF - w_ITRF_NED_in_ITRF_ref).norm()) ; // // }