Branch data Line data Source code
1 : : //- Class: AnalyticGeometryTool
2 : : //- Description: This class performs calculations on analytic geometry
3 : : // (points, lines, arcs, planes, polygons). Capabilities
4 : : // include vector and point math, matrix operations,
5 : : // measurements, intersections and comparison/containment checks.
6 : : //
7 : : //- Owner: Steve Storm, Caterpillar Inc.
8 : : //- Checked by:
9 : :
10 : :
11 : : #include <float.h>
12 : : #include "AnalyticGeometryTool.hpp"
13 : : #include "CubitMessage.hpp"
14 : : #include "CubitBox.hpp"
15 : : #include "CubitPlane.hpp"
16 : : #include "CubitVector.hpp"
17 : : #include "DLIList.hpp"
18 : : #include <math.h>
19 : : #include "CastTo.hpp"
20 : : #include "GfxPreview.hpp"
21 : : #include "GMem.hpp"
22 : :
23 : : #include <fstream>
24 : : using std::cout;
25 : : using std::endl;
26 : : using std::ofstream;
27 : : using std::ios;
28 : : using std::ostream;
29 : :
30 : : static double AGT_IDENTITY_MTX_4X4[4][4] = { {1.0, 0.0, 0.0, 0.0},
31 : : {0.0, 1.0, 0.0, 0.0},
32 : : {0.0, 0.0, 1.0, 0.0},
33 : : {0.0, 0.0, 0.0, 1.0} };
34 : :
35 : : static double AGT_IDENTITY_MTX_3X3[3][3] = { {1.0, 0.0, 0.0},
36 : : {0.0, 1.0, 0.0},
37 : : {0.0, 0.0, 1.0} };
38 : :
39 : : AnalyticGeometryTool* AnalyticGeometryTool::instance_ = 0;
40 : :
41 : 0 : Point2 operator+ (const Point2& p, const Point2& q)
42 : : {
43 : : Point2 add;
44 : 0 : add.x = p.x + q.x;
45 : 0 : add.y = p.y + q.y;
46 : 0 : return add;
47 : : }
48 : :
49 : 0 : Point2 operator- (const Point2& p, const Point2& q)
50 : : {
51 : : Point2 sub;
52 : 0 : sub.x = p.x - q.x;
53 : 0 : sub.y = p.y - q.y;
54 : 0 : return sub;
55 : : }
56 : :
57 : 0 : Point2 operator* (double t, const Point2& p)
58 : : {
59 : : Point2 prod;
60 : 0 : prod.x = t*p.x;
61 : 0 : prod.y = t*p.y;
62 : 0 : return prod;
63 : : }
64 : :
65 : 0 : Point2 operator* (const Point2& p, double t)
66 : : {
67 : : Point2 prod;
68 : 0 : prod.x = t*p.x;
69 : 0 : prod.y = t*p.y;
70 : 0 : return prod;
71 : : }
72 : :
73 : 0 : Point2 operator- (const Point2& p)
74 : : {
75 : : Point2 neg;
76 : 0 : neg.x = -p.x;
77 : 0 : neg.y = -p.y;
78 : 0 : return neg;
79 : : }
80 : :
81 : : inline double AnalyticGeometryTool::Dot (const Point2& p, const Point2& q)
82 : : {
83 : : return double(p.x*q.x + p.y*q.y);
84 : : }
85 : :
86 : 0 : Point3 operator+ (const Point3& p, const Point3& q)
87 : : {
88 : : Point3 add;
89 : 0 : add.x = p.x + q.x;
90 : 0 : add.y = p.y + q.y;
91 : 0 : add.z = p.z + q.z;
92 : 0 : return add;
93 : : }
94 : :
95 : 0 : Point3 operator- (const Point3& p, const Point3& q)
96 : : {
97 : : Point3 sub;
98 : 0 : sub.x = p.x - q.x;
99 : 0 : sub.y = p.y - q.y;
100 : 0 : sub.z = p.z - q.z;
101 : 0 : return sub;
102 : : }
103 : :
104 : 0 : Point3 operator* (double t, const Point3& p)
105 : : {
106 : : Point3 prod;
107 : 0 : prod.x = t*p.x;
108 : 0 : prod.y = t*p.y;
109 : 0 : prod.z = t*p.z;
110 : 0 : return prod;
111 : : }
112 : :
113 : 0 : Point3 operator* (const Point3& p, double t)
114 : : {
115 : : Point3 prod;
116 : 0 : prod.x = t*p.x;
117 : 0 : prod.y = t*p.y;
118 : 0 : prod.z = t*p.z;
119 : 0 : return prod;
120 : : }
121 : :
122 : 0 : Point3 operator- (const Point3& p)
123 : : {
124 : : Point3 neg;
125 : 0 : neg.x = -p.x;
126 : 0 : neg.y = -p.y;
127 : 0 : neg.z = -p.z;
128 : 0 : return neg;
129 : : }
130 : :
131 : : // Method: instance
132 : : // provides access to the unique model for this execution.
133 : : // sets up this instance on first access
134 : 1635 : AnalyticGeometryTool* AnalyticGeometryTool::instance()
135 : : {
136 [ + - ]: 1635 : if (instance_ == 0) {
137 [ + - ]: 1635 : instance_ = new AnalyticGeometryTool;
138 : : }
139 : 1635 : return instance_;
140 : : }
141 : :
142 : 1635 : AnalyticGeometryTool::AnalyticGeometryTool()
143 : : {
144 : 1635 : agtEpsilon = 1e-8;
145 : 1635 : }
146 : :
147 : 271 : AnalyticGeometryTool::~AnalyticGeometryTool()
148 : : {
149 : 271 : instance_ = NULL;
150 : 271 : }
151 : :
152 : : //***************************************************************************
153 : : // Double numbers
154 : : //***************************************************************************
155 : :
156 : :
157 : 0 : void AnalyticGeometryTool::round_near_val( double &val )
158 : : {
159 [ # # ]: 0 : if( dbl_eq( val, 0.0 ) )
160 : 0 : val = 0.0;
161 [ # # ]: 0 : else if( dbl_eq( val, 1.0 ) )
162 : 0 : val = 1.0;
163 [ # # ]: 0 : else if( dbl_eq( val, -1.0 ) )
164 : 0 : val = -1.0;
165 : 0 : }
166 : :
167 : : //***************************************************************************
168 : : // Matrices & Transforms
169 : : //***************************************************************************
170 : 0 : void AnalyticGeometryTool::transform_pnt( double m[4][4],
171 : : double pin[3],
172 : : double pout[3] )
173 : : {
174 : : double p[3]; // working buffer
175 : :
176 : : // Check if transformation can occur
177 [ # # ]: 0 : if (!m) {
178 [ # # ][ # # ]: 0 : if (pin && pout)
179 [ # # ]: 0 : copy_pnt(pin, pout);
180 : 0 : return;
181 : : }
182 : :
183 : : // Perform transformation
184 : 0 : p[0] = m[0][0] * pin[0] + m[1][0] * pin[1] + m[2][0] * pin[2] + m[3][0];
185 : 0 : p[1] = m[0][1] * pin[0] + m[1][1] * pin[1] + m[2][1] * pin[2] + m[3][1];
186 : 0 : p[2] = m[0][2] * pin[0] + m[1][2] * pin[1] + m[2][2] * pin[2] + m[3][2];
187 : :
188 : : // Copy work buffer to out point
189 [ # # ]: 0 : copy_pnt(p,pout);
190 : : }
191 : :
192 : 0 : void AnalyticGeometryTool::transform_vec( double m3[3][3],
193 : : double vin[3],
194 : : double vout[3] )
195 : : {
196 : : double v[3]; // working buffer
197 : :
198 : : // Determine if transformation can occur
199 [ # # ]: 0 : if (!m3) {
200 [ # # ][ # # ]: 0 : if (vin && vout)
201 [ # # ]: 0 : copy_vec(vin, vout);
202 : 0 : return;
203 : : }
204 : :
205 : : // Perform transformation
206 : 0 : v[0] = m3[0][0] * vin[0] + m3[1][0] * vin[1] + m3[2][0] * vin[2];
207 : 0 : v[1] = m3[0][1] * vin[0] + m3[1][1] * vin[1] + m3[2][1] * vin[2];
208 : 0 : v[2] = m3[0][2] * vin[0] + m3[1][2] * vin[1] + m3[2][2] * vin[2];
209 : :
210 : : // Copy work buffer to vector out
211 [ # # ]: 0 : copy_pnt(v,vout);
212 : : }
213 : :
214 : 0 : void AnalyticGeometryTool::transform_vec( double m4[4][4],
215 : : double vin[3],
216 : : double vout[3] )
217 : : {
218 : : double v[3]; // working buffer
219 : :
220 : : // Determine if transformation can occur
221 [ # # ]: 0 : if (!m4) {
222 [ # # ][ # # ]: 0 : if (vin && vout)
223 [ # # ]: 0 : copy_vec(vin, vout);
224 : 0 : return;
225 : : }
226 : :
227 : : // Perform transformation
228 : :
229 : 0 : v[0] = m4[0][0] * vin[0] + m4[1][0] * vin[1] + m4[2][0] * vin[2];
230 : 0 : v[1] = m4[0][1] * vin[0] + m4[1][1] * vin[1] + m4[2][1] * vin[2];
231 : 0 : v[2] = m4[0][2] * vin[0] + m4[1][2] * vin[1] + m4[2][2] * vin[2];
232 : :
233 : : // Copy work buffer to vector out
234 [ # # ]: 0 : copy_pnt(v,vout);
235 : : }
236 : :
237 : 0 : void AnalyticGeometryTool::transform_line( double rot_mtx[4][4],
238 : : double origin[3], double axis[3] )
239 : : {
240 : : double end_pnt[3]; // Find arbitrary end point on line
241 [ # # ]: 0 : next_pnt( origin, axis, 10.0, end_pnt );
242 : :
243 [ # # ]: 0 : transform_pnt( rot_mtx, origin, origin );
244 [ # # ]: 0 : transform_pnt( rot_mtx, end_pnt, end_pnt );
245 : :
246 : 0 : axis[0] = end_pnt[0] - origin[0];
247 : 0 : axis[1] = end_pnt[1] - origin[1];
248 : 0 : axis[2] = end_pnt[2] - origin[2];
249 : 0 : }
250 : :
251 : 0 : void AnalyticGeometryTool::transform_line( double rot_mtx[4][4],
252 : : CubitVector &origin, CubitVector &axis )
253 : : {
254 [ # # ]: 0 : CubitVector end_point; // Find arbitrary end point on line
255 [ # # ]: 0 : origin.next_point( axis, 10.0, end_point );
256 : :
257 : : double start_pnt[3], end_pnt[3];
258 [ # # ]: 0 : copy_pnt( origin, start_pnt );
259 [ # # ]: 0 : copy_pnt( end_point, end_pnt );
260 : :
261 [ # # ]: 0 : transform_pnt( rot_mtx, start_pnt, start_pnt );
262 [ # # ]: 0 : transform_pnt( rot_mtx, end_pnt, end_pnt );
263 : :
264 [ # # ]: 0 : axis.x( end_pnt[0] - start_pnt[0] );
265 [ # # ]: 0 : axis.y( end_pnt[1] - start_pnt[1] );
266 [ # # ]: 0 : axis.z( end_pnt[2] - start_pnt[2] );
267 : :
268 [ # # ]: 0 : origin.set( start_pnt );
269 : 0 : }
270 : :
271 : 0 : void AnalyticGeometryTool::copy_mtx( double from[3][3],double to[3][3] )
272 : : {
273 : : // Determine if identity matrix needed
274 [ # # ]: 0 : if (!from) // copy in the identity matrix
275 : 0 : memcpy(to, AGT_IDENTITY_MTX_3X3, sizeof(double)*9);
276 : : else // Copy from to to
277 : 0 : memcpy(to, from, sizeof(double)*9);
278 : 0 : }
279 : :
280 : 0 : void AnalyticGeometryTool::copy_mtx( double from[4][4], double to[4][4] )
281 : : {
282 : : // determine if identity matrix needed
283 [ # # ]: 0 : if (!from) // copy in the identity matrix
284 : 0 : memcpy(to, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
285 : : else // copy from to to
286 : 0 : memcpy(to, from, sizeof(double)*16);
287 : 0 : }
288 : :
289 : 0 : void AnalyticGeometryTool::copy_mtx( double from[4][4], double to[3][3] )
290 : : {
291 : : size_t dbl3;
292 : :
293 : 0 : dbl3 = sizeof(double) * 3;
294 : :
295 : : // Determine if identity matrix needed
296 [ # # ]: 0 : if (!from) // Copy in the identity matrix
297 : 0 : memcpy(to, AGT_IDENTITY_MTX_3X3, sizeof(double)*9);
298 : : else { // Copy each upper left element of from to to
299 : 0 : memcpy(to[0], from[0], dbl3);
300 : 0 : memcpy(to[1], from[1], dbl3);
301 : 0 : memcpy(to[2], from[2], dbl3);
302 : : }
303 : 0 : }
304 : :
305 : 0 : void AnalyticGeometryTool::copy_mtx(double from[3][3], double to[4][4],
306 : : double* origin )
307 : : {
308 : : size_t dbl3;
309 : :
310 : 0 : dbl3 = sizeof(double) * 3;
311 : :
312 : : // Determine if identity matrix needed
313 [ # # ]: 0 : if (!from) { // Copy in the identity matrix
314 : :
315 : 0 : memcpy(to, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
316 : :
317 [ # # ]: 0 : if (origin)
318 : 0 : memcpy(to[3], origin, dbl3);
319 : : }
320 : :
321 : : else { // Copy each upper element of from to to
322 : :
323 : 0 : memcpy(to[0], from[0], dbl3);
324 : 0 : memcpy(to[1], from[1], dbl3);
325 : 0 : memcpy(to[2], from[2], dbl3);
326 : :
327 : 0 : to[0][3] = 0.0;
328 : 0 : to[1][3] = 0.0;
329 : 0 : to[2][3] = 0.0;
330 : 0 : to[3][3] = 1.0;
331 : :
332 [ # # ]: 0 : if (origin)
333 : : {
334 : 0 : memcpy(to[3], origin, dbl3);
335 : : // to[3][0] = origin[0];
336 : : // to[3][1] = origin[1];
337 : : // to[3][2] = origin[2];
338 : : }
339 : : else
340 : 0 : memcpy(to[3], AGT_IDENTITY_MTX_4X4[3], sizeof(double)*3);
341 : : }
342 : 0 : }
343 : :
344 : 0 : void AnalyticGeometryTool::create_rotation_mtx( double theta, double v[3],
345 : : double mtx3x3[3][3] )
346 : : {
347 : : double coeff1;
348 : : double coeff2;
349 : : double coeff3;
350 : : double v_unit[3];
351 : :
352 [ # # ]: 0 : if (!mtx3x3)
353 : 0 : return;
354 : :
355 : 0 : coeff1 = cos(theta);
356 : 0 : coeff2 = (1.0l - coeff1);
357 : 0 : coeff3 = sin(theta);
358 : :
359 [ # # ]: 0 : unit_vec(v, v_unit);
360 : :
361 : 0 : mtx3x3[0][0] = coeff1 + coeff2 * (v_unit[0] * v_unit[0]);
362 : 0 : mtx3x3[0][1] = coeff2 * v_unit[1] * v_unit[0] + coeff3 * v_unit[2];
363 : 0 : mtx3x3[0][2] = coeff2 * v_unit[2] * v_unit[0] - coeff3 * v_unit[1];
364 : :
365 : 0 : mtx3x3[1][0] = coeff2 * v_unit[1] * v_unit[0] - coeff3 * v_unit[2];
366 : 0 : mtx3x3[1][1] = coeff1 + coeff2 * (v_unit[1] * v_unit[1]);
367 : 0 : mtx3x3[1][2] = coeff2 * v_unit[1] * v_unit[2] + coeff3 * v_unit[0];
368 : :
369 : 0 : mtx3x3[2][0] = coeff2 * v_unit[2] * v_unit[0] + coeff3 * v_unit[1];
370 : 0 : mtx3x3[2][1] = coeff2 * v_unit[2] * v_unit[1] - coeff3 * v_unit[0];
371 : 0 : mtx3x3[2][2] = coeff1 + coeff2 * (v_unit[2] * v_unit[2]);
372 : : }
373 : :
374 : 0 : void AnalyticGeometryTool::create_rotation_mtx( double theta, double v[3],
375 : : double mtx4x4[4][4] )
376 : : {
377 : : double coeff1;
378 : : double coeff2;
379 : : double coeff3;
380 : : double v_unit[3];
381 : :
382 [ # # ]: 0 : if (!mtx4x4)
383 : 0 : return;
384 : :
385 : 0 : coeff1 = cos(theta);
386 : 0 : coeff2 = (1.0l - coeff1);
387 : 0 : coeff3 = sin(theta);
388 : :
389 [ # # ]: 0 : unit_vec(v, v_unit);
390 : :
391 : 0 : mtx4x4[0][0] = coeff1 + coeff2 * (v_unit[0] * v_unit[0]);
392 : 0 : mtx4x4[0][1] = coeff2 * v_unit[1] * v_unit[0] + coeff3 * v_unit[2];
393 : 0 : mtx4x4[0][2] = coeff2 * v_unit[2] * v_unit[0] - coeff3 * v_unit[1];
394 : :
395 : 0 : mtx4x4[1][0] = coeff2 * v_unit[1] * v_unit[0] - coeff3 * v_unit[2];
396 : 0 : mtx4x4[1][1] = coeff1 + coeff2 * (v_unit[1] * v_unit[1]);
397 : 0 : mtx4x4[1][2] = coeff2 * v_unit[1] * v_unit[2] + coeff3 * v_unit[0];
398 : :
399 : 0 : mtx4x4[2][0] = coeff2 * v_unit[2] * v_unit[0] + coeff3 * v_unit[1];
400 : 0 : mtx4x4[2][1] = coeff2 * v_unit[2] * v_unit[1] - coeff3 * v_unit[0];
401 : 0 : mtx4x4[2][2] = coeff1 + coeff2 * (v_unit[2] * v_unit[2]);
402 : :
403 : 0 : mtx4x4[0][3] = 0.0;
404 : 0 : mtx4x4[1][3] = 0.0;
405 : 0 : mtx4x4[2][3] = 0.0;
406 : 0 : mtx4x4[3][3] = 1.0;
407 : 0 : mtx4x4[3][0] = 0.0;
408 : 0 : mtx4x4[3][1] = 0.0;
409 : 0 : mtx4x4[3][2] = 0.0;
410 : : }
411 : :
412 : 0 : void AnalyticGeometryTool::add_origin_to_rotation_mtx( double rot_mtx[4][4],
413 : : double origin[3] )
414 : : {
415 : : double tmp_mtx[4][4];
416 : :
417 : : // Translate to origin
418 : : double t[4][4];
419 : 0 : memcpy(t, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
420 : : //PRINT_INFO( "Rotation matrix, before origin: \n" );
421 : : //print_mtx( rot_mtx );
422 : 0 : t[3][0]=-origin[0]; t[3][1]=-origin[1]; t[3][2]=-origin[2];
423 [ # # ]: 0 : mult_mtx( t, rot_mtx, tmp_mtx );
424 : :
425 : : //PRINT_INFO( "Origin times rotation: \n" );
426 : : //print_mtx( tmp_mtx );
427 : :
428 : : // Translate back
429 : 0 : t[3][0]=origin[0]; t[3][1]=origin[1]; t[3][2]=origin[2];
430 [ # # ]: 0 : mult_mtx( tmp_mtx, t, rot_mtx );
431 : :
432 : : //PRINT_INFO( "Rotation x -origin: \n" );
433 : : //print_mtx( rot_mtx );
434 : 0 : }
435 : :
436 : 0 : void AnalyticGeometryTool::identity_mtx( double mtx3x3[3][3] )
437 : : {
438 : 0 : memcpy(mtx3x3, AGT_IDENTITY_MTX_3X3, sizeof(double)*9);
439 : 0 : }
440 : :
441 : 0 : void AnalyticGeometryTool::identity_mtx( double mtx4x4[4][4] )
442 : : {
443 : 0 : memcpy(mtx4x4, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
444 : 0 : }
445 : :
446 : 0 : void AnalyticGeometryTool::mtx_to_angs( double mtx3x3[3][3],
447 : : double &ax, double &ay,
448 : : double &az )
449 : : {
450 : : // METHOD:
451 : : // o Rotate x-vector onto xz plane
452 : : // o Check xp dotted into y
453 : : // o If xp dot y is zero ==> az = 0 (x-vector already in xz plane)
454 : : // o Otherwise, compute rotation of vector into xz plane to acquire *az
455 : : // o Use atan2 (on x-vector) to get *az
456 : : // o Rotate the system about z
457 : : // o Use atan2 function (on x-vector in xz plane) to determine *ay
458 : : // o Rotate the system about y
459 : : // o Compute ax using y-vector
460 : : // o Resultant angles are negated (to reverse above procedure)
461 : :
462 : : double x[3]; // x-axis vector
463 : : double y[3]; // y-axis vector
464 : : double z[3]; // z-axis vector
465 : : double ar[3][3]; // Rotation matrix
466 : : double sinr,cosr; // Used for atan2 function
467 : : double work_sys[3][3]; // Temporary holder for system
468 : 0 : double *xp = work_sys[0]; // x-axis vector of system: x-primed
469 : 0 : double *yp = work_sys[1]; // y-axis vector of system: y-primed
470 : :
471 : 0 : x[0] = 1.0; x[1] = 0.0; x[2] = 0.0;
472 : 0 : y[0] = 0.0; y[1] = 1.0; y[2] = 0.0;
473 : 0 : z[0] = 0.0; z[1] = 0.0; z[2] = 1.0;
474 : :
475 [ # # ]: 0 : if (!mtx3x3)
476 : 0 : return;
477 : :
478 : : // Copy matrix over to work csys
479 [ # # ]: 0 : copy_mtx(mtx3x3,work_sys);
480 : :
481 : : // Check xp dotted into y
482 : : // If xp dot y is zero ==> az = 0
483 : :
484 : : // Otherwise, compute rotation of vector into xz plane to acquire *az
485 : :
486 [ # # ][ # # ]: 0 : if (dbl_eq(dot_vec(xp,y), 0.0))
[ # # ]
487 : :
488 : 0 : az = 0.0;
489 : :
490 : : else {
491 : :
492 : : /*
493 : : Compute *az - rotate xp to xz-plane about z-axis
494 : : y xp
495 : : | /
496 : : | / negative angle about z (negative of atan2)
497 : : -----x (use RH rule about z)
498 : : \
499 : : \ positive angle about z (negative of atan2)
500 : : xp
501 : : */
502 : :
503 [ # # ]: 0 : sinr = dot_vec(xp,y);
504 [ # # ]: 0 : cosr = dot_vec(xp,x);
505 : 0 : az = - atan2(sinr, cosr);
506 : :
507 : : // Rotate the system about z
508 [ # # ]: 0 : create_rotation_mtx(az,z,ar);
509 [ # # ]: 0 : rotate_system(ar,work_sys,work_sys);
510 : :
511 : : }
512 : :
513 : : /*
514 : : Compute *ay - rotate xp to x-axis about y-axis
515 : : z xp
516 : : | /
517 : : | / positive angle about y (positive of atan2)
518 : : -----x (use RH rule about y)
519 : : \
520 : : \ negative angle about y (positive of atan2)
521 : : xp
522 : : */
523 : :
524 [ # # ]: 0 : sinr = dot_vec(xp,z);
525 [ # # ]: 0 : cosr = dot_vec(xp,x);
526 : 0 : ay = atan2(sinr, cosr);
527 : :
528 : : // Rotate the system about y
529 [ # # ]: 0 : create_rotation_mtx(ay,y,ar);
530 [ # # ]: 0 : rotate_system(ar,work_sys,work_sys);
531 : :
532 : : /*
533 : : Compute *ax - rotate yp to y-axis about x-axis
534 : : z yp
535 : : | /
536 : : | / negative angle about x (negative of atan2)
537 : : -----y (use RH rule about x)
538 : : \
539 : : \ positive angle about x (negative of atan2)
540 : : yp
541 : : */
542 : :
543 [ # # ]: 0 : sinr = dot_vec(yp,z);
544 [ # # ]: 0 : cosr = dot_vec(yp,y);
545 : 0 : ax = atan2(sinr,cosr); // Negative of negative - see below
546 : :
547 : : // Negate above angles for rotation of the system back to original
548 : 0 : az = -(az);
549 : 0 : ay = -(ay);
550 : :
551 : : // Make sure near zero angles are actually zero
552 [ # # ][ # # ]: 0 : if (dbl_eq(ax, 0.0))
553 : 0 : ax = 0.0;
554 : :
555 [ # # ][ # # ]: 0 : if (dbl_eq(ay, 0.0))
556 : 0 : ay = 0.0;
557 : : }
558 : :
559 : 0 : void AnalyticGeometryTool::mtx_to_angs( double mtx4x4[4][4],
560 : : double &ax, double &ay,
561 : : double &az )
562 : : {
563 : : double work_sys[3][3];
564 : :
565 [ # # ]: 0 : if(!mtx4x4)
566 : 0 : return;
567 : :
568 [ # # ]: 0 : copy_mtx(mtx4x4,work_sys);
569 [ # # ]: 0 : mtx_to_angs( work_sys, ax, ay, az );
570 : : }
571 : :
572 : 0 : void AnalyticGeometryTool::rotate_system( double mtx[3][3],
573 : : double sys_in[3][3],
574 : : double sys_out[3][3] )
575 : : {
576 : : double sys_tmp[3][3];
577 : : double *p_sys_tmp;
578 : :
579 : : // Check to see if rotating in place
580 [ # # ]: 0 : if (sys_in == sys_out) {
581 [ # # ]: 0 : copy_mtx( sys_in, sys_tmp );
582 : 0 : p_sys_tmp = (double *)sys_tmp;
583 : : }
584 : : else
585 : : // Have sys_tmp point at outgoing memory location
586 : 0 : p_sys_tmp = (double *)sys_out;
587 : :
588 : :
589 : : // X-vector
590 : 0 : p_sys_tmp[0] = mtx[0][0] * sys_in[0][0]
591 : 0 : + mtx[1][0] * sys_in[0][1]
592 : 0 : + mtx[2][0] * sys_in[0][2];
593 : 0 : p_sys_tmp[1] = mtx[0][1] * sys_in[0][0]
594 : 0 : + mtx[1][1] * sys_in[0][1]
595 : 0 : + mtx[2][1] * sys_in[0][2];
596 : 0 : p_sys_tmp[2] = mtx[0][2] * sys_in[0][0]
597 : 0 : + mtx[1][2] * sys_in[0][1]
598 : 0 : + mtx[2][2] * sys_in[0][2];
599 : :
600 : : // Y-vector
601 : 0 : p_sys_tmp[3] = mtx[0][0] * sys_in[1][0]
602 : 0 : + mtx[1][0] * sys_in[1][1]
603 : 0 : + mtx[2][0] * sys_in[1][2];
604 : 0 : p_sys_tmp[4] = mtx[0][1] * sys_in[1][0]
605 : 0 : + mtx[1][1] * sys_in[1][1]
606 : 0 : + mtx[2][1] * sys_in[1][2];
607 : 0 : p_sys_tmp[5] = mtx[0][2] * sys_in[1][0]
608 : 0 : + mtx[1][2] * sys_in[1][1]
609 : 0 : + mtx[2][2] * sys_in[1][2];
610 : :
611 : : // Z-vector
612 : 0 : p_sys_tmp[6] = mtx[0][0] * sys_in[2][0]
613 : 0 : + mtx[1][0] * sys_in[2][1]
614 : 0 : + mtx[2][0] * sys_in[2][2];
615 : 0 : p_sys_tmp[7] = mtx[0][1] * sys_in[2][0]
616 : 0 : + mtx[1][1] * sys_in[2][1]
617 : 0 : + mtx[2][1] * sys_in[2][2];
618 : 0 : p_sys_tmp[8] = mtx[0][2] * sys_in[2][0]
619 : 0 : + mtx[1][2] * sys_in[2][1]
620 : 0 : + mtx[2][2] * sys_in[2][2];
621 : :
622 : : // Copy sys_tmp to sys_out if rotating in place
623 [ # # ]: 0 : if (sys_in == sys_out) {
624 : 0 : memcpy(sys_out, sys_tmp, sizeof(double)*9);
625 : : }
626 : 0 : }
627 : :
628 : 0 : void AnalyticGeometryTool::rotate_system( double mtx[4][4],
629 : : double sys_in[4][4],
630 : : double sys_out[4][4] )
631 : : {
632 : : double sys_tmp[4][4];
633 : : double* p_sys_tmp;
634 : :
635 : : // Check to see if rotating in place
636 [ # # ]: 0 : if (sys_in == sys_out) {
637 [ # # ]: 0 : copy_mtx( sys_in, sys_tmp );
638 : 0 : p_sys_tmp = (double *)sys_tmp;
639 : : }
640 : : else
641 : : // Have p_sys_tmp point at outgoing memory location
642 : 0 : p_sys_tmp = (double *)sys_out;
643 : :
644 : :
645 : : // X-vector
646 : 0 : p_sys_tmp[0] = mtx[0][0] * sys_in[0][0]
647 : 0 : + mtx[1][0] * sys_in[0][1]
648 : 0 : + mtx[2][0] * sys_in[0][2];
649 : 0 : p_sys_tmp[1] = mtx[0][1] * sys_in[0][0]
650 : 0 : + mtx[1][1] * sys_in[0][1]
651 : 0 : + mtx[2][1] * sys_in[0][2];
652 : 0 : p_sys_tmp[2] = mtx[0][2] * sys_in[0][0]
653 : 0 : + mtx[1][2] * sys_in[0][1]
654 : 0 : + mtx[2][2] * sys_in[0][2];
655 : :
656 : : // Y-vector
657 : 0 : p_sys_tmp[4] = mtx[0][0] * sys_in[1][0]
658 : 0 : + mtx[1][0] * sys_in[1][1]
659 : 0 : + mtx[2][0] * sys_in[1][2];
660 : 0 : p_sys_tmp[5] = mtx[0][1] * sys_in[1][0]
661 : 0 : + mtx[1][1] * sys_in[1][1]
662 : 0 : + mtx[2][1] * sys_in[1][2];
663 : 0 : p_sys_tmp[6] = mtx[0][2] * sys_in[1][0]
664 : 0 : + mtx[1][2] * sys_in[1][1]
665 : 0 : + mtx[2][2] * sys_in[1][2];
666 : :
667 : : // Z-vector
668 : 0 : p_sys_tmp[8] = mtx[0][0] * sys_in[2][0]
669 : 0 : + mtx[1][0] * sys_in[2][1]
670 : 0 : + mtx[2][0] * sys_in[2][2];
671 : 0 : p_sys_tmp[9] = mtx[0][1] * sys_in[2][0]
672 : 0 : + mtx[1][1] * sys_in[2][1]
673 : 0 : + mtx[2][1] * sys_in[2][2];
674 : 0 : p_sys_tmp[10] = mtx[0][2] * sys_in[2][0]
675 : 0 : + mtx[1][2] * sys_in[2][1]
676 : 0 : + mtx[2][2] * sys_in[2][2];
677 : :
678 : : // Maintain the origin
679 : 0 : p_sys_tmp[3] = sys_in[0][3];
680 : 0 : p_sys_tmp[7] = sys_in[1][3];
681 : 0 : p_sys_tmp[11] = sys_in[2][3];
682 : 0 : p_sys_tmp[15] = sys_in[3][3];
683 : 0 : p_sys_tmp[12] = sys_in[3][0];
684 : 0 : p_sys_tmp[13] = sys_in[3][1];
685 : 0 : p_sys_tmp[14] = sys_in[3][2];
686 : :
687 : : // Copy sys_tmp to sys_out if rotating in place
688 [ # # ]: 0 : if (sys_in == sys_out) {
689 : 0 : memcpy(sys_out, sys_tmp, sizeof(double)*16);
690 : : }
691 : 0 : }
692 : :
693 : 0 : double AnalyticGeometryTool::det_mtx( double m[3][3] )
694 : : {
695 : : double determinant;
696 : :
697 [ # # ]: 0 : if (!m)
698 : 0 : return (0.0);
699 : :
700 : 0 : determinant = m[0][0]*(m[1][1]*m[2][2]-m[1][2]*m[2][1])
701 : 0 : + m[0][1]*(m[1][2]*m[2][0]-m[1][0]*m[2][2])
702 : 0 : + m[0][2]*(m[1][0]*m[2][1]-m[1][1]*m[2][0]);
703 : :
704 : 0 : return (determinant);
705 : : }
706 : :
707 : 0 : void AnalyticGeometryTool::mult_mtx( double a[3][3],double b[3][3],
708 : : double d[3][3] )
709 : : {
710 : : double c[3][3]; // working buffer
711 : :
712 [ # # ][ # # ]: 0 : if( a != 0 && b != 0 ) { // a & b are valid
713 : :
714 : 0 : c[0][0] = ( a[0][0] * b[0][0] + a[0][1] * b[1][0]
715 : 0 : + a[0][2] * b[2][0]);
716 : 0 : c[1][0] = ( a[1][0] * b[0][0] + a[1][1] * b[1][0]
717 : 0 : + a[1][2] * b[2][0]);
718 : 0 : c[2][0] = ( a[2][0] * b[0][0] + a[2][1] * b[1][0]
719 : 0 : + a[2][2] * b[2][0]);
720 : :
721 : 0 : c[0][1] = ( a[0][0] * b[0][1] + a[0][1] * b[1][1]
722 : 0 : + a[0][2] * b[2][1]);
723 : 0 : c[1][1] = ( a[1][0] * b[0][1] + a[1][1] * b[1][1]
724 : 0 : + a[1][2] * b[2][1]);
725 : 0 : c[2][1] = ( a[2][0] * b[0][1] + a[2][1] * b[1][1]
726 : 0 : + a[2][2] * b[2][1]);
727 : :
728 : 0 : c[0][2] = ( a[0][0] * b[0][2] + a[0][1] * b[1][2]
729 : 0 : + a[0][2] * b[2][2]);
730 : 0 : c[1][2] = ( a[1][0] * b[0][2] + a[1][1] * b[1][2]
731 : 0 : + a[1][2] * b[2][2]);
732 : 0 : c[2][2] = ( a[2][0] * b[0][2] + a[2][1] * b[1][2]
733 : 0 : + a[2][2] * b[2][2]);
734 : :
735 [ # # ]: 0 : copy_mtx(c, d);
736 : :
737 : : }
738 [ # # ]: 0 : else if (a) { // b equals 0
739 [ # # ]: 0 : copy_mtx(a, d);
740 : : }
741 [ # # ]: 0 : else if (b) { // a equals 0
742 [ # # ]: 0 : copy_mtx(b, d);
743 : : }
744 : : else { // a & b equal 0
745 : :
746 [ # # ]: 0 : copy_mtx(AGT_IDENTITY_MTX_3X3, d);
747 : : }
748 : 0 : }
749 : :
750 : 0 : void AnalyticGeometryTool::mult_mtx( double a[4][4],
751 : : double b[4][4],
752 : : double d[4][4] )
753 : : {
754 : : double c[4][4]; // working buffer
755 : :
756 [ # # ][ # # ]: 0 : if( a != 0 && b != 0 ) { // a & b are valid
757 : :
758 : 0 : c[0][0] = ( a[0][0] * b[0][0] + a[0][1] * b[1][0]
759 : 0 : + a[0][2] * b[2][0] + a[0][3] * b[3][0]);
760 : 0 : c[1][0] = ( a[1][0] * b[0][0] + a[1][1] * b[1][0]
761 : 0 : + a[1][2] * b[2][0] + a[1][3] * b[3][0]);
762 : 0 : c[2][0] = ( a[2][0] * b[0][0] + a[2][1] * b[1][0]
763 : 0 : + a[2][2] * b[2][0] + a[2][3] * b[3][0]);
764 : 0 : c[3][0] = ( a[3][0] * b[0][0] + a[3][1] * b[1][0]
765 : 0 : + a[3][2] * b[2][0] + a[3][3] * b[3][0]);
766 : :
767 : 0 : c[0][1] = ( a[0][0] * b[0][1] + a[0][1] * b[1][1]
768 : 0 : + a[0][2] * b[2][1] + a[0][3] * b[3][1]);
769 : 0 : c[1][1] = ( a[1][0] * b[0][1] + a[1][1] * b[1][1]
770 : 0 : + a[1][2] * b[2][1] + a[1][3] * b[3][1]);
771 : 0 : c[2][1] = ( a[2][0] * b[0][1] + a[2][1] * b[1][1]
772 : 0 : + a[2][2] * b[2][1] + a[2][3] * b[3][1]);
773 : 0 : c[3][1] = ( a[3][0] * b[0][1] + a[3][1] * b[1][1]
774 : 0 : + a[3][2] * b[2][1] + a[3][3] * b[3][1]);
775 : :
776 : 0 : c[0][2] = ( a[0][0] * b[0][2] + a[0][1] * b[1][2]
777 : 0 : + a[0][2] * b[2][2] + a[0][3] * b[3][2]);
778 : 0 : c[1][2] = ( a[1][0] * b[0][2] + a[1][1] * b[1][2]
779 : 0 : + a[1][2] * b[2][2] + a[1][3] * b[3][2]);
780 : 0 : c[2][2] = ( a[2][0] * b[0][2] + a[2][1] * b[1][2]
781 : 0 : + a[2][2] * b[2][2] + a[2][3] * b[3][2]);
782 : 0 : c[3][2] = ( a[3][0] * b[0][2] + a[3][1] * b[1][2]
783 : 0 : + a[3][2] * b[2][2] + a[3][3] * b[3][2]);
784 : :
785 : 0 : c[0][3] = ( a[0][0] * b[0][3] + a[0][1] * b[1][3]
786 : 0 : + a[0][2] * b[2][3] + a[0][3] * b[3][3]);
787 : 0 : c[1][3] = ( a[1][0] * b[0][3] + a[1][1] * b[1][3]
788 : 0 : + a[1][2] * b[2][3] + a[1][3] * b[3][3]);
789 : 0 : c[2][3] = ( a[2][0] * b[0][3] + a[2][1] * b[1][3]
790 : 0 : + a[2][2] * b[2][3] + a[2][3] * b[3][3]);
791 : 0 : c[3][3] = ( a[3][0] * b[0][3] + a[3][1] * b[1][3]
792 : 0 : + a[3][2] * b[2][3] + a[3][3] * b[3][3]);
793 : :
794 [ # # ]: 0 : copy_mtx(c, d);
795 : : }
796 [ # # ]: 0 : else if (a) { // b equals 0
797 [ # # ]: 0 : copy_mtx(a, d);
798 : : }
799 [ # # ]: 0 : else if (b) { // a equals 0
800 [ # # ]: 0 : copy_mtx(b, d);
801 : : }
802 : : else { // a & b equal 0
803 : :
804 [ # # ]: 0 : copy_mtx(AGT_IDENTITY_MTX_4X4, d);
805 : : }
806 : 0 : }
807 : :
808 : 0 : CubitStatus AnalyticGeometryTool::inv_mtx_adj( double mtx[3][3],
809 : : double inv_mtx[3][3] )
810 : : {
811 : : int i,i1,i2,j,j1,j2;
812 : : double work_mtx[3][3];
813 : : double determinant;
814 : :
815 : : // Check for null input
816 [ # # ]: 0 : if (!mtx) {
817 [ # # ]: 0 : copy_mtx(AGT_IDENTITY_MTX_3X3, inv_mtx);
818 : 0 : return CUBIT_SUCCESS;
819 : : }
820 : :
821 : : // Calculate determinant
822 [ # # ]: 0 : determinant = det_mtx(mtx);
823 : :
824 : : // Check for singularity
825 [ # # ][ # # ]: 0 : if (dbl_eq(determinant,0.0))
826 : 0 : return CUBIT_FAILURE;
827 : :
828 : : // Get work matrix (allow inverting in place)
829 [ # # ]: 0 : copy_mtx(mtx, work_mtx);
830 : :
831 : : // Inverse is adjoint matrix divided by determinant
832 [ # # ]: 0 : for (i=1; i<4; i++) {
833 : :
834 : 0 : i1 = (i % 3) + 1; i2 = (i1 % 3) + 1;
835 : :
836 [ # # ]: 0 : for (j=1; j<4; j++) {
837 : :
838 : 0 : j1 = (j % 3) + 1; j2 = (j1 % 3) + 1;
839 : :
840 : 0 : inv_mtx[j-1][i-1] = (work_mtx[i1-1][j1-1]*work_mtx[i2-1][j2-1] -
841 : 0 : work_mtx[i1-1][j2-1]*work_mtx[i2-1][j1-1])
842 : 0 : / determinant;
843 : :
844 : : }
845 : : }
846 : 0 : return CUBIT_SUCCESS;
847 : : }
848 : :
849 : 0 : CubitStatus AnalyticGeometryTool::inv_trans_mtx( double transf[4][4],
850 : : double inv_transf[4][4] )
851 : : {
852 : : double scale_sq;
853 : : double inv_sq_scale;
854 : : double tmp_transf[4][4]; // For temporary storage of incoming matrix
855 : 0 : double *p_tmp_transf = NULL;
856 : :
857 : : // If input transform is 0 set output to identity matrix
858 [ # # ]: 0 : if (!transf) {
859 [ # # ]: 0 : copy_mtx( AGT_IDENTITY_MTX_4X4, inv_transf );
860 : 0 : return CUBIT_SUCCESS;
861 : : }
862 : :
863 : : // Obtain the matrix scale
864 : 0 : scale_sq = transf[0][0]*transf[0][0] + transf[0][1]*transf[0][1] +
865 : 0 : transf[0][2]*transf[0][2];
866 : :
867 : : // Check for singular matrix
868 [ # # ]: 0 : if (scale_sq < (.000000001 * .000000001))
869 : 0 : return CUBIT_FAILURE;
870 : :
871 : : // Need the inverse scale squared
872 : 0 : inv_sq_scale = 1.0 / scale_sq;
873 : :
874 : : // Check to see if inverting in place
875 [ # # ]: 0 : if (transf == inv_transf) {
876 [ # # ]: 0 : copy_mtx( transf, tmp_transf );
877 : 0 : p_tmp_transf = (double *)tmp_transf;
878 : : }
879 : : else
880 : 0 : p_tmp_transf = (double *)transf;
881 : :
882 : : // The X vector
883 : 0 : inv_transf[0][0] = p_tmp_transf[0] * inv_sq_scale;
884 : 0 : inv_transf[1][0] = p_tmp_transf[1] * inv_sq_scale;
885 : 0 : inv_transf[2][0] = p_tmp_transf[2] * inv_sq_scale;
886 : :
887 : : // The Y vector
888 : 0 : inv_transf[0][1] = p_tmp_transf[4] * inv_sq_scale;
889 : 0 : inv_transf[1][1] = p_tmp_transf[5] * inv_sq_scale;
890 : 0 : inv_transf[2][1] = p_tmp_transf[6] * inv_sq_scale;
891 : :
892 : : // The Z vector
893 : 0 : inv_transf[0][2] = p_tmp_transf[8] * inv_sq_scale;
894 : 0 : inv_transf[1][2] = p_tmp_transf[9] * inv_sq_scale;
895 : 0 : inv_transf[2][2] = p_tmp_transf[10] * inv_sq_scale;
896 : :
897 : : // Column 4
898 : 0 : inv_transf[0][3] = 0.0;
899 : 0 : inv_transf[1][3] = 0.0;
900 : 0 : inv_transf[2][3] = 0.0;
901 : :
902 : : // The X origin
903 : 0 : inv_transf[3][0] = -inv_sq_scale * ( p_tmp_transf[0] * p_tmp_transf[12]
904 : 0 : + p_tmp_transf[1] * p_tmp_transf[13]
905 : 0 : + p_tmp_transf[2] * p_tmp_transf[14]);
906 : :
907 : : // The Y origin
908 : 0 : inv_transf[3][1] = -inv_sq_scale * ( p_tmp_transf[4] * p_tmp_transf[12]
909 : 0 : + p_tmp_transf[5] * p_tmp_transf[13]
910 : 0 : + p_tmp_transf[6] * p_tmp_transf[14]);
911 : :
912 : : // The Z origin
913 : 0 : inv_transf[3][2] = -inv_sq_scale * ( p_tmp_transf[8] * p_tmp_transf[12]
914 : 0 : + p_tmp_transf[9] * p_tmp_transf[13]
915 : 0 : + p_tmp_transf[10] * p_tmp_transf[14]);
916 : :
917 : : // This is always one
918 : 0 : inv_transf[3][3] = 1.0;
919 : :
920 : 0 : return CUBIT_SUCCESS;
921 : : }
922 : :
923 : 0 : void AnalyticGeometryTool::vecs_to_mtx( double xvec[3],
924 : : double yvec[3],
925 : : double zvec[3],
926 : : double matrix[3][3] )
927 : : {
928 [ # # ]: 0 : if (xvec)
929 : 0 : copy_pnt(xvec, matrix[0]);
930 : : else
931 : 0 : copy_pnt(AGT_IDENTITY_MTX_3X3[0], matrix[0]);
932 : :
933 [ # # ]: 0 : if (yvec)
934 : 0 : copy_pnt(yvec, matrix[1]);
935 : : else
936 : 0 : copy_pnt(AGT_IDENTITY_MTX_3X3[1], matrix[1]);
937 : :
938 [ # # ]: 0 : if (zvec)
939 : 0 : copy_pnt(zvec, matrix[2]);
940 : : else
941 : 0 : copy_pnt(AGT_IDENTITY_MTX_3X3[2], matrix[2]);
942 : 0 : }
943 : :
944 : 0 : void AnalyticGeometryTool::vecs_to_mtx( double xvec[3],
945 : : double yvec[3],
946 : : double zvec[3],
947 : : double origin[3],
948 : : double matrix[4][4] )
949 : : {
950 [ # # ]: 0 : if (xvec)
951 : 0 : copy_pnt(xvec, matrix[0]);
952 : : else
953 : 0 : copy_pnt(AGT_IDENTITY_MTX_3X3[0], matrix[0]);
954 : :
955 [ # # ]: 0 : if (yvec)
956 : 0 : copy_pnt(yvec, matrix[1]);
957 : : else
958 : 0 : copy_pnt(AGT_IDENTITY_MTX_3X3[1], matrix[1]);
959 : :
960 [ # # ]: 0 : if (zvec)
961 : 0 : copy_pnt(zvec, matrix[2]);
962 : : else
963 : 0 : copy_pnt(AGT_IDENTITY_MTX_3X3[2], matrix[2]);
964 : :
965 [ # # ]: 0 : if( origin )
966 : 0 : copy_pnt(origin, matrix[3]);
967 : : else
968 : : {
969 : 0 : matrix[3][0] = 0.0;
970 : 0 : matrix[3][1] = 0.0;
971 : 0 : matrix[3][2] = 0.0;
972 : : }
973 : :
974 : 0 : matrix[0][3] = 0.0;
975 : 0 : matrix[1][3] = 0.0;
976 : 0 : matrix[2][3] = 0.0;
977 : 0 : matrix[3][3] = 1.0;
978 : 0 : }
979 : :
980 : 0 : void AnalyticGeometryTool::print_mtx( double mtx[3][3] )
981 : : {
982 [ # # ][ # # ]: 0 : PRINT_INFO( "%f %f %f\n", mtx[0][0], mtx[0][1], mtx[0][2] );
983 [ # # ][ # # ]: 0 : PRINT_INFO( "%f %f %f\n", mtx[1][0], mtx[1][1], mtx[1][2] );
984 [ # # ][ # # ]: 0 : PRINT_INFO( "%f %f %f\n", mtx[2][0], mtx[2][1], mtx[2][2] );
985 : 0 : }
986 : :
987 : 0 : void AnalyticGeometryTool::print_mtx( double mtx[4][4] )
988 : : {
989 [ # # ][ # # ]: 0 : PRINT_INFO( "%f %f %f %f\n", mtx[0][0], mtx[0][1], mtx[0][2], mtx[0][3] );
990 [ # # ][ # # ]: 0 : PRINT_INFO( "%f %f %f %f\n", mtx[1][0], mtx[1][1], mtx[1][2], mtx[1][3] );
991 [ # # ][ # # ]: 0 : PRINT_INFO( "%f %f %f %f\n", mtx[2][0], mtx[2][1], mtx[2][2], mtx[2][3] );
992 [ # # ][ # # ]: 0 : PRINT_INFO( "%f %f %f %f\n", mtx[3][0], mtx[3][1], mtx[3][2], mtx[3][3] );
993 : 0 : }
994 : :
995 : : //***************************************************************************
996 : : // 3D Points
997 : : //***************************************************************************
998 : 0 : void AnalyticGeometryTool::copy_pnt( double pnt_in[3], double pnt_out[3] )
999 : : {
1000 [ # # ]: 0 : if (pnt_in == pnt_out)
1001 : 0 : return;
1002 : :
1003 [ # # ]: 0 : if (pnt_out == NULL)
1004 : 0 : return;
1005 : :
1006 [ # # ]: 0 : if (pnt_in == NULL) {
1007 : 0 : pnt_out[0] = 0.0;
1008 : 0 : pnt_out[1] = 0.0;
1009 : 0 : pnt_out[2] = 0.0;
1010 : 0 : return;
1011 : : }
1012 : :
1013 : : // Simply copy first point into second point
1014 : 0 : memcpy(pnt_out, pnt_in, sizeof(double)*3);
1015 : : }
1016 : :
1017 : 0 : void AnalyticGeometryTool::copy_pnt( double pnt_in[3], CubitVector &cubit_vec )
1018 : : {
1019 : 0 : cubit_vec.set( pnt_in );
1020 : 0 : }
1021 : :
1022 : 0 : void AnalyticGeometryTool::copy_pnt( CubitVector &cubit_vec, double pnt_out[3] )
1023 : : {
1024 : 0 : pnt_out[0] = cubit_vec.x();
1025 : 0 : pnt_out[1] = cubit_vec.y();
1026 : 0 : pnt_out[2] = cubit_vec.z();
1027 : 0 : }
1028 : :
1029 : :
1030 : 0 : CubitBoolean AnalyticGeometryTool::pnt_eq( double pnt1[3],double pnt2[3] )
1031 : : {
1032 : 0 : double x = pnt2[0] - pnt1[0]; // difference in the x direction
1033 : 0 : double y = pnt2[1] - pnt1[1]; // difference in the y direction
1034 : 0 : double z = pnt2[2] - pnt1[2]; // difference in the z direction
1035 : :
1036 : 0 : return (dbl_eq(sqrt(x*x + y*y + z*z), 0.0));
1037 : : }
1038 : :
1039 : :
1040 : 0 : CubitStatus AnalyticGeometryTool::mirror_pnt( double pnt[3],
1041 : : double pln_orig[3],
1042 : : double pln_norm[3],
1043 : : double m_pnt[3])
1044 : : {
1045 : : double int_pnt[3],
1046 : : vec[3];
1047 : :
1048 : : // Find intersection of point and plane
1049 [ # # ][ # # ]: 0 : if (int_pnt_pln(pnt, pln_orig, pln_norm, int_pnt)) {
1050 : : // If intersection is on the plane, return
1051 [ # # ]: 0 : copy_pnt(pnt, m_pnt);
1052 : 0 : return CUBIT_FAILURE;
1053 : : }
1054 : :
1055 : : // Find vector from pnt to int_pnt
1056 [ # # ]: 0 : get_vec(pnt, int_pnt, vec);
1057 : :
1058 : : // Traverse twice the length of vec in vec direction
1059 : 0 : m_pnt[0] = pnt[0] + 2.0 * vec[0];
1060 : 0 : m_pnt[1] = pnt[1] + 2.0 * vec[1];
1061 : 0 : m_pnt[2] = pnt[2] + 2.0 * vec[2];
1062 : :
1063 : 0 : return CUBIT_SUCCESS;
1064 : : }
1065 : :
1066 : :
1067 : 0 : CubitStatus AnalyticGeometryTool::next_pnt( double str_pnt[3],
1068 : : double vec_dir[3],
1069 : : double len,
1070 : : double new_pnt[3])
1071 : : {
1072 : : double uv[3]; // unit vector representation of vector direction
1073 : :
1074 : : // unitize specified direction
1075 [ # # ][ # # ]: 0 : if (!unit_vec(vec_dir,uv)) {
1076 [ # # ]: 0 : copy_pnt(str_pnt, new_pnt);
1077 : 0 : return CUBIT_FAILURE;
1078 : : }
1079 : :
1080 : : // determine next point in space
1081 : :
1082 : 0 : new_pnt[0] = str_pnt[0] + (len * uv[0]);
1083 : 0 : new_pnt[1] = str_pnt[1] + (len * uv[1]);
1084 : 0 : new_pnt[2] = str_pnt[2] + (len * uv[2]);
1085 : :
1086 : 0 : return CUBIT_SUCCESS;
1087 : : }
1088 : :
1089 : :
1090 : : //***************************************************************************
1091 : : // 3D Vectors
1092 : : //***************************************************************************
1093 : 0 : CubitStatus AnalyticGeometryTool::unit_vec( double vin[3], double vout[3] )
1094 : : {
1095 : : double rmag; // holds magnitude of vector
1096 : :
1097 : : // Calculate vector magnitude
1098 : 0 : rmag = sqrt(vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2]);
1099 : :
1100 : : // check if vector has a magnitude - catch division by zero
1101 : :
1102 [ # # ]: 0 : if (dbl_eq(rmag, 0.0)) {
1103 [ # # ]: 0 : if (vin != vout)
1104 : 0 : copy_pnt(vin, vout);
1105 : 0 : return CUBIT_FAILURE;
1106 : : }
1107 : :
1108 : : // divide each element of the vector by the magnitude
1109 : :
1110 : 0 : vout[0] = vin[0] / rmag;
1111 : 0 : vout[1] = vin[1] / rmag;
1112 : 0 : vout[2] = vin[2] / rmag;
1113 : :
1114 : 0 : return CUBIT_SUCCESS;
1115 : : }
1116 : :
1117 : 0 : double AnalyticGeometryTool::dot_vec( double uval[3], double vval[3] )
1118 : : {
1119 : : double dot_val;
1120 : :
1121 : : // perform dot calculation = v[0]*u[0] + v[1]*u[1] + v[1]*u[1]
1122 : :
1123 : 0 : dot_val = uval[0]*vval[0] + uval[1]*vval[1] + uval[2]*vval[2];
1124 : :
1125 : 0 : return(dot_val);
1126 : : }
1127 : :
1128 : 0 : void AnalyticGeometryTool::cross_vec( double uval[3], double vval[3],
1129 : : double cross[3] )
1130 : : {
1131 : : // determine cross product of the two vectors
1132 : :
1133 : 0 : cross[0] = uval[1] * vval[2] - uval[2] * vval[1];
1134 : 0 : cross[1] = uval[2] * vval[0] - uval[0] * vval[2];
1135 : 0 : cross[2] = uval[0] * vval[1] - uval[1] * vval[0];
1136 : 0 : }
1137 : :
1138 : 0 : void AnalyticGeometryTool::cross_vec_unit( double uval[3], double vval[3],
1139 : : double cross[3] )
1140 : : {
1141 : : // determine cross product of the two vectors
1142 : 0 : cross_vec(uval,vval,cross);
1143 : :
1144 : : // convert to unit vector
1145 : 0 : unit_vec(cross,cross);
1146 : 0 : }
1147 : :
1148 : 0 : void AnalyticGeometryTool::orth_vecs( double uvect[3], double vvect[3],
1149 : : double wvect[3] )
1150 : : {
1151 : : double x[3];
1152 : 0 : unsigned short i = 0;
1153 : 0 : unsigned short imin = 3;
1154 : 0 : double rmin = 1.0E20;
1155 : : unsigned short iperm1[3];
1156 : : unsigned short iperm2[3];
1157 : 0 : unsigned short cont_flag = 1;
1158 : : double vec[3];
1159 : :
1160 : : // Initialize perm flags
1161 : 0 : iperm1[0] = 1; iperm1[1] = 2; iperm1[2] = 0;
1162 : 0 : iperm2[0] = 2; iperm2[1] = 0; iperm2[2] = 1;
1163 : :
1164 : : // unitize vector
1165 : :
1166 [ # # ]: 0 : unit_vec(uvect,vec);
1167 : :
1168 [ # # ][ # # ]: 0 : while (i<3 && cont_flag ) {
1169 [ # # ][ # # ]: 0 : if (dbl_eq(vec[i], 0.0)) {
1170 : 0 : vvect[i] = 1.0;
1171 : 0 : vvect[iperm1[i]] = 0.0;
1172 : 0 : vvect[iperm2[i]] = 0.0;
1173 : 0 : cont_flag = 0;
1174 : : }
1175 : :
1176 [ # # ]: 0 : if (fabs(vec[i]) < rmin) {
1177 : 0 : imin = i;
1178 : 0 : rmin = fabs(vec[i]);
1179 : : }
1180 : 0 : ++i;
1181 : : }
1182 : :
1183 [ # # ]: 0 : if (cont_flag) {
1184 : 0 : x[imin] = 1.0;
1185 : 0 : x[iperm1[imin]] = 0.0;
1186 : 0 : x[iperm2[imin]] = 0.0;
1187 : :
1188 : : // determine cross product
1189 [ # # ]: 0 : cross_vec_unit(vec,x,vvect);
1190 : : }
1191 : :
1192 : : // cross vector to determine last orthogonal vector
1193 [ # # ]: 0 : cross_vec(vvect,vec,wvect);
1194 : 0 : }
1195 : :
1196 : 0 : double AnalyticGeometryTool::mag_vec( double vec[3] )
1197 : : {
1198 : 0 : return (sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]));
1199 : : }
1200 : :
1201 : 0 : CubitStatus AnalyticGeometryTool::get_vec ( double str_pnt[3],
1202 : : double stp_pnt[3],
1203 : : double vector_out[3] )
1204 : : {
1205 : : // Make sure we can create a vector
1206 [ # # ]: 0 : if (pnt_eq(str_pnt, stp_pnt)) {
1207 : 0 : vector_out[0] = 0.0;
1208 : 0 : vector_out[1] = 0.0;
1209 : 0 : vector_out[2] = 0.0;
1210 : 0 : return CUBIT_FAILURE;
1211 : : }
1212 : :
1213 : : // determine vector by subtracting starting point from stopping point
1214 : :
1215 : 0 : vector_out[0] = stp_pnt[0] - str_pnt[0];
1216 : 0 : vector_out[1] = stp_pnt[1] - str_pnt[1];
1217 : 0 : vector_out[2] = stp_pnt[2] - str_pnt[2];
1218 : :
1219 : 0 : return CUBIT_SUCCESS;
1220 : : }
1221 : :
1222 : 0 : CubitStatus AnalyticGeometryTool::get_vec_unit( double str_pnt[3],
1223 : : double stp_pnt[3],
1224 : : double uv_out[3] )
1225 : : {
1226 : : // determine vector between points
1227 [ # # ]: 0 : if (!get_vec(str_pnt,stp_pnt,uv_out))
1228 : 0 : return CUBIT_FAILURE;
1229 : :
1230 : : // unitize vector
1231 [ # # ]: 0 : if (!unit_vec(uv_out,uv_out))
1232 : 0 : return CUBIT_FAILURE;
1233 : :
1234 : 0 : return CUBIT_SUCCESS;
1235 : : }
1236 : :
1237 : 0 : void AnalyticGeometryTool::mult_vecxconst( double constant,
1238 : : double vec[3],
1239 : : double vec_out[3] )
1240 : : {
1241 : : // multiply each element of the vector by the constant
1242 : 0 : vec_out[0] = constant * vec[0];
1243 : 0 : vec_out[1] = constant * vec[1];
1244 : 0 : vec_out[2] = constant * vec[2];
1245 : 0 : }
1246 : :
1247 : :
1248 : 0 : void AnalyticGeometryTool::reverse_vec( double vin[3],double vout[3] )
1249 : : {
1250 : : // Multiply the vector components by a -1.0
1251 : 0 : mult_vecxconst(-1.0, vin, vout);
1252 : 0 : }
1253 : :
1254 : 0 : double AnalyticGeometryTool::angle_vec_vec( double v1[3],double v2[3] )
1255 : : {
1256 : : double denom, dot, cosang, sinang, acrsb, angle;
1257 : : double crossed_vec[3];
1258 : :
1259 : : // For accuracy, use cosine for large angles, sine for small angles
1260 : 0 : denom = sqrt((v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2])
1261 : 0 : *(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]));
1262 : :
1263 : : // Check for a zero length vector
1264 [ # # ][ # # ]: 0 : if (dbl_eq(denom, 0.0))
1265 : 0 : return (0.0);
1266 : :
1267 [ # # ]: 0 : dot = dot_vec(v1, v2);
1268 : :
1269 : 0 : cosang = dot/denom;
1270 : :
1271 [ # # ]: 0 : if (1.0 - fabs(cosang) < 0.01) {
1272 [ # # ]: 0 : cross_vec(v1, v2, crossed_vec);
1273 [ # # ]: 0 : acrsb = mag_vec(crossed_vec);
1274 : 0 : sinang = acrsb/denom;
1275 [ # # ]: 0 : if (cosang > 0.0)
1276 : 0 : angle = asin(sinang);
1277 : : else
1278 : 0 : angle = AGT_PI - asin(sinang);
1279 : : }
1280 : : else
1281 : 0 : angle = acos(cosang);
1282 : :
1283 : 0 : return (angle);
1284 : : }
1285 : :
1286 : : //***************************************************************************
1287 : : // Distances
1288 : : //***************************************************************************
1289 : 0 : double AnalyticGeometryTool::dist_pnt_pnt( double pnt1[3], double pnt2[3] )
1290 : : {
1291 : 0 : double x = pnt2[0] - pnt1[0]; // difference in the x direction
1292 : 0 : double y = pnt2[1] - pnt1[1]; // difference in the y direction
1293 : 0 : double z = pnt2[2] - pnt1[2]; // difference in the z direction
1294 : :
1295 : : // return the distance
1296 : 0 : return(sqrt(x*x + y*y + z*z));
1297 : : }
1298 : :
1299 : :
1300 : :
1301 : 0 : double AnalyticGeometryTool::dist_pln_pln( double pln_1_orig[3],
1302 : : double pln_1_norm[3],
1303 : : double pln_2_orig[3],
1304 : : double pln_2_norm[3],
1305 : : AgtSide *side,
1306 : : AgtOrientation *orien,
1307 : : unsigned short *status )
1308 : : {
1309 : : double distance;
1310 : : double int_pnt[3];
1311 : : double vec[3];
1312 : :
1313 : : // Check to see if planes are parallel
1314 [ # # ][ # # ]: 0 : if (is_vec_par(pln_1_norm, pln_2_norm)) {
1315 : :
1316 : : // Set successful status
1317 [ # # ]: 0 : if (status)
1318 : 0 : *status = 1;
1319 : :
1320 : : // Calculate perpendicular line plane intersection on plane_2 from
1321 : : // pln_1_origin
1322 : : int_ln_pln(pln_1_orig, pln_1_norm, pln_2_orig, pln_2_norm,
1323 [ # # ]: 0 : int_pnt);
1324 : :
1325 : : // Find distance between pln_1_origin and this intersection pnt
1326 [ # # ]: 0 : distance = dist_pnt_pnt(pln_1_orig, int_pnt);
1327 : :
1328 : : // Get side if required
1329 [ # # ]: 0 : if (side) {
1330 : :
1331 [ # # ][ # # ]: 0 : if (dbl_eq(distance, 0.0))
1332 : :
1333 : 0 : *side = AGT_ON_PLANE;
1334 : :
1335 : : else {
1336 : :
1337 : : // Get vector to intersection point
1338 [ # # ]: 0 : get_vec(pln_1_orig, int_pnt, vec);
1339 : :
1340 : : // Compare angles
1341 [ # # ][ # # ]: 0 : if (dbl_eq(angle_vec_vec(vec, pln_1_norm), 0.0))
[ # # ]
1342 : 0 : *side = AGT_POS_SIDE;
1343 : : else
1344 : 0 : *side = AGT_NEG_SIDE;
1345 : :
1346 : : }
1347 : : }
1348 : :
1349 : : // Get orientation if required
1350 [ # # ]: 0 : if (orien) {
1351 : :
1352 : : // Compare surface normals
1353 [ # # ][ # # ]: 0 : if (dbl_eq(angle_vec_vec(pln_1_norm, pln_2_norm), 0.0))
[ # # ]
1354 : 0 : *orien = AGT_SAME_DIR;
1355 : : else
1356 : 0 : *orien = AGT_OPP_DIR;
1357 : : }
1358 : :
1359 : : }
1360 : :
1361 : : else {
1362 : :
1363 [ # # ]: 0 : if (status)
1364 : 0 : *status = 0;
1365 : :
1366 [ # # ]: 0 : if (side)
1367 : 0 : *side = AGT_CROSS;
1368 : :
1369 : 0 : distance = 0.0;
1370 : :
1371 : : }
1372 : :
1373 : 0 : return (distance);
1374 : : }
1375 : :
1376 : :
1377 : :
1378 : : //***************************************************************************
1379 : : // Intersections
1380 : : //***************************************************************************
1381 : 0 : CubitStatus AnalyticGeometryTool::int_ln_pln( double ln_orig[3],
1382 : : double ln_vec[3],
1383 : : double pln_orig[3],
1384 : : double pln_norm[3],
1385 : : double int_pnt[3] )
1386 : : {
1387 : : double denom;
1388 : : double t;
1389 : :
1390 : : // Set parametric eqns of line equal to parametric eqn of plane & solve
1391 : : // for t
1392 : 0 : denom = pln_norm[0]*ln_vec[0] + pln_norm[1]*ln_vec[1] +
1393 : 0 : pln_norm[2]*ln_vec[2];
1394 : :
1395 [ # # ]: 0 : if (dbl_eq(denom, 0.0))
1396 : 0 : return CUBIT_FAILURE;
1397 : :
1398 : 0 : t = (pln_norm[0]*(pln_orig[0]-ln_orig[0]) +
1399 : 0 : pln_norm[1]*(pln_orig[1]-ln_orig[1]) +
1400 : 0 : pln_norm[2]*(pln_orig[2]-ln_orig[2]))/denom;
1401 : :
1402 : : // Substitute t back into equations of line to get xyz
1403 : 0 : int_pnt[0] = ln_orig[0] + ln_vec[0]*t;
1404 : 0 : int_pnt[1] = ln_orig[1] + ln_vec[1]*t;
1405 : 0 : int_pnt[2] = ln_orig[2] + ln_vec[2]*t;
1406 : :
1407 : 0 : return CUBIT_SUCCESS;
1408 : : }
1409 : :
1410 : 0 : int AnalyticGeometryTool::int_ln_ln( double p1[3], double v1[3],
1411 : : double p2[3], double v2[3],
1412 : : double int_pnt1[3], double int_pnt2[3] )
1413 : : {
1414 : : double norm[3];
1415 : : double pln1_norm[3];
1416 : : double pln2_norm[3];
1417 : :
1418 : : // Cross the two vectors to get a normal vector
1419 [ # # ]: 0 : cross_vec(v1, v2, norm);
1420 : :
1421 [ # # ][ # # ]: 0 : if (dbl_eq(mag_vec(norm), 0.0))
[ # # ]
1422 : 0 : return 0;
1423 : :
1424 : : // Cross v2 & normal to get normal to plane 2
1425 [ # # ]: 0 : cross_vec(v2, norm, pln2_norm);
1426 : :
1427 : : // Find intersection of pln2_norm and vector 1 - this is int_pnt1
1428 [ # # ]: 0 : int_ln_pln(p1, v1, p2, pln2_norm, int_pnt1);
1429 : :
1430 : : // Cross v1 & normal to get normal to plane 1
1431 [ # # ]: 0 : cross_vec(v1, norm, pln1_norm);
1432 : :
1433 : : // Find intersection of pln2_norm and vector 1 - this is int_pnt2
1434 [ # # ]: 0 : int_ln_pln(p2, v2, p1, pln1_norm, int_pnt2);
1435 : :
1436 : : // Check to see if the intersection points are the same & return
1437 [ # # ][ # # ]: 0 : if (pnt_eq(int_pnt1, int_pnt2))
1438 : 0 : return 1;
1439 : : else
1440 : 0 : return 2;
1441 : : }
1442 : :
1443 : :
1444 : 0 : int AnalyticGeometryTool::int_pnt_pln( double pnt[3],
1445 : : double pln_orig[3],
1446 : : double pln_norm[3],
1447 : : double pln_int[3] )
1448 : : {
1449 : : // Calculate line plane intersection w/plane normal as line vector
1450 : 0 : int_ln_pln(pnt, pln_norm, pln_orig, pln_norm, pln_int);
1451 : :
1452 : : // Check to see if point is on the plane
1453 [ # # ]: 0 : if (pnt_eq(pln_int, pnt))
1454 : 0 : return 1;
1455 : : else
1456 : 0 : return 0;
1457 : : }
1458 : :
1459 : :
1460 : :
1461 : : //***************************************************************************
1462 : : // Comparison/Containment Tests
1463 : : //***************************************************************************
1464 : 0 : CubitBoolean AnalyticGeometryTool::is_vec_par( double vec_1[3],
1465 : : double vec_2[3] )
1466 : : {
1467 : : double cross[3];
1468 : :
1469 : : // Get cross product & see if its magnitude is zero
1470 [ # # ]: 0 : cross_vec(vec_1, vec_2, cross);
1471 : :
1472 [ # # ][ # # ]: 0 : if (dbl_eq(mag_vec(cross), 0.0))
[ # # ]
1473 : 0 : return CUBIT_TRUE;
1474 : : else
1475 : 0 : return CUBIT_FALSE;
1476 : : }
1477 : :
1478 : 0 : CubitBoolean AnalyticGeometryTool::is_vec_perp( double vec_1[3],double vec_2[3])
1479 : : {
1480 : : // Check angle between vectors
1481 [ # # ]: 0 : if (dbl_eq(angle_vec_vec(vec_1, vec_2), AGT_PI_DIV_2))
1482 : 0 : return CUBIT_TRUE;
1483 : : else
1484 : 0 : return CUBIT_FALSE;
1485 : : }
1486 : :
1487 : 0 : CubitBoolean AnalyticGeometryTool::is_vecs_same_dir( double vec_1[3],
1488 : : double vec_2[3] )
1489 : : {
1490 : : // Check to see if angle between vectors can be considered zero
1491 [ # # ]: 0 : if (dbl_eq(angle_vec_vec(vec_1, vec_2), 0.0))
1492 : 0 : return CUBIT_TRUE;
1493 : : else
1494 : 0 : return CUBIT_FALSE;
1495 : : }
1496 : :
1497 : :
1498 : 0 : CubitBoolean AnalyticGeometryTool::is_pnt_on_ln( double pnt[3],
1499 : : double ln_orig[3],
1500 : : double ln_vec[3] )
1501 : : {
1502 : : double vec[3];
1503 : :
1504 : : // Compare pnt and line origin
1505 [ # # ][ # # ]: 0 : if (pnt_eq(pnt, ln_orig))
1506 : 0 : return CUBIT_TRUE;
1507 : :
1508 : : // Get a vector from line origin to the point
1509 [ # # ]: 0 : get_vec(ln_orig, pnt, vec);
1510 : :
1511 : : // If this vector is parallel with line vector, point is on the line
1512 [ # # ][ # # ]: 0 : if (is_vec_par(vec, ln_vec))
1513 : 0 : return CUBIT_TRUE;
1514 : : else
1515 : 0 : return CUBIT_FALSE;
1516 : : }
1517 : :
1518 : 0 : CubitBoolean AnalyticGeometryTool::is_pnt_on_ln_seg( double pnt[3],
1519 : : double end1[3],
1520 : : double end2[3] )
1521 : : {
1522 : : // METHOD:
1523 : : // o Use parametric equations of line
1524 : : //
1525 : : // x = x1 + t(x2 - x1)
1526 : : // y = y1 + t(y2 - y1)
1527 : : // z = z1 + t(z2 - z1)
1528 : : //
1529 : : // o Note: two other method's were considered:
1530 : : // 1) Comparing sum of distance of point to both end points to the
1531 : : // line length.
1532 : : // 2) Checking to see if area of a triangle with the vertices is zero
1533 : : //
1534 : : // Using parametric equations is more efficient in many cases.
1535 : 0 : double t1 = 0.0,
1536 : 0 : t2 = 0.0,
1537 : 0 : t3 = 0.0,
1538 : : neg_range,
1539 : : pos_range;
1540 : :
1541 : 0 : unsigned short flg1 = 0,
1542 : 0 : flg2 = 0,
1543 : 0 : flg3 = 0;
1544 : :
1545 : 0 : neg_range = 0.0 - agtEpsilon;
1546 : 0 : pos_range = 1.0 + agtEpsilon;
1547 : :
1548 [ # # ]: 0 : if (fabs(end2[0] - end1[0]) < agtEpsilon)
1549 : : {
1550 [ # # ]: 0 : if (fabs(pnt[0] - end1[0]) < agtEpsilon)
1551 : 0 : flg1 = 1;
1552 : : else
1553 : 0 : return CUBIT_FALSE;
1554 : : }
1555 : : else
1556 : : {
1557 : 0 : t1 = (pnt[0] - end1[0])/(end2[0] - end1[0]);
1558 : :
1559 [ # # ][ # # ]: 0 : if (t1<neg_range || t1>pos_range)
1560 : 0 : return CUBIT_FALSE;
1561 : : }
1562 : :
1563 [ # # ]: 0 : if (fabs(end2[1] - end1[1]) < agtEpsilon)
1564 : : {
1565 [ # # ]: 0 : if (fabs(pnt[1] - end1[1]) < agtEpsilon)
1566 : 0 : flg2 = 1;
1567 : : else
1568 : 0 : return CUBIT_FALSE;
1569 : : }
1570 : : else
1571 : : {
1572 : 0 : t2 = (pnt[1] - end1[1])/(end2[1] - end1[1]);
1573 : :
1574 [ # # ][ # # ]: 0 : if (t2<neg_range || t2>pos_range)
1575 : 0 : return CUBIT_FALSE;
1576 : : }
1577 : :
1578 [ # # ]: 0 : if (fabs(end2[2] - end1[2]) < agtEpsilon)
1579 : : {
1580 [ # # ]: 0 : if (fabs(pnt[2] - end1[2]) < agtEpsilon)
1581 : 0 : flg3 = 1;
1582 : : else
1583 : 0 : return CUBIT_FALSE;
1584 : : }
1585 : : else
1586 : : {
1587 : 0 : t3 = (pnt[2] - end1[2])/(end2[2] - end1[2]);
1588 : :
1589 [ # # ][ # # ]: 0 : if (t3<neg_range || t3>pos_range)
1590 : 0 : return CUBIT_FALSE;
1591 : : }
1592 : :
1593 : : // If any 2 flags are 1, point is on the line,
1594 : : // otherwise, check remaining T's for equality
1595 : :
1596 [ # # ]: 0 : if (flg1)
1597 : : {
1598 : : // Here, flg1 = 1
1599 : :
1600 [ # # ]: 0 : if (flg2)
1601 : : {
1602 : : // Here, flg1 = 1
1603 : : // Here, flg2 = 1
1604 : 0 : return CUBIT_TRUE;
1605 : : }
1606 : : else
1607 : : {
1608 : : // Here, flg1 = 1
1609 : : // Here, flg2 = 0
1610 : :
1611 [ # # ]: 0 : if (flg3)
1612 : : // Here, flg1 = 1
1613 : : // Here, flg2 = 0
1614 : : // Here, flg3 = 1
1615 : 0 : return CUBIT_TRUE;
1616 : : else
1617 : : {
1618 : : // Here, flg1 = 1
1619 : : // Here, flg2 = 0
1620 : : // Here, flg3 = 0
1621 [ # # ]: 0 : if (dbl_eq(t2, t3))
1622 : 0 : return CUBIT_TRUE;
1623 : : else
1624 : 0 : return CUBIT_FALSE;
1625 : : }
1626 : : }
1627 : : }
1628 : : else
1629 : : {
1630 : : // Here, flg1 = 0
1631 [ # # ]: 0 : if (flg2)
1632 : : {
1633 : : // Here, flg1 = 0
1634 : : // Here, flg2 = 1
1635 [ # # ]: 0 : if (flg3)
1636 : 0 : return CUBIT_TRUE;
1637 : : // Here, flg1 = 0
1638 : : // Here, flg2 = 1
1639 : : // Here, flg3 = 1
1640 : : else
1641 : : {
1642 : : // Here, flg1 = 0
1643 : : // Here, flg2 = 1
1644 : : // Here, flg3 = 0
1645 [ # # ]: 0 : if (dbl_eq(t1, t3))
1646 : 0 : return CUBIT_TRUE;
1647 : : else
1648 : 0 : return CUBIT_FALSE;
1649 : : }
1650 : : }
1651 : : else
1652 : : {
1653 : : // Here, flg1 = 0
1654 : : // Here, flg2 = 0
1655 [ # # ]: 0 : if (flg3)
1656 : : {
1657 : : // Here, flg1 = 0
1658 : : // Here, flg2 = 0
1659 : : // Here, flg3 = 1
1660 [ # # ]: 0 : if (dbl_eq(t1, t2))
1661 : 0 : return CUBIT_TRUE;
1662 : : else
1663 : 0 : return CUBIT_FALSE;
1664 : : }
1665 : : else
1666 : : {
1667 : : // Here, flg1 = 0
1668 : : // Here, flg2 = 0
1669 : : // Here, flg3 = 0
1670 [ # # ][ # # ]: 0 : if (dbl_eq(t1, t2) && dbl_eq(t1, t3))
[ # # ]
1671 : 0 : return CUBIT_TRUE;
1672 : : else
1673 : 0 : return CUBIT_FALSE;
1674 : : }
1675 : : }
1676 : : }
1677 : :
1678 : : // This would be a programmer's error if we got to this point
1679 : : // return CUBIT_FALSE;
1680 : : }
1681 : :
1682 : 0 : CubitBoolean AnalyticGeometryTool::is_pnt_on_pln( double pnt[3],
1683 : : double pln_orig[3],
1684 : : double pln_norm[3] )
1685 : : {
1686 : : double result;
1687 : :
1688 : : // See if point satisfies parametric equation of plane
1689 : :
1690 : 0 : result = pln_norm[0] * (pnt[0] - pln_orig[0]) +
1691 : 0 : pln_norm[1] * (pnt[1] - pln_orig[1]) +
1692 : 0 : pln_norm[2] * (pnt[2] - pln_orig[2]);
1693 : :
1694 [ # # ]: 0 : if (dbl_eq(result, 0.0))
1695 : 0 : return CUBIT_TRUE;
1696 : : else
1697 : 0 : return CUBIT_FALSE;
1698 : : }
1699 : :
1700 : 0 : CubitBoolean AnalyticGeometryTool::is_ln_on_pln( double ln_orig[3],
1701 : : double ln_vec[3],
1702 : : double pln_orig[3],
1703 : : double pln_norm[3] )
1704 : : {
1705 : :
1706 : : // Check to see if line origin is on the plane. If so, check to see if
1707 : : // line vector is perpendicular to plane normal.
1708 : :
1709 [ # # # # ]: 0 : if (is_pnt_on_pln(ln_orig, pln_orig, pln_norm) &&
[ # # ]
1710 : 0 : is_vec_perp(ln_vec, pln_norm))
1711 : 0 : return CUBIT_TRUE;
1712 : : else
1713 : 0 : return CUBIT_FALSE;
1714 : : }
1715 : :
1716 : :
1717 : :
1718 : 0 : CubitBoolean AnalyticGeometryTool::is_pln_on_pln( double pln_orig1[3],
1719 : : double pln_norm1[3],
1720 : : double pln_orig2[3],
1721 : : double pln_norm2[3] )
1722 : : {
1723 : : // If 1st plane origin is on the 2nd plane and the normals are
1724 : : // parallel they are coincident
1725 [ # # # # ]: 0 : if( is_vec_par( pln_norm1, pln_norm2 ) &&
[ # # ]
1726 : 0 : is_pnt_on_pln( pln_orig1, pln_orig2, pln_norm2 ) )
1727 : 0 : return CUBIT_TRUE;
1728 : : else
1729 : 0 : return CUBIT_FALSE;
1730 : : }
1731 : :
1732 : : //***************************************************************************
1733 : : // Arcs/Circles
1734 : : //***************************************************************************
1735 : 0 : void AnalyticGeometryTool::setup_arc( double vec_1[3], double vec_2[3],
1736 : : double origin[3], double start_angle,
1737 : : double end_angle, double radius,
1738 : : AGT_3D_Arc &arc )
1739 : : {
1740 : 0 : copy_pnt( vec_1, arc.Vec1 );
1741 : 0 : copy_pnt( vec_2, arc.Vec2 );
1742 : 0 : copy_pnt( origin, arc.Origin );
1743 : 0 : arc.StartAngle = start_angle;
1744 : 0 : arc.EndAngle = end_angle;
1745 : 0 : arc.Radius = radius;
1746 : 0 : }
1747 : :
1748 : 0 : void AnalyticGeometryTool::setup_arc( CubitVector& vec_1, CubitVector& vec_2,
1749 : : CubitVector origin, double start_angle,
1750 : : double end_angle, double radius,
1751 : : AGT_3D_Arc &arc )
1752 : : {
1753 : 0 : vec_1.get_xyz( arc.Vec1 );
1754 : 0 : vec_2.get_xyz( arc.Vec2 );
1755 : 0 : origin.get_xyz( arc.Origin );
1756 : 0 : arc.StartAngle = start_angle;
1757 : 0 : arc.EndAngle = end_angle;
1758 : 0 : arc.Radius = radius;
1759 : 0 : }
1760 : :
1761 : 0 : void AnalyticGeometryTool::get_arc_xyz( AGT_3D_Arc &arc, double param, double pnt[3] )
1762 : : {
1763 : : double Tp;
1764 : :
1765 : : // Un-normalized parameter
1766 : 0 : Tp = arc.StartAngle * ( 1.0 - param ) + arc.EndAngle * param;
1767 : :
1768 : : // Solve for XYZ
1769 : 0 : pnt[0] = arc.Radius * ( cos( Tp ) * arc.Vec1[0] +
1770 : 0 : sin( Tp ) * arc.Vec2[0] ) +
1771 : 0 : arc.Origin[0];
1772 : :
1773 : 0 : pnt[1] = arc.Radius * ( cos( Tp ) * arc.Vec1[1] +
1774 : 0 : sin( Tp ) * arc.Vec2[1] ) +
1775 : 0 : arc.Origin[1];
1776 : :
1777 : 0 : pnt[2] = arc.Radius * ( cos( Tp ) * arc.Vec1[2] +
1778 : 0 : sin( Tp ) * arc.Vec2[2] ) +
1779 : 0 : arc.Origin[2];
1780 : 0 : }
1781 : :
1782 : 0 : void AnalyticGeometryTool::get_arc_xyz( AGT_3D_Arc &arc, double param, CubitVector& pnt )
1783 : : {
1784 : : double Tp;
1785 : :
1786 : : // Un-normalized parameter
1787 : 0 : Tp = arc.StartAngle * ( 1.0 - param ) + arc.EndAngle * param;
1788 : :
1789 : : // Solve for XYZ
1790 : 0 : pnt.x( arc.Radius * ( cos( Tp ) * arc.Vec1[0] +
1791 : 0 : sin( Tp ) * arc.Vec2[0] ) +
1792 : 0 : arc.Origin[0] );
1793 : :
1794 : 0 : pnt.y( arc.Radius * ( cos( Tp ) * arc.Vec1[1] +
1795 : 0 : sin( Tp ) * arc.Vec2[1] ) +
1796 : 0 : arc.Origin[1] );
1797 : :
1798 : 0 : pnt.z( arc.Radius * ( cos( Tp ) * arc.Vec1[2] +
1799 : 0 : sin( Tp ) * arc.Vec2[2] ) +
1800 : 0 : arc.Origin[2] );
1801 : 0 : }
1802 : :
1803 : : int
1804 : 0 : AnalyticGeometryTool::get_num_circle_tess_pnts( double radius,
1805 : : double len_tol )
1806 : : {
1807 : : double cmin, cmax;
1808 : :
1809 : 0 : double c = 2*CUBIT_PI*radius; // Circumference
1810 : :
1811 : : // Find the number of points required for the given accuracy. Use
1812 : : // a bisection method.
1813 : 0 : int nmin = 8, nmax = 100;
1814 : 0 : cmin = 2.0*nmin*radius*sin(CUBIT_PI/nmin); // Circumference of circle using segments
1815 : 0 : cmax = 2.0*nmax*radius*sin(CUBIT_PI/nmax);
1816 : :
1817 [ # # ]: 0 : if( dbl_eq( cmin, c ) )
1818 : 0 : return nmin;
1819 : :
1820 : 0 : double old_epsilon = set_epsilon( len_tol );
1821 : :
1822 : : // Find an n that is more than accurate enough
1823 [ # # ]: 0 : while( !dbl_eq( cmax, c ) )
1824 : : {
1825 : 0 : nmin = nmax;
1826 : 0 : nmax = nmin * 10;
1827 : 0 : cmin = 2.0*nmin*radius*sin(CUBIT_PI/nmin);
1828 : 0 : cmax = 2.0*nmax*radius*sin(CUBIT_PI/nmax);
1829 : : }
1830 : :
1831 : : // Biscect until the minimum number of segments satisfying
1832 : : // the tolerance is found.
1833 : : int n;
1834 : : while( 1 )
1835 : : {
1836 : 0 : n = (nmin + nmax)/2;
1837 : 0 : double cn = 2.0*n*radius*sin(CUBIT_PI/n);
1838 [ # # ]: 0 : if( dbl_eq( cn, c ) )
1839 : : {
1840 : : // Go lower
1841 : 0 : nmax = n;
1842 : : }
1843 : : else
1844 : : {
1845 : : // Go higher
1846 : 0 : nmin = n;
1847 : : }
1848 [ # # ]: 0 : if( nmax-nmin < 2 )
1849 : 0 : break;
1850 : : }
1851 : 0 : set_epsilon( old_epsilon );
1852 : :
1853 : 0 : return nmax;
1854 : : }
1855 : :
1856 : : //***************************************************************************
1857 : : // Miscellaneous
1858 : : //***************************************************************************
1859 : 0 : void AnalyticGeometryTool::get_pln_orig_norm( double A, double B, double C,
1860 : : double D, double pln_orig[3],
1861 : : double pln_norm[3] )
1862 : : {
1863 : 0 : double x = 0.0, y = 0.0, z = 0.0;
1864 : :
1865 : : // Try to have origin aligned with one of the principal axes
1866 [ # # ]: 0 : if( !dbl_eq( C, 0.0 ) )
1867 : 0 : z = -D/C;
1868 [ # # ]: 0 : else if (!dbl_eq( A, 0.0 ) )
1869 : 0 : x = -D/A;
1870 [ # # ]: 0 : else if (!dbl_eq( B, 0.0 ) )
1871 : 0 : y = -D/B;
1872 : :
1873 : 0 : pln_orig[0] = x;
1874 : 0 : pln_orig[1] = y;
1875 : 0 : pln_orig[2] = z;
1876 : :
1877 [ # # ]: 0 : if( pln_norm )
1878 : : {
1879 : 0 : pln_norm[0] = A;
1880 : 0 : pln_norm[1] = B;
1881 : 0 : pln_norm[2] = C;
1882 : : }
1883 : 0 : }
1884 : :
1885 : 0 : void AnalyticGeometryTool::get_box_corners( double box_min[3],
1886 : : double box_max[3],
1887 : : double c[8][3] )
1888 : : {
1889 : : // Left-Bottom-Front // Left-Top-Front
1890 : 0 : c[0][0] = box_min[0]; c[1][0] = box_min[0];
1891 : 0 : c[0][1] = box_min[1]; c[1][1] = box_max[1];
1892 : 0 : c[0][2] = box_max[2]; c[1][2] = box_max[2];
1893 : :
1894 : : // Right-Top-Front // Right-Bottom-Front
1895 : 0 : c[2][0] = box_max[0]; c[3][0] = box_max[0];
1896 : 0 : c[2][1] = box_max[1]; c[3][1] = box_min[1];
1897 : 0 : c[2][2] = box_max[2]; c[3][2] = box_max[2];
1898 : :
1899 : : // Left-Bottom-Back // Left-Top-Back
1900 : 0 : c[4][0] = box_min[0]; c[5][0] = box_min[0];
1901 : 0 : c[4][1] = box_min[1]; c[5][1] = box_max[1];
1902 : 0 : c[4][2] = box_min[2]; c[5][2] = box_min[2];
1903 : :
1904 : : // Right-Top-Back // Right-Bottom-Back
1905 : 0 : c[6][0] = box_max[0]; c[7][0] = box_max[0];
1906 : 0 : c[6][1] = box_max[1]; c[7][1] = box_min[1];
1907 : 0 : c[6][2] = box_min[2]; c[7][2] = box_min[2];
1908 : :
1909 : 0 : }
1910 : :
1911 : : CubitStatus
1912 : 0 : AnalyticGeometryTool::min_pln_box_int_corners( const CubitPlane& plane,
1913 : : const CubitBox& box,
1914 : : int extension_type,
1915 : : double extension,
1916 : : CubitVector& p1, CubitVector& p2,
1917 : : CubitVector& p3, CubitVector& p4,
1918 : : CubitBoolean silent )
1919 : : {
1920 [ # # ]: 0 : CubitVector box_min = box.minimum();
1921 [ # # ]: 0 : CubitVector box_max = box.maximum();
1922 : :
1923 [ # # ][ # # ]: 0 : CubitVector plane_norm = plane.normal();
1924 : :
1925 : : double box_min_pnt[3], box_max_pnt[3], pln_norm[3];
1926 [ # # ][ # # ]: 0 : box_min.get_xyz( box_min_pnt ); box_max.get_xyz( box_max_pnt );
1927 [ # # ]: 0 : plane_norm.get_xyz( pln_norm );
1928 : :
1929 : : double pnt1[3], pnt2[3], pnt3[3], pnt4[3];
1930 : :
1931 [ # # ]: 0 : if( min_pln_box_int_corners( pln_norm, plane.coefficient(),
1932 : : box_min_pnt, box_max_pnt,
1933 : : extension_type, extension,
1934 [ # # ][ # # ]: 0 : pnt1, pnt2, pnt3, pnt4, silent ) == CUBIT_FAILURE )
1935 : 0 : return CUBIT_FAILURE;
1936 : :
1937 [ # # ][ # # ]: 0 : p1.set( pnt1 ); p2.set( pnt2 ); p3.set( pnt3 ); p4.set( pnt4 );
[ # # ][ # # ]
1938 : :
1939 : 0 : return CUBIT_SUCCESS;
1940 : : }
1941 : :
1942 : : CubitStatus
1943 : 0 : AnalyticGeometryTool::min_pln_box_int_corners( CubitVector& vec1,
1944 : : CubitVector& vec2,
1945 : : CubitVector& vec3,
1946 : : CubitVector& box_min,
1947 : : CubitVector& box_max,
1948 : : int extension_type,
1949 : : double extension,
1950 : : CubitVector& p1, CubitVector& p2,
1951 : : CubitVector& p3, CubitVector& p4,
1952 : : CubitBoolean silent )
1953 : : {
1954 [ # # ]: 0 : CubitPlane plane;
1955 [ # # ][ # # ]: 0 : if( plane.mk_plane_with_points( vec1, vec2, vec3 ) == CUBIT_FAILURE )
1956 : 0 : return CUBIT_FAILURE;
1957 : :
1958 [ # # ][ # # ]: 0 : CubitVector plane_norm = plane.normal();
1959 [ # # ]: 0 : double coefficient = plane.coefficient();
1960 : :
1961 : : double plane_norm3[3];
1962 : : double box_min3[3];
1963 : : double box_max3[3];
1964 : :
1965 [ # # ]: 0 : box_min.get_xyz( box_min3 );
1966 [ # # ]: 0 : box_max.get_xyz( box_max3 );
1967 [ # # ]: 0 : plane_norm.get_xyz( plane_norm3 );
1968 : :
1969 : : double p1_3[3], p2_3[3], p3_3[3], p4_3[3];
1970 [ # # ]: 0 : p1.get_xyz( p1_3 );
1971 [ # # ]: 0 : p2.get_xyz( p2_3 );
1972 [ # # ]: 0 : p3.get_xyz( p3_3 );
1973 [ # # ]: 0 : p4.get_xyz( p4_3 );
1974 : :
1975 [ # # ]: 0 : if( min_pln_box_int_corners( plane_norm3, coefficient, box_min3, box_max3,
1976 [ # # ]: 0 : extension_type, extension, p1_3, p2_3, p3_3, p4_3, silent ) == CUBIT_FAILURE )
1977 : 0 : return CUBIT_FAILURE;
1978 : :
1979 [ # # ]: 0 : p1.set( p1_3 );
1980 [ # # ]: 0 : p2.set( p2_3 );
1981 [ # # ]: 0 : p3.set( p3_3 );
1982 [ # # ]: 0 : p4.set( p4_3 );
1983 : :
1984 : 0 : return CUBIT_SUCCESS;
1985 : : }
1986 : :
1987 : : CubitStatus
1988 : 0 : AnalyticGeometryTool::min_pln_box_int_corners( double pln_norm[3],
1989 : : double pln_coeff,
1990 : : double box_min[3],
1991 : : double box_max[3],
1992 : : int extension_type,
1993 : : double extension,
1994 : : double p1[3], double p2[3],
1995 : : double p3[3], double p4[3],
1996 : : CubitBoolean silent )
1997 : : {
1998 : : int i;
1999 : : double cubit2pln_mtx[4][4],
2000 : : pln2cubit_mtx[4][4];
2001 : : double pln_orig[3];
2002 : 0 : double box_tol = 1e-6;
2003 : :
2004 : : // Adjust bounding box if it is zero in any direction
2005 : : double abox_min[3], abox_max[3];
2006 [ # # ]: 0 : copy_pnt( box_min, abox_min );
2007 [ # # ]: 0 : copy_pnt( box_max, abox_max );
2008 : :
2009 : 0 : double x_range = fabs(box_max[0]-box_min[0]);
2010 : 0 : double y_range = fabs(box_max[1]-box_min[1]);
2011 : 0 : double z_range = fabs(box_max[2]-box_min[2]);
2012 : :
2013 [ # # ][ # # ]: 0 : if( x_range < box_tol || y_range < box_tol || z_range < box_tol )
[ # # ]
2014 : : {
2015 : : // Adjust zero length side(s) to maximum range
2016 [ # # ][ # # ]: 0 : double adj = CUBIT_MAX((CUBIT_MAX((x_range),(y_range))),(z_range))/2.0;
[ # # ]
2017 : :
2018 : : // Arbitrarily expand box if it is zero in size
2019 [ # # ]: 0 : if( adj < box_tol )
2020 : 0 : adj = 5.0;
2021 : :
2022 [ # # ]: 0 : if( x_range < box_tol )
2023 : : {
2024 : 0 : abox_min[0] -= adj;
2025 : 0 : abox_max[0] += adj;
2026 : : }
2027 [ # # ]: 0 : if( y_range < box_tol )
2028 : : {
2029 : 0 : abox_min[1] -= adj;
2030 : 0 : abox_max[1] += adj;
2031 : : }
2032 [ # # ]: 0 : if( z_range < box_tol )
2033 : : {
2034 : 0 : abox_min[2] -= adj;
2035 : 0 : abox_max[2] += adj;
2036 : : }
2037 : : }
2038 : :
2039 : : // Get plane origin
2040 : 0 : double A = pln_norm[0];
2041 : 0 : double B = pln_norm[1];
2042 : 0 : double C = pln_norm[2];
2043 : 0 : double D = pln_coeff;
2044 [ # # ]: 0 : get_pln_orig_norm( A, B, C, D, pln_orig );
2045 : :
2046 : : // Find intersections of edges with plane. Add to unique array. At most
2047 : : // there are 6 intersections...
2048 : : double int_array[6][3];
2049 : 0 : int num_int = 0;
2050 : : num_int = get_plane_bbox_intersections( abox_min, abox_max, pln_orig, pln_norm,
2051 [ # # ]: 0 : int_array );
2052 : :
2053 : : // Adjust so plane intercepts bounding box
2054 [ # # ]: 0 : if( num_int < 3 )
2055 : : {
2056 : : // Move the plane to the center of the bounding box
2057 : : double box_cent[3];
2058 : 0 : box_cent[0] = (abox_min[0]+abox_max[0])/2.0;
2059 : 0 : box_cent[1] = (abox_min[1]+abox_max[1])/2.0;
2060 : 0 : box_cent[2] = (abox_min[2]+abox_max[2])/2.0;
2061 : :
2062 : : // Get the intersections
2063 [ # # ]: 0 : num_int = get_plane_bbox_intersections( abox_min, abox_max, box_cent, pln_norm, int_array );
2064 : :
2065 : : // Project the points back to the original plane
2066 [ # # # # : 0 : switch (num_int)
# # # ]
2067 : : {
2068 : : case 6:
2069 [ # # ]: 0 : int_pnt_pln( int_array[5], pln_orig, pln_norm, int_array[5] );
2070 : : case 5:
2071 [ # # ]: 0 : int_pnt_pln( int_array[4], pln_orig, pln_norm, int_array[4] );
2072 : : case 4:
2073 [ # # ]: 0 : int_pnt_pln( int_array[3], pln_orig, pln_norm, int_array[3] );
2074 : : case 3:
2075 [ # # ]: 0 : int_pnt_pln( int_array[2], pln_orig, pln_norm, int_array[2] );
2076 : : case 2:
2077 [ # # ]: 0 : int_pnt_pln( int_array[1], pln_orig, pln_norm, int_array[1] );
2078 : : case 1:
2079 [ # # ]: 0 : int_pnt_pln( int_array[0], pln_orig, pln_norm, int_array[0] );
2080 : : }
2081 : : }
2082 : :
2083 [ # # ]: 0 : if( num_int == 0 )
2084 : : {
2085 [ # # ]: 0 : if( silent == CUBIT_FALSE )
2086 [ # # ][ # # ]: 0 : PRINT_ERROR( "Plane does not intersect the bounding box\n"
[ # # ]
2087 [ # # ]: 0 : " Can't find 4 corners of plane\n" );
2088 : 0 : return CUBIT_FAILURE;
2089 : : }
2090 [ # # ]: 0 : if( num_int < 3 )
2091 : : {
2092 [ # # ]: 0 : if( silent == CUBIT_FALSE )
2093 [ # # ][ # # ]: 0 : PRINT_ERROR( "Plane intersects the bounding box at only %d locations\n"
[ # # ]
2094 [ # # ]: 0 : " Can't calculate 4 corners of plane\n", num_int );
2095 : 0 : return CUBIT_FAILURE;
2096 : : }
2097 : :
2098 : : // Transform pnts to plane coordinate system
2099 : : double pln_x[3], pln_y[3];
2100 [ # # ]: 0 : orth_vecs( pln_norm, pln_x, pln_y );
2101 [ # # ]: 0 : vecs_to_mtx( pln_x, pln_y, pln_norm, pln_orig, pln2cubit_mtx );
2102 [ # # ]: 0 : inv_trans_mtx( pln2cubit_mtx, cubit2pln_mtx );
2103 : :
2104 : : double int_arr_pln[6][3];
2105 [ # # ]: 0 : for( i=0; i<num_int; i++ )
2106 [ # # ]: 0 : transform_pnt( cubit2pln_mtx, int_array[i], int_arr_pln[i] );
2107 : :
2108 [ # # ]: 0 : for(i=0; i<num_int; i++)
2109 : : {
2110 [ # # ][ # # ]: 0 : if( !dbl_eq( int_arr_pln[i][2], 0.0 ) )
2111 : : {
2112 [ # # ]: 0 : if( silent == CUBIT_FALSE )
2113 [ # # ][ # # ]: 0 : PRINT_ERROR( "in AnalyticGeometryTool::min_box_pln_int_corners\n"
[ # # ]
2114 [ # # ]: 0 : " Transform to plane wrong\n" );
2115 : 0 : return CUBIT_FAILURE;
2116 : : }
2117 : : }
2118 : :
2119 : : // Place into format for mimimal box calculation
2120 : : Point2 pt[6];
2121 [ # # ]: 0 : for ( i=0; i<num_int; i++ )
2122 : : {
2123 : 0 : pt[i].x = int_arr_pln[i][0];
2124 : 0 : pt[i].y = int_arr_pln[i][1];
2125 : : }
2126 : :
2127 : : // Find rectangle with minimal area to surround the points
2128 : : // (this is definitely overkill esp. for the simple cases.....)
2129 [ # # ]: 0 : OBBox2 minimal = MinimalBox2( num_int, pt );
2130 : :
2131 : : // Strip out results
2132 [ # # ]: 0 : double old_epsilon = set_epsilon( 1e-10 );
2133 : : double centroid[3];
2134 : 0 : centroid[0] = minimal.center.x;
2135 : 0 : centroid[1] = minimal.center.y;
2136 : 0 : centroid[2] = 0.0;
2137 [ # # ]: 0 : round_near_val( centroid[0] ); // Makes near -1, 0, 1 values -1, 0, 1
2138 [ # # ]: 0 : round_near_val( centroid[1] );
2139 [ # # ]: 0 : transform_pnt( pln2cubit_mtx, centroid, centroid );
2140 : :
2141 : : double x_axis[3];
2142 : 0 : x_axis[0] = minimal.axis[0].x;
2143 : 0 : x_axis[1] = minimal.axis[0].y;
2144 : 0 : x_axis[2] = 0.0;
2145 [ # # ]: 0 : round_near_val( x_axis[0] );
2146 [ # # ]: 0 : round_near_val( x_axis[1] );
2147 [ # # ]: 0 : transform_vec( pln2cubit_mtx, x_axis, x_axis );
2148 : :
2149 : : double y_axis[3];
2150 : 0 : y_axis[0] = minimal.axis[1].x;
2151 : 0 : y_axis[1] = minimal.axis[1].y;
2152 : 0 : y_axis[2] = 0.0;
2153 [ # # ]: 0 : round_near_val( y_axis[0] );
2154 [ # # ]: 0 : round_near_val( y_axis[1] );
2155 [ # # ]: 0 : transform_vec( pln2cubit_mtx, y_axis, y_axis );
2156 : :
2157 [ # # ]: 0 : set_epsilon( old_epsilon );
2158 : :
2159 : : double dist_x;
2160 : : double dist_y;
2161 : 0 : double extension_distance = 0.0;
2162 [ # # ]: 0 : if( extension_type == 1 ) // Percentage (of 1/2 diagonal)
2163 : : {
2164 : 0 : double diag_len = sqrt( minimal.extent[0]*minimal.extent[0]
2165 : 0 : + minimal.extent[1]*minimal.extent[1] );
2166 : 0 : extension_distance = diag_len*extension/100.0;
2167 : : }
2168 [ # # ]: 0 : else if( extension_type == 2 ) // Absolute distance in x and y
2169 : 0 : extension_distance = extension;
2170 : :
2171 : 0 : dist_x = minimal.extent[0] + extension_distance;
2172 : 0 : dist_y = minimal.extent[1] + extension_distance;
2173 : :
2174 [ # # ]: 0 : next_pnt( centroid, x_axis, -dist_x, p1 );
2175 [ # # ]: 0 : next_pnt( p1, y_axis, -dist_y, p1 );
2176 : :
2177 [ # # ]: 0 : next_pnt( centroid, x_axis, -dist_x, p2 );
2178 [ # # ]: 0 : next_pnt( p2, y_axis, dist_y, p2 );
2179 : :
2180 [ # # ]: 0 : next_pnt( centroid, x_axis, dist_x, p3 );
2181 [ # # ]: 0 : next_pnt( p3, y_axis, dist_y, p3 );
2182 : :
2183 [ # # ]: 0 : next_pnt( centroid, x_axis, dist_x, p4 );
2184 [ # # ]: 0 : next_pnt( p4, y_axis, -dist_y, p4 );
2185 : :
2186 : 0 : return CUBIT_SUCCESS;
2187 : : }
2188 : :
2189 : 0 : int AnalyticGeometryTool::get_plane_bbox_intersections( double box_min[3],
2190 : : double box_max[3],
2191 : : double pln_orig[3],
2192 : : double pln_norm[3],
2193 : : double int_array[6][3])
2194 : : {
2195 : : // Fill in an array with all 8 box corners
2196 : : double corner[8][3];
2197 [ # # ]: 0 : get_box_corners( box_min, box_max, corner );
2198 : :
2199 : : // Get 12 edges of the box
2200 : : double ln_start[12][3], ln_end[12][3];
2201 [ # # ][ # # ]: 0 : copy_pnt( corner[0], ln_start[0] ); copy_pnt( corner[1], ln_end[0] );
2202 [ # # ][ # # ]: 0 : copy_pnt( corner[1], ln_start[1] ); copy_pnt( corner[2], ln_end[1] );
2203 [ # # ][ # # ]: 0 : copy_pnt( corner[2], ln_start[2] ); copy_pnt( corner[3], ln_end[2] );
2204 [ # # ][ # # ]: 0 : copy_pnt( corner[3], ln_start[3] ); copy_pnt( corner[0], ln_end[3] );
2205 [ # # ][ # # ]: 0 : copy_pnt( corner[4], ln_start[4] ); copy_pnt( corner[5], ln_end[4] );
2206 [ # # ][ # # ]: 0 : copy_pnt( corner[5], ln_start[5] ); copy_pnt( corner[6], ln_end[5] );
2207 [ # # ][ # # ]: 0 : copy_pnt( corner[6], ln_start[6] ); copy_pnt( corner[7], ln_end[6] );
2208 [ # # ][ # # ]: 0 : copy_pnt( corner[7], ln_start[7] ); copy_pnt( corner[4], ln_end[7] );
2209 [ # # ][ # # ]: 0 : copy_pnt( corner[0], ln_start[8] ); copy_pnt( corner[4], ln_end[8] );
2210 [ # # ][ # # ]: 0 : copy_pnt( corner[1], ln_start[9] ); copy_pnt( corner[5], ln_end[9] );
2211 [ # # ][ # # ]: 0 : copy_pnt( corner[2], ln_start[10] ); copy_pnt( corner[6], ln_end[10] );
2212 [ # # ][ # # ]: 0 : copy_pnt( corner[3], ln_start[11] ); copy_pnt( corner[7], ln_end[11] );
2213 : :
2214 : : double ln_vec[3];
2215 : : double int_pnt[3];
2216 : 0 : int num_int = 0;
2217 : : int i, j, found;
2218 [ # # ]: 0 : for( i=0; i<12; i++ )
2219 : : {
2220 [ # # ]: 0 : get_vec_unit( ln_start[i], ln_end[i], ln_vec );
2221 [ # # ][ # # ]: 0 : if( int_ln_pln( ln_start[i], ln_vec, pln_orig, pln_norm, int_pnt ) )
2222 : : {
2223 : : // Only add if on the bounded line segment
2224 [ # # ][ # # ]: 0 : if( is_pnt_on_ln_seg( int_pnt, ln_start[i], ln_end[i] ) )
2225 : : {
2226 : : // Only add if unique
2227 : 0 : found = 0;
2228 [ # # ]: 0 : for( j=0; j<num_int; j++ )
2229 : : {
2230 [ # # ][ # # ]: 0 : if( pnt_eq( int_pnt, int_array[j] ) )
2231 : : {
2232 : 0 : found = 1;
2233 : 0 : break;
2234 : : }
2235 : : }
2236 [ # # ]: 0 : if( !found )
2237 : : {
2238 [ # # ]: 0 : copy_pnt( int_pnt, int_array[num_int] );
2239 : 0 : num_int++;
2240 : : }
2241 : : }
2242 : : }
2243 : : }
2244 : 0 : return num_int;
2245 : : }
2246 : :
2247 : : CubitStatus
2248 : 0 : AnalyticGeometryTool::get_tight_bounding_box( DLIList<CubitVector*> &point_list,
2249 : : CubitVector ¢er,
2250 : : CubitVector axes[3],
2251 : : CubitVector &extension )
2252 : : {
2253 [ # # ]: 0 : int num_pnts = point_list.size();
2254 [ # # ]: 0 : if( num_pnts == 0 )
2255 : 0 : return CUBIT_FAILURE;
2256 [ # # ][ # # ]: 0 : Point3 *pnt_arr = new Point3[num_pnts];
2257 : :
2258 : : int i;
2259 [ # # ]: 0 : point_list.reset();
2260 : : CubitVector *vec;
2261 [ # # ]: 0 : for( i=0; i<num_pnts; i++ )
2262 : : {
2263 [ # # ]: 0 : vec = point_list.get_and_step();
2264 : : //GfxPreview::draw_point( vec, 3 );
2265 : :
2266 [ # # ]: 0 : pnt_arr[i].x = vec->x();
2267 [ # # ]: 0 : pnt_arr[i].y = vec->y();
2268 [ # # ]: 0 : pnt_arr[i].z = vec->z();
2269 : : }
2270 : :
2271 [ # # ][ # # ]: 0 : OBBox3 minimal = MinimalBox3 (point_list.size(), pnt_arr);
2272 : :
2273 : : //PRINT_INFO( "MinBox center = %lf, %lf, %lf\n", minimal.center.x, minimal.center.y, minimal.center.z );
2274 : : //PRINT_INFO( "MinBox axis 1 = %lf, %lf, %lf\n", minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z );
2275 : : //PRINT_INFO( "MinBox axis 2 = %lf, %lf, %lf\n", minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z );
2276 : : //PRINT_INFO( "MinBox axis 3 = %lf, %lf, %lf\n", minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z );
2277 : : //PRINT_INFO( "MinBox extent = %lf, %lf, %lf\n", minimal.extent[0], minimal.extent[1], minimal.extent[2] );
2278 : :
2279 [ # # ]: 0 : center.set(minimal.center.x, minimal.center.y, minimal.center.z);
2280 [ # # ]: 0 : axes[0].set(minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z);
2281 [ # # ]: 0 : axes[1].set(minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z);
2282 [ # # ]: 0 : axes[2].set(minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z);
2283 [ # # ]: 0 : extension.set(minimal.extent[0], minimal.extent[1], minimal.extent[2]);
2284 : :
2285 [ # # ]: 0 : delete [] pnt_arr;
2286 : :
2287 : 0 : return CUBIT_SUCCESS;
2288 : : }
2289 : :
2290 : : CubitStatus
2291 : 0 : AnalyticGeometryTool::get_tight_bounding_box( DLIList<CubitVector*> &point_list,
2292 : : CubitVector& p1, CubitVector& p2,
2293 : : CubitVector& p3, CubitVector& p4,
2294 : : CubitVector& p5, CubitVector& p6,
2295 : : CubitVector& p7, CubitVector& p8)
2296 : : {
2297 [ # # ]: 0 : int num_pnts = point_list.size();
2298 [ # # ]: 0 : if( num_pnts == 0 )
2299 : 0 : return CUBIT_FAILURE;
2300 [ # # ][ # # ]: 0 : Point3 *pnt_arr = new Point3[num_pnts];
2301 : :
2302 : : int i;
2303 [ # # ]: 0 : point_list.reset();
2304 : : CubitVector *vec;
2305 [ # # ]: 0 : for( i=0; i<num_pnts; i++ )
2306 : : {
2307 [ # # ]: 0 : vec = point_list.get_and_step();
2308 : :
2309 [ # # ]: 0 : pnt_arr[i].x = vec->x();
2310 [ # # ]: 0 : pnt_arr[i].y = vec->y();
2311 [ # # ]: 0 : pnt_arr[i].z = vec->z();
2312 : : }
2313 : :
2314 [ # # ][ # # ]: 0 : OBBox3 minimal = MinimalBox3 (point_list.size(), pnt_arr);
2315 : :
2316 : : //PRINT_INFO( "MinBox center = %lf, %lf, %lf\n", minimal.center.x, minimal.center.y, minimal.center.z );
2317 : : //PRINT_INFO( "MinBox axis 1 = %lf, %lf, %lf\n", minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z );
2318 : : //PRINT_INFO( "MinBox axis 2 = %lf, %lf, %lf\n", minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z );
2319 : : //PRINT_INFO( "MinBox axis 3 = %lf, %lf, %lf\n", minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z );
2320 : : //PRINT_INFO( "MinBox extent = %lf, %lf, %lf\n", minimal.extent[0], minimal.extent[1], minimal.extent[2] );
2321 : :
2322 [ # # ]: 0 : CubitVector center(minimal.center.x, minimal.center.y, minimal.center.z);
2323 [ # # ]: 0 : CubitVector x(minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z);
2324 [ # # ]: 0 : CubitVector y(minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z);
2325 [ # # ]: 0 : CubitVector z(minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z);
2326 [ # # ]: 0 : CubitVector extent(minimal.extent[0], minimal.extent[1], minimal.extent[2]);
2327 : :
2328 [ # # ][ # # ]: 0 : center.next_point( -x, extent.x(), p1 ); p1.next_point( -y, extent.y(), p1 );
[ # # ][ # # ]
[ # # ][ # # ]
2329 [ # # ][ # # ]: 0 : p1.next_point( z, extent.z(), p1 );
2330 : :
2331 [ # # ][ # # ]: 0 : center.next_point( -x, extent.x(), p2 ); p2.next_point( y, extent.y(), p2 );
[ # # ][ # # ]
[ # # ]
2332 [ # # ][ # # ]: 0 : p2.next_point( z, extent.z(), p2 );
2333 : :
2334 [ # # ][ # # ]: 0 : center.next_point( x, extent.x(), p3 ); p3.next_point( y, extent.y(), p3 );
[ # # ][ # # ]
2335 [ # # ][ # # ]: 0 : p3.next_point( z, extent.z(), p3 );
2336 : :
2337 [ # # ][ # # ]: 0 : center.next_point( x, extent.x(), p4 ); p4.next_point( -y, extent.y(), p4 );
[ # # ][ # # ]
[ # # ]
2338 [ # # ][ # # ]: 0 : p4.next_point( z, extent.z(), p4 );
2339 : :
2340 [ # # ][ # # ]: 0 : center.next_point( -x, extent.x(), p5 ); p5.next_point( -y, extent.y(), p5 );
[ # # ][ # # ]
[ # # ][ # # ]
2341 [ # # ][ # # ]: 0 : p5.next_point( -z, extent.z(), p5 );
[ # # ]
2342 : :
2343 [ # # ][ # # ]: 0 : center.next_point( -x, extent.x(), p6 ); p6.next_point( y, extent.y(), p6 );
[ # # ][ # # ]
[ # # ]
2344 [ # # ][ # # ]: 0 : p6.next_point( -z, extent.z(), p6 );
[ # # ]
2345 : :
2346 [ # # ][ # # ]: 0 : center.next_point( x, extent.x(), p7 ); p7.next_point( y, extent.y(), p7 );
[ # # ][ # # ]
2347 [ # # ][ # # ]: 0 : p7.next_point( -z, extent.z(), p7 );
[ # # ]
2348 : :
2349 [ # # ][ # # ]: 0 : center.next_point( x, extent.x(), p8 ); p8.next_point( -y, extent.y(), p8 );
[ # # ][ # # ]
[ # # ]
2350 [ # # ][ # # ]: 0 : p8.next_point( -z, extent.z(), p8 );
[ # # ]
2351 : :
2352 [ # # ]: 0 : delete [] pnt_arr;
2353 : :
2354 : 0 : return CUBIT_SUCCESS;
2355 : : }
2356 : :
2357 : : CubitStatus
2358 : 0 : AnalyticGeometryTool::min_cyl_box_int( double radius,
2359 : : CubitVector& axis,
2360 : : CubitVector& center,
2361 : : CubitBox& box,
2362 : : int extension_type,
2363 : : double extension,
2364 : : CubitVector &start,
2365 : : CubitVector &end,
2366 : : int num_tess_pnts )
2367 : :
2368 : : {
2369 [ # # ]: 0 : CubitVector box_min = box.minimum();
2370 [ # # ]: 0 : CubitVector box_max = box.maximum();
2371 : :
2372 : : double box_min_pnt[3], box_max_pnt[3], axis_vec[3], center_pnt[3];
2373 [ # # ][ # # ]: 0 : box_min.get_xyz( box_min_pnt ); box_max.get_xyz( box_max_pnt );
2374 [ # # ][ # # ]: 0 : axis.get_xyz( axis_vec ); center.get_xyz( center_pnt );
2375 : :
2376 : : double start_pnt[3], end_pnt[3];
2377 : :
2378 [ # # ]: 0 : if( min_cyl_box_int( radius, axis_vec, center_pnt,
2379 : : box_min_pnt, box_max_pnt,
2380 : : extension_type, extension,
2381 [ # # ]: 0 : start_pnt, end_pnt, num_tess_pnts )
2382 : : == CUBIT_FAILURE )
2383 : 0 : return CUBIT_FAILURE;
2384 : :
2385 [ # # ][ # # ]: 0 : start.set( start_pnt ); end.set( end_pnt );
2386 : :
2387 : 0 : return CUBIT_SUCCESS;
2388 : : }
2389 : :
2390 : : CubitStatus
2391 : 0 : AnalyticGeometryTool::min_cyl_box_int( double radius, double axis[3],
2392 : : double center[3],
2393 : : double box_min[3], double box_max[3],
2394 : : int extension_type, double extension,
2395 : : double start[3], double end[3],
2396 : : int num_tess_pnts )
2397 : : {
2398 : : double cyl_z[3];
2399 [ # # ]: 0 : unit_vec( axis, cyl_z );
2400 : :
2401 : : //PRINT_INFO( "Axis = %f, %f, %f\n", cyl_z[0], cyl_z[1], cyl_z[2] );
2402 : : //PRINT_INFO( "Center = %f, %f, %f\n", center[0], center[1], center[2] );
2403 : :
2404 : : // Find transformation matrix to take a point into cylinder's
2405 : : // coordinate system
2406 : : double cubit2cyl_mtx[4][4], cyl2cubit_mtx[4][4];
2407 : : double cyl_x[3], cyl_y[3];
2408 [ # # ]: 0 : orth_vecs( cyl_z, cyl_x, cyl_y );
2409 [ # # ]: 0 : vecs_to_mtx( cyl_x, cyl_y, cyl_z, center, cyl2cubit_mtx );
2410 [ # # ]: 0 : inv_trans_mtx( cyl2cubit_mtx, cubit2cyl_mtx );
2411 : :
2412 : : // Setup the circle
2413 : : double vec_1[3], vec_2[3];
2414 [ # # ]: 0 : orth_vecs( cyl_z, vec_1, vec_2 );
2415 : : AGT_3D_Arc arc;
2416 [ # # ]: 0 : setup_arc( vec_1, vec_2, center, 0.0, 2.0 * CUBIT_PI, radius, arc );
2417 : :
2418 : : // Setup the planes of the box
2419 : : double pln_norm[6][3], pln_orig[6][3];
2420 : : // Front
2421 : 0 : pln_orig[0][0] = 0.0; pln_orig[0][1] = 0.0; pln_orig[0][2] = box_max[2];
2422 : 0 : pln_norm[0][0] = 0.0; pln_norm[0][1] = 0.0; pln_norm[0][2] = 1.0;
2423 : : // Left
2424 : 0 : pln_orig[1][0] = box_min[0]; pln_orig[1][1] = 0.0; pln_orig[1][2] = 0.0;
2425 : 0 : pln_norm[1][0] = -1.0; pln_norm[1][1] = 0.0; pln_norm[1][2] = 0.0;
2426 : : // Top
2427 : 0 : pln_orig[2][0] = 0.0; pln_orig[2][1] = box_max[1]; pln_orig[2][2] = 0.0;
2428 : 0 : pln_norm[2][0] = 0.0; pln_norm[2][1] = 1.0; pln_norm[2][2] = 0.0;
2429 : : // Right
2430 : 0 : pln_orig[3][0] = box_max[0]; pln_orig[3][1] = 0.0; pln_orig[3][2] = 0.0;
2431 : 0 : pln_norm[3][0] = 1.0; pln_norm[3][1] = 0.0; pln_norm[3][2] = 0.0;
2432 : : // Bottom
2433 : 0 : pln_orig[4][0] = 0.0; pln_orig[4][1] = box_min[1]; pln_orig[4][2] = 0.0;
2434 : 0 : pln_norm[4][0] = 0.0; pln_norm[4][1] = -1.0; pln_norm[4][2] = 0.0;
2435 : : // Back
2436 : 0 : pln_orig[5][0] = 0.0; pln_orig[5][1] = 0.0; pln_orig[5][2] = box_min[2];
2437 : 0 : pln_norm[5][0] = 0.0; pln_norm[5][1] = 0.0; pln_norm[5][2] = -1.0;
2438 : :
2439 : : double z; // Intersection along cylinder's axis
2440 : 0 : double min_z = CUBIT_DBL_MAX, max_z = -CUBIT_DBL_MAX;
2441 : :
2442 : 0 : double t = 0.0, dt;
2443 : 0 : dt = 1.0/(double)num_tess_pnts;
2444 : : double pnt[3];
2445 : : double int_pnt[3];
2446 : 0 : double box_tol = 1e-14;
2447 : 0 : double box_min_0 = box_min[0]-box_tol;
2448 : 0 : double box_min_1 = box_min[1]-box_tol;
2449 : 0 : double box_min_2 = box_min[2]-box_tol;
2450 : 0 : double box_max_0 = box_max[0]+box_tol;
2451 : 0 : double box_max_1 = box_max[1]+box_tol;
2452 : 0 : double box_max_2 = box_max[2]+box_tol;
2453 : :
2454 : : int i,j;
2455 [ # # ]: 0 : for( i=0; i<num_tess_pnts; i++ )
2456 : : {
2457 [ # # ]: 0 : get_arc_xyz( arc, t, pnt );
2458 : :
2459 [ # # ]: 0 : for( j=0; j<6; j++ )
2460 : : {
2461 : : // Evaluate the intersection at this point
2462 [ # # ][ # # ]: 0 : if( int_ln_pln( pnt, cyl_z, pln_orig[j], pln_norm[j], int_pnt )
2463 : : == CUBIT_FAILURE )
2464 : 0 : continue;
2465 : :
2466 : : // Throw-out if intersection not on physical box
2467 [ # # ][ # # ]: 0 : if( int_pnt[0] < box_min_0 || int_pnt[1] < box_min_1 ||
[ # # ]
2468 [ # # ][ # # ]: 0 : int_pnt[2] < box_min_2 || int_pnt[0] > box_max_0 ||
2469 [ # # ]: 0 : int_pnt[1] > box_max_1 || int_pnt[2] > box_max_2 )
2470 : 0 : continue;
2471 : :
2472 : : // Find min/max cylinder z on box so far
2473 : : // z-distance (in cylinder coordinate system)
2474 : 0 : z = cubit2cyl_mtx[0][2]*int_pnt[0] + cubit2cyl_mtx[1][2]*int_pnt[1] +
2475 : 0 : cubit2cyl_mtx[2][2]*int_pnt[2] + cubit2cyl_mtx[3][2];
2476 : :
2477 [ # # ]: 0 : if( z < min_z ) min_z = z;
2478 [ # # ]: 0 : if( z > max_z ) max_z = z;
2479 : :
2480 : : }
2481 : :
2482 : 0 : t += dt;
2483 : :
2484 : : }
2485 : :
2486 : : // Check the 8 corners of the box - they are likely min/max's.
2487 : : double box_corners[8][3];
2488 [ # # ]: 0 : get_box_corners( box_min, box_max, box_corners );
2489 [ # # ]: 0 : for( i=0; i<8; i++ )
2490 : : {
2491 : : // Get the corner in the cylinder csys
2492 [ # # ]: 0 : transform_pnt( cubit2cyl_mtx, box_corners[i], pnt );
2493 : : // If the pnt is within the circle's radius, check it's z-coord
2494 : : // (distance from center)
2495 [ # # ]: 0 : if( sqrt( pnt[0]*pnt[0] + pnt[1]*pnt[1] ) <= radius+box_tol )
2496 : : {
2497 [ # # ]: 0 : if( pnt[2] < min_z ) min_z = pnt[2];
2498 [ # # ]: 0 : if( pnt[2] > max_z ) max_z = pnt[2];
2499 : : }
2500 : : }
2501 : :
2502 [ # # ][ # # ]: 0 : if( min_z == CUBIT_DBL_MAX || max_z == -CUBIT_DBL_MAX )
2503 : : {
2504 [ # # ][ # # ]: 0 : PRINT_ERROR( "Unable to find cylinder/box intersection\n" );
[ # # ][ # # ]
2505 : 0 : return CUBIT_FAILURE;
2506 : : }
2507 : :
2508 [ # # ]: 0 : if( min_z == max_z )
2509 : : {
2510 [ # # ][ # # ]: 0 : PRINT_ERROR( "Unable to find cylinder/box intersection\n" );
[ # # ][ # # ]
2511 : 0 : return CUBIT_FAILURE;
2512 : : }
2513 : :
2514 : : //PRINT_INFO( "Min dist = %f\n", min_z );
2515 : : //PRINT_INFO( "Max dist = %f\n", max_z );
2516 : :
2517 : : // Find the start and end of the cylinder
2518 [ # # ]: 0 : next_pnt( center, cyl_z, min_z, start );
2519 [ # # ]: 0 : next_pnt( center, cyl_z, max_z, end );
2520 : :
2521 : : //PRINT_INFO( "Start = %f, %f, %f\n", start[0], start[1], start[2] );
2522 : : //PRINT_INFO( "End = %f, %f, %f\n", end[0], end[1], end[2] );
2523 : :
2524 : : // Extend start and end, if necessary
2525 [ # # ]: 0 : if( extension_type > 0 )
2526 : : {
2527 : 0 : double ext_distance = 0.0;
2528 [ # # ]: 0 : if( extension_type == 1 ) // percentage
2529 : : {
2530 [ # # ]: 0 : double cyl_length = dist_pnt_pnt( start, end );
2531 : 0 : ext_distance = extension/100.0 * cyl_length;
2532 : : }
2533 : : else
2534 : 0 : ext_distance = extension;
2535 : :
2536 [ # # ]: 0 : next_pnt( end, cyl_z, ext_distance, end );
2537 [ # # ]: 0 : reverse_vec( cyl_z, cyl_z );
2538 [ # # ]: 0 : next_pnt( start, cyl_z, ext_distance, start );
2539 : : }
2540 : :
2541 : 0 : return CUBIT_SUCCESS;
2542 : : }
2543 : :
2544 : : // MAGIC SOFTWARE - see .hpp file
2545 : : // FILE: minbox2.cpp
2546 : : //---------------------------------------------------------------------------
2547 : 0 : double AnalyticGeometryTool::Area (int N, Point2* pt, double angle)
2548 : : {
2549 : 0 : double cs = cos(angle), sn = sin(angle);
2550 : :
2551 : 0 : double umin = +cs*pt[0].x+sn*pt[0].y, umax = umin;
2552 : 0 : double vmin = -sn*pt[0].x+cs*pt[0].y, vmax = vmin;
2553 [ # # ]: 0 : for (int i = 1; i < N; i++)
2554 : : {
2555 : 0 : double u = +cs*pt[i].x+sn*pt[i].y;
2556 [ # # ]: 0 : if ( u < umin )
2557 : 0 : umin = u;
2558 [ # # ]: 0 : else if ( u > umax )
2559 : 0 : umax = u;
2560 : :
2561 : 0 : double v = -sn*pt[i].x+cs*pt[i].y;
2562 [ # # ]: 0 : if ( v < vmin )
2563 : 0 : vmin = v;
2564 [ # # ]: 0 : else if ( v > vmax )
2565 : 0 : vmax = v;
2566 : : }
2567 : :
2568 : 0 : double area = (umax-umin)*(vmax-vmin);
2569 : 0 : return area;
2570 : : }
2571 : : //---------------------------------------------------------------------------
2572 : 0 : void AnalyticGeometryTool::MinimalBoxForAngle (int N, Point2* pt, double angle,
2573 : : OBBox2& box)
2574 : : {
2575 : 0 : double cs = cos(angle), sn = sin(angle);
2576 : :
2577 : :
2578 : 0 : double umin = +cs*pt[0].x+sn*pt[0].y, umax = umin;
2579 : 0 : double vmin = -sn*pt[0].x+cs*pt[0].y, vmax = vmin;
2580 [ # # ]: 0 : for (int i = 1; i < N; i++)
2581 : : {
2582 : 0 : double u = +cs*pt[i].x+sn*pt[i].y;
2583 [ # # ]: 0 : if ( u < umin )
2584 : 0 : umin = u;
2585 [ # # ]: 0 : else if ( u > umax )
2586 : 0 : umax = u;
2587 : :
2588 : 0 : double v = -sn*pt[i].x+cs*pt[i].y;
2589 [ # # ]: 0 : if ( v < vmin )
2590 : 0 : vmin = v;
2591 [ # # ]: 0 : else if ( v > vmax )
2592 : 0 : vmax = v;
2593 : : }
2594 : :
2595 : 0 : double umid = 0.5*(umax+umin);
2596 : 0 : double vmid = 0.5*(vmax+vmin);
2597 : 0 : box.center.x = umid*cs-vmid*sn;
2598 : 0 : box.center.y = umid*sn+vmid*cs;
2599 : 0 : box.axis[0].x = cs;
2600 : 0 : box.axis[0].y = sn;
2601 : 0 : box.axis[1].x = -sn;
2602 : 0 : box.axis[1].y = cs;
2603 : 0 : box.extent[0] = 0.5*(umax-umin);
2604 : 0 : box.extent[1] = 0.5*(vmax-vmin);
2605 : 0 : }
2606 : : //---------------------------------------------------------------------------
2607 : 0 : OBBox2 AnalyticGeometryTool::MinimalBox2 (int N, Point2* pt)
2608 : : {
2609 : : OBBox2 box;
2610 : :
2611 : : // bracket a minimum for angles in [-pi,pi]
2612 : : double angle, area;
2613 : 0 : int imin = 0;
2614 : 0 : double areaMin = Area(N,pt,angleMin);
2615 : :
2616 : : int i;
2617 [ # # ]: 0 : for (i = 1; i <= maxPartition; i++)
2618 : : {
2619 : 0 : angle = angleMin+i*angleRange/maxPartition;
2620 : 0 : area = Area(N,pt,angle);
2621 [ # # ]: 0 : if ( area < areaMin )
2622 : : {
2623 : 0 : imin = i;
2624 : 0 : areaMin = area;
2625 : : }
2626 : : }
2627 : :
2628 : 0 : double angle0 = angleMin+(imin-1)*angleRange/maxPartition;
2629 : 0 : double area0 = Area(N,pt,angle0);
2630 : 0 : double angle1 = angleMin+(imin+1)*angleRange/maxPartition;
2631 : 0 : double area1 = Area(N,pt,angle1);
2632 : :
2633 : : // use inverse parabolic interpolation to find the minimum
2634 [ # # ]: 0 : for (i = 0; i <= invInterp; i++)
2635 : : {
2636 : : double angleMid, areaMid;
2637 : :
2638 : : // test for convergence (do not change these parameters)
2639 : 0 : const double epsilon = 1e-08, tol = 1e-04;
2640 : : // const double omtol = 1.0-tol;
2641 [ # # ]: 0 : if ( fabs(angle1-angle0) <= 2*tol*fabs(angle)+epsilon )
2642 : 0 : break;
2643 : :
2644 : : // compute vertex of interpolating parabola
2645 : 0 : double dangle0 = angle0-angle, dangle1 = angle1-angle;
2646 : 0 : double darea0 = area0-area, darea1 = area1-area;
2647 : 0 : double temp0 = dangle0*darea1, temp1 = dangle1*darea0;
2648 : 0 : double delta = temp1-temp0;
2649 [ # # ]: 0 : if ( fabs(delta) < epsilon )
2650 : 0 : break;
2651 : :
2652 : 0 : angleMid = angle+0.5*(dangle1*temp1-dangle0*temp0)/(temp1-temp0);
2653 : :
2654 : : // update bracket
2655 [ # # ]: 0 : if ( angleMid < angle )
2656 : : {
2657 : 0 : areaMid = Area(N,pt,angleMid);
2658 [ # # ]: 0 : if ( areaMid <= area )
2659 : : {
2660 : 0 : angle1 = angle;
2661 : 0 : area1 = area;
2662 : 0 : angle = angleMid;
2663 : 0 : area = areaMid;
2664 : : }
2665 : : else
2666 : : {
2667 : 0 : angle0 = angleMid;
2668 : 0 : area0 = areaMid;
2669 : : }
2670 : : }
2671 [ # # ]: 0 : else if ( angleMid > angle )
2672 : : {
2673 : 0 : areaMid = Area(N,pt,angleMid);
2674 [ # # ]: 0 : if ( areaMid <= area )
2675 : : {
2676 : 0 : angle0 = angle;
2677 : 0 : area0 = area;
2678 : 0 : angle = angleMid;
2679 : 0 : area = areaMid;
2680 : : }
2681 : : else
2682 : : {
2683 : 0 : angle1 = angleMid;
2684 : 0 : area1 = areaMid;
2685 : : }
2686 : : }
2687 : : else
2688 : : {
2689 : : // bracket middle already vertex of parabola
2690 : 0 : break;
2691 : : }
2692 : : }
2693 : :
2694 : 0 : MinimalBoxForAngle(N,pt,angle,box);
2695 : 0 : return box;
2696 : : }
2697 : : //---------------------------------------------------------------------------
2698 : :
2699 : : #ifdef MINBOX2_TEST
2700 : :
2701 : : #include <stdlib.h>
2702 : :
2703 : : void main ()
2704 : : {
2705 : : const int N = 128;
2706 : : Point2 pt[N];
2707 : :
2708 : : for (int i = 0; i < N; i++)
2709 : : {
2710 : : pt[i].x = rand()/double(RAND_MAX);
2711 : : pt[i].y = rand()/double(RAND_MAX);
2712 : : }
2713 : :
2714 : : OBBox2 minimal = MinimalBox2(N,pt);
2715 : : }
2716 : :
2717 : : #endif
2718 : :
2719 : : #if 1
2720 : : // FILE: minbox3.cpp
2721 : : //---------------------------------------------------------------------------
2722 : 0 : void AnalyticGeometryTool::MatrixToAngleAxis (double** R, double& angle, double axis[3])
2723 : : {
2724 : : // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
2725 : : // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
2726 : : // I is the identity and
2727 : : //
2728 : : // +- -+
2729 : : // P = | 0 +z -y |
2730 : : // | -z 0 +x |
2731 : : // | +y -x 0 |
2732 : : // +- -+
2733 : : //
2734 : : // Some algebra will show that
2735 : : //
2736 : : // cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P
2737 : : //
2738 : : // In the event that A = pi, R-R^t = 0 which prevents us from extracting
2739 : : // the axis through P. Instead note that R = I+2*P^2 when A = pi, so
2740 : : // P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and z^2-1.
2741 : : // We can solve these for axis (x,y,z). Because the angle is pi, it does
2742 : : // not matter which sign you choose on the square roots.
2743 : :
2744 : 0 : double trace = R[0][0]+R[1][1]+R[2][2];
2745 : 0 : double cs = 0.5*(trace-1.0);
2746 [ # # ]: 0 : if ( -1 < cs )
2747 : : {
2748 [ # # ]: 0 : if ( cs < 1 )
2749 : 0 : angle = acos(cs);
2750 : : else
2751 : 0 : angle = 0;
2752 : : }
2753 : : else
2754 : : {
2755 : 0 : angle = AGT_PI;
2756 : : }
2757 : :
2758 : 0 : axis[0] = R[1][2]-R[2][1];
2759 : 0 : axis[1] = R[2][0]-R[0][2];
2760 : 0 : axis[2] = R[0][1]-R[1][0];
2761 : 0 : double length = sqrt(axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2]);
2762 : 0 : const double epsilon = 1e-06;
2763 [ # # ]: 0 : if ( length > epsilon )
2764 : : {
2765 : 0 : axis[0] /= length;
2766 : 0 : axis[1] /= length;
2767 : 0 : axis[2] /= length;
2768 : : }
2769 : : else // angle is 0 or pi
2770 : : {
2771 [ # # ]: 0 : if ( angle > 1.0 ) // any number strictly between 0 and pi works
2772 : : {
2773 : : // angle must be pi
2774 : 0 : axis[0] = sqrt(0.5*(1.0+R[0][0]));
2775 : 0 : axis[1] = sqrt(0.5*(1.0+R[1][1]));
2776 : 0 : axis[2] = sqrt(0.5*(1.0+R[2][2]));
2777 : :
2778 : : // determine signs of axis components
2779 : : double test[3];
2780 : 0 : test[0] = R[0][0]*axis[0]+R[0][1]*axis[1]+R[0][2]*axis[2]-axis[0];
2781 : 0 : test[1] = R[1][0]*axis[0]+R[1][1]*axis[1]+R[1][2]*axis[2]-axis[1];
2782 : 0 : test[2] = R[2][0]*axis[0]+R[2][1]*axis[1]+R[2][2]*axis[2]-axis[2];
2783 : 0 : length = test[0]*test[0]+test[1]*test[1]+test[2]*test[2];
2784 [ # # ]: 0 : if ( length < epsilon )
2785 : 0 : return;
2786 : :
2787 : 0 : axis[1] = -axis[1];
2788 : 0 : test[0] = R[0][0]*axis[0]+R[0][1]*axis[1]+R[0][2]*axis[2]-axis[0];
2789 : 0 : test[1] = R[1][0]*axis[0]+R[1][1]*axis[1]+R[1][2]*axis[2]-axis[1];
2790 : 0 : test[2] = R[2][0]*axis[0]+R[2][1]*axis[1]+R[2][2]*axis[2]-axis[2];
2791 : 0 : length = test[0]*test[0]+test[1]*test[1]+test[2]*test[2];
2792 [ # # ]: 0 : if ( length < epsilon )
2793 : 0 : return;
2794 : :
2795 : 0 : axis[2] = -axis[2];
2796 : 0 : test[0] = R[0][0]*axis[0]+R[0][1]*axis[1]+R[0][2]*axis[2]-axis[0];
2797 : 0 : test[1] = R[1][0]*axis[0]+R[1][1]*axis[1]+R[1][2]*axis[2]-axis[1];
2798 : 0 : test[2] = R[2][0]*axis[0]+R[2][1]*axis[1]+R[2][2]*axis[2]-axis[2];
2799 : 0 : length = test[0]*test[0]+test[1]*test[1]+test[2]*test[2];
2800 [ # # ]: 0 : if ( length < epsilon )
2801 : 0 : return;
2802 : :
2803 : 0 : axis[1] = -axis[1];
2804 : 0 : test[0] = R[0][0]*axis[0]+R[0][1]*axis[1]+R[0][2]*axis[2]-axis[0];
2805 : 0 : test[1] = R[1][0]*axis[0]+R[1][1]*axis[1]+R[1][2]*axis[2]-axis[1];
2806 : 0 : test[2] = R[2][0]*axis[0]+R[2][1]*axis[1]+R[2][2]*axis[2]-axis[2];
2807 : 0 : length = test[0]*test[0]+test[1]*test[1]+test[2]*test[2];
2808 [ # # ]: 0 : if ( length < epsilon )
2809 : 0 : return;
2810 : : }
2811 : : else
2812 : : {
2813 : : // Angle is zero, matrix is the identity, no unique axis, so
2814 : : // return (1,0,0) for as good a guess as any.
2815 : 0 : axis[0] = 1.0;
2816 : 0 : axis[1] = 0.0;
2817 : 0 : axis[2] = 0.0;
2818 : : }
2819 : : }
2820 : : }
2821 : : //---------------------------------------------------------------------------
2822 : 0 : void AnalyticGeometryTool::AngleAxisToMatrix (double angle, double axis[3], double R[3][3])
2823 : : {
2824 : 0 : double cs = cos(angle), sn = sin(angle);
2825 : 0 : double length = sqrt(axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2]);
2826 : 0 : double x = axis[0]/length;
2827 : 0 : double y = axis[1]/length;
2828 : 0 : double z = axis[2]/length;
2829 : 0 : double omc = 1.0-cs;
2830 : 0 : double x2 = x*x, y2 = y*y, z2 = z*z;
2831 : 0 : double xy = x*y, xz = x*z, yz = y*z;
2832 : 0 : double snx = sn*x, sny = sn*y, snz = sn*z;
2833 : :
2834 : 0 : R[0][0] = 1.0-omc*(y2+z2);
2835 : 0 : R[0][1] = +snz+omc*xy;
2836 : 0 : R[0][2] = -sny+omc*xz;
2837 : 0 : R[1][0] = -snz+omc*xy;
2838 : 0 : R[1][1] = 1.0-omc*(x2+z2);
2839 : 0 : R[1][2] = +snx+omc*yz;
2840 : 0 : R[2][0] = +sny+omc*xz;
2841 : 0 : R[2][1] = -snx+omc*yz;
2842 : 0 : R[2][2] = 1.0-omc*(x2+y2);
2843 : 0 : }
2844 : : //---------------------------------------------------------------------------
2845 : 0 : double AnalyticGeometryTool::Volume (int N, Point3* pt, double angle[3])
2846 : : {
2847 : 0 : double cs0 = cos(angle[0]), sn0 = sin(angle[0]);
2848 : 0 : double cs1 = cos(angle[1]), sn1 = sin(angle[1]);
2849 : 0 : double axis[3] = { cs0*sn1, sn0*sn1, cs1 };
2850 : : double rot[3][3];
2851 [ # # ]: 0 : AngleAxisToMatrix(angle[2],axis,rot);
2852 : :
2853 : : double min[3] =
2854 : : {
2855 : 0 : rot[0][0]*pt[0].x+rot[1][0]*pt[0].y+rot[2][0]*pt[0].z,
2856 : 0 : rot[0][1]*pt[0].x+rot[1][1]*pt[0].y+rot[2][1]*pt[0].z,
2857 : 0 : rot[0][2]*pt[0].x+rot[1][2]*pt[0].y+rot[2][2]*pt[0].z
2858 : 0 : };
2859 : :
2860 : 0 : double max[3] = { min[0], min[1], min[2] };
2861 : :
2862 [ # # ]: 0 : for (int i = 1; i < N; i++)
2863 : : {
2864 : : double test[3] =
2865 : : {
2866 : 0 : rot[0][0]*pt[i].x+rot[1][0]*pt[i].y+rot[2][0]*pt[i].z,
2867 : 0 : rot[0][1]*pt[i].x+rot[1][1]*pt[i].y+rot[2][1]*pt[i].z,
2868 : 0 : rot[0][2]*pt[i].x+rot[1][2]*pt[i].y+rot[2][2]*pt[i].z
2869 : 0 : };
2870 : :
2871 [ # # ]: 0 : if ( test[0] < min[0] )
2872 : 0 : min[0] = test[0];
2873 [ # # ]: 0 : else if ( test[0] > max[0] )
2874 : 0 : max[0] = test[0];
2875 : :
2876 [ # # ]: 0 : if ( test[1] < min[1] )
2877 : 0 : min[1] = test[1];
2878 [ # # ]: 0 : else if ( test[1] > max[1] )
2879 : 0 : max[1] = test[1];
2880 : :
2881 [ # # ]: 0 : if ( test[2] < min[2] )
2882 : 0 : min[2] = test[2];
2883 [ # # ]: 0 : else if ( test[2] > max[2] )
2884 : 0 : max[2] = test[2];
2885 : : }
2886 : :
2887 : 0 : double volume = (max[0]-min[0])*(max[1]-min[1])*(max[2]-min[2]);
2888 : 0 : return volume;
2889 : : }
2890 : : //---------------------------------------------------------------------------
2891 : 0 : void AnalyticGeometryTool::MinimalBoxForAngles (int N, Point3* pt, double angle[3],
2892 : : OBBox3& box)
2893 : : {
2894 : 0 : double cs0 = cos(angle[0]), sn0 = sin(angle[0]);
2895 : 0 : double cs1 = cos(angle[1]), sn1 = sin(angle[1]);
2896 : 0 : double axis[3] = { cs0*sn1, sn0*sn1, cs1 };
2897 : : double rot[3][3];
2898 [ # # ]: 0 : AngleAxisToMatrix(angle[2],axis,rot);
2899 : :
2900 : : double min[3] =
2901 : : {
2902 : 0 : rot[0][0]*pt[0].x+rot[1][0]*pt[0].y+rot[2][0]*pt[0].z,
2903 : 0 : rot[0][1]*pt[0].x+rot[1][1]*pt[0].y+rot[2][1]*pt[0].z,
2904 : 0 : rot[0][2]*pt[0].x+rot[1][2]*pt[0].y+rot[2][2]*pt[0].z
2905 : 0 : };
2906 : :
2907 : 0 : double max[3] = { min[0], min[1], min[2] };
2908 : :
2909 [ # # ]: 0 : for (int i = 1; i < N; i++)
2910 : : {
2911 : : double test[3] =
2912 : : {
2913 : 0 : rot[0][0]*pt[i].x+rot[1][0]*pt[i].y+rot[2][0]*pt[i].z,
2914 : 0 : rot[0][1]*pt[i].x+rot[1][1]*pt[i].y+rot[2][1]*pt[i].z,
2915 : 0 : rot[0][2]*pt[i].x+rot[1][2]*pt[i].y+rot[2][2]*pt[i].z
2916 : 0 : };
2917 : :
2918 [ # # ]: 0 : if ( test[0] < min[0] )
2919 : 0 : min[0] = test[0];
2920 [ # # ]: 0 : else if ( test[0] > max[0] )
2921 : 0 : max[0] = test[0];
2922 : :
2923 [ # # ]: 0 : if ( test[1] < min[1] )
2924 : 0 : min[1] = test[1];
2925 [ # # ]: 0 : else if ( test[1] > max[1] )
2926 : 0 : max[1] = test[1];
2927 : :
2928 [ # # ]: 0 : if ( test[2] < min[2] )
2929 : 0 : min[2] = test[2];
2930 [ # # ]: 0 : else if ( test[2] > max[2] )
2931 : 0 : max[2] = test[2];
2932 : : }
2933 : :
2934 : : double mid[3] =
2935 : : {
2936 : 0 : 0.5*(max[0]+min[0]), 0.5*(max[1]+min[1]), 0.5*(max[2]+min[2])
2937 : 0 : };
2938 : :
2939 : 0 : box.center.x = mid[0]*rot[0][0]+mid[1]*rot[0][1]+mid[2]*rot[0][2];
2940 : 0 : box.center.y = mid[0]*rot[1][0]+mid[1]*rot[1][1]+mid[2]*rot[1][2];
2941 : 0 : box.center.z = mid[0]*rot[2][0]+mid[1]*rot[2][1]+mid[2]*rot[2][2];
2942 : 0 : box.axis[0].x = rot[0][0];
2943 : 0 : box.axis[0].y = rot[1][0];
2944 : 0 : box.axis[0].z = rot[2][0];
2945 : 0 : box.axis[1].x = rot[0][1];
2946 : 0 : box.axis[1].y = rot[1][1];
2947 : 0 : box.axis[1].z = rot[2][1];
2948 : 0 : box.axis[2].x = rot[0][2];
2949 : 0 : box.axis[2].y = rot[1][2];
2950 : 0 : box.axis[2].z = rot[2][2];
2951 : 0 : box.extent[0] = 0.5*(max[0]-min[0]);
2952 : 0 : box.extent[1] = 0.5*(max[1]-min[1]);
2953 : 0 : box.extent[2] = 0.5*(max[2]-min[2]);
2954 : 0 : }
2955 : : //---------------------------------------------------------------------------
2956 : 0 : void AnalyticGeometryTool::GetInterval (double A[3], double D[3], double& tmin, double& tmax)
2957 : : {
2958 : : //static const double angle_min[3] = { -AGT_PI, 0.0, 0.0 };
2959 : : //static const double angle_max[3] = { AGT_PI, AGT_PI, AGT_PI };
2960 : : //The pgCC compiler running on solars and cross-compiling for
2961 : : //janus has a bug such that it dies if the initialization
2962 : : //of angle_min is mixed symbolic and literal constants, so
2963 : : //define a symbolid constant for 0.0.
2964 : : // -- J.Kraftcheck, 06/05/2001
2965 : : static const double ZERO = 0.0;
2966 [ # # ][ # # ]: 0 : static const double angle_min[3] = { -AGT_PI, ZERO, ZERO };
2967 : : static const double angle_max[3] = { AGT_PI, AGT_PI, AGT_PI };
2968 : :
2969 : 0 : tmin = -DBL_MAX;
2970 : 0 : tmax = +DBL_MAX;
2971 : :
2972 [ # # ]: 0 : for (int i = 0; i < 3; i++)
2973 : : {
2974 : 0 : const double epsilon = 1e-08;
2975 : 0 : double b0 = angle_min[i]-A[i];
2976 : 0 : double b1 = angle_max[i]-A[i];
2977 : :
2978 : : double inv, tmp;
2979 [ # # ]: 0 : if ( D[i] > epsilon )
2980 : : {
2981 : 0 : inv = 1.0/D[i];
2982 : 0 : tmp = inv*b0;
2983 [ # # ]: 0 : if ( tmp > tmin )
2984 : 0 : tmin = tmp;
2985 : 0 : tmp = inv*b1;
2986 [ # # ]: 0 : if ( tmp < tmax )
2987 : 0 : tmax = tmp;
2988 : : }
2989 [ # # ]: 0 : else if ( D[i] < -epsilon )
2990 : : {
2991 : 0 : inv = 1.0/D[i];
2992 : 0 : tmp = inv*b0;
2993 [ # # ]: 0 : if ( tmp < tmax )
2994 : 0 : tmax = tmp;
2995 : 0 : tmp = inv*b1;
2996 [ # # ]: 0 : if ( tmp > tmin )
2997 : 0 : tmin = tmp;
2998 : : }
2999 : : }
3000 : :
3001 [ # # ][ # # ]: 0 : if( tmin == -DBL_MAX || tmax == DBL_MAX ) // Added by SRS 10-6-2000
3002 : : {
3003 : : //PRINT_WARNING( "tmin/tmax not set\n" );
3004 : 0 : tmin = -1.0;
3005 : 0 : tmax = 1.0;
3006 : : }
3007 : 0 : }
3008 : : //---------------------------------------------------------------------------
3009 : 0 : void AnalyticGeometryTool::Combine (double result[3], double A[3], double t, double D[3])
3010 : : {
3011 [ # # ]: 0 : for (int i = 0; i < 3; i++)
3012 : 0 : result[i] = A[i]+t*D[i];
3013 : 0 : }
3014 : : //---------------------------------------------------------------------------
3015 : 0 : double AnalyticGeometryTool::MinimizeOnInterval (int N, Point3* pt, double A[3], double D[3])
3016 : : {
3017 : : // compute intersection of line A+t*D with domain of function
3018 : : double tmin, tmax;
3019 [ # # ]: 0 : GetInterval(A,D,tmin,tmax);
3020 : 0 : double tran = tmax-tmin;
3021 : : double angle[3];
3022 : :
3023 [ # # ]: 0 : if( tran == 0.0 ) // Added by SRS 10-6-2000
3024 : 0 : tran = 1.0;
3025 : :
3026 : : // bracket a minimum for angles in [A+tmin*D,A+tmax*D]
3027 : 0 : double t = 0.0;
3028 [ # # ]: 0 : double volumeMin = Volume(N,pt,A);
3029 : : double volume;
3030 : :
3031 : 0 : const int max_partition = 64;
3032 : : int i, imin;
3033 [ # # ]: 0 : for (i = 0, imin = -1; i <= max_partition; i++)
3034 : : {
3035 : 0 : t = tmin+i*tran/max_partition;
3036 [ # # ]: 0 : Combine(angle,A,t,D);
3037 : :
3038 [ # # ]: 0 : volume = Volume(N,pt,angle);
3039 [ # # ]: 0 : if ( volume < volumeMin )
3040 : : {
3041 : 0 : imin = i;
3042 : 0 : volumeMin = volume;
3043 : : }
3044 : : }
3045 : :
3046 [ # # ]: 0 : if ( imin != -1 )
3047 : : {
3048 : 0 : t = tmin+imin*tran/max_partition;
3049 : : }
3050 : : else
3051 : : {
3052 : 0 : t = 0.0;
3053 : :
3054 : : // interval in which t=0 lies
3055 : 0 : imin = int(-tmin*max_partition/tran+0.5);
3056 : : }
3057 : 0 : volume = volumeMin;
3058 : :
3059 : 0 : double t0 = tmin+(imin-1)*tran/max_partition;
3060 [ # # ]: 0 : Combine(angle,A,t0,D);
3061 [ # # ]: 0 : double volume0 = Volume(N,pt,angle);
3062 : :
3063 : 0 : double t1 = tmin+(imin+1)*tran/max_partition;
3064 [ # # ]: 0 : Combine(angle,A,t1,D);
3065 [ # # ]: 0 : double volume1 = Volume(N,pt,angle);
3066 : :
3067 : : // use inverse parabolic interpolation to find the minimum
3068 : 0 : const int inv_interp = 64;
3069 [ # # ]: 0 : for (i = 0; i <= inv_interp; i++)
3070 : : {
3071 : : double tMid, volumeMid;
3072 : :
3073 : : // test for convergence (do not change these parameters)
3074 : 0 : const double epsilon = 1e-08, tol = 1e-04;
3075 : : // const double omtol = 1.0-tol;
3076 [ # # ]: 0 : if ( fabs(t1-t0) <= 2*tol*fabs(t)+epsilon )
3077 : 0 : break;
3078 : :
3079 : : // compute vertex of interpolating parabola
3080 : 0 : double dt0 = t0-t, dt1 = t1-t;
3081 : 0 : double dvolume0 = volume0-volume, dvolume1 = volume1-volume;
3082 : 0 : double temp0 = dt0*dvolume1, temp1 = dt1*dvolume0;
3083 : 0 : double delta = temp1-temp0;
3084 [ # # ]: 0 : if ( fabs(delta) < epsilon )
3085 : 0 : break;
3086 : :
3087 : 0 : tMid = t+0.5*(dt1*temp1-dt0*temp0)/(temp1-temp0);
3088 : :
3089 : : // update bracket
3090 [ # # ]: 0 : if ( tMid < t )
3091 : : {
3092 [ # # ]: 0 : Combine(angle,A,tMid,D);
3093 [ # # ]: 0 : volumeMid = Volume(N,pt,angle);
3094 [ # # ]: 0 : if ( volumeMid <= volume )
3095 : : {
3096 : 0 : t1 = t;
3097 : 0 : volume1 = volume;
3098 : 0 : t = tMid;
3099 : 0 : volume = volumeMid;
3100 : : }
3101 : : else
3102 : : {
3103 : 0 : t0 = tMid;
3104 : 0 : volume0 = volumeMid;
3105 : : }
3106 : : }
3107 [ # # ]: 0 : else if ( tMid > t )
3108 : : {
3109 [ # # ]: 0 : Combine(angle,A,tMid,D);
3110 [ # # ]: 0 : volumeMid = Volume(N,pt,angle);
3111 [ # # ]: 0 : if ( volumeMid <= volume )
3112 : : {
3113 : 0 : t0 = t;
3114 : 0 : volume0 = volume;
3115 : 0 : t = tMid;
3116 : 0 : volume = volumeMid;
3117 : : }
3118 : : else
3119 : : {
3120 : 0 : t1 = tMid;
3121 : 0 : volume1 = volumeMid;
3122 : : }
3123 : : }
3124 : : else
3125 : : {
3126 : : // bracket middle already vertex of parabola
3127 : 0 : break;
3128 : : }
3129 : : }
3130 : :
3131 [ # # ]: 0 : Combine(A,A,t,D);
3132 : 0 : return volume;
3133 : : }
3134 : : //---------------------------------------------------------------------------
3135 : 0 : double AnalyticGeometryTool::MinimizeOnLattice (int N, Point3* pt, double A[3], int layers,
3136 : : double thickness)
3137 : : {
3138 : 0 : int xmin = 0, ymin = 0, zmin = 0;
3139 [ # # ]: 0 : double volume = Volume(N,pt,A);
3140 : :
3141 : : double angle[3];
3142 [ # # ]: 0 : for (int z = -layers; z <= layers; z++)
3143 : : {
3144 : 0 : angle[2] = A[2]+thickness*z/layers;
3145 [ # # ]: 0 : for (int y = -layers; y <= layers; y++)
3146 : : {
3147 : 0 : angle[1] = A[1]+thickness*y/layers;
3148 [ # # ]: 0 : for (int x = -layers; x <= layers; x++)
3149 : : {
3150 : 0 : angle[0] = A[0]+thickness*x/layers;
3151 : :
3152 [ # # ]: 0 : double v = Volume(N,pt,angle);
3153 [ # # ]: 0 : if ( v < volume )
3154 : : {
3155 : 0 : xmin = x;
3156 : 0 : ymin = y;
3157 : 0 : zmin = z;
3158 : 0 : volume = v;
3159 : : }
3160 : : }
3161 : : }
3162 : : }
3163 : :
3164 : 0 : A[0] += thickness*xmin/layers;
3165 : 0 : A[1] += thickness*ymin/layers;
3166 : 0 : A[2] += thickness*zmin/layers;
3167 : :
3168 : 0 : return volume;
3169 : : }
3170 : : //---------------------------------------------------------------------------
3171 : 0 : void AnalyticGeometryTool::InitialGuess (int N, Point3* pt, double angle[3])
3172 : : {
3173 : : int i;
3174 : :
3175 : : // compute mean of points
3176 : 0 : double xsum = 0.0f, ysum = 0.0f, zsum = 0.0f;;
3177 [ # # ]: 0 : for (i = 0; i < N; i++)
3178 : : {
3179 : 0 : xsum += pt[i].x;
3180 : 0 : ysum += pt[i].y;
3181 : 0 : zsum += pt[i].z;
3182 : : }
3183 : 0 : double xmean = xsum/N;
3184 : 0 : double ymean = ysum/N;
3185 : 0 : double zmean = zsum/N;
3186 : :
3187 : : // compute covariances of points
3188 : 0 : double xxsum = 0.0f, xysum = 0.0f, xzsum = 0.0f;
3189 : 0 : double yysum = 0.0f, yzsum = 0.0f, zzsum = 0.0f;
3190 [ # # ]: 0 : for (i = 0; i < N; i++)
3191 : : {
3192 : 0 : double dx = pt[i].x - xmean;
3193 : 0 : double dy = pt[i].y - ymean;
3194 : 0 : double dz = pt[i].z - zmean;
3195 : 0 : xxsum += dx*dx;
3196 : 0 : xysum += dx*dy;
3197 : 0 : xzsum += dx*dz;
3198 : 0 : yysum += dy*dy;
3199 : 0 : yzsum += dy*dz;
3200 : 0 : zzsum += dz*dz;
3201 : : }
3202 : 0 : double xxcov = xxsum/N;
3203 : 0 : double xycov = xysum/N;
3204 : 0 : double xzcov = xzsum/N;
3205 : 0 : double yycov = yysum/N;
3206 : 0 : double yzcov = yzsum/N;
3207 : 0 : double zzcov = zzsum/N;
3208 : :
3209 : : // compute eigenvectors for covariance matrix
3210 [ # # ]: 0 : mgcEigenD eig(3);
3211 [ # # ]: 0 : eig.Matrix(0,0) = xxcov;
3212 [ # # ]: 0 : eig.Matrix(0,1) = xycov;
3213 [ # # ]: 0 : eig.Matrix(0,2) = xzcov;
3214 [ # # ]: 0 : eig.Matrix(1,0) = xycov;
3215 [ # # ]: 0 : eig.Matrix(1,1) = yycov;
3216 [ # # ]: 0 : eig.Matrix(1,2) = yzcov;
3217 [ # # ]: 0 : eig.Matrix(2,0) = xzcov;
3218 [ # # ]: 0 : eig.Matrix(2,1) = yzcov;
3219 [ # # ]: 0 : eig.Matrix(2,2) = zzcov;
3220 [ # # ]: 0 : eig.EigenStuff3();
3221 : :
3222 : : // Use eigenvectors as the box axes. Eigenmatrix must not have a
3223 : : // reflection component, thus the check for negative determinant.
3224 : 0 : const double epsilon = 1e-06;
3225 [ # # ]: 0 : double** R = (double**)eig.Eigenvector();
3226 : : double det =
3227 : 0 : +R[0][0]*R[1][1]*R[2][2]
3228 : 0 : +R[0][1]*R[1][2]*R[2][0]
3229 : 0 : +R[0][2]*R[1][0]*R[2][1]
3230 : 0 : -R[0][2]*R[1][1]*R[2][0]
3231 : 0 : -R[0][1]*R[1][0]*R[2][2]
3232 : 0 : -R[0][0]*R[1][2]*R[2][1];
3233 [ # # ]: 0 : if ( det < 0.0 )
3234 : : {
3235 : 0 : R[0][0] = -R[0][0];
3236 : 0 : R[1][0] = -R[1][0];
3237 : 0 : R[2][0] = -R[2][0];
3238 : : }
3239 : :
3240 : : // extract angles from rotation axis = (cos(u)sin(v),sin(u)sin(v),cos(v))
3241 : : double axis[3];
3242 [ # # ]: 0 : MatrixToAngleAxis(R,angle[2],axis);
3243 [ # # ]: 0 : if ( -1+epsilon < axis[2] )
3244 : : {
3245 [ # # ]: 0 : if ( axis[2] < 1-epsilon )
3246 : : {
3247 : 0 : angle[0] = atan2(axis[1],axis[0]);
3248 : 0 : angle[1] = acos(axis[2]);
3249 : : }
3250 : : else
3251 : : {
3252 : 0 : angle[0] = 0;
3253 : 0 : angle[1] = 0;
3254 : : }
3255 : : }
3256 : : else
3257 : : {
3258 : 0 : angle[0] = 0;
3259 : 0 : angle[1] = AGT_PI;
3260 [ # # ]: 0 : }
3261 : 0 : }
3262 : : //---------------------------------------------------------------------------
3263 : 0 : OBBox3 AnalyticGeometryTool::MinimalBox3 (int N, Point3* pt)
3264 : : {
3265 : : // compute a good initial guess for an oriented bounding box
3266 : : double angle[3];
3267 [ # # ]: 0 : InitialGuess(N,pt,angle);
3268 [ # # ]: 0 : double oldVolume = Volume(N,pt,angle);
3269 : :
3270 : : //if initial guess gives angles that are zero, the rest of this code
3271 : : //can give bounding box that isn't tight. This probably
3272 : : //isn't the best fix but it works. CDE 4-20-2009
3273 [ # # ]: 0 : if( angle[0] == 0 )
3274 : 0 : angle[0] = 0.5;
3275 [ # # ]: 0 : if( angle[1] == 0 )
3276 : 0 : angle[1] = 0.5;
3277 [ # # ]: 0 : if( angle[2] == 0 )
3278 : 0 : angle[2] = 0.5;
3279 : :
3280 : 0 : double saveAngle[3] = { angle[0], angle[1], angle[2] };
3281 : :
3282 : : // Powell's direction set method
3283 : : double U[3][3], volume;
3284 : 0 : const int maxiters = 3*32;
3285 [ # # ]: 0 : for (int iter = 0; iter < maxiters; iter++)
3286 : : {
3287 : : // reset directions to avoid linear dependence degeneration
3288 [ # # ]: 0 : if ( iter % 3 == 0 )
3289 : : {
3290 : 0 : U[0][0] = 1.0; U[0][1] = 0.0; U[0][2] = 0.0;
3291 : 0 : U[1][0] = 0.0; U[1][1] = 1.0; U[1][2] = 0.0;
3292 : 0 : U[2][0] = 0.0; U[2][1] = 0.0; U[2][2] = 1.0;
3293 : : }
3294 : :
3295 : : // find minima in specified directions
3296 [ # # ]: 0 : for (int d = 0; d < 3; d++)
3297 [ # # ]: 0 : volume = MinimizeOnInterval(N,pt,angle,U[d]);
3298 : :
3299 : : // estimate a conjugate direction
3300 : : double conj[3] =
3301 : : {
3302 : 0 : angle[0]-saveAngle[0],
3303 : 0 : angle[1]-saveAngle[1],
3304 : 0 : angle[2]-saveAngle[2]
3305 : 0 : };
3306 : 0 : double length = sqrt(conj[0]*conj[0]+conj[1]*conj[1]+conj[2]*conj[2]);
3307 [ # # ]: 0 : if ( length >= 1e-06 )
3308 : : {
3309 : 0 : double invLen = 1.0/length;
3310 : 0 : conj[0] *= invLen;
3311 : 0 : conj[1] *= invLen;
3312 : 0 : conj[2] *= invLen;
3313 : :
3314 : : // minimize in conjugate direction
3315 [ # # ]: 0 : volume = MinimizeOnInterval(N,pt,angle,conj);
3316 : : }
3317 : : else
3318 : : {
3319 : : // Possible local, but not global, minimum. Search nearby for
3320 : : // a smaller volume.
3321 [ # # ]: 0 : volume = MinimizeOnLattice(N,pt,angle,2,0.0001);
3322 [ # # ]: 0 : volume = MinimizeOnLattice(N,pt,angle,2,0.0010);
3323 [ # # ]: 0 : volume = MinimizeOnLattice(N,pt,angle,2,0.0100);
3324 [ # # ]: 0 : volume = MinimizeOnLattice(N,pt,angle,2,0.1000);
3325 : : }
3326 : :
3327 : : // test for convergence
3328 : 0 : const double epsilon = 1e-04;
3329 : 0 : double diff = fabs(volume-oldVolume);
3330 [ # # ]: 0 : if ( diff <= epsilon )
3331 : : {
3332 : : // Possible local, but not global, minimum. Search nearby for
3333 : : // a smaller volume.
3334 [ # # ]: 0 : volume = MinimizeOnLattice(N,pt,angle,2,0.0001);
3335 [ # # ]: 0 : volume = MinimizeOnLattice(N,pt,angle,2,0.0010);
3336 [ # # ]: 0 : volume = MinimizeOnLattice(N,pt,angle,2,0.0100);
3337 [ # # ]: 0 : volume = MinimizeOnLattice(N,pt,angle,2,0.1000);
3338 : 0 : diff = fabs(volume-oldVolume);
3339 [ # # ]: 0 : if ( diff <= epsilon )
3340 : 0 : break;
3341 : : }
3342 : :
3343 : : // cycle the directions and add conjugate direction to list
3344 : 0 : U[0][0] = U[1][0]; U[0][1] = U[1][1]; U[0][2] = U[1][2];
3345 : 0 : U[1][0] = U[2][0]; U[1][1] = U[2][1]; U[1][2] = U[2][2];
3346 : 0 : U[2][0] = conj[0]; U[2][1] = conj[1]; U[2][2] = conj[2];
3347 : :
3348 : : // set parameters for next pass
3349 : 0 : oldVolume = volume;
3350 : 0 : saveAngle[0] = angle[0];
3351 : 0 : saveAngle[1] = angle[1];
3352 : 0 : saveAngle[2] = angle[2];
3353 : : }
3354 : :
3355 : : OBBox3 box;
3356 [ # # ]: 0 : MinimalBoxForAngles(N,pt,angle,box);
3357 : 0 : return box;
3358 : : }
3359 : : //---------------------------------------------------------------------------
3360 : :
3361 : : #ifdef MINBOX3_TEST
3362 : :
3363 : : #define RAND (rand()/double(RAND_MAX))
3364 : :
3365 : : void main ()
3366 : : {
3367 : : // build box with axes parallel to coordinate axes
3368 : : const int N = 16;
3369 : : const double ex = 1.0;
3370 : : const double ey = 2.0;
3371 : : const double ez = 3.0;
3372 : : Point3 pt[N];
3373 : : pt[0].x = -ex; pt[0].y = -ey; pt[0].z = -ez;
3374 : : pt[1].x = -ex; pt[1].y = +ey; pt[1].z = -ez;
3375 : : pt[2].x = +ex; pt[2].y = +ey; pt[2].z = -ez;
3376 : : pt[3].x = +ex; pt[3].y = -ey; pt[3].z = -ez;
3377 : : pt[4].x = -ex; pt[4].y = -ey; pt[4].z = +ez;
3378 : : pt[5].x = -ex; pt[5].y = +ey; pt[5].z = +ez;
3379 : : pt[6].x = +ex; pt[6].y = +ey; pt[6].z = +ez;
3380 : : pt[7].x = +ex; pt[7].y = -ey; pt[7].z = +ez;
3381 : :
3382 : : for (int k = 8; k < N; k++)
3383 : : {
3384 : : // generate random points inside box to confound initial Gaussian fit
3385 : : pt[k].x = -ex+2.0*ex*RAND;
3386 : : pt[k].y = -ey+2.0*ey*RAND;
3387 : : pt[k].z = -ez+2.0*ez*RAND;
3388 : : }
3389 : :
3390 : : double maxNorm = 0.0;
3391 : : int iMaxNorm = -1;
3392 : : for (int iter = 0; iter < 1024; iter++)
3393 : : {
3394 : : // build arbitrary rotation matrix
3395 : : double angle = RAND;
3396 : : double line[3] = { RAND, RAND, RAND };
3397 : : double rot[3][3];
3398 : : double length = sqrt(line[0]*line[0]+line[1]*line[1]+line[2]*line[2]);
3399 : : line[0] /= length;
3400 : : line[1] /= length;
3401 : : line[2] /= length;
3402 : : AngleAxisToMatrix(angle,line,rot);
3403 : :
3404 : : // rotate box
3405 : : Point3 rpt[N];
3406 : : for (int i = 0; i < N; i++)
3407 : : {
3408 : : rpt[i].x = rot[0][0]*pt[i].x+rot[0][1]*pt[i].y+rot[0][2]*pt[i].z;
3409 : : rpt[i].y = rot[1][0]*pt[i].x+rot[1][1]*pt[i].y+rot[1][2]*pt[i].z;
3410 : : rpt[i].z = rot[2][0]*pt[i].x+rot[2][1]*pt[i].y+rot[2][2]*pt[i].z;
3411 : : }
3412 : :
3413 : : OBBox3 minimal = MinimalBox3(N,rpt);
3414 : :
3415 : : int index[3];
3416 : : if ( minimal.extent[0] <= minimal.extent[1] )
3417 : : {
3418 : : if ( minimal.extent[1] <= minimal.extent[2] )
3419 : : {
3420 : : index[0] = 0;
3421 : : index[1] = 1;
3422 : : index[2] = 2;
3423 : : }
3424 : : else
3425 : : {
3426 : : if ( minimal.extent[0] <= minimal.extent[2] )
3427 : : {
3428 : : index[0] = 0;
3429 : : index[1] = 2;
3430 : : index[2] = 1;
3431 : : }
3432 : : else
3433 : : {
3434 : : index[0] = 2;
3435 : : index[1] = 0;
3436 : : index[2] = 1;
3437 : : }
3438 : : }
3439 : : }
3440 : : else
3441 : : {
3442 : : if ( minimal.extent[0] <= minimal.extent[2] )
3443 : : {
3444 : : index[0] = 1;
3445 : : index[1] = 0;
3446 : : index[2] = 2;
3447 : : }
3448 : : else
3449 : : {
3450 : : if ( minimal.extent[1] <= minimal.extent[2] )
3451 : : {
3452 : : index[0] = 1;
3453 : : index[1] = 2;
3454 : : index[2] = 0;
3455 : : }
3456 : : else
3457 : : {
3458 : : index[0] = 2;
3459 : : index[1] = 1;
3460 : : index[2] = 0;
3461 : : }
3462 : : }
3463 : : }
3464 : :
3465 : : double dx = ex-minimal.extent[index[0]];
3466 : : double dy = ey-minimal.extent[index[1]];
3467 : : double dz = ez-minimal.extent[index[2]];
3468 : : double norm = sqrt(dx*dx+dy*dy+dz*dz);
3469 : : if ( norm > maxNorm )
3470 : : {
3471 : : maxNorm = norm;
3472 : : iMaxNorm = iter;
3473 : : }
3474 : : }
3475 : : }
3476 : :
3477 : : #endif // MINBOX3_TEST
3478 : :
3479 : : // FILE: eigen.cpp
3480 : : //===========================================================================
3481 : : // error handling
3482 : : int mgcEigen::verbose1 = 0;
3483 : : unsigned mgcEigen::error = 0;
3484 : : const unsigned mgcEigen::invalid_size = 0x00000001;
3485 : : const unsigned mgcEigen::allocation_failed = 0x00000002;
3486 : : const unsigned mgcEigen::ql_exceeded = 0x00000004;
3487 : : const char* mgcEigen::message[3] = {
3488 : : "invalid matrix size",
3489 : : "allocation failed",
3490 : : "QL algorithm - exceeded maximum iterations"
3491 : : };
3492 : : //---------------------------------------------------------------------------
3493 : 0 : void mgcEigen::
3494 : : Tridiagonal2 (float** pmat, float* pdiag, float* psubd)
3495 : : {
3496 : : // matrix is already tridiagonal
3497 : :
3498 : 0 : pdiag[0] = pmat[0][0];
3499 : 0 : pdiag[1] = pmat[1][1];
3500 : 0 : psubd[0] = pmat[0][1];
3501 : 0 : psubd[1] = 0;
3502 : 0 : pmat[0][0] = 1; pmat[0][1] = 0;
3503 : 0 : pmat[1][0] = 0; pmat[1][1] = 1;
3504 : 0 : }
3505 : : //---------------------------------------------------------------------------
3506 : 0 : void mgcEigen::
3507 : : Tridiagonal3 (float** pmat, float* pdiag, float* psubd)
3508 : : {
3509 : 0 : float a = pmat[0][0], b = pmat[0][1], c = pmat[0][2],
3510 : 0 : d = pmat[1][1], e = pmat[1][2],
3511 : 0 : f = pmat[2][2];
3512 : :
3513 : 0 : pdiag[0] = a;
3514 : 0 : psubd[2] = 0;
3515 [ # # ]: 0 : if ( c != 0 ) {
3516 : 0 : float ell = float(sqrt(b*b+c*c));
3517 : 0 : b /= ell;
3518 : 0 : c /= ell;
3519 : 0 : float q = 2*b*e+c*(f-d);
3520 : 0 : pdiag[1] = d+c*q;
3521 : 0 : pdiag[2] = f-c*q;
3522 : 0 : psubd[0] = ell;
3523 : 0 : psubd[1] = e-b*q;
3524 : 0 : pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0;
3525 : 0 : pmat[1][0] = 0; pmat[1][1] = b; pmat[1][2] = c;
3526 : 0 : pmat[2][0] = 0; pmat[2][1] = c; pmat[2][2] = -b;
3527 : : }
3528 : : else {
3529 : 0 : pdiag[1] = d;
3530 : 0 : pdiag[2] = f;
3531 : 0 : psubd[0] = b;
3532 : 0 : psubd[1] = e;
3533 : 0 : pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0;
3534 : 0 : pmat[1][0] = 0; pmat[1][1] = 1; pmat[1][2] = 0;
3535 : 0 : pmat[2][0] = 0; pmat[2][1] = 0; pmat[2][2] = 1;
3536 : : }
3537 : 0 : }
3538 : : //---------------------------------------------------------------------------
3539 : 0 : void mgcEigen::
3540 : : Tridiagonal4 (float** pmat, float* pdiag, float* psubd)
3541 : : {
3542 : : // save pmatrix M
3543 : : float
3544 : 0 : a = pmat[0][0], b = pmat[0][1], c = pmat[0][2], d = pmat[0][3],
3545 : 0 : e = pmat[1][1], f = pmat[1][2], g = pmat[1][3],
3546 : 0 : h = pmat[2][2], i = pmat[2][3],
3547 : 0 : j = pmat[3][3];
3548 : :
3549 : 0 : pdiag[0] = a;
3550 : 0 : psubd[3] = 0;
3551 : :
3552 : 0 : pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0; pmat[0][3] = 0;
3553 : 0 : pmat[1][0] = 0;
3554 : 0 : pmat[2][0] = 0;
3555 : 0 : pmat[3][0] = 0;
3556 : :
3557 [ # # ][ # # ]: 0 : if ( c != 0 || d != 0 ) {
3558 : : float q11, q12, q13;
3559 : : float q21, q22, q23;
3560 : : float q31, q32, q33;
3561 : :
3562 : : // build column Q1
3563 : 0 : float len = float(sqrt(b*b+c*c+d*d));
3564 : 0 : q11 = b/len;
3565 : 0 : q21 = c/len;
3566 : 0 : q31 = d/len;
3567 : :
3568 : 0 : psubd[0] = len;
3569 : :
3570 : : // compute S*Q1
3571 : 0 : float v0 = e*q11+f*q21+g*q31;
3572 : 0 : float v1 = f*q11+h*q21+i*q31;
3573 : 0 : float v2 = g*q11+i*q21+j*q31;
3574 : :
3575 : 0 : pdiag[1] = q11*v0+q21*v1+q31*v2;
3576 : :
3577 : : // build column Q3 = Q1x(S*Q1)
3578 : 0 : q13 = q21*v2-q31*v1;
3579 : 0 : q23 = q31*v0-q11*v2;
3580 : 0 : q33 = q11*v1-q21*v0;
3581 : 0 : len = float(sqrt(q13*q13+q23*q23+q33*q33));
3582 [ # # ]: 0 : if ( len > 0 ) {
3583 : 0 : q13 /= len;
3584 : 0 : q23 /= len;
3585 : 0 : q33 /= len;
3586 : :
3587 : : // build column Q2 = Q3xQ1
3588 : 0 : q12 = q23*q31-q33*q21;
3589 : 0 : q22 = q33*q11-q13*q31;
3590 : 0 : q32 = q13*q21-q23*q11;
3591 : :
3592 : 0 : v0 = q12*e+q22*f+q32*g;
3593 : 0 : v1 = q12*f+q22*h+q32*i;
3594 : 0 : v2 = q12*g+q22*i+q32*j;
3595 : 0 : psubd[1] = q11*v0+q21*v1+q31*v2;
3596 : 0 : pdiag[2] = q12*v0+q22*v1+q32*v2;
3597 : 0 : psubd[2] = q13*v0+q23*v1+q33*v2;
3598 : :
3599 : 0 : v0 = q13*e+q23*f+q33*g;
3600 : 0 : v1 = q13*f+q23*h+q33*i;
3601 : 0 : v2 = q13*g+q23*i+q33*j;
3602 : 0 : pdiag[3] = q13*v0+q23*v1+q33*v2;
3603 : : }
3604 : : else { // S*Q1 parallel to Q1, choose any valid Q2 and Q3
3605 : 0 : psubd[1] = 0;
3606 : :
3607 : 0 : len = q21*q21+q31*q31;
3608 [ # # ]: 0 : if ( len > 0 ) {
3609 : 0 : float tmp = q11-1;
3610 : 0 : q12 = -q21;
3611 : 0 : q22 = 1+tmp*q21*q21/len;
3612 : 0 : q32 = tmp*q21*q31/len;
3613 : :
3614 : 0 : q13 = -q31;
3615 : 0 : q23 = q32;
3616 : 0 : q33 = 1+tmp*q31*q31/len;
3617 : :
3618 : 0 : v0 = q12*e+q22*f+q32*g;
3619 : 0 : v1 = q12*f+q22*h+q32*i;
3620 : 0 : v2 = q12*g+q22*i+q32*j;
3621 : 0 : pdiag[2] = q12*v0+q22*v1+q32*v2;
3622 : 0 : psubd[2] = q13*v0+q23*v1+q33*v2;
3623 : :
3624 : 0 : v0 = q13*e+q23*f+q33*g;
3625 : 0 : v1 = q13*f+q23*h+q33*i;
3626 : 0 : v2 = q13*g+q23*i+q33*j;
3627 : 0 : pdiag[3] = q13*v0+q23*v1+q33*v2;
3628 : : }
3629 : : else { // Q1 = (+-1,0,0)
3630 : 0 : q12 = 0; q22 = 1; q32 = 0;
3631 : 0 : q13 = 0; q23 = 0; q33 = 1;
3632 : :
3633 : 0 : pdiag[2] = h;
3634 : 0 : pdiag[3] = j;
3635 : 0 : psubd[2] = i;
3636 : : }
3637 : : }
3638 : :
3639 : 0 : pmat[1][1] = q11; pmat[1][2] = q12; pmat[1][3] = q13;
3640 : 0 : pmat[2][1] = q21; pmat[2][2] = q22; pmat[2][3] = q23;
3641 : 0 : pmat[3][1] = q31; pmat[3][2] = q32; pmat[3][3] = q33;
3642 : : }
3643 : : else {
3644 : 0 : pdiag[1] = e;
3645 : 0 : psubd[0] = b;
3646 : 0 : pmat[1][1] = 1;
3647 : 0 : pmat[2][1] = 0;
3648 : 0 : pmat[3][1] = 0;
3649 : :
3650 [ # # ]: 0 : if ( g != 0 ) {
3651 : 0 : float ell = float(sqrt(f*f+g*g));
3652 : 0 : f /= ell;
3653 : 0 : g /= ell;
3654 : 0 : float Q = 2*f*i+g*(j-h);
3655 : :
3656 : 0 : pdiag[2] = h+g*Q;
3657 : 0 : pdiag[3] = j-g*Q;
3658 : 0 : psubd[1] = ell;
3659 : 0 : psubd[2] = i-f*Q;
3660 : 0 : pmat[1][2] = 0; pmat[1][3] = 0;
3661 : 0 : pmat[2][2] = f; pmat[2][3] = g;
3662 : 0 : pmat[3][2] = g; pmat[3][3] = -f;
3663 : : }
3664 : : else {
3665 : 0 : pdiag[2] = h;
3666 : 0 : pdiag[3] = j;
3667 : 0 : psubd[1] = f;
3668 : 0 : psubd[2] = i;
3669 : 0 : pmat[1][2] = 0; pmat[1][3] = 0;
3670 : 0 : pmat[2][2] = 1; pmat[2][3] = 0;
3671 : 0 : pmat[3][2] = 0; pmat[3][3] = 1;
3672 : : }
3673 : : }
3674 : 0 : }
3675 : : //---------------------------------------------------------------------------
3676 : 0 : void mgcEigen::
3677 : : TridiagonalN (int n, float** pmat, float* pdiag, float* psubd)
3678 : : {
3679 : : int i, j, k, ell;
3680 : :
3681 [ # # ]: 0 : for (i = n-1, ell = n-2; i >= 1; i--, ell--) {
3682 : 0 : float h = 0, scale = 0;
3683 : :
3684 [ # # ]: 0 : if ( ell > 0 ) {
3685 [ # # ]: 0 : for (k = 0; k <= ell; k++)
3686 : 0 : scale += float(fabs(pmat[i][k]));
3687 [ # # ]: 0 : if ( scale == 0 )
3688 : 0 : psubd[i] = pmat[i][ell];
3689 : : else {
3690 [ # # ]: 0 : for (k = 0; k <= ell; k++) {
3691 : 0 : pmat[i][k] /= scale;
3692 : 0 : h += pmat[i][k]*pmat[i][k];
3693 : : }
3694 : 0 : float f = pmat[i][ell];
3695 [ # # ]: 0 : float g = ( f > 0 ? -float(sqrt(h)) : float(sqrt(h)) );
3696 : 0 : psubd[i] = scale*g;
3697 : 0 : h -= f*g;
3698 : 0 : pmat[i][ell] = f-g;
3699 : 0 : f = 0;
3700 [ # # ]: 0 : for (j = 0; j <= ell; j++) {
3701 : 0 : pmat[j][i] = pmat[i][j]/h;
3702 : 0 : g = 0;
3703 [ # # ]: 0 : for (k = 0; k <= j; k++)
3704 : 0 : g += pmat[j][k]*pmat[i][k];
3705 [ # # ]: 0 : for (k = j+1; k <= ell; k++)
3706 : 0 : g += pmat[k][j]*pmat[i][k];
3707 : 0 : psubd[j] = g/h;
3708 : 0 : f += psubd[j]*pmat[i][j];
3709 : : }
3710 : 0 : float hh = f/(h+h);
3711 [ # # ]: 0 : for (j = 0; j <= ell; j++) {
3712 : 0 : f = pmat[i][j];
3713 : 0 : psubd[j] = g = psubd[j] - hh*f;
3714 [ # # ]: 0 : for (k = 0; k <= j; k++)
3715 : 0 : pmat[j][k] -= f*psubd[k]+g*pmat[i][k];
3716 : : }
3717 : : }
3718 : : }
3719 : : else
3720 : 0 : psubd[i] = pmat[i][ell];
3721 : :
3722 : 0 : pdiag[i] = h;
3723 : : }
3724 : :
3725 : 0 : pdiag[0] = psubd[0] = 0;
3726 [ # # ]: 0 : for (i = 0, ell = -1; i <= n-1; i++, ell++) {
3727 [ # # ]: 0 : if ( pdiag[i] ) {
3728 [ # # ]: 0 : for (j = 0; j <= ell; j++) {
3729 : 0 : float sum = 0;
3730 [ # # ]: 0 : for (k = 0; k <= ell; k++)
3731 : 0 : sum += pmat[i][k]*pmat[k][j];
3732 [ # # ]: 0 : for (k = 0; k <= ell; k++)
3733 : 0 : pmat[k][j] -= sum*pmat[k][i];
3734 : : }
3735 : : }
3736 : 0 : pdiag[i] = pmat[i][i];
3737 : 0 : pmat[i][i] = 1;
3738 [ # # ]: 0 : for (j = 0; j <= ell; j++)
3739 : 0 : pmat[j][i] = pmat[i][j] = 0;
3740 : : }
3741 : :
3742 : : // re-ordering if mgcEigen::QLAlgorithm is used subsequently
3743 [ # # ]: 0 : for (i = 1, ell = 0; i < n; i++, ell++)
3744 : 0 : psubd[ell] = psubd[i];
3745 : 0 : psubd[n-1] = 0;
3746 : 0 : }
3747 : : //---------------------------------------------------------------------------
3748 : 0 : void mgcEigen::
3749 : : QLAlgorithm (int n, float* pdiag, float* psubd, float** pmat)
3750 : : {
3751 : 0 : const int eigen_maxiter = 30;
3752 : :
3753 [ # # ]: 0 : for (int ell = 0; ell < n; ell++) {
3754 : : int iter;
3755 [ # # ]: 0 : for (iter = 0; iter < eigen_maxiter; iter++) {
3756 : : int m;
3757 [ # # ]: 0 : for (m = ell; m <= n-2; m++) {
3758 : 0 : float dd = float(fabs(pdiag[m])+fabs(pdiag[m+1]));
3759 [ # # ]: 0 : if ( (float)(fabs(psubd[m])+dd) == dd )
3760 : 0 : break;
3761 : : }
3762 [ # # ]: 0 : if ( m == ell )
3763 : 0 : break;
3764 : :
3765 : 0 : float g = (pdiag[ell+1]-pdiag[ell])/(2*psubd[ell]);
3766 : 0 : float r = float(sqrt(g*g+1));
3767 [ # # ]: 0 : if ( g < 0 )
3768 : 0 : g = pdiag[m]-pdiag[ell]+psubd[ell]/(g-r);
3769 : : else
3770 : 0 : g = pdiag[m]-pdiag[ell]+psubd[ell]/(g+r);
3771 : 0 : float s = 1, c = 1, p = 0;
3772 [ # # ]: 0 : for (int i = m-1; i >= ell; i--) {
3773 : 0 : float f = s*psubd[i], b = c*psubd[i];
3774 [ # # ]: 0 : if ( fabs(f) >= fabs(g) ) {
3775 : 0 : c = g/f;
3776 : 0 : r = float(sqrt(c*c+1));
3777 : 0 : psubd[i+1] = f*r;
3778 : 0 : c *= (s = 1/r);
3779 : : }
3780 : : else {
3781 : 0 : s = f/g;
3782 : 0 : r = float(sqrt(s*s+1));
3783 : 0 : psubd[i+1] = g*r;
3784 : 0 : s *= (c = 1/r);
3785 : : }
3786 : 0 : g = pdiag[i+1]-p;
3787 : 0 : r = (pdiag[i]-g)*s+2*b*c;
3788 : 0 : p = s*r;
3789 : 0 : pdiag[i+1] = g+p;
3790 : 0 : g = c*r-b;
3791 : :
3792 [ # # ]: 0 : for (int k = 0; k < n; k++) {
3793 : 0 : f = pmat[k][i+1];
3794 : 0 : pmat[k][i+1] = s*pmat[k][i]+c*f;
3795 : 0 : pmat[k][i] = c*pmat[k][i]-s*f;
3796 : : }
3797 : : }
3798 : 0 : pdiag[ell] -= p;
3799 : 0 : psubd[ell] = g;
3800 : 0 : psubd[m] = 0;
3801 : : }
3802 [ # # ]: 0 : if ( iter == eigen_maxiter ) {
3803 : 0 : Report(ql_exceeded);
3804 : 0 : return;
3805 : : }
3806 : : }
3807 : : }
3808 : : //---------------------------------------------------------------------------
3809 : 0 : void mgcEigen::
3810 : : DecreasingSort (int n, float* eigval, float** eigvec)
3811 : : {
3812 : : // sort eigenvalues in decreasing order, e[0] >= ... >= e[n-1]
3813 [ # # ]: 0 : for (int i = 0, k; i <= n-2; i++) {
3814 : : // locate maximum eigenvalue
3815 : 0 : float max = eigval[k=i];
3816 : : int j;
3817 [ # # ]: 0 : for (j = i+1; j < n; j++)
3818 [ # # ]: 0 : if ( eigval[j] > max )
3819 : 0 : max = eigval[k=j];
3820 : :
3821 [ # # ]: 0 : if ( k != i ) {
3822 : : // swap eigenvalues
3823 : 0 : eigval[k] = eigval[i];
3824 : 0 : eigval[i] = max;
3825 : :
3826 : : // swap eigenvectors
3827 [ # # ]: 0 : for (j = 0; j < n; j++) {
3828 : 0 : float tmp = eigvec[j][i];
3829 : 0 : eigvec[j][i] = eigvec[j][k];
3830 : 0 : eigvec[j][k] = tmp;
3831 : : }
3832 : : }
3833 : : }
3834 : 0 : }
3835 : : //---------------------------------------------------------------------------
3836 : 0 : void mgcEigen::
3837 : : IncreasingSort (int n, float* eigval, float** eigvec)
3838 : : {
3839 : : // sort eigenvalues in increasing order, e[0] <= ... <= e[n-1]
3840 [ # # ]: 0 : for (int i = 0, k; i <= n-2; i++) {
3841 : : // locate minimum eigenvalue
3842 : 0 : float min = eigval[k=i];
3843 : : int j;
3844 [ # # ]: 0 : for (j = i+1; j < n; j++)
3845 [ # # ]: 0 : if ( eigval[j] < min )
3846 : 0 : min = eigval[k=j];
3847 : :
3848 [ # # ]: 0 : if ( k != i ) {
3849 : : // swap eigenvalues
3850 : 0 : eigval[k] = eigval[i];
3851 : 0 : eigval[i] = min;
3852 : :
3853 : : // swap eigenvectors
3854 [ # # ]: 0 : for (j = 0; j < n; j++) {
3855 : 0 : float tmp = eigvec[j][i];
3856 : 0 : eigvec[j][i] = eigvec[j][k];
3857 : 0 : eigvec[j][k] = tmp;
3858 : : }
3859 : : }
3860 : : }
3861 : 0 : }
3862 : : //---------------------------------------------------------------------------
3863 : 0 : int mgcEigen::
3864 : : Number (unsigned single_error)
3865 : : {
3866 : : int result;
3867 [ # # ]: 0 : for (result = -1; single_error; single_error >>= 1)
3868 : 0 : result++;
3869 : 0 : return result;
3870 : : }
3871 : : //---------------------------------------------------------------------------
3872 : 0 : void mgcEigen::
3873 : : Report (unsigned single_error)
3874 : : {
3875 [ # # ]: 0 : if ( mgcEigen::verbose1 )
3876 : 0 : cout << "mgcEigen: " << message[Number(single_error)] << endl;
3877 : : else {
3878 [ # # ][ # # ]: 0 : ofstream ostr("eigen.err",ios::out|ios::app);
3879 [ # # ][ # # ]: 0 : ostr << "mgcEigen: " << message[Number(single_error)] << endl;
[ # # ][ # # ]
[ # # ]
3880 : : }
3881 : 0 : error |= single_error;
3882 : 0 : }
3883 : : //---------------------------------------------------------------------------
3884 : 0 : void mgcEigen::
3885 : : Report (ostream& ostr)
3886 : : {
3887 [ # # ]: 0 : for (unsigned single_error = 1; single_error; single_error <<= 1)
3888 [ # # ]: 0 : if ( error & single_error )
3889 : 0 : ostr << "mgcEigen: " << message[Number(single_error)] << endl;
3890 : :
3891 : 0 : error = 0;
3892 : 0 : }
3893 : : //===========================================================================
3894 : : // error handling
3895 : : int mgcEigenD::verbose1 = 0;
3896 : : unsigned mgcEigenD::error = 0;
3897 : : const unsigned mgcEigenD::invalid_size = 0x00000001;
3898 : : const unsigned mgcEigenD::allocation_failed = 0x00000002;
3899 : : const unsigned mgcEigenD::ql_exceeded = 0x00000004;
3900 : : const char* mgcEigenD::message[3] = {
3901 : : "invalid matrix size",
3902 : : "allocation failed",
3903 : : "QL algorithm - exceeded maximum iterations"
3904 : : };
3905 : : //---------------------------------------------------------------------------
3906 : 0 : mgcEigenD::
3907 : : mgcEigenD (int _size)
3908 : : {
3909 [ # # ]: 0 : if ( (size = _size) <= 1 ) {
3910 : 0 : Report(invalid_size);
3911 : 0 : return;
3912 : : }
3913 [ # # ][ # # ]: 0 : if ( (mat = new double*[size]) == 0 ) {
3914 : 0 : Report(allocation_failed);
3915 : 0 : return;
3916 : : }
3917 [ # # ]: 0 : for (int d = 0; d < size; d++)
3918 [ # # ][ # # ]: 0 : if ( (mat[d] = new double[size]) == 0 ) {
3919 : 0 : Report(allocation_failed);
3920 : 0 : return;
3921 : : }
3922 [ # # ][ # # ]: 0 : if ( (diag = new double[size]) == 0 ) {
3923 : 0 : Report(allocation_failed);
3924 : 0 : return;
3925 : : }
3926 [ # # ][ # # ]: 0 : if ( (subd = new double[size]) == 0 ) {
3927 : 0 : Report(allocation_failed);
3928 : 0 : return;
3929 : : }
3930 : : }
3931 : : //---------------------------------------------------------------------------
3932 : 0 : mgcEigenD::
3933 : : ~mgcEigenD ()
3934 : : {
3935 [ # # ]: 0 : delete[] subd;
3936 [ # # ]: 0 : delete[] diag;
3937 [ # # ]: 0 : for (int d = 0; d < size; d++)
3938 [ # # ]: 0 : delete[] mat[d];
3939 [ # # ]: 0 : delete[] mat;
3940 : 0 : }
3941 : : //---------------------------------------------------------------------------
3942 : 0 : void mgcEigenD::
3943 : : Tridiagonal2 (double** pmat, double* pdiag, double* psubd)
3944 : : {
3945 : : // matrix is already tridiagonal
3946 : :
3947 : 0 : pdiag[0] = pmat[0][0];
3948 : 0 : pdiag[1] = pmat[1][1];
3949 : 0 : psubd[0] = pmat[0][1];
3950 : 0 : psubd[1] = 0;
3951 : 0 : pmat[0][0] = 1; pmat[0][1] = 0;
3952 : 0 : pmat[1][0] = 0; pmat[1][1] = 1;
3953 : 0 : }
3954 : : //---------------------------------------------------------------------------
3955 : 0 : void mgcEigenD::
3956 : : Tridiagonal3 (double** pmat, double* pdiag, double* psubd)
3957 : : {
3958 : 0 : double a = pmat[0][0], b = pmat[0][1], c = pmat[0][2],
3959 : 0 : d = pmat[1][1], e = pmat[1][2], f = pmat[2][2];
3960 : :
3961 : 0 : pdiag[0] = a;
3962 : 0 : psubd[2] = 0;
3963 [ # # ]: 0 : if ( c != 0 ) {
3964 : 0 : double ell = sqrt(b*b+c*c);
3965 : 0 : b /= ell;
3966 : 0 : c /= ell;
3967 : 0 : double q = 2*b*e+c*(f-d);
3968 : 0 : pdiag[1] = d+c*q;
3969 : 0 : pdiag[2] = f-c*q;
3970 : 0 : psubd[0] = ell;
3971 : 0 : psubd[1] = e-b*q;
3972 : 0 : pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0;
3973 : 0 : pmat[1][0] = 0; pmat[1][1] = b; pmat[1][2] = c;
3974 : 0 : pmat[2][0] = 0; pmat[2][1] = c; pmat[2][2] = -b;
3975 : : }
3976 : : else {
3977 : 0 : pdiag[1] = d;
3978 : 0 : pdiag[2] = f;
3979 : 0 : psubd[0] = b;
3980 : 0 : psubd[1] = e;
3981 : 0 : pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0;
3982 : 0 : pmat[1][0] = 0; pmat[1][1] = 1; pmat[1][2] = 0;
3983 : 0 : pmat[2][0] = 0; pmat[2][1] = 0; pmat[2][2] = 1;
3984 : : }
3985 : 0 : }
3986 : : //---------------------------------------------------------------------------
3987 : 0 : void mgcEigenD::
3988 : : Tridiagonal4 (double** pmat, double* pdiag, double* psubd)
3989 : : {
3990 : : // save pmatrix M
3991 : : double
3992 : 0 : a = pmat[0][0], b = pmat[0][1], c = pmat[0][2], d = pmat[0][3],
3993 : 0 : e = pmat[1][1], f = pmat[1][2], g = pmat[1][3],
3994 : 0 : h = pmat[2][2], i = pmat[2][3], j = pmat[3][3];
3995 : :
3996 : 0 : pdiag[0] = a;
3997 : 0 : psubd[3] = 0;
3998 : :
3999 : 0 : pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0; pmat[0][3] = 0;
4000 : 0 : pmat[1][0] = 0;
4001 : 0 : pmat[2][0] = 0;
4002 : 0 : pmat[3][0] = 0;
4003 : :
4004 [ # # ][ # # ]: 0 : if ( c != 0 || d != 0 ) {
4005 : : double q11, q12, q13;
4006 : : double q21, q22, q23;
4007 : : double q31, q32, q33;
4008 : :
4009 : : // build column Q1
4010 : 0 : double len = sqrt(b*b+c*c+d*d);
4011 : 0 : q11 = b/len;
4012 : 0 : q21 = c/len;
4013 : 0 : q31 = d/len;
4014 : :
4015 : 0 : psubd[0] = len;
4016 : :
4017 : : // compute S*Q1
4018 : 0 : double v0 = e*q11+f*q21+g*q31;
4019 : 0 : double v1 = f*q11+h*q21+i*q31;
4020 : 0 : double v2 = g*q11+i*q21+j*q31;
4021 : :
4022 : 0 : pdiag[1] = q11*v0+q21*v1+q31*v2;
4023 : :
4024 : : // build column Q3 = Q1x(S*Q1)
4025 : 0 : q13 = q21*v2-q31*v1;
4026 : 0 : q23 = q31*v0-q11*v2;
4027 : 0 : q33 = q11*v1-q21*v0;
4028 : 0 : len = sqrt(q13*q13+q23*q23+q33*q33);
4029 [ # # ]: 0 : if ( len > 0 ) {
4030 : 0 : q13 /= len;
4031 : 0 : q23 /= len;
4032 : 0 : q33 /= len;
4033 : :
4034 : : // build column Q2 = Q3xQ1
4035 : 0 : q12 = q23*q31-q33*q21;
4036 : 0 : q22 = q33*q11-q13*q31;
4037 : 0 : q32 = q13*q21-q23*q11;
4038 : :
4039 : 0 : v0 = q12*e+q22*f+q32*g;
4040 : 0 : v1 = q12*f+q22*h+q32*i;
4041 : 0 : v2 = q12*g+q22*i+q32*j;
4042 : 0 : psubd[1] = q11*v0+q21*v1+q31*v2;
4043 : 0 : pdiag[2] = q12*v0+q22*v1+q32*v2;
4044 : 0 : psubd[2] = q13*v0+q23*v1+q33*v2;
4045 : :
4046 : 0 : v0 = q13*e+q23*f+q33*g;
4047 : 0 : v1 = q13*f+q23*h+q33*i;
4048 : 0 : v2 = q13*g+q23*i+q33*j;
4049 : 0 : pdiag[3] = q13*v0+q23*v1+q33*v2;
4050 : : }
4051 : : else { // S*Q1 parallel to Q1, choose any valid Q2 and Q3
4052 : 0 : psubd[1] = 0;
4053 : :
4054 : 0 : len = q21*q21+q31*q31;
4055 [ # # ]: 0 : if ( len > 0 ) {
4056 : 0 : double tmp = q11-1;
4057 : 0 : q12 = -q21;
4058 : 0 : q22 = 1+tmp*q21*q21/len;
4059 : 0 : q32 = tmp*q21*q31/len;
4060 : :
4061 : 0 : q13 = -q31;
4062 : 0 : q23 = q32;
4063 : 0 : q33 = 1+tmp*q31*q31/len;
4064 : :
4065 : 0 : v0 = q12*e+q22*f+q32*g;
4066 : 0 : v1 = q12*f+q22*h+q32*i;
4067 : 0 : v2 = q12*g+q22*i+q32*j;
4068 : 0 : pdiag[2] = q12*v0+q22*v1+q32*v2;
4069 : 0 : psubd[2] = q13*v0+q23*v1+q33*v2;
4070 : :
4071 : 0 : v0 = q13*e+q23*f+q33*g;
4072 : 0 : v1 = q13*f+q23*h+q33*i;
4073 : 0 : v2 = q13*g+q23*i+q33*j;
4074 : 0 : pdiag[3] = q13*v0+q23*v1+q33*v2;
4075 : : }
4076 : : else { // Q1 = (+-1,0,0)
4077 : 0 : q12 = 0; q22 = 1; q32 = 0;
4078 : 0 : q13 = 0; q23 = 0; q33 = 1;
4079 : :
4080 : 0 : pdiag[2] = h;
4081 : 0 : pdiag[3] = j;
4082 : 0 : psubd[2] = i;
4083 : : }
4084 : : }
4085 : :
4086 : 0 : pmat[1][1] = q11; pmat[1][2] = q12; pmat[1][3] = q13;
4087 : 0 : pmat[2][1] = q21; pmat[2][2] = q22; pmat[2][3] = q23;
4088 : 0 : pmat[3][1] = q31; pmat[3][2] = q32; pmat[3][3] = q33;
4089 : : }
4090 : : else {
4091 : 0 : pdiag[1] = e;
4092 : 0 : psubd[0] = b;
4093 : 0 : pmat[1][1] = 1;
4094 : 0 : pmat[2][1] = 0;
4095 : 0 : pmat[3][1] = 0;
4096 : :
4097 [ # # ]: 0 : if ( g != 0 ) {
4098 : 0 : double ell = sqrt(f*f+g*g);
4099 : 0 : f /= ell;
4100 : 0 : g /= ell;
4101 : 0 : double Q = 2*f*i+g*(j-h);
4102 : :
4103 : 0 : pdiag[2] = h+g*Q;
4104 : 0 : pdiag[3] = j-g*Q;
4105 : 0 : psubd[1] = ell;
4106 : 0 : psubd[2] = i-f*Q;
4107 : 0 : pmat[1][2] = 0; pmat[1][3] = 0;
4108 : 0 : pmat[2][2] = f; pmat[2][3] = g;
4109 : 0 : pmat[3][2] = g; pmat[3][3] = -f;
4110 : : }
4111 : : else {
4112 : 0 : pdiag[2] = h;
4113 : 0 : pdiag[3] = j;
4114 : 0 : psubd[1] = f;
4115 : 0 : psubd[2] = i;
4116 : 0 : pmat[1][2] = 0; pmat[1][3] = 0;
4117 : 0 : pmat[2][2] = 1; pmat[2][3] = 0;
4118 : 0 : pmat[3][2] = 0; pmat[3][3] = 1;
4119 : : }
4120 : : }
4121 : 0 : }
4122 : : //---------------------------------------------------------------------------
4123 : 0 : void mgcEigenD::
4124 : : TridiagonalN (int n, double** pmat, double* pdiag, double* psubd)
4125 : : {
4126 : : int i, j, k, ell;
4127 : :
4128 [ # # ]: 0 : for (i = n-1, ell = n-2; i >= 1; i--, ell--) {
4129 : 0 : double h = 0, scale = 0;
4130 : :
4131 [ # # ]: 0 : if ( ell > 0 ) {
4132 [ # # ]: 0 : for (k = 0; k <= ell; k++)
4133 : 0 : scale += fabs(pmat[i][k]);
4134 [ # # ]: 0 : if ( scale == 0 )
4135 : 0 : psubd[i] = pmat[i][ell];
4136 : : else {
4137 [ # # ]: 0 : for (k = 0; k <= ell; k++) {
4138 : 0 : pmat[i][k] /= scale;
4139 : 0 : h += pmat[i][k]*pmat[i][k];
4140 : : }
4141 : 0 : double f = pmat[i][ell];
4142 [ # # ]: 0 : double g = ( f > 0 ? -sqrt(h) : sqrt(h) );
4143 : 0 : psubd[i] = scale*g;
4144 : 0 : h -= f*g;
4145 : 0 : pmat[i][ell] = f-g;
4146 : 0 : f = 0;
4147 [ # # ]: 0 : for (j = 0; j <= ell; j++) {
4148 : 0 : pmat[j][i] = pmat[i][j]/h;
4149 : 0 : g = 0;
4150 [ # # ]: 0 : for (k = 0; k <= j; k++)
4151 : 0 : g += pmat[j][k]*pmat[i][k];
4152 [ # # ]: 0 : for (k = j+1; k <= ell; k++)
4153 : 0 : g += pmat[k][j]*pmat[i][k];
4154 : 0 : psubd[j] = g/h;
4155 : 0 : f += psubd[j]*pmat[i][j];
4156 : : }
4157 : 0 : double hh = f/(h+h);
4158 [ # # ]: 0 : for (j = 0; j <= ell; j++) {
4159 : 0 : f = pmat[i][j];
4160 : 0 : psubd[j] = g = psubd[j] - hh*f;
4161 [ # # ]: 0 : for (k = 0; k <= j; k++)
4162 : 0 : pmat[j][k] -= f*psubd[k]+g*pmat[i][k];
4163 : : }
4164 : : }
4165 : : }
4166 : : else
4167 : 0 : psubd[i] = pmat[i][ell];
4168 : :
4169 : 0 : pdiag[i] = h;
4170 : : }
4171 : :
4172 : 0 : pdiag[0] = psubd[0] = 0;
4173 [ # # ]: 0 : for (i = 0, ell = -1; i <= n-1; i++, ell++) {
4174 [ # # ]: 0 : if ( pdiag[i] ) {
4175 [ # # ]: 0 : for (j = 0; j <= ell; j++) {
4176 : 0 : double sum = 0;
4177 [ # # ]: 0 : for (k = 0; k <= ell; k++)
4178 : 0 : sum += pmat[i][k]*pmat[k][j];
4179 [ # # ]: 0 : for (k = 0; k <= ell; k++)
4180 : 0 : pmat[k][j] -= sum*pmat[k][i];
4181 : : }
4182 : : }
4183 : 0 : pdiag[i] = pmat[i][i];
4184 : 0 : pmat[i][i] = 1;
4185 [ # # ]: 0 : for (j = 0; j <= ell; j++)
4186 : 0 : pmat[j][i] = pmat[i][j] = 0;
4187 : : }
4188 : :
4189 : : // re-ordering if mgcEigenD::QLAlgorithm is used subsequently
4190 [ # # ]: 0 : for (i = 1, ell = 0; i < n; i++, ell++)
4191 : 0 : psubd[ell] = psubd[i];
4192 : 0 : psubd[n-1] = 0;
4193 : 0 : }
4194 : : //---------------------------------------------------------------------------
4195 : 0 : void mgcEigenD::
4196 : : QLAlgorithm (int n, double* pdiag, double* psubd, double** pmat)
4197 : : {
4198 : 0 : const int eigen_maxiter = 30;
4199 : :
4200 [ # # ]: 0 : for (int ell = 0; ell < n; ell++) {
4201 : : int iter;
4202 [ # # ]: 0 : for (iter = 0; iter < eigen_maxiter; iter++) {
4203 : : int m;
4204 [ # # ]: 0 : for (m = ell; m <= n-2; m++) {
4205 : 0 : double dd = fabs(pdiag[m])+fabs(pdiag[m+1]);
4206 [ # # ]: 0 : if ( (double)(fabs(psubd[m])+dd) == dd )
4207 : 0 : break;
4208 : : }
4209 [ # # ]: 0 : if ( m == ell )
4210 : 0 : break;
4211 : :
4212 : 0 : double g = (pdiag[ell+1]-pdiag[ell])/(2*psubd[ell]);
4213 : 0 : double r = sqrt(g*g+1);
4214 [ # # ]: 0 : if ( g < 0 )
4215 : 0 : g = pdiag[m]-pdiag[ell]+psubd[ell]/(g-r);
4216 : : else
4217 : 0 : g = pdiag[m]-pdiag[ell]+psubd[ell]/(g+r);
4218 : 0 : double s = 1, c = 1, p = 0;
4219 [ # # ]: 0 : for (int i = m-1; i >= ell; i--) {
4220 : 0 : double f = s*psubd[i], b = c*psubd[i];
4221 [ # # ]: 0 : if ( fabs(f) >= fabs(g) ) {
4222 : 0 : c = g/f;
4223 : 0 : r = sqrt(c*c+1);
4224 : 0 : psubd[i+1] = f*r;
4225 : 0 : c *= (s = 1/r);
4226 : : }
4227 : : else {
4228 : 0 : s = f/g;
4229 : 0 : r = sqrt(s*s+1);
4230 : 0 : psubd[i+1] = g*r;
4231 : 0 : s *= (c = 1/r);
4232 : : }
4233 : 0 : g = pdiag[i+1]-p;
4234 : 0 : r = (pdiag[i]-g)*s+2*b*c;
4235 : 0 : p = s*r;
4236 : 0 : pdiag[i+1] = g+p;
4237 : 0 : g = c*r-b;
4238 : :
4239 [ # # ]: 0 : for (int k = 0; k < n; k++) {
4240 : 0 : f = pmat[k][i+1];
4241 : 0 : pmat[k][i+1] = s*pmat[k][i]+c*f;
4242 : 0 : pmat[k][i] = c*pmat[k][i]-s*f;
4243 : : }
4244 : : }
4245 : 0 : pdiag[ell] -= p;
4246 : 0 : psubd[ell] = g;
4247 : 0 : psubd[m] = 0;
4248 : : }
4249 [ # # ]: 0 : if ( iter == eigen_maxiter ) {
4250 : 0 : Report(ql_exceeded);
4251 : 0 : return;
4252 : : }
4253 : : }
4254 : : }
4255 : : //---------------------------------------------------------------------------
4256 : 0 : void mgcEigenD::
4257 : : DecreasingSort (int n, double* eigval, double** eigvec)
4258 : : {
4259 : : // sort eigenvalues in decreasing order, e[0] >= ... >= e[n-1]
4260 [ # # ]: 0 : for (int i = 0, k; i <= n-2; i++) {
4261 : : // locate maximum eigenvalue
4262 : 0 : double max = eigval[k=i];
4263 : : int j;
4264 [ # # ]: 0 : for (j = i+1; j < n; j++)
4265 [ # # ]: 0 : if ( eigval[j] > max )
4266 : 0 : max = eigval[k=j];
4267 : :
4268 [ # # ]: 0 : if ( k != i ) {
4269 : : // swap eigenvalues
4270 : 0 : eigval[k] = eigval[i];
4271 : 0 : eigval[i] = max;
4272 : :
4273 : : // swap eigenvectors
4274 [ # # ]: 0 : for (j = 0; j < n; j++) {
4275 : 0 : double tmp = eigvec[j][i];
4276 : 0 : eigvec[j][i] = eigvec[j][k];
4277 : 0 : eigvec[j][k] = tmp;
4278 : : }
4279 : : }
4280 : : }
4281 : 0 : }
4282 : : //---------------------------------------------------------------------------
4283 : 0 : void mgcEigenD::
4284 : : IncreasingSort (int n, double* eigval, double** eigvec)
4285 : : {
4286 : : // sort eigenvalues in increasing order, e[0] <= ... <= e[n-1]
4287 [ # # ]: 0 : for (int i = 0, k; i <= n-2; i++) {
4288 : : // locate minimum eigenvalue
4289 : 0 : double min = eigval[k=i];
4290 : : int j;
4291 [ # # ]: 0 : for (j = i+1; j < n; j++)
4292 [ # # ]: 0 : if ( eigval[j] < min )
4293 : 0 : min = eigval[k=j];
4294 : :
4295 [ # # ]: 0 : if ( k != i ) {
4296 : : // swap eigenvalues
4297 : 0 : eigval[k] = eigval[i];
4298 : 0 : eigval[i] = min;
4299 : :
4300 : : // swap eigenvectors
4301 [ # # ]: 0 : for (j = 0; j < n; j++) {
4302 : 0 : double tmp = eigvec[j][i];
4303 : 0 : eigvec[j][i] = eigvec[j][k];
4304 : 0 : eigvec[j][k] = tmp;
4305 : : }
4306 : : }
4307 : : }
4308 : 0 : }
4309 : : //---------------------------------------------------------------------------
4310 : 0 : mgcEigenD& mgcEigenD::
4311 : : Matrix (double** inmat)
4312 : : {
4313 [ # # ]: 0 : for (int row = 0; row < size; row++)
4314 [ # # ]: 0 : for (int col = 0; col < size; col++)
4315 : 0 : mat[row][col] = inmat[row][col];
4316 : 0 : return *this;
4317 : : }
4318 : : //---------------------------------------------------------------------------
4319 : 0 : void mgcEigenD::
4320 : : EigenStuff3 ()
4321 : : {
4322 : 0 : Tridiagonal3(mat,diag,subd);
4323 : 0 : QLAlgorithm(size,diag,subd,mat);
4324 : 0 : }
4325 : : //---------------------------------------------------------------------------
4326 : 0 : int mgcEigenD::
4327 : : Number (unsigned single_error)
4328 : : {
4329 : : int result;
4330 [ # # ]: 0 : for (result = -1; single_error; single_error >>= 1)
4331 : 0 : result++;
4332 : 0 : return result;
4333 : : }
4334 : : //---------------------------------------------------------------------------
4335 : 0 : void mgcEigenD::
4336 : : Report (unsigned single_error)
4337 : : {
4338 [ # # ]: 0 : if ( mgcEigenD::verbose1 )
4339 : 0 : cout << "mgcEigenD: " << message[Number(single_error)] << endl;
4340 : : else {
4341 [ # # ][ # # ]: 0 : ofstream ostr("eigen.err",ios::out|ios::app);
4342 [ # # ][ # # ]: 0 : ostr << "mgcEigenD: " << message[Number(single_error)] << endl;
[ # # ][ # # ]
[ # # ]
4343 : : }
4344 : 0 : error |= single_error;
4345 : 0 : }
4346 : : //---------------------------------------------------------------------------
4347 : 0 : void mgcEigenD::
4348 : : Report (ostream& ostr)
4349 : : {
4350 [ # # ]: 0 : for (unsigned single_error = 1; single_error; single_error <<= 1)
4351 [ # # ]: 0 : if ( error & single_error )
4352 : 0 : ostr << "mgcEigenD: " << message[Number(single_error)] << endl;
4353 : :
4354 : 0 : error = 0;
4355 : 0 : }
4356 : : //===========================================================================
4357 : :
4358 : : #ifdef EIGEN_TEST
4359 : :
4360 : : int main ()
4361 : : {
4362 : : mgcEigenD eig(3);
4363 : :
4364 : : eig.Matrix(0,0) = 2; eig.Matrix(0,1) = 1; eig.Matrix(0,2) = 1;
4365 : : eig.Matrix(1,0) = 1; eig.Matrix(1,1) = 2; eig.Matrix(1,2) = 1;
4366 : : eig.Matrix(2,0) = 1; eig.Matrix(2,1) = 1; eig.Matrix(2,2) = 2;
4367 : :
4368 : : eig.IncrSortEigenStuff3();
4369 : :
4370 : : cout.setf(ios::fixed);
4371 : :
4372 : : cout << "eigenvalues = " << endl;
4373 : : for (int row = 0; row < 3; row++)
4374 : : cout << eig.Eigenvalue(row) << ' ';
4375 : : cout << endl;
4376 : :
4377 : : cout << "eigenvectors = " << endl;
4378 : : for (row = 0; row < 3; row++) {
4379 : : for (int col = 0; col < 3; col++)
4380 : : cout << eig.Eigenvector(row,col) << ' ';
4381 : : cout << endl;
4382 : : }
4383 : :
4384 : : // eigenvalues =
4385 : : // 1.000000 1.000000 4.000000
4386 : : // eigenvectors =
4387 : : // 0.411953 0.704955 0.577350
4388 : : // 0.404533 -0.709239 0.577350
4389 : : // -0.816485 0.004284 0.577350
4390 : :
4391 : : return 0;
4392 : : }
4393 : : #endif
4394 : : #endif
4395 : :
4396 : : // FILE: tri3tri3.cpp
4397 : : //---------------------------------------------------------------------------
4398 : : // MinTriangleTriangle
4399 : : //
4400 : : // The quadratic form representing the squared distance between two planes
4401 : : // never has an isolated global minimum. The generic case is for it to
4402 : : // have an entire line of zeros (the line of intersection of the two
4403 : : // planes). Therefore, it is sufficient to compare edges of each triangle
4404 : : // with the entire other triangle, looking for the minimum distance.
4405 : : //---------------------------------------------------------------------------
4406 : :
4407 : : // NOTE: This code is not fully optimized as is the code in pt3lin3,
4408 : : // pt3tri3, and lin3lin3.
4409 : :
4410 : : //---------------------------------------------------------------------------
4411 : : double
4412 : 0 : AnalyticGeometryTool::MinTriangleTriangle (Triangle3& tri0,
4413 : : Triangle3& tri1,
4414 : : double& s, double& t, double& u,
4415 : : double& v)
4416 : : {
4417 : : double s0, t0, u0, v0, min, min0;
4418 : : Line3 line;
4419 : :
4420 : : // compare edges of tri0 against all of tri1
4421 : 0 : line.b = tri0.b;
4422 : 0 : line.m = tri0.e0;
4423 [ # # ]: 0 : min = MinLineSegmentTriangle(line,tri1,s,u,v);
4424 : 0 : t = 0;
4425 : :
4426 : 0 : line.m = tri0.e1;
4427 [ # # ]: 0 : min0 = MinLineSegmentTriangle(line,tri1,t0,u0,v0);
4428 : 0 : s0 = 0;
4429 [ # # ]: 0 : if ( min0 < min )
4430 : : {
4431 : 0 : min = min0;
4432 : 0 : s = s0;
4433 : 0 : t = t0;
4434 : 0 : u = u0;
4435 : 0 : v = v0;
4436 : : }
4437 : :
4438 [ # # ]: 0 : line.b = line.b + tri0.e0;
4439 [ # # ]: 0 : line.m = line.m - tri0.e0;
4440 [ # # ]: 0 : min0 = MinLineSegmentTriangle(line,tri1,t0,u0,v0);
4441 : 0 : s0 = 1-t0;
4442 [ # # ]: 0 : if ( min0 < min )
4443 : : {
4444 : 0 : min = min0;
4445 : 0 : s = s0;
4446 : 0 : t = t0;
4447 : 0 : u = u0;
4448 : 0 : v = v0;
4449 : : }
4450 : :
4451 : : // compare edges of tri1 against all of tri0
4452 : 0 : line.b = tri1.b;
4453 : 0 : line.m = tri1.e0;
4454 [ # # ]: 0 : min0 = MinLineSegmentTriangle(line,tri0,u0,s0,t0);
4455 : 0 : v0 = 0;
4456 [ # # ]: 0 : if ( min0 < min )
4457 : : {
4458 : 0 : min = min0;
4459 : 0 : s = s0;
4460 : 0 : t = t0;
4461 : 0 : u = u0;
4462 : 0 : v = v0;
4463 : : }
4464 : :
4465 : 0 : line.m = tri1.e1;
4466 [ # # ]: 0 : min0 = MinLineSegmentTriangle(line,tri0,v0,s0,t0);
4467 : 0 : u0 = 0;
4468 [ # # ]: 0 : if ( min0 < min )
4469 : : {
4470 : 0 : min = min0;
4471 : 0 : s = s0;
4472 : 0 : t = t0;
4473 : 0 : u = u0;
4474 : 0 : v = v0;
4475 : : }
4476 : :
4477 [ # # ]: 0 : line.b = line.b + tri1.e0;
4478 [ # # ]: 0 : line.m = line.m - tri1.e0;
4479 [ # # ]: 0 : min0 = MinLineSegmentTriangle(line,tri0,v0,s0,t0);
4480 : 0 : u0 = 1-v0;
4481 [ # # ]: 0 : if ( min0 < min )
4482 : : {
4483 : 0 : min = min0;
4484 : 0 : s = s0;
4485 : 0 : t = t0;
4486 : 0 : u = u0;
4487 : 0 : v = v0;
4488 : : }
4489 : :
4490 : 0 : return min;
4491 : : }
4492 : :
4493 : : //---------------------------------------------------------------------------
4494 : :
4495 : : #ifdef TRI3TRI3_TEST
4496 : :
4497 : : //#define RAND (2.0f*rand()/double(RAND_MAX)-1)
4498 : :
4499 : : //ofstream ostr("data.txt");
4500 : :
4501 : : void main ()
4502 : : {
4503 : : Triangle3 tri0, tri1;
4504 : : Point3 p, q, diff;
4505 : : double u, v, s, t, u0, v0, s0, t0, min0, min1, dist;
4506 : :
4507 : : double maxdiff = 0;
4508 : :
4509 : : for (int i = 0; i < 128; i++)
4510 : : {
4511 : : tri0.b.x = RAND;
4512 : : tri0.b.y = RAND;
4513 : : tri0.b.z = RAND;
4514 : : tri0.e0.x = RAND;
4515 : : tri0.e0.y = RAND;
4516 : : tri0.e0.z = RAND;
4517 : : tri0.e1.x = RAND;
4518 : : tri0.e1.y = RAND;
4519 : : tri0.e1.z = RAND;
4520 : :
4521 : : tri1.b.x = RAND;
4522 : : tri1.b.y = RAND;
4523 : : tri1.b.z = RAND;
4524 : : tri1.e0.x = RAND;
4525 : : tri1.e0.y = RAND;
4526 : : tri1.e0.z = RAND;
4527 : : tri1.e1.x = RAND;
4528 : : tri1.e1.y = RAND;
4529 : : tri1.e1.z = RAND;
4530 : :
4531 : : min0 = FLT_MAX;
4532 : : int max = 32;
4533 : : for (int w = 0; w <= max; w++)
4534 : : {
4535 : : t0 = w/double(max);
4536 : : for (int z = 0; w+z <= max; z++)
4537 : : {
4538 : : s0 = z/double(max);
4539 : : p = tri0.b+s0*tri0.e0+t0*tri0.e1;
4540 : : for (int y = 0; y <= max; y++)
4541 : : {
4542 : : v0 = y/double(max);
4543 : : for (int x = 0; x+y <= max; x++)
4544 : : {
4545 : : u0 = x/double(max);
4546 : : q = tri1.b+u0*tri1.e0+v0*tri1.e1;
4547 : :
4548 : : diff = p-q;
4549 : : dist = Length(diff);
4550 : : if ( dist < min0 )
4551 : : {
4552 : : min0 = dist;
4553 : : s = s0;
4554 : : t = t0;
4555 : : u = u0;
4556 : : v = v0;
4557 : : }
4558 : : }
4559 : : }
4560 : : }
4561 : : }
4562 : : ostr << "i = " << i << endl;
4563 : : ostr << "sampled = " << s << ' ' << t << ' ' << u << ' '
4564 : : << v << ' ' << min0 << endl;
4565 : :
4566 : : min1 = MinTriangleTriangle(tri0,tri1,s,t,u,v);
4567 : : ostr << "analytic = " << s << ' ' << t << ' ' << u << ' '
4568 : : << v << ' ' << min1 << endl;
4569 : :
4570 : : ostr << "diff = " << min1-min0 << endl;
4571 : :
4572 : : if ( min1-min0 > maxdiff )
4573 : : maxdiff = min1-min0;
4574 : :
4575 : : ostr << endl;
4576 : : }
4577 : :
4578 : : ostr << "max diff = " << maxdiff << endl;
4579 : : }
4580 : : #endif
4581 : :
4582 : : //---------------------------------------------------------------------------
4583 : : double
4584 : 0 : AnalyticGeometryTool::MinLineSegmentLineSegment (const Line3& seg0, const Line3& seg1,
4585 : : double& s, double& t)
4586 : : {
4587 [ # # ]: 0 : Point3 diff = seg0.b - seg1.b;
4588 [ # # ]: 0 : double A = Dot(seg0.m,seg0.m);
4589 [ # # ]: 0 : double B = -Dot(seg0.m,seg1.m);
4590 [ # # ]: 0 : double C = Dot(seg1.m,seg1.m);
4591 [ # # ]: 0 : double D = Dot(seg0.m,diff);
4592 : : double E; // -Dot(seg1.m,diff), defer until needed
4593 [ # # ]: 0 : double F = Dot(diff,diff);
4594 [ # # ]: 0 : double det = Abs(A*C-B*B); // A*C-B*B = |Cross(M0,M1)|^2 >= 0
4595 : :
4596 : : double tmp;
4597 : :
4598 [ # # ]: 0 : if ( det >= par_tolerance )
4599 : : {
4600 : : // line segments are not parallel
4601 [ # # ]: 0 : E = -Dot(seg1.m,diff);
4602 : 0 : s = B*E-C*D;
4603 : 0 : t = B*D-A*E;
4604 : :
4605 [ # # ]: 0 : if ( s >= 0 )
4606 : : {
4607 [ # # ]: 0 : if ( s <= det )
4608 : : {
4609 [ # # ]: 0 : if ( t >= 0 )
4610 : : {
4611 [ # # ]: 0 : if ( t <= det ) // region 0 (interior)
4612 : : {
4613 : : // minimum at two interior points of 3D lines
4614 : 0 : double invDet = 1.0f/det;
4615 : 0 : s *= invDet;
4616 : 0 : t *= invDet;
4617 [ # # ][ # # ]: 0 : return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
4618 : : }
4619 : : else // region 3 (side)
4620 : : {
4621 : 0 : t = 1;
4622 : 0 : tmp = B+D;
4623 [ # # ]: 0 : if ( tmp >= 0 )
4624 : : {
4625 : 0 : s = 0;
4626 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
4627 : : }
4628 [ # # ]: 0 : else if ( -tmp >= A )
4629 : : {
4630 : 0 : s = 1;
4631 [ # # ][ # # ]: 0 : return DIST(A+C+F+2*(E+tmp));
4632 : : }
4633 : : else
4634 : : {
4635 : 0 : s = -tmp/A;
4636 [ # # ][ # # ]: 0 : return DIST(tmp*s+C+2*E+F);
4637 : : }
4638 : : }
4639 : : }
4640 : : else // region 7 (side)
4641 : : {
4642 : 0 : t = 0;
4643 [ # # ]: 0 : if ( D >= 0 )
4644 : : {
4645 : 0 : s = 0;
4646 [ # # ][ # # ]: 0 : return DIST(F);
4647 : : }
4648 [ # # ]: 0 : else if ( -D >= A )
4649 : : {
4650 : 0 : s = 1;
4651 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
4652 : : }
4653 : : else
4654 : : {
4655 : 0 : s = -D/A;
4656 [ # # ][ # # ]: 0 : return DIST(D*s+F);
4657 : : }
4658 : : }
4659 : : }
4660 : : else
4661 : : {
4662 [ # # ]: 0 : if ( t >= 0 )
4663 : : {
4664 [ # # ]: 0 : if ( t <= det ) // region 1 (side)
4665 : : {
4666 : 0 : s = 1;
4667 : 0 : tmp = B+E;
4668 [ # # ]: 0 : if ( tmp >= 0 )
4669 : : {
4670 : 0 : t = 0;
4671 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
4672 : : }
4673 [ # # ]: 0 : else if ( -tmp >= C )
4674 : : {
4675 : 0 : t = 1;
4676 [ # # ][ # # ]: 0 : return DIST(A+C+F+2*(D+tmp));
4677 : : }
4678 : : else
4679 : : {
4680 : 0 : t = -tmp/C;
4681 [ # # ][ # # ]: 0 : return DIST(tmp*t+A+2*D+F);
4682 : : }
4683 : : }
4684 : : else // region 2 (corner)
4685 : : {
4686 : 0 : tmp = B+D;
4687 [ # # ]: 0 : if ( -tmp <= A )
4688 : : {
4689 : 0 : t = 1;
4690 [ # # ]: 0 : if ( tmp >= 0 )
4691 : : {
4692 : 0 : s = 0;
4693 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
4694 : : }
4695 : : else
4696 : : {
4697 : 0 : s = -tmp/A;
4698 [ # # ][ # # ]: 0 : return DIST(tmp*s+C+2*E+F);
4699 : : }
4700 : : }
4701 : : else
4702 : : {
4703 : 0 : s = 1;
4704 : 0 : tmp = B+E;
4705 [ # # ]: 0 : if ( tmp >= 0 )
4706 : : {
4707 : 0 : t = 0;
4708 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
4709 : : }
4710 [ # # ]: 0 : else if ( -tmp >= C )
4711 : : {
4712 : 0 : t = 1;
4713 [ # # ][ # # ]: 0 : return DIST(A+C+F+2*(D+tmp));
4714 : : }
4715 : : else
4716 : : {
4717 : 0 : t = -tmp/C;
4718 [ # # ][ # # ]: 0 : return DIST(tmp*t+A+2*D+F);
4719 : : }
4720 : : }
4721 : : }
4722 : : }
4723 : : else // region 8 (corner)
4724 : : {
4725 [ # # ]: 0 : if ( -D < A )
4726 : : {
4727 : 0 : t = 0;
4728 [ # # ]: 0 : if ( D >= 0 )
4729 : : {
4730 : 0 : s = 0;
4731 [ # # ][ # # ]: 0 : return DIST(F);
4732 : : }
4733 : : else
4734 : : {
4735 : 0 : s = -D/A;
4736 [ # # ][ # # ]: 0 : return DIST(D*s+F);
4737 : : }
4738 : : }
4739 : : else
4740 : : {
4741 : 0 : s = 1;
4742 : 0 : tmp = B+E;
4743 [ # # ]: 0 : if ( tmp >= 0 )
4744 : : {
4745 : 0 : t = 0;
4746 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
4747 : : }
4748 [ # # ]: 0 : else if ( -tmp >= C )
4749 : : {
4750 : 0 : t = 1;
4751 [ # # ][ # # ]: 0 : return DIST(A+C+F+2*(D+tmp));
4752 : : }
4753 : : else
4754 : : {
4755 : 0 : t = -tmp/C;
4756 [ # # ][ # # ]: 0 : return DIST(tmp*t+A+2*D+F);
4757 : : }
4758 : : }
4759 : : }
4760 : : }
4761 : : }
4762 : : else
4763 : : {
4764 [ # # ]: 0 : if ( t >= 0 )
4765 : : {
4766 [ # # ]: 0 : if ( t <= det ) // region 5 (side)
4767 : : {
4768 : 0 : s = 0;
4769 [ # # ]: 0 : if ( E >= 0 )
4770 : : {
4771 : 0 : t = 0;
4772 [ # # ][ # # ]: 0 : return DIST(F);
4773 : : }
4774 [ # # ]: 0 : else if ( -E >= C )
4775 : : {
4776 : 0 : t = 1;
4777 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
4778 : : }
4779 : : else
4780 : : {
4781 : 0 : t = -E/C;
4782 [ # # ][ # # ]: 0 : return DIST(E*t+F);
4783 : : }
4784 : : }
4785 : : else // region 4 (corner)
4786 : : {
4787 : 0 : tmp = B+D;
4788 [ # # ]: 0 : if ( tmp < 0 )
4789 : : {
4790 : 0 : t = 1;
4791 [ # # ]: 0 : if ( -tmp >= A )
4792 : : {
4793 : 0 : s = 1;
4794 [ # # ][ # # ]: 0 : return DIST(A+C+F+2*(E+tmp));
4795 : : }
4796 : : else
4797 : : {
4798 : 0 : s = -tmp/A;
4799 [ # # ][ # # ]: 0 : return DIST(tmp*s+C+2*E+F);
4800 : : }
4801 : : }
4802 : : else
4803 : : {
4804 : 0 : s = 0;
4805 [ # # ]: 0 : if ( E >= 0 )
4806 : : {
4807 : 0 : t = 0;
4808 [ # # ][ # # ]: 0 : return DIST(F);
4809 : : }
4810 [ # # ]: 0 : else if ( -E >= C )
4811 : : {
4812 : 0 : t = 1;
4813 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
4814 : : }
4815 : : else
4816 : : {
4817 : 0 : t = -E/C;
4818 [ # # ][ # # ]: 0 : return DIST(E*t+F);
4819 : : }
4820 : : }
4821 : : }
4822 : : }
4823 : : else // region 6 (corner)
4824 : : {
4825 [ # # ]: 0 : if ( D < 0 )
4826 : : {
4827 : 0 : t = 0;
4828 [ # # ]: 0 : if ( -D >= A )
4829 : : {
4830 : 0 : s = 1;
4831 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
4832 : : }
4833 : : else
4834 : : {
4835 : 0 : s = -D/A;
4836 [ # # ][ # # ]: 0 : return DIST(D*s+F);
4837 : : }
4838 : : }
4839 : : else
4840 : : {
4841 : 0 : s = 0;
4842 [ # # ]: 0 : if ( E >= 0 )
4843 : : {
4844 : 0 : t = 0;
4845 [ # # ][ # # ]: 0 : return DIST(F);
4846 : : }
4847 [ # # ]: 0 : else if ( -E >= C )
4848 : : {
4849 : 0 : t = 1;
4850 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
4851 : : }
4852 : : else
4853 : : {
4854 : 0 : t = -E/C;
4855 [ # # ][ # # ]: 0 : return DIST(E*t+F);
4856 : : }
4857 : : }
4858 : : }
4859 : : }
4860 : : }
4861 : : else
4862 : : {
4863 : : // line segments are parallel
4864 [ # # ]: 0 : if ( B > 0 )
4865 : : {
4866 : : // direction vectors form an obtuse angle
4867 [ # # ]: 0 : if ( D >= 0 )
4868 : : {
4869 : 0 : s = 0;
4870 : 0 : t = 0;
4871 [ # # ][ # # ]: 0 : return DIST(F);
4872 : : }
4873 [ # # ]: 0 : else if ( -D <= A )
4874 : : {
4875 : 0 : s = -D/A;
4876 : 0 : t = 0;
4877 [ # # ][ # # ]: 0 : return DIST(D*s+F);
4878 : : }
4879 : : else
4880 : : {
4881 [ # # ]: 0 : E = -Dot(seg1.m,diff);
4882 : 0 : s = 1;
4883 : 0 : tmp = A+D;
4884 [ # # ]: 0 : if ( -tmp >= B )
4885 : : {
4886 : 0 : t = 1;
4887 [ # # ][ # # ]: 0 : return DIST(A+C+F+2*(B+D+E));
4888 : : }
4889 : : else
4890 : : {
4891 : 0 : t = -tmp/B;
4892 [ # # ][ # # ]: 0 : return DIST(A+2*D+F+t*(C*t+2*(B+E)));
4893 : : }
4894 : : }
4895 : : }
4896 : : else
4897 : : {
4898 : : // direction vectors form an acute angle
4899 [ # # ]: 0 : if ( -D >= A )
4900 : : {
4901 : 0 : s = 1;
4902 : 0 : t = 0;
4903 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
4904 : : }
4905 [ # # ]: 0 : else if ( D <= 0 )
4906 : : {
4907 : 0 : s = -D/A;
4908 : 0 : t = 0;
4909 [ # # ][ # # ]: 0 : return DIST(D*s+F);
4910 : : }
4911 : : else
4912 : : {
4913 [ # # ]: 0 : E = -Dot(seg1.m,diff);
4914 : 0 : s = 0;
4915 [ # # ]: 0 : if ( D >= -B )
4916 : : {
4917 : 0 : t = 1;
4918 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
4919 : : }
4920 : : else
4921 : : {
4922 : 0 : t = -D/B;
4923 [ # # ][ # # ]: 0 : return DIST(F+t*(2*E+C*t));
4924 : : }
4925 : : }
4926 : : }
4927 : : }
4928 : : }
4929 : : //---------------------------------------------------------------------------
4930 : :
4931 : : #ifdef LIN3LIN3_TEST
4932 : :
4933 : : //#include <stdlib.h>
4934 : : //#include <fstream.h>
4935 : :
4936 : : //#define RAND (2.0f*rand()/double(RAND_MAX)-1)
4937 : :
4938 : : //ofstream ostr("data.txt");
4939 : :
4940 : : void TestSegSeg ()
4941 : : {
4942 : : Line3 seg0, seg1;
4943 : : Point3 p0, p1, diff;
4944 : : double s, t, s0, t0, min0, min1, dist;
4945 : : double maxDiff = 0.0f;
4946 : :
4947 : : for (int i = 0; i < 128; i++)
4948 : : {
4949 : : seg0.b.x = RAND;
4950 : : seg0.b.y = RAND;
4951 : : seg0.b.z = RAND;
4952 : :
4953 : : seg0.m.x = RAND;
4954 : : seg0.m.y = RAND;
4955 : : seg0.m.z = RAND;
4956 : :
4957 : : seg1.b.x = RAND;
4958 : : seg1.b.y = RAND;
4959 : : seg1.b.z = RAND;
4960 : :
4961 : : if ( i % 2 )
4962 : : {
4963 : : // non-parallel line segments
4964 : : seg1.m.x = RAND;
4965 : : seg1.m.y = RAND;
4966 : : seg1.m.z = RAND;
4967 : : }
4968 : : else
4969 : : {
4970 : : // parallel line segments
4971 : : double scale = RAND;
4972 : : seg1.m = scale*seg0.m;
4973 : : }
4974 : :
4975 : : min0 = FLT_MAX;
4976 : : int ymax = 128, xmax = 128;
4977 : : for (int y = 0; y < ymax; y++)
4978 : : {
4979 : : s0 = y/double(ymax-1);
4980 : : p0 = seg0.b+s0*seg0.m;
4981 : : for (int x = 0; x < xmax; x++)
4982 : : {
4983 : : t0 = x/double(xmax-1);
4984 : : p1 = seg1.b+t0*seg1.m;
4985 : :
4986 : : diff = p1-p0;
4987 : : dist = Length(diff);
4988 : : if ( dist < min0 )
4989 : : {
4990 : : min0 = dist;
4991 : : s = s0;
4992 : : t = t0;
4993 : : }
4994 : : }
4995 : : }
4996 : : ostr << "sampled = " << s << ' ' << t << ' ' << min0 << endl;
4997 : :
4998 : : min1 = MinLineSegmentLineSegment(seg0,seg1,s,t);
4999 : : ostr << "analytic = " << s << ' ' << t << ' ' << min1 << endl;
5000 : :
5001 : : double compDiff = min1-min0;
5002 : : ostr << "diff = " << compDiff << endl;
5003 : : if ( compDiff > maxDiff )
5004 : : maxDiff = compDiff;
5005 : :
5006 : : ostr << endl;
5007 : : }
5008 : : ostr << "max diff = " << maxDiff << endl;
5009 : : }
5010 : : #endif
5011 : :
5012 : : // FILE: pt3tri3.cpp
5013 : : //---------------------------------------------------------------------------
5014 : : double
5015 : 0 : AnalyticGeometryTool::MinPointTriangle (const Point3& p, const Triangle3& tri,
5016 : : double& s, double& t)
5017 : : {
5018 [ # # ]: 0 : Point3 diff = tri.b - p;
5019 [ # # ]: 0 : double A = Dot(tri.e0,tri.e0);
5020 [ # # ]: 0 : double B = Dot(tri.e0,tri.e1);
5021 [ # # ]: 0 : double C = Dot(tri.e1,tri.e1);
5022 [ # # ]: 0 : double D = Dot(tri.e0,diff);
5023 [ # # ]: 0 : double E = Dot(tri.e1,diff);
5024 [ # # ]: 0 : double F = Dot(diff,diff);
5025 [ # # ]: 0 : double det = Abs(A*C-B*B); // A*C-B*B = |Cross(e0,e1)|^2 >= 0
5026 : : // A-2*B+C = Dot(e0,e0)-2*Dot(e0,e1)+Dot(e1,e1) = |e0-e1|^2 > 0
5027 : :
5028 : 0 : s = B*E-C*D;
5029 : 0 : t = B*D-A*E;
5030 : :
5031 [ # # ]: 0 : if ( s+t <= det )
5032 : : {
5033 [ # # ]: 0 : if ( s < 0 )
5034 : : {
5035 [ # # ]: 0 : if ( t < 0 ) // region 4
5036 : : {
5037 [ # # ]: 0 : if ( D < 0 )
5038 : : {
5039 : 0 : t = 0;
5040 [ # # ]: 0 : if ( -D >= A )
5041 : : {
5042 : 0 : s = 1;
5043 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
5044 : : }
5045 : : else
5046 : : {
5047 : 0 : s = -D/A;
5048 [ # # ][ # # ]: 0 : return DIST(D*s+F);
5049 : : }
5050 : : }
5051 : : else
5052 : : {
5053 : 0 : s = 0;
5054 [ # # ]: 0 : if ( E >= 0 )
5055 : : {
5056 : 0 : t = 0;
5057 [ # # ][ # # ]: 0 : return DIST(F);
5058 : : }
5059 [ # # ]: 0 : else if ( -E >= C )
5060 : : {
5061 : 0 : t = 1;
5062 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
5063 : : }
5064 : : else
5065 : : {
5066 : 0 : t = -E/C;
5067 [ # # ][ # # ]: 0 : return DIST(E*t+F);
5068 : : }
5069 : : }
5070 : : }
5071 : : else // region 3
5072 : : {
5073 : 0 : s = 0;
5074 [ # # ]: 0 : if ( E >= 0 )
5075 : : {
5076 : 0 : t = 0;
5077 [ # # ][ # # ]: 0 : return DIST(F);
5078 : : }
5079 [ # # ]: 0 : else if ( -E >= C )
5080 : : {
5081 : 0 : t = 1;
5082 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
5083 : : }
5084 : : else
5085 : : {
5086 : 0 : t = -E/C;
5087 [ # # ][ # # ]: 0 : return DIST(E*t+F);
5088 : : }
5089 : : }
5090 : : }
5091 [ # # ]: 0 : else if ( t < 0 ) // region 5
5092 : : {
5093 : 0 : t = 0;
5094 [ # # ]: 0 : if ( D >= 0 )
5095 : : {
5096 : 0 : s = 0;
5097 [ # # ][ # # ]: 0 : return DIST(F);
5098 : : }
5099 [ # # ]: 0 : else if ( -D >= A )
5100 : : {
5101 : 0 : s = 1;
5102 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
5103 : : }
5104 : : else
5105 : : {
5106 : 0 : s = -D/A;
5107 [ # # ][ # # ]: 0 : return DIST(D*s+F);
5108 : : }
5109 : : }
5110 : : else // region 0
5111 : : {
5112 : : // minimum at interior point
5113 [ # # ]: 0 : if( det == 0.0 )
5114 : : {
5115 : : //PRINT_WARNING( "Found zero determinant\n" );
5116 : 0 : return CUBIT_DBL_MAX;
5117 : : }
5118 : :
5119 : 0 : double invDet = 1.0f/det;
5120 : 0 : s *= invDet;
5121 : 0 : t *= invDet;
5122 [ # # ][ # # ]: 0 : return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
5123 : : }
5124 : : }
5125 : : else
5126 : : {
5127 : : double tmp0, tmp1, numer, denom;
5128 : :
5129 [ # # ]: 0 : if ( s < 0 ) // region 2
5130 : : {
5131 : 0 : tmp0 = B+D;
5132 : 0 : tmp1 = C+E;
5133 [ # # ]: 0 : if ( tmp1 > tmp0 )
5134 : : {
5135 : 0 : numer = tmp1 - tmp0;
5136 : 0 : denom = A-2*B+C;
5137 [ # # ]: 0 : if ( numer >= denom )
5138 : : {
5139 : 0 : s = 1;
5140 : 0 : t = 0;
5141 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
5142 : : }
5143 : : else
5144 : : {
5145 : 0 : s = numer/denom;
5146 : 0 : t = 1-s;
5147 [ # # ][ # # ]: 0 : return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
5148 : : }
5149 : : }
5150 : : else
5151 : : {
5152 : 0 : s = 0;
5153 [ # # ]: 0 : if ( tmp1 <= 0 )
5154 : : {
5155 : 0 : t = 1;
5156 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
5157 : : }
5158 [ # # ]: 0 : else if ( E >= 0 )
5159 : : {
5160 : 0 : t = 0;
5161 [ # # ][ # # ]: 0 : return DIST(F);
5162 : : }
5163 : : else
5164 : : {
5165 : 0 : t = -E/C;
5166 [ # # ][ # # ]: 0 : return DIST(E*t+F);
5167 : : }
5168 : : }
5169 : : }
5170 [ # # ]: 0 : else if ( t < 0 ) // region 6
5171 : : {
5172 : 0 : tmp0 = B+E;
5173 : 0 : tmp1 = A+D;
5174 [ # # ]: 0 : if ( tmp1 > tmp0 )
5175 : : {
5176 : 0 : numer = tmp1 - tmp0;
5177 : 0 : denom = A-2*B+C;
5178 [ # # ]: 0 : if ( numer >= denom )
5179 : : {
5180 : 0 : t = 1;
5181 : 0 : s = 0;
5182 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
5183 : : }
5184 : : else
5185 : : {
5186 : 0 : t = numer/denom;
5187 : 0 : s = 1-t;
5188 [ # # ][ # # ]: 0 : return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
5189 : : }
5190 : : }
5191 : : else
5192 : : {
5193 : 0 : t = 0;
5194 [ # # ]: 0 : if ( tmp1 <= 0 )
5195 : : {
5196 : 0 : s = 1;
5197 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
5198 : : }
5199 [ # # ]: 0 : else if ( D >= 0 )
5200 : : {
5201 : 0 : s = 0;
5202 [ # # ][ # # ]: 0 : return DIST(F);
5203 : : }
5204 : : else
5205 : : {
5206 : 0 : s = -D/A;
5207 [ # # ][ # # ]: 0 : return DIST(D*s+F);
5208 : : }
5209 : : }
5210 : : }
5211 : : else // region 1
5212 : : {
5213 : 0 : numer = C+E-B-D;
5214 [ # # ]: 0 : if ( numer <= 0 )
5215 : : {
5216 : 0 : s = 0;
5217 : 0 : t = 1;
5218 [ # # ][ # # ]: 0 : return DIST(C+2*E+F);
5219 : : }
5220 : : else
5221 : : {
5222 : 0 : denom = A-2*B+C;
5223 [ # # ]: 0 : if ( numer >= denom )
5224 : : {
5225 : 0 : s = 1;
5226 : 0 : t = 0;
5227 [ # # ][ # # ]: 0 : return DIST(A+2*D+F);
5228 : : }
5229 : : else
5230 : : {
5231 : 0 : s = numer/denom;
5232 : 0 : t = 1-s;
5233 [ # # ][ # # ]: 0 : return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
5234 : : }
5235 : : }
5236 : : }
5237 : : }
5238 : : }
5239 : : //---------------------------------------------------------------------------
5240 : :
5241 : : #ifdef PT3TRI3_TEST
5242 : :
5243 : : //#include <float.h>
5244 : : //#include <fstream.h>
5245 : : //#include <stdlib.h>
5246 : :
5247 : : //ofstream ostr("data.txt");
5248 : :
5249 : : //#define RAND (2.0f*rand()/double(RAND_MAX)-1)
5250 : :
5251 : : void main ()
5252 : : {
5253 : : Triangle3 tri;
5254 : : Point3 p, q, diff;
5255 : : double s, t, s0, t0, min0, min1, dist;
5256 : :
5257 : : double maxdiff = 0;
5258 : :
5259 : : for (int i = 0; i < 128; i++)
5260 : : {
5261 : : tri.b.x = RAND;
5262 : : tri.b.y = RAND;
5263 : : tri.b.z = RAND;
5264 : : tri.e0.x = RAND;
5265 : : tri.e0.y = RAND;
5266 : : tri.e0.z = RAND;
5267 : : tri.e1.x = RAND;
5268 : : tri.e1.y = RAND;
5269 : : tri.e1.z = RAND;
5270 : :
5271 : : p.x = RAND;
5272 : : p.y = RAND;
5273 : : p.z = RAND;
5274 : :
5275 : : min0 = FLT_MAX;
5276 : : int max = 128;
5277 : : for (int y = 0; y <= max; y++)
5278 : : {
5279 : : s0 = y/double(max);
5280 : : for (int x = 0; x+y <= max; x++)
5281 : : {
5282 : : t0 = x/double(max);
5283 : : q = tri.b+s0*tri.e0+t0*tri.e1;
5284 : :
5285 : : diff = p-q;
5286 : : dist = Length(diff);
5287 : : if ( dist < min0 )
5288 : : {
5289 : : min0 = dist;
5290 : : s = s0;
5291 : : t = t0;
5292 : : }
5293 : : }
5294 : : }
5295 : : ostr << "sampled = " << s << ' ' << t << ' ' << min0 << endl;
5296 : :
5297 : : min1 = MinPointTriangle(p,tri,s,t);
5298 : : ostr << "analytic = " << s << ' ' << t << ' ' << min1 << endl;
5299 : :
5300 : : ostr << "diff = " << min1-min0 << endl;
5301 : :
5302 : : if ( min1-min0 > maxdiff )
5303 : : maxdiff = min1-min0;
5304 : :
5305 : : ostr << endl;
5306 : : }
5307 : : ostr << "max diff = " << maxdiff << endl;
5308 : : }
5309 : : #endif
5310 : :
5311 : : // FILE: lin3tri3.cpp
5312 : : //---------------------------------------------------------------------------
5313 : : // This code computes the closest points of a line L(r) = a0+r*a1 and a
5314 : : // triangle Tri(s,t) = b0+s*b1+t*b2, where 0 <= r <= 1 and 0 <= s <= 1,
5315 : : // 0 <= t <= 1, and 0 <= s+t <= 1.
5316 : : //
5317 : : // In calculus terms, the goal is to minimize the squared-distance function
5318 : : // Q(r,s,t) = Dot(L(r)-Tri(s,t),L(r)-Tri(s,t)) over the prism domain
5319 : : // 0 <= r <= 1, 0 <= s <= 1, 0 <= t <= 1, 0 <= s+t <= 1. This function is
5320 : : //
5321 : : // Q(r,s,t) = [r s t] A [r s t] + 2 B [r s t] + C
5322 : : //
5323 : : // where A is a 3x3 symmetric matrix, B is a 3x1 column vector, and C is
5324 : : // a constant. The entries of A and B are various dot products derived
5325 : : // from the vectors a0, a1, b0, b1, and b2.
5326 : : //
5327 : : // The analysis is similar to that of MinPointTriangle. The (s,t) domain
5328 : : // is partitioned similarly, but the prism is obtained by extruding the
5329 : : // domain in the r-direction. The three cases for r are: r < 0, 0 <= r <= 1,
5330 : : // and r > 1. For each case the partitioning in (s,t) is identical to that
5331 : : // of MinPointTriangle. The region numbers for r < 0 are appended with an
5332 : : // 'm' and the region numbers for r > 1 are appended with a 'p'.
5333 : : //---------------------------------------------------------------------------
5334 : :
5335 : : // NOTE: This code is not fully optimized as is the code in pt3lin3,
5336 : : // pt3tri3, and lin3lin3.
5337 : :
5338 : : //---------------------------------------------------------------------------
5339 : : double
5340 : 0 : AnalyticGeometryTool::MinLineSegmentTriangle (const Line3& seg, const Triangle3& tri,
5341 : : double& r, double& s, double& t)
5342 : : {
5343 [ # # ]: 0 : Point3 diff = tri.b - seg.b;
5344 [ # # ]: 0 : double A00 = Dot(seg.m,seg.m);
5345 [ # # ]: 0 : double A01 = -Dot(seg.m,tri.e0);
5346 [ # # ]: 0 : double A02 = -Dot(seg.m,tri.e1);
5347 [ # # ]: 0 : double A11 = Dot(tri.e0,tri.e0);
5348 [ # # ]: 0 : double A12 = Dot(tri.e0,tri.e1);
5349 [ # # ]: 0 : double A22 = Dot(tri.e1,tri.e1);
5350 [ # # ]: 0 : double B0 = -Dot(diff,seg.m);
5351 [ # # ]: 0 : double B1 = Dot(diff,tri.e0);
5352 [ # # ]: 0 : double B2 = Dot(diff,tri.e1);
5353 : 0 : double cof00 = A11*A22-A12*A12;
5354 : 0 : double cof01 = A02*A12-A01*A22;
5355 : 0 : double cof02 = A01*A12-A02*A11;
5356 : 0 : double det = A00*cof00+A01*cof01+A02*cof02;
5357 : :
5358 : : Line3 triseg;
5359 : : Point3 pt;
5360 : : double min, min0, r0, s0, t0;
5361 : :
5362 [ # # ][ # # ]: 0 : if ( Abs(det) >= par_tolerance )
5363 : : {
5364 : 0 : double cof11 = A00*A22-A02*A02;
5365 : 0 : double cof12 = A02*A01-A00*A12;
5366 : 0 : double cof22 = A00*A11-A01*A01;
5367 : 0 : double invDet = 1.0f/det;
5368 : 0 : double rhs0 = -B0*invDet;
5369 : 0 : double rhs1 = -B1*invDet;
5370 : 0 : double rhs2 = -B2*invDet;
5371 : :
5372 : 0 : r = cof00*rhs0+cof01*rhs1+cof02*rhs2;
5373 : 0 : s = cof01*rhs0+cof11*rhs1+cof12*rhs2;
5374 : 0 : t = cof02*rhs0+cof12*rhs1+cof22*rhs2;
5375 : :
5376 [ # # ]: 0 : if ( r < 0 )
5377 : : {
5378 [ # # ]: 0 : if ( s+t <= 1 )
5379 : : {
5380 [ # # ]: 0 : if ( s < 0 )
5381 : : {
5382 [ # # ]: 0 : if ( t < 0 ) // region 4m
5383 : : {
5384 : : // min on face s=0 or t=0 or r=0
5385 : 0 : triseg.b = tri.b;
5386 : 0 : triseg.m = tri.e1;
5387 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5388 : 0 : s = 0;
5389 : 0 : triseg.b = tri.b;
5390 : 0 : triseg.m = tri.e0;
5391 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,s0);
5392 : 0 : t0 = 0;
5393 [ # # ]: 0 : if ( min0 < min )
5394 : : {
5395 : 0 : min = min0;
5396 : 0 : r = r0;
5397 : 0 : s = s0;
5398 : 0 : t = t0;
5399 : : }
5400 [ # # ]: 0 : min0 = MinPointTriangle(seg.b,tri,s0,t0);
5401 : 0 : r0 = 0;
5402 [ # # ]: 0 : if ( min0 < min )
5403 : : {
5404 : 0 : min = min0;
5405 : 0 : r = r0;
5406 : 0 : s = s0;
5407 : 0 : t = t0;
5408 : : }
5409 : : }
5410 : : else // region 3m
5411 : : {
5412 : : // min on face s=0 or r=0
5413 : 0 : triseg.b = tri.b;
5414 : 0 : triseg.m = tri.e1;
5415 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5416 : 0 : s = 0;
5417 [ # # ]: 0 : min0 = MinPointTriangle(seg.b,tri,s0,t0);
5418 : 0 : r0 = 0;
5419 [ # # ]: 0 : if ( min0 < min )
5420 : : {
5421 : 0 : min = min0;
5422 : 0 : r = r0;
5423 : 0 : s = s0;
5424 : 0 : t = t0;
5425 : : }
5426 : : }
5427 : : }
5428 [ # # ]: 0 : else if ( t < 0 ) // region 5m
5429 : : {
5430 : : // min on face t=0 or r=0
5431 : 0 : triseg.b = tri.b;
5432 : 0 : triseg.m = tri.e0;
5433 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,s);
5434 : 0 : t = 0;
5435 [ # # ]: 0 : min0 = MinPointTriangle(seg.b,tri,s0,t0);
5436 : 0 : r0 = 0;
5437 [ # # ]: 0 : if ( min0 < min )
5438 : : {
5439 : 0 : min = min0;
5440 : 0 : r = r0;
5441 : 0 : s = s0;
5442 : 0 : t = t0;
5443 : : }
5444 : : }
5445 : : else // region 0m
5446 : : {
5447 : : // min face on r=0
5448 [ # # ]: 0 : min = MinPointTriangle(seg.b,tri,s,t);
5449 : 0 : r = 0;
5450 : : }
5451 : : }
5452 : : else
5453 : : {
5454 [ # # ]: 0 : if ( s < 0 ) // region 2m
5455 : : {
5456 : : // min on face s=0 or s+t=1 or r=0
5457 : 0 : triseg.b = tri.b;
5458 : 0 : triseg.m = tri.e1;
5459 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5460 : 0 : s = 0;
5461 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5462 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5463 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
5464 : 0 : s0 = 1-t0;
5465 [ # # ]: 0 : if ( min0 < min )
5466 : : {
5467 : 0 : min = min0;
5468 : 0 : r = r0;
5469 : 0 : s = s0;
5470 : 0 : t = t0;
5471 : : }
5472 [ # # ]: 0 : min0 = MinPointTriangle(seg.b,tri,s0,t0);
5473 : 0 : r0 = 0;
5474 [ # # ]: 0 : if ( min0 < min )
5475 : : {
5476 : 0 : min = min0;
5477 : 0 : r = r0;
5478 : 0 : s = s0;
5479 : 0 : t = t0;
5480 : : }
5481 : : }
5482 [ # # ]: 0 : else if ( t < 0 ) // region 6m
5483 : : {
5484 : : // min on face t=0 or s+t=1 or r=0
5485 : 0 : triseg.b = tri.b;
5486 : 0 : triseg.m = tri.e0;
5487 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,s);
5488 : 0 : t = 0;
5489 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5490 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5491 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
5492 : 0 : s0 = 1-t0;
5493 [ # # ]: 0 : if ( min0 < min )
5494 : : {
5495 : 0 : min = min0;
5496 : 0 : r = r0;
5497 : 0 : s = s0;
5498 : 0 : t = t0;
5499 : : }
5500 [ # # ]: 0 : min0 = MinPointTriangle(seg.b,tri,s0,t0);
5501 : 0 : r0 = 0;
5502 [ # # ]: 0 : if ( min0 < min )
5503 : : {
5504 : 0 : min = min0;
5505 : 0 : r = r0;
5506 : 0 : s = s0;
5507 : 0 : t = t0;
5508 : : }
5509 : : }
5510 : : else // region 1m
5511 : : {
5512 : : // min on face s+t=1 or r=0
5513 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5514 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5515 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5516 : 0 : s = 1-t;
5517 [ # # ]: 0 : min0 = MinPointTriangle(seg.b,tri,s0,t0);
5518 : 0 : r0 = 0;
5519 [ # # ]: 0 : if ( min0 < min )
5520 : : {
5521 : 0 : min = min0;
5522 : 0 : r = r0;
5523 : 0 : s = s0;
5524 : 0 : t = t0;
5525 : : }
5526 : : }
5527 : : }
5528 : : }
5529 [ # # ]: 0 : else if ( r <= 1 )
5530 : : {
5531 [ # # ]: 0 : if ( s+t <= 1 )
5532 : : {
5533 [ # # ]: 0 : if ( s < 0 )
5534 : : {
5535 [ # # ]: 0 : if ( t < 0 ) // region 4
5536 : : {
5537 : : // min on face s=0 or t=0
5538 : 0 : triseg.b = tri.b;
5539 : 0 : triseg.m = tri.e1;
5540 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5541 : 0 : s = 0;
5542 : 0 : triseg.b = tri.b;
5543 : 0 : triseg.m = tri.e0;
5544 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,s0);
5545 : 0 : t0 = 0;
5546 [ # # ]: 0 : if ( min0 < min )
5547 : : {
5548 : 0 : min = min0;
5549 : 0 : r = r0;
5550 : 0 : s = s0;
5551 : 0 : t = t0;
5552 : : }
5553 : : }
5554 : : else // region 3
5555 : : {
5556 : : // min on face s=0
5557 : 0 : triseg.b = tri.b;
5558 : 0 : triseg.m = tri.e1;
5559 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5560 : 0 : s = 0;
5561 : : }
5562 : : }
5563 [ # # ]: 0 : else if ( t < 0 ) // region 5
5564 : : {
5565 : : // min on face t=0
5566 : 0 : triseg.b = tri.b;
5567 : 0 : triseg.m = tri.e0;
5568 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,s);
5569 : 0 : t = 0;
5570 : : }
5571 : : else // region 0
5572 : : {
5573 : : // global minimum is interior, done
5574 : 0 : min = Sqrt(Abs(r*(A00*r+A01*s+A02*t+2.0f*B0)
5575 : 0 : +s*(A01*r+A11*s+A12*t+2.0f*B1)
5576 : 0 : +t*(A02*r+A12*s+A22*t+2.0f*B2)
5577 [ # # ][ # # ]: 0 : +Dot(diff,diff)));
[ # # ]
5578 : : }
5579 : : }
5580 : : else
5581 : : {
5582 [ # # ]: 0 : if ( s < 0 ) // region 2
5583 : : {
5584 : : // min on face s=0 or s+t=1
5585 : 0 : triseg.b = tri.b;
5586 : 0 : triseg.m = tri.e1;
5587 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5588 : 0 : s = 0;
5589 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5590 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5591 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
5592 : 0 : s0 = 1-t0;
5593 [ # # ]: 0 : if ( min0 < min )
5594 : : {
5595 : 0 : min = min0;
5596 : 0 : r = r0;
5597 : 0 : s = s0;
5598 : 0 : t = t0;
5599 : : }
5600 : : }
5601 [ # # ]: 0 : else if ( t < 0 ) // region 6
5602 : : {
5603 : : // min on face t=0 or s+t=1
5604 : 0 : triseg.b = tri.b;
5605 : 0 : triseg.m = tri.e0;
5606 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,s);
5607 : 0 : t = 0;
5608 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5609 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5610 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
5611 : 0 : s0 = 1-t0;
5612 [ # # ]: 0 : if ( min0 < min )
5613 : : {
5614 : 0 : min = min0;
5615 : 0 : r = r0;
5616 : 0 : s = s0;
5617 : 0 : t = t0;
5618 : : }
5619 : : }
5620 : : else // region 1
5621 : : {
5622 : : // min on face s+t=1
5623 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5624 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5625 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5626 : 0 : s = 1-t;
5627 : : }
5628 : : }
5629 : : }
5630 : : else // r > 1
5631 : : {
5632 [ # # ]: 0 : if ( s+t <= 1 )
5633 : : {
5634 [ # # ]: 0 : if ( s < 0 )
5635 : : {
5636 [ # # ]: 0 : if ( t < 0 ) // region 4p
5637 : : {
5638 : : // min on face s=0 or t=0 or r=1
5639 : 0 : triseg.b = tri.b;
5640 : 0 : triseg.m = tri.e1;
5641 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5642 : 0 : s = 0;
5643 : 0 : triseg.b = tri.b;
5644 : 0 : triseg.m = tri.e0;
5645 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,s0);
5646 : 0 : t0 = 0;
5647 [ # # ]: 0 : if ( min0 < min )
5648 : : {
5649 : 0 : min = min0;
5650 : 0 : r = r0;
5651 : 0 : s = s0;
5652 : 0 : t = t0;
5653 : : }
5654 [ # # ]: 0 : pt = seg.b+seg.m;
5655 [ # # ]: 0 : min0 = MinPointTriangle(pt,tri,s0,t0);
5656 : 0 : r0 = 1;
5657 [ # # ]: 0 : if ( min0 < min )
5658 : : {
5659 : 0 : min = min0;
5660 : 0 : r = r0;
5661 : 0 : s = s0;
5662 : 0 : t = t0;
5663 : : }
5664 : : }
5665 : : else // region 3p
5666 : : {
5667 : : // min on face s=0 or r=1
5668 : 0 : triseg.b = tri.b;
5669 : 0 : triseg.m = tri.e1;
5670 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5671 : 0 : s = 0;
5672 [ # # ]: 0 : pt = seg.b+seg.m;
5673 [ # # ]: 0 : min0 = MinPointTriangle(pt,tri,s0,t0);
5674 : 0 : r0 = 1;
5675 [ # # ]: 0 : if ( min0 < min )
5676 : : {
5677 : 0 : min = min0;
5678 : 0 : r = r0;
5679 : 0 : s = s0;
5680 : 0 : t = t0;
5681 : : }
5682 : : }
5683 : : }
5684 [ # # ]: 0 : else if ( t < 0 ) // region 5p
5685 : : {
5686 : : // min on face t=0 or r=1
5687 : 0 : triseg.b = tri.b;
5688 : 0 : triseg.m = tri.e0;
5689 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,s);
5690 : 0 : t = 0;
5691 [ # # ]: 0 : pt = seg.b+seg.m;
5692 [ # # ]: 0 : min0 = MinPointTriangle(pt,tri,s0,t0);
5693 : 0 : r0 = 1;
5694 [ # # ]: 0 : if ( min0 < min )
5695 : : {
5696 : 0 : min = min0;
5697 : 0 : r = r0;
5698 : 0 : s = s0;
5699 : 0 : t = t0;
5700 : : }
5701 : : }
5702 : : else // region 0p
5703 : : {
5704 : : // min face on r=1
5705 [ # # ]: 0 : pt = seg.b+seg.m;
5706 [ # # ]: 0 : min = MinPointTriangle(pt,tri,s,t);
5707 : 0 : r = 1;
5708 : : }
5709 : : }
5710 : : else
5711 : : {
5712 [ # # ]: 0 : if ( s < 0 ) // region 2p
5713 : : {
5714 : : // min on face s=0 or s+t=1 or r=1
5715 : 0 : triseg.b = tri.b;
5716 : 0 : triseg.m = tri.e1;
5717 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5718 : 0 : s = 0;
5719 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5720 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5721 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
5722 : 0 : s0 = 1-t0;
5723 [ # # ]: 0 : if ( min0 < min )
5724 : : {
5725 : 0 : min = min0;
5726 : 0 : r = r0;
5727 : 0 : s = s0;
5728 : 0 : t = t0;
5729 : : }
5730 [ # # ]: 0 : pt = seg.b+seg.m;
5731 [ # # ]: 0 : min0 = MinPointTriangle(pt,tri,s0,t0);
5732 : 0 : r0 = 1;
5733 [ # # ]: 0 : if ( min0 < min )
5734 : : {
5735 : 0 : min = min0;
5736 : 0 : r = r0;
5737 : 0 : s = s0;
5738 : 0 : t = t0;
5739 : : }
5740 : : }
5741 [ # # ]: 0 : else if ( t < 0 ) // region 6p
5742 : : {
5743 : : // min on face t=0 or s+t=1 or r=1
5744 : 0 : triseg.b = tri.b;
5745 : 0 : triseg.m = tri.e0;
5746 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,s);
5747 : 0 : t = 0;
5748 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5749 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5750 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
5751 : 0 : s0 = 1-t0;
5752 [ # # ]: 0 : if ( min0 < min )
5753 : : {
5754 : 0 : min = min0;
5755 : 0 : r = r0;
5756 : 0 : s = s0;
5757 : 0 : t = t0;
5758 : : }
5759 [ # # ]: 0 : pt = seg.b+seg.m;
5760 [ # # ]: 0 : min0 = MinPointTriangle(pt,tri,s0,t0);
5761 : 0 : r0 = 1;
5762 [ # # ]: 0 : if ( min0 < min )
5763 : : {
5764 : 0 : min = min0;
5765 : 0 : r = r0;
5766 : 0 : s = s0;
5767 : 0 : t = t0;
5768 : : }
5769 : : }
5770 : : else // region 1p
5771 : : {
5772 : : // min on face s+t=1 or r=1
5773 [ # # ]: 0 : triseg.b = tri.b+tri.e0;
5774 [ # # ]: 0 : triseg.m = tri.e1-tri.e0;
5775 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,t);
5776 : 0 : s = 1-t;
5777 [ # # ]: 0 : pt = seg.b+seg.m;
5778 [ # # ]: 0 : min0 = MinPointTriangle(pt,tri,s0,t0);
5779 : 0 : r0 = 1;
5780 [ # # ]: 0 : if ( min0 < min )
5781 : : {
5782 : 0 : min = min0;
5783 : 0 : r = r0;
5784 : 0 : s = s0;
5785 : 0 : t = t0;
5786 : : }
5787 : : }
5788 : : }
5789 : : }
5790 : : }
5791 : : else
5792 : : {
5793 : : // line and triangle are parallel
5794 : 0 : triseg.b = tri.b;
5795 : 0 : triseg.m = tri.e0;
5796 [ # # ]: 0 : min = MinLineSegmentLineSegment(seg,triseg,r,s);
5797 : 0 : t = 0;
5798 : :
5799 : 0 : triseg.m = tri.e1;
5800 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
5801 : 0 : s0 = 0;
5802 [ # # ]: 0 : if ( min0 < min )
5803 : : {
5804 : 0 : min = min0;
5805 : 0 : r = r0;
5806 : 0 : s = s0;
5807 : 0 : t = t0;
5808 : : }
5809 : :
5810 [ # # ]: 0 : triseg.b = triseg.b + tri.e0;
5811 [ # # ]: 0 : triseg.m = triseg.m - tri.e0;
5812 [ # # ]: 0 : min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
5813 : 0 : s0 = 1-t0;
5814 [ # # ]: 0 : if ( min0 < min )
5815 : : {
5816 : 0 : min = min0;
5817 : 0 : r = r0;
5818 : 0 : s = s0;
5819 : 0 : t = t0;
5820 : : }
5821 : :
5822 [ # # ]: 0 : min0 = MinPointTriangle(seg.b,tri,s0,t0);
5823 : 0 : r0 = 0;
5824 [ # # ]: 0 : if ( min0 < min )
5825 : : {
5826 : 0 : min = min0;
5827 : 0 : r = r0;
5828 : 0 : s = s0;
5829 : 0 : t = t0;
5830 : : }
5831 : :
5832 [ # # ]: 0 : pt = seg.b+seg.m;
5833 [ # # ]: 0 : min0 = MinPointTriangle(pt,tri,s0,t0);
5834 : 0 : r0 = 1;
5835 [ # # ]: 0 : if ( min0 < min )
5836 : : {
5837 : 0 : min = min0;
5838 : 0 : r = r0;
5839 : 0 : s = s0;
5840 : 0 : t = t0;
5841 : : }
5842 : : }
5843 : :
5844 : 0 : return min;
5845 : : }
5846 : : //---------------------------------------------------------------------------
5847 : :
5848 : : #ifdef LIN3TRI3_TEST
5849 : :
5850 : : //#define RAND (2.0f*rand()/float(RAND_MAX)-1)
5851 : :
5852 : : //ofstream ostr("data.txt");
5853 : :
5854 : : void main ()
5855 : : {
5856 : : Triangle3 tri;
5857 : : Line3 line;
5858 : : Point3 p, q, diff;
5859 : : double r, s, t, r0, s0, t0, min0, min1, dist;
5860 : :
5861 : : double maxdiff = 0;
5862 : :
5863 : : for (int i = 0; i < 128; i++)
5864 : : {
5865 : : tri.b.x = RAND;
5866 : : tri.b.y = RAND;
5867 : : tri.b.z = RAND;
5868 : : tri.e0.x = RAND;
5869 : : tri.e0.y = RAND;
5870 : : tri.e0.z = RAND;
5871 : : tri.e1.x = RAND;
5872 : : tri.e1.y = RAND;
5873 : : tri.e1.z = RAND;
5874 : :
5875 : : line.b.x = RAND;
5876 : : line.b.y = RAND;
5877 : : line.b.z = RAND;
5878 : : if ( i % 2 )
5879 : : {
5880 : : // non-parallel line and triangle
5881 : : line.m.x = RAND;
5882 : : line.m.y = RAND;
5883 : : line.m.z = RAND;
5884 : : }
5885 : : else
5886 : : {
5887 : : // line is parallel to triangle
5888 : : double c0 = RAND;
5889 : : double c1 = RAND;
5890 : : line.m = c0*tri.e0+c1*tri.e1;
5891 : : }
5892 : :
5893 : : min0 = FLT_MAX;
5894 : : int max = 32;
5895 : : for (int z = 0; z <= max; z++)
5896 : : {
5897 : : r0 = z/double(max);
5898 : : p = line.b+r0*line.m;
5899 : : for (int y = 0; y <= max; y++)
5900 : : {
5901 : : s0 = y/double(max);
5902 : : for (int x = 0; x+y <= max; x++)
5903 : : {
5904 : : t0 = x/double(max);
5905 : : q = tri.b+s0*tri.e0+t0*tri.e1;
5906 : :
5907 : : diff = p-q;
5908 : : dist = Length(diff);
5909 : : if ( dist < min0 )
5910 : : {
5911 : : min0 = dist;
5912 : : r = r0;
5913 : : s = s0;
5914 : : t = t0;
5915 : : }
5916 : : }
5917 : : }
5918 : : }
5919 : : ostr << "i = " << i << endl;
5920 : : ostr << "sampled = " << r << ' ' << s << ' ' << t << ' '
5921 : : << min0 << endl;
5922 : :
5923 : : min1 = MinLineSegmentTriangle(line,tri,r,s,t);
5924 : : ostr << "analytic = " << r << ' ' << s << ' ' << t << ' '
5925 : : << min1 << endl;
5926 : :
5927 : : ostr << "diff = " << min1-min0 << endl;
5928 : :
5929 : : if ( min1-min0 > maxdiff )
5930 : : maxdiff = min1-min0;
5931 : :
5932 : : ostr << endl;
5933 : : }
5934 : :
5935 : : ostr << "max diff = " << maxdiff << endl;
5936 : : }
5937 : : #endif
5938 : :
5939 : : //FILE: triasect.cpp
5940 : : //---------------------------------------------------------------------------
5941 : : AgtLine
5942 : 0 : AnalyticGeometryTool::EdgeToLine (Point2* v0, Point2* v1)
5943 : : {
5944 : : // assert: v0 and v1 are distinct
5945 : :
5946 : 0 : Point2 edge = { v1->x - v0->x, v1->y - v0->y };
5947 : :
5948 : : AgtLine line;
5949 : 0 : line.N.x = edge.y;
5950 : 0 : line.N.y = -edge.x;
5951 : 0 : line.c = line.N.x*v0->x + line.N.y*v0->y;
5952 : :
5953 : 0 : return line;
5954 : : }
5955 : : //---------------------------------------------------------------------------
5956 : : void
5957 : 0 : AnalyticGeometryTool::TriangleLines (Triangle* T, AgtLine line[3])
5958 : : {
5959 [ # # ]: 0 : line[0] = EdgeToLine(&T->v[0],&T->v[1]);
5960 [ # # ]: 0 : line[1] = EdgeToLine(&T->v[0],&T->v[2]);
5961 [ # # ]: 0 : line[2] = EdgeToLine(&T->v[1],&T->v[2]);
5962 : :
5963 : : // make sure normals point outwards
5964 : 0 : Point2 avr = { 0.0, 0.0 };
5965 : : int i;
5966 [ # # ]: 0 : for (i = 0; i < 3; i++)
5967 : : {
5968 : 0 : avr.x += T->v[i].x;
5969 : 0 : avr.y += T->v[i].y;
5970 : : }
5971 : : static double oneThird = 1.0/3.0;
5972 : 0 : avr.x *= oneThird;
5973 : 0 : avr.y *= oneThird;
5974 : :
5975 [ # # ]: 0 : for (i = 0; i < 3; i++)
5976 : : {
5977 : 0 : double dot = line[i].N.x*avr.x+line[i].N.y*avr.y-line[i].c;
5978 [ # # ]: 0 : if ( dot > 0.0 )
5979 : : {
5980 : 0 : line[i].N.x = -line[i].N.x;
5981 : 0 : line[i].N.y = -line[i].N.y;
5982 : 0 : line[i].c = -line[i].c;
5983 : : }
5984 : : }
5985 : 0 : }
5986 : : //---------------------------------------------------------------------------
5987 : : AgtTriList*
5988 : 0 : AnalyticGeometryTool::SplitAndDecompose (Triangle* T, AgtLine* line)
5989 : : {
5990 : : double c[3];
5991 : 0 : int positive = 0, ip[3];
5992 : 0 : int negative = 0, in[3];
5993 : : int i;
5994 [ # # ]: 0 : for (i = 0; i < 3; i++)
5995 : : {
5996 : 0 : c[i] = line->N.x*T->v[i].x + line->N.y*T->v[i].y - line->c;
5997 [ # # ]: 0 : if ( c[i] > 0.0 )
5998 : : {
5999 : 0 : ip[positive++] = i;
6000 : : }
6001 [ # # ]: 0 : else if ( c[i] < 0.0 )
6002 : : {
6003 : 0 : in[negative++] = i;
6004 : : }
6005 : : }
6006 : :
6007 : : // For a split to occur, one of the c_i must be positive and one must
6008 : : // be negative.
6009 : 0 : AgtTriList* list = NULL;
6010 : :
6011 [ # # ]: 0 : if ( negative == 0 ) // T is completely on the positive side of line
6012 : 0 : return 0;
6013 : :
6014 [ # # ]: 0 : if ( positive == 0 )
6015 : : {
6016 : : // T is completely on the negative side of line
6017 [ # # ]: 0 : list = new AgtTriList;
6018 : 0 : list->tri = T;
6019 : 0 : list->next = 0;
6020 : 0 : return list;
6021 : : }
6022 : :
6023 : : // T is split by line. Determine how it is split and how to decompose
6024 : : // the negative-side portion into triangles (3 cases).
6025 : : double w0, w1, cdiff;
6026 : : Point2 intrp[2];
6027 : :
6028 [ # # ]: 0 : if ( positive == 2 )
6029 : : {
6030 : : // ++-
6031 [ # # ]: 0 : for (i = 0; i < 2 /* = positive */; i++)
6032 : : {
6033 : 0 : cdiff = c[ip[i]]-c[in[0]];
6034 : 0 : w0 = -c[in[0]]/cdiff;
6035 : 0 : w1 = c[ip[i]]/cdiff;
6036 : 0 : T->v[ip[i]].x = w0*T->v[ip[i]].x+w1*T->v[in[0]].x;
6037 : 0 : T->v[ip[i]].y = w0*T->v[ip[i]].y+w1*T->v[in[0]].y;
6038 : : }
6039 [ # # ]: 0 : list = new AgtTriList;
6040 : 0 : list->tri = T;
6041 : 0 : list->next = 0;
6042 : : }
6043 [ # # ]: 0 : else if ( positive == 1 )
6044 : : {
6045 [ # # ]: 0 : if ( negative == 2 )
6046 : : {
6047 : : // +--
6048 [ # # ]: 0 : for (i = 0; i < 2 /* = negative */; i++)
6049 : : {
6050 : 0 : cdiff = c[ip[0]]-c[in[i]];
6051 : 0 : w0 = -c[in[i]]/cdiff;
6052 : 0 : w1 = c[ip[0]]/cdiff;
6053 : 0 : intrp[i].x = w0*T->v[ip[0]].x+w1*T->v[in[i]].x;
6054 : 0 : intrp[i].y = w0*T->v[ip[0]].y+w1*T->v[in[i]].y;
6055 : : }
6056 : :
6057 : 0 : T->v[ip[0]] = intrp[0];
6058 [ # # ]: 0 : list = new AgtTriList;
6059 : 0 : list->tri = T;
6060 : :
6061 [ # # ]: 0 : Triangle* T1 = new Triangle;
6062 : 0 : T1->v[0] = intrp[0];
6063 : 0 : T1->v[1] = T->v[in[1]];
6064 : 0 : T1->v[2] = intrp[1];
6065 [ # # ]: 0 : list->next = new AgtTriList;
6066 : 0 : list->next->tri = T1;
6067 : 0 : list->next->next = 0;
6068 : : }
6069 : : else
6070 : : {
6071 : : // +-0
6072 : 0 : cdiff = c[ip[0]]-c[in[0]];
6073 : 0 : w0 = -c[in[0]]/cdiff;
6074 : 0 : w1 = c[ip[0]]/cdiff;
6075 : 0 : T->v[ip[0]].x = w0*T->v[ip[0]].x+w1*T->v[in[0]].x;
6076 : 0 : T->v[ip[0]].y = w0*T->v[ip[0]].y+w1*T->v[in[0]].y;
6077 [ # # ]: 0 : list = new AgtTriList;
6078 : 0 : list->tri = T;
6079 : 0 : list->next = 0;
6080 : : }
6081 : : }
6082 : :
6083 : 0 : return list;
6084 : : }
6085 : : //---------------------------------------------------------------------------
6086 : : AgtTriList*
6087 : 0 : AnalyticGeometryTool::Intersection (Triangle* T0, Triangle* T1)
6088 : : {
6089 : : // build edges of T0
6090 : : AgtLine line[3];
6091 [ # # ]: 0 : TriangleLines(T0,line);
6092 : :
6093 : : // initial list is copy of T1 (since triangle may be deleted)
6094 [ # # ]: 0 : AgtTriList* list = new AgtTriList;
6095 [ # # ]: 0 : list->tri = new Triangle;
6096 : 0 : memcpy(list->tri,T1,sizeof(Triangle));
6097 : 0 : list->next = 0;
6098 : :
6099 : : // process subtriangles of T1 against lines of T0
6100 [ # # ]: 0 : for (int i = 0; i < 3; i++)
6101 : : {
6102 : 0 : AgtTriList* save = 0;
6103 [ # # ]: 0 : while ( list )
6104 : : {
6105 : : // get head of list
6106 : 0 : Triangle* T = list->tri;
6107 : :
6108 [ # # ]: 0 : AgtTriList* sad = SplitAndDecompose(T,&line[i]);
6109 [ # # ]: 0 : if ( sad )
6110 : : {
6111 : : // search for end of list
6112 : 0 : AgtTriList* end = sad;
6113 [ # # ]: 0 : while ( end->next )
6114 : 0 : end = end->next;
6115 : :
6116 : : // attach decomposition to front of save-list
6117 : 0 : end->next = save;
6118 : 0 : save = sad;
6119 : : }
6120 : :
6121 : : // remove head of list
6122 : 0 : AgtTriList* tmp = list;
6123 : 0 : list = list->next;
6124 : :
6125 [ # # ]: 0 : if( sad == NULL )
6126 : 0 : delete tmp->tri;
6127 : 0 : delete tmp;
6128 : : }
6129 : 0 : list = save;
6130 : : }
6131 : :
6132 : 0 : return list;
6133 : : }
6134 : : //---------------------------------------------------------------------------
6135 : : double
6136 : 0 : AnalyticGeometryTool::AreaTriangle (Triangle* T)
6137 : : {
6138 : 0 : Point2 e1 = { T->v[1].x - T->v[0].x, T->v[1].y - T->v[0].y };
6139 : 0 : Point2 e2 = { T->v[2].x - T->v[0].x, T->v[2].y - T->v[0].y };
6140 : :
6141 : 0 : return fabs(0.5*(e1.x*e2.y-e1.y*e2.x));
6142 : : }
6143 : : //---------------------------------------------------------------------------
6144 : : double
6145 : 0 : AnalyticGeometryTool::Area (AgtTriList* list)
6146 : : {
6147 : 0 : double area = 0.0;
6148 : :
6149 [ # # ]: 0 : while ( list )
6150 : : {
6151 : 0 : area += AreaTriangle(list->tri);
6152 : 0 : list = list->next;
6153 : : }
6154 : :
6155 : 0 : return area;
6156 : : }
6157 : : //---------------------------------------------------------------------------
6158 : :
6159 : : double
6160 : 0 : AnalyticGeometryTool::AreaIntersection( Triangle &tri1, Triangle &tri2 )
6161 : : {
6162 : 0 : AgtTriList* list = Intersection(&tri1,&tri2);
6163 : 0 : double area = Area(list);
6164 [ # # ]: 0 : while ( list )
6165 : : {
6166 : 0 : AgtTriList* save = list;
6167 : 0 : list = list->next;
6168 : 0 : delete save->tri;
6169 : 0 : delete save;
6170 : : }
6171 : 0 : delete list;
6172 : 0 : return area;
6173 : : }
6174 : :
6175 : : #ifdef TRIASECT_TEST
6176 : :
6177 : : void main ()
6178 : : {
6179 : : Triangle T0;
6180 : : T0.v[0].x = 0.0; T0.v[0].y = 0.0;
6181 : : T0.v[1].x = 1.0; T0.v[1].y = 0.0;
6182 : : T0.v[2].x = 0.0; T0.v[2].y = 1.0;
6183 : :
6184 : : Triangle T1;
6185 : : const double eps = 0.001;
6186 : : T1.v[0].x = 0.5+eps; T1.v[0].y = 0.5+eps;
6187 : : T1.v[1].x = -eps; T1.v[1].y = 0.5;
6188 : : T1.v[2].x = 0.5; T1.v[2].y = -eps;
6189 : :
6190 : : AgtTriList* list = Intersection(&T0,&T1);
6191 : : double area = Area(list);
6192 : :
6193 : : while ( list )
6194 : : {
6195 : : AgtTriList* save = list;
6196 : : list = list->next;
6197 : : delete save->tri;
6198 : : delete save;
6199 : : }
6200 : : }
6201 : : #endif
6202 : :
6203 : : double
6204 : 0 : AnalyticGeometryTool::Angle( Triangle3& tri1, Triangle3& tri2 )
6205 : : {
6206 : : double norm1[3];
6207 [ # # ]: 0 : Normal( tri1, norm1 );
6208 : :
6209 : : double norm2[3];
6210 [ # # ]: 0 : Normal( tri2, norm2 );
6211 : :
6212 [ # # ]: 0 : return angle_vec_vec( norm1, norm2 );
6213 : : }
6214 : :
6215 : :
6216 : : void
6217 : 0 : AnalyticGeometryTool::Normal( Triangle3& tri, double normal[3] )
6218 : : {
6219 : : double vec1[3];
6220 : 0 : vec1[0]=tri.e0.x; vec1[1]=tri.e0.y; vec1[2]=tri.e0.z;
6221 : :
6222 : : double vec2[3];
6223 : 0 : vec2[0]=tri.e1.x; vec2[1]=tri.e1.y; vec2[2]=tri.e1.z;
6224 : :
6225 [ # # ]: 0 : cross_vec_unit( vec1, vec2, normal );
6226 : 0 : }
6227 : :
6228 : : double
6229 : 0 : AnalyticGeometryTool::ProjectedOverlap( Triangle3& tri1, Triangle3& tri2, bool draw_overlap )
6230 : : {
6231 : : // Transform both triangles into local coordinate system of tri1
6232 : : // to eliminate z-coordinate.
6233 : :
6234 : : // Use normal as z-axis
6235 : : double z[3];
6236 [ # # ]: 0 : Normal( tri1, z );
6237 : :
6238 : : // x-axis goes from b to e0
6239 : : double x[3];
6240 : 0 : x[0] = tri1.e0.x;
6241 : 0 : x[1] = tri1.e0.y;
6242 : 0 : x[2] = tri1.e0.z;
6243 : :
6244 : : // Cross z with x to get y-axis
6245 : : double y[3];
6246 [ # # ]: 0 : cross_vec( z, x, y );
6247 : :
6248 : : // Unitize them all
6249 [ # # ]: 0 : unit_vec( x, x );
6250 [ # # ]: 0 : unit_vec( y, y );
6251 [ # # ]: 0 : unit_vec( z, z );
6252 : :
6253 : : double origin[3];
6254 : 0 : origin[0] = tri1.b.x;
6255 : 0 : origin[1] = tri1.b.y;
6256 : 0 : origin[2] = tri1.b.z;
6257 : :
6258 : : double mtxTriOne2Global[4][4];
6259 [ # # ]: 0 : vecs_to_mtx( x, y, z, origin, mtxTriOne2Global ); // Really mtxTriOne2Global
6260 : : double mtxGlobal2TriOne[4][4];
6261 [ # # ]: 0 : inv_trans_mtx( mtxTriOne2Global, mtxGlobal2TriOne );
6262 : :
6263 : : double v0[3], v1[3], v2[3]; // Vertices of triangle
6264 : 0 : v0[0]=tri1.b.x; v0[1]=tri1.b.y; v0[2]=tri1.b.z;
6265 : 0 : v1[0]=tri1.e0.x+tri1.b.x; v1[1]=tri1.e0.y+tri1.b.y; v1[2]=tri1.e0.z+tri1.b.z;
6266 : 0 : v2[0]=tri1.e1.x+tri1.b.x; v2[1]=tri1.e1.y+tri1.b.y; v2[2]=tri1.e1.z+tri1.b.z;
6267 [ # # ]: 0 : transform_pnt( mtxGlobal2TriOne, v0, v0 );
6268 [ # # ]: 0 : transform_pnt( mtxGlobal2TriOne, v1, v1 );
6269 [ # # ]: 0 : transform_pnt( mtxGlobal2TriOne, v2, v2 );
6270 : :
6271 : : // Load this into a 2D triangle T1 ( z-axis is now zero )
6272 : : Triangle T1;
6273 : 0 : T1.v[0].x = v0[0]; T1.v[0].y = v0[1];
6274 : 0 : T1.v[1].x = v1[0]; T1.v[1].y = v1[1];
6275 : 0 : T1.v[2].x = v2[0]; T1.v[2].y = v2[1];
6276 : :
6277 : : // Setup the next triangle, in this coordinate system
6278 : 0 : v0[0]=tri2.b.x; v0[1]=tri2.b.y; v0[2]=tri2.b.z;
6279 : 0 : v1[0]=tri2.e0.x+tri2.b.x; v1[1]=tri2.e0.y+tri2.b.y; v1[2]=tri2.e0.z+tri2.b.z;
6280 : 0 : v2[0]=tri2.e1.x+tri2.b.x; v2[1]=tri2.e1.y+tri2.b.y; v2[2]=tri2.e1.z+tri2.b.z;
6281 : :
6282 : : // The following creates coordinates of tri2 in tri1's csys
6283 [ # # ]: 0 : transform_pnt( mtxGlobal2TriOne, v0, v0 );
6284 [ # # ]: 0 : transform_pnt( mtxGlobal2TriOne, v1, v1 );
6285 [ # # ]: 0 : transform_pnt( mtxGlobal2TriOne, v2, v2 );
6286 : :
6287 : : // Now we can project tri2 to tri1 simply by dropping the z-coordinate
6288 : : Triangle T2;
6289 : 0 : T2.v[0].x = v0[0]; T2.v[0].y = v0[1];
6290 : 0 : T2.v[1].x = v1[0]; T2.v[1].y = v1[1];
6291 : 0 : T2.v[2].x = v2[0]; T2.v[2].y = v2[1];
6292 : :
6293 : : // Now find area of overlap
6294 [ # # ]: 0 : if( draw_overlap )
6295 : : {
6296 [ # # ]: 0 : AgtTriList* list = Intersection(&T1, &T2);
6297 [ # # ]: 0 : double overlap_area = Area(list);
6298 [ # # ]: 0 : while ( list )
6299 : : {
6300 : 0 : Triangle *tmp_tri = list->tri;
6301 : :
6302 : 0 : v0[0]=tmp_tri->v[0].x;
6303 : 0 : v0[1]=tmp_tri->v[0].y;
6304 : 0 : v0[2]=0;
6305 : :
6306 : 0 : v1[0]=tmp_tri->v[1].x;
6307 : 0 : v1[1]=tmp_tri->v[1].y;
6308 : 0 : v1[2]=0;
6309 : :
6310 : 0 : v2[0]=tmp_tri->v[2].x;
6311 : 0 : v2[1]=tmp_tri->v[2].y;
6312 : 0 : v2[2]=0;
6313 : :
6314 [ # # ]: 0 : transform_pnt( mtxTriOne2Global, v0, v0 );
6315 [ # # ]: 0 : transform_pnt( mtxTriOne2Global, v1, v1 );
6316 [ # # ]: 0 : transform_pnt( mtxTriOne2Global, v2, v2 );
6317 : :
6318 : : //Now Draw the thing
6319 [ # # ]: 0 : GMem g_mem;
6320 [ # # ]: 0 : g_mem.allocate_tri(1);
6321 : 0 : g_mem.pointListCount = 3;
6322 : :
6323 [ # # ]: 0 : g_mem.point_list()[0].x = v0[0];
6324 [ # # ]: 0 : g_mem.point_list()[0].y = v0[1];
6325 [ # # ]: 0 : g_mem.point_list()[0].z = v0[2];
6326 : :
6327 [ # # ]: 0 : g_mem.point_list()[1].x = v1[0];
6328 [ # # ]: 0 : g_mem.point_list()[1].y = v1[1];
6329 [ # # ]: 0 : g_mem.point_list()[1].z = v1[2];
6330 : :
6331 [ # # ]: 0 : g_mem.point_list()[2].x = v2[0];
6332 [ # # ]: 0 : g_mem.point_list()[2].y = v2[1];
6333 [ # # ]: 0 : g_mem.point_list()[2].z = v2[2];
6334 : :
6335 [ # # ]: 0 : g_mem.facet_list()[0] = 3;
6336 [ # # ]: 0 : g_mem.facet_list()[1] = 0;
6337 [ # # ]: 0 : g_mem.facet_list()[2] = 1;
6338 [ # # ]: 0 : g_mem.facet_list()[3] = 2;
6339 : :
6340 : 0 : g_mem.fListCount = 1;
6341 [ # # ][ # # ]: 0 : GfxPreview::draw_polygon(g_mem.point_list(),3,4,4,true);
6342 : :
6343 : 0 : AgtTriList* save = list;
6344 : 0 : list = list->next;
6345 : 0 : delete save->tri;
6346 : 0 : delete save;
6347 [ # # ]: 0 : }
6348 : 0 : delete list;
6349 : 0 : return overlap_area;
6350 : : }
6351 : : else
6352 [ # # ]: 0 : return AreaIntersection( T1, T2 );
6353 [ + - ][ + - ]: 6540 : }
|
