/** 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;
}
void psv2pl_c ( ConstSpiceDouble point[3],
ConstSpiceDouble span1[3],
ConstSpiceDouble span2[3],
SpicePlane * plane )
/*
-Brief_I/O
Variable I/O Description
-------- --- --------------------------------------------------
point,
span1,
span2 I A point and two spanning vectors defining a plane.
plane O A CSPICE plane representing the plane.
-Detailed_Input
point,
span1,
span2 are, respectively, a point and two spanning vectors
that define a geometric plane in three-dimensional
space. The plane is the set of vectors
point + s * span1 + t * span2
where s and t are real numbers. The spanning
vectors span1 and span2 must be linearly
independent, but they need not be orthogonal or
unitized.
-Detailed_Output
plane is a CSPICE plane that represents the geometric
plane defined by point, span1, and span2.
-Parameters
None.
-Exceptions
1) If span1 and span2 are linearly dependent, then the vectors
point, span1, and span2 do not define a plane. The error
SPICE(DEGENERATECASE) is signaled.
-Files
None.
-Particulars
CSPICE geometry routines that deal with planes use the `plane'
data type to represent input and output planes. This data type
makes the subroutine interfaces simpler and more uniform.
The CSPICE routines that produce CSPICE planes from data that
define a plane are:
nvc2pl_c ( Normal vector and constant to plane )
nvp2pl_c ( Normal vector and point to plane )
psv2pl_c ( Point and spanning vectors to plane )
The CSPICE routines that convert CSPICE planes to data that
define a plane are:
pl2nvc_c ( Plane to normal vector and constant )
pl2nvp_c ( Plane to normal vector and point )
pl2psv_c ( Plane to point and spanning vectors )
Any of these last three routines may be used to convert this
routine's output, plane, to another representation of a
geometric plane.
-Examples
1) Project a vector v orthogonally onto a plane defined by
point, span1, and span2. proj is the projection we want; it
is the closest vector in the plane to v.
psv2pl_c ( point, span1, span2, &plane );
vprjp_c ( v, &plane, proj );
2) Find the plane determined by a spacecraft's position vector
relative to a central body and the spacecraft's velocity
vector. We assume that all vectors are given in the same
coordinate system.
/.
pos is the spacecraft's position, relative to
the central body. vel is the spacecraft's velocity
vector. pos is a point (vector, if you like) in
the orbit plane, and it is also one of the spanning
vectors of the plane.
./
psv2pl_c ( pos, pos, vel, &plane );
-Restrictions
None.
-Literature_References
[1] `Calculus and Analytic Geometry', Thomas and Finney.
-Author_and_Institution
N.J. Bachman (JPL)
-Version
-CSPICE Version 1.0.0, 05-MAR-1999 (NJB)
-Index_Entries
point and spanning vectors to plane
-&
*/
{ /* Begin psv2pl_c */
/*
This routine checks in only if an error is discovered.
*/
if ( return_c () )
{
return;
}
/*
Find the unitized cross product of SPAN1 and SPAN2; this is our
unit normal vector, or possibly its inverse.
*/
ucrss_c ( span1, span2, plane->normal );
if ( vzero_c ( plane->normal ) )
{
chkin_c ( "psv2pl_c" );
setmsg_c ( "Spanning vectors are parallel." );
sigerr_c ( "SPICE(DEGENERATECASE)" );
chkout_c ( "psv2pl_c" );
return;
}
/*
Find the plane constant corresponding to the unit normal
vector we've found.
*/
plane->constant = vdot_c ( plane->normal, point );
/*
The constant should be the distance of the plane from the
origin. If the constant is negative, negate both it and the
normal vector.
*/
if ( plane->constant < 0. )
{
plane->constant = - (plane->constant);
vminus_c ( plane->normal, plane->normal );
}
} /* End psv2pl_c */
void npedln_c ( SpiceDouble a,
SpiceDouble b,
SpiceDouble c,
ConstSpiceDouble linept[3],
ConstSpiceDouble linedr[3],
SpiceDouble pnear[3],
SpiceDouble * dist )
/*
-Brief_I/O
Variable I/O Description
-------- --- --------------------------------------------------
a I Length of ellipsoid's semi-axis in the x direction
b I Length of ellipsoid's semi-axis in the y direction
c I Length of ellipsoid's semi-axis in the z direction
linept I Point on line
linedr I Direction vector of line
pnear O Nearest point on ellipsoid to line
dist O Distance of ellipsoid from line
-Detailed_Input
a,
b,
c are the lengths of the semi-axes of a triaxial
ellipsoid which is centered at the origin and
oriented so that its axes lie on the x-, y- and
z- coordinate axes. a, b, and c are the lengths of
the semi-axes that point in the x, y, and z
directions respectively.
linept
linedr are, respectively, a point and a direction vector
that define a line. The line is the set of vectors
linept + t * linedr
where t is any real number.
-Detailed_Output
pnear is the point on the ellipsoid that is closest to
the line, if the line doesn't intersect the
ellipsoid.
If the line intersects the ellipsoid, pnear will
be a point of intersection. If linept is outside
of the ellipsoid, pnear will be the closest point
of intersection. If linept is inside the
ellipsoid, pnear will not necessarily be the
closest point of intersection.
dist is the distance of the line from the ellipsoid.
This is the minimum distance between any point on
the line and any point on the ellipsoid.
If the line intersects the ellipsoid, dist is zero.
-Parameters
None.
-Exceptions
If this routine detects an error, the output arguments nearp and
dist are not modified.
1) If the length of any semi-axis of the ellipsoid is
non-positive, the error SPICE(INVALIDAXISLENGTH) is signaled.
2) If the line's direction vector is the zero vector, the error
SPICE(ZEROVECTOR) is signaled.
3) If the length of any semi-axis of the ellipsoid is zero after
the semi-axis lengths are scaled by the reciprocal of the
magnitude of the longest semi-axis and then squared, the error
SPICE(DEGENERATECASE) is signaled.
4) If the input ellipsoid is extremely flat or needle-shaped
and has its shortest axis close to perpendicular to the input
line, numerical problems could cause this routine's algorithm
to fail, in which case the error SPICE(DEGENERATECASE) is
signaled.
-Files
None.
-Particulars
For any ellipsoid and line, if the line does not intersect the
ellipsoid, there is a unique point on the ellipsoid that is
closest to the line. Therefore, the distance dist between
ellipsoid and line is well-defined. The unique line segment of
length dist that connects the line and ellipsoid is normal to
both of these objects at its endpoints.
If the line intersects the ellipsoid, the distance between the
line and ellipsoid is zero.
-Examples
1) We can find the distance between an instrument optic axis ray
and the surface of a body modelled as a tri-axial ellipsoid
using this routine. If the instrument position and pointing
unit vector in body-fixed coordinates are
linept = ( 1.0e6, 2.0e6, 3.0e6 )
and
linedr = ( -4.472091234e-1
-8.944182469e-1,
-4.472091234e-3 )
and the body semi-axes lengths are
a = 7.0e5
b = 7.0e5
c = 6.0e5,
then the call to npedln_c
npedln_c ( a, b, c, linept, linedr, pnear, &dist );
yields a value for pnear, the nearest point on the body to
the optic axis ray, of
( -.16333110792340931E+04,
-.32666222157820771E+04,
.59999183350006724E+06 )
and a value for dist, the distance to the ray, of
.23899679338299707E+06
(These results were obtained on a PC-Linux system under gcc.)
In some cases, it may not be clear that the closest point
on the line containing an instrument boresight ray is on
the boresight ray itself; the point may lie on the ray
having the same vertex as the boresight ray and pointing in
the opposite direction. To rule out this possibility, we
can make the following test:
/.
Find the difference vector between the closest point
on the ellipsoid to the line containing the boresight
ray and the boresight ray's vertex. Find the
angular separation between this difference vector
and the boresight ray. If the angular separation
does not exceed pi/2, we have the nominal geometry.
Otherwise, we have an error.
./
vsub_c ( pnear, linept, diff );
sep = vsep_c ( diff, linedr );
if ( sep <= halfpi_c() )
{
[ perform normal processing ]
}
else
{
[ handle error case ]
}
-Restrictions
None.
-Literature_References
None.
-Author_and_Institution
N.J. Bachman (JPL)
-Version
-CSPICE Version 1.1.0, 01-JUN-2010 (NJB)
Added touchd_ calls to tests for squared, scaled axis length
underflow. This forces rounding to zero in certain cases where
it otherwise might not occur due to use of extended registers.
-CSPICE Version 1.0.1, 06-DEC-2002 (NJB)
Outputs shown in header example have been corrected to
be consistent with those produced by this routine.
-CSPICE Version 1.0.0, 03-SEP-1999 (NJB)
-Index_Entries
distance between line and ellipsoid
distance between line of sight and body
nearest point on ellipsoid to line
-&
*/
{ /* Begin npedln_c */
/*
Local variables
*/
SpiceBoolean found [2];
SpiceBoolean ifound;
SpiceBoolean xfound;
SpiceDouble oppdir [3];
SpiceDouble mag;
SpiceDouble normal [3];
SpiceDouble prjpt [3];
SpiceDouble prjnpt [3];
SpiceDouble pt [2][3];
SpiceDouble scale;
SpiceDouble scla;
SpiceDouble scla2;
SpiceDouble sclb;
SpiceDouble sclb2;
SpiceDouble sclc;
SpiceDouble sclc2;
SpiceDouble sclpt [3];
SpiceDouble udir [3];
SpiceEllipse cand;
SpiceEllipse prjel;
SpiceInt i;
SpicePlane candpl;
SpicePlane prjpl;
/*
Static variables
*/
/*
Participate in error tracing.
*/
chkin_c ( "npedln_c" );
/*
The algorithm used in this routine has two parts. The first
part handles the case where the input line and ellipsoid
intersect. Our procedure is simple in that case; we just
call surfpt_c twice, passing it first one ray determined by the
input line, then a ray pointing in the opposite direction.
The second part of the algorithm handles the case where surfpt_c
doesn't find an intersection.
Finding the nearest point on the ellipsoid to the line, when the
two do not intersect, is a matter of following four steps:
1) Find the points on the ellipsoid where the surface normal
is normal to the line's direction. This set of points is
an ellipse centered at the origin. The point we seek MUST
lie on this `candidate' ellipse.
2) Project the candidate ellipse onto a plane that is normal
to the line's direction. This projection preserves
distance from the line; the nearest point to the line on
this new ellipse is the projection of the nearest point to
the line on the candidate ellipse, and these two points are
exactly the same distance from the line. If computed using
infinite-precision arithmetic, this projection would be
guaranteed to be non-degenerate as long as the input
ellipsoid were non-degenerate. This can be verified by
taking the inner product of the scaled normal to the candidate
ellipse plane and the line's unitized direction vector
(these vectors are called normal and udir in the code below);
the inner product is strictly greater than 1 if the ellipsoid
is non-degenerate.
3) The nearest point on the line to the projected ellipse will
be contained in the plane onto which the projection is done;
we find this point and then find the nearest point to it on
the projected ellipse. The distance between these two points
is the distance between the line and the ellipsoid.
4) Finally, we find the point on the candidate ellipse that was
projected to the nearest point to the line on the projected
ellipse that was found in step 3. This is the nearest point
on the ellipsoid to the line.
Glossary of Geometric Variables
a,
b,
c Input ellipsoid's semi-axis lengths.
point Point of intersection of line and ellipsoid
if the intersection is non-empty.
candpl Plane containing candidate ellipse.
normal Normal vector to the candidate plane candpl.
cand Candidate ellipse.
linept,
linedr, Point and direction vector on input line.
udir Unitized line direction vector.
prjpl Projection plane; the candidate ellipse is
projected onto this plane to yield prjel.
prjel Projection of the candidate ellipse cand onto
the projection plane prjel.
prjpt Projection of line point.
prjnpt Nearest point on projected ellipse to
projection of line point.
pnear Nearest point on ellipsoid to line.
*/
/*
We need a valid normal vector.
*/
unorm_c ( linedr, udir, &mag );
if ( mag == 0. )
{
setmsg_c( "Line direction vector is the zero vector. " );
sigerr_c( "SPICE(ZEROVECTOR)" );
chkout_c( "npedln_c" );
return;
}
if ( ( a <= 0. )
|| ( b <= 0. )
|| ( c <= 0. ) )
{
setmsg_c ( "Semi-axis lengths: a = #, b = #, c = #." );
errdp_c ( "#", a );
errdp_c ( "#", b );
errdp_c ( "#", c );
sigerr_c ( "SPICE(INVALIDAXISLENGTH)" );
chkout_c ( "npedln_c" );
return;
}
/*
Scale the semi-axes lengths for better numerical behavior.
If squaring any one of the scaled lengths causes it to
underflow to zero, we cannot continue the computation. Otherwise,
scale the viewing point too.
*/
scale = maxd_c ( 3, a, b, c );
scla = a / scale;
sclb = b / scale;
sclc = c / scale;
scla2 = scla*scla;
sclb2 = sclb*sclb;
sclc2 = sclc*sclc;
if ( ( (SpiceDouble)touchd_(&scla2) == 0. )
|| ( (SpiceDouble)touchd_(&sclb2) == 0. )
|| ( (SpiceDouble)touchd_(&sclc2) == 0. ) )
{
setmsg_c ( "Semi-axis too small: a = #, b = #, c = #. " );
errdp_c ( "#", a );
errdp_c ( "#", b );
errdp_c ( "#", c );
sigerr_c ( "SPICE(DEGENERATECASE)" );
chkout_c ( "npedln_c" );
return;
}
/*
Scale linept.
*/
sclpt[0] = linept[0] / scale;
sclpt[1] = linept[1] / scale;
sclpt[2] = linept[2] / scale;
/*
Hand off the intersection case to surfpt_c. surfpt_c determines
whether rays intersect a body, so we treat the line as a pair
of rays.
*/
vminus_c ( udir, oppdir );
surfpt_c ( sclpt, udir, scla, sclb, sclc, pt[0], &(found[0]) );
surfpt_c ( sclpt, oppdir, scla, sclb, sclc, pt[1], &(found[1]) );
for ( i = 0; i < 2; i++ )
{
if ( found[i] )
{
*dist = 0.0;
vequ_c ( pt[i], pnear );
vscl_c ( scale, pnear, pnear );
chkout_c ( "npedln_c" );
return;
}
}
/*
Getting here means the line doesn't intersect the ellipsoid.
Find the candidate ellipse CAND. NORMAL is a normal vector to
the plane containing the candidate ellipse. Mathematically the
ellipse must exist, since it's the intersection of an ellipsoid
centered at the origin and a plane containing the origin. Only
numerical problems can prevent the intersection from being found.
*/
normal[0] = udir[0] / scla2;
normal[1] = udir[1] / sclb2;
normal[2] = udir[2] / sclc2;
nvc2pl_c ( normal, 0., &candpl );
inedpl_c ( scla, sclb, sclc, &candpl, &cand, &xfound );
if ( !xfound )
{
setmsg_c ( "Candidate ellipse could not be found." );
sigerr_c ( "SPICE(DEGENERATECASE)" );
chkout_c ( "npedln_c" );
return;
}
/*
Project the candidate ellipse onto a plane orthogonal to the
line. We'll call the plane prjpl and the projected ellipse prjel.
*/
nvc2pl_c ( udir, 0., &prjpl );
pjelpl_c ( &cand, &prjpl, &prjel );
/*
Find the point on the line lying in the projection plane, and
then find the near point PRJNPT on the projected ellipse. Here
PRJPT is the point on the line lying in the projection plane.
The distance between PRJPT and PRJNPT is DIST.
*/
vprjp_c ( sclpt, &prjpl, prjpt );
npelpt_c ( prjpt, &prjel, prjnpt, dist );
/*
Find the near point pnear on the ellipsoid by taking the inverse
orthogonal projection of prjnpt; this is the point on the
candidate ellipse that projects to prjnpt. Note that the
output dist was computed in step 3 and needs only to be re-scaled.
The inverse projection of pnear ought to exist, but may not
be calculable due to numerical problems (this can only happen
when the input ellipsoid is extremely flat or needle-shaped).
*/
vprjpi_c ( prjnpt, &prjpl, &candpl, pnear, &ifound );
if ( !ifound )
{
setmsg_c ( "Inverse projection could not be found." );
sigerr_c ( "SPICE(DEGENERATECASE)" );
chkout_c ( "npedln_c" );
return;
}
/*
Undo the scaling.
*/
vscl_c ( scale, pnear, pnear );
*dist *= scale;
chkout_c ( "npedln_c" );
} /* End npedln_c */