Isis 3.0 Application Source Code Reference |
Home |
00001 /** 00002 * @file 00003 * $Revision: 1.1 $ 00004 * $Date: 2009/09/09 23:42:41 $ 00005 00006 * 00007 * Unless noted otherwise, the portions of Isis written by the USGS are 00008 * public domain. See individual third-party library and package descriptions 00009 * for intellectual property information, user agreements, and related 00010 * information. 00011 * 00012 * Although Isis has been used by the USGS, no warranty, expressed or 00013 * implied, is made by the USGS as to the accuracy and functioning of such 00014 * software and related material nor shall the fact of distribution 00015 * constitute any such warranty, and no responsibility is assumed by the 00016 * USGS in connection therewith. 00017 * 00018 * For additional information, launch 00019 * $ISISROOT/doc//documents/Disclaimers/Disclaimers.html 00020 * in a browser or see the Privacy & Disclaimers page on the Isis website, 00021 * http://isis.astrogeology.usgs.gov, and the USGS privacy and disclaimers on 00022 * http://www.usgs.gov/privacy.html. 00023 */ 00024 #include <cmath> 00025 #include <cstdlib> 00026 #include <sstream> 00027 #include <iostream> 00028 #include <iomanip> 00029 00030 #include "Camera.h" 00031 #include "Projection.h" 00032 #include "SpecialPixel.h" 00033 #include "Stereo.h" 00034 00035 #include "naif/SpiceUsr.h" 00036 #include "naif/SpiceZfc.h" 00037 #include "naif/SpiceZmc.h" 00038 00039 namespace Isis { 00040 00041 00042 bool Stereo::Elevation(Camera &cam1, Camera &cam2, double &radius, 00043 double &latitude, double &longitude, 00044 double &sepang, double &error) { 00045 00046 // Gut check on input Camera points 00047 radius = latitude = longitude = Isis::Null; 00048 if ( !cam1.HasSurfaceIntersection() ) return (false); 00049 if ( !cam2.HasSurfaceIntersection() ) return (false); 00050 00051 // Get spacecraft position from target 00052 double TC1[3], TC2[3]; 00053 TargetToSpacecraft(cam1, TC1); 00054 TargetToSpacecraft(cam2, TC2); 00055 00056 00057 // Get surface vectors from center of body to surface 00058 double TP1[3], TP2[3]; 00059 TargetToSurface(cam1, TP1); 00060 TargetToSurface(cam2, TP2); 00061 00062 // Stereo angle 00063 sepang = vsep_c(TC1, TC2) * dpr_c(); 00064 00065 SpiceDouble CP1[3], CP2[3]; 00066 vsub_c(TC1, TP1, CP1); 00067 vsub_c(TC2, TP2, CP2); 00068 00069 sepang = vsep_c(CP1, CP2) * dpr_c(); 00070 00071 double DR1, DR2; 00072 DR1 = vnorm_c(CP1); 00073 DR2 = vnorm_c(CP2); 00074 00075 vscl_c(1.0/DR1, CP1, CP1); 00076 vscl_c(1.0/DR2, CP2, CP2); 00077 00078 // Do stereo intersections 00079 double aa = CP2[0]; 00080 double bb = CP2[1]; 00081 double cc = CP2[2]; 00082 double xx = CP1[0]; 00083 double yy = CP1[1]; 00084 double zz = CP1[2]; 00085 00086 // Vector between both spacecraft 00087 double dd = TC2[0] - TC1[0]; 00088 double ee = TC2[1] - TC1[1]; 00089 double ff = TC2[2] - TC1[2]; 00090 00091 // Do the stereo intersection 00092 double bzcy = bb*zz - cc*yy; 00093 double cebf = cc*ee - bb*ff; 00094 double cxaz = cc*xx - aa*zz; 00095 double afcd = aa*ff - cc*dd; 00096 double aybx = aa*yy - bb*xx; 00097 double bdae = bb*dd - aa*ee; 00098 00099 // Get fraction `T' along left vector to "intersection point" 00100 double T=-(bzcy*cebf+cxaz*afcd+aybx*bdae)/ 00101 (bzcy*bzcy+cxaz*cxaz+aybx*aybx); 00102 double lx=TC1[0] + T * CP1[0]; 00103 double ly=TC1[1] + T * CP1[1]; 00104 double lz=TC1[2] + T * CP1[2]; 00105 00106 // Find the Perp. vector between both lines (at shortest sep.) 00107 double x = TC2[0] - lx; 00108 double y = TC2[1] - ly; 00109 double z = TC2[2] - lz; 00110 00111 // Find the separation distance - useful later 00112 double rx = y * CP2[2] - CP2[1] * z; 00113 double ry = CP2[0] * z - x * CP2[2]; 00114 double rz = x * CP2[1] - CP2[0] * y; 00115 double dr = std::sqrt(rx*rx+ry*ry+rz*rz); 00116 00117 // Find position of intersection on lower line 00118 rx = CP1[1] * CP2[2] - CP2[1] * CP1[2]; 00119 ry = CP2[0] * CP1[2] - CP1[0] * CP2[2]; 00120 rz = CP1[0] * CP2[1] - CP2[0] * CP1[1]; 00121 double raa = std::sqrt(rx*rx+ry*ry+rz*rz); 00122 00123 // Normalize our new Perpendicular vector 00124 rx = rx/raa; 00125 ry = ry/raa; 00126 rz = rz/raa; 00127 00128 // Get the other intersection position 00129 rx = lx - rx*dr; 00130 ry = ly - ry*dr; 00131 rz = lz - rz*dr; 00132 00133 // Compute the mid point of the intersection on the planets 00134 // surface 00135 double mx = (lx+rx)/2.0; 00136 double my = (ly+ry)/2.0; 00137 double mz = (lz+rz)/2.0; 00138 Rectangular(mx, my, mz, latitude, longitude, radius); 00139 radius *= 1000.0 ; // convert to meters 00140 error = dr * 1000.0; 00141 return (true); 00142 } 00143 00144 00145 void Stereo::Spherical(const double latitude, const double longitude, 00146 const double radius, 00147 double &x, double &y, double &z) { 00148 SpiceDouble rec[3]; 00149 latrec_c(radius/1000.0, longitude*rpd_c(), latitude*rpd_c(), &rec[0]); 00150 x = rec[0]; 00151 y = rec[1]; 00152 z = rec[2]; 00153 return; 00154 } 00155 00156 void Stereo::Rectangular(const double x, const double y, const double z, 00157 double &latitude, double &longitude, 00158 double &radius) { 00159 SpiceDouble rec[3]; 00160 rec[0] = x; 00161 rec[1] = y; 00162 rec[2] = z; 00163 reclat_c(&rec[0], &radius, &longitude, &latitude); 00164 longitude *= dpr_c(); 00165 latitude *= dpr_c(); 00166 longitude = Projection::To360Domain(longitude); 00167 return; 00168 return; 00169 } 00170 00171 std::vector<double> Stereo::Array2StdVec(const double d[3]) { 00172 std::vector<double> v; 00173 for ( int i = 0 ; i < 3 ; i++ ) { 00174 v.push_back(d[i]); 00175 } 00176 return (v); 00177 } 00178 00179 double *Stereo::StdVec2Array(const std::vector<double> &v, double *d) { 00180 if ( d == NULL ) { 00181 d = new double[v.size()]; 00182 } 00183 00184 for ( unsigned int i = 0 ; i < v.size() ; i++ ) { 00185 d[i] = v[i]; 00186 } 00187 return (d); 00188 } 00189 00190 void Stereo::TargetToSpacecraft(Camera &camera, double TP[3]) { 00191 camera.InstrumentPosition(TP); 00192 return; 00193 } 00194 00195 void Stereo::TargetToSurface(Camera &camera, double TC[3]) { 00196 camera.Coordinate(TC); 00197 return; 00198 } 00199 00200 }