1 /*
2 * Copyright (C) 1998, 2000-2007, 2010, 2011, 2012, 2013 SINTEF ICT,
3 * Applied Mathematics, Norway.
4 *
5 * Contact information: E-mail: tor.dokken@sintef.no
6 * SINTEF ICT, Department of Applied Mathematics,
7 * P.O. Box 124 Blindern,
8 * 0314 Oslo, Norway.
9 *
10 * This file is part of SISL.
11 *
12 * SISL is free software: you can redistribute it and/or modify
13 * it under the terms of the GNU Affero General Public License as
14 * published by the Free Software Foundation, either version 3 of the
15 * License, or (at your option) any later version.
16 *
17 * SISL is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU Affero General Public License for more details.
21 *
22 * You should have received a copy of the GNU Affero General Public
23 * License along with SISL. If not, see
24 * <http://www.gnu.org/licenses/>.
25 *
26 * In accordance with Section 7(b) of the GNU Affero General Public
27 * License, a covered work must retain the producer line in every data
28 * file that is created or manipulated using SISL.
29 *
30 * Other Usage
31 * You can be released from the requirements of the license by purchasing
32 * a commercial license. Buying such a license is mandatory as soon as you
33 * develop commercial activities involving the SISL library without
34 * disclosing the source code of your own applications.
35 *
36 * This file may be used in accordance with the terms contained in a
37 * written agreement between you and SINTEF ICT.
38 */
39
40 #include "sisl-copyright.h"
41
42 /*
43 *
44 * $Id: sh1834.c,v 1.5 2001-03-19 15:59:05 afr Exp $
45 *
46 */
47
48
49 #define SH1834
50
51 #include "sislP.h"
52
53 /*
54 * Forward declarations.
55 * ---------------------
56 */
57
58 #if defined(SISLNEEDPROTOTYPES)
59 static void sh1834_s9mat2d(double [],double []);
60 static void sh1834_s9mat3d(double [],double [],double []);
61 #else
62 static void sh1834_s9mat2d();
63 static void sh1834_s9mat3d();
64 #endif
65
66 #if defined(SISLNEEDPROTOTYPES)
sh1834(SISLObject * po1,SISLObject * po2,double aepsge,int idim,double edir1[],double edir2[],int * jstat)67 void sh1834(SISLObject *po1,SISLObject *po2,double aepsge,int idim,
68 double edir1[],double edir2[],int *jstat)
69 #else
70 void sh1834(po1,po2,aepsge,idim,edir1,edir2,jstat)
71 SISLObject *po1;
72 SISLObject *po2;
73 double aepsge;
74 int idim;
75 double edir1[];
76 double edir2[];
77 int *jstat;
78 #endif
79 /*
80 *********************************************************************
81 *
82 *********************************************************************
83 *
84 * PURPOSE : Perform a box-test to check if two objects overlap in
85 * the rotated coordinate system where edir1 defines the
86 * x-axis and (edir1 x edir2) defines the z-axis if
87 * idim = 3.
88 *
89 *
90 *
91 * INPUT : po1 - Pointer to the first object.
92 * po2 - Pointer to the second object.
93 * aepsge - Geometry resolution.
94 * idim - The dimension of the space in which the objects
95 * lie. idim = 2 or idim = 3.
96 * edir1 - First direction vector. Defines x-axis in rotated
97 * coordinate system.
98 * edir2 - Second direction vector.
99 *
100 *
101 *
102 * OUTPUT : jstat - status messages
103 * = 2 : Boundaries just touch.
104 * = 1 : Rotated SISLbox overlaps point.
105 * = 0 : No overlap.
106 * < 0 : error
107 *
108 *
109 * METHOD : The coordinate system is rotated such that if idim = 2,
110 * the x-axis of the new coordinate system is parallell to
111 * the vector edir1. If idim = 3, the cross-product of edir1
112 * and edir2 is rotated to be parallell to the z-axis and
113 * edir1 rotated to be parallell to the x-axis. The objects
114 * are moved into this rotated coordinate system and a
115 * box-test is performed.
116 *
117 *
118 * REFERENCES :
119 *
120 *-
121 * CALLS : s1790 - Perform box test.
122 * s6scpr - Compute scalar product of two vectors.
123 * sh1834_s9mat2d - Set up rotation matrix for 2 dimensional space.
124 * sh1834_s9mat3d - Set up rotation matrix for 3 dimensional space.
125 *
126 * WRITTEN BY : Vibeke Skytt, SI, 88-06.
127 * REVISED BY : Vibeke Skytt, SI, 91-01.
128 * REVISED BY : Mike Floater, SI, 94-07. Updated for rationals
129 * (s1834 didn't need changing).
130 * REVISED BY : Vibeke Skytt, SINTEF Oslo, 94-11. Introduced 2D point-surface
131 * intersection.
132 *
133 *********************************************************************
134 */
135 {
136 int kstat = 0; /* Local status variable. */
137 int kpos = 0; /* Position of error. */
138 int kinnerexp = 12; /* Expand box in the inner. No rotation. */
139 int kn1=0,kn2=0; /* Number of coefficients of objects. */
140 double *sc1,*sc2;/* Pointers to coefficients of objects. */
141 double *scoef1=SISL_NULL; /* Rotated coefficients of first object. */
142 double *scoef2=SISL_NULL; /* Rotated coefficients of second object.*/
143 double *smat=SISL_NULL; /* Rotation matrix. */
144 double *s1,*s2,*s3,*s4,*s5; /* Pointers used to traverse arrays. */
145 SISLObject *qo1=SISL_NULL; /* First object after rotation. */
146 SISLObject *qo2=SISL_NULL; /* Second object after rotation. */
147 /* long time_before;
148 long time_used=0; */
149
150 double *rc1,*rc2; /* Pointers to homogeneous coefficients. */
151 double *rcoef1=SISL_NULL; /* Possibly homogeneous coefficients. */
152 double *rcoef2=SISL_NULL; /* Possibly homogeneous coefficients. */
153 int ikind1=0, ikind2=0; /* Kinds of objects 1 and 2. */
154 int i,i1,i2,j,k; /* Loop variables. */
155
156 /* Test input. */
157
158 if (idim != 2 && idim != 3) goto err105;
159
160 /* Fetch coefficients of the objects. */
161
162 if (po1->iobj == SISLCURVE)
163 {
164 kn1 = po1->c1->in;
165 sc1 = po1->c1->ecoef;
166 rc1 = po1->c1->rcoef;
167 ikind1 = po1->c1->ikind;
168 }
169 else if (po1->iobj == SISLSURFACE)
170 {
171 kn1 = po1->s1->in1*po1->s1->in2;
172 sc1 = po1->s1->ecoef;
173 rc1 = po1->s1->rcoef;
174 ikind1 = po1->s1->ikind;
175 }
176 else
177 {
178 kn1 = 1;
179 sc1 = po1->p1->ecoef;
180 rc1 = SISL_NULL;
181 ikind1 = 1;
182 }
183
184 if (po2->iobj == SISLCURVE)
185 {
186 kn2 = po2->c1->in;
187 sc2 = po2->c1->ecoef;
188 rc2 = po2->c1->rcoef;
189 ikind2 = po2->c1->ikind;
190 }
191 else if (po2->iobj == SISLSURFACE)
192 {
193 kn2 = po2->s1->in1*po2->s1->in2;
194 sc2 = po2->s1->ecoef;
195 rc2 = po2->s1->rcoef;
196 ikind2 = po2->s1->ikind;
197 }
198 else
199 {
200 kn2 = 1;
201 sc2 = po2->p1->ecoef;
202 rc2 = SISL_NULL;
203 ikind2 = 1;
204 }
205
206 /* Allocate space for local parameters. */
207
208 if ((scoef1 = newarray(idim*kn1,DOUBLE)) == SISL_NULL) goto err101;
209 if ((scoef2 = newarray(idim*kn2,DOUBLE)) == SISL_NULL) goto err101;
210 if ((smat = new0array(idim*idim,DOUBLE)) == SISL_NULL) goto err101;
211
212 /* Find the rotation matrix. */
213
214 if (idim == 2)
215
216 /* After normalization edir1[0] will contain the cosine of the
217 rotation angle and edir1[1] will contain the sine. */
218
219 sh1834_s9mat2d(smat,edir1);
220 else
221
222 /* Set up the rotation matrix when idim = 3. (edir1 x edir2) is
223 rotated to be parallell to the z-axis and edir1 to be parallell
224 to the x-axis. */
225
226 sh1834_s9mat3d(smat,edir1,edir2);
227
228 /* The objects is moved into the new coordinate system by rotating
229 them using the rotation matrix. */
230
231 /* Rotate first object. */
232
233 for (s2=sc1,s4=s2+idim*kn1,s5=scoef1; s2<s4; s2+=idim)
234 for (s1=smat,s3=smat+idim*idim; s1<s3; s1+=idim,s5++)
235 *s5 = s6scpr(s1,s2,idim);
236
237 /* Rotate second object. */
238
239 for (s2=sc2,s4=s2+idim*kn2,s5=scoef2; s2<s4; s2+=idim)
240 for (s1=smat,s3=smat+idim*idim; s1<s3; s1+=idim,s5++)
241 *s5 = s6scpr(s1,s2,idim);
242
243 /* Make rotated objects. */
244
245 if ((qo1 = newObject(po1->iobj)) == SISL_NULL) goto err101;
246 if ((qo2 = newObject(po2->iobj)) == SISL_NULL) goto err101;
247
248 if(ikind1 == 2 || ikind1 == 4)
249 {
250 if ((rcoef1 = newarray((idim+1)*kn1,DOUBLE)) == SISL_NULL) goto err101;
251 for(i=0,i1=0,i2=0; i<kn1; i++)
252 {
253 k = i1 + idim;
254 for(j=0; j<idim; j++, i1++, i2++)
255 {
256 rcoef1[i1] = scoef1[i2] * rc1[k];
257 }
258 rcoef1[i1] = rc1[k];
259 i1++;
260 }
261 }
262 else
263 {
264 rcoef1 = scoef1;
265 }
266
267 if(ikind2 == 2 || ikind2 == 4)
268 {
269 if ((rcoef2 = newarray((idim+1)*kn2,DOUBLE)) == SISL_NULL) goto err101;
270 for(i=0,i1=0,i2=0; i<kn2; i++)
271 {
272 k = i1 + idim;
273 for(j=0; j<idim; j++, i1++, i2++)
274 {
275 rcoef2[i1] = scoef2[i2] * rc2[k];
276 }
277 rcoef2[i1] = rc2[k];
278 i1++;
279 }
280 }
281 else
282 {
283 rcoef2 = scoef2;
284 }
285
286
287 if (po1->iobj == SISLCURVE)
288 {
289 if ((qo1->c1 = newCurve(po1->c1->in,po1->c1->ik,po1->c1->et,
290 rcoef1,po1->c1->ikind,idim,0)) == SISL_NULL)
291 goto err101;
292 /* printf("Rotated box test. Curve - "); */
293 }
294 else if (po1->iobj == SISLSURFACE)
295 {
296 if ((qo1->s1 = newSurf(po1->s1->in1,po1->s1->in2,po1->s1->ik1,
297 po1->s1->ik2,po1->s1->et1,po1->s1->et2,
298 rcoef1,po1->s1->ikind,idim,0)) == SISL_NULL)
299 goto err101;
300 /* printf("Rotated box test. Surface - "); */
301
302 }
303 else
304 {
305 if ((qo1->p1 = newPoint(rcoef1,idim,0)) == SISL_NULL) goto err101;
306 }
307
308 if (po2->iobj == SISLCURVE)
309 {
310 if ((qo2->c1 = newCurve(po2->c1->in,po2->c1->ik,po2->c1->et,
311 rcoef2,po2->c1->ikind,idim,0)) == SISL_NULL)
312 goto err101;
313 /* printf("curve. "); */
314 }
315 else if (po2->iobj == SISLSURFACE)
316 {
317 if ((qo2->s1 = newSurf(po2->s1->in1,po2->s1->in2,po2->s1->ik1,
318 po2->s1->ik2,po2->s1->et1,po2->s1->et2,
319 rcoef2,po2->s1->ikind,idim,0)) == SISL_NULL)
320 goto err101;
321 /* printf("surface. "); */
322 }
323 else
324 {
325 if ((qo2->p1 = newPoint(rcoef2,idim,0)) == SISL_NULL) goto err101;
326 }
327
328 /* Make box test. */
329
330 /* time_before = clock();
331 boxrot_nmb++; */
332 sh1790(qo1,qo2,kinnerexp,aepsge,&kstat);
333 /* time_used = clock() - time_before;
334 boxrot_time += time_used; */
335 if (kstat < 0) goto error;
336 /* printf("Status = %d \n",kstat); */
337
338 /* Box-test permformed. */
339
340 *jstat = kstat;
341 goto out;
342
343 /* Error in space allocation. */
344
345 err101: *jstat = -101;
346 s6err("sh1834",*jstat,kpos);
347 goto out;
348
349 /* Error in input. Dimension not equal to 2 or 3. */
350
351 err105: *jstat = -105;
352 s6err("sh1834",*jstat,kpos);
353 goto out;
354
355 /* Error in lower level routine. */
356
357 error : *jstat = kstat;
358 goto out;
359
360 out:
361
362 /* Free space occupied by local arrays and objects. */
363
364 if (qo1 != SISL_NULL) freeObject(qo1);
365 if (qo2 != SISL_NULL) freeObject(qo2);
366 if (rcoef1 != SISL_NULL && rcoef1 != scoef1) freearray(rcoef1);
367 if (rcoef2 != SISL_NULL && rcoef2 != scoef2) freearray(rcoef2);
368 if (scoef1 != SISL_NULL) freearray(scoef1);
369 if (scoef2 != SISL_NULL) freearray(scoef2);
370 if (smat != SISL_NULL) free0array(smat);
371
372 return;
373 }
374
375 #if defined(SISLNEEDPROTOTYPES)
376 static void
sh1834_s9mat2d(double emat[],double edir[])377 sh1834_s9mat2d(double emat[],double edir[])
378 #else
379 static void sh1834_s9mat2d(emat,edir)
380 double emat[];
381 double edir[];
382 #endif
383 /*
384 *********************************************************************
385 *
386 * PURPOSE : Set up rotation matrix in two dimensions when the x-axis
387 * is supposed to be rotated to be parallell with edir.
388 *
389 * INPUT : edir - Direction of rotated x-axis.
390 *
391 * OUTPUT : emat - Rotation matrix. emat is supposed to be
392 * initialized to zero before this routine is entered.
393 *
394 *********************************************************************
395 */
396 {
397 int kstat = 0; /* Local status variable. */
398 double tlength; /* Length of vector edir. */
399 double sdir[2]; /* Normalized vertion of vector edir. */
400
401 tlength = s6norm(edir,2,sdir,&kstat);
402 if (kstat == 0)
403
404 /* Length of edir equal to zero. Let the rotation matrix be
405 the identity matrix. */
406
407 emat[0] = emat[3] = (double)1.0;
408 else
409 {
410
411 /* Make rotation matrix. */
412
413 emat[0] = sdir[0];
414 emat[1] = sdir[1];
415 emat[2] = sdir[1];
416 emat[3] = -sdir[0];
417 }
418 }
419
420 #if defined(SISLNEEDPROTOTYPES)
421 static void
sh1834_s9mat3d(double emat[],double edir1[],double edir2[])422 sh1834_s9mat3d(double emat[],double edir1[],double edir2[])
423 #else
424 static void sh1834_s9mat3d(emat,edir1,edir2)
425 double emat[];
426 double edir1[];
427 double edir2[];
428 #endif
429 /*
430 *********************************************************************
431 *
432 * PURPOSE : Set up rotation matrix in three dimensions when edir1
433 * is supposed to be rotated to be parallell with the x-axis
434 * and (edir1 x edir2) to be parallell with the z-axis.
435 *
436 * INPUT : edir1 - First direction vector.
437 * edir2 - Second direction vector.
438 *
439 * OUTPUT : emat - Rotation matrix. The matrix is supposed to be
440 * initialized to zero before this routine is enterd.
441 *
442 *********************************************************************
443 */
444 {
445 int kstat = 0; /* Local status variable. */
446 double snorm[3]; /* Cross-product of edir1 and edir2. */
447 double sdir[3]; /* Normalized vertion of edir1. */
448 double *s1; /* Pointer into emat array. */
449 double tleng1,tleng2; /* Length of snorm and edir1 respectively. */
450 double ta1,ta2,ta3,tb1,tb2,tb3,td1,td2,tl1,tl2,tl3; /* Help variables. */
451
452 /* Calculate cross-product of edir1 and edir2. */
453
454 s6crss(edir1,edir2,snorm);
455
456 /* Normalize snorm. */
457
458 tleng1 = s6norm(snorm,3,snorm,&kstat);
459
460 /* Normalize edir1. */
461
462 tleng2 = s6norm(edir1,3,sdir,&kstat);
463
464 /* Initialize help variables. */
465
466 ta1 = snorm[0];
467 ta2 = snorm[1];
468 ta3 = snorm[2];
469 tl1 = sqrt(ta2*ta2+ta3*ta3);
470
471 /* Set up rotation matrix. */
472
473 if ((DEQUAL(tleng1,DZERO) || DEQUAL(tl1,DZERO)) && DEQUAL(tleng2,DZERO))
474
475 /* The rotation matrix is the identity matrix. */
476
477 emat[0] = emat[4] = emat[8] = (double)1.0;
478 else if (DEQUAL(tleng1,DZERO) || DEQUAL(tl1,DZERO))
479 {
480
481 /* The rotation matrix is supposed to rotate edir1 to be parallell
482 to the x-axis. */
483
484 tb1 = sdir[0];
485 tb2 = sdir[1];
486 tb3 = sdir[2];
487 tl3 = sqrt(tb1*tb1+tb2*tb2);
488
489 if (DEQUAL(tl3,DZERO)) emat[0] = emat[4] = emat[8] = (double)1.0;
490 else
491 {
492 s1 = emat;
493 *(s1++) = tb1;
494 *(s1++) = tb2;
495 *(s1++) = tb3;
496 *(s1++) = -tb2/tl3;
497 *(s1++) = tb1/tl3;
498 *(s1++) = DZERO;
499 *(s1++) = -tb1*tb3/tl3;
500 *(s1++) = -tb2*tb3/tl3;
501 *(s1++) = tl3;
502 }
503 }
504 else
505 {
506 td1 = edir1[0]/tl1;
507 td2 = (ta3*edir1[1] - ta2*edir1[2])/tl1;
508 tl2 = sqrt(td1*td1+td2*td2);
509
510 if (DEQUAL(tl2,DZERO))
511 {
512
513 /* The normal snorm is rotated to be parallell to the z-axis. */
514
515 s1 = emat;
516 *(s1++) = tl1;
517 *(s1++) = -ta1*ta2/tl1;
518 *(s1++) = -ta1*ta3/tl1;
519 *(s1++) = DZERO;
520 *(s1++) = ta3/tl1;
521 *(s1++) = -ta2/tl1;
522 *(s1++) = ta1;
523 *(s1++) = ta2;
524 *(s1++) = ta3;
525 }
526 else
527 {
528
529 /* The normal is rotated to be parallell to the z-axis and edir1
530 to be parallell to the x-axis. */
531
532 s1 = emat;
533 *(s1++) = td1*tl1/tl2;
534 *(s1++) = (-ta1*ta2*td1 + ta3*td2)/(tl1*tl2);
535 *(s1++) = (-ta1*ta3*td1 - ta2*td2)/(tl1*tl2);
536 *(s1++) = -td2*tl1/tl2;
537 *(s1++) = (ta1*ta2*td2 + ta3*td1)/(tl1*tl2);
538 *(s1++) = (ta1*ta3*td2 - ta2*td1)/(tl1*tl2);
539 *(s1++) = ta1;
540 *(s1++) = ta2;
541 *(s1++) = ta3;
542 }
543 }
544 }
545