00001 #pragma ident "$Id: Triple.cpp 2944 2011-10-27 08:04:41Z yanweignss $"
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 Vector<double> Triple::toVector()
00097 {
00098 Vector<double> toReturn(3,0.0);
00099 for(int i=0;i<3;i++) toReturn(i) = theArray[i];
00100 return toReturn;
00101 }
00102
00103
00104
00105 std::vector<double> Triple::toStdVector()
00106 {
00107 std::vector<double> toReturn;
00108
00109 return toReturn;
00110 }
00111
00112
00113 double Triple :: dot(const Triple& right) const
00114 throw()
00115 {
00116 Triple z;
00117 z = (this->theArray)*(right.theArray);
00118 double a = z.theArray.sum();
00119 return a;
00120 }
00121
00122
00123
00124 Triple Triple :: cross(const Triple& right) const
00125 throw()
00126 {
00127 Triple cp;
00128 cp[0] = (*this)[1] * right[2] - (*this)[2] * right[1];
00129 cp[1] = (*this)[2] * right[0] - (*this)[0] * right[2];
00130 cp[2] = (*this)[0] * right[1] - (*this)[1] * right[0];
00131 return cp;
00132 }
00133
00134
00135 double Triple :: mag() const throw()
00136 {
00137 return std::sqrt(dot(*this));
00138 }
00139
00140 Triple Triple::unitVector() const
00141 throw(GeometryException)
00142 {
00143 double mag = std::sqrt(dot(*this));
00144
00145 if (mag <= 1e-14)
00146 GPSTK_THROW(GeometryException("Divide by Zero Error"));
00147
00148 Triple retArg;
00149 retArg[0] = (*this)[0] / mag;
00150 retArg[1] = (*this)[1] / mag;
00151 retArg[2] = (*this)[2] / mag;
00152 return(retArg);
00153 }
00154
00155
00156 double Triple :: cosVector(const Triple& right) const
00157 throw(GeometryException)
00158 {
00159 double rx, ry, cosvects;
00160
00161 rx = dot(*this);
00162 ry = right.dot(right);
00163
00164 if (rx <= 1e-14 || ry <= 1e-14)
00165 {
00166 GPSTK_THROW(GeometryException("Divide by Zero Error"));
00167 }
00168 cosvects = dot(right) / ::sqrt(rx * ry);
00169
00170
00171 if (fabs(cosvects) > 1.0e0)
00172 {
00173 cosvects = fabs(cosvects) / cosvects;
00174 }
00175
00176 return cosvects;
00177 }
00178
00179
00180
00181 double Triple :: slantRange(const Triple& right) const
00182 throw()
00183 {
00184 Triple z;
00185 z = right.theArray - this->theArray;
00186 double r = z.mag();
00187 return r;
00188 }
00189
00190
00191
00192
00193 double Triple :: elvAngle(const Triple& right) const
00194 throw(GeometryException)
00195 {
00196 Triple z;
00197 z = right.theArray - this->theArray;
00198 double c = z.cosVector(*this);
00199 return 90.0 - ::acos(c) * RAD_TO_DEG;
00200 }
00201
00202
00203
00204 double Triple :: azAngle(const Triple& right) const
00205 throw(GeometryException)
00206 {
00207 double xy, xyz, cosl, sinl, sint, xn1, xn2, xn3, xe1, xe2;
00208 double z1, z2, z3, p1, p2, test, alpha;
00209
00210 xy = (*this)[0] * (*this)[0] + (*this)[1] * (*this)[1] ;
00211 xyz = xy + (*this)[2] * (*this)[2] ;
00212 xy = ::sqrt(xy);
00213 xyz = ::sqrt(xyz);
00214
00215 if (xy <= 1e-14 || xyz <=1e-14)
00216 GPSTK_THROW(GeometryException("Divide by Zero Error"))
00217
00218 cosl = (*this)[0] / xy;
00219 sinl = (*this)[1] / xy;
00220 sint = (*this)[2] / xyz;
00221
00222 xn1 = -sint * cosl;
00223 xn2 = -sint * sinl;
00224 xn3 = xy/xyz;
00225
00226 xe1 = -sinl;
00227 xe2 = cosl;
00228
00229 z1 = right[0] - (*this)[0];
00230 z2 = right[1] - (*this)[1];
00231 z3 = right[2] - (*this)[2];
00232
00233 p1 = (xn1 * z1) + (xn2 * z2) + (xn3 * z3) ;
00234 p2 = (xe1 * z1) + (xe2 * z2) ;
00235
00236 test = fabs(p1) + fabs(p2);
00237
00238 if (test < 1.0e-16)
00239 {
00240 GPSTK_THROW(GeometryException("azAngle(), failed p1+p2 test."));
00241 }
00242
00243 alpha = 90 - ::atan2(p1, p2) * RAD_TO_DEG;
00244 if (alpha < 0)
00245 {
00246 return alpha + 360;
00247 }
00248 else
00249 {
00250 return alpha;
00251 }
00252 }
00253
00254
00255
00256
00257
00258
00259 Triple Triple::R1(const double& angle) const
00260 throw()
00261 {
00262 double ang(angle*DEG_TO_RAD);
00263 double sinangle(std::sin(ang));
00264 double cosangle(std::cos(ang));
00265 Triple rot;
00266 rot[0] = (*this)[0];
00267 rot[1] = cosangle*(*this)[1] + sinangle*(*this)[2];
00268 rot[2] = -sinangle*(*this)[1] + cosangle*(*this)[2];
00269 return rot;
00270 }
00271
00272
00273
00274
00275
00276
00277 Triple Triple::R2(const double& angle) const
00278 throw()
00279 {
00280 double ang(angle*DEG_TO_RAD);
00281 double sinangle(std::sin(ang));
00282 double cosangle(std::cos(ang));
00283 Triple rot;
00284 rot[0] = cosangle*(*this)[0] - sinangle*(*this)[2];
00285 rot[1] = (*this)[1];
00286 rot[2] = sinangle*(*this)[0] + cosangle*(*this)[2];
00287 return rot;
00288 }
00289
00290
00291
00292
00293
00294
00295 Triple Triple::R3(const double& angle) const
00296 throw()
00297 {
00298 double ang(angle*DEG_TO_RAD);
00299 double sinangle(std::sin(ang));
00300 double cosangle(std::cos(ang));
00301 Triple rot;
00302 rot[0] = cosangle*(*this)[0] + sinangle*(*this)[1];
00303 rot[1] = -sinangle*(*this)[0] + cosangle*(*this)[1];
00304 rot[2] = (*this)[2];
00305 return rot;
00306 }
00307
00308
00309 bool Triple :: operator== (const Triple& right) const
00310 {
00311 return (*this)[0]==right[0] && (*this)[1]==right[1] && (*this)[2]==right[2];
00312 }
00313
00314 Triple Triple :: operator-(const Triple& right) const
00315 {
00316 Triple tmp;
00317 tmp.theArray = this->theArray - right.theArray;
00318 return tmp;
00319 }
00320
00321 Triple Triple :: operator+(const Triple& right) const
00322 {
00323 Triple tmp;
00324 tmp.theArray = this->theArray + right.theArray;
00325 return tmp;
00326 }
00327
00328 Triple operator*(double scale, const Triple& rhs)
00329 {
00330 Triple tmp;
00331 tmp.theArray = rhs.theArray * scale;
00332 return tmp;
00333 }
00334
00335 std::ostream& operator<<(std::ostream& s,
00336 const gpstk::Triple& v)
00337 {
00338 if (v.size() > 0)
00339 {
00340 s << "(" << v[0];
00341 for (int i = 1; i < v.size(); i++)
00342 {
00343 s << ", " << v[i];
00344 }
00345 s << ")";
00346 }
00347
00348 return s;
00349 }
00350
00351 }
00352