Branch data Line data Source code
1 : : /*=========================================================================
2 : :
3 : : Module: $RCSfile: VerdictVector.cpp,v $
4 : :
5 : : Copyright (c) 2006 Sandia Corporation.
6 : : All rights reserved.
7 : : See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
8 : :
9 : : This software is distributed WITHOUT ANY WARRANTY; without even
10 : : the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
11 : : PURPOSE. See the above copyright notice for more information.
12 : :
13 : : =========================================================================*/
14 : :
15 : : /*
16 : : *
17 : : * VerdictVector.cpp contains implementation of Vector operations
18 : : *
19 : : * This file is part of VERDICT
20 : : *
21 : : */
22 : :
23 : : #define VERDICT_EXPORTS
24 : :
25 : : #include "moab/verdict.h"
26 : : #include <math.h>
27 : : #include "VerdictVector.hpp"
28 : : #include <float.h>
29 : :
30 : : #if defined( __BORLANDC__ )
31 : : #pragma warn - 8004 /* "assigned a value that is never used" */
32 : : #endif
33 : :
34 : : const double TWO_VERDICT_PI = 2.0 * VERDICT_PI;
35 : :
36 : 0 : VerdictVector& VerdictVector::length( const double new_length )
37 : : {
38 : 0 : double len = this->length();
39 : 0 : xVal *= new_length / len;
40 : 0 : yVal *= new_length / len;
41 : 0 : zVal *= new_length / len;
42 : 0 : return *this;
43 : : }
44 : :
45 : 0 : double VerdictVector::distance_between( const VerdictVector& test_vector )
46 : : {
47 : 0 : double xv = xVal - test_vector.x();
48 : 0 : double yv = yVal - test_vector.y();
49 : 0 : double zv = zVal - test_vector.z();
50 : :
51 : 0 : return ( sqrt( xv * xv + yv * yv + zv * zv ) );
52 : : }
53 : :
54 : : /*
55 : : void VerdictVector::print_me()
56 : : {
57 : : PRINT_INFO("X: %f\n",xVal);
58 : : PRINT_INFO("Y: %f\n",yVal);
59 : : PRINT_INFO("Z: %f\n",zVal);
60 : :
61 : : }
62 : : */
63 : :
64 : 74 : double VerdictVector::interior_angle( const VerdictVector& otherVector )
65 : : {
66 : 74 : double cosAngle = 0., angleRad = 0., len1, len2 = 0.;
67 : :
68 [ + - ][ + - ]: 74 : if( ( ( len1 = this->length() ) > 0 ) && ( ( len2 = otherVector.length() ) > 0 ) )
[ + - ]
69 : 74 : cosAngle = ( *this % otherVector ) / ( len1 * len2 );
70 : : else
71 : : {
72 [ # # ]: 0 : assert( len1 > 0 );
73 [ # # ]: 0 : assert( len2 > 0 );
74 : : }
75 : :
76 [ - + ][ # # ]: 74 : if( ( cosAngle > 1.0 ) && ( cosAngle < 1.0001 ) )
77 : : {
78 : 0 : cosAngle = 1.0;
79 : 0 : angleRad = acos( cosAngle );
80 : : }
81 [ - + ][ # # ]: 74 : else if( cosAngle < -1.0 && cosAngle > -1.0001 )
82 : : {
83 : 0 : cosAngle = -1.0;
84 : 0 : angleRad = acos( cosAngle );
85 : : }
86 [ + - ][ + - ]: 74 : else if( cosAngle >= -1.0 && cosAngle <= 1.0 )
87 : 74 : angleRad = acos( cosAngle );
88 : : else
89 : : {
90 [ # # ][ # # ]: 0 : assert( cosAngle < 1.0001 && cosAngle > -1.0001 );
91 : : }
92 : :
93 : 74 : return ( ( angleRad * 180. ) / VERDICT_PI );
94 : : }
95 : :
96 : : // Interpolate between two vectors.
97 : : // Returns (1-param)*v1 + param*v2
98 : 0 : VerdictVector interpolate( const double param, const VerdictVector& v1, const VerdictVector& v2 )
99 : : {
100 : 0 : VerdictVector temp = ( 1.0 - param ) * v1;
101 [ # # ]: 0 : temp += param * v2;
102 : 0 : return temp;
103 : : }
104 : :
105 : 0 : void VerdictVector::xy_to_rtheta()
106 : : {
107 : : // careful about overwriting
108 : 0 : double r_ = length();
109 : 0 : double theta_ = atan2( y(), x() );
110 [ # # ]: 0 : if( theta_ < 0.0 ) theta_ += TWO_VERDICT_PI;
111 : :
112 : 0 : r( r_ );
113 : 0 : theta( theta_ );
114 : 0 : }
115 : :
116 : 0 : void VerdictVector::rtheta_to_xy()
117 : : {
118 : : // careful about overwriting
119 : 0 : double x_ = r() * cos( theta() );
120 : 0 : double y_ = r() * sin( theta() );
121 : :
122 : 0 : x( x_ );
123 : 0 : y( y_ );
124 : 0 : }
125 : :
126 : 0 : void VerdictVector::rotate( double angle, double )
127 : : {
128 : 0 : xy_to_rtheta();
129 : 0 : theta() += angle;
130 : 0 : rtheta_to_xy();
131 : 0 : }
132 : :
133 : 0 : void VerdictVector::blow_out( double gamma, double rmin )
134 : : {
135 : : // if gamma == 1, then
136 : : // map on a circle : r'^2 = sqrt( 1 - (1-r)^2 )
137 : : // if gamma ==0, then map back to itself
138 : : // in between, linearly interpolate
139 : 0 : xy_to_rtheta();
140 : : // r() = sqrt( (2. - r()) * r() ) * gamma + r() * (1-gamma);
141 [ # # ]: 0 : assert( gamma > 0.0 );
142 : : // the following limits should really be roundoff-based
143 [ # # ][ # # ]: 0 : if( r() > rmin * 1.001 && r() < 1.001 ) { r() = rmin + pow( r(), gamma ) * ( 1.0 - rmin ); }
[ # # ]
144 : 0 : rtheta_to_xy();
145 : 0 : }
146 : :
147 : 0 : void VerdictVector::reflect_about_xaxis( double, double )
148 : : {
149 : 0 : yVal = -yVal;
150 : 0 : }
151 : :
152 : 0 : void VerdictVector::scale_angle( double gamma, double )
153 : : {
154 : 0 : const double r_factor = 0.3;
155 : 0 : const double theta_factor = 0.6;
156 : :
157 : 0 : xy_to_rtheta();
158 : :
159 : : // if neary 2pi, treat as zero
160 : : // some near zero stuff strays due to roundoff
161 [ # # ]: 0 : if( theta() > TWO_VERDICT_PI - 0.02 ) theta() = 0;
162 : : // the above screws up on big sheets - need to overhaul at the sheet level
163 : :
164 [ # # ]: 0 : if( gamma < 1 )
165 : : {
166 : : // squeeze together points of short radius so that
167 : : // long chords won't cross them
168 : 0 : theta() += ( VERDICT_PI - theta() ) * ( 1 - gamma ) * theta_factor * ( 1 - r() );
169 : :
170 : : // push away from center of circle, again so long chords won't cross
171 : 0 : r( ( r_factor + r() ) / ( 1 + r_factor ) );
172 : :
173 : : // scale angle by gamma
174 : 0 : theta() *= gamma;
175 : : }
176 : : else
177 : : {
178 : : // scale angle by gamma, making sure points nearly 2pi are treated as zero
179 : 0 : double new_theta = theta() * gamma;
180 [ # # ][ # # ]: 0 : if( new_theta < 2.5 * VERDICT_PI || r() < 0.2 ) theta( new_theta );
[ # # ]
181 : : }
182 : 0 : rtheta_to_xy();
183 : 0 : }
184 : :
185 : 0 : double VerdictVector::vector_angle_quick( const VerdictVector& vec1, const VerdictVector& vec2 )
186 : : {
187 : : //- compute the angle between two vectors in the plane defined by this vector
188 : : // build yAxis and xAxis such that xAxis is the projection of
189 : : // vec1 onto the normal plane of this vector
190 : :
191 : : // NOTE: vec1 and vec2 are Vectors from the vertex of the angle along
192 : : // the two sides of the angle.
193 : : // The angle returned is the right-handed angle around this vector
194 : : // from vec1 to vec2.
195 : :
196 : : // NOTE: vector_angle_quick gives exactly the same answer as vector_angle below
197 : : // providing this vector is normalized. It does so with two fewer
198 : : // cross-product evaluations and two fewer vector normalizations.
199 : : // This can be a substantial time savings if the function is called
200 : : // a significant number of times (e.g Hexer) ... (jrh 11/28/94)
201 : : // NOTE: vector_angle() is much more robust. Do not use vector_angle_quick()
202 : : // unless you are very sure of the safety of your input vectors.
203 : :
204 [ # # ]: 0 : VerdictVector ry = ( *this ) * vec1;
205 [ # # ]: 0 : VerdictVector rx = ry * ( *this );
206 : :
207 [ # # ]: 0 : double xv = vec2 % rx;
208 [ # # ]: 0 : double yv = vec2 % ry;
209 : :
210 : : double angle;
211 [ # # ][ # # ]: 0 : assert( xv != 0.0 || yv != 0.0 );
212 : :
213 : 0 : angle = atan2( yv, xv );
214 : :
215 [ # # ]: 0 : if( angle < 0.0 ) { angle += TWO_VERDICT_PI; }
216 : 0 : return angle;
217 : : }
218 : :
219 : 0 : VerdictVector vectorRotate( const double angle, const VerdictVector& normalAxis, const VerdictVector& referenceAxis )
220 : : {
221 : : // A new coordinate system is created with the xy plane corresponding
222 : : // to the plane normal to the normal axis, and the x axis corresponding to
223 : : // the projection of the reference axis onto the normal plane. The normal
224 : : // plane is the tangent plane at the root point. A unit vector is
225 : : // constructed along the local x axis and then rotated by the given
226 : : // ccw angle to form the new point. The new point, then is a unit
227 : : // distance from the global origin in the tangent plane.
228 : :
229 : : double x, y;
230 : :
231 : : // project a unit distance from root along reference axis
232 : :
233 [ # # ]: 0 : VerdictVector yAxis = normalAxis * referenceAxis;
234 [ # # ]: 0 : VerdictVector xAxis = yAxis * normalAxis;
235 [ # # ]: 0 : yAxis.normalize();
236 [ # # ]: 0 : xAxis.normalize();
237 : :
238 : 0 : x = cos( angle );
239 : 0 : y = sin( angle );
240 : :
241 [ # # ]: 0 : xAxis *= x;
242 [ # # ]: 0 : yAxis *= y;
243 [ # # ]: 0 : return VerdictVector( xAxis + yAxis );
244 : : }
245 : :
246 : 0 : double VerdictVector::vector_angle( const VerdictVector& vector1, const VerdictVector& vector2 ) const
247 : : {
248 : : // This routine does not assume that any of the input vectors are of unit
249 : : // length. This routine does not normalize the input vectors.
250 : : // Special cases:
251 : : // If the normal vector is zero length:
252 : : // If a new one can be computed from vectors 1 & 2:
253 : : // the normal is replaced with the vector cross product
254 : : // else the two vectors are colinear and zero or 2PI is returned.
255 : : // If the normal is colinear with either (or both) vectors
256 : : // a new one is computed with the cross products
257 : : // (and checked again).
258 : :
259 : : // Check for zero length normal vector
260 [ # # ]: 0 : VerdictVector normal = *this;
261 [ # # ]: 0 : double normal_lensq = normal.length_squared();
262 : 0 : double len_tol = 0.0000001;
263 [ # # ]: 0 : if( normal_lensq <= len_tol )
264 : : {
265 : : // null normal - make it the normal to the plane defined by vector1
266 : : // and vector2. If still null, the vectors are colinear so check
267 : : // for zero or 180 angle.
268 [ # # ][ # # ]: 0 : normal = vector1 * vector2;
269 [ # # ]: 0 : normal_lensq = normal.length_squared();
270 [ # # ]: 0 : if( normal_lensq <= len_tol )
271 : : {
272 [ # # ]: 0 : double cosine = vector1 % vector2;
273 [ # # ]: 0 : if( cosine > 0.0 )
274 : 0 : return 0.0;
275 : : else
276 : 0 : return VERDICT_PI;
277 : : }
278 : : }
279 : :
280 : : // Trap for normal vector colinear to one of the other vectors. If so,
281 : : // use a normal defined by the two vectors.
282 : 0 : double dot_tol = 0.985;
283 [ # # ]: 0 : double dot = vector1 % normal;
284 [ # # ][ # # ]: 0 : if( dot * dot >= vector1.length_squared() * normal_lensq * dot_tol )
285 : : {
286 [ # # ][ # # ]: 0 : normal = vector1 * vector2;
287 [ # # ]: 0 : normal_lensq = normal.length_squared();
288 : :
289 : : // Still problems if all three vectors were colinear
290 [ # # ]: 0 : if( normal_lensq <= len_tol )
291 : : {
292 [ # # ]: 0 : double cosine = vector1 % vector2;
293 [ # # ]: 0 : if( cosine >= 0.0 )
294 : 0 : return 0.0;
295 : : else
296 : 0 : return VERDICT_PI;
297 : : }
298 : : }
299 : : else
300 : : {
301 : : // The normal and vector1 are not colinear, now check for vector2
302 [ # # ]: 0 : dot = vector2 % normal;
303 [ # # ][ # # ]: 0 : if( dot * dot >= vector2.length_squared() * normal_lensq * dot_tol ) { normal = vector1 * vector2; }
[ # # ][ # # ]
304 : : }
305 : :
306 : : // Assume a plane such that the normal vector is the plane's normal.
307 : : // Create yAxis perpendicular to both the normal and vector1. yAxis is
308 : : // now in the plane. Create xAxis as the perpendicular to both yAxis and
309 : : // the normal. xAxis is in the plane and is the projection of vector1
310 : : // into the plane.
311 : :
312 [ # # ]: 0 : normal.normalize();
313 [ # # ]: 0 : VerdictVector yAxis = normal;
314 [ # # ]: 0 : yAxis *= vector1;
315 [ # # ]: 0 : double yv = vector2 % yAxis;
316 : : // yAxis memory slot will now be used for xAxis
317 [ # # ]: 0 : yAxis *= normal;
318 [ # # ]: 0 : double xv = vector2 % yAxis;
319 : :
320 : : // assert(x != 0.0 || y != 0.0);
321 [ # # ][ # # ]: 0 : if( xv == 0.0 && yv == 0.0 ) { return 0.0; }
322 : 0 : double angle = atan2( yv, xv );
323 : :
324 [ # # ]: 0 : if( angle < 0.0 ) { angle += TWO_VERDICT_PI; }
325 : 0 : return angle;
326 : : }
327 : :
328 : 0 : bool VerdictVector::within_tolerance( const VerdictVector& vectorPtr2, double tolerance ) const
329 : : {
330 [ # # ]: 0 : if( ( fabs( this->x() - vectorPtr2.x() ) < tolerance ) && ( fabs( this->y() - vectorPtr2.y() ) < tolerance ) &&
[ # # # # ]
[ # # ]
331 : 0 : ( fabs( this->z() - vectorPtr2.z() ) < tolerance ) )
332 : 0 : { return true; }
333 : :
334 : 0 : return false;
335 : : }
336 : :
337 : 0 : void VerdictVector::orthogonal_vectors( VerdictVector& vector2, VerdictVector& vector3 )
338 : : {
339 : : double xv[3];
340 : 0 : unsigned short i = 0;
341 : 0 : unsigned short imin = 0;
342 : 0 : double rmin = 1.0E20;
343 : : unsigned short iperm1[3];
344 : : unsigned short iperm2[3];
345 : 0 : unsigned short cont_flag = 1;
346 : : double vec1[3], vec2[3];
347 : : double rmag;
348 : :
349 : : // Copy the input vector and normalize it
350 [ # # ]: 0 : VerdictVector vector1 = *this;
351 [ # # ]: 0 : vector1.normalize();
352 : :
353 : : // Initialize perm flags
354 : 0 : iperm1[0] = 1;
355 : 0 : iperm1[1] = 2;
356 : 0 : iperm1[2] = 0;
357 : 0 : iperm2[0] = 2;
358 : 0 : iperm2[1] = 0;
359 : 0 : iperm2[2] = 1;
360 : :
361 : : // Get into the array format we can work with
362 [ # # ]: 0 : vector1.get_xyz( vec1 );
363 : :
364 [ # # ][ # # ]: 0 : while( i < 3 && cont_flag )
365 : : {
366 [ # # ]: 0 : if( fabs( vec1[i] ) < 1e-6 )
367 : : {
368 : 0 : vec2[i] = 1.0;
369 : 0 : vec2[iperm1[i]] = 0.0;
370 : 0 : vec2[iperm2[i]] = 0.0;
371 : 0 : cont_flag = 0;
372 : : }
373 : :
374 [ # # ]: 0 : if( fabs( vec1[i] ) < rmin )
375 : : {
376 : 0 : imin = i;
377 : 0 : rmin = fabs( vec1[i] );
378 : : }
379 : 0 : ++i;
380 : : }
381 : :
382 [ # # ]: 0 : if( cont_flag )
383 : : {
384 : 0 : xv[imin] = 1.0;
385 : 0 : xv[iperm1[imin]] = 0.0;
386 : 0 : xv[iperm2[imin]] = 0.0;
387 : :
388 : : // Determine cross product
389 : 0 : vec2[0] = vec1[1] * xv[2] - vec1[2] * xv[1];
390 : 0 : vec2[1] = vec1[2] * xv[0] - vec1[0] * xv[2];
391 : 0 : vec2[2] = vec1[0] * xv[1] - vec1[1] * xv[0];
392 : :
393 : : // Unitize
394 : 0 : rmag = sqrt( vec2[0] * vec2[0] + vec2[1] * vec2[1] + vec2[2] * vec2[2] );
395 : 0 : vec2[0] /= rmag;
396 : 0 : vec2[1] /= rmag;
397 : 0 : vec2[2] /= rmag;
398 : : }
399 : :
400 : : // Copy 1st orthogonal vector into VerdictVector vector2
401 [ # # ]: 0 : vector2.set( vec2 );
402 : :
403 : : // Cross vectors to determine last orthogonal vector
404 [ # # ][ # # ]: 0 : vector3 = vector1 * vector2;
405 : 0 : }
406 : :
407 : : //- Find next point from this point using a direction and distance
408 : 0 : void VerdictVector::next_point( const VerdictVector& direction, double distance, VerdictVector& out_point )
409 : : {
410 [ # # ]: 0 : VerdictVector my_direction = direction;
411 [ # # ]: 0 : my_direction.normalize();
412 : :
413 : : // Determine next point in space
414 [ # # ][ # # ]: 0 : out_point.x( xVal + ( distance * my_direction.x() ) );
415 [ # # ][ # # ]: 0 : out_point.y( yVal + ( distance * my_direction.y() ) );
416 [ # # ][ # # ]: 0 : out_point.z( zVal + ( distance * my_direction.z() ) );
417 : :
418 : 0 : return;
419 : : }
420 : :
421 : 0 : VerdictVector::VerdictVector( const double xyz[3] ) : xVal( xyz[0] ), yVal( xyz[1] ), zVal( xyz[2] ) {}
|
