Merge pull request #28 from open-space-collective/users/lucas/flight-…

…profile Flight profile angular velocity support
open-space-collective · Mar 23, 2020 · 44bbc65 · 44bbc65
2 parents fd61c5d + 0b1593d
commit 44bbc65
Show file tree

Hide file tree

Showing 6 changed files with 702 additions and 146 deletions.
diff --git 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
@@ -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 } ;
 
 }