From c5adb85741158b1d46eb630ec1d6dbd967248c52 Mon Sep 17 00:00:00 2001 From: curt Date: Fri, 16 Oct 1998 00:50:29 +0000 Subject: [PATCH] Added point3d.hxx to replace cheezy fgPoint3d struct. --- Math/point3d.hxx | 318 +++++++++++++++++++++++++++++++++++++++++++++++ Math/polar3d.cxx | 36 +++--- Math/polar3d.hxx | 16 ++- Math/vector.cxx | 9 +- 4 files changed, 354 insertions(+), 25 deletions(-) create mode 100644 Math/point3d.hxx diff --git a/Math/point3d.hxx b/Math/point3d.hxx new file mode 100644 index 00000000..ab30d94e --- /dev/null +++ b/Math/point3d.hxx @@ -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 + +#include "Include/fg_stl_config.h" + +#ifdef NEEDNAMESPACESTD +using namespace std; +#endif + +/* +#include +#include +#include +*/ + +#include + +#include + + +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. +// +// diff --git a/Math/polar3d.cxx b/Math/polar3d.cxx index 19fd26ab..a7a44048 100644 --- a/Math/polar3d.cxx +++ b/Math/polar3d.cxx @@ -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 diff --git a/Math/polar3d.hxx b/Math/polar3d.hxx index c568c31f..bc873e1e 100644 --- a/Math/polar3d.hxx +++ b/Math/polar3d.hxx @@ -34,33 +34,37 @@ #include -#include +// #include +#include /* 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 diff --git a/Math/vector.cxx b/Math/vector.cxx index aac81abf..641c056e 100644 --- a/Math/vector.cxx +++ b/Math/vector.cxx @@ -27,7 +27,7 @@ #include #include -#include +// #include #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(). -- 2.39.5