double cpa_calc_ownship_cog = CogTrial; double cpa_calc_target_cog = ptarget->COG; // Ownship is not reporting valid SOG, so no way to calculate CPA if( wxIsNaN(SogTrial) || ( SogTrial > 102.2 ) ) { ptarget->bCPA_Valid = false; return; } // Ownship is maybe anchored and not reporting COG if( wxIsNaN(CogTrial) || CogTrial == 360.0 ) { if( SogTrial < .01 ) cpa_calc_ownship_cog = 0.; // substitute value // for the case where SOG ~= 0, and COG is unknown. else { ptarget->bCPA_Valid = false; return; } } // Target is maybe anchored and not reporting COG if( ptarget->COG == 360.0 ) { if( ptarget->SOG > 102.2 ) { ptarget->bCPA_Valid = false; return; } else if( ptarget->SOG < .01 ) cpa_calc_target_cog = 0.; // substitute value // for the case where SOG ~= 0, and COG is unknown. else { ptarget->bCPA_Valid = false; return; } } // Express the SOGs as meters per hour double v0 = SogTrial * 1852.; double v1 = ptarget->SOG * 1852.; if( ( v0 < 1e-6 ) && ( v1 < 1e-6 ) ) { ptarget->TCPA = 0.; ptarget->CPA = 0.; ptarget->bCPA_Valid = false; } else { // Calculate the TCPA first // Working on a Reduced Lat/Lon orthogonal plotting sheet.... // Get easting/northing to target, in meters double east1 = ( ptarget->Lon - LonTrial ) * 60 * 1852; double north1 = ( ptarget->Lat - LatTrial ) * 60 * 1852; double east = east1 * ( cos( LatTrial * PI / 180. ) ); double north = north1; // Convert COGs trigonometry to standard unit circle double cosa = cos( ( 90. - cpa_calc_ownship_cog ) * PI / 180. ); double sina = sin( ( 90. - cpa_calc_ownship_cog ) * PI / 180. ); double cosb = cos( ( 90. - cpa_calc_target_cog ) * PI / 180. ); double sinb = sin( ( 90. - cpa_calc_target_cog ) * PI / 180. ); // These will be useful double fc = ( v0 * cosa ) - ( v1 * cosb ); double fs = ( v0 * sina ) - ( v1 * sinb ); double d = ( fc * fc ) + ( fs * fs ); double tcpa; // the tracks are almost parallel if( fabs( d ) < 1e-6 ) tcpa = 0.; else // Here is the equation for t, which will be in hours tcpa = ( ( fc * east ) + ( fs * north ) ) / d; // Convert to minutes ptarget->TCPA = tcpa * 60.; // Calculate CPA // Using TCPA, predict ownship and target positions double OwnshipLatCPA, OwnshipLonCPA, TargetLatCPA, TargetLonCPA; ll_gc_ll( LatTrial, LonTrial, cpa_calc_ownship_cog, SogTrial * tcpa, &OwnshipLatCPA, &OwnshipLonCPA ); ll_gc_ll( ptarget->Lat, ptarget->Lon, cpa_calc_target_cog, ptarget->SOG * tcpa, &TargetLatCPA, &TargetLonCPA ); // And compute the distance ptarget->CPA = DistGreatCircle( OwnshipLatCPA, OwnshipLonCPA, TargetLatCPA, TargetLonCPA ); ptarget->bCPA_Valid = true; if( ptarget->TCPA < 0 ) ptarget->bCPA_Valid = false; } }