Branch data Line data Source code
1 : : /* *****************************************************************
2 : : MESQUITE -- The Mesh Quality Improvement Toolkit
3 : :
4 : : Copyright 2007 Sandia National Laboratories. Developed at the
5 : : University of Wisconsin--Madison under SNL contract number
6 : : 624796. The U.S. Government and the University of Wisconsin
7 : : retain certain rights to this software.
8 : :
9 : : This library is free software; you can redistribute it and/or
10 : : modify it under the terms of the GNU Lesser General Public
11 : : License as published by the Free Software Foundation; either
12 : : version 2.1 of the License, or (at your option) any later version.
13 : :
14 : : This library is distributed in the hope that it will be useful,
15 : : but WITHOUT ANY WARRANTY; without even the implied warranty of
16 : : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 : : Lesser General Public License for more details.
18 : :
19 : : You should have received a copy of the GNU Lesser General Public License
20 : : (lgpl.txt) along with this library; if not, write to the Free Software
21 : : Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
22 : :
23 : : (2007) [email protected]
24 : :
25 : : ***************************************************************** */
26 : :
27 : : /** \file MsqGeomPrim.cpp
28 : : * \brief
29 : : * \author Jason Kraftcheck
30 : : */
31 : :
32 : : #include "Mesquite.hpp"
33 : : #include "MsqGeomPrim.hpp"
34 : :
35 : : namespace MBMesquite
36 : : {
37 : :
38 : 0 : bool MsqLine::intersect( const MsqLine& other, double& param, double epsilon ) const
39 : : {
40 [ # # ][ # # ]: 0 : if( !closest( other, param ) ) return false;
41 [ # # ]: 0 : Vector3D p1 = point( param );
42 [ # # ][ # # ]: 0 : Vector3D p2 = other.point( other.closest( p1 ) );
43 [ # # ][ # # ]: 0 : return ( p1 - p2 ).length_squared() < epsilon * epsilon;
44 : : }
45 : :
46 : 0 : bool MsqLine::closest( const MsqLine& other, double& param ) const
47 : : {
48 [ # # ][ # # ]: 0 : const Vector3D N = other.direction() * ( direction() * other.direction() );
[ # # ][ # # ]
[ # # ]
49 [ # # ][ # # ]: 0 : const double D = -( N % other.point() );
50 [ # # ][ # # ]: 0 : const double dot = N % direction();
51 [ # # ]: 0 : if( dot < DBL_EPSILON ) return false; // parallel
52 [ # # ][ # # ]: 0 : param = -( N % point() + D ) / dot;
53 : 0 : return true;
54 : : }
55 : :
56 : 0 : bool MsqCircle::three_point( const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, MsqCircle& result )
57 : : {
58 [ # # ][ # # ]: 0 : Vector3D norm = ( p1 - p2 ) * ( p3 - p2 );
[ # # ]
59 [ # # ][ # # ]: 0 : if( norm.length_squared() < DBL_EPSILON ) return false;
60 : :
61 [ # # ][ # # ]: 0 : MsqLine line1( 0.5 * ( p1 + p2 ), norm * ( p1 - p2 ) );
[ # # ][ # # ]
[ # # ]
62 [ # # ][ # # ]: 0 : MsqLine line2( 0.5 * ( p2 + p3 ), norm * ( p3 - p2 ) );
[ # # ][ # # ]
[ # # ]
63 : : double t_xsect;
64 [ # # ][ # # ]: 0 : if( !line1.closest( line2, t_xsect ) ) return false;
65 : :
66 [ # # ]: 0 : Vector3D center = line1.point( t_xsect );
67 [ # # ][ # # ]: 0 : double radius = ( ( center - p1 ).length() + ( center - p2 ).length() + ( center - p3 ).length() ) / 3.0;
[ # # ][ # # ]
[ # # ][ # # ]
68 [ # # ][ # # ]: 0 : result = MsqCircle( center, norm, radius );
69 : 0 : return true;
70 : : }
71 : :
72 : 0 : bool MsqCircle::two_point( const Vector3D& center, const Vector3D& p1, const Vector3D& p2, MsqCircle& result )
73 : : {
74 [ # # ][ # # ]: 0 : Vector3D norm = ( p1 - center ) * ( p2 - center );
[ # # ]
75 [ # # ][ # # ]: 0 : if( norm.length_squared() < DBL_EPSILON ) return false;
76 : :
77 [ # # ][ # # ]: 0 : double radius = 0.5 * ( ( center - p1 ).length() + ( center - p2 ).length() );
[ # # ][ # # ]
78 [ # # ][ # # ]: 0 : result = MsqCircle( center, norm, radius );
79 : 0 : return true;
80 : : }
81 : :
82 : 0 : Vector3D MsqCircle::radial_vector() const
83 : : {
84 : 0 : int min_idx = 0;
85 [ # # ]: 0 : if( normal()[1] < normal()[min_idx] ) min_idx = 1;
86 [ # # ]: 0 : if( normal()[2] < normal()[min_idx] ) min_idx = 2;
87 : 0 : Vector3D vect( 0, 0, 0 );
88 : 0 : vect[min_idx] = 1;
89 [ # # ]: 0 : vect = normal() * vect;
90 : 0 : vect *= radius() / vect.length();
91 : 0 : return vect;
92 : : }
93 : :
94 : 0 : Vector3D MsqCircle::closest( const Vector3D& point ) const
95 : : {
96 [ # # ][ # # ]: 0 : const Vector3D from_center = point - center();
97 [ # # ][ # # ]: 0 : const Vector3D norm_proj = normal() * ( normal() % from_center ); // unit normal!
[ # # ][ # # ]
98 [ # # ]: 0 : const Vector3D in_plane = from_center - norm_proj;
99 [ # # ]: 0 : const double length = in_plane.length();
100 [ # # ]: 0 : if( length < DBL_EPSILON )
101 [ # # ][ # # ]: 0 : return center() + radial_vector();
[ # # ]
102 : : else
103 [ # # ][ # # ]: 0 : return center() + in_plane * radius() / length;
[ # # ][ # # ]
[ # # ]
104 : : }
105 : :
106 : 0 : bool MsqCircle::closest( const Vector3D& point, Vector3D& result_pt, Vector3D& result_tngt ) const
107 : : {
108 [ # # ][ # # ]: 0 : const Vector3D from_center = point - center();
109 [ # # ][ # # ]: 0 : Vector3D in_plane = from_center - ( from_center % normal() );
[ # # ][ # # ]
110 [ # # ][ # # ]: 0 : if( in_plane.length_squared() < DBL_EPSILON ) return false;
111 : :
112 [ # # ][ # # ]: 0 : result_pt = center() + in_plane * radius() / in_plane.length();
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
113 [ # # ][ # # ]: 0 : result_tngt = in_plane * normal();
[ # # ]
114 : 0 : return true;
115 : : }
116 : :
117 : 0 : MsqPlane::MsqPlane( const Vector3D& p_normal, double coeff )
118 : : {
119 : 0 : const double len = p_normal.length();
120 [ # # ]: 0 : mNormal = p_normal / len;
121 : 0 : mCoeff = coeff / len;
122 : 0 : }
123 : :
124 : 0 : MsqPlane::MsqPlane( const Vector3D& p_normal, const Vector3D& p_point )
125 : 0 : : mNormal( p_normal / p_normal.length() ), mCoeff( -( mNormal % p_point ) )
126 : : {
127 : 0 : }
128 : :
129 : 0 : MsqPlane::MsqPlane( double a, double b, double c, double d ) : mNormal( a, b, c ), mCoeff( d )
130 : : {
131 : 0 : const double len = mNormal.length();
132 : 0 : mNormal /= len;
133 : 0 : mCoeff /= len;
134 : 0 : }
135 : :
136 : 0 : bool MsqPlane::intersect( const MsqPlane& plane, MsqLine& result ) const
137 : : {
138 : 0 : const double dot = normal() % plane.normal();
139 : 0 : const double det = dot * dot - 1.0;
140 [ # # ]: 0 : if( fabs( det ) < DBL_EPSILON ) // parallel
141 : 0 : return false;
142 : :
143 : 0 : const double s1 = ( coefficient() - dot * plane.coefficient() ) / det;
144 : 0 : const double s2 = ( plane.coefficient() - dot * coefficient() ) / det;
145 [ # # ][ # # ]: 0 : result = MsqLine( s1 * normal() + s2 * plane.normal(), normal() * plane.normal() );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
146 : 0 : return true;
147 : : }
148 : :
149 : 0 : bool MsqPlane::intersect( const MsqLine& line, double& result ) const
150 : : {
151 : 0 : const double dot = line.direction() % normal();
152 [ # # ]: 0 : if( fabs( dot ) < DBL_EPSILON ) return false;
153 : :
154 : 0 : result = -( normal() % line.point() + coefficient() ) / dot;
155 : 0 : return true;
156 : : }
157 : :
158 : 0 : Vector3D MsqSphere::closest( const Vector3D& point ) const
159 : : {
160 [ # # ][ # # ]: 0 : Vector3D diff = point - center();
161 [ # # ]: 0 : double len = diff.length();
162 [ # # ]: 0 : if( len < DBL_EPSILON )
163 : : {
164 : : // pick any point
165 [ # # ][ # # ]: 0 : diff = Vector3D( 1, 0, 0 );
166 : 0 : len = 1;
167 : : }
168 : :
169 [ # # ][ # # ]: 0 : return center() + diff * radius() / len;
[ # # ][ # # ]
[ # # ]
170 : : }
171 : :
172 : 0 : bool MsqSphere::closest( const Vector3D& point, Vector3D& point_on_sphere, Vector3D& normal_at_point ) const
173 : : {
174 [ # # ]: 0 : normal_at_point = point - center();
175 : 0 : double len = normal_at_point.length();
176 [ # # ]: 0 : if( len < DBL_EPSILON ) return false;
177 : :
178 : 0 : normal_at_point /= len;
179 [ # # ][ # # ]: 0 : point_on_sphere = center() + radius() * normal_at_point;
[ # # ]
180 : 0 : return true;
181 : : }
182 : :
183 : 0 : bool MsqSphere::intersect( const MsqPlane& plane, MsqCircle& result ) const
184 : : {
185 [ # # ][ # # ]: 0 : const Vector3D plane_pt = plane.closest( center() );
186 [ # # ][ # # ]: 0 : const Vector3D plane_dir = plane_pt - center();
187 [ # # ]: 0 : const double dir_sqr = plane_dir.length_squared();
188 [ # # ]: 0 : if( dir_sqr < DBL_EPSILON )
189 : : { // plane passes through center of sphere
190 [ # # ][ # # ]: 0 : result = MsqCircle( center(), plane.normal(), radius() );
[ # # ][ # # ]
[ # # ]
191 : 0 : return true;
192 : : }
193 : :
194 [ # # ][ # # ]: 0 : double rad_sqr = radius() * radius() - plane_dir.length_squared();
[ # # ]
195 [ # # ]: 0 : if( rad_sqr < 0 ) // no intersection
196 : 0 : return false;
197 : :
198 [ # # ][ # # ]: 0 : result = MsqCircle( plane_pt, plane_dir, sqrt( rad_sqr ) );
199 : 0 : return true;
200 : : }
201 : :
202 : 0 : bool MsqSphere::intersect( const MsqSphere& sphere, MsqCircle& result ) const
203 : : {
204 [ # # ][ # # ]: 0 : const Vector3D d = sphere.center() - center();
[ # # ]
205 [ # # ]: 0 : const double dist = d.length();
206 [ # # ][ # # ]: 0 : if( dist > ( radius() + sphere.radius() ) ) return false;
[ # # ]
207 : :
208 [ # # ][ # # ]: 0 : const double r1_sqr = radius() * radius();
209 [ # # ][ # # ]: 0 : const double r2_sqr = sphere.radius() * sphere.radius();
210 [ # # ]: 0 : const double f = ( d % d + r1_sqr - r2_sqr ) / 2;
211 : : // const double d1 = f / dist;
212 : :
213 [ # # ]: 0 : const double rad = sqrt( r1_sqr - f * f / ( d % d ) );
214 [ # # ][ # # ]: 0 : Vector3D c = center() + d * f / ( d % d );
[ # # ][ # # ]
[ # # ]
215 [ # # ][ # # ]: 0 : result = MsqCircle( c, d, rad );
216 : 0 : return true;
217 : : }
218 : :
219 : : } // namespace MBMesquite
|
