Added point3d.hxx to replace cheezy fgPoint3d struct.
This commit is contained in:
curt
parent
2f9619f02a
commit
c5adb85741
4 changed files with 354 additions and 25 deletions
318
Math/point3d.hxx
Normal file
318
Math/point3d.hxx
Normal file
|
|
@ -0,0 +1,318 @@
|
|||
// point3d.hxx -- a 3d point class.
|
||||
//
|
||||
// Adapted from algebra3 by Jean-Francois Doue, started October 1998.
|
||||
//
|
||||
// Copyright (C) 1998 Curtis L. Olson - curt@me.umn.edu
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or
|
||||
// modify it under the terms of the GNU General Public License as
|
||||
// published by the Free Software Foundation; either version 2 of the
|
||||
// License, or (at your option) any later version.
|
||||
//
|
||||
// This program 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 GNU
|
||||
// General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU General Public License
|
||||
// along with this program; if not, write to the Free Software
|
||||
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
||||
//
|
||||
// $Id$
|
||||
// (Log is kept at end of this file)
|
||||
|
||||
|
||||
#ifndef _POINT3D_HXX
|
||||
#define _POINT3D_HXX
|
||||
|
||||
#ifndef __cplusplus
|
||||
# error This library requires C++
|
||||
#endif
|
||||
|
||||
|
||||
#include <Misc/fgstream.hxx>
|
||||
|
||||
#include "Include/fg_stl_config.h"
|
||||
|
||||
#ifdef NEEDNAMESPACESTD
|
||||
using namespace std;
|
||||
#endif
|
||||
|
||||
/*
|
||||
#include <stdlib.h>
|
||||
#include <yvals.h>
|
||||
#include <math.h>
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
#include <Include/fg_constants.h>
|
||||
|
||||
|
||||
enum {PX, PY, PZ}; // axes
|
||||
|
||||
|
||||
///////////////////////////
|
||||
//
|
||||
// 3D Point
|
||||
//
|
||||
///////////////////////////
|
||||
|
||||
class Point3D {
|
||||
|
||||
protected:
|
||||
|
||||
double n[3];
|
||||
|
||||
public:
|
||||
|
||||
// Constructors
|
||||
|
||||
Point3D();
|
||||
Point3D(const double x, const double y, const double z);
|
||||
Point3D(const double d);
|
||||
Point3D(const Point3D &p);
|
||||
|
||||
// Assignment operators
|
||||
|
||||
Point3D& operator = ( const Point3D& p ); // assignment of a Point3D
|
||||
Point3D& operator += ( const Point3D& p ); // incrementation by a Point3D
|
||||
Point3D& operator -= ( const Point3D& p ); // decrementation by a Point3D
|
||||
Point3D& operator *= ( const double d ); // multiplication by a constant
|
||||
Point3D& operator /= ( const double d ); // division by a constant
|
||||
void setvals(const double x, const double y, const double z);
|
||||
void setx(const double x);
|
||||
void sety(const double y);
|
||||
void setz(const double z);
|
||||
|
||||
// Queries
|
||||
|
||||
double& operator [] ( int i); // indexing
|
||||
double operator[] (int i) const; // read-only indexing
|
||||
|
||||
double x() const; // cartesian x
|
||||
double y() const; // cartesian y
|
||||
double z() const; // cartesian z
|
||||
|
||||
double lon() const; // polar longitude
|
||||
double lat() const; // polar latitude
|
||||
double radius() const; // polar radius
|
||||
double elev() const; // geodetic elevation (if specifying a surface point)
|
||||
|
||||
// friends
|
||||
|
||||
friend Point3D operator - (const Point3D& p); // -p1
|
||||
friend Point3D operator + (const Point3D& a, const Point3D& b); // p1 + p2
|
||||
friend Point3D operator - (const Point3D& a, const Point3D& b); // p1 - p2
|
||||
friend Point3D operator * (const Point3D& a, const double d); // p1 * 3.0
|
||||
friend Point3D operator * (const double d, const Point3D& a); // 3.0 * p1
|
||||
friend Point3D operator / (const Point3D& a, const double d); // p1 / 3.0
|
||||
friend bool operator == (const Point3D& a, const Point3D& b); // p1 == p2?
|
||||
friend bool operator != (const Point3D& a, const Point3D& b); // p1 != p2?
|
||||
|
||||
// Special functions
|
||||
double distance3D(const Point3D& a, const Point3D& b); // distance between
|
||||
};
|
||||
|
||||
|
||||
// input from stream
|
||||
inline istream&
|
||||
operator >> ( istream& in, Point3D& p)
|
||||
{
|
||||
double x, y, z;
|
||||
char c;
|
||||
|
||||
in >> x;
|
||||
|
||||
// read past optional comma
|
||||
while ( in.get(c) ) {
|
||||
if ( (c != ' ') && (c != ',') ) {
|
||||
// push back on the stream
|
||||
in.putback(c);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
in >> y;
|
||||
|
||||
// read past optional comma
|
||||
while ( in.get(c) ) {
|
||||
if ( (c != ' ') && (c != ',') ) {
|
||||
// push back on the stream
|
||||
in.putback(c);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
in >> z;
|
||||
|
||||
p.setvals(x, y, z);
|
||||
|
||||
return in;
|
||||
}
|
||||
|
||||
///////////////////////////
|
||||
//
|
||||
// Point3D Member functions
|
||||
//
|
||||
///////////////////////////
|
||||
|
||||
// CONSTRUCTORS
|
||||
|
||||
inline Point3D::Point3D() {}
|
||||
|
||||
inline Point3D::Point3D(const double x, const double y, const double z)
|
||||
{
|
||||
n[PX] = x; n[PY] = y; n[PZ] = z;
|
||||
}
|
||||
|
||||
inline Point3D::Point3D(const double d)
|
||||
{
|
||||
n[PX] = n[PY] = n[PZ] = d;
|
||||
}
|
||||
|
||||
inline Point3D::Point3D(const Point3D& p)
|
||||
{
|
||||
n[PX] = p.n[PX]; n[PY] = p.n[PY]; n[PZ] = p.n[PZ];
|
||||
}
|
||||
|
||||
// ASSIGNMENT OPERATORS
|
||||
|
||||
inline Point3D& Point3D::operator = (const Point3D& p)
|
||||
{
|
||||
n[PX] = p.n[PX]; n[PY] = p.n[PY]; n[PZ] = p.n[PZ]; return *this;
|
||||
}
|
||||
|
||||
inline Point3D& Point3D::operator += ( const Point3D& p )
|
||||
{
|
||||
n[PX] += p.n[PX]; n[PY] += p.n[PY]; n[PZ] += p.n[PZ]; return *this;
|
||||
}
|
||||
|
||||
inline Point3D& Point3D::operator -= ( const Point3D& p )
|
||||
{
|
||||
n[PX] -= p.n[PX]; n[PY] -= p.n[PY]; n[PZ] -= p.n[PZ]; return *this;
|
||||
}
|
||||
|
||||
inline Point3D& Point3D::operator *= ( const double d )
|
||||
{
|
||||
n[PX] *= d; n[PY] *= d; n[PZ] *= d; return *this;
|
||||
}
|
||||
|
||||
inline Point3D& Point3D::operator /= ( const double d )
|
||||
{
|
||||
double d_inv = 1./d; n[PX] *= d_inv; n[PY] *= d_inv; n[PZ] *= d_inv;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline void Point3D::setvals(const double x, const double y, const double z) {
|
||||
n[PX] = x; n[PY] = y; n[PZ] = z;
|
||||
}
|
||||
|
||||
inline void Point3D::setx(const double x) {
|
||||
n[PX] = x;
|
||||
}
|
||||
|
||||
inline void Point3D::sety(const double y) {
|
||||
n[PY] = y;
|
||||
}
|
||||
|
||||
inline void Point3D::setz(const double z) {
|
||||
n[PZ] = z;
|
||||
}
|
||||
|
||||
// QUERIES
|
||||
|
||||
inline double& Point3D::operator [] ( int i)
|
||||
{
|
||||
assert(! (i < PX || i > PZ));
|
||||
return n[i];
|
||||
}
|
||||
|
||||
inline double Point3D::operator [] ( int i) const {
|
||||
assert(! (i < PX || i > PZ));
|
||||
return n[i];
|
||||
}
|
||||
|
||||
|
||||
inline double Point3D::x() const { return n[PX]; }
|
||||
|
||||
inline double Point3D::y() const { return n[PY]; }
|
||||
|
||||
inline double Point3D::z() const { return n[PZ]; }
|
||||
|
||||
inline double Point3D::lon() const { return n[PX]; }
|
||||
|
||||
inline double Point3D::lat() const { return n[PY]; }
|
||||
|
||||
inline double Point3D::radius() const { return n[PZ]; }
|
||||
|
||||
inline double Point3D::elev() const { return n[PZ]; }
|
||||
|
||||
|
||||
// FRIENDS
|
||||
|
||||
inline Point3D operator - (const Point3D& a)
|
||||
{
|
||||
return Point3D(-a.n[PX],-a.n[PY],-a.n[PZ]);
|
||||
}
|
||||
|
||||
inline Point3D operator + (const Point3D& a, const Point3D& b)
|
||||
{
|
||||
return Point3D(a.n[PX]+ b.n[PX], a.n[PY] + b.n[PY], a.n[PZ] + b.n[PZ]);
|
||||
}
|
||||
|
||||
inline Point3D operator - (const Point3D& a, const Point3D& b)
|
||||
{
|
||||
return Point3D(a.n[PX]-b.n[PX], a.n[PY]-b.n[PY], a.n[PZ]-b.n[PZ]);
|
||||
}
|
||||
|
||||
inline Point3D operator * (const Point3D& a, const double d)
|
||||
{
|
||||
return Point3D(d*a.n[PX], d*a.n[PY], d*a.n[PZ]);
|
||||
}
|
||||
|
||||
inline Point3D operator * (const double d, const Point3D& a)
|
||||
{
|
||||
return a*d;
|
||||
}
|
||||
|
||||
inline Point3D operator / (const Point3D& a, const double d)
|
||||
{
|
||||
double d_inv = 1./d;
|
||||
return Point3D(a.n[PX]*d_inv, a.n[PY]*d_inv, a.n[PZ]*d_inv);
|
||||
}
|
||||
|
||||
inline bool operator == (const Point3D& a, const Point3D& b)
|
||||
{
|
||||
return
|
||||
(a.n[PX] - b.n[PX]) < FG_EPSILON &&
|
||||
(a.n[PY] - b.n[PY]) < FG_EPSILON &&
|
||||
(a.n[PZ] - b.n[PZ]) < FG_EPSILON;
|
||||
}
|
||||
|
||||
inline bool operator != (const Point3D& a, const Point3D& b)
|
||||
{
|
||||
return !(a == b);
|
||||
}
|
||||
|
||||
// Special functions
|
||||
|
||||
inline double distance3D(const Point3D& a, const Point3D& b)
|
||||
{
|
||||
double x, y, z;
|
||||
|
||||
x = a[PX] - b[PX];
|
||||
y = a[PY] - b[PY];
|
||||
z = a[PZ] - b[PZ];
|
||||
|
||||
return sqrt(x*x + y*y + z*z);
|
||||
}
|
||||
|
||||
#endif // _POINT3D_HXX
|
||||
|
||||
|
||||
// $Log$
|
||||
// Revision 1.1 1998/10/16 00:50:29 curt
|
||||
// Added point3d.hxx to replace cheezy fgPoint3d struct.
|
||||
//
|
||||
//
|
||||
|
|
@ -35,15 +35,15 @@
|
|||
/* Convert a polar coordinate to a cartesian coordinate. Lon and Lat
|
||||
* must be specified in radians. The FG convention is for distances
|
||||
* to be specified in meters */
|
||||
fgPoint3d fgPolarToCart3d(fgPoint3d p) {
|
||||
fgPoint3d pnew;
|
||||
Point3D fgPolarToCart3d(const Point3D& p) {
|
||||
Point3D pnew;
|
||||
double tmp;
|
||||
|
||||
tmp = cos(p.lat) * p.radius;
|
||||
tmp = cos( p.lat() ) * p.radius();
|
||||
|
||||
pnew.x = cos(p.lon) * tmp;
|
||||
pnew.y = sin(p.lon) * tmp;
|
||||
pnew.z = sin(p.lat) * p.radius;
|
||||
pnew.setvals( cos( p.lon() ) * tmp,
|
||||
sin( p.lon() ) * tmp,
|
||||
sin( p.lat() ) * p.radius() );
|
||||
|
||||
return(pnew);
|
||||
}
|
||||
|
|
@ -51,15 +51,16 @@ fgPoint3d fgPolarToCart3d(fgPoint3d p) {
|
|||
|
||||
/* Convert a cartesian coordinate to polar coordinates (lon/lat
|
||||
* specified in radians. Distances are specified in meters. */
|
||||
fgPoint3d fgCartToPolar3d(fgPoint3d cp) {
|
||||
fgPoint3d pp;
|
||||
Point3D fgCartToPolar3d(const Point3D& cp) {
|
||||
Point3D pp;
|
||||
|
||||
pp.lon = atan2( cp.y, cp.x );
|
||||
pp.lat = FG_PI_2 - atan2( sqrt(cp.x*cp.x + cp.y*cp.y), cp.z );
|
||||
pp.radius = sqrt(cp.x*cp.x + cp.y*cp.y + cp.z*cp.z);
|
||||
pp.setvals( atan2( cp.y(), cp.x() ),
|
||||
FG_PI_2 - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
|
||||
sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
|
||||
|
||||
/* printf("lon = %.2f lat = %.2f radius = %.2f\n",
|
||||
pp.lon, pp.lat, pp.radius); */
|
||||
|
||||
return(pp);
|
||||
}
|
||||
|
||||
|
|
@ -67,14 +68,14 @@ fgPoint3d fgCartToPolar3d(fgPoint3d cp) {
|
|||
/* Find the Altitude above the Ellipsoid (WGS84) given the Earth
|
||||
* Centered Cartesian coordinate vector Distances are specified in
|
||||
* meters. */
|
||||
double fgGeodAltFromCart(fgPoint3d cp)
|
||||
double fgGeodAltFromCart(const Point3D& cp)
|
||||
{
|
||||
double t_lat, x_alpha, mu_alpha;
|
||||
double lat_geoc, radius;
|
||||
double result;
|
||||
|
||||
lat_geoc = FG_PI_2 - atan2( sqrt(cp.x*cp.x + cp.y*cp.y), cp.z );
|
||||
radius = sqrt(cp.x*cp.x + cp.y*cp.y + cp.z*cp.z);
|
||||
lat_geoc = FG_PI_2 - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() );
|
||||
radius = sqrt( cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z() );
|
||||
|
||||
if( ( (FG_PI_2 - lat_geoc) < ONE_SECOND ) /* near North pole */
|
||||
|| ( (FG_PI_2 + lat_geoc) < ONE_SECOND ) ) /* near South pole */
|
||||
|
|
@ -95,9 +96,12 @@ double fgGeodAltFromCart(fgPoint3d cp)
|
|||
|
||||
|
||||
/* $Log$
|
||||
/* Revision 1.2 1998/08/24 20:04:11 curt
|
||||
/* Various "inline" code optimizations contributed by Norman Vine.
|
||||
/* Revision 1.3 1998/10/16 00:50:29 curt
|
||||
/* Added point3d.hxx to replace cheezy fgPoint3d struct.
|
||||
/*
|
||||
* Revision 1.2 1998/08/24 20:04:11 curt
|
||||
* Various "inline" code optimizations contributed by Norman Vine.
|
||||
*
|
||||
* Revision 1.1 1998/07/08 14:40:08 curt
|
||||
* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx
|
||||
* Updated fg_geodesy comments to reflect that routines expect and produce
|
||||
|
|
|
|||
|
|
@ -34,33 +34,37 @@
|
|||
|
||||
|
||||
#include <Include/fg_constants.h>
|
||||
#include <Include/fg_types.h>
|
||||
// #include <Include/fg_types.h>
|
||||
#include <Math/point3d.hxx>
|
||||
|
||||
|
||||
/* Convert a polar coordinate to a cartesian coordinate. Lon and Lat
|
||||
* must be specified in radians. The FG convention is for distances
|
||||
* to be specified in meters */
|
||||
fgPoint3d fgPolarToCart3d(fgPoint3d p);
|
||||
Point3D fgPolarToCart3d(const Point3D& p);
|
||||
|
||||
|
||||
/* Convert a cartesian coordinate to polar coordinates (lon/lat
|
||||
* specified in radians. Distances are specified in meters. */
|
||||
fgPoint3d fgCartToPolar3d(fgPoint3d cp);
|
||||
Point3D fgCartToPolar3d(const Point3D& cp);
|
||||
|
||||
|
||||
/* Find the Altitude above the Ellipsoid (WGS84) given the Earth
|
||||
* Centered Cartesian coordinate vector Distances are specified in
|
||||
* meters. */
|
||||
double fgGeodAltFromCart(fgPoint3d cp);
|
||||
double fgGeodAltFromCart(const Point3D& cp);
|
||||
|
||||
|
||||
#endif /* _POLAR_HXX */
|
||||
|
||||
|
||||
/* $Log$
|
||||
/* Revision 1.2 1998/08/24 20:04:12 curt
|
||||
/* Various "inline" code optimizations contributed by Norman Vine.
|
||||
/* Revision 1.3 1998/10/16 00:50:30 curt
|
||||
/* Added point3d.hxx to replace cheezy fgPoint3d struct.
|
||||
/*
|
||||
* Revision 1.2 1998/08/24 20:04:12 curt
|
||||
* Various "inline" code optimizations contributed by Norman Vine.
|
||||
*
|
||||
* Revision 1.1 1998/07/08 14:40:09 curt
|
||||
* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx
|
||||
* Updated fg_geodesy comments to reflect that routines expect and produce
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <Include/fg_types.h>
|
||||
// #include <Include/fg_types.h>
|
||||
|
||||
#include "vector.hxx"
|
||||
|
||||
|
|
@ -135,9 +135,12 @@ double fgPointLineSquared(MAT3vec p, MAT3vec p0, MAT3vec d) {
|
|||
|
||||
|
||||
/* $Log$
|
||||
/* Revision 1.3 1998/08/24 20:04:12 curt
|
||||
/* Various "inline" code optimizations contributed by Norman Vine.
|
||||
/* Revision 1.4 1998/10/16 00:50:31 curt
|
||||
/* Added point3d.hxx to replace cheezy fgPoint3d struct.
|
||||
/*
|
||||
* Revision 1.3 1998/08/24 20:04:12 curt
|
||||
* Various "inline" code optimizations contributed by Norman Vine.
|
||||
*
|
||||
* Revision 1.2 1998/07/24 21:34:38 curt
|
||||
* fgPointLine() rewritten into fgPointLineSquared() ... this ultimately saves
|
||||
* us from doing a sqrt().
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue