45 bool RadarGroundMap::SetFocalPlane(
const double ux,
const double uy,
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);
59 bfSsc = bodyFrame->ReferenceVector(Ssc);
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;
90 double slantRangeSqr = (ux * p_rangeSigma) / 1000.;
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);
143 bool RadarGroundMap::Iterate(SpiceDouble &R,
const double &slantRangeSqr,
const SpiceDouble c[],
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;
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);
289 bfSsc = bodyFrame->ReferenceVector(Ssc);
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);
331 p_camera->SetFocalLength(p_slantRange * 1000.0);
332 p_focalPlaneX = p_slantRange * 1000.0 / p_rangeSigma;
334 p_camera->target()->shape()->setSurfacePoint(surfacePoint);
338 return p_camera->Sensor::SetGround(surfacePoint,
true);
362 double *cudy,
bool test) {
366 X[0] = spoint.GetX().kilometers();
367 X[1] = spoint.GetY().kilometers();
368 X[2] = spoint.GetZ().kilometers();
374 std::vector<double> sJ(6);
376 vequ_c((SpiceDouble *) & (spaceCraft->Coordinate()[0]), &sJ[0]);
377 vequ_c((SpiceDouble *) & (spaceCraft->Velocity()[0]), &sJ[3]);
381 p_sB = bodyFrame->ReferenceVector(sJ);
386 vequ_c(&p_sB[0], PsB);
387 vequ_c(&p_sB[3], VsB);
390 vsub_c(X, PsB, &p_lookB[0]);
392 p_groundSlantRange = vnorm_c(&p_lookB[0]);
393 p_groundDopplerFreq = 2. / p_waveLength / p_groundSlantRange * vdot_c(&p_lookB[0], &VsB[0]);
394 *cudx = p_groundSlantRange * 1000.0 / p_rangeSigma;
395 *cudy = p_groundDopplerFreq / p_dopplerSigma;
398 QString msg =
"Back of planet test is not enabled for Radar images";
399 throw IException(IException::Programmer, msg, _FILEINFO_);
461 bool RadarGroundMap::GetdXYdPosition(
const SpicePosition::PartialType varType,
int coefIndex,
462 double *dx,
double *dy) {
466 std::vector <double> d_lookJ(6);
467 vequ_c(&(instPos->CoordinatePartial(varType, coefIndex))[0], &d_lookJ[0]);
468 vequ_c(&(instPos->VelocityPartial(varType, coefIndex))[0], &d_lookJ[3]);
470 std::vector<double> d_lookB = bodyRot->ReferenceVector(d_lookJ);
472 double d_slantRange = (-1.) * vdot_c(&p_lookB[0], &d_lookB[0]) / p_groundSlantRange;
473 double d_dopplerFreq = (-1.) * p_groundDopplerFreq * d_slantRange / p_groundSlantRange -
474 2. / p_waveLength / p_groundSlantRange * vdot_c(&d_lookB[0], &p_sB[3]) +
475 2. / p_waveLength / p_groundSlantRange * vdot_c(&p_lookB[0], &d_lookB[3]);
477 *dx = d_slantRange * 1000.0 / p_rangeSigma;
478 *dy = d_dopplerFreq / p_dopplerSigma;
496 bool RadarGroundMap::GetdXYdPoint(std::vector<double> d_lookB,
497 double *dx,
double *dy) {
501 double d_slantRange = vdot_c(&p_lookB[0], &d_lookB[0]) / p_groundSlantRange;
505 double d_dopplerFreq = (-1.) * p_groundDopplerFreq * d_slantRange / p_groundSlantRange
506 + 2. / p_waveLength / p_groundSlantRange * vdot_c(&d_lookB[0], &p_sB[3]);
509 *dx = d_slantRange * 1000.0 / p_rangeSigma;
510 *dy = d_dopplerFreq / p_dopplerSigma;