Branch data Line data Source code
1 : : //- Class: CubitVector
2 : : //- Description: This file defines the CubitVector class.
3 : : //- Owner: Greg Sjaardema
4 : : //- Checked by:
5 : :
6 : : #include <math.h>
7 : : #include "CubitVector.hpp"
8 : : #include <string>
9 : : #include <stdexcept>
10 : : #include "CubitPlane.hpp"
11 : : #include "GeometryDefines.h"
12 : : #include "CubitBox.hpp"
13 : :
14 : : // Define PI and TWO_PI
15 : : #undef PI
16 : : #ifdef M_PI
17 : : const double PI = M_PI;
18 : : #else
19 : : const double PI = 3.14159265358979323846;
20 : : #endif
21 : : const double TWO_PI = 2.0 * PI;
22 : :
23 : :
24 : 0 : CubitVector &CubitVector::length(const double new_length)
25 : : {
26 : 0 : double length = this->length();
27 [ # # ]: 0 : if(length > 1e-6)
28 : : {
29 : 0 : xVal *= new_length / length;
30 : 0 : yVal *= new_length / length;
31 : 0 : zVal *= new_length / length;
32 : : }
33 : 0 : return *this;
34 : : }
35 : :
36 : :
37 : 22214 : double CubitVector::distance_between_squared(const CubitVector& test_vector) const
38 : : {
39 : 22214 : double x = xVal - test_vector.xVal;
40 : 22214 : double y = yVal - test_vector.yVal;
41 : 22214 : double z = zVal - test_vector.zVal;
42 : :
43 : 22214 : return(x*x + y*y + z*z);
44 : : }
45 : :
46 : 50391 : double CubitVector::distance_between(const CubitVector& test_vector) const
47 : : {
48 : 50391 : double x = xVal - test_vector.xVal;
49 : 50391 : double y = yVal - test_vector.yVal;
50 : 50391 : double z = zVal - test_vector.zVal;
51 : :
52 : 50391 : return(sqrt(x*x + y*y + z*z));
53 : : }
54 : :
55 : 0 : double CubitVector::distance_from_infinite_line(const CubitVector& point_on_line,
56 : : const CubitVector& line_direction) const
57 : : {
58 : 0 : return sqrt(distance_from_infinite_line_squared(point_on_line, line_direction));
59 : : }
60 : :
61 : 0 : double CubitVector::distance_from_infinite_line_squared(
62 : : const CubitVector& point_on_line,
63 : : const CubitVector& line_direction) const
64 : : {
65 [ # # ][ # # ]: 0 : if (line_direction == CubitVector(0, 0, 0))
[ # # ]
66 [ # # ]: 0 : return distance_between_squared(point_on_line);
67 : :
68 [ # # ]: 0 : CubitVector v = *this - point_on_line;
69 [ # # ]: 0 : double v_dot_d = v % line_direction;
70 [ # # ][ # # ]: 0 : return fabs(v.length_squared() - v_dot_d * v_dot_d / line_direction.length_squared());
71 : : }
72 : :
73 : : // double CubitVector::distance_between(const CubitVector& test_vector, RefEdge* test_edge)
74 : : // {
75 : : // return( test_edge->get_arc_length(*this, test_vector) );
76 : : // }
77 : :
78 : :
79 : 0 : void CubitVector::print_me()
80 : : {
81 : 0 : printf("X: %f\n",xVal);
82 : 0 : printf("Y: %f\n",yVal);
83 : 0 : printf("Z: %f\n",zVal);
84 : :
85 : 0 : }
86 : :
87 : :
88 : 0 : double CubitVector::interior_angle(const CubitVector &otherVector) const
89 : : {
90 : 0 : double cosAngle = 0.0, angleRad = 0.0, len1, len2 = 0.0;
91 : :
92 [ # # ][ # # ]: 0 : if (((len1 = this->length()) > 0) && ((len2 = otherVector.length()) > 0))
[ # # ]
93 : 0 : cosAngle = (*this % otherVector)/(len1 * len2);
94 : : else
95 : : {
96 [ # # ][ # # ]: 0 : if(len1<=0||len2<=0)
97 [ # # ][ # # ]: 0 : throw std::invalid_argument ("Length of 'this' or parameter must be > 0");
98 : : // assert(len1 > 0);
99 : : // assert(len2 > 0);
100 : : }
101 : :
102 [ # # ][ # # ]: 0 : if ((cosAngle > 1.0) && (cosAngle < 1.0001))
103 : : {
104 : 0 : cosAngle = 1.0;
105 : 0 : angleRad = acos(cosAngle);
106 : : }
107 [ # # ][ # # ]: 0 : else if (cosAngle < -1.0 && cosAngle > -1.0001)
108 : : {
109 : 0 : cosAngle = -1.0;
110 : 0 : angleRad = acos(cosAngle);
111 : : }
112 [ # # ][ # # ]: 0 : else if (cosAngle >= -1.0 && cosAngle <= 1.0)
113 : 0 : angleRad = acos(cosAngle);
114 : : else
115 : : {
116 [ # # ][ # # ]: 0 : if(cosAngle > -1.0001 && cosAngle < 1.0001)
117 [ # # ][ # # ]: 0 : throw std::invalid_argument ("cosAngle must be between -1.0001 and 1.0001");
118 : : // assert(cosAngle < 1.0001 && cosAngle > -1.0001);
119 : : }
120 : :
121 : 0 : return( (angleRad * 180.) / PI );
122 : : }
123 : :
124 : :
125 : 0 : void CubitVector::xy_to_rtheta()
126 : : {
127 : : //careful about overwriting
128 : 0 : double r_ = length();
129 : 0 : double theta_ = atan2( yVal, xVal );
130 [ # # ]: 0 : if (theta_ < 0.0)
131 : 0 : theta_ += TWO_PI;
132 : :
133 : 0 : r( r_ );
134 : 0 : theta( theta_ );
135 : 0 : }
136 : :
137 : 0 : void CubitVector::rtheta_to_xy()
138 : : {
139 : : //careful about overwriting
140 : 0 : double x_ = r() * cos( theta() );
141 : 0 : double y_ = r() * sin( theta() );
142 : :
143 : 0 : x( x_ );
144 : 0 : y( y_ );
145 : 0 : }
146 : :
147 : 0 : void CubitVector::rotate(double angle, double )
148 : : {
149 : 0 : xy_to_rtheta();
150 : 0 : theta() += angle;
151 : 0 : rtheta_to_xy();
152 : 0 : }
153 : :
154 : 0 : void CubitVector::blow_out(double gamma, double rmin)
155 : : {
156 : : // if gamma == 1, then
157 : : // map on a circle : r'^2 = sqrt( 1 - (1-r)^2 )
158 : : // if gamma ==0, then map back to itself
159 : : // in between, linearly interpolate
160 : 0 : xy_to_rtheta();
161 : : // r() = sqrt( (2. - r()) * r() ) * gamma + r() * (1-gamma);
162 [ # # ]: 0 : if(gamma <= 0.0)
163 : : {
164 [ # # ][ # # ]: 0 : throw std::invalid_argument( "Gamma must be greater than zero" );
165 : : }
166 : : // the following limits should really be roundoff-based
167 [ # # ][ # # ]: 0 : if (r() > rmin*1.001 && r() < 1.001) {
[ # # ]
168 : 0 : r() = rmin + pow(r(), gamma) * (1.0 - rmin);
169 : : }
170 : 0 : rtheta_to_xy();
171 : 0 : }
172 : :
173 : 0 : void CubitVector::reflect_about_xaxis(double, double )
174 : : {
175 : 0 : yVal = -yVal;
176 : 0 : }
177 : :
178 : 0 : void CubitVector::scale_angle(double gamma, double )
179 : : {
180 : 0 : const double r_factor = 0.3;
181 : 0 : const double theta_factor = 0.6;
182 : :
183 : 0 : xy_to_rtheta();
184 : :
185 : : // if neary 2pi, treat as zero
186 : : // some near zero stuff strays due to roundoff
187 [ # # ]: 0 : if (theta() > TWO_PI - 0.02)
188 : 0 : theta() = 0;
189 : : // the above screws up on big sheets - need to overhaul at the sheet level
190 : :
191 [ # # ]: 0 : if ( gamma < 1 )
192 : : {
193 : : //squeeze together points of short radius so that
194 : : //long chords won't cross them
195 : 0 : theta() += (CUBIT_PI-theta())*(1-gamma)*theta_factor*(1-r());
196 : :
197 : : //push away from center of circle, again so long chords won't cross
198 : 0 : r( (r_factor + r()) / (1 + r_factor) );
199 : :
200 : : //scale angle by gamma
201 : 0 : theta() *= gamma;
202 : : }
203 : : else
204 : : {
205 : : //scale angle by gamma, making sure points nearly 2pi are treated as zero
206 : 0 : double new_theta = theta() * gamma;
207 [ # # ][ # # ]: 0 : if ( new_theta < 2.5 * CUBIT_PI || r() < 0.2)
[ # # ]
208 : 0 : theta( new_theta );
209 : : }
210 : 0 : rtheta_to_xy();
211 : 0 : }
212 : :
213 : 0 : double CubitVector::vector_angle_quick(const CubitVector& vec1,
214 : : const CubitVector& vec2)
215 : : {
216 : : //- compute the angle between two vectors in the plane defined by this vector
217 : : // build yAxis and xAxis such that xAxis is the projection of
218 : : // vec1 onto the normal plane of this vector
219 : :
220 : : // NOTE: vec1 and vec2 are Vectors from the vertex of the angle along
221 : : // the two sides of the angle.
222 : : // The angle returned is the right-handed angle around this vector
223 : : // from vec1 to vec2.
224 : :
225 : : // NOTE: vector_angle_quick gives exactly the same answer as vector_angle below
226 : : // providing this vector is normalized. It does so with two fewer
227 : : // cross-product evaluations and two fewer vector normalizations.
228 : : // This can be a substantial time savings if the function is called
229 : : // a significant number of times (e.g Hexer) ... (jrh 11/28/94)
230 : : // NOTE: vector_angle() is much more robust. Do not use vector_angle_quick()
231 : : // unless you are very sure of the safety of your input vectors.
232 : :
233 [ # # ]: 0 : CubitVector ry = (*this) * vec1;
234 [ # # ]: 0 : CubitVector rx = ry * (*this);
235 : :
236 [ # # ]: 0 : double x = vec2 % rx;
237 [ # # ]: 0 : double y = vec2 % ry;
238 : :
239 : : double angle;
240 [ # # ][ # # ]: 0 : if( x == 0.0 && y == 0.0 )
241 : : {
242 : 0 : return 0.0;
243 : : }
244 : :
245 : 0 : angle = atan2(y, x);
246 : :
247 [ # # ]: 0 : if (angle < 0.0)
248 : : {
249 : 0 : angle += TWO_PI;
250 : : }
251 : :
252 : : // Sometimes angle was slightly less than zero,
253 : : // but adding TWO_PI puts us at exactly TWO_PI.
254 : : // More likely on optimized builds.
255 : : // "volatile" is to remove false precision
256 : : // maintained within the scope of this function
257 [ # # ]: 0 : if((*(volatile double*)&angle) >= TWO_PI)
258 : : {
259 : 0 : angle -= TWO_PI;
260 : : }
261 : :
262 : 0 : return angle;
263 : : }
264 : :
265 : 0 : CubitVector vectorRotate(const double angle,
266 : : const CubitVector &normalAxis,
267 : : const CubitVector &referenceAxis)
268 : : {
269 : : // A new coordinate system is created with the xy plane corresponding
270 : : // to the plane normal to the normal axis, and the x axis corresponding to
271 : : // the projection of the reference axis onto the normal plane. The normal
272 : : // plane is the tangent plane at the root point. A unit vector is
273 : : // constructed along the local x axis and then rotated by the given
274 : : // ccw angle to form the new point. The new point, then is a unit
275 : : // distance from the global origin in the tangent plane.
276 : :
277 : : double x, y;
278 : :
279 : : // project a unit distance from root along reference axis
280 : :
281 [ # # ]: 0 : CubitVector yAxis = normalAxis * referenceAxis;
282 [ # # ]: 0 : CubitVector xAxis = yAxis * normalAxis;
283 [ # # ]: 0 : yAxis.normalize();
284 [ # # ]: 0 : xAxis.normalize();
285 : :
286 : 0 : x = cos(angle);
287 : 0 : y = sin(angle);
288 : :
289 [ # # ]: 0 : xAxis *= x;
290 [ # # ]: 0 : yAxis *= y;
291 [ # # ]: 0 : return CubitVector(xAxis + yAxis);
292 : : }
293 : :
294 : 264 : double CubitVector::vector_angle(const CubitVector &vector1,
295 : : const CubitVector &vector2) const
296 : : {
297 : : // This routine does not assume that any of the input vectors are of unit
298 : : // length. This routine does not normalize the input vectors.
299 : : // Special cases:
300 : : // If the normal vector is zero length:
301 : : // If a new one can be computed from vectors 1 & 2:
302 : : // the normal is replaced with the vector cross product
303 : : // else the two vectors are colinear and zero or 2PI is returned.
304 : : // If the normal is colinear with either (or both) vectors
305 : : // a new one is computed with the cross products
306 : : // (and checked again).
307 : :
308 : : // Check for zero length normal vector
309 [ + - ]: 264 : CubitVector normal = *this;
310 [ + - ]: 264 : double normal_lensq = normal.length_squared();
311 : 264 : double len_tol = 0.0000001;
312 [ - + ]: 264 : if( normal_lensq <= len_tol )
313 : : {
314 : : // null normal - make it the normal to the plane defined by vector1
315 : : // and vector2. If still null, the vectors are colinear so check
316 : : // for zero or 180 angle.
317 [ # # ][ # # ]: 0 : normal = vector1 * vector2;
318 [ # # ]: 0 : normal_lensq = normal.length_squared();
319 [ # # ]: 0 : if( normal_lensq <= len_tol )
320 : : {
321 [ # # ]: 0 : double cosine = vector1 % vector2;
322 [ # # ]: 0 : if( cosine > 0.0 ) return 0.0;
323 : 0 : else return CUBIT_PI;
324 : : }
325 : : }
326 : :
327 : : //Trap for normal vector colinear to one of the other vectors. If so,
328 : : //use a normal defined by the two vectors.
329 : 264 : double dot_tol = 0.985;
330 [ + - ]: 264 : double dot = vector1 % normal;
331 [ + - ][ - + ]: 264 : if( dot * dot >= vector1.length_squared() * normal_lensq * dot_tol )
332 : : {
333 [ # # ][ # # ]: 0 : normal = vector1 * vector2;
334 [ # # ]: 0 : normal_lensq = normal.length_squared();
335 : :
336 : : //Still problems if all three vectors were colinear
337 [ # # ]: 0 : if( normal_lensq <= len_tol )
338 : : {
339 [ # # ]: 0 : double cosine = vector1 % vector2;
340 [ # # ]: 0 : if( cosine >= 0.0 ) return 0.0;
341 : 0 : else return CUBIT_PI;
342 : : }
343 : : }
344 : : else
345 : : {
346 : : //The normal and vector1 are not colinear, now check for vector2
347 [ + - ]: 264 : dot = vector2 % normal;
348 [ + - ][ - + ]: 264 : if( dot * dot >= vector2.length_squared() * normal_lensq * dot_tol )
349 : : {
350 [ # # ][ # # ]: 0 : normal = vector1 * vector2;
351 : : }
352 : : }
353 : :
354 : : // Assume a plane such that the normal vector is the plane's normal.
355 : : // Create yAxis perpendicular to both the normal and vector1. yAxis is
356 : : // now in the plane. Create xAxis as the perpendicular to both yAxis and
357 : : // the normal. xAxis is in the plane and is the projection of vector1
358 : : // into the plane.
359 : :
360 [ + - ]: 264 : normal.normalize();
361 [ + - ]: 264 : CubitVector yAxis = normal;
362 [ + - ]: 264 : yAxis *= vector1;
363 [ + - ]: 264 : double y = vector2 % yAxis;
364 : : // yAxis memory slot will now be used for xAxis
365 [ + - ]: 264 : yAxis *= normal;
366 [ + - ]: 264 : double x = vector2 % yAxis;
367 : :
368 : :
369 : : // assert(x != 0.0 || y != 0.0);
370 [ + - ][ - + ]: 264 : if( x == 0.0 && y == 0.0 )
371 : : {
372 : 0 : return 0.0;
373 : : }
374 : 264 : double angle = atan2(y, x);
375 : :
376 [ - + ]: 264 : if (angle < 0.0)
377 : : {
378 : 0 : angle += TWO_PI;
379 : : }
380 : :
381 : : // Sometimes angle was slightly less than zero,
382 : : // but adding TWO_PI puts us at exactly TWO_PI.
383 : : // More likely on optimized builds.
384 : : // "volatile" is to remove false precision
385 : : // maintained within the scope of this function
386 [ - + ]: 264 : if((*(volatile double*)&angle) >= TWO_PI)
387 : : {
388 : 0 : angle -= TWO_PI;
389 : : }
390 : :
391 : 264 : return angle;
392 : : }
393 : :
394 : 10879 : CubitBoolean CubitVector::within_tolerance( const CubitVector &vectorPtr2,
395 : : double tolerance) const
396 : : {
397 [ + + ]: 5775 : return (( fabs (this->xVal - vectorPtr2.xVal) < tolerance) &&
398 [ + + ][ + + ]: 16654 : ( fabs (this->yVal - vectorPtr2.yVal) < tolerance) &&
399 : 3278 : ( fabs (this->zVal - vectorPtr2.zVal) < tolerance)
400 : 10879 : );
401 : : }
402 : :
403 : 0 : CubitBoolean CubitVector::within_scaled_tolerance(const CubitVector &v2, double tol) const
404 : : {
405 [ # # ]: 0 : if (tol < 0)
406 : 0 : tol = -tol;
407 : :
408 [ # # ][ # # ]: 0 : return (((fabs (xVal - v2.xVal) < tol) || (((xVal > 0) == (v2.xVal > 0)) && fabs(xVal) > tol && fabs(v2.xVal/xVal - 1) < tol)) &&
[ # # ][ # # ]
409 [ # # ][ # # ]: 0 : ((fabs (yVal - v2.yVal) < tol) || (((yVal > 0) == (v2.yVal > 0)) && fabs(yVal) > tol && fabs(v2.yVal/yVal - 1) < tol)) &&
[ # # ][ # # ]
[ # # ]
410 [ # # ][ # # ]: 0 : ((fabs (zVal - v2.zVal) < tol) || (((zVal > 0) == (v2.zVal > 0)) && fabs(zVal) > tol && fabs(v2.zVal/zVal - 1) < tol))
[ # # ]
411 : 0 : );
412 : : }
413 : :
414 : 231999 : CubitBoolean CubitVector::about_equal(const CubitVector &w,
415 : : const double relative_tolerance,
416 : : const double absolute_tolerance ) const
417 : : {
418 [ - + ][ # # ]: 231999 : if ( absolute_tolerance == 0. &&
419 : : relative_tolerance == 0. )
420 : : {
421 [ # # ][ # # ]: 0 : if ( xVal == w.xVal &&
422 [ # # ]: 0 : yVal == w.yVal &&
423 : 0 : zVal == w.zVal )
424 : 0 : return CUBIT_TRUE;
425 : : }
426 : : else
427 : : {
428 [ + - ]: 231999 : const CubitVector diff = *this - w;
429 [ + - ]: 231999 : const double diff_size = diff.length_squared();
430 : 231999 : const double a_tol_size = absolute_tolerance * absolute_tolerance;
431 : : // catches v == w
432 [ + + ]: 231999 : if ( diff_size <= a_tol_size )
433 : 73052 : return CUBIT_TRUE;
434 [ + - ]: 158991 : if ( relative_tolerance > 0. )
435 : : {
436 [ + - ]: 158991 : const CubitVector sum = *this + w;
437 [ + - ]: 158991 : const double sum_size = sum.length_squared();
438 : 158991 : const double r_tol_size = relative_tolerance * relative_tolerance;
439 [ + + ]: 158991 : if ( 4. * diff_size <= sum_size * r_tol_size )
440 : : // Q: why this formula?
441 : : // A: because if v = 1,0,eps, w = 1,0,0, then
442 : : // diff_size = eps^2
443 : : // sum_size = about 4.
444 : : // and function returns true if eps^2 <= tolerance^2
445 : 158991 : return CUBIT_TRUE;
446 : : }
447 : : }
448 : 231999 : return CUBIT_FALSE;
449 : : }
450 : :
451 : :
452 : 0 : void CubitVector::orthogonal_vectors( CubitVector &vector2,
453 : : CubitVector &vector3 ) const
454 : : {
455 : : double x[3];
456 : 0 : unsigned short i = 0;
457 : 0 : unsigned short imin = 0;
458 : 0 : double rmin = 1.0E20;
459 : : unsigned short iperm1[3];
460 : : unsigned short iperm2[3];
461 : 0 : unsigned short cont_flag = 1;
462 : : double vec1[3], vec2[3];
463 : : double rmag;
464 : :
465 : : // Copy the input vector and normalize it
466 [ # # ]: 0 : CubitVector vector1 = *this;
467 [ # # ]: 0 : vector1.normalize();
468 : :
469 : : // Initialize perm flags
470 : 0 : iperm1[0] = 1; iperm1[1] = 2; iperm1[2] = 0;
471 : 0 : iperm2[0] = 2; iperm2[1] = 0; iperm2[2] = 1;
472 : :
473 : : // Get into the array format we can work with
474 [ # # ]: 0 : vector1.get_xyz( vec1 );
475 : :
476 [ # # ][ # # ]: 0 : while (i<3 && cont_flag )
477 : : {
478 [ # # ]: 0 : if (fabs(vec1[i]) < 1e-6)
479 : : {
480 : 0 : vec2[i] = 1.0;
481 : 0 : vec2[iperm1[i]] = 0.0;
482 : 0 : vec2[iperm2[i]] = 0.0;
483 : 0 : cont_flag = 0;
484 : : }
485 : :
486 [ # # ]: 0 : if (fabs(vec1[i]) < rmin)
487 : : {
488 : 0 : imin = i;
489 : 0 : rmin = fabs(vec1[i]);
490 : : }
491 : 0 : ++i;
492 : : }
493 : :
494 [ # # ]: 0 : if (cont_flag)
495 : : {
496 : 0 : x[imin] = 1.0;
497 : 0 : x[iperm1[imin]] = 0.0;
498 : 0 : x[iperm2[imin]] = 0.0;
499 : :
500 : : // Determine cross product
501 : 0 : vec2[0] = vec1[1] * x[2] - vec1[2] * x[1];
502 : 0 : vec2[1] = vec1[2] * x[0] - vec1[0] * x[2];
503 : 0 : vec2[2] = vec1[0] * x[1] - vec1[1] * x[0];
504 : :
505 : : // Unitize
506 : 0 : rmag = sqrt(vec2[0]*vec2[0] + vec2[1]*vec2[1] + vec2[2]*vec2[2]);
507 : 0 : vec2[0] /= rmag;
508 : 0 : vec2[1] /= rmag;
509 : 0 : vec2[2] /= rmag;
510 : : }
511 : :
512 : : // Copy 1st orthogonal vector into CubitVector vector2
513 [ # # ]: 0 : vector2.set( vec2 );
514 : :
515 : : // Cross vectors to determine last orthogonal vector
516 [ # # ][ # # ]: 0 : vector3 = vector1 * vector2;
517 : :
518 : 0 : return;
519 : : }
520 : :
521 : : //- Find next point from this point using a direction and distance
522 : 55 : void CubitVector::next_point( const CubitVector &direction,
523 : : double distance, CubitVector& out_point ) const
524 : : {
525 [ + - ]: 55 : CubitVector my_direction = direction;
526 [ + - ]: 55 : my_direction.normalize();
527 : :
528 : : // Determine next point in space
529 [ + - ]: 55 : out_point.x( xVal + (distance * my_direction.xVal) );
530 [ + - ]: 55 : out_point.y( yVal + (distance * my_direction.yVal) );
531 [ + - ]: 55 : out_point.z( zVal + (distance * my_direction.zVal) );
532 : :
533 : 55 : return;
534 : : }
535 : :
536 : : //- Project this vector onto the plane specified by the input plane normal
537 : 0 : void CubitVector::project_to_plane( const CubitVector &planenormal )
538 : : {
539 [ # # ]: 0 : CubitVector tmp = planenormal;
540 [ # # ]: 0 : tmp.normalize();
541 : :
542 : : // Cross the vector with the normal to get a vector on the plane
543 [ # # ]: 0 : CubitVector planevec = tmp * (*this);
544 : :
545 : : // Cross the vector on the plane with the normal to get the
546 : : // projection of the vector on the plane
547 [ # # ][ # # ]: 0 : *this = planevec * tmp;
548 : 0 : }
549 : :
550 : : //============================================================================
551 : : // Function: barycentric_coordinates
552 : : // Author: mlstate
553 : : // Description: compute the barycentric coordinates of a point w.r.t. to a
554 : : // triangle.
555 : : //============================================================================
556 : 0 : bool CubitVector::barycentric_coordinates
557 : : (
558 : : const CubitVector &v1,
559 : : const CubitVector &v2,
560 : : const CubitVector &v3,
561 : : const CubitVector &point,
562 : : double &coord_A,
563 : : double &coord_B,
564 : : double &coord_C
565 : : )
566 : : {
567 : 0 : return private_barycentric_coordinates(true, v1, v2, v3, point, coord_A, coord_B, coord_C );
568 : : }
569 : :
570 : : //============================================================================
571 : : // Function: private_barycentric_coordinates
572 : : // Author: mlstate
573 : : // Description: compute the barycentric coordinates of a point w.r.t. to a
574 : : // triangle. The private version.
575 : : //============================================================================
576 : 0 : bool CubitVector::private_barycentric_coordinates
577 : : (
578 : : bool adjust_on_fail,
579 : : const CubitVector &v1,
580 : : const CubitVector &v2,
581 : : const CubitVector &v3,
582 : : const CubitVector &point,
583 : : double &coord_A,
584 : : double &coord_B,
585 : : double &coord_C
586 : : )
587 : : {
588 : : #define DETERM3(p1,q1,p2,q2,p3,q3) ((q3)*((p2)-(p1)) + \
589 : : (q2)*((p1)-(p3)) + \
590 : : (q1)*((p3)-(p2)))
591 : :
592 [ # # ][ # # ]: 0 : if ( CubitVector::colinear(v1, v2, v3) )
593 : : {
594 : 0 : return false;
595 : : }
596 : :
597 [ # # ]: 0 : CubitPlane tri_plane;
598 [ # # ]: 0 : tri_plane.mk_plane_with_points( v1, v2, v3 );
599 [ # # ]: 0 : CubitVector pt = tri_plane.project( point );
600 : : double area2;
601 [ # # ][ # # ]: 0 : CubitVector normal = tri_plane.normal();
602 : 0 : double tol = CUBIT_RESABS;
603 [ # # ][ # # ]: 0 : CubitVector absnorm( fabs(normal.x()), fabs(normal.y()), fabs(normal.z()) );
[ # # ][ # # ]
604 : :
605 : : // project to the closest coordinate plane so we only have to do this in 2D
606 [ # # ][ # # ]: 0 : if (absnorm.x() >= absnorm.y() && absnorm.x() >= absnorm.z())
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
607 : : {
608 [ # # ][ # # ]: 0 : area2 = DETERM3(v1.y(), v1.z(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
609 : : v2.y(), v2.z(),
610 : 0 : v3.y(), v3.z());
611 [ # # ]: 0 : if (fabs(area2) < tol)
612 : : {
613 [ # # ]: 0 : if ( adjust_on_fail )
614 : : {
615 : : return attempt_barycentric_coordinates_adjustment(v1, v2, v3, point,
616 : : coord_A, coord_B,
617 [ # # ]: 0 : coord_C);
618 : : }
619 : 0 : return false;
620 : : }
621 [ # # ][ # # ]: 0 : coord_A = ( DETERM3( pt.y(), pt.z(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
622 : : v2.y(), v2.z(),
623 : 0 : v3.y(), v3.z() ) / area2 );
624 [ # # ][ # # ]: 0 : coord_B = ( DETERM3( v1.y(), v1.z(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
625 : : pt.y(), pt.z(),
626 : 0 : v3.y(), v3.z() ) / area2 );
627 [ # # ][ # # ]: 0 : coord_C = ( DETERM3( v1.y(), v1.z(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
628 : : v2.y(), v2.z(),
629 : 0 : pt.y(), pt.z() ) / area2 );
630 : : }
631 [ # # ][ # # ]: 0 : else if(absnorm.y() >= absnorm.x() && absnorm.y() >= absnorm.z())
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
632 : : {
633 [ # # ][ # # ]: 0 : area2 = DETERM3(v1.x(), v1.z(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
634 : : v2.x(), v2.z(),
635 : 0 : v3.x(), v3.z());
636 [ # # ]: 0 : if (fabs(area2) < tol)
637 : : {
638 [ # # ]: 0 : if ( adjust_on_fail )
639 : : {
640 : : return attempt_barycentric_coordinates_adjustment(v1, v2, v3, point,
641 : : coord_A, coord_B,
642 [ # # ]: 0 : coord_C);
643 : : }
644 : 0 : return false;
645 : : }
646 [ # # ][ # # ]: 0 : coord_A = ( DETERM3( pt.x(), pt.z(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
647 : : v2.x(), v2.z(),
648 : 0 : v3.x(), v3.z() ) / area2 );
649 [ # # ][ # # ]: 0 : coord_B = ( DETERM3( v1.x(), v1.z(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
650 : : pt.x(), pt.z(),
651 : 0 : v3.x(), v3.z() ) / area2 );
652 [ # # ][ # # ]: 0 : coord_C = ( DETERM3( v1.x(), v1.z(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
653 : : v2.x(), v2.z(),
654 : 0 : pt.x(), pt.z() ) / area2 );
655 : : }
656 : : else
657 : : {
658 [ # # ][ # # ]: 0 : area2 = DETERM3(v1.x(), v1.y(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
659 : : v2.x(), v2.y(),
660 : 0 : v3.x(), v3.y());
661 [ # # ]: 0 : if (fabs(area2) < tol)
662 : : {
663 [ # # ]: 0 : if ( adjust_on_fail )
664 : : {
665 : : return attempt_barycentric_coordinates_adjustment(v1, v2, v3, point,
666 : : coord_A, coord_B,
667 [ # # ]: 0 : coord_C);
668 : : }
669 : 0 : return false;
670 : : }
671 [ # # ][ # # ]: 0 : coord_A = ( DETERM3( pt.x(), pt.y(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
672 : : v2.x(), v2.y(),
673 : 0 : v3.x(), v3.y() ) / area2 );
674 [ # # ][ # # ]: 0 : coord_B = ( DETERM3( v1.x(), v1.y(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
675 : : pt.x(), pt.y(),
676 : 0 : v3.x(), v3.y() ) / area2 );
677 [ # # ][ # # ]: 0 : coord_C = ( DETERM3( v1.x(), v1.y(),
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
678 : : v2.x(), v2.y(),
679 : 0 : pt.x(), pt.y() ) / area2 );
680 : : }
681 : 0 : return true;
682 : : }
683 : :
684 : 0 : bool CubitVector::attempt_barycentric_coordinates_adjustment
685 : : (
686 : : const CubitVector &v1,
687 : : const CubitVector &v2,
688 : : const CubitVector &v3,
689 : : const CubitVector &point,
690 : : double &coord_A,
691 : : double &coord_B,
692 : : double &coord_C
693 : : )
694 : : {
695 : : #if 0
696 : : CubitVector v1_adjusted = v1-point;
697 : : CubitVector v2_adjusted = v2-point;
698 : : CubitVector v3_adjusted = v3-point;
699 : : CubitVector origin(0,0,0);
700 : : return private_barycentric_coordinates(false,
701 : : v1_adjusted, v2_adjusted, v3_adjusted, origin,
702 : : coord_A, coord_B, coord_C);
703 : : #else
704 [ # # ]: 0 : CubitBox bbox(v1);
705 [ # # ]: 0 : bbox |= v2;
706 [ # # ]: 0 : bbox |= v3;
707 [ # # ][ # # ]: 0 : double dist2 = bbox.diagonal().length();
708 : :
709 [ # # ]: 0 : CubitVector v1_adjusted = v1 / dist2;
710 [ # # ]: 0 : CubitVector v2_adjusted = v2 / dist2;
711 [ # # ]: 0 : CubitVector v3_adjusted = v3 / dist2;
712 [ # # ]: 0 : CubitVector point_adjusted = point / dist2;
713 : : return private_barycentric_coordinates(false,
714 : : v1_adjusted, v2_adjusted, v3_adjusted, point_adjusted,
715 [ # # ][ # # ]: 0 : coord_A, coord_B, coord_C);
716 : : #endif
717 : : }
718 : :
719 : 0 : bool CubitVector::colinear( const CubitVector &p0,
720 : : const CubitVector &p1,
721 : : const CubitVector &p2 )
722 : : {
723 [ # # ]: 0 : CubitVector v1 = p1 - p0;
724 [ # # ]: 0 : CubitVector v2 = p2 - p0;
725 [ # # ]: 0 : v1.normalize();
726 [ # # ]: 0 : v2.normalize();
727 : :
728 : : // If the 3 points are collinear, then the cross product of these two
729 : : // vectors will yield a null vector (one whose length is zero).
730 [ # # ]: 0 : CubitVector norm = v1 * v2;
731 : :
732 [ # # ][ # # ]: 0 : if(norm.length() <= CUBIT_RESABS)
733 : : {
734 : 0 : return true;
735 : : }
736 : 0 : return false;
737 : : }
738 : :
739 : 0 : void CubitVector::project_to_line_segment
740 : : (
741 : : const CubitVector &pt0,
742 : : const CubitVector &pt1
743 : : )
744 : : {
745 [ # # ]: 0 : CubitVector v0 = pt1-pt0;
746 [ # # ]: 0 : CubitVector v1 = *this-pt0;
747 : :
748 [ # # ]: 0 : double len = v0.normalize();
749 [ # # ]: 0 : double dot = v0%v1;
750 [ # # ]: 0 : CubitVector close_pt;
751 [ # # ]: 0 : if ( dot <= 0 )
752 [ # # ]: 0 : close_pt = pt0;
753 [ # # ]: 0 : else if ( dot >= len )
754 [ # # ]: 0 : close_pt = pt1;
755 : : else
756 [ # # ][ # # ]: 0 : close_pt = pt0 + dot *v0;
[ # # ]
757 : :
758 [ # # ]: 0 : set(close_pt);
759 : 0 : }
|
