MOAB: Mesh Oriented datABase
(version 5.2.1)
|
00001 /*========================================================================= 00002 00003 Module: $RCSfile: VerdictVector.cpp,v $ 00004 00005 Copyright (c) 2006 Sandia Corporation. 00006 All rights reserved. 00007 See Copyright.txt or http://www.kitware.com/Copyright.htm for details. 00008 00009 This software is distributed WITHOUT ANY WARRANTY; without even 00010 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 00011 PURPOSE. See the above copyright notice for more information. 00012 00013 =========================================================================*/ 00014 00015 /* 00016 * 00017 * VerdictVector.cpp contains implementation of Vector operations 00018 * 00019 * This file is part of VERDICT 00020 * 00021 */ 00022 00023 #define VERDICT_EXPORTS 00024 00025 #include "moab/verdict.h" 00026 #include <math.h> 00027 #include "VerdictVector.hpp" 00028 #include <float.h> 00029 00030 #if defined( __BORLANDC__ ) 00031 #pragma warn - 8004 /* "assigned a value that is never used" */ 00032 #endif 00033 00034 const double TWO_VERDICT_PI = 2.0 * VERDICT_PI; 00035 00036 VerdictVector& VerdictVector::length( const double new_length ) 00037 { 00038 double len = this->length(); 00039 xVal *= new_length / len; 00040 yVal *= new_length / len; 00041 zVal *= new_length / len; 00042 return *this; 00043 } 00044 00045 double VerdictVector::distance_between( const VerdictVector& test_vector ) 00046 { 00047 double xv = xVal - test_vector.x(); 00048 double yv = yVal - test_vector.y(); 00049 double zv = zVal - test_vector.z(); 00050 00051 return ( sqrt( xv * xv + yv * yv + zv * zv ) ); 00052 } 00053 00054 /* 00055 void VerdictVector::print_me() 00056 { 00057 PRINT_INFO("X: %f\n",xVal); 00058 PRINT_INFO("Y: %f\n",yVal); 00059 PRINT_INFO("Z: %f\n",zVal); 00060 00061 } 00062 */ 00063 00064 double VerdictVector::interior_angle( const VerdictVector& otherVector ) 00065 { 00066 double cosAngle = 0., angleRad = 0., len1, len2 = 0.; 00067 00068 if( ( ( len1 = this->length() ) > 0 ) && ( ( len2 = otherVector.length() ) > 0 ) ) 00069 cosAngle = ( *this % otherVector ) / ( len1 * len2 ); 00070 else 00071 { 00072 assert( len1 > 0 ); 00073 assert( len2 > 0 ); 00074 } 00075 00076 if( ( cosAngle > 1.0 ) && ( cosAngle < 1.0001 ) ) 00077 { 00078 cosAngle = 1.0; 00079 angleRad = acos( cosAngle ); 00080 } 00081 else if( cosAngle < -1.0 && cosAngle > -1.0001 ) 00082 { 00083 cosAngle = -1.0; 00084 angleRad = acos( cosAngle ); 00085 } 00086 else if( cosAngle >= -1.0 && cosAngle <= 1.0 ) 00087 angleRad = acos( cosAngle ); 00088 else 00089 { 00090 assert( cosAngle < 1.0001 && cosAngle > -1.0001 ); 00091 } 00092 00093 return ( ( angleRad * 180. ) / VERDICT_PI ); 00094 } 00095 00096 // Interpolate between two vectors. 00097 // Returns (1-param)*v1 + param*v2 00098 VerdictVector interpolate( const double param, const VerdictVector& v1, const VerdictVector& v2 ) 00099 { 00100 VerdictVector temp = ( 1.0 - param ) * v1; 00101 temp += param * v2; 00102 return temp; 00103 } 00104 00105 void VerdictVector::xy_to_rtheta() 00106 { 00107 // careful about overwriting 00108 double r_ = length(); 00109 double theta_ = atan2( y(), x() ); 00110 if( theta_ < 0.0 ) theta_ += TWO_VERDICT_PI; 00111 00112 r( r_ ); 00113 theta( theta_ ); 00114 } 00115 00116 void VerdictVector::rtheta_to_xy() 00117 { 00118 // careful about overwriting 00119 double x_ = r() * cos( theta() ); 00120 double y_ = r() * sin( theta() ); 00121 00122 x( x_ ); 00123 y( y_ ); 00124 } 00125 00126 void VerdictVector::rotate( double angle, double ) 00127 { 00128 xy_to_rtheta(); 00129 theta() += angle; 00130 rtheta_to_xy(); 00131 } 00132 00133 void VerdictVector::blow_out( double gamma, double rmin ) 00134 { 00135 // if gamma == 1, then 00136 // map on a circle : r'^2 = sqrt( 1 - (1-r)^2 ) 00137 // if gamma ==0, then map back to itself 00138 // in between, linearly interpolate 00139 xy_to_rtheta(); 00140 // r() = sqrt( (2. - r()) * r() ) * gamma + r() * (1-gamma); 00141 assert( gamma > 0.0 ); 00142 // the following limits should really be roundoff-based 00143 if( r() > rmin * 1.001 && r() < 1.001 ) { r() = rmin + pow( r(), gamma ) * ( 1.0 - rmin ); } 00144 rtheta_to_xy(); 00145 } 00146 00147 void VerdictVector::reflect_about_xaxis( double, double ) 00148 { 00149 yVal = -yVal; 00150 } 00151 00152 void VerdictVector::scale_angle( double gamma, double ) 00153 { 00154 const double r_factor = 0.3; 00155 const double theta_factor = 0.6; 00156 00157 xy_to_rtheta(); 00158 00159 // if neary 2pi, treat as zero 00160 // some near zero stuff strays due to roundoff 00161 if( theta() > TWO_VERDICT_PI - 0.02 ) theta() = 0; 00162 // the above screws up on big sheets - need to overhaul at the sheet level 00163 00164 if( gamma < 1 ) 00165 { 00166 // squeeze together points of short radius so that 00167 // long chords won't cross them 00168 theta() += ( VERDICT_PI - theta() ) * ( 1 - gamma ) * theta_factor * ( 1 - r() ); 00169 00170 // push away from center of circle, again so long chords won't cross 00171 r( ( r_factor + r() ) / ( 1 + r_factor ) ); 00172 00173 // scale angle by gamma 00174 theta() *= gamma; 00175 } 00176 else 00177 { 00178 // scale angle by gamma, making sure points nearly 2pi are treated as zero 00179 double new_theta = theta() * gamma; 00180 if( new_theta < 2.5 * VERDICT_PI || r() < 0.2 ) theta( new_theta ); 00181 } 00182 rtheta_to_xy(); 00183 } 00184 00185 double VerdictVector::vector_angle_quick( const VerdictVector& vec1, const VerdictVector& vec2 ) 00186 { 00187 //- compute the angle between two vectors in the plane defined by this vector 00188 // build yAxis and xAxis such that xAxis is the projection of 00189 // vec1 onto the normal plane of this vector 00190 00191 // NOTE: vec1 and vec2 are Vectors from the vertex of the angle along 00192 // the two sides of the angle. 00193 // The angle returned is the right-handed angle around this vector 00194 // from vec1 to vec2. 00195 00196 // NOTE: vector_angle_quick gives exactly the same answer as vector_angle below 00197 // providing this vector is normalized. It does so with two fewer 00198 // cross-product evaluations and two fewer vector normalizations. 00199 // This can be a substantial time savings if the function is called 00200 // a significant number of times (e.g Hexer) ... (jrh 11/28/94) 00201 // NOTE: vector_angle() is much more robust. Do not use vector_angle_quick() 00202 // unless you are very sure of the safety of your input vectors. 00203 00204 VerdictVector ry = ( *this ) * vec1; 00205 VerdictVector rx = ry * ( *this ); 00206 00207 double xv = vec2 % rx; 00208 double yv = vec2 % ry; 00209 00210 double angle; 00211 assert( xv != 0.0 || yv != 0.0 ); 00212 00213 angle = atan2( yv, xv ); 00214 00215 if( angle < 0.0 ) { angle += TWO_VERDICT_PI; } 00216 return angle; 00217 } 00218 00219 VerdictVector vectorRotate( const double angle, const VerdictVector& normalAxis, const VerdictVector& referenceAxis ) 00220 { 00221 // A new coordinate system is created with the xy plane corresponding 00222 // to the plane normal to the normal axis, and the x axis corresponding to 00223 // the projection of the reference axis onto the normal plane. The normal 00224 // plane is the tangent plane at the root point. A unit vector is 00225 // constructed along the local x axis and then rotated by the given 00226 // ccw angle to form the new point. The new point, then is a unit 00227 // distance from the global origin in the tangent plane. 00228 00229 double x, y; 00230 00231 // project a unit distance from root along reference axis 00232 00233 VerdictVector yAxis = normalAxis * referenceAxis; 00234 VerdictVector xAxis = yAxis * normalAxis; 00235 yAxis.normalize(); 00236 xAxis.normalize(); 00237 00238 x = cos( angle ); 00239 y = sin( angle ); 00240 00241 xAxis *= x; 00242 yAxis *= y; 00243 return VerdictVector( xAxis + yAxis ); 00244 } 00245 00246 double VerdictVector::vector_angle( const VerdictVector& vector1, const VerdictVector& vector2 ) const 00247 { 00248 // This routine does not assume that any of the input vectors are of unit 00249 // length. This routine does not normalize the input vectors. 00250 // Special cases: 00251 // If the normal vector is zero length: 00252 // If a new one can be computed from vectors 1 & 2: 00253 // the normal is replaced with the vector cross product 00254 // else the two vectors are colinear and zero or 2PI is returned. 00255 // If the normal is colinear with either (or both) vectors 00256 // a new one is computed with the cross products 00257 // (and checked again). 00258 00259 // Check for zero length normal vector 00260 VerdictVector normal = *this; 00261 double normal_lensq = normal.length_squared(); 00262 double len_tol = 0.0000001; 00263 if( normal_lensq <= len_tol ) 00264 { 00265 // null normal - make it the normal to the plane defined by vector1 00266 // and vector2. If still null, the vectors are colinear so check 00267 // for zero or 180 angle. 00268 normal = vector1 * vector2; 00269 normal_lensq = normal.length_squared(); 00270 if( normal_lensq <= len_tol ) 00271 { 00272 double cosine = vector1 % vector2; 00273 if( cosine > 0.0 ) 00274 return 0.0; 00275 else 00276 return VERDICT_PI; 00277 } 00278 } 00279 00280 // Trap for normal vector colinear to one of the other vectors. If so, 00281 // use a normal defined by the two vectors. 00282 double dot_tol = 0.985; 00283 double dot = vector1 % normal; 00284 if( dot * dot >= vector1.length_squared() * normal_lensq * dot_tol ) 00285 { 00286 normal = vector1 * vector2; 00287 normal_lensq = normal.length_squared(); 00288 00289 // Still problems if all three vectors were colinear 00290 if( normal_lensq <= len_tol ) 00291 { 00292 double cosine = vector1 % vector2; 00293 if( cosine >= 0.0 ) 00294 return 0.0; 00295 else 00296 return VERDICT_PI; 00297 } 00298 } 00299 else 00300 { 00301 // The normal and vector1 are not colinear, now check for vector2 00302 dot = vector2 % normal; 00303 if( dot * dot >= vector2.length_squared() * normal_lensq * dot_tol ) { normal = vector1 * vector2; } 00304 } 00305 00306 // Assume a plane such that the normal vector is the plane's normal. 00307 // Create yAxis perpendicular to both the normal and vector1. yAxis is 00308 // now in the plane. Create xAxis as the perpendicular to both yAxis and 00309 // the normal. xAxis is in the plane and is the projection of vector1 00310 // into the plane. 00311 00312 normal.normalize(); 00313 VerdictVector yAxis = normal; 00314 yAxis *= vector1; 00315 double yv = vector2 % yAxis; 00316 // yAxis memory slot will now be used for xAxis 00317 yAxis *= normal; 00318 double xv = vector2 % yAxis; 00319 00320 // assert(x != 0.0 || y != 0.0); 00321 if( xv == 0.0 && yv == 0.0 ) { return 0.0; } 00322 double angle = atan2( yv, xv ); 00323 00324 if( angle < 0.0 ) { angle += TWO_VERDICT_PI; } 00325 return angle; 00326 } 00327 00328 bool VerdictVector::within_tolerance( const VerdictVector& vectorPtr2, double tolerance ) const 00329 { 00330 if( ( fabs( this->x() - vectorPtr2.x() ) < tolerance ) && ( fabs( this->y() - vectorPtr2.y() ) < tolerance ) && 00331 ( fabs( this->z() - vectorPtr2.z() ) < tolerance ) ) 00332 { return true; } 00333 00334 return false; 00335 } 00336 00337 void VerdictVector::orthogonal_vectors( VerdictVector& vector2, VerdictVector& vector3 ) 00338 { 00339 double xv[3]; 00340 unsigned short i = 0; 00341 unsigned short imin = 0; 00342 double rmin = 1.0E20; 00343 unsigned short iperm1[3]; 00344 unsigned short iperm2[3]; 00345 unsigned short cont_flag = 1; 00346 double vec1[3], vec2[3]; 00347 double rmag; 00348 00349 // Copy the input vector and normalize it 00350 VerdictVector vector1 = *this; 00351 vector1.normalize(); 00352 00353 // Initialize perm flags 00354 iperm1[0] = 1; 00355 iperm1[1] = 2; 00356 iperm1[2] = 0; 00357 iperm2[0] = 2; 00358 iperm2[1] = 0; 00359 iperm2[2] = 1; 00360 00361 // Get into the array format we can work with 00362 vector1.get_xyz( vec1 ); 00363 00364 while( i < 3 && cont_flag ) 00365 { 00366 if( fabs( vec1[i] ) < 1e-6 ) 00367 { 00368 vec2[i] = 1.0; 00369 vec2[iperm1[i]] = 0.0; 00370 vec2[iperm2[i]] = 0.0; 00371 cont_flag = 0; 00372 } 00373 00374 if( fabs( vec1[i] ) < rmin ) 00375 { 00376 imin = i; 00377 rmin = fabs( vec1[i] ); 00378 } 00379 ++i; 00380 } 00381 00382 if( cont_flag ) 00383 { 00384 xv[imin] = 1.0; 00385 xv[iperm1[imin]] = 0.0; 00386 xv[iperm2[imin]] = 0.0; 00387 00388 // Determine cross product 00389 vec2[0] = vec1[1] * xv[2] - vec1[2] * xv[1]; 00390 vec2[1] = vec1[2] * xv[0] - vec1[0] * xv[2]; 00391 vec2[2] = vec1[0] * xv[1] - vec1[1] * xv[0]; 00392 00393 // Unitize 00394 rmag = sqrt( vec2[0] * vec2[0] + vec2[1] * vec2[1] + vec2[2] * vec2[2] ); 00395 vec2[0] /= rmag; 00396 vec2[1] /= rmag; 00397 vec2[2] /= rmag; 00398 } 00399 00400 // Copy 1st orthogonal vector into VerdictVector vector2 00401 vector2.set( vec2 ); 00402 00403 // Cross vectors to determine last orthogonal vector 00404 vector3 = vector1 * vector2; 00405 } 00406 00407 //- Find next point from this point using a direction and distance 00408 void VerdictVector::next_point( const VerdictVector& direction, double distance, VerdictVector& out_point ) 00409 { 00410 VerdictVector my_direction = direction; 00411 my_direction.normalize(); 00412 00413 // Determine next point in space 00414 out_point.x( xVal + ( distance * my_direction.x() ) ); 00415 out_point.y( yVal + ( distance * my_direction.y() ) ); 00416 out_point.z( zVal + ( distance * my_direction.z() ) ); 00417 00418 return; 00419 } 00420 00421 VerdictVector::VerdictVector( const double xyz[3] ) : xVal( xyz[0] ), yVal( xyz[1] ), zVal( xyz[2] ) {}