SGGeoc(void);
/// Initialize from a cartesian vector assumed to be in meters
/// Note that this conversion is relatively expensive to compute
+ /// depricated
SGGeoc(const SGVec3<double>& cart);
/// Initialize from a geodetic position
/// Note that this conversion is relatively expensive to compute
+ /// depricated
SGGeoc(const SGGeod& geod);
/// Factory from angular values in radians and radius in ft
static SGGeoc fromRadM(double lon, double lat, double radius);
/// Factory from angular values in degrees and radius in m
static SGGeoc fromDegM(double lon, double lat, double radius);
+ /// Factory to convert position from a cartesian position assumed to be
+ /// in wgs84 measured in meters
+ /// Note that this conversion is relatively expensive to compute
+ static SGGeoc fromCart(const SGVec3<double>& cart);
+ /// Factory to convert position from a geodetic position
+ /// Note that this conversion is relatively expensive to compute
+ static SGGeoc fromGeod(const SGGeod& geod);
/// Return the geocentric longitude in radians
double getLongitudeRad(void) const;
#endif
}
+inline
+SGGeoc
+SGGeoc::fromCart(const SGVec3<double>& cart)
+{
+ SGGeoc geoc;
+ SGGeodesy::SGCartToGeoc(cart, geoc);
+ return geoc;
+}
+
+inline
+SGGeoc
+SGGeoc::fromGeod(const SGGeod& geod)
+{
+ SGVec3<double> cart;
+ SGGeodesy::SGGeodToCart(geod, cart);
+ SGGeoc geoc;
+ SGGeodesy::SGCartToGeoc(cart, geoc);
+ return geoc;
+}
+
inline
double
SGGeoc::getLongitudeRad(void) const
SGGeod(void);
/// Initialize from a cartesian vector assumed to be in meters
/// Note that this conversion is relatively expensive to compute
+ /// depricated
SGGeod(const SGVec3<double>& cart);
/// Initialize from a geocentric position
/// Note that this conversion is relatively expensive to compute
+ /// depricated
SGGeod(const SGGeoc& geoc);
/// Factory from angular values in radians and elevation is 0
static SGGeod fromRadM(double lon, double lat, double elevation);
/// Factory from angular values in degrees and elevation in m
static SGGeod fromDegM(double lon, double lat, double elevation);
+ /// Factory to convert position from a cartesian position assumed to be
+ /// in wgs84 measured in meters
+ /// Note that this conversion is relatively expensive to compute
+ static SGGeod fromCart(const SGVec3<double>& cart);
+ /// Factory to convert position from a geocentric position
+ /// Note that this conversion is relatively expensive to compute
+ static SGGeod fromGeoc(const SGGeoc& geoc);
/// Return the geodetic longitude in radians
double getLongitudeRad(void) const;
#endif
}
+inline
+SGGeod
+SGGeod::fromCart(const SGVec3<double>& cart)
+{
+ SGGeod geod;
+ SGGeodesy::SGCartToGeod(cart, geod);
+ return geod;
+}
+
+inline
+SGGeod
+SGGeod::fromGeoc(const SGGeoc& geoc)
+{
+ SGVec3<double> cart;
+ SGGeodesy::SGGeocToCart(geoc, cart);
+ SGGeod geod;
+ SGGeodesy::SGCartToGeod(cart, geod);
+ return geod;
+}
+
inline
double
SGGeod::getLongitudeRad(void) const
#include "SGGeodesy.hxx"
#include "SGVec3.hxx"
#include "SGVec4.hxx"
-#include "SGQuat.hxx"
-#include "SGMatrix.hxx"
#include "SGGeoc.hxx"
#include "SGGeod.hxx"
+#include "SGQuat.hxx"
+#include "SGMatrix.hxx"
#endif
geod0 = SGGeod::fromDegM(30, 20, 17);
// Test the conversion routines to cartesian coordinates
- cart0 = geod0;
- geod1 = cart0;
+ cart0 = SGVec3<double>::fromGeod(geod0);
+ geod1 = SGGeod::fromCart(cart0);
if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
return false;
// Test the conversion routines to radial coordinates
- geoc0 = cart0;
- cart1 = geoc0;
+ geoc0 = SGGeoc::fromCart(cart0);
+ cart1 = SGVec3<double>::fromGeoc(geoc0);
if (!equivalent(cart0, cart1))
return false;
return EXIT_FAILURE;
// Check geodetic/geocentric/cartesian conversions
-// if (!GeodesyTest())
-// return EXIT_FAILURE;
+ if (!GeodesyTest())
+ return EXIT_FAILURE;
// Check interaction with sg*/sgd*
if (!sgInterfaceTest())
static SGQuat fromLonLatDeg(T lon, T lat)
{ return fromLonLatRad(SGMisc<T>::deg2rad(lon), SGMisc<T>::deg2rad(lat)); }
+ /// Return a quaternion rotation the the horizontal local frame from given
+ /// longitude and latitude
+ static SGQuat fromLonLat(const SGGeod& geod)
+ { return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
+
/// Create a quaternion from the angle axis representation
static SGQuat fromAngleAxis(T angle, const SGVec3<T>& axis)
{
{ _data[0] = data[0]; _data[1] = data[1]; _data[2] = data[2]; }
/// Constructor. Initialize by a geodetic coordinate
/// Note that this conversion is relatively expensive to compute
+ /// depricated
SGVec3(const SGGeod& geod)
{ SGGeodesy::SGGeodToCart(geod, *this); }
/// Constructor. Initialize by a geocentric coordinate
/// Note that this conversion is relatively expensive to compute
+ /// depricated
SGVec3(const SGGeoc& geoc)
{ SGGeodesy::SGGeocToCart(geoc, *this); }
static SGVec3 e3(void)
{ return SGVec3(0, 0, 1); }
+ /// Constructor. Initialize by a geodetic coordinate
+ /// Note that this conversion is relatively expensive to compute
+ static SGVec3 fromGeod(const SGGeod& geod);
+ /// Constructor. Initialize by a geocentric coordinate
+ /// Note that this conversion is relatively expensive to compute
+ static SGVec3 fromGeoc(const SGGeoc& geoc);
+
private:
/// The actual data
T _data[3];
};
+template<>
+inline
+SGVec3<double>
+SGVec3<double>::fromGeod(const SGGeod& geod)
+{
+ SGVec3<double> cart;
+ SGGeodesy::SGGeodToCart(geod, cart);
+ return cart;
+}
+
+template<>
+inline
+SGVec3<float>
+SGVec3<float>::fromGeod(const SGGeod& geod)
+{
+ SGVec3<double> cart;
+ SGGeodesy::SGGeodToCart(geod, cart);
+ return SGVec3<float>(cart(0), cart(1), cart(2));
+}
+
+template<>
+inline
+SGVec3<double>
+SGVec3<double>::fromGeoc(const SGGeoc& geoc)
+{
+ SGVec3<double> cart;
+ SGGeodesy::SGGeocToCart(geoc, cart);
+ return cart;
+}
+
+template<>
+inline
+SGVec3<float>
+SGVec3<float>::fromGeoc(const SGGeoc& geoc)
+{
+ SGVec3<double> cart;
+ SGGeodesy::SGGeocToCart(geoc, cart);
+ return SGVec3<float>(cart(0), cart(1), cart(2));
+}
+
/// Unary +, do nothing ...
template<typename T>
inline
}
-// given, alt, lat1, lon1, az1 and distance (s), calculate lat2, lon2
+// given, lat1, lon1, az1 and distance (s), calculate lat2, lon2
// and az2. Lat, lon, and azimuth are in degrees. distance in meters
-int geo_direct_wgs_84 ( double alt, double lat1,
- double lon1, double az1,
+int geo_direct_wgs_84 ( double lat1, double lon1, double az1,
double s, double *lat2, double *lon2,
double *az2 )
{
double dM = a*M0(e2) - s;
double paz = ( phi1 < 0.0 ? 180.0 : 0.0 );
double zero = 0.0f;
- return geo_direct_wgs_84( alt, zero, lon1, paz, dM, lat2, lon2, az2 );
+ return geo_direct_wgs_84( zero, lon1, paz, dM, lat2, lon2, az2 );
}
}
-// given alt, lat1, lon1, lat2, lon2, calculate starting and ending
+// given lat1, lon1, lat2, lon2, calculate starting and ending
// az1, az2 and distance (s). Lat, lon, and azimuth are in degrees.
// distance in meters
-int geo_inverse_wgs_84( double alt, double lat1,
- double lon1, double lat2,
+int geo_inverse_wgs_84( double lat1, double lon1, double lat2,
double lon2, double *az1, double *az2,
double *s )
{
return 0;
} else if( fabs(cosphi1) < testv ) {
// initial point is polar
- int k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s );
+ int k = geo_inverse_wgs_84( lat2,lon2,lat1,lon1, az1,az2,s );
k = k; // avoid compiler error since return result is unused
b = *az1; *az1 = *az2; *az2 = b;
return 0;
} else if( fabs(cosphi2) < testv ) {
// terminal point is polar
double _lon1 = lon1 + 180.0f;
- int k = geo_inverse_wgs_84( alt, lat1, lon1, lat1, _lon1,
+ int k = geo_inverse_wgs_84( lat1, lon1, lat1, _lon1,
az1, az2, s );
k = k; // avoid compiler error since return result is unused
*s /= 2.0;
{
// Geodesic passes through the pole (antipodal)
double s1,s2;
- geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 );
- geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 );
+ geo_inverse_wgs_84( lat1,lon1, lat1,lon2, az1,az2, &s1 );
+ geo_inverse_wgs_84( lat2,lon2, lat1,lon2, az1,az2, &s2 );
*az2 = *az1;
*s = s1 + s2;
return 0;
#include <simgear/math/point3d.hxx>
#include "SGMath.hxx"
-// Returns the insersection of the line joining the center of the
-// earth and the specified cylindrical point with the surface of the
-// WGS84 ellipsoid. Works by finding a normalization constant (in
-// squashed space) that places the squashed point on the surface of
-// the sphere.
-inline double seaLevelRadius(double r, double z)
-{
- double sr = r * SGGeodesy::SQUASH;
- double zz = z*z;
- return SGGeodesy::POLRAD*sqrt((r*r + zz)/(sr*sr + zz));
-}
-
/**
* Convert from geocentric coordinates to geodetic coordinates
* @param lat_geoc (in) Geocentric latitude, radians, + = North
double *sl_radius, double *lat_geoc)
{
SGVec3<double> cart;
- SGGeodesy::SGGeodToCart(SGGeod::fromRadM(0, lat_geod, alt), cart);
+ SGGeod geod = SGGeod::fromRadM(0, lat_geod, alt);
+ SGGeodesy::SGGeodToCart(geod, cart);
SGGeoc geoc;
SGGeodesy::SGCartToGeoc(cart, geoc);
*lat_geoc = geoc.getLatitudeRad();
- *sl_radius = seaLevelRadius(cart(0), cart(2));
+ *sl_radius = SGGeodesy::SGGeodToSeaLevelRadius(geod);
}
/**
* Given a starting position and an offset radial and distance,
* calculate an ending positon on a wgs84 ellipsoid.
- * @param alt (in) meters
+ * @param alt (in) meters (unused)
* @param lat1 (in) degrees
* @param lon1 (in) degrees
* @param az1 (in) degrees
* @param lon2 (out) degrees
* @param az2 (out) return course in degrees
*/
-int geo_direct_wgs_84 ( double alt, double lat1,
- double lon1, double az1,
+int geo_direct_wgs_84 ( double lat1, double lon1, double az1,
double s, double *lat2, double *lon2,
double *az2 );
+inline int geo_direct_wgs_84 ( double alt, double lat1,
+ double lon1, double az1,
+ double s, double *lat2, double *lon2,
+ double *az2 )
+{ return geo_direct_wgs_84(lat1, lon1, az1, s, lat2, lon2, az2); }
+/**
+ * Given a starting position and an offset radial and distance,
+ * calculate an ending positon on a wgs84 ellipsoid.
+ * @param p1 (in) geodetic position
+ * @param az1 (in) degrees
+ * @param s (in) distance in meters
+ * @param p2 (out) geodetic position
+ * @param az2 (out) return course in degrees
+ */
+inline int geo_direct_wgs_84(const SGGeod& p1, double az1,
+ double s, SGGeod& p2, double *az2 )
+{
+ double lat2, lon2;
+ int ret = geo_direct_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(),
+ az1, s, &lat2, &lon2, az2);
+ p2.setLatitudeDeg(lat2);
+ p2.setLongitudeDeg(lon2);
+ return ret;
+}
/**
* Given an altitude and two sets of (lat, lon) calculate great circle
* distance between them as well as the starting and ending azimuths.
- * @param alt (in) meters
+ * @param alt (in) meters (unused)
* @param lat1 (in) degrees
* @param lon1 (in) degrees
* @param lat2 (in) degrees
* @param az2 (out) end heading degrees
* @param s (out) distance meters
*/
-int geo_inverse_wgs_84( double alt, double lat1,
- double lon1, double lat2,
+int geo_inverse_wgs_84( double lat1, double lon1, double lat2,
double lon2, double *az1, double *az2,
double *s );
+inline int geo_inverse_wgs_84( double alt, double lat1,
+ double lon1, double lat2,
+ double lon2, double *az1, double *az2,
+ double *s )
+{ return geo_inverse_wgs_84(lat1, lon1, lat2, lon2, az1, az2, s); }
+
+
+/**
+ * Given an altitude and two sets of (lat, lon) calculate great circle
+ * distance between them as well as the starting and ending azimuths.
+ * @param p1 (in) first position
+ * @param p2 (in) fsecond position
+ * @param az1 (out) start heading degrees
+ * @param az2 (out) end heading degrees
+ * @param s (out) distance meters
+ */
+inline int geo_inverse_wgs_84(const SGGeod& p1, const SGGeod& p2,
+ double *az1, double *az2, double *s )
+{
+ return geo_inverse_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(),
+ p2.getLatitudeDeg(), p2.getLongitudeDeg(),
+ az1, az2, s);
+}
#endif // _SG_GEODESY_HXX
_elev_ft = elev_ft;
}
+void
+SGModelPlacement::setPosition(const SGGeod& position)
+{
+ _lon_deg = position.getLongitudeDeg();
+ _lat_deg = position.getLatitudeDeg();
+ _elev_ft = position.getElevationFt();
+}
+
void
SGModelPlacement::setRollDeg (double roll_deg)
{
_heading_deg = heading_deg;
}
+void
+SGModelPlacement::setOrientation (const SGQuatd& orientation)
+{
+ orientation.getEulerDeg(_heading_deg, _pitch_deg, _roll_deg);
+}
+
// end of model.cxx
virtual void setLatitudeDeg (double lat_deg);
virtual void setElevationFt (double elev_ft);
virtual void setPosition (double lon_deg, double lat_deg, double elev_ft);
+ void setPosition(const SGGeod& position);
virtual double getRollDeg () const { return _roll_deg; }
virtual double getPitchDeg () const { return _pitch_deg; }
virtual void setHeadingDeg (double heading_deg);
virtual void setOrientation (double roll_deg, double pitch_deg,
double heading_deg);
+ void setOrientation(const SGQuatd& orientation);
ssgPlacementTransform * getTransform(void)
{ return _position; }
* written for FlightGear, in order to store Timezone control points.
*
************************************************************************/
+#include <simgear/math/SGMath.hxx>
#include "geocoord.h"
-#include <plib/sg.h>
SGGeoCoord::SGGeoCoord(const SGGeoCoord& other)
{
lon = other.lon;
}
-// double SGGeoCoord::getAngle(const SGGeoCoord& other) const
-// {
-// Vector first( getX(), getY(), getZ());
-// Vector secnd(other.getX(), other.getY(), other.getZ());
-// double
-// dot = VecDot(first, secnd),
-// len1 = first.VecLen(),
-// len2 = secnd.VecLen(),
-// len = len1 * len2,
-// angle = 0;
-// //printf ("Dot: %f, len1: %f len2: %f\n", dot, len1, len2);
-// /*Vector pPos = prevPos - Reference->prevPos;
-// Vector pVel = prevVel - Reference->prevVel;*/
-
-
-// if ( ( (dot / len) < 1) && (dot / len > -1) && len )
-// angle = acos(dot / len);
-// return angle;
-// }
-
-// SGGeoCoord* SGGeoCoordContainer::getNearest(const SGGeoCoord& ref) const
-// {
-// float angle, maxAngle = 180;
-
-// SGGeoCoordVectorConstIterator i, nearest;
-// for (i = data.begin(); i != data.end(); i++)
-// {
-// angle = SGD_RADIANS_TO_DEGREES * (*i)->getAngle(ref);
-// if (angle < maxAngle)
-// {
-// maxAngle = angle;
-// nearest = i;
-// }
-// }
-// return *nearest;
-// }
-
-
SGGeoCoord* SGGeoCoordContainer::getNearest(const SGGeoCoord& ref) const
{
- sgVec3 first, secnd;
- float dist, maxDist=SG_MAX;
- sgSetVec3( first, ref.getX(), ref.getY(), ref.getZ());
+ if (data.empty())
+ return 0;
+
+ float maxCosAng = -2;
+ SGVec3f refVec(ref.getX(), ref.getY(), ref.getZ());
SGGeoCoordVectorConstIterator i, nearest;
- for (i = data.begin(); i != data.end(); i++)
+ for (i = data.begin(); i != data.end(); ++i)
{
- sgSetVec3(secnd, (*i)->getX(), (*i)->getY(), (*i)->getZ());
- dist = sgDistanceSquaredVec3(first, secnd);
- if (dist < maxDist)
+ float cosAng = dot(refVec, SGVec3f((*i)->getX(), (*i)->getY(), (*i)->getZ()));
+ if (maxCosAng < cosAng)
{
- maxDist = dist;
+ maxCosAng = cosAng;
nearest = i;
}
}
#include <math.h>
-#include <string>
-#include STL_IOSTREAM
-//#include <streambuf> // looks like streambuf does not exist on linux.
-// But it looks like it isn't used anyways -:)
#include <vector>
SG_USING_NAMESPACE(std);
float getZ() const { return sin(SGD_DEGREES_TO_RADIANS*lat); };
- //double getAngle(const SGGeoCoord& other) const;
- virtual void print() {} ;
virtual const char * getDescription() {return 0;};
};
#define _TIMEZONE_H_
#include <stdio.h>
+#include <string>
#include <simgear/timing/geocoord.h>
*/
virtual ~SGTimeZone() { };
- /**
- * Print the descriptor string
- */
- virtual void print() { printf("%s", descriptor.c_str()); }
-
/**
* Return the descriptor string
* @return descriptor string (char array)
projects / simgear.git / commitdiff