7#include "RadarGroundMap.h"
16 RadarGroundMap::RadarGroundMap(
Camera *parent, Radar::LookDirection ldir,
20 p_lookDirection = ldir;
21 p_waveLength = waveLength;
32 double et1 = p_camera->Spice::cacheStartTime().Et();
33 double et2 = p_camera->Spice::cacheEndTime().Et();
34 p_timeTolerance = (et2 - et1) / p_camera->Lines() / 20.0;
52 std::vector<double> Ssc(6);
54 vequ_c((SpiceDouble *) & (spaceCraft->
Coordinate()[0]), &Ssc[0]);
55 vequ_c((SpiceDouble *) & (spaceCraft->
Velocity()[0]), &Ssc[3]);
58 std::vector<double> bfSsc(6);
62 std::vector<double> Vsc(3);
63 std::vector<double> Xsc(3);
64 vequ_c(&bfSsc[0], (SpiceDouble *) & (Xsc[0]));
65 vequ_c(&bfSsc[3], (SpiceDouble *) & (Vsc[0]));
73 dp = vdot_c(&Xsc[0], i);
74 SpiceDouble p[3], q[3];
76 vsub_c(&Xsc[0], p, q);
84 p_camera->radii(radii);
87 SpiceDouble lat = DBL_MAX;
88 SpiceDouble lon = DBL_MAX;
91 slantRangeSqr = slantRangeSqr * slantRangeSqr;
102 bool useSlopeEqn =
false;
104 bool success =
Iterate(R,slantRangeSqr,c,r,X,lat,lon,Xsc,useSlopeEqn,slope);
109 success =
Iterate(R,slantRangeSqr,c,r,X,lat,lon,Xsc,useSlopeEqn,slope);
112 if(!success)
return false;
116 while(lon < 0.0) lon += 360.0;
119 std::vector<double> lookB;
121 lookB[0] = X[0] - Xsc[0];
122 lookB[1] = X[1] - Xsc[1];
123 lookB[2] = X[2] - Xsc[2];
125 std::vector<double> lookJ = bodyFrame->
J2000Vector(lookB);
129 SpiceDouble unitLookC[3];
130 vhat_c(&lookC[0], unitLookC);
132 return p_camera->Sensor::SetUniversalGround(lat, lon);
144 const SpiceDouble r[], SpiceDouble X[], SpiceDouble &lat,
145 SpiceDouble &lon,
const std::vector<double> &Xsc,
146 const bool &useSlopeEqn,
const double &slope) {
150 SpiceDouble lastR = DBL_MAX;
155 double normXsc = vnorm_c(&Xsc[0]);
156 double alpha = (R * R - slantRangeSqr - normXsc * normXsc) /
157 (2.0 * vdot_c(&Xsc[0], c));
159 double arg = slantRangeSqr - alpha * alpha;
160 if(arg < 0.0)
return false;
162 double beta = sqrt(arg);
163 if(p_lookDirection == Radar::Left) beta *= -1.0;
165 SpiceDouble alphac[3], betar[3];
166 vscl_c(alpha, c, alphac);
167 vscl_c(beta, r, betar);
169 vadd_c(alphac, betar, alphac);
170 vadd_c(&Xsc[0], alphac, X);
174 reclat_c(X, &R, &lon, &lat);
179 R = lastR + slope * (p_camera->LocalRadius(rlat, rlon).kilometers() - lastR);
181 R = p_camera->LocalRadius(rlat, rlon).kilometers();
186 while(fabs(R - lastR) > p_tolerance && iter < 100);
188 if(fabs(R - lastR) > p_tolerance)
return false;
201 Distance localRadius(p_camera->LocalRadius(lat, lon));
220 if(!surfacePoint.Valid())
return false;
226 double et1 = p_camera->Spice::cacheStartTime().Et();
227 p_camera->Sensor::setTime(et1);
228 double xv1 = ComputeXv(X);
231 double et2 = p_camera->Spice::cacheEndTime().Et();
232 p_camera->Sensor::setTime(et2);
233 double xv2 = ComputeXv(X);
236 if((xv1 < 0.0) && (xv2 < 0.0))
return false;
237 if((xv1 > 0.0) && (xv2 > 0.0))
return false;
240 double fl, fh, xl, xh;
255 for(
int j = 0; j < 30; j++) {
257 double etGuess = xl + (xh - xl) * fl / (fl - fh);
261 p_camera->Sensor::setTime(etGuess);
262 double fGuess = ComputeXv(X);
267 delTime = xl - etGuess;
272 delTime = xh - etGuess;
278 if((fabs(delTime) <= p_timeTolerance) || (fGuess == 0.0)) {
283 std::vector<double> Ssc(6);
286 vequ_c((SpiceDouble *) & (spaceCraft->
Coordinate()[0]), &Ssc[0]);
287 vequ_c((SpiceDouble *) & (spaceCraft->
Velocity()[0]), &Ssc[3]);
288 std::vector<double> bfSsc(6);
292 std::vector<double> Vsc(3);
293 std::vector<double> Xsc(3);
294 vequ_c(&bfSsc[0], (SpiceDouble *) & (Xsc[0]));
295 vequ_c(&bfSsc[3], (SpiceDouble *) & (Vsc[0]));
305 SpiceDouble vout1[3];
306 SpiceDouble vout2[3];
308 vsub_c(X, &Xsc[0], vout1);
309 vcrss_c(&Vsc[0], &Xsc[0], vout2);
310 dp = vdot_c(vout1, vout2);
311 if(dp > 0.0 && p_lookDirection == Radar::Left)
return false;
312 if(dp < 0.0 && p_lookDirection == Radar::Right)
return false;
313 if(dp == 0.0)
return false;
318 std::vector<double> lookB;
320 lookB[0] = X[0] - Xsc[0];
321 lookB[1] = X[1] - Xsc[1];
322 lookB[2] = X[2] - Xsc[2];
324 std::vector<double> lookJ = bodyFrame->
J2000Vector(lookB);
328 SpiceDouble unitLookC[3];
329 vhat_c(&lookC[0], unitLookC);
334 p_camera->target()->shape()->setSurfacePoint(surfacePoint);
338 return p_camera->Sensor::SetGround(surfacePoint,
true);
362 double *cudy,
bool test) {
374 std::vector<double> sJ(6);
376 vequ_c((SpiceDouble *) & (spaceCraft->
Coordinate()[0]), &sJ[0]);
377 vequ_c((SpiceDouble *) & (spaceCraft->
Velocity()[0]), &sJ[3]);
386 vequ_c(&p_sB[0], PsB);
387 vequ_c(&p_sB[3], VsB);
390 vsub_c(X, PsB, &p_lookB[0]);
398 QString msg =
"Back of planet test is not enabled for Radar images";
406 double RadarGroundMap::ComputeXv(SpiceDouble X[3]) {
413 std::vector<double> Ssc(6);
414 vequ_c((SpiceDouble *) & (spaceCraft->
Coordinate()[0]), &Ssc[0]);
415 vequ_c((SpiceDouble *) & (spaceCraft->
Velocity()[0]), &Ssc[3]);
418 std::vector<double> bfSsc(6);
422 std::vector<double> Vsc(3);
423 std::vector<double> Xsc(3);
424 vequ_c(&bfSsc[0], &Xsc[0]);
425 vequ_c(&bfSsc[3], &Vsc[0]);
428 SpiceDouble lookB[3];
429 vsub_c(&Xsc[0], X, lookB);
430 p_slantRange = vnorm_c(lookB);
435 double xv = -2.0 * vdot_c(lookB, &Vsc[0]) / (vnorm_c(lookB) * WaveLength());
462 double *dx,
double *dy) {
466 std::vector <double> d_lookJ(6);
468 vequ_c(&(instPos->
VelocityPartial(varType, coefIndex))[0], &d_lookJ[3]);
497 double *dx,
double *dy) {
Convert between undistorted focal plane and ground coordinates.
double p_focalPlaneX
Camera's x focal plane coordinate.
double p_focalPlaneY
Camera's y focal plane coordinate.
double kilometers() const
Get the displacement in kilometers.
Distance measurement, usually in meters.
bool isValid() const
Test if this distance has been initialized or not.
double kilometers() const
Get the distance in kilometers.
@ Programmer
This error is for when a programmer made an API call that was illegal.
This class is designed to encapsulate the concept of a Latitude.
This class is designed to encapsulate the concept of a Longitude.
virtual bool GetXY(const SurfacePoint &spoint, double *cudx, double *cudy, bool test=false)
Compute undistorted focal plane coordinate from ground position using current Spice from SetImage cal...
virtual bool GetdXYdPoint(std::vector< double > d_lookB, double *dx, double *dy)
Compute derivative of focal plane coordinate w/r to ground point from ground position using current S...
double p_slantRange
units are km
bool Iterate(SpiceDouble &R, const double &slantRangeSqr, const SpiceDouble c[], const SpiceDouble r[], SpiceDouble X[], SpiceDouble &lat, SpiceDouble &lon, const std::vector< double > &Xsc, const bool &useSlopeEqn, const double &slope)
Iteration loop for computing ground position from slant range.
virtual bool SetFocalPlane(const double ux, const double uy, const double uz)
Compute ground position from slant range.
virtual bool GetdXYdPosition(const SpicePosition::PartialType varType, int coefIndex, double *cudx, double *cudy)
Compute derivative w/r to position of focal plane coordinate from ground position using current Spice...
virtual bool SetGround(const Latitude &lat, const Longitude &lon)
Compute undistorted focal plane coordinate from ground position.
double p_groundSlantRange
units are km
double p_groundDopplerFreq
units are hertz
double p_rangeSigma
Scaling factor to convert meters to focal plane coord.
double p_dopplerSigma
Scaling factor to convert hertz to focal plane coord.
Obtain SPICE position information for a body.
const std::vector< double > & Velocity()
Return the current J2000 velocity.
std::vector< double > CoordinatePartial(SpicePosition::PartialType partialVar, int coeffIndex)
Set the coefficients of a polynomial fit to each of the three coordinates of the position vector for ...
virtual const std::vector< double > & Coordinate()
Return the current J2000 position.
std::vector< double > VelocityPartial(SpicePosition::PartialType partialVar, int coeffIndex)
Compute the derivative of the velocity with respect to the specified variable.
Obtain SPICE rotation information for a body.
std::vector< double > ReferenceVector(const std::vector< double > &jVec)
Given a direction vector in J2000, return a reference frame direction.
std::vector< double > J2000Vector(const std::vector< double > &rVec)
Given a direction vector in the reference frame, return a J2000 direction.
This class defines a body-fixed surface point.
void ToNaifArray(double naifOutput[3]) const
A naif array is a c-style array of size 3.
This is free and unencumbered software released into the public domain.
const double PI
The mathematical constant PI.
Namespace for the standard library.