00001 #pragma ident "$Id: Triple.cpp 935 2007-11-30 15:34:18Z architest $"
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00049 #include "geometry.hpp"
00050 #include "Triple.hpp"
00051 #include <cmath>
00052
00053 namespace gpstk
00054 {
00055 using namespace std;
00056
00057 Triple :: Triple()
00058 : theArray(3)
00059 {
00060 }
00061
00062 Triple :: Triple(const Triple& right)
00063 : theArray(right.theArray)
00064 {
00065 }
00066
00067 Triple :: Triple(double a,
00068 double b,
00069 double c)
00070 : theArray(3)
00071 {
00072 theArray[0] = a;
00073 theArray[1] = b;
00074 theArray[2] = c;
00075 }
00076
00077 Triple& Triple :: operator=(const Triple& right)
00078 {
00079 theArray = right.theArray;
00080 return *this;
00081 }
00082
00083 Triple& Triple :: operator=(const valarray<double>& right)
00084 throw(GeometryException)
00085 {
00086 if (right.size() != 3)
00087 {
00088 GPSTK_THROW(GeometryException("Incorrect vector size"));
00089 }
00090
00091 theArray = right;
00092 return *this;
00093 }
00094
00095
00096 double Triple :: dot(const Triple& right) const
00097 throw()
00098 {
00099 Triple z;
00100 z = (this->theArray)*(right.theArray);
00101 double a = z.theArray.sum();
00102 return a;
00103 }
00104
00105
00106
00107 Triple Triple :: cross(const Triple& right) const
00108 throw()
00109 {
00110 Triple cp;
00111 cp[0] = (*this)[1] * right[2] - (*this)[2] * right[1];
00112 cp[1] = (*this)[2] * right[0] - (*this)[0] * right[2];
00113 cp[2] = (*this)[0] * right[1] - (*this)[1] * right[0];
00114 return cp;
00115 }
00116
00117
00118 double Triple :: mag() const throw()
00119 {
00120 return ::sqrt(dot(*this));
00121 }
00122
00123 Triple Triple::unitVector() const
00124 throw(GeometryException)
00125 {
00126 double mag = sqrt(dot(*this));
00127
00128 if (mag <= 1e-14)
00129 GPSTK_THROW(GeometryException("Divide by Zero Error"));
00130
00131 Triple retArg;
00132 retArg[0] = (*this)[0] / mag;
00133 retArg[1] = (*this)[1] / mag;
00134 retArg[2] = (*this)[2] / mag;
00135 return(retArg);
00136 }
00137
00138
00139 double Triple :: cosVector(const Triple& right) const
00140 throw(GeometryException)
00141 {
00142 double rx, ry, cosvects;
00143
00144 rx = dot(*this);
00145 ry = right.dot(right);
00146
00147 if (rx <= 1e-14 || ry <= 1e-14)
00148 {
00149 GPSTK_THROW(GeometryException("Divide by Zero Error"));
00150 }
00151 cosvects = dot(right) / ::sqrt(rx * ry);
00152
00153
00154 if (fabs(cosvects) > 1.0e0)
00155 {
00156 cosvects = fabs(cosvects) / cosvects;
00157 }
00158
00159 return cosvects;
00160 }
00161
00162
00163
00164 double Triple :: slantRange(const Triple& right) const
00165 throw()
00166 {
00167 Triple z;
00168 z = right.theArray - this->theArray;
00169 double r = z.mag();
00170 return r;
00171 }
00172
00173
00174
00175
00176 double Triple :: elvAngle(const Triple& right) const
00177 throw(GeometryException)
00178 {
00179 Triple z;
00180 z = right.theArray - this->theArray;
00181 double c = z.cosVector(*this);
00182 return 90.0 - ::acos(c) * RAD_TO_DEG;
00183 }
00184
00185
00186
00187 double Triple :: azAngle(const Triple& right) const
00188 throw(GeometryException)
00189 {
00190 double xy, xyz, cosl, sinl, sint, xn1, xn2, xn3, xe1, xe2;
00191 double z1, z2, z3, p1, p2, test, alpha;
00192
00193 xy = (*this)[0] * (*this)[0] + (*this)[1] * (*this)[1] ;
00194 xyz = xy + (*this)[2] * (*this)[2] ;
00195 xy = ::sqrt(xy);
00196 xyz = ::sqrt(xyz);
00197
00198 if (xy <= 1e-14 || xyz <=1e-14)
00199 GPSTK_THROW(GeometryException("Divide by Zero Error"))
00200
00201 cosl = (*this)[0] / xy;
00202 sinl = (*this)[1] / xy;
00203 sint = (*this)[2] / xyz;
00204
00205 xn1 = -sint * cosl;
00206 xn2 = -sint * sinl;
00207 xn3 = xy/xyz;
00208
00209 xe1 = -sinl;
00210 xe2 = cosl;
00211
00212 z1 = right[0] - (*this)[0];
00213 z2 = right[1] - (*this)[1];
00214 z3 = right[2] - (*this)[2];
00215
00216 p1 = (xn1 * z1) + (xn2 * z2) + (xn3 * z3) ;
00217 p2 = (xe1 * z1) + (xe2 * z2) ;
00218
00219 test = fabs(p1) + fabs(p2);
00220
00221 if (test < 1.0e-16)
00222 {
00223 GPSTK_THROW(GeometryException("azAngle(), failed p1+p2 test."));
00224 }
00225
00226 alpha = 90 - ::atan2(p1, p2) * RAD_TO_DEG;
00227 if (alpha < 0)
00228 {
00229 return alpha + 360;
00230 }
00231 else
00232 {
00233 return alpha;
00234 }
00235 }
00236
00237
00238
00239
00240
00241
00242 Triple Triple::R1(const double& angle) const
00243 throw()
00244 {
00245 double ang(angle*DEG_TO_RAD);
00246 double sinangle(std::sin(ang));
00247 double cosangle(std::cos(ang));
00248 Triple rot;
00249 rot[0] = (*this)[0];
00250 rot[1] = cosangle*(*this)[1] + sinangle*(*this)[2];
00251 rot[2] = -sinangle*(*this)[1] + cosangle*(*this)[2];
00252 return rot;
00253 }
00254
00255
00256
00257
00258
00259
00260 Triple Triple::R2(const double& angle) const
00261 throw()
00262 {
00263 double ang(angle*DEG_TO_RAD);
00264 double sinangle(std::sin(ang));
00265 double cosangle(std::cos(ang));
00266 Triple rot;
00267 rot[0] = cosangle*(*this)[0] - sinangle*(*this)[2];
00268 rot[1] = (*this)[1];
00269 rot[2] = sinangle*(*this)[0] + cosangle*(*this)[2];
00270 return rot;
00271 }
00272
00273
00274
00275
00276
00277
00278 Triple Triple::R3(const double& angle) const
00279 throw()
00280 {
00281 double ang(angle*DEG_TO_RAD);
00282 double sinangle(std::sin(ang));
00283 double cosangle(std::cos(ang));
00284 Triple rot;
00285 rot[0] = cosangle*(*this)[0] + sinangle*(*this)[1];
00286 rot[1] = -sinangle*(*this)[0] + cosangle*(*this)[1];
00287 rot[2] = (*this)[2];
00288 return rot;
00289 }
00290
00291
00292 bool Triple :: operator== (const Triple& right) const
00293 {
00294 return (*this)[0]==right[0] && (*this)[1]==right[1] && (*this)[2]==right[2];
00295 }
00296
00297 Triple Triple :: operator-(const Triple& right) const
00298 {
00299 Triple tmp;
00300 tmp.theArray = this->theArray - right.theArray;
00301 return tmp;
00302 }
00303
00304 Triple Triple :: operator+(const Triple& right) const
00305 {
00306 Triple tmp;
00307 tmp.theArray = this->theArray + right.theArray;
00308 return tmp;
00309 }
00310
00311 Triple operator*(double scale, const Triple& rhs)
00312 {
00313 Triple tmp;
00314 tmp.theArray = rhs.theArray * scale;
00315 return tmp;
00316 }
00317
00318 std::ostream& operator<<(std::ostream& s,
00319 const gpstk::Triple& v)
00320 {
00321 if (v.size() > 0)
00322 {
00323 s << "(" << v[0];
00324 for (int i = 1; i < v.size(); i++)
00325 {
00326 s << ", " << v[i];
00327 }
00328 s << ")";
00329 }
00330
00331 return s;
00332 }
00333
00334 }
00335