d003972a5f
normal computation. Its really small issue, maybe not even worth submitting ;-). But one never knows when inadequate precision hits him. "
228 lines
8.7 KiB
C++
228 lines
8.7 KiB
C++
/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
|
|
*
|
|
* This library is open source and may be redistributed and/or modified under
|
|
* the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
|
|
* (at your option) any later version. The full license is in LICENSE file
|
|
* included with this distribution, and on the openscenegraph.org website.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* OpenSceneGraph Public License for more details.
|
|
*/
|
|
|
|
#ifndef OSG_COORDINATESYSTEMNODE
|
|
#define OSG_COORDINATESYSTEMNODE 1
|
|
|
|
#include <osg/Group>
|
|
#include <osg/Matrixd>
|
|
|
|
namespace osg
|
|
{
|
|
|
|
const double WGS_84_RADIUS_EQUATOR = 6378137.0;
|
|
const double WGS_84_RADIUS_POLAR = 6356752.3142;
|
|
|
|
/** EllipsoidModel encapsulates the ellipsoid used to model astronomical bodies,
|
|
* such as sun, planets, moon etc. */
|
|
class EllipsoidModel : public Object
|
|
{
|
|
public:
|
|
|
|
/** WGS_84 is a common representation of the earth's spheroid */
|
|
EllipsoidModel(double radiusEquator = WGS_84_RADIUS_EQUATOR,
|
|
double radiusPolar = WGS_84_RADIUS_POLAR):
|
|
_radiusEquator(radiusEquator),
|
|
_radiusPolar(radiusPolar) { computeCoefficients(); }
|
|
|
|
EllipsoidModel(const EllipsoidModel& et,const CopyOp& copyop=CopyOp::SHALLOW_COPY):
|
|
Object(et,copyop),
|
|
_radiusEquator(et._radiusEquator),
|
|
_radiusPolar(et._radiusPolar) { computeCoefficients(); }
|
|
|
|
META_Object(osg,EllipsoidModel);
|
|
|
|
void setRadiusEquator(double radius) { _radiusEquator = radius; computeCoefficients(); }
|
|
double getRadiusEquator() const { return _radiusEquator; }
|
|
|
|
void setRadiusPolar(double radius) { _radiusPolar = radius; computeCoefficients(); }
|
|
double getRadiusPolar() const { return _radiusPolar; }
|
|
|
|
inline void convertLatLongHeightToXYZ(double latitude, double longitude, double height,
|
|
double& X, double& Y, double& Z) const;
|
|
|
|
inline void convertXYZToLatLongHeight(double X, double Y, double Z,
|
|
double& latitude, double& longitude, double& height) const;
|
|
|
|
inline void computeLocalToWorldTransformFromLatLongHeight(double latitude, double longitude, double height, osg::Matrixd& localToWorld) const;
|
|
|
|
inline void computeLocalToWorldTransformFromXYZ(double X, double Y, double Z, osg::Matrixd& localToWorld) const;
|
|
|
|
inline osg::Vec3d computeLocalUpVector(double X, double Y, double Z) const;
|
|
|
|
protected:
|
|
|
|
void computeCoefficients()
|
|
{
|
|
double flattening = (_radiusEquator-_radiusPolar)/_radiusEquator;
|
|
_eccentricitySquared = 2*flattening - flattening*flattening;
|
|
}
|
|
|
|
double _radiusEquator;
|
|
double _radiusPolar;
|
|
double _eccentricitySquared;
|
|
|
|
};
|
|
|
|
/** CoordinateFrame encapsulates the orientation of east, north and up.*/
|
|
typedef Matrixd CoordinateFrame;
|
|
|
|
/** CoordinateSystem encapsulate the coordinate system that is associated with objects in a scene.
|
|
For an overview of common earth bases coordinate systems see http://www.colorado.edu/geography/gcraft/notes/coordsys/coordsys_f.html */
|
|
class OSG_EXPORT CoordinateSystemNode : public Group
|
|
{
|
|
public:
|
|
|
|
CoordinateSystemNode();
|
|
|
|
CoordinateSystemNode(const std::string& format, const std::string& cs);
|
|
|
|
/** Copy constructor using CopyOp to manage deep vs shallow copy.*/
|
|
CoordinateSystemNode(const CoordinateSystemNode&,const osg::CopyOp& copyop=osg::CopyOp::SHALLOW_COPY);
|
|
|
|
META_Node(osg,CoordinateSystemNode);
|
|
|
|
|
|
/** Set the coordinate system node up by copy the format, coordinate system string, and ellipsoid model of another coordinate system node.*/
|
|
void set(const CoordinateSystemNode& csn);
|
|
|
|
/** Set the coordinate system format string. Typical values would be WKT, PROJ4, USGS etc.*/
|
|
void setFormat(const std::string& format) { _format = format; }
|
|
|
|
/** Get the coordinate system format string.*/
|
|
const std::string& getFormat() const { return _format; }
|
|
|
|
/** Set the CoordinateSystem reference string, should be stored in a form consistent with the Format.*/
|
|
void setCoordinateSystem(const std::string& cs) { _cs = cs; }
|
|
|
|
/** Get the CoordinateSystem reference string.*/
|
|
const std::string& getCoordinateSystem() const { return _cs; }
|
|
|
|
|
|
/** Set EllipsoidModel to describe the model used to map lat, long and height into geocentric XYZ and back. */
|
|
void setEllipsoidModel(EllipsoidModel* ellipsode) { _ellipsoidModel = ellipsode; }
|
|
|
|
/** Get the EllipsoidModel.*/
|
|
EllipsoidModel* getEllipsoidModel() { return _ellipsoidModel.get(); }
|
|
|
|
/** Get the const EllipsoidModel.*/
|
|
const EllipsoidModel* getEllipsoidModel() const { return _ellipsoidModel.get(); }
|
|
|
|
/** Compute the local coordinate frame for specified point.*/
|
|
CoordinateFrame computeLocalCoordinateFrame(const Vec3d& position) const;
|
|
|
|
/** Compute the local coordinate frame for specified point.*/
|
|
osg::Vec3d computeLocalUpVector(const Vec3d& position) const;
|
|
|
|
protected:
|
|
|
|
virtual ~CoordinateSystemNode() {}
|
|
|
|
std::string _format;
|
|
std::string _cs;
|
|
ref_ptr<EllipsoidModel> _ellipsoidModel;
|
|
|
|
};
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// implement inline methods.
|
|
//
|
|
inline void EllipsoidModel::convertLatLongHeightToXYZ(double latitude, double longitude, double height,
|
|
double& X, double& Y, double& Z) const
|
|
{
|
|
// for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif
|
|
double sin_latitude = sin(latitude);
|
|
double cos_latitude = cos(latitude);
|
|
double N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);
|
|
X = (N+height)*cos_latitude*cos(longitude);
|
|
Y = (N+height)*cos_latitude*sin(longitude);
|
|
Z = (N*(1-_eccentricitySquared)+height)*sin_latitude;
|
|
}
|
|
|
|
|
|
inline void EllipsoidModel::convertXYZToLatLongHeight(double X, double Y, double Z,
|
|
double& latitude, double& longitude, double& height) const
|
|
{
|
|
// http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif
|
|
double p = sqrt(X*X + Y*Y);
|
|
double theta = atan2(Z*_radiusEquator , (p*_radiusPolar));
|
|
double eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar)/
|
|
(_radiusPolar*_radiusPolar);
|
|
|
|
double sin_theta = sin(theta);
|
|
double cos_theta = cos(theta);
|
|
|
|
latitude = atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) /
|
|
(p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) );
|
|
longitude = atan2(Y,X);
|
|
|
|
double sin_latitude = sin(latitude);
|
|
double N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);
|
|
|
|
height = p/cos(latitude) - N;
|
|
}
|
|
|
|
inline void EllipsoidModel::computeLocalToWorldTransformFromLatLongHeight(double latitude, double longitude, double height, osg::Matrixd& localToWorld) const
|
|
{
|
|
double X, Y, Z;
|
|
convertLatLongHeightToXYZ(latitude,longitude,height,X,Y,Z);
|
|
computeLocalToWorldTransformFromXYZ(X,Y,Z,localToWorld);
|
|
}
|
|
|
|
inline void EllipsoidModel::computeLocalToWorldTransformFromXYZ(double X, double Y, double Z, osg::Matrixd& localToWorld) const
|
|
{
|
|
localToWorld.makeTranslate(X,Y,Z);
|
|
|
|
|
|
// normalize X,Y,Z
|
|
double inverse_length = 1.0/sqrt(X*X + Y*Y + Z*Z);
|
|
|
|
X *= inverse_length;
|
|
Y *= inverse_length;
|
|
Z *= inverse_length;
|
|
|
|
double length_XY = sqrt(X*X + Y*Y);
|
|
double inverse_length_XY = 1.0/length_XY;
|
|
|
|
// Vx = |(-Y,X,0)|
|
|
localToWorld(0,0) = -Y*inverse_length_XY;
|
|
localToWorld(0,1) = X*inverse_length_XY;
|
|
localToWorld(0,2) = 0.0;
|
|
|
|
// Vy = /(-Z*X/(sqrt(X*X+Y*Y), -Z*Y/(sqrt(X*X+Y*Y),sqrt(X*X+Y*Y))|
|
|
double Vy_x = -Z*X*inverse_length_XY;
|
|
double Vy_y = -Z*Y*inverse_length_XY;
|
|
double Vy_z = length_XY;
|
|
inverse_length = 1.0/sqrt(Vy_x*Vy_x + Vy_y*Vy_y + Vy_z*Vy_z);
|
|
localToWorld(1,0) = Vy_x*inverse_length;
|
|
localToWorld(1,1) = Vy_y*inverse_length;
|
|
localToWorld(1,2) = Vy_z*inverse_length;
|
|
|
|
// Vz = (X,Y,Z)
|
|
localToWorld(2,0) = X;
|
|
localToWorld(2,1) = Y;
|
|
localToWorld(2,2) = Z;
|
|
}
|
|
|
|
inline osg::Vec3d EllipsoidModel::computeLocalUpVector(double X, double Y, double Z) const
|
|
{
|
|
osg::Vec3d normal(X,Y,Z);
|
|
normal.normalize();
|
|
return normal;
|
|
}
|
|
|
|
}
|
|
#endif
|