[Gcblue-commits] gcb_wx/src/common simmath.cpp,1.21,1.22

[Dewitt C. <ddc...@us...>]

Update of /cvsroot/gcblue/gcb_wx/src/common
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31072/src/common

Modified Files:
	simmath.cpp 
Log Message:
Passive sonar seeker work

Index: simmath.cpp
===================================================================
RCS file: /cvsroot/gcblue/gcb_wx/src/common/simmath.cpp,v
retrieving revision 1.21
retrieving revision 1.22
diff -C2 -d -r1.21 -r1.22
*** simmath.cpp	24 Feb 2005 22:19:15 -0000	1.21
--- simmath.cpp	7 Mar 2005 01:54:19 -0000	1.22
***************
*** 339,345 ****
     bool bAltValid = (mnFlags & TRACK_ALT_VALID) != 0;
     bool bClimbValid = (mnFlags & TRACK_CLIMB_VALID) != 0;
  
!    if (bHeadingValid) {td.mfHeading_rad = mfHeading_rad;}
!    else {td.mfHeading_rad = 0;}
  
     if (bSpeedValid && bHeadingValid) 
--- 339,370 ----
     bool bAltValid = (mnFlags & TRACK_ALT_VALID) != 0;
     bool bClimbValid = (mnFlags & TRACK_CLIMB_VALID) != 0;
+    bool bearingRateValid = (mnFlags & TRACK_BEARINGRATE_VALID) != 0;
+    bool isBearingOnly = (mnFlags & TRACK_BEARING_ONLY) != 0;
  
!    if (isBearingOnly) 
!    {
!        if (bearingRateValid)
!        {
!            const float dh_max = C_PIOVER180 * 15.0f;
!            const float dh_min = -C_PIOVER180 * 15.0f;
!            float dHeading = dt_s * bearingRate_radps;
!            if (dHeading < dh_min) dHeading = dh_min;
!            else if (dHeading > dh_max) dHeading = dh_max;
! 
!            td.mfHeading_rad = mfHeading_rad + dHeading;
!        }
!        else
!        {
!            td.mfHeading_rad = mfHeading_rad;
!        }
!    }
!    else if (bHeadingValid) 
!    {
!        td.mfHeading_rad = mfHeading_rad;
!    }
!    else
!    {
!        td.mfHeading_rad = 0;
!    }
  
     if (bSpeedValid && bHeadingValid) 
***************
*** 378,381 ****
--- 403,407 ----
     }
  
+    td.bearingRate_radps = bearingRate_radps; // for bearing only tracks, copy rate
     td.mnFlags = mnFlags;
     td.mnID = mnID;
***************
*** 530,544 ****
  }
  
! // returns radian heading to track
  float tcKinematics::HeadingToTrack(const tcTrack& track) {
     return nsNav::GCHeadingApprox_rad((float)mfLat_rad,(float)mfLon_rad,
        (float)track.mfLat_rad,(float)track.mfLon_rad);
  }
! // returns radian heading to (lat,lon) point
  float tcKinematics::HeadingToGeoRad(const GeoPoint *apGeoPoint) {
     return nsNav::GCHeadingApprox_rad((float)mfLat_rad,(float)mfLon_rad,
        (float)apGeoPoint->mfLat_rad,(float)apGeoPoint->mfLon_rad);
  }
! // returns range in km
  float tcKinematics::RangeToKm(tcKinematics& k) {
     return C_RADTOKM * nsNav::GCDistanceApprox_rad((float)mfLat_rad,(float)mfLon_rad,
--- 556,598 ----
  }
  
! /**
! * @return bearing rate to track in radians per second
! * Ignores altitude difference
! * @param range_km range to target
! * @param bearing_rad bearing to target
! * @param speet_kts target speed
! * @param heading_rad target heading
! */
! float tcKinematics::BearingRateTo(float range_km, float bearing_rad, float speed_kts, float heading_rad)
! {
!     float inv_range = 1.0f / range_km;
!     float dx = sinf(bearing_rad) * inv_range;
!     float dy = cosf(bearing_rad) * inv_range;
! 
!     float dvx = C_KTSTOKMPS * (sinf(heading_rad) * speed_kts - 
!         sinf(mfHeading_rad) * mfSpeed_kts);
!     float dvy = C_KTSTOKMPS * (cosf(heading_rad) * speed_kts - 
!         cosf(mfHeading_rad) * mfSpeed_kts);
! 
!     return ((dy * dvx) - (dx * dvy));
! }
! 
! /**
! * @returns radian heading to track
! */
  float tcKinematics::HeadingToTrack(const tcTrack& track) {
     return nsNav::GCHeadingApprox_rad((float)mfLat_rad,(float)mfLon_rad,
        (float)track.mfLat_rad,(float)track.mfLon_rad);
  }
! /**
! * @returns radian heading to (lat,lon) point
! */
  float tcKinematics::HeadingToGeoRad(const GeoPoint *apGeoPoint) {
     return nsNav::GCHeadingApprox_rad((float)mfLat_rad,(float)mfLon_rad,
        (float)apGeoPoint->mfLat_rad,(float)apGeoPoint->mfLon_rad);
  }
! /**
! * @returns range in km
! */
  float tcKinematics::RangeToKm(tcKinematics& k) {
     return C_RADTOKM * nsNav::GCDistanceApprox_rad((float)mfLat_rad,(float)mfLon_rad,