// ================================================ // ExtrapChild // vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv void CoordinateConverterTM::ExtrapChild( CoordinateSet &CigiCoord, CoordinateSet &DBaseCoord, ExtrapolationSet &ExtrapData, float dTime, bool TrajEn ) { Vect3 RateV; // Rate vector (in parent entity) Vect3 RateVb; // Rate vector (in body) Vect3 RateVa; // Rate vector - from accel parameters double tRate; double mRate; double cRate; // All rates are in entity body coordinates // Get the current rate RateVb.Set( ExtrapData.XRate, ExtrapData.YRate, ExtrapData.ZRate ); // Generate current rotation matrix Mtx3 RotMtx; RotMtx.GenRot( CigiCoord.Yaw, CigiCoord.Pitch, CigiCoord.Roll ); // Convert Rates to Parent entity coordinates RateV = RotMtx * RateVb; // Trajectory affects on child entities are not // processed at this time /* if( TrajEn ) { mRate = RateV.mag(); RateVa.Set( ExtrapData.AccelX, ExtrapData.AccelY, ExtrapData.AccelZ ); // Determine drag coef. if( ExtrapData.TermVel > 0.000001 ) ExtrapData.DragF = ( double )RateVa.mag() / ExtrapData.TermVel; else if( mRate > 0.000001 ) ExtrapData.DragF = ( double )ExtrapData.RetardationRate / mRate; else ExtrapData.DragF = 0.0; RateVa = RateVa * dTime; RateV = RateV + RateVa; cRate = RateV.mag(); tRate = cRate - ( cRate * ExtrapData.DragF * dTime ); RateV = RateV.Unit() * tRate; // Store current rates ExtrapData.XRate = RateV.getx(); ExtrapData.YRate = RateV.gety(); ExtrapData.ZRate = RateV.getz(); } */ // update position // (DBase coord in enu not ned // even in parent coord system) DBaseCoord.LatX += ( RateV.gety() * dTime ); DBaseCoord.LonY += ( RateV.getx() * dTime ); DBaseCoord.AltZ -= ( RateV.getz() * dTime ); // update attitude ApplyAttitudeRates( DBaseCoord, ExtrapData, dTime ); CigiCoord.LatX = DBaseCoord.LonY; CigiCoord.LonY = DBaseCoord.LatX; CigiCoord.AltZ = -DBaseCoord.AltZ; CigiCoord.Yaw = DBaseCoord.Yaw; CigiCoord.Pitch = DBaseCoord.Pitch; CigiCoord.Roll = DBaseCoord.Roll; }
// ================================================ // Extrapolate // vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv void CoordinateConverterTM::Extrapolate( CoordinateSet &CigiCoord, CoordinateSet &DBaseCoord, ExtrapolationSet &ExtrapData, float dTime, bool TrajEn ) { int stat; double lla[3]; double xyz[3]; Vect3 RateV; // Rate vector Vect3 RateVb; // Rate vector (in body) Vect3 RateVa; // Rate vector - from accel parameters Vect3 EnuRate; // Rate vector - East-North-Up referance double tRate; double mRate; double cRate; float ypr[3]; float nedv[3]; float neda[3]; float gypr[3]; float gccv[3]; float gcca[3]; double gridDeclination = sin( CigiCoord.LatX ) * ( CigiCoord.LatX - TMcentMeridian ); // All rates are in entity body coordinates // All accels are based on the North-East-Down // coordinate system. // Get the current rate RateVb.Set( ExtrapData.XRate, ExtrapData.YRate, ExtrapData.ZRate ); // Generate current rotation matrix Mtx3 RotMtx; RotMtx.GenRot( CigiCoord.Yaw, CigiCoord.Pitch, CigiCoord.Roll ); // Convert Rates to ned RateV = RotMtx * RateVb; if( TrajEn ) { mRate = RateV.mag(); RateVa.Set( ExtrapData.AccelX, ExtrapData.AccelY, ExtrapData.AccelZ ); // Determine drag coef. if( ExtrapData.TermVel > 0.000001 ) ExtrapData.DragF = ( double )RateVa.mag() / ExtrapData.TermVel; else if( mRate > 0.000001 ) ExtrapData.DragF = ( double )ExtrapData.RetardationRate / mRate; else ExtrapData.DragF = 0.0; RateVa = RateVa * dTime; RateV = RateV + RateVa; cRate = RateV.mag(); tRate = cRate - ( cRate * ExtrapData.DragF * dTime ); RateV = RateV.Unit() * tRate; // Convert back to body coordinate system RateVb = RateV * RotMtx; // Store current rates ExtrapData.XRate = RateVb.getx(); ExtrapData.YRate = RateVb.gety(); ExtrapData.ZRate = RateVb.getz(); } // Convert Linear Rates to act in the Database Coordinate system // and apply them to Database Corrdinate system entity position if( ConvMeth == 1 ) // WCS { // Not Supported at this time } else { if( ConvMeth == 2 ) // TM { // Convert from NED to ENU EnuRate.Set( RateV.gety(), RateV.getx(), -RateV.getz() ); // this only affects the x and y rates Vect3 XYNorm( 0.0, 0.0, 1.0 ); EnuRate.Rotate( XYNorm, gridDeclination ); } else // LCC (ConvMeth == 3) { // Not Supported at this time } ApplyAttitudeRates( DBaseCoord, ExtrapData, dTime ); } // Note: This is in east-north-up DBaseCoord.LatX += EnuRate.getx() * dTime; DBaseCoord.LonY += EnuRate.gety() * dTime; DBaseCoord.AltZ += EnuRate.getz() * dTime; // Generate lla position and attitude CoordDBase2Cigi( DBaseCoord, CigiCoord ); }