USGS

Isis 3.0 Object Programmers' Reference

 Home
qmini.cpp
1 #include <cfloat>
2 #include <cmath>
3 #include <iomanip>
4 
5 #include "Camera.h"
6 
7 using namespace std;
8 using namespace Isis;
9 /* qmini.f -- translated by f2c (version 19980913).
10  You must link the resulting object file with the libraries:
11  -lf2c -lm (in that order)
12 */
13 
14 //#include "f2c.h"
15 
16 /* Table of constant values */
17 
18 static doublereal c_b2 = -1.;
19 static doublereal c_b3 = 1.;
20 
21 /* $Procedure QMINI ( Quaternion linear interpolation ) */
22 /* Subroutine */
23 int qmini(doublereal *init, doublereal *final, doublereal frac, doublereal *qintrp) {
24  /* System generated locals */
25  doublereal d__1;
26 
27  /* Local variables */
28  doublereal vmag, axis[3];
29  doublereal q[4], angle;
30  doublereal qscale[4];
31  doublereal intang, instar[4];
32 
33  /* $ Abstract */
34 
35  /* Interpolate between two quaternions using a constant angular */
36  /* rate. */
37 
38  /* $ Disclaimer */
39 
40  /* THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE */
41  /* CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. */
42  /* GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE */
43  /* ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE */
44  /* PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" */
45  /* TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY */
46  /* WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A */
47  /* PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC */
48  /* SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE */
49  /* SOFTWARE AND RELATED MATERIALS, HOWEVER USED. */
50 
51  /* IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA */
52  /* BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT */
53  /* LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, */
54  /* INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, */
55  /* REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE */
56  /* REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. */
57 
58  /* RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF */
59  /* THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY */
60  /* CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE */
61  /* ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. */
62 
63  /* $ Required_Reading */
64 
65  /* ROTATIONS */
66 
67  /* $ Keywords */
68 
69  /* MATH */
70  /* QUATERNION */
71  /* ROTATION */
72 
73  /* $ Declarations */
74  /* $ Brief_I/O */
75 
76  /* Variable I/O Description */
77  /* -------- --- -------------------------------------------------- */
78  /* INIT I Initial quaternion representing a rotation. */
79  /* FINAL I Final quaternion representing a rotation. */
80  /* FRAC I Fraction of rotation from INIT to FINAL by which */
81  /* to interpolate. */
82  /* QINTRP O Linearly interpolated quaternion. */
83 
84  /* $ Detailed_Input */
85 
86  /* INIT, */
87  /* FINAL, */
88  /* FRAC are, respectively, two unit quaternions between */
89  /* which to interpolate, and an interpolation */
90  /* fraction. See the Detailed_Output and Particulars */
91  /* sections for details. */
92 
93  /* $ Detailed_Output */
94 
95  /* QINTRP is the quaternion resulting from linear */
96  /* interpolation between INIT and FINAL by the */
97  /* fraction FRAC. By "linear interpolation" we mean */
98  /* the following: */
99 
100  /* We view INIT and FINAL as quaternions */
101  /* representing two values of a time-varying */
102  /* rotation quaternion R(t) that rotates at a */
103  /* constant angular velocity (that is, the row */
104  /* vectors of R(t) rotate with constant angular */
105  /* velocity). We can say that */
106 
107  /* INIT represents R(t0) */
108  /* FINAL represents R(t1) */
109 
110  /* Equivalently, the SPICELIB routine Q2M maps */
111  /* INIT and FINAL to rotation matrices */
112  /* corresponding to R(t0) and R(t1) respectively. */
113 
114  /* "Linear interpolation by the fraction FRAC" */
115  /* means that we evaluate R(t) at time */
116 
117  /* t0 + FRAC * (t1 - t0). */
118 
119 
120  /* $ Parameters */
121 
122  /* None. */
123 
124  /* $ Exceptions */
125 
126  /* 1) If either of INIT or FINAL is not a unit quaternion, the error */
127  /* SPICE(NOTAROTATION) is signaled. */
128 
129  /* 2) This routine assumes that the rotation that maps INIT to FINAL */
130  /* has a rotation angle THETA radians, where */
131 
132  /* 0 < THETA < pi. */
133  /* _ */
134 
135  /* This routine cannot distinguish between rotations of THETA */
136  /* radians, where THETA is in the interval [0, pi), and */
137  /* rotations of */
138 
139  /* THETA + 2 * k * pi */
140 
141  /* radians, where k is any integer. These "large" rotations will */
142  /* yield invalid results when interpolated. You must ensure */
143  /* that the inputs you provide to this routine will not be */
144  /* subject to this sort of ambiguity. If in fact you are */
145  /* interpolating a time-depedent rotation with constant angular */
146  /* velocity AV between times t0 and t1, you must ensure that */
147 
148  /* || AV || * |t1 - t0| < pi. */
149 
150  /* Here we assume that the magnitude of AV is the angular rate */
151  /* of the rotation in units of radians per second. */
152 
153 
154  /* 3) When FRAC is outside of the interval [0, 1], the process */
155  /* performed is "extrapolation", not interpolation. Such */
156  /* values of FRAC are permitted. */
157 
158  /* $ Files */
159 
160  /* None. */
161 
162  /* $ Particulars */
163 
164  /* In the discussion below, we assume that the conditions specified */
165  /* in item (2) of the Exceptions section have been satisfied. */
166 
167  /* As we've said, we view INIT and FINAL as quaternions representing */
168  /* two values of a time-varying rotation matrix R(t) that rotates at */
169  /* a constant angular velocity; we define R(t), t0, and t1 so that */
170 
171  /* INIT represents R(t0) */
172  /* FINAL represents R(t1). */
173 
174  /* The output quaternion QINTRP represents R(t) evaluated at the */
175  /* time */
176 
177  /* t0 + FRAC * (t1 - t0). */
178 
179  /* How do we evaluate R at times between t0 and t1? Since the row */
180  /* vectors of R are presumed to rotate with constant angular */
181  /* velocity, we will first find the rotation axis of the quotient */
182  /* rotation Q that maps the row vectors of R from their initial to */
183  /* final position. Since the rows of R are the columns of the */
184  /* transpose of R, we can write: */
185 
186  /* T T */
187  /* R(t1) = Q * R(t0), */
188 
189  /* Since */
190 
191  /* T T T */
192  /* R(t1) = ( R(t1) * R(t0) ) * R(t0) */
193 
194 
195  /* we can find Q, as well as a rotation axis A and an angle THETA */
196  /* in the range [0, pi] such that Q rotates vectors by THETA */
197  /* radians about axis A. */
198 
199  /* We'll use the notation */
200 
201  /* [ x ] */
202  /* N */
203 
204  /* to indicate a coordinate system rotation of x radians about the */
205  /* vector N. Having found A and THETA, we can write (note that */
206  /* the sign of the rotation angle is negated because we're using */
207  /* a coordinate system rotation) */
208 
209  /* T (t - t0) T */
210  /* R(t) = [ - THETA * --------- ] * R(t0) */
211  /* (t1 - t0) A */
212 
213  /* Thus R(t) and QINTRP are determined. */
214 
215  /* The input argument FRAC plays the role of the quotient */
216 
217  /* t - t0 */
218  /* ------- */
219  /* t1 - t0 */
220 
221  /* shown above. */
222 
223 
224  /* $ Examples */
225 
226  /* 1) Suppose we want to interpolate between quaternions */
227  /* Q1 and Q2 that give the orientation of a spacecraft structure */
228  /* at times t1 and t2. We wish to find an approximation of the */
229  /* structure's orientation at the midpoint of the time interval */
230  /* [t1, t2]. We assume that the angular velocity of the */
231  /* structure equals the constant AV between times t1 and t2. We */
232  /* also assume that */
233 
234  /* || AV || * (t2 - t1) < pi. */
235 
236  /* Then the code fragment */
237 
238  /* CALL QMINI ( Q1, Q2, 0.5D0, QINTRP, SCLDAV ) */
239 
240  /* produces the approximation we desire. */
241 
242 
243 
244  /* $ Restrictions */
245 
246  /* None. */
247 
248  /* $ Literature_References */
249 
250  /* None. */
251 
252  /* $ Author_and_Institution */
253 
254  /* N.J. Bachman (JPL) */
255 
256  /* $ Version */
257 
258  /* - SPICELIB Version 1.0.0, 19-JUL-2005 (NJB) */
259 
260  /* -& */
261  /* $ Index_Entries */
262 
263  /* linear interpolation between quaternions */
264 
265  /* -& */
266 
267  /* SPICELIB functions */
268 
269 
270  /* Local variables */
271 
272 
273  /* Use discovery check-in. */
274 
275 
276 
277  /* Find the conjugate INSTAR of the input quaternion INIT. */
278 
279  instar[0] = init[0];
280  vminus_c(&init[1], &instar[1]);
281 
282  /* Find the quotient quaternion Q that maps INIT to FINAL. */
283 
284  qxq_c(final, instar, q);
285 
286  /* Extract the rotation angle from Q. Use arccosine for */
287  /* speed, sacrificing some accuracy. */
288 
289  angle = acos(brcktd_c(q[0], c_b2, c_b3)) * 2.;
290 
291  /* Create a quaternion QSCALE from the rotation axis of the quotient */
292  /* and the scaled rotation angle. */
293 
294  intang = frac * angle / 2.;
295  qscale[0] = cos(intang);
296 
297  /* Get the unit vector parallel to the vector part of Q. */
298  /* UNORM does exactly what we want here, because if the vector */
299  /* part of Q is zero, the returned "unit" vector will be the */
300  /* zero vector. */
301 
302  unorm_c(&q[1], axis, &vmag);
303 
304  /* Form the vector part of QSCALE. */
305 
306  d__1 = sin(intang);
307  vscl_c(d__1, axis, &qscale[1]);
308 
309  /* Apply QSCALE to INIT to produce the interpolated quaternion we */
310  /* seek. */
311 
312  qxq_c(qscale, init, qintrp);
313  return 0;
314 } /* qmini_ */
315