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