Tools:
[blender.git] / intern / boolop / intern / BOP_MathUtils.cpp
1 /**
2  *
3  * $Id$
4  *
5  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
6  *
7  * This program is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU General Public License
9  * as published by the Free Software Foundation; either version 2
10  * of the License, or (at your option) any later version. The Blender
11  * Foundation also sells licenses for use in proprietary software under
12  * the Blender License.  See http://www.blender.org/BL/ for information
13  * about this.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with this program; if not, write to the Free Software Foundation,
22  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
23  *
24  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
25  * All rights reserved.
26  *
27  * The Original Code is: all of this file.
28  *
29  * Contributor(s): Marc Freixas, Ken Hughes
30  *
31  * ***** END GPL/BL DUAL LICENSE BLOCK *****
32  */
33  
34 #include "BOP_MathUtils.h"
35 #include <iostream>
36 using namespace std;
37
38 /**
39  * Compares two scalars with EPSILON accuracy.
40  * @param A scalar
41  * @param B scalar
42  * @return 1 if A > B, -1 if A < B, 0 otherwise
43  */
44 int BOP_comp(const MT_Scalar A, const MT_Scalar B)
45 {
46         if (A >= B + BOP_EPSILON) return 1;
47         else if (B >= A + BOP_EPSILON) return -1;
48         else return 0; 
49 }
50
51 /**
52  * Compares two scalar triplets with EPSILON accuracy.
53  * @param A scalar triplet
54  * @param B scalar triplet
55  * @return 1 if A > B, -1 if A < B, 0 otherwise
56  */
57 int BOP_comp(const MT_Tuple3& A, const MT_Tuple3& B)
58 {
59         if (A.x() >= (B.x() + BOP_EPSILON)) return 1;
60         else if (B.x() >= (A.x() + BOP_EPSILON)) return -1;
61         else if (A.y() >= (B.y() + BOP_EPSILON)) return 1;
62         else if (B.y() >= (A.y() + BOP_EPSILON)) return -1;
63         else if (A.z() >= (B.z() + BOP_EPSILON)) return 1;
64         else if (B.z() >= (A.z() + BOP_EPSILON)) return -1;
65         else return 0;
66 }
67
68 /**
69  * Compares two scalars strictly.
70  * @param A scalar
71  * @param B scalar
72  * @return 1 if A > B, -1 if A < B, 0 otherwise
73  */
74 int BOP_exactComp(const MT_Scalar A, const MT_Scalar B)
75 {
76         if (A > B) return 1;
77         else if (B > A) return -1;
78         else return 0; 
79 }
80 /**
81  * Compares two scalar strictly.
82  * @param A scalar triplet
83  * @param B scalar triplet
84  * @return 1 if A > B, -1 if A < B, 0 otherwise
85  */
86 int BOP_exactComp(const MT_Tuple3& A, const MT_Tuple3& B)
87 {
88         if (A.x() > B.x()) return 1;
89         else if (B.x() > A.x()) return -1;
90         else if (A.y() > B.y()) return 1;
91         else if (B.y() > A.y()) return -1;
92         else if (A.z() > B.z()) return 1;
93         else if (B.z() > A.z()) return -1;
94         else return 0;
95 }
96
97 /**
98  * Returns if p1 is between p2 and p3 and lay on the same line (are collinears).
99  * @param p1 point
100  * @param p2 point
101  * @param p3 point
102  * @return true if p1 is between p2 and p3 and lay on the same line, false otherwise
103  */
104 bool BOP_between(const MT_Point3& p1, const MT_Point3& p2, const MT_Point3& p3)
105 {
106         MT_Scalar distance = p2.distance(p3);
107         return (p1.distance(p2) < distance && p1.distance(p3) < distance) && BOP_collinear(p1,p2,p3);
108 }
109
110 /**
111  * Returns if three points lay on the same line (are collinears).
112  * @param p1 point
113  * @param p2 point
114  * @param p3 point
115  * @return true if the three points lay on the same line, false otherwise
116  */
117 bool BOP_collinear(const MT_Point3& p1, const MT_Point3& p2, const MT_Point3& p3)
118 {
119         MT_Vector3 v1 = p2 - p1;
120         MT_Vector3 v2 = p3 - p2;
121
122         /* normalize vectors before taking their cross product, so its length 
123      * has some actual meaning */
124         v1.normalize(); 
125         v2.normalize();
126
127         MT_Vector3 w = v1.cross(v2);
128         
129         return (BOP_comp(w.x(),0.0) == 0) && (BOP_comp(w.y(),0.0) == 0) && (BOP_comp(w.z(),0.0) == 0);
130 }
131
132 /**
133  * Returns if a quad (coplanar) is convex.
134  * @return true if the quad is convex, false otherwise
135  */
136 bool BOP_convex(const MT_Point3& p1, const MT_Point3& p2, const MT_Point3& p3, const MT_Point3& p4)
137 {
138         MT_Vector3 v1 = p3 - p1;
139         MT_Vector3 v2 = p4 - p2;
140         MT_Vector3 quadPlane = v1.cross(v2);
141         // plane1 is the perpendicular plane that contains the quad diagonal (p2,p4)
142         MT_Plane3 plane1(quadPlane.cross(v2),p2);
143         // if p1 and p3 are classified in the same region, the quad is not convex 
144         if (BOP_classify(p1,plane1) == BOP_classify(p3,plane1)) return false;
145         else {
146                 // Test the other quad diagonal (p1,p3) and perpendicular plane
147                 MT_Plane3 plane2(quadPlane.cross(v1),p1);
148                 // if p2 and p4 are classified in the same region, the quad is not convex
149                 return (BOP_classify(p2,plane2) != BOP_classify(p4,plane2));
150         }
151 }
152
153 /**
154  * Returns if a quad (coplanar) is concave and where is the split edge.
155  * @return 0 if is convex, 1 if is concave and split edge is p1-p3 and -1 if is
156  * cancave and split edge is p2-p4.
157  */
158 int BOP_concave(const MT_Point3& p1, const MT_Point3& p2, const MT_Point3& p3, const MT_Point3& p4)
159 {
160         MT_Vector3 v1 = p3 - p1;
161         MT_Vector3 v2 = p4 - p2;
162         MT_Vector3 quadPlane = v1.cross(v2);
163         // plane1 is the perpendicular plane that contains the quad diagonal (p2,p4)
164         MT_Plane3 plane1(quadPlane.cross(v2),p2);
165         // if p1 and p3 are classified in the same region, the quad is not convex 
166         if (BOP_classify(p1,plane1) == BOP_classify(p3,plane1)) return 1;
167         else {
168                 // Test the other quad diagonal (p1,p3) and perpendicular plane
169                 MT_Plane3 plane2(quadPlane.cross(v1),p1);
170                 // if p2 and p4 are classified in the same region, the quad is not convex
171                 if (BOP_classify(p2,plane2) == BOP_classify(p4,plane2)) return -1;
172                 else return 0;
173         }
174 }
175
176 /**
177  * Computes the intersection between two lines (on the same plane).
178  * @param vL1 first line vector
179  * @param pL1 first line point
180  * @param vL2 second line vector
181  * @param pL2 second line point
182  * @param intersection intersection point (if exists)
183  * @return false if lines are parallels, true otherwise
184  */
185 bool BOP_intersect(const MT_Vector3& vL1, const MT_Point3& pL1, const MT_Vector3& vL2, 
186                                    const MT_Point3& pL2, MT_Point3 &intersection)
187 {
188         // NOTE: 
189     // If the lines aren't on the same plane, the intersection point will not be valid. 
190         // So be careful !!
191
192         MT_Scalar t = -1;
193         MT_Scalar den = (vL1.y()*vL2.x() - vL1.x() * vL2.y());
194         
195         if (BOP_comp(den,0)) {
196                 t =  (pL2.y()*vL1.x() - vL1.y()*pL2.x() + pL1.x()*vL1.y() - pL1.y()*vL1.x()) / den ;
197         }
198         else {
199                 den = (vL1.y()*vL2.z() - vL1.z() * vL2.y());
200                 if (BOP_comp(den,0)) {
201                         t =  (pL2.y()*vL1.z() - vL1.y()*pL2.z() + pL1.z()*vL1.y() - pL1.y()*vL1.z()) / den ;
202                 }
203                 else {
204                         den = (vL1.x()*vL2.z() - vL1.z() * vL2.x());
205                         if (BOP_comp(den,0)) {
206                                 t =  (pL2.x()*vL1.z() - vL1.x()*pL2.z() + pL1.z()*vL1.x() - pL1.x()*vL1.z()) / den ;
207                         }
208                         else {
209                                 return false;
210                         }
211                 }
212         }
213         
214         intersection.setValue(vL2.x()*t + pL2.x(), vL2.y()*t + pL2.y(), vL2.z()*t + pL2.z());
215         return true;
216 }
217
218 /**
219  * Returns the center of the circle defined by three points.
220  * @param p1 point
221  * @param p2 point
222  * @param p3 point
223  * @param center circle center
224  * @return false if points are collinears, true otherwise
225  */
226 bool BOP_getCircleCenter(const MT_Point3& p1, const MT_Point3& p2, const MT_Point3& p3, 
227                                                  MT_Point3& center)
228 {
229         // Compute quad plane
230         MT_Vector3 p1p2 = p2-p1;
231         MT_Vector3 p1p3 = p3-p1;
232         MT_Plane3 plane1(p1,p2,p3);
233         MT_Vector3 plane = plane1.Normal();
234         
235         // Compute first line vector, perpendicular to plane vector and edge (p1,p2)
236         MT_Vector3 vL1 = p1p2.cross(plane);
237         if( MT_fuzzyZero(vL1.length() ) )
238                         return false;
239         vL1.normalize();
240         
241         // Compute first line point, middle point of edge (p1,p2)
242         MT_Point3 pL1 = p1.lerp(p2, 0.5);
243
244         // Compute second line vector, perpendicular to plane vector and edge (p1,p3)
245         MT_Vector3 vL2 = p1p3.cross(plane);
246         if( MT_fuzzyZero(vL2.length() ) )
247                         return false;
248         vL2.normalize();
249         
250         // Compute second line point, middle point of edge (p1,p3)
251         MT_Point3 pL2 = p1.lerp(p3, 0.5);
252
253         // Compute intersection (the lines lay on the same plane, so the intersection exists
254     // only if they are not parallel!!)
255         return BOP_intersect(vL1,pL1,vL2,pL2,center);
256 }
257
258 /**
259  * Returns if points q is inside the circle defined by p1, p2 and p3.
260  * @param p1 point
261  * @param p2 point
262  * @param p3 point
263  * @param q point
264  * @return true if p4 or p5 are inside the circle, false otherwise. If 
265  * the circle does not exist (p1, p2 and p3 are collinears) returns true
266  */
267 bool BOP_isInsideCircle(const MT_Point3& p1, const MT_Point3& p2, const MT_Point3& p3, 
268                                                 const MT_Point3& q)
269 {
270         MT_Point3 center;
271
272         // Compute circle center
273         bool ok = BOP_getCircleCenter(p1,p2,p3,center);
274         
275         if (!ok) return true; // p1,p2 and p3 are collinears
276
277         // Check if q is inside the circle
278         MT_Scalar r = p1.distance(center);
279         MT_Scalar d = q.distance(center);    
280         return (BOP_comp(d,r) <= 0);
281 }
282
283 /**
284  * Returns if points p4 or p5 is inside the circle defined by p1, p2 and p3.
285  * @param p1 point
286  * @param p2 point
287  * @param p3 point
288  * @param p4 point
289  * @param p5 point
290  * @return true if p4 or p5 is inside the circle, false otherwise. If 
291  * the circle does not exist (p1, p2 and p3 are collinears) returns true
292  */
293 bool BOP_isInsideCircle(const MT_Point3& p1, const MT_Point3& p2, const MT_Point3& p3, 
294                                                 const MT_Point3& p4, const MT_Point3& p5)
295 {
296         MT_Point3 center;
297         bool ok = BOP_getCircleCenter(p1,p2,p3,center);
298
299         if (!ok) return true; // Collinear points!
300
301         // Check if p4 or p5 is inside the circle
302         MT_Scalar r = p1.distance(center);
303         MT_Scalar d1 = p4.distance(center);
304         MT_Scalar d2 = p5.distance(center);
305         return (BOP_comp(d1,r) <= 0 || BOP_comp(d2,r) <= 0);
306 }
307
308 /**
309  * Returns if two planes share the same orientation.
310  * @return >0 if planes share the same orientation
311  */
312 MT_Scalar BOP_orientation(const MT_Plane3& p1, const MT_Plane3& p2)
313 {
314         // Dot product between plane normals
315         return (p1.x()*p2.x() + p1.y()*p2.y() + p1.z()*p2.z());
316 }
317
318 /**
319  * Classifies a point according to the specified plane with EPSILON accuracy.
320  * @param p point
321  * @param plane plane
322  * @return >0 if the point is above (OUT), 
323  *         =0 if the point is on (ON), 
324  *         <0 if the point is below (IN)
325  */
326 int BOP_classify(const MT_Point3& p, const MT_Plane3& plane)
327 {
328         // Compare plane - point distance with zero
329         return BOP_comp(plane.signedDistance(p),0);
330 }
331
332 /**
333  * Intersects a plane with the line that contains the specified points.
334  * @param plane split plane
335  * @param p1 first line point
336  * @param p2 second line point
337  * @return intersection between plane and line that contains p1 and p2
338  */
339 MT_Point3 BOP_intersectPlane(const MT_Plane3& plane, const MT_Point3& p1, const MT_Point3& p2)
340 {
341         // Compute intersection between plane and line ...
342     //
343         //       L: (p2-p1)lambda + p1
344         //
345         // supposes resolve equation ...
346         //
347         //       coefA*((p2.x - p1.y)*lambda + p1.x) + ... + coefD = 0
348         
349     MT_Point3 intersection = MT_Point3(0,0,0); //never ever return anything undefined! 
350     MT_Scalar den = plane.x()*(p2.x()-p1.x()) + 
351                                         plane.y()*(p2.y()-p1.y()) + 
352                                         plane.z()*(p2.z()-p1.z());
353         if (den != 0) {
354                 MT_Scalar lambda = (-plane.x()*p1.x()-plane.y()*p1.y()-plane.z()*p1.z()-plane.w()) / den;
355                 intersection.setValue(p1.x() + (p2.x()-p1.x())*lambda, 
356                                                   p1.y() + (p2.y()-p1.y())*lambda, 
357                                                   p1.z() + (p2.z()-p1.z())*lambda);
358                 return intersection;
359         }
360         return intersection;
361 }
362
363 /**
364  * Returns if a plane contains a point with EPSILON accuracy.
365  * @param plane plane
366  * @param point point
367  * @return true if the point is on the plane, false otherwise
368  */
369 bool BOP_containsPoint(const MT_Plane3& plane, const MT_Point3& point)
370 {
371         return BOP_comp(plane.signedDistance(point),0) == 0;
372 }
373
374 /**
375  * Pre: p0, p1 and p2 is a triangle and q is an interior point.
376  * @param p0 point
377  * @param p1 point
378  * @param p2 point
379  * @param q point
380  * @return intersection point I
381  *                v 
382  *  (p0)-----(I)----->(p1)
383  *    \       ^        /
384  *     \      |w      /
385  *      \     |      /
386  *       \   (q)    /
387  *        \   |    /
388  *         \  |   /
389  *          \ |  /
390  *           (p2)
391  *
392  * v = P1-P2
393  * w = P3-Q
394  * r0(t) = v*t+P1
395  * r1(t) = w*t+P3
396  * I = r0^r1
397  */
398 MT_Point3 BOP_4PointIntersect(const MT_Point3& p0, const MT_Point3& p1, const MT_Point3& p2, 
399                                                           const MT_Point3& q)
400 {
401         MT_Vector3 v(p0.x()-p1.x(), p0.y()-p1.y(), p0.z()-p1.z());
402         MT_Vector3 w(p2.x()-q.x(), p2.y()-q.y(), p2.z()-q.z());
403         MT_Point3 I;
404         
405         BOP_intersect(v,p0,w,p2,I);
406         return I;
407 }
408
409 /**
410  * Pre: p0, p1 and q are collinears.
411  * @param p0 point
412  * @param p1 point
413  * @param q point
414  * @return 0 if q == p0, 1 if q == p1, or a value between 0 and 1 otherwise
415  *
416  * (p0)-----(q)------------(p1)
417  *   |<-d1-->|               |
418  *   |<---------d0---------->|
419  * 
420  */
421 MT_Scalar BOP_EpsilonDistance(const MT_Point3& p0, const MT_Point3& p1, const MT_Point3& q)
422 {
423         MT_Scalar d0 = p0.distance(p1);
424         MT_Scalar d1 = p0.distance(q);
425         MT_Scalar d;
426         
427         if (BOP_comp(d0,0)==0) d = 1.0;
428         else if (BOP_comp(d1,0)==0) d = 0.0;
429         else d = d1 / d0;
430         return d;
431 }