static VALUE latrec(VALUE self, VALUE radius, VALUE longitude, VALUE latitude) {
double vector[3];
latrec_c(NUM2DBL(radius), NUM2DBL(longitude), NUM2DBL(latitude), vector);
return rb_nmatrix_dense_create(FLOAT64, (size_t *) VECTOR_SHAPE, 2, (void *) vector, 3);
}
/** Compute undistorted focal plane coordinate from ground position using current Spice from SetImage call
*
* This method will compute the undistorted focal plane coordinate for
* a ground position, using the current Spice settings (time and kernels)
* without resetting the current point values for lat/lon/radius/x/y.
*
* @param lat planetocentric latitude in degrees
* @param lon planetocentric longitude in degrees
* @param radius local radius in m
*
* @return conversion was successful
*/
bool CameraGroundMap::GetXY(const double lat, const double lon, const double radius,
std::vector<double> &lookJ) {
// Check for Sky images
if ( p_camera->IsSky() ) {
return false;
}
// Should a check be added to make sure SetImage has been called???
// Compute the look vector in body-fixed coordinates
double pB[3]; // Point on surface
latrec_c( radius/1000.0, lon*Isis::PI/180.0, lat*Isis::PI/180.0, pB);
// Get spacecraft vector in body-fixed coordinates
SpiceRotation *bodyRot = p_camera->BodyRotation();
std::vector<double> sB = bodyRot->ReferenceVector(p_camera->InstrumentPosition()->Coordinate());
std::vector<double> lookB(3);
for (int ic=0; ic<3; ic++) lookB[ic] = pB[ic] - sB[ic];
// Check for point on back of planet by checking to see if surface point is viewable (test emission angle)
// During iterations, we may not want to do the back of planet test???
double upsB[3],upB[3],dist;
vminus_c ( (SpiceDouble *) &lookB[0], upsB);
unorm_c (upsB, upsB, &dist);
unorm_c (pB, upB, &dist);
double angle = vdot_c(upB, upsB);
double emission;
if (angle > 1) {
emission = 0;
}
else if (angle < -1) {
emission = 180.;
}
else {
emission = acos (angle) * 180.0 / Isis::PI;
}
if (fabs(emission) > 90.) return false;
// Get the look vector in the camera frame and the instrument rotation
lookJ.resize(3);
lookJ = p_camera->BodyRotation()->J2000Vector( lookB );
return true;
}
/** Compute undistorted focal plane coordinate from ground position that includes a local radius
*
* @param lat planetocentric latitude in degrees
* @param lon planetocentric longitude in degrees
* @param radius local radius in meters
*
* @return conversion was successful
*/
bool RadarGroundMap::SetGround(const double lat, const double lon, const double radius) {
// Get the ground point in rectangular coordinates (X)
SpiceDouble X[3];
SpiceDouble rlat = lat*Isis::PI/180.0;
SpiceDouble rlon = lon*Isis::PI/180.0;
latrec_c(radius,rlon,rlat,X);
// Compute lower bound for Doppler shift
double et1 = p_camera->Spice::CacheStartTime();
p_camera->Sensor::SetEphemerisTime(et1);
double xv1 = ComputeXv(X);
// Compute upper bound for Doppler shift
double et2 = p_camera->Spice::CacheEndTime();
p_camera->Sensor::SetEphemerisTime(et2);
double xv2 = ComputeXv(X);
// Make sure we bound root (xv = 0.0)
if ((xv1 < 0.0) && (xv2 < 0.0)) return false;
if ((xv1 > 0.0) && (xv2 > 0.0)) return false;
// Order the bounds
double fl,fh,xl,xh;
if (xv1 < xv2) {
fl = xv1;
fh = xv2;
xl = et1;
xh = et2;
}
else {
fl = xv2;
fh = xv1;
xl = et2;
xh = et1;
}
// Iterate a max of 30 times
for (int j=0; j<30; j++) {
// Use the secant method to guess the next et
double etGuess = xl + (xh - xl) * fl / (fl - fh);
// Compute the guessed Doppler shift. Hopefully
// this guess converges to zero at some point
p_camera->Sensor::SetEphemerisTime(etGuess);
double fGuess = ComputeXv(X);
// Update the bounds
double delTime;
if (fGuess < 0.0) {
delTime = xl - etGuess;
xl = etGuess;
fl = fGuess;
}
else {
delTime = xh - etGuess;
xh = etGuess;
fh = fGuess;
}
// See if we are done
if ((fabs(delTime) <= p_timeTolerance) || (fGuess == 0.0)) {
SpiceRotation *bodyFrame = p_camera->BodyRotation();
SpicePosition *spaceCraft = p_camera->InstrumentPosition();
// Get body fixed spacecraft velocity and position
std::vector<double> Ssc(6);
// Load the state into Ssc and rotate to body-fixed
vequ_c ( (SpiceDouble *) &(spaceCraft->Coordinate()[0]), &Ssc[0]);
vequ_c ( (SpiceDouble *) &(spaceCraft->Velocity()[0]), &Ssc[3]);
std::vector<double> bfSsc(6);
bfSsc = bodyFrame->ReferenceVector(Ssc);
// Extract the body-fixed position and velocity from the state
std::vector<double> Vsc(3);
std::vector<double> Xsc(3);
vequ_c ( &bfSsc[0], (SpiceDouble *) &(Xsc[0]) );
vequ_c ( &bfSsc[3], (SpiceDouble *) &(Vsc[0]) );
// Determine if focal plane coordinate falls on the correct side of the
// spacecraft. Radar has both left and right look directions. Make sure
// the coordinate is on the same side as the look direction. This is done
// by (X - S) . (V x S) where X=ground point vector, S=spacecraft position
// vector, and V=velocity vector. If the dot product is greater than 0, then
// the point is on the right side. If the dot product is less than 0, then
// the point is on the left side. If the dot product is 0, then the point is
// directly under the spacecraft (neither left or right) and is invalid.
SpiceDouble vout1[3];
SpiceDouble vout2[3];
SpiceDouble dp;
vsub_c(X,&Xsc[0],vout1);
vcrss_c(&Vsc[0],&Xsc[0],vout2);
dp = vdot_c(vout1,vout2);
if (dp > 0.0 && p_lookDirection == Radar::Left) return false;
if (dp < 0.0 && p_lookDirection == Radar::Right) return false;
if (dp == 0.0) return false;
// Compute body fixed look direction
std::vector<double> lookB;
lookB.resize(3);
lookB[0] = X[0] - Xsc[0];
lookB[1] = X[1] - Xsc[1];
lookB[2] = X[2] - Xsc[2];
std::vector<double> lookJ = bodyFrame->J2000Vector(lookB);
SpiceRotation *cameraFrame = p_camera->InstrumentRotation();
std::vector<double> lookC = cameraFrame->ReferenceVector(lookJ);
SpiceDouble unitLookC[3];
vhat_c(&lookC[0],unitLookC);
p_camera->SetLookDirection(unitLookC);
p_camera->SetFocalLength(p_slantRange*1000.0);
p_focalPlaneX = p_slantRange / p_rangeSigma;
p_focalPlaneY = 0.0;
return true;
}
}
return false;
}
void radrec_c ( SpiceDouble range,
SpiceDouble ra,
SpiceDouble dec,
SpiceDouble rectan[3] )
/*
-Brief_I/O
VARIABLE I/O DESCRIPTION
-------- --- ---------------------------------------------------
range I Distance of a point from the origin.
ra I Right ascension of point in radians.
dec I Declination of point in radians.
rectan O Rectangular coordinates of the point.
-Detailed_Input
range is the distance of the point from the origin. Output
units are the same as the units associated with `range.'
ra is the right ascension of the input point: the angular
distance measured toward the east from the prime meridian
to the meridian containing the input point. The direction
of increasing right ascension is from the +X axis towards
the +Y axis.
The range (i.e., the set of allowed values) of
`ra' is unrestricted. Units are radians.
dec is the declination of the point. This is the angular
distance from the XY plane to the point.
The range of `dec' is unrestricted. Units are radians.
-Detailed_Output
rectan is the array containing the rectangular coordinates of
the point. The output units associated with `rectan'
are those associated with the input `range.'
-Parameters
None.
-Exceptions
Error free.
-Files
None.
-Particulars
None.
-Examples
The following code fragment converts right ascension and
declination from the B1950 reference frame to the J2000 frame.
#include "SpiceUsr.h"
SpiceDouble ra;
SpiceDouble dec;
SpiceDouble r;
SpiceDouble rotab [ 3 ][ 3 ];
SpiceDouble oldvec [ 3 ];
SpiceDouble newvec [ 3 ];
radrec_c ( 1.0, ra, dec, oldvec );
pxform_c ( "B1950", "J2000", 0.0, rotab );
mxv_c ( rotab, oldvec, newvec );
recrad_c ( newvec, &r, &ra, &dec );
-Restrictions
None.
-Author_and_Institution
N.J. Bachman (JPL)
H.A. Neilan (JPL)
E.D. Wright (JPL)
-Literature_References
"Celestial Mechanics, A Computational Guide for the Practitioner"
by Laurence G. Taff
-Version
-CSPICE Version 1.0.2, 28-JUL-2003 (NJB)
Various header corrections were made.
-CSPICE Version 1.0.1, 13-APR-2000 (NJB)
Made some minor updates and corrections in the code example.
-CSPICE Version 1.0.0, 08-FEB-1998 (EDW)
-Index_Entries
range ra and dec to rectangular coordinates
right_ascension and declination to rectangular
-&
*/
{ /* Begin radrec_c */
/*
There isn't much to say or do...
*/
latrec_c ( range, ra, dec, rectan );
} /* End radrec_c */
/** Compute undistorted focal plane coordinate from ground position
*
* @param lat Planetocentric latitude in degrees
* @param lon Planetocentric longitude in degrees
*
* @return @b bool Indicates whether the conversion was successful
*
* @internal
* @history 2007-04-18 Tracie Sucharski - Added check for reasonable
* match when attempting to find closest
* lat/lon in map arrays.
* @history 2007-09-14 Tracie Sucharski - Added check for longitude
* outside min/max bounds. Don't know why
* this wasn't put in before (lat check was
* in), was it oversight, or did I take it out
* for some reason???
* @history 2007-12-14 Tracie Sucharski - Remove resolution test, too
* image dependent and the resolution for vims is
* incorrect due to the instrument having
* rectangular pixels.
* @history 2008-01-02 Tracie Sucharski - Check validity of resulting
* sample and line against edge of starting
* ending pixels (0.5/Parent+0.5) instead of
* center of pixels.
* @history 2012-12-03 Tracie Sucharski - Check for valid minLat/maxLat, minLon/maxLon. If
* none are valid, this means the latMap and lonMap have no valid
* data, therefore we cannot back project, so return false.
*
*/
bool VimsGroundMap::SetGround(const Latitude &lat, const Longitude &lon) {
QVector3D xyz;
if (p_camera->target()->shape()->name() == "Plane") {
double radius = lat.degrees();
if(radius <= 0.0)
return false;
double xCheck = radius * 0.001 * cos(lon.radians());
double yCheck = radius * 0.001 * sin(lon.radians());
xyz.setX(xCheck);
xyz.setY(yCheck);
xyz.setZ(0.);
}
else {
// Convert lat/lon to x/y/z
Distance radius = p_camera->LocalRadius(lat, lon);
SpiceDouble pB[3];
latrec_c(radius.kilometers(), lon.radians(), lat.radians(), pB);
xyz.setX(pB[0]);
xyz.setY(pB[1]);
xyz.setZ(pB[2]);
}
double minDist = DBL_MAX;
int minSamp = -1;
int minLine = -1;
// Find closest points ??? what tolerance ???
for (int line = 0; line < p_camera->ParentLines(); line++) {
for (int samp = 0; samp < p_camera->ParentSamples(); samp++) {
if (p_xyzMap[line][samp].isNull()) continue;
// Subtract map from coordinate then get length
QVector3D deltaXyz = xyz - p_xyzMap[line][samp];
if (deltaXyz.length() < minDist) {
minDist = deltaXyz.length();
minSamp = samp;
minLine = line;
}
}
}
//-----------------------------------------------------------------
// If dist is less than some ??? tolerance ??? this is the
// closest point. Use this point and surrounding 8 pts as
// control pts.
//----------------------------------------------------------------
if (minDist >= DBL_MAX) return false;
//-------------------------------------------------------------
// Set-up for LU decomposition (least2 fit).
// Assume we will have 9 control points, this may not be true
// and will need to be adjusted before the final solution.
//-------------------------------------------------------------
BasisFunction sampXyzBasis("Sample", 4, 4);
BasisFunction lineXyzBasis("Line", 4, 4);
LeastSquares sampXyzLsq(sampXyzBasis);
LeastSquares lineXyzLsq(lineXyzBasis);
vector<double> knownXyz(4);
// Solve using x/y/z
for (int line = minLine - 1; line < minLine + 2; line++) {
if (line < 0 || line > p_camera->ParentLines() - 1) continue;
for (int samp = minSamp - 1; samp < minSamp + 2; samp++) {
// Check for edges
if (samp < 0 || samp > p_camera->ParentSamples() - 1) continue;
if (p_xyzMap[line][samp].isNull()) continue;
knownXyz[0] = p_xyzMap[line][samp].x();
knownXyz[1] = p_xyzMap[line][samp].y();
knownXyz[2] = p_xyzMap[line][samp].z();
knownXyz[3] = 1;
sampXyzLsq.AddKnown(knownXyz, samp + 1);
lineXyzLsq.AddKnown(knownXyz, line + 1);
}
}
if (sampXyzLsq.Knowns() < 4) return false;
sampXyzLsq.Solve();
lineXyzLsq.Solve();
// Solve for sample, line position corresponding to input lat, lon
knownXyz[0] = xyz.x();
knownXyz[1] = xyz.y();
knownXyz[2] = xyz.z();
knownXyz[3] = 1;
double inSamp = sampXyzLsq.Evaluate(knownXyz);
double inLine = lineXyzLsq.Evaluate(knownXyz);
if (inSamp < 0.5 || inSamp > p_camera->ParentSamples() + 0.5 ||
inLine < 0.5 || inLine > p_camera->ParentLines() + 0.5) {
return false;
}
p_camera->IgnoreProjection(true);
p_camera->SetImage(inSamp, inLine);
p_camera->IgnoreProjection(false);
if (!p_camera->HasSurfaceIntersection()) return false;
p_focalPlaneX = inSamp;
p_focalPlaneY = inLine;
return true;
}
