// ================================================
// 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 );
}
