Branch data Line data Source code
1 : : //- Class: CubitPlane
2 : : //- Description: This file defines the CubitPlane class.
3 : : //- Owner: Greg Sjaardema
4 : : //- Checked by:
5 : :
6 : :
7 : : #include <cstdio>
8 : : #include <cstdlib>
9 : : #include <math.h>
10 : : #include "CubitPlane.hpp"
11 : : #include "CubitMatrix.hpp"
12 : : #include "CubitVector.hpp"
13 : : #include "DLIList.hpp"
14 : : #include "CubitMessage.hpp"
15 : : #include "CubitDefines.h"
16 : : #include "GeometryDefines.h"
17 : :
18 : 0 : CubitPlane::CubitPlane(const double A, const double B,
19 : 0 : const double C, const double D) : normal_(A, B, C)
20 : : {
21 : 0 : double length = normal_.length();
22 : 0 : d_ = D / length;
23 : 0 : normal_ /= length;
24 : 0 : }
25 : :
26 : 0 : CubitPlane::CubitPlane() : normal_(0.0, 0.0, 0.0)
27 : : {
28 : 0 : d_ = 0.0;
29 : 0 : }
30 : :
31 : 0 : CubitPlane::CubitPlane(const CubitVector &Normal, const double D) :
32 : 0 : normal_(Normal)
33 : : {
34 : : // make sure that Normal is not (0,0,0) causing a division by 0
35 [ # # ]: 0 : if(normal_.length()==0)
36 [ # # ][ # # ]: 0 : throw std::invalid_argument ("Normal Length must not be 0");
37 : : //assert(!normal_.length()==0);
38 : :
39 : 0 : double length = normal_.length();
40 : 0 : d_ = D / length;
41 : 0 : normal_ /= length;
42 : 0 : }
43 : :
44 : 1584 : CubitPlane::CubitPlane(const CubitVector &Normal, const CubitVector &point) :
45 : 1584 : normal_(Normal)
46 : : {
47 : : // plane_D = -(plane_normal.x()*X + plane_normal.y()*Y + plane_normal.z()*Z)
48 : 1584 : normal_.normalize();
49 : 1584 : d_ = -1.0 * (normal_.x()*point.x() + normal_.y()*point.y() +
50 : 1584 : normal_.z()*point.z());
51 : 1584 : }
52 : :
53 : 264 : void CubitPlane::set(const CubitVector &Normal, const CubitVector &point)
54 : : {
55 : 264 : normal_ = Normal;
56 : 264 : normal_.normalize();
57 : 264 : d_ = -1.0 * (normal_.x()*point.x() + normal_.y()*point.y() +
58 : 264 : normal_.z()*point.z());
59 : 264 : }
60 : :
61 : 0 : CubitPlane::CubitPlane(DLIList<CubitVector> &positions)
62 : : {
63 : : // Newells method for determining approximate plane equation.
64 : : // Plane equation is of the form:
65 : : // 0 = plane_D +plane_normal.x()*X + plane_normal.y()*Y + plane_normal.z()*Z
66 [ # # ][ # # ]: 0 : if(positions.size() < 3)
67 [ # # ][ # # ]: 0 : throw std::invalid_argument("Must have 3 or more Points");
68 [ # # ]: 0 : CubitVector vector_diff;
69 [ # # ]: 0 : CubitVector vector_sum;
70 [ # # ]: 0 : CubitVector ref_point = CubitVector (0.0, 0.0, 0.0);
71 [ # # ]: 0 : CubitVector tmp_vector;
72 [ # # ][ # # ]: 0 : normal_ = CubitVector(0.0, 0.0, 0.0);
73 [ # # ][ # # ]: 0 : CubitVector vector1, vector2;
74 : :
75 [ # # ][ # # ]: 0 : for (int i=positions.size(); i > 0; i--)
76 : : {
77 [ # # ][ # # ]: 0 : vector1 = positions.get_and_step();
78 [ # # ][ # # ]: 0 : vector2 = positions.get();
79 [ # # ][ # # ]: 0 : vector_diff = (vector2) - (vector1);
80 [ # # ]: 0 : ref_point += (vector1);
81 [ # # ][ # # ]: 0 : vector_sum = (vector1) + (vector2);
82 : :
83 [ # # ][ # # ]: 0 : tmp_vector.set(vector_diff.y() * vector_sum.z(),
84 [ # # ][ # # ]: 0 : vector_diff.z() * vector_sum.x(),
85 [ # # ][ # # ]: 0 : vector_diff.x() * vector_sum.y());
[ # # ]
86 [ # # ]: 0 : normal_ += tmp_vector;
87 : : }
88 [ # # ][ # # ]: 0 : double divisor = positions.size() * normal_.length();
89 : : // if (divisor < .000000001)
90 : : // {
91 : : // PRINT_ERROR("Colinear basis vector in CubitPlane constructor");
92 : : // }
93 [ # # ]: 0 : d_ = (ref_point % normal_) / divisor;
94 [ # # ]: 0 : normal_.normalize();
95 [ # # ]: 0 : normal_ *= -1.0;
96 : 0 : }
97 : :
98 : 0 : bool CubitPlane::fit_points(const std::vector<CubitVector> &positions)
99 : : {
100 : : //- use the method of linear regression
101 : : //
102 : : // by creating an Ax=b system where:
103 : : //
104 : : // ___ ___
105 : : // | x[i]*x[i], x[i]*y[i], x[i] |
106 : : // A = sum_i | x[i]*y[i], y[i]*y[i], y[i] |
107 : : // | x[i], y[i], n |
108 : : // --- ---
109 : : // ___ ___
110 : : // | x[i]*z[i] |
111 : : // b = sum_i | y[i]*z[i] |
112 : : // | z[i]} |
113 : : // --- ---
114 : : // But this assumes that z is a linear function of x and y (i.e. z component
115 : : // of plane normal != 0.). We check the rank of A to catch this case. We
116 : : // handle this case, by transposing the x or y coordinates for the z,
117 : : // computing the normal, then transposing back (see itry below).
118 : :
119 [ # # ]: 0 : for ( int itry=0; itry < 3; itry++ )
120 : : {
121 [ # # ]: 0 : CubitMatrix A(3,3);
122 [ # # ][ # # ]: 0 : std::vector<double> b(3);
[ # # ]
123 [ # # ][ # # ]: 0 : A.set(2,2,positions.size());
124 [ # # ][ # # ]: 0 : for ( size_t ipt=0; ipt<positions.size(); ipt++ )
125 : : {
126 : 0 : double x=0., y=0., z=0.;
127 [ # # ]: 0 : if ( itry == 0 )
128 : : {
129 [ # # ][ # # ]: 0 : x = positions[ipt].x();
130 [ # # ][ # # ]: 0 : y = positions[ipt].y();
131 [ # # ][ # # ]: 0 : z = positions[ipt].z();
132 : : }
133 [ # # ]: 0 : else if ( itry == 1 )
134 : : {
135 [ # # ][ # # ]: 0 : x = positions[ipt].x();
136 [ # # ][ # # ]: 0 : z = positions[ipt].y();
137 [ # # ][ # # ]: 0 : y = positions[ipt].z();
138 : : }
139 [ # # ]: 0 : else if ( itry == 2 )
140 : : {
141 [ # # ][ # # ]: 0 : z = positions[ipt].x();
142 [ # # ][ # # ]: 0 : y = positions[ipt].y();
143 [ # # ][ # # ]: 0 : x = positions[ipt].z();
144 : : }
145 [ # # ]: 0 : double sum_xx = x*x + A.get(0,0);
146 [ # # ]: 0 : double sum_yy = y*y + A.get(1,1);
147 [ # # ]: 0 : double sum_xy = x*y + A.get(1,0);
148 [ # # ]: 0 : double sum_x = x + A.get(2,0);
149 [ # # ]: 0 : double sum_y = y + A.get(2,1);
150 [ # # ]: 0 : A.set(0,0,sum_xx);
151 [ # # ]: 0 : A.set(1,1,sum_yy);
152 [ # # ][ # # ]: 0 : A.set(1,0,sum_xy); A.set(0,1,sum_xy);
153 [ # # ][ # # ]: 0 : A.set(2,0,sum_x); A.set(0,2,sum_x);
154 [ # # ][ # # ]: 0 : A.set(1,2,sum_y); A.set(2,1,sum_y);
155 [ # # ]: 0 : b[0] += x*z;
156 [ # # ]: 0 : b[1] += y*z;
157 [ # # ]: 0 : b[2] += z;
158 : : }
159 : :
160 : : // Make sure we have full rank. If not, it means:
161 : : // if itry==0, plane is parallel to z-axis.
162 : : // if itry==1, plane is parallel to z-axis & y-axis
163 : : // if itry==2, points are colinear
164 [ # # ]: 0 : int rank = A.rank();
165 [ # # ]: 0 : if ( rank == 3 )
166 : : {
167 [ # # ]: 0 : std::vector<double> coef(3);
168 [ # # ]: 0 : A.solveNxN( b, coef );
169 [ # # ]: 0 : if ( itry == 0 )
170 [ # # ][ # # ]: 0 : normal_.set(coef[0], coef[1], -1);
[ # # ]
171 [ # # ]: 0 : else if ( itry == 1 )
172 [ # # ][ # # ]: 0 : normal_.set(coef[0], -1, coef[1]);
[ # # ]
173 [ # # ]: 0 : else if ( itry == 2 )
174 [ # # ][ # # ]: 0 : normal_.set(-1, coef[1], coef[0]);
[ # # ]
175 [ # # ]: 0 : normal_.normalize();
176 : 0 : d_ = 0;
177 [ # # ][ # # ]: 0 : for ( size_t ipt=0; ipt<positions.size(); ipt++ )
178 : : {
179 [ # # ][ # # ]: 0 : d_ += normal_.x()*positions[ipt].x() +
[ # # ]
180 [ # # ][ # # ]: 0 : normal_.y()*positions[ipt].y() +
[ # # ]
181 [ # # ][ # # ]: 0 : normal_.z()*positions[ipt].z();
[ # # ]
182 : : }
183 [ # # ]: 0 : d_ /= positions.size();
184 [ # # ][ # # ]: 0 : return true;
[ # # ]
185 : : }
186 : 0 : }
187 : 0 : d_ = 0.0;
188 : 0 : normal_.set(0.,0.,0.);
189 : 0 : return false;
190 : : }
191 : :
192 : 28204 : CubitPlane::CubitPlane(const CubitPlane& copy_from)
193 : : {
194 : 14102 : normal_ = copy_from.normal_;
195 : 14102 : d_ = copy_from.d_;
196 : 14102 : }
197 : :
198 : 0 : int CubitPlane::mk_plane_with_points( const CubitVector& V0,
199 : : const CubitVector& V1,
200 : : const CubitVector& V2 )
201 : : {
202 : : // First check to make sure that the points are not collinear
203 : :
204 : : // Vector going from Vertex 0 to Vertex 1
205 [ # # ]: 0 : CubitVector vector01 = V1 - V0;
206 [ # # ]: 0 : vector01.normalize();
207 : :
208 : : // Vector going from Vertex 0 to Vertex 2
209 [ # # ]: 0 : CubitVector vector02 = V2 - V0;
210 [ # # ]: 0 : vector02.normalize();
211 : :
212 : : // If the 3 points are collinear, then the cross product of these two
213 : : // vectors will yield a null vector (one whose length is zero).
214 [ # # ][ # # ]: 0 : normal_ = vector01 * vector02;
215 : :
216 [ # # ][ # # ]: 0 : if(normal_.length() <= CUBIT_RESABS)
217 : : {
218 [ # # ][ # # ]: 0 : PRINT_ERROR("Points are collinear.\n"
[ # # ]
219 [ # # ]: 0 : " Cannot create a CubitPlane object.\n");
220 : 0 : d_ = 0.0;
221 : 0 : return CUBIT_FAILURE;
222 : : }
223 : :
224 [ # # ]: 0 : normal_.normalize();
225 : :
226 [ # # ]: 0 : double D0 = -(normal_ % V0);
227 [ # # ]: 0 : double D1 = -(normal_ % V1);
228 [ # # ]: 0 : double D2 = -(normal_ % V2);
229 : 0 : d_ = (D0 + D1 + D2) / 3.0;
230 : :
231 : 0 : return CUBIT_SUCCESS;
232 : : }
233 : :
234 : 0 : CubitVector CubitPlane::point_on_plane() const
235 : : {
236 [ # # ]: 0 : if ( fabs( normal_.x() ) > CUBIT_RESABS )
237 : 0 : return CubitVector(-d_ / normal_.x(), 0, 0);
238 [ # # ]: 0 : else if ( fabs( normal_.y() ) > CUBIT_RESABS )
239 : 0 : return CubitVector(0, -d_ / normal_.y(), 0);
240 [ # # ]: 0 : else if ( fabs( normal_.z() ) > CUBIT_RESABS )
241 : 0 : return CubitVector(0, 0, -d_ / normal_.z());
242 : : // If A B and C are all zero, the plane is invalid,
243 : : // Just return <0,0,0>
244 : 0 : return CubitVector(0,0,0);
245 : : }
246 : :
247 : : //- Plane assignment operator.
248 : 0 : CubitPlane& CubitPlane::operator=(const CubitPlane &plane)
249 : : {
250 [ # # ]: 0 : if (this != &plane)
251 : : {
252 : 0 : normal_ = plane.normal_;
253 : 0 : d_ = plane.d_;
254 : : }
255 : 0 : return *this;
256 : : }
257 : :
258 : 0 : double CubitPlane::distance(const CubitVector &vector) const
259 : : {
260 : 0 : return normal_ % vector + d_;
261 : : }
262 : :
263 : 0 : CubitVector CubitPlane::intersect(const CubitVector &base,
264 : : const CubitVector &direction) const
265 : : {
266 : 0 : double t = -(normal_ % base + d_) / (normal_ % direction);
267 [ # # ]: 0 : return (base + direction * t);
268 : : }
269 : :
270 : 0 : int CubitPlane::intersect(const CubitPlane &plane_2,
271 : : CubitVector &origin, CubitVector &vector) const
272 : : {
273 : : // Code from Graphics Gems III, Georgiades
274 : : // Calculate the line of intersection between two planes.
275 : : // Initialize the unit direction vector of the line of intersection in
276 : : // xdir.
277 : : // Pick the point on the line of intersection on the coordinate plane most
278 : : // normal to xdir.
279 : : // Return TRUE if successful, FALSE otherwise (indicating that the planes
280 : : // don't intersect). The order in which the planes are given determines the
281 : : // choice of direction of xdir.
282 : : //
283 : : // int GetXLine(vect4 *pl1, vect4 *plane_2, vect3 *vector, vect3 *xpt)
284 : : double invdet; // inverse of 2x2 matrix determinant
285 [ # # ][ # # ]: 0 : vector = normal() * plane_2.normal();
[ # # ][ # # ]
286 [ # # ][ # # ]: 0 : CubitVector dir2(vector.x()*vector.x(),
287 [ # # ][ # # ]: 0 : vector.y()*vector.y(),
288 [ # # ][ # # ]: 0 : vector.z()*vector.z());
[ # # ]
289 [ # # ][ # # ]: 0 : CubitVector plane1n = normal();
290 [ # # ][ # # ]: 0 : CubitVector plane2n = plane_2.normal();
291 : :
292 [ # # ][ # # ]: 0 : if (dir2.z() > dir2.y() && dir2.z() > dir2.x() && dir2.z() > CUBIT_RESABS)
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
293 : : {
294 : : // then get a point on the XY plane
295 [ # # ]: 0 : invdet = 1.0 / vector.z();
296 : :
297 : : //solve < pl1.x * origin.x + pl1.y * origin.y = - pl1.w >
298 : : // < plane2n.x * origin.x + plane2n.y * origin.y = - plane2n.w >
299 [ # # ][ # # ]: 0 : origin.set(plane1n.y() * plane_2.coefficient() -
300 [ # # ][ # # ]: 0 : plane2n.y() * coefficient(),
301 [ # # ][ # # ]: 0 : plane2n.x() * coefficient() -
302 [ # # ][ # # ]: 0 : plane1n.x() * plane_2.coefficient(),
303 [ # # ]: 0 : 0.0);
304 : : }
305 [ # # ][ # # ]: 0 : else if (dir2.y() > dir2.x() && dir2.y() > CUBIT_RESABS)
[ # # ][ # # ]
[ # # ][ # # ]
306 : : {
307 : : // then get a point on the XZ plane
308 [ # # ]: 0 : invdet = -1.0 / vector.y();
309 : :
310 : : // solve < pl1.x * origin.x + pl1.z * origin.z = -pl1.w >
311 : : // < plane2n.x * origin.x + plane2n.z * origin.z = -plane2n.w >
312 [ # # ][ # # ]: 0 : origin.set(plane1n.z() * plane_2.coefficient() -
313 [ # # ][ # # ]: 0 : plane2n.z() * coefficient(),
314 : : 0.0,
315 [ # # ][ # # ]: 0 : plane2n.x() * coefficient() -
316 [ # # ][ # # ]: 0 : plane1n.x() * plane_2.coefficient());
[ # # ]
317 : : }
318 [ # # ][ # # ]: 0 : else if (dir2.x() > CUBIT_RESABS)
319 : : {
320 : : // then get a point on the YZ plane
321 [ # # ]: 0 : invdet = 1.0 / vector.x();
322 : :
323 : : // solve < pl1.y * origin.y + pl1.z * origin.z = - pl1.w >
324 : : // < plane2n.y * origin.y + plane2n.z * origin.z = - plane2n.w >
325 : : origin.set(0.0,
326 [ # # ][ # # ]: 0 : plane1n.z() * plane_2.coefficient() -
327 [ # # ][ # # ]: 0 : plane2n.z() * coefficient(),
328 [ # # ][ # # ]: 0 : plane2n.y() * coefficient() -
329 [ # # ][ # # ]: 0 : plane1n.y() * plane_2.coefficient());
[ # # ]
330 : : }
331 : : else // vector is zero, then no point of intersection exists
332 : 0 : return CUBIT_FALSE;
333 : :
334 [ # # ]: 0 : origin *= invdet;
335 [ # # ][ # # ]: 0 : invdet = 1.0 / (float)sqrt(dir2.x() + dir2.y() + dir2.z());
[ # # ]
336 [ # # ]: 0 : vector *= invdet;
337 : 0 : return CUBIT_TRUE;
338 : : }
339 : :
340 : 0 : CubitVector CubitPlane::project( const CubitVector& point ) const
341 : : {
342 [ # # ]: 0 : return point - distance(point) * normal();
343 : : }
|
