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,
|
