@@ -192,31 +192,31 @@ Shared<const Frame> State::getFrame ( )
192
192
193
193
}
194
194
195
- // State State::inFrame ( const Shared<const Frame>& aFrameSPtr ) const
196
- // {
195
+ State State::inFrame ( const Shared<const Frame>& aFrameSPtr ) const
196
+ {
197
197
198
- // using library::physics::coord::Transform ;
198
+ using library::physics::coord::Transform ;
199
199
200
- // if ((aFrameSPtr == nullptr) || (!aFrameSPtr->isDefined()))
201
- // {
202
- // throw library::core::error::runtime::Undefined("Frame") ;
203
- // }
200
+ if ((aFrameSPtr == nullptr ) || (!aFrameSPtr->isDefined ()))
201
+ {
202
+ throw library::core::error::runtime::Undefined (" Frame" ) ;
203
+ }
204
204
205
- // if (!this->isDefined())
206
- // {
207
- // throw library::core::error::runtime::Undefined("State") ;
208
- // }
205
+ if (!this ->isDefined ())
206
+ {
207
+ throw library::core::error::runtime::Undefined (" State" ) ;
208
+ }
209
209
210
- // const Transform transform_NEW_OLD = frameSPtr_->getTransformTo(aFrameSPtr, instant_) ;
210
+ const Transform transform_NEW_OLD = frameSPtr_->getTransformTo (aFrameSPtr, instant_) ;
211
211
212
- // const Vector3d position = transform_NEW_OLD.applyToPosition(position_) ;
213
- // const Vector3d velocity = transform_NEW_OLD.applyToVelocity(position_, velocity_) ;
214
- // const Quaternion attitude = attitude * transform_NEW_OLD.getOrientation().toConjugate() ;
215
- // const Vector3d angularVelocity = transform_NEW_OLD.getAngularVelocity() [TBI] ;
212
+ const Vector3d position = transform_NEW_OLD.applyToPosition (position_) ;
213
+ const Vector3d velocity = transform_NEW_OLD.applyToVelocity (position_, velocity_) ;
214
+ const Quaternion attitude = attitude_ * transform_NEW_OLD.getOrientation ().toConjugate () ;
215
+ const Vector3d angularVelocity = { 0.0 , 0.0 , 0.0 } ; // transform_NEW_OLD.getAngularVelocity() [TBI] ;
216
216
217
- // return { instant_, position, velocity, attitude, angularVelocity, aFrameSPtr } ;
217
+ return { instant_, position, velocity, attitude, angularVelocity, aFrameSPtr } ;
218
218
219
- // }
219
+ }
220
220
221
221
State State::Undefined ( )
222
222
{
0 commit comments