1 /* zzcxbrut.f -- translated by f2c (version 19980913).
2 You must link the resulting object file with the libraries:
3 -lf2c -lm (in that order)
4 */
5
6 #include "f2c.h"
7
8 /* Table of constant values */
9
10 static doublereal c_b6 = 1.;
11
12 /* $Procedure ZZCXBRUT ( Cone-segment intersection by brute force ) */
zzcxbrut_(doublereal * apex,doublereal * axis,doublereal * angle,doublereal * endpt1,doublereal * endpt2,doublereal * xpt,logical * isbrck)13 /* Subroutine */ int zzcxbrut_(doublereal *apex, doublereal *axis, doublereal
14 *angle, doublereal *endpt1, doublereal *endpt2, doublereal *xpt,
15 logical *isbrck)
16 {
17 /* System generated locals */
18 doublereal d__1;
19
20 /* Builtin functions */
21 double cos(doublereal);
22
23 /* Local variables */
24 extern /* Subroutine */ int vadd_(doublereal *, doublereal *, doublereal *
25 );
26 doublereal high;
27 extern /* Subroutine */ int vhat_(doublereal *, doublereal *);
28 extern doublereal vdot_(doublereal *, doublereal *);
29 integer nitr;
30 extern /* Subroutine */ int vsub_(doublereal *, doublereal *, doublereal *
31 ), vequ_(doublereal *, doublereal *);
32 doublereal uoff1[3], uoff2[3], x[3], delta;
33 extern /* Subroutine */ int chkin_(char *, ftnlen);
34 doublereal midpt;
35 logical state;
36 extern /* Subroutine */ int vlcom_(doublereal *, doublereal *, doublereal
37 *, doublereal *, doublereal *);
38 extern logical vzero_(doublereal *);
39 logical state1, state2;
40 doublereal dp;
41 extern doublereal pi_(void), halfpi_(void);
42 doublereal locang, cosang, ux[3], locaxi[3];
43 extern /* Subroutine */ int sigerr_(char *, ftnlen), vhatip_(doublereal *)
44 , chkout_(char *, ftnlen), setmsg_(char *, ftnlen);
45 doublereal dp1, dp2, prvdlt;
46 extern logical return_(void);
47 extern /* Subroutine */ int vminus_(doublereal *, doublereal *);
48 doublereal seg[3], low, off1[3], off2[3];
49
50 /* $ Abstract */
51
52
53 /* SPICE Private routine intended solely for the support of SPICE */
54 /* routines. Users should not call this routine directly due */
55 /* to the volatile nature of this routine. */
56
57 /* Compute a bracketed point of intersection of a specified nappe of */
58 /* a cone and a line segment by "brute force"---specifically by */
59 /* bisection. */
60
61 /* $ Disclaimer */
62
63 /* THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE */
64 /* CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S. */
65 /* GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE */
66 /* ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE */
67 /* PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS" */
68 /* TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY */
69 /* WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A */
70 /* PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC */
71 /* SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE */
72 /* SOFTWARE AND RELATED MATERIALS, HOWEVER USED. */
73
74 /* IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA */
75 /* BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT */
76 /* LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND, */
77 /* INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS, */
78 /* REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE */
79 /* REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY. */
80
81 /* RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF */
82 /* THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY */
83 /* CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE */
84 /* ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE. */
85
86 /* $ Required_Reading */
87
88 /* None. */
89
90 /* $ Keywords */
91
92 /* CONE */
93 /* GEOMETRY */
94 /* INTERSECTION */
95 /* LINE */
96 /* MATH */
97 /* SEGMENT */
98
99 /* $ Declarations */
100 /* $ Brief_I/O */
101
102 /* Variable I/O Description */
103 /* -------- --- -------------------------------------------------- */
104 /* APEX I Apex of cone. */
105 /* AXIS I Axis of cone. */
106 /* ANGLE I Angle of cone. */
107 /* ENDPT1, */
108 /* ENDPT2 I Endpoints of line segment. */
109 /* XPT O Intersection point, if it exists. */
110 /* ISBRCK O Logical flag indicating whether root is bracketed. */
111
112 /* $ Detailed_Input */
113
114 /* APEX is the apex (tip) of the cone. In this routine's */
115 /* documentation, we'll consider the cone to be a */
116 /* semi-infinite pyramid with circular cross-section. In */
117 /* some contexts, this object is called one "nappe" of */
118 /* the complete cone. */
119
120 /* AXIS is an axis vector of the cone. */
121
122 /* ANGLE is the angular separation from AXIS of the rays */
123 /* comprising the cone. Let the notation */
124
125 /* < A, B > */
126
127 /* denote the dot product of vectors A and B, and let */
128
129 /* ||A|| */
130
131 /* denote the norm of vector A. Then the cone is the set */
132 /* of points */
133
134 /* X-APEX AXIS */
135 /* { X: < ----------, -------- > = cos(ANGLE) } */
136 /* ||X-APEX|| ||AXIS|| */
137
138
139 /* ENDPT1, */
140 /* ENDPT2 are endpoints of a line segment. These points */
141 /* must be distinct. */
142
143 /* Exactly one of ENDPT1 and ENDPT2 must be inside */
144 /* the cone. */
145
146 /* $ Detailed_Output */
147
148 /* NXPTS is the number of points of intersection of the input */
149 /* line segment and cone. */
150
151 /* XPT is the unique point of intersection of the segment and */
152 /* cone that lies on the line segment connecting ENDPT1 */
153 /* and ENDPT2. */
154
155 /* ISBRCK is a logical flag that is set to .TRUE. if and only if */
156 /* ENDPT1 and ENDPT2 bracket a root. Equivalently, the */
157 /* endpoints bracket a root when exactly one of ENDPT1 */
158 /* and ENDPT2 is inside the cone. */
159
160
161 /* $ Parameters */
162
163 /* None. */
164
165 /* $ Exceptions */
166
167 /* 1) If AXIS is the zero vector, the error SPICE(ZEROVECTOR) */
168 /* will be signaled. */
169
170 /* $ Files */
171
172 /* None. */
173
174 /* $ Particulars */
175
176 /* This routine is used by the SPICELIB routine INCNSG to handle */
177 /* cases where solution of a quadratic equation is subject to */
178 /* excessive loss of precision. */
179
180 /* $ Examples */
181
182 /* See usage in INCNSG. */
183
184 /* $ Restrictions */
185
186 /* This is a private SPICELIB routine. */
187
188 /* $ Literature_References */
189
190 /* None. */
191
192 /* $ Author_and_Institution */
193
194 /* N.J. Bachman (JPL) */
195
196 /* $ Version */
197
198 /* - SPICELIB Version 1.0.0 30-SEP-2016 (NJB) */
199
200 /* -& */
201 /* $ Index_Entries */
202
203 /* brute force intersection of line segment and cone */
204
205 /* -& */
206
207 /* SPICELIB functions */
208
209
210 /* Local parameters */
211
212
213 /* Local variables */
214
215
216 /* Use discovery check-in. */
217
218 if (return_()) {
219 return 0;
220 }
221
222 /* Check the axis. */
223
224 if (vzero_(axis)) {
225 chkin_("ZZCXBRUT", (ftnlen)8);
226 setmsg_("Cone axis is the zero vector", (ftnlen)28);
227 sigerr_("SPICE(ZEROVECTOR)", (ftnlen)17);
228 chkout_("ZZCXBRUT", (ftnlen)8);
229 return 0;
230 }
231
232 /* Make a local version of the cone's axis and angle. The */
233 /* angle will be less than or equal to pi/2 radians. */
234
235 if (*angle > halfpi_()) {
236 locang = pi_() - *angle;
237 vminus_(axis, locaxi);
238 } else {
239 locang = *angle;
240 vequ_(axis, locaxi);
241 }
242 vhatip_(locaxi);
243 cosang = cos(locang);
244
245 /* Calculate the offsets of the endpoints from the apex, */
246 /* and get unit-length versions of these. */
247
248 vsub_(endpt1, apex, off1);
249 vsub_(endpt2, apex, off2);
250 vhat_(off1, uoff1);
251 vhat_(off2, uoff2);
252
253 /* Get the dot products of the unit offsets with the axis. */
254 /* These will serve as proxies for latitude. */
255
256 dp1 = vdot_(uoff1, locaxi);
257 dp2 = vdot_(uoff2, locaxi);
258
259 /* The "state" variables at the endpoints are .TRUE. if */
260 /* the endpoints are on or inside the cone. */
261
262 state1 = dp1 >= cosang;
263 state2 = dp2 >= cosang;
264
265 /* The intersection is supposed to be bracketed. Return */
266 /* if not, indicating the situation via ISBRCK. */
267
268 *isbrck = state1 != state2;
269 if (! (*isbrck)) {
270 return 0;
271 }
272
273 /* Prepare for a solution by bisection. */
274
275 vsub_(off2, off1, seg);
276 low = 0.;
277 high = 1.;
278 delta = (d__1 = high - low, abs(d__1));
279 prvdlt = 2.;
280 nitr = 0;
281 while(delta > 1e-15 && delta < prvdlt && nitr < 1000) {
282 midpt = (low + high) / 2;
283 vlcom_(&c_b6, off1, &midpt, seg, x);
284 vhat_(x, ux);
285 dp = vdot_(ux, locaxi);
286 state = dp >= cosang;
287 if (state == state1) {
288
289 /* There has been no state change between OFF1 */
290 /* and XPT. */
291
292 low = midpt;
293 } else {
294 high = midpt;
295 }
296 prvdlt = delta;
297 delta = (d__1 = high - low, abs(d__1));
298 ++nitr;
299 }
300
301 /* X is an offset from APEX. The solution is an offset from the */
302 /* origin. */
303
304 vadd_(apex, x, xpt);
305 return 0;
306 } /* zzcxbrut_ */
307
308