double RadarGroundMap::ComputeXv(SpiceDouble X[3]) {
// Get the spacecraft position (Xsc) and velocity (Vsc) in body fixed
// coordinates
SpiceRotation *bodyFrame = p_camera->BodyRotation();
SpicePosition *spaceCraft = p_camera->InstrumentPosition();
std::vector<double> Ssc(6);
// Load the state into Ssc
vequ_c ( (SpiceDouble *) &(spaceCraft->Coordinate()[0]), &Ssc[0]);
vequ_c ( (SpiceDouble *) &(spaceCraft->Velocity()[0]), &Ssc[3]);
// Rotate the state to body-fixed
std::vector<double> bfSsc(6);
bfSsc = bodyFrame->ReferenceVector(Ssc);
// Extract the body-fixed position and velocity
std::vector<double> Vsc(3);
std::vector<double> Xsc(3);
vequ_c ( &bfSsc[0], &Xsc[0] );
vequ_c ( &bfSsc[3], &Vsc[0] );
// Compute the slant range
SpiceDouble lookB[3];
vsub_c(&Xsc[0],X,lookB);
p_slantRange = vnorm_c(lookB);
// Compute and return xv
double xv = -2.0 * vdot_c(lookB,&Vsc[0]) / (vnorm_c(lookB) * p_waveLength);
return xv;
}
/** 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;
}
/**
* Computes and returns emission angle in degrees given the observer position.
*
* Emission Angle: The angle between the surface normal vector at the
* intersection point and a vector from the intersection point to the
* spacecraft. The emission angle varies from 0 degrees when the spacecraft is
* viewing the sub-spacecraft point (nadir viewing) to 90 degrees when the
* intercept is tangent to the surface of the target body. Thus, higher values
* of emission angle indicate more oblique viewing of the target.
*
* @param sB: Spacecraft position in body-fixed coordinates
*
* @return Emmision angle in decimal degrees
*
*/
double PlaneShape::emissionAngle(const std::vector<double> & sB) {
SpiceDouble pB[3]; // surface intersection in body-fixed coordinates
SpiceDouble psB[3]; // vector from spacecraft to surface intersection
SpiceDouble upsB[3]; // unit vector from spacecraft to surface intersection
SpiceDouble dist; // vector magnitude
// Get vector from center of body to surface point
pB[0] = surfaceIntersection()->GetX().kilometers();
pB[1] = surfaceIntersection()->GetY().kilometers();
pB[2] = surfaceIntersection()->GetZ().kilometers();
// Get vector from surface intersect point to observer and normalize it
vsub_c((ConstSpiceDouble *) &sB[0], pB, psB);
unorm_c(psB, upsB, &dist);
// temporary normal vector
SpiceDouble n[3];
n[0] = 0.0;
n[1] = 0.0;
n[2] = 1.0;
// flip normal if observer is "below" the plane, assuming that the target
// body north pole defines the "up" direction
if (sB[2] < 0.0)
n[2] = -n[2];
// dot product of surface normal and observer-surface intersection vector
double angle = vdot_c(n, upsB);
if (angle > 1.0)
return 0.0;
if (angle < -1.0)
return 180.0;
return acos(angle) * RAD2DEG;
}
/**
* Computes and returns incidence angle in degrees given the sun position.
*
* Incidence Angle: The incidence angle provides a measure of the lighting
* condition at the surface intersection point. The angle between the surface
* normal vector at the intersection point and a vector from the intersection
* point to the sun. The incidence angle varies from 0 degrees when the
* intersection point coincides with the sub-solar point to 90 degrees when
* the intersection point is at the terminator (i.e., in the shadowed or dark
* portion of the target body). Thus, higher values of incidence angles
* indicate the existence of a greater number of surface shadows.
*
* @param uB: Sun position in body-fixed coordinates
*
* @return Incidence angle in decimal degrees
*
*/
double PlaneShape::incidenceAngle(const std::vector<double> &uB) {
SpiceDouble pB[3]; // surface intersection in body-fixed coordinates
SpiceDouble puB[3]; // vector from sun to surface intersection
SpiceDouble upuB[3]; // unit vector from sun to surface intersection
SpiceDouble dist; // vector magnitude
// Get vector from center of body to surface point
pB[0] = surfaceIntersection()->GetX().kilometers();
pB[1] = surfaceIntersection()->GetY().kilometers();
pB[2] = surfaceIntersection()->GetZ().kilometers();
// Get vector from surface intersect point to sun and normalize it
vsub_c((SpiceDouble *) &uB[0], pB, puB);
unorm_c(puB, upuB, &dist);
// temporary normal vector
SpiceDouble n[3];
n[0] = 0.0;
n[1] = 0.0;
n[2] = 1.0;
// flip normal if sun is "below" the plane, assuming that the target
// body north pole defines the "up" direction
if (uB[2] < 0.0)
n[2] = -n[2];
double angle = vdot_c((SpiceDouble *) &n[0], upuB);
if (angle > 1.0)
return 0.0;
if(angle < -1.0)
return 180.0;
return acos(angle) * RAD2DEG;
}
/** Compute ground position from slant range
*
* @param ux Slant range distance
* @param uy Doppler shift (always 0.0)
* @param uz Not used
*
* @return conversion was successful
*/
bool RadarGroundMap::SetFocalPlane(const double ux, const double uy,
double uz) {
SpiceRotation *bodyFrame = p_camera->BodyRotation();
SpicePosition *spaceCraft = p_camera->InstrumentPosition();
// Get spacecraft position and velocity to create a state vector
std::vector<double> Ssc(6);
// Load the state into Ssc
vequ_c ( (SpiceDouble *) &(spaceCraft->Coordinate()[0]), &Ssc[0]);
vequ_c ( (SpiceDouble *) &(spaceCraft->Velocity()[0]), &Ssc[3]);
// Rotate state vector to body-fixed
std::vector<double> bfSsc(6);
bfSsc = bodyFrame->ReferenceVector(Ssc);
// Extract body-fixed position and velocity
std::vector<double> Vsc(3);
std::vector<double> Xsc(3);
vequ_c ( &bfSsc[0], (SpiceDouble *) &(Xsc[0]) );
vequ_c ( &bfSsc[3], (SpiceDouble *) &(Vsc[0]) );
// Compute intrack, crosstrack, and radial coordinate
SpiceDouble i[3];
vhat_c (&Vsc[0],i);
SpiceDouble c[3];
SpiceDouble dp;
dp = vdot_c(&Xsc[0],i);
SpiceDouble p[3],q[3];
vscl_c(dp,i,p);
vsub_c(&Xsc[0],p,q);
vhat_c(q,c);
SpiceDouble r[3];
vcrss_c(i,c,r);
// What is the initial guess for R
double radii[3];
p_camera->Radii(radii);
SpiceDouble R = radii[0];
SpiceDouble lastR = DBL_MAX;
SpiceDouble rlat;
SpiceDouble rlon;
SpiceDouble lat = DBL_MAX;
SpiceDouble lon = DBL_MAX;
double slantRangeSqr = (ux * p_rangeSigma) / 1000.;
slantRangeSqr = slantRangeSqr*slantRangeSqr;
SpiceDouble X[3];
int iter = 0;
do {
double normXsc = vnorm_c(&Xsc[0]);
double alpha = (R*R - slantRangeSqr - normXsc*normXsc) /
(2.0 * vdot_c(&Xsc[0],c));
double arg = slantRangeSqr - alpha*alpha;
if (arg < 0.0) return false;
double beta = sqrt(arg);
if (p_lookDirection == Radar::Left) beta *= -1.0;
SpiceDouble alphac[3],betar[3];
vscl_c(alpha,c,alphac);
vscl_c(beta,r,betar);
vadd_c(alphac,betar,alphac);
vadd_c(&Xsc[0],alphac,X);
// Convert X to lat,lon
lastR = R;
reclat_c(X,&R,&lon,&lat);
rlat = lat*180.0/Isis::PI;
rlon = lon*180.0/Isis::PI;
R = GetRadius(rlat,rlon);
iter++;
}
while (fabs(R-lastR) > p_tolerance && iter < 30);
if (fabs(R-lastR) > p_tolerance) return false;
lat = lat*180.0/Isis::PI;
lon = lon*180.0/Isis::PI;
while (lon < 0.0) lon += 360.0;
// 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);
return p_camera->Sensor::SetUniversalGround(lat,lon);
}
/** 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 vprjp_c ( ConstSpiceDouble vin [3],
ConstSpicePlane * plane,
SpiceDouble vout [3] )
/*
-Brief_I/O
Variable I/O Description
-------- --- --------------------------------------------------
vin I Vector to be projected.
plane I A CSPICE plane onto which vin is projected.
vout O Vector resulting from projection.
-Detailed_Input
vin is a 3-vector that is to be orthogonally projected
onto a specified plane.
plane is a CSPICE plane that represents the geometric
plane onto which vin is to be projected.
-Detailed_Output
vout is the vector resulting from the orthogonal
projection of vin onto plane. vout is the closest
point in the specified plane to vin.
-Parameters
None.
-Exceptions
1) Invalid input planes are diagnosed by the routine pl2nvc_c,
which is called by this routine.
-Files
None.
-Particulars
Projecting a vector v orthogonally onto a plane can be thought of
as finding the closest vector in the plane to v. This `closest
vector' always exists; it may be coincident with the original
vector.
Two related routines are vprjpi_c, which inverts an orthogonal
projection of a vector onto a plane, and vproj_c, which projects
a vector orthogonally onto another vector.
-Examples
1) Find the closest point in the ring plane of a planet to a
spacecraft located at positn (in body-fixed coordinates).
Suppose the vector normal is normal to the ring plane, and
that origin, which represents the body center, is in the
ring plane. Then we can make a `plane' with the code
pnv2pl_c ( origin, normal, &plane );
can find the projection by making the call
vprjp_c ( positn, &plane, proj );
-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
vector projection onto plane
-&
*/
{ /* Begin vprjp_c */
/*
Local variables
*/
SpiceDouble constant;
SpiceDouble normal [3];
/*
Participate in error tracing.
*/
if ( return_c() )
{
return;
}
chkin_c ( "vprjp_c" );
/*
Obtain a unit vector normal to the input plane, and a constant
for the plane.
*/
pl2nvc_c ( plane, normal, &constant );
/*
Let the notation < a, b > indicate the inner product of vectors
a and b.
vin differs from its projection onto plane by some multiple of
normal. That multiple is
< vin - vout, normal > * normal
= ( < vin, normal > - < vout, normal > ) * normal
= ( < vin, normal > - const ) * normal
Subtracting this multiple of normal from vin yields vout.
*/
vlcom_c ( 1.0,
vin,
constant - vdot_c ( vin, normal ),
normal,
vout );
chkout_c ( "vprjp_c" );
} /* End vprjp_c */
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 vprjpi_c ( ConstSpiceDouble vin [3],
ConstSpicePlane * projpl,
ConstSpicePlane * invpl,
SpiceDouble vout [3],
SpiceBoolean * found )
/*
-Brief_I/O
Variable I/O Description
-------- --- --------------------------------------------------
vin I The projected vector.
projpl I Plane containing vin.
invpl I Plane containing inverse image of vin.
vout O Inverse projection of vin.
found O Flag indicating whether vout could be calculated.
-Detailed_Input
vin,
projpl,
invpl are, respectively, a 3-vector, a CSPICE plane
containing the vector, and a CSPICE plane
containing the inverse image of the vector under
orthogonal projection onto projpl.
-Detailed_Output
vout is the inverse orthogonal projection of vin. This
is the vector lying in the plane invpl whose
orthogonal projection onto the plane projpl is
vin. vout is valid only when found (defined below)
is SPICETRUE. Otherwise, vout is undefined.
found indicates whether the inverse orthogonal projection
of vin could be computed. found is SPICETRUE if so,
SPICEFALSE otherwise.
-Parameters
None.
-Exceptions
1) If the geometric planes defined by projpl and invpl are
orthogonal, or nearly so, the inverse orthogonal projection
of vin may be undefined or have magnitude too large to
represent with double precision numbers. In either such
case, found will be set to SPICEFALSE.
2) Even when found is SPICETRUE, vout may be a vector of extremely
large magnitude, perhaps so large that it is impractical to
compute with it. It's up to you to make sure that this
situation does not occur in your application of this routine.
-Files
None.
-Particulars
Projecting a vector orthogonally onto a plane can be thought of
as finding the closest vector in the plane to the original vector.
This `closest vector' always exists; it may be coincident with the
original vector. Inverting an orthogonal projection means finding
the vector in a specified plane whose orthogonal projection onto
a second specified plane is a specified vector. The vector whose
projection is the specified vector is the inverse projection of
the specified vector, also called the `inverse image under
orthogonal projection' of the specified vector. This routine
finds the inverse orthogonal projection of a vector onto a plane.
Related routines are vprjp_c, which projects a vector onto a plane
orthogonally, and vproj_c, which projects a vector onto another
vector orthogonally.
-Examples
1) Suppose
vin = ( 0.0, 1.0, 0.0 ),
and that projpl has normal vector
projn = ( 0.0, 0.0, 1.0 ).
Also, let's suppose that invpl has normal vector and constant
invn = ( 0.0, 2.0, 2.0 )
invc = 4.0.
Then vin lies on the y-axis in the x-y plane, and we want to
find the vector vout lying in invpl such that the orthogonal
projection of vout the x-y plane is vin. Let the notation
< a, b > indicate the inner product of vectors a and b.
Since every point x in invpl satisfies the equation
< x, (0.0, 2.0, 2.0) > = 4.0,
we can verify by inspection that the vector
( 0.0, 1.0, 1.0 )
is in invpl and differs from vin by a multiple of projn. So
( 0.0, 1.0, 1.0 )
must be vout.
To find this result using CSPICE, we can create the
CSPICE planes projpl and invpl using the code fragment
nvp2pl_c ( projn, vin, &projpl );
nvc2pl_c ( invn, invc, &invpl );
and then perform the inverse projection using the call
vprjpi_c ( vin, &projpl, &invpl, vout );
vprjpi_c will return the value
vout = ( 0.0, 1.0, 1.0 );
-Restrictions
None.
-Literature_References
[1] `Calculus and Analytic Geometry', Thomas and Finney.
-Author_and_Institution
N.J. Bachman (JPL)
-Version
-CSPICE Version 1.1.0, 05-APR-2004 (NJB)
Computation of LIMIT was re-structured to avoid
run-time underflow warnings on some platforms.
-CSPICE Version 1.0.0, 05-MAR-1999 (NJB)
-Index_Entries
vector projection onto plane inverted
-&
*/
/*
-Revisions
-CSPICE Version 1.1.0, 05-APR-2004 (NJB)
Computation of LIMIT was re-structured to avoid run-time
underflow warnings on some platforms. In the revised code,
BOUND/dpmax_c() is never scaled by a number having absolute value
< 1.
-&
*/
{ /* Begin vprjpi_c */
/*
Local constants
*/
/*
BOUND is used to bound the magnitudes of the numbers that we
try to take the reciprocal of, since we can't necessarily invert
any non-zero number. We won't try to invert any numbers with
magnitude less than
BOUND / dpmax_c()
BOUND is chosen somewhat arbitrarily....
*/
#define BOUND 10.0
/*
Local variables
*/
SpiceDouble denom;
SpiceDouble invc;
SpiceDouble invn [3];
SpiceDouble limit;
SpiceDouble mult;
SpiceDouble numer;
SpiceDouble projc;
SpiceDouble projn [3];
/*
Participate in error tracing.
*/
if ( return_c() )
{
return;
}
chkin_c ( "vprjpi_c" );
/*
Unpack the planes.
*/
pl2nvc_c ( projpl, projn, &projc );
pl2nvc_c ( invpl, invn, &invc );
/*
We'll first discuss the computation of VOUT in the nominal case,
and then deal with the exceptional cases.
When projpl and invpl are not orthogonal to each other, the
inverse projection of vin will differ from vin by a multiple of
projn, the unit normal vector to projpl. We find this multiple
by using the fact that the inverse projection vout satisfies the
plane equation for the inverse projection plane invpl.
We have
vout = vin + mult * projn; (1)
since vout satisfies
< vout, invn > = invc
we must have
< vin + mult * projn, invn > = invc
which in turn implies
invc - < vin, invn >
mult = ------------------------. (2)
< projn, invn >
Having mult, we can compute vout according to equation (1).
Now, if the denominator in the above expression for mult is zero
or just too small, performing the division would cause a
divide-by-zero error or an overflow of mult. In either case, we
will avoid carrying out the division, and we'll set found to
SPICEFALSE.
Compute the numerator and denominator of the right side of (2).
*/
numer = invc - vdot_c ( vin, invn );
denom = vdot_c ( projn, invn );
/*
If the magnitude of the denominator is greater than
BOUND
limit = abs ( ---------- * numer ),
dpmax_c()
we can safely divide the numerator by the denominator, and the
magnitude of the result will be no greater than
dpmax_c()
----------- .
BOUND
Note that we have ruled out the case where numer and denom are
both zero by insisting on strict inequality in the comparison of
denom and limit:
*/
if ( fabs(numer) < 1.0 )
{
limit = fabs ( BOUND / dpmax_c() );
}
else
{
limit = fabs ( ( BOUND / dpmax_c() ) * numer );
}
*found = ( fabs (denom) > limit );
if ( *found )
{
/*
We'll compute vout after all.
*/
mult = numer / denom;
vlcom_c ( 1.0, vin, mult, projn, vout );
}
chkout_c ( "vprjpi_c" );
} /* End vprjpi_c */
void qxq_c ( ConstSpiceDouble q1 [4],
ConstSpiceDouble q2 [4],
SpiceDouble qout [4] )
/*
-Brief_I/O
VARIABLE I/O DESCRIPTION
-------- --- --------------------------------------------------
q1 I First SPICE quaternion factor.
q2 I Second SPICE quaternion factor.
qout O Product of `q1' and `q2'.
-Detailed_Input
q1 is a 4-vector representing a SPICE-style quaternion.
See the discussion of "Quaternion Styles" in the
Particulars section below.
Note that multiple styles of quaternions are in use.
This routine will not work properly if the input
quaternions do not conform to the SPICE convention.
q2 is a second SPICE-style quaternion.
-Detailed_Output
qout is 4-vector representing the quaternion product
q1 * q2
Representing q(i) as the sums of scalar (real)
part s(i) and vector (imaginary) part v(i)
respectively,
q1 = s1 + v1
q2 = s2 + v2
qout has scalar part s3 defined by
s3 = s1 * s2 - <v1, v2>
and vector part v3 defined by
v3 = s1 * v2 + s2 * v1 + v1 x v2
where the notation < , > denotes the inner
product operator and x indicates the cross
product operator.
-Parameters
None.
-Exceptions
Error free.
-Files
None.
-Particulars
Quaternion Styles
-----------------
There are different "styles" of quaternions used in
science and engineering applications. Quaternion styles
are characterized by
- The order of quaternion elements
- The quaternion multiplication formula
- The convention for associating quaternions
with rotation matrices
Two of the commonly used styles are
- "SPICE"
> Invented by Sir William Rowan Hamilton
> Frequently used in mathematics and physics textbooks
- "Engineering"
> Widely used in aerospace engineering applications
CSPICE function interfaces ALWAYS use SPICE quaternions.
Quaternions of any other style must be converted to SPICE
quaternions before they are passed to CSPICE functions.
Relationship between SPICE and Engineering Quaternions
------------------------------------------------------
Let M be a rotation matrix such that for any vector V,
M*V
is the result of rotating V by theta radians in the
counterclockwise direction about unit rotation axis vector A.
Then the SPICE quaternions representing M are
(+/-) ( cos(theta/2),
sin(theta/2) A(1),
sin(theta/2) A(2),
sin(theta/2) A(3) )
while the engineering quaternions representing M are
(+/-) ( -sin(theta/2) A(1),
-sin(theta/2) A(2),
-sin(theta/2) A(3),
cos(theta/2) )
For both styles of quaternions, if a quaternion q represents
a rotation matrix M, then -q represents M as well.
Given an engineering quaternion
QENG = ( q0, q1, q2, q3 )
the equivalent SPICE quaternion is
QSPICE = ( q3, -q0, -q1, -q2 )
Associating SPICE Quaternions with Rotation Matrices
----------------------------------------------------
Let FROM and TO be two right-handed reference frames, for
example, an inertial frame and a spacecraft-fixed frame. Let the
symbols
V , V
FROM TO
denote, respectively, an arbitrary vector expressed relative to
the FROM and TO frames. Let M denote the transformation matrix
that transforms vectors from frame FROM to frame TO; then
V = M * V
TO FROM
where the expression on the right hand side represents left
multiplication of the vector by the matrix.
Then if the unit-length SPICE quaternion q represents M, where
q = (q0, q1, q2, q3)
the elements of M are derived from the elements of q as follows:
+- -+
| 2 2 |
| 1 - 2*( q2 + q3 ) 2*(q1*q2 - q0*q3) 2*(q1*q3 + q0*q2) |
| |
| |
| 2 2 |
M = | 2*(q1*q2 + q0*q3) 1 - 2*( q1 + q3 ) 2*(q2*q3 - q0*q1) |
| |
| |
| 2 2 |
| 2*(q1*q3 - q0*q2) 2*(q2*q3 + q0*q1) 1 - 2*( q1 + q2 ) |
| |
+- -+
Note that substituting the elements of -q for those of q in the
right hand side leaves each element of M unchanged; this shows
that if a quaternion q represents a matrix M, then so does the
quaternion -q.
To map the rotation matrix M to a unit quaternion, we start by
decomposing the rotation matrix as a sum of symmetric
and skew-symmetric parts:
2
M = [ I + (1-cos(theta)) OMEGA ] + [ sin(theta) OMEGA ]
symmetric skew-symmetric
OMEGA is a skew-symmetric matrix of the form
+- -+
| 0 -n3 n2 |
| |
OMEGA = | n3 0 -n1 |
| |
| -n2 n1 0 |
+- -+
The vector N of matrix entries (n1, n2, n3) is the rotation axis
of M and theta is M's rotation angle. Note that N and theta
are not unique.
Let
C = cos(theta/2)
S = sin(theta/2)
Then the unit quaternions Q corresponding to M are
Q = +/- ( C, S*n1, S*n2, S*n3 )
The mappings between quaternions and the corresponding rotations
are carried out by the CSPICE routines
q2m_c {quaternion to matrix}
m2q_c {matrix to quaternion}
m2q_c always returns a quaternion with scalar part greater than
or equal to zero.
SPICE Quaternion Multiplication Formula
---------------------------------------
Given a SPICE quaternion
Q = ( q0, q1, q2, q3 )
corresponding to rotation axis A and angle theta as above, we can
represent Q using "scalar + vector" notation as follows:
s = q0 = cos(theta/2)
v = ( q1, q2, q3 ) = sin(theta/2) * A
Q = s + v
Let Q1 and Q2 be SPICE quaternions with respective scalar
and vector parts s1, s2 and v1, v2:
Q1 = s1 + v1
Q2 = s2 + v2
We represent the dot product of v1 and v2 by
<v1, v2>
and the cross product of v1 and v2 by
v1 x v2
Then the SPICE quaternion product is
Q1*Q2 = s1*s2 - <v1,v2> + s1*v2 + s2*v1 + (v1 x v2)
If Q1 and Q2 represent the rotation matrices M1 and M2
respectively, then the quaternion product
Q1*Q2
represents the matrix product
M1*M2
-Examples
1) Let qid, qi, qj, qk be the "basis" quaternions
qid = ( 1, 0, 0, 0 )
qi = ( 0, 1, 0, 0 )
qj = ( 0, 0, 1, 0 )
qk = ( 0, 0, 0, 1 )
respectively. Then the calls
qxq_c ( qi, qj, ixj );
qxq_c ( qj, qk, jxk );
qxq_c ( qk, qi, kxi );
produce the results
ixj == qk
jxk == qi
kxi == qj
All of the calls
qxq_c ( qi, qi, qout );
qxq_c ( qj, qj, qout );
qxq_c ( qk, qk, qout );
produce the result
qout == -qid
For any quaternion Q, the calls
qxq_c ( qid, q, qout );
qxq_c ( q, qid, qout );
produce the result
qout == q
2) Composition of rotations: let `cmat1' and `cmat2' be two
C-matrices (which are rotation matrices). Then the
following code fragment computes the product cmat1 * cmat2:
/.
Convert the C-matrices to quaternions.
./
m2q_c ( cmat1, q1 );
m2q_c ( cmat2, q2 );
/.
Find the product.
./
qxq_c ( q1, q2, qout );
/.
Convert the result to a C-matrix.
./
q2m_c ( qout, cmat3 );
/.
Multiply `cmat1' and `cmat2' directly.
./
mxm_c ( cmat1, cmat2, cmat4 );
/.
Compare the results. The difference `diff' of
`cmat3' and `cmat4' should be close to the zero
matrix.
./
vsubg_c ( 9, cmat3, cmat4, diff );
-Restrictions
None.
-Literature_References
None.
-Author_and_Institution
N.J. Bachman (JPL)
-Version
-CSPICE Version 1.0.1, 27-FEB-2008 (NJB)
Updated header; added information about SPICE
quaternion conventions.
-CSPICE Version 1.0.0, 27-OCT-2005 (NJB)
-Index_Entries
quaternion times quaternion
multiply quaternion by quaternion
-&
*/
{ /* Begin qxq_c */
/*
Local variables
*/
SpiceDouble cross[3];
/*
This routine is error free.
*/
/*
Assign the scalar portion of the product `vout'.
*/
qout[0] = q1[0]*q2[0] - vdot_c( q1+1, q2+1 );
/*
Compute the cross product term of the vector component of
vout.
*/
vcrss_c ( q1+1, q2+1, cross );
/*
Assign the vector portion of the product `vout'.
*/
vlcom3_c ( q1[0], q2+1,
q2[0], q1+1,
1.0, cross, qout+1 );
} /* End qxq_c */
