svn merge -r36603:36651 https://svn.blender.org/svnroot/bf-blender/trunk/blender...
[blender.git] / source / blender / blenlib / intern / math_geom.c
1 /*
2  * $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  *
20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
21  * All rights reserved.
22  
23  * The Original Code is: some of this file.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  * */
27
28 /** \file blender/blenlib/intern/math_geom.c
29  *  \ingroup bli
30  */
31
32
33
34 #include "MEM_guardedalloc.h"
35
36 #include "BLI_math.h"
37 #include "BLI_memarena.h"
38 #include "BLI_utildefines.h"
39
40
41
42 /********************************** Polygons *********************************/
43
44 void cent_tri_v3(float cent[3], const float v1[3], const float v2[3], const float v3[3])
45 {
46         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
47         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
48         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
49 }
50
51 void cent_quad_v3(float cent[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
52 {
53         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
54         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
55         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
56 }
57
58 float normal_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3])
59 {
60         float n1[3],n2[3];
61
62         n1[0]= v1[0]-v2[0];
63         n2[0]= v2[0]-v3[0];
64         n1[1]= v1[1]-v2[1];
65         n2[1]= v2[1]-v3[1];
66         n1[2]= v1[2]-v2[2];
67         n2[2]= v2[2]-v3[2];
68         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
69         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
70         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
71
72         return normalize_v3(n);
73 }
74
75 float normal_quad_v3(float n[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
76 {
77         /* real cross! */
78         float n1[3],n2[3];
79
80         n1[0]= v1[0]-v3[0];
81         n1[1]= v1[1]-v3[1];
82         n1[2]= v1[2]-v3[2];
83
84         n2[0]= v2[0]-v4[0];
85         n2[1]= v2[1]-v4[1];
86         n2[2]= v2[2]-v4[2];
87
88         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
89         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
90         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
91
92         return normalize_v3(n);
93 }
94
95 float area_tri_v2(const float v1[2], const float v2[2], const float v3[2])
96 {
97         return 0.5f * fabsf((v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]));
98 }
99
100 float area_tri_signed_v2(const float v1[2], const float v2[2], const float v3[2])
101 {
102         return 0.5f * ((v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0]));
103 }
104
105 float area_quad_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3])  /* only convex Quadrilaterals */
106 {
107         float len, vec1[3], vec2[3], n[3];
108
109         sub_v3_v3v3(vec1, v2, v1);
110         sub_v3_v3v3(vec2, v4, v1);
111         cross_v3_v3v3(n, vec1, vec2);
112         len= normalize_v3(n);
113
114         sub_v3_v3v3(vec1, v4, v3);
115         sub_v3_v3v3(vec2, v2, v3);
116         cross_v3_v3v3(n, vec1, vec2);
117         len+= normalize_v3(n);
118
119         return (len/2.0f);
120 }
121
122 float area_tri_v3(const float v1[3], const float v2[3], const float v3[3])  /* Triangles */
123 {
124         float len, vec1[3], vec2[3], n[3];
125
126         sub_v3_v3v3(vec1, v3, v2);
127         sub_v3_v3v3(vec2, v1, v2);
128         cross_v3_v3v3(n, vec1, vec2);
129         len= normalize_v3(n);
130
131         return (len/2.0f);
132 }
133
134 float area_poly_v3(int nr, float verts[][3], const float normal[3])
135 {
136         float x, y, z, area, max;
137         float *cur, *prev;
138         int a, px=0, py=1;
139
140         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
141         x= (float)fabs(normal[0]);
142         y= (float)fabs(normal[1]);
143         z= (float)fabs(normal[2]);
144         max = MAX3(x, y, z);
145         if(max==y) py=2;
146         else if(max==x) {
147                 px=1; 
148                 py= 2;
149         }
150
151         /* The Trapezium Area Rule */
152         prev= verts[nr-1];
153         cur= verts[0];
154         area= 0;
155         for(a=0; a<nr; a++) {
156                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
157                 prev= verts[a];
158                 cur= verts[a+1];
159         }
160
161         return fabsf(0.5f * area / max);
162 }
163
164 /********************************* Distance **********************************/
165
166 /* distance v1 to line v2-v3 */
167 /* using Hesse formula, NO LINE PIECE! */
168 float dist_to_line_v2(const float v1[2], const float v2[2], const float v3[2])
169 {
170         float a[2],deler;
171
172         a[0]= v2[1]-v3[1];
173         a[1]= v3[0]-v2[0];
174         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
175         if(deler== 0.0f) return 0;
176
177         return fabsf((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler;
178
179 }
180
181 /* distance v1 to line-piece v2-v3 */
182 float dist_to_line_segment_v2(const float v1[2], const float v2[2], const float v3[2])
183 {
184         float labda, rc[2], pt[2], len;
185         
186         rc[0]= v3[0]-v2[0];
187         rc[1]= v3[1]-v2[1];
188         len= rc[0]*rc[0]+ rc[1]*rc[1];
189         if(len==0.0f) {
190                 rc[0]= v1[0]-v2[0];
191                 rc[1]= v1[1]-v2[1];
192                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
193         }
194         
195         labda= (rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]))/len;
196         if(labda <= 0.0f) {
197                 pt[0]= v2[0];
198                 pt[1]= v2[1];
199         }
200         else if(labda >= 1.0f) {
201                 pt[0]= v3[0];
202                 pt[1]= v3[1];
203         }
204         else {
205                 pt[0]= labda*rc[0]+v2[0];
206                 pt[1]= labda*rc[1]+v2[1];
207         }
208
209         rc[0]= pt[0]-v1[0];
210         rc[1]= pt[1]-v1[1];
211         return sqrtf(rc[0]*rc[0]+ rc[1]*rc[1]);
212 }
213
214 /* point closest to v1 on line v2-v3 in 3D */
215 void closest_to_line_segment_v3(float closest[3], const float v1[3], const float v2[3], const float v3[3])
216 {
217         float lambda, cp[3];
218
219         lambda= closest_to_line_v3(cp,v1, v2, v3);
220
221         if(lambda <= 0.0f)
222                 copy_v3_v3(closest, v2);
223         else if(lambda >= 1.0f)
224                 copy_v3_v3(closest, v3);
225         else
226                 copy_v3_v3(closest, cp);
227 }
228
229 /* distance v1 to line-piece v2-v3 in 3D */
230 float dist_to_line_segment_v3(const float v1[3], const float v2[3], const float v3[3])
231 {
232         float closest[3];
233
234         closest_to_line_segment_v3(closest, v1, v2, v3);
235
236         return len_v3v3(closest, v1);
237 }
238
239 /******************************* Intersection ********************************/
240
241 /* intersect Line-Line, shorts */
242 int isect_line_line_v2_int(const int v1[2], const int v2[2], const int v3[2], const int v4[2])
243 {
244         float div, labda, mu;
245         
246         div= (float)((v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]));
247         if(div==0.0f) return ISECT_LINE_LINE_COLINEAR;
248         
249         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
250         
251         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
252         
253         if(labda>=0.0f && labda<=1.0f && mu>=0.0f && mu<=1.0f) {
254                 if(labda==0.0f || labda==1.0f || mu==0.0f || mu==1.0f) return ISECT_LINE_LINE_EXACT;
255                 return ISECT_LINE_LINE_CROSS;
256         }
257         return ISECT_LINE_LINE_NONE;
258 }
259
260 /* intersect Line-Line, floats */
261 int isect_line_line_v2(const float v1[2], const float v2[2], const float v3[2], const float v4[2])
262 {
263         float div, labda, mu;
264         
265         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
266         if(div==0.0f) return ISECT_LINE_LINE_COLINEAR;
267         
268         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
269         
270         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
271         
272         if(labda>=0.0f && labda<=1.0f && mu>=0.0f && mu<=1.0f) {
273                 if(labda==0.0f || labda==1.0f || mu==0.0f || mu==1.0f) return ISECT_LINE_LINE_EXACT;
274                 return ISECT_LINE_LINE_CROSS;
275         }
276         return ISECT_LINE_LINE_NONE;
277 }
278
279 /* get intersection point of two 2D segments and return intersection type:
280     -1: colliniar
281      1: intersection */
282 int isect_seg_seg_v2_point(const float v1[2], const float v2[2], const float v3[2], const float v4[2], float vi[2])
283 {
284         float a1, a2, b1, b2, c1, c2, d;
285         float u, v;
286         const float eps= 0.000001f;
287
288         a1= v2[0]-v1[0];
289         b1= v4[0]-v3[0];
290         c1= v1[0]-v4[0];
291
292         a2= v2[1]-v1[1];
293         b2= v4[1]-v3[1];
294         c2= v1[1]-v4[1];
295
296         d= a1*b2-a2*b1;
297
298         if(d==0) {
299                 if(a1*c2-a2*c1==0.0f && b1*c2-b2*c1==0.0f) { /* equal lines */
300                         float a[2], b[2], c[2];
301                         float u2;
302
303                         if(len_v2v2(v1, v2)==0.0f) {
304                                 if(len_v2v2(v3, v4)>eps) {
305                                         /* use non-point segment as basis */
306                                         SWAP(const float *, v1, v3);
307                                         SWAP(const float *, v2, v4);
308                                 } else { /* both of segments are points */
309                                         if(equals_v2v2(v1, v3)) { /* points are equal */
310                                                 copy_v2_v2(vi, v1);
311                                                 return 1;
312                                         }
313
314                                         /* two different points */
315                                         return -1;
316                                 }
317                         }
318
319                         sub_v2_v2v2(a, v3, v1);
320                         sub_v2_v2v2(b, v2, v1);
321                         sub_v2_v2v2(c, v2, v1);
322                         u= dot_v2v2(a, b) / dot_v2v2(c, c);
323
324                         sub_v2_v2v2(a, v4, v1);
325                         u2= dot_v2v2(a, b) / dot_v2v2(c, c);
326
327                         if(u>u2) SWAP(float, u, u2);
328
329                         if(u>1.0f+eps || u2<-eps) return -1; /* non-ovlerlapping segments */
330                         else if(maxf(0.0f, u) == minf(1.0f, u2)){ /* one common point: can return result */
331                                 interp_v2_v2v2(vi, v1, v2, maxf(0, u));
332                                 return 1;
333                         }
334                 }
335
336                 /* lines are colliniar */
337                 return -1;
338         }
339
340         u= (c2*b1-b2*c1)/d;
341         v= (c1*a2-a1*c2)/d;
342
343         if(u>=-eps && u<=1.0f+eps && v>=-eps && v<=1.0f+eps) { /* intersection */
344                 interp_v2_v2v2(vi, v1, v2, u);
345                 return 1;
346         }
347
348         /* out of segment intersection */
349         return -1;
350 }
351
352 /*
353 -1: colliniar
354  1: intersection
355
356 */
357 static short IsectLLPt2Df(const float x0, const float y0, const float x1, const float y1,
358                                          const float x2, const float y2, const float x3, const float y3, float *xi,float *yi)
359
360 {
361         /*
362         this function computes the intersection of the sent lines
363         and returns the intersection point, note that the function assumes
364         the lines intersect. the function can handle vertical as well
365         as horizontal lines. note the function isn't very clever, it simply
366         applies the math, but we don't need speed since this is a
367         pre-processing step
368         */
369         float c1,c2, // constants of linear equations
370         det_inv,  // the inverse of the determinant of the coefficient
371         m1,m2;    // the slopes of each line
372         /*
373         compute slopes, note the cludge for infinity, however, this will
374         be close enough
375         */
376         if (fabs(x1-x0) > 0.000001)
377                 m1 = (y1-y0) / (x1-x0);
378         else
379                 return -1; /*m1 = (float) 1e+10;*/   // close enough to infinity
380
381         if (fabs(x3-x2) > 0.000001)
382                 m2 = (y3-y2) / (x3-x2);
383         else
384                 return -1; /*m2 = (float) 1e+10;*/   // close enough to infinity
385
386         if (fabs(m1-m2) < 0.000001)
387                 return -1; /* parallel lines */
388         
389 // compute constants
390
391         c1 = (y0-m1*x0);
392         c2 = (y2-m2*x2);
393
394 // compute the inverse of the determinate
395
396         det_inv = 1.0f / (-m1 + m2);
397
398 // use Kramers rule to compute xi and yi
399
400         *xi= ((-c2 + c1) *det_inv);
401         *yi= ((m2*c1 - m1*c2) *det_inv);
402         
403         return 1; 
404 } // end Intersect_Lines
405
406 /* point in tri */
407
408 int isect_point_tri_v2(const float pt[2], const float v1[2], const float v2[2], const float v3[2])
409 {
410         if (line_point_side_v2(v1,v2,pt)>=0.0f) {
411                 if (line_point_side_v2(v2,v3,pt)>=0.0f) {
412                         if (line_point_side_v2(v3,v1,pt)>=0.0f) {
413                                 return 1;
414                         }
415                 }
416         } else {
417                 if (! (line_point_side_v2(v2,v3,pt)>=0.0f)) {
418                         if (! (line_point_side_v2(v3,v1,pt)>=0.0f)) {
419                                 return -1;
420                         }
421                 }
422         }
423         
424         return 0;
425 }
426 /* point in quad - only convex quads */
427 int isect_point_quad_v2(const float pt[2], const float v1[2], const float v2[2], const float v3[2], const float v4[2])
428 {
429         if (line_point_side_v2(v1,v2,pt)>=0.0f) {
430                 if (line_point_side_v2(v2,v3,pt)>=0.0f) {
431                         if (line_point_side_v2(v3,v4,pt)>=0.0f) {
432                                 if (line_point_side_v2(v4,v1,pt)>=0.0f) {
433                                         return 1;
434                                 }
435                         }
436                 }
437         } else {
438                 if (! (line_point_side_v2(v2,v3,pt)>=0.0f)) {
439                         if (! (line_point_side_v2(v3,v4,pt)>=0.0f)) {
440                                 if (! (line_point_side_v2(v4,v1,pt)>=0.0f)) {
441                                         return -1;
442                                 }
443                         }
444                 }
445         }
446         
447         return 0;
448 }
449
450 /* moved from effect.c
451    test if the line starting at p1 ending at p2 intersects the triangle v0..v2
452    return non zero if it does 
453 */
454 int isect_line_tri_v3(const float p1[3], const float p2[3], const float v0[3], const float v1[3], const float v2[3], float *lambda, float uv[2])
455 {
456
457         float p[3], s[3], d[3], e1[3], e2[3], q[3];
458         float a, f, u, v;
459         
460         sub_v3_v3v3(e1, v1, v0);
461         sub_v3_v3v3(e2, v2, v0);
462         sub_v3_v3v3(d, p2, p1);
463         
464         cross_v3_v3v3(p, d, e2);
465         a = dot_v3v3(e1, p);
466         if ((a > -0.000001f) && (a < 0.000001f)) return 0;
467         f = 1.0f/a;
468         
469         sub_v3_v3v3(s, p1, v0);
470         
471         u = f * dot_v3v3(s, p);
472         if ((u < 0.0f)||(u > 1.0f)) return 0;
473         
474         cross_v3_v3v3(q, s, e1);
475         
476         v = f * dot_v3v3(d, q);
477         if ((v < 0.0f)||((u + v) > 1.0f)) return 0;
478
479         *lambda = f * dot_v3v3(e2, q);
480         if ((*lambda < 0.0f)||(*lambda > 1.0f)) return 0;
481
482         if(uv) {
483                 uv[0]= u;
484                 uv[1]= v;
485         }
486         
487         return 1;
488 }
489 /* moved from effect.c
490    test if the ray starting at p1 going in d direction intersects the triangle v0..v2
491    return non zero if it does 
492 */
493 int isect_ray_tri_v3(const float p1[3], const float d[3], const float v0[3], const float v1[3], const float v2[3], float *lambda, float uv[2])
494 {
495         float p[3], s[3], e1[3], e2[3], q[3];
496         float a, f, u, v;
497         
498         sub_v3_v3v3(e1, v1, v0);
499         sub_v3_v3v3(e2, v2, v0);
500         
501         cross_v3_v3v3(p, d, e2);
502         a = dot_v3v3(e1, p);
503         /* note: these values were 0.000001 in 2.4x but for projection snapping on
504          * a human head (1BU==1m), subsurf level 2, this gave many errors - campbell */
505         if ((a > -0.00000001f) && (a < 0.00000001f)) return 0;
506         f = 1.0f/a;
507         
508         sub_v3_v3v3(s, p1, v0);
509         
510         u = f * dot_v3v3(s, p);
511         if ((u < 0.0f)||(u > 1.0f)) return 0;
512
513         cross_v3_v3v3(q, s, e1);
514         
515         v = f * dot_v3v3(d, q);
516         if ((v < 0.0f)||((u + v) > 1.0f)) return 0;
517
518         *lambda = f * dot_v3v3(e2, q);
519         if ((*lambda < 0.0f)) return 0;
520
521         if(uv) {
522                 uv[0]= u;
523                 uv[1]= v;
524         }
525         
526         return 1;
527 }
528
529 int isect_ray_plane_v3(float p1[3], float d[3], float v0[3], float v1[3], float v2[3], float *lambda, int clip)
530 {
531         float p[3], s[3], e1[3], e2[3], q[3];
532         float a, f;
533         /* float  u, v; */ /*UNUSED*/
534
535         sub_v3_v3v3(e1, v1, v0);
536         sub_v3_v3v3(e2, v2, v0);
537         
538         cross_v3_v3v3(p, d, e2);
539         a = dot_v3v3(e1, p);
540         /* note: these values were 0.000001 in 2.4x but for projection snapping on
541          * a human head (1BU==1m), subsurf level 2, this gave many errors - campbell */
542         if ((a > -0.00000001f) && (a < 0.00000001f)) return 0;
543         f = 1.0f/a;
544         
545         sub_v3_v3v3(s, p1, v0);
546         
547         /* u = f * dot_v3v3(s, p); */ /*UNUSED*/
548
549         cross_v3_v3v3(q, s, e1);
550         
551         /* v = f * dot_v3v3(d, q); */ /*UNUSED*/
552
553         *lambda = f * dot_v3v3(e2, q);
554         if (clip && (*lambda < 0.0f)) return 0;
555
556         return 1;
557 }
558
559 int isect_ray_tri_epsilon_v3(const float p1[3], const float d[3], const float v0[3], const float v1[3], const float v2[3], float *lambda, float uv[2], const float epsilon)
560 {
561         float p[3], s[3], e1[3], e2[3], q[3];
562         float a, f, u, v;
563
564         sub_v3_v3v3(e1, v1, v0);
565         sub_v3_v3v3(e2, v2, v0);
566
567         cross_v3_v3v3(p, d, e2);
568         a = dot_v3v3(e1, p);
569         if (a == 0.0f) return 0;
570         f = 1.0f/a;
571
572         sub_v3_v3v3(s, p1, v0);
573
574         u = f * dot_v3v3(s, p);
575         if ((u < -epsilon)||(u > 1.0f+epsilon)) return 0;
576
577         cross_v3_v3v3(q, s, e1);
578
579         v = f * dot_v3v3(d, q);
580         if ((v < -epsilon)||((u + v) > 1.0f+epsilon)) return 0;
581
582         *lambda = f * dot_v3v3(e2, q);
583         if ((*lambda < 0.0f)) return 0;
584
585         if(uv) {
586                 uv[0]= u;
587                 uv[1]= v;
588         }
589
590         return 1;
591 }
592
593 int isect_ray_tri_threshold_v3(const float p1[3], const float d[3], const float v0[3], const float v1[3], const float v2[3], float *lambda, float *uv, const float threshold)
594 {
595         float p[3], s[3], e1[3], e2[3], q[3];
596         float a, f, u, v;
597         float du = 0, dv = 0;
598         
599         sub_v3_v3v3(e1, v1, v0);
600         sub_v3_v3v3(e2, v2, v0);
601         
602         cross_v3_v3v3(p, d, e2);
603         a = dot_v3v3(e1, p);
604         if ((a > -0.000001f) && (a < 0.000001f)) return 0;
605         f = 1.0f/a;
606         
607         sub_v3_v3v3(s, p1, v0);
608         
609         cross_v3_v3v3(q, s, e1);
610         *lambda = f * dot_v3v3(e2, q);
611         if ((*lambda < 0.0f)) return 0;
612         
613         u = f * dot_v3v3(s, p);
614         v = f * dot_v3v3(d, q);
615         
616         if (u < 0) du = u;
617         if (u > 1) du = u - 1;
618         if (v < 0) dv = v;
619         if (v > 1) dv = v - 1;
620         if (u > 0 && v > 0 && u + v > 1)
621         {
622                 float t = u + v - 1;
623                 du = u - t/2;
624                 dv = v - t/2;
625         }
626
627         mul_v3_fl(e1, du);
628         mul_v3_fl(e2, dv);
629         
630         if (dot_v3v3(e1, e1) + dot_v3v3(e2, e2) > threshold * threshold)
631         {
632                 return 0;
633         }
634
635         if(uv) {
636                 uv[0]= u;
637                 uv[1]= v;
638         }
639         
640         return 1;
641 }
642
643
644 /* Adapted from the paper by Kasper Fauerby */
645 /* "Improved Collision detection and Response" */
646 static int getLowestRoot(const float a, const float b, const float c, const float maxR, float *root)
647 {
648         // Check if a solution exists
649         float determinant = b*b - 4.0f*a*c;
650
651         // If determinant is negative it means no solutions.
652         if (determinant >= 0.0f)
653         {
654                 // calculate the two roots: (if determinant == 0 then
655                 // x1==x2 but lets disregard that slight optimization)
656                 float sqrtD = (float)sqrt(determinant);
657                 float r1 = (-b - sqrtD) / (2.0f*a);
658                 float r2 = (-b + sqrtD) / (2.0f*a);
659                 
660                 // Sort so x1 <= x2
661                 if (r1 > r2)
662                         SWAP(float, r1, r2);
663
664                 // Get lowest root:
665                 if (r1 > 0.0f && r1 < maxR)
666                 {
667                         *root = r1;
668                         return 1;
669                 }
670
671                 // It is possible that we want x2 - this can happen
672                 // if x1 < 0
673                 if (r2 > 0.0f && r2 < maxR)
674                 {
675                         *root = r2;
676                         return 1;
677                 }
678         }
679         // No (valid) solutions
680         return 0;
681 }
682
683 int isect_sweeping_sphere_tri_v3(const float p1[3], const float p2[3], const float radius, const float v0[3], const float v1[3], const float v2[3], float *lambda, float ipoint[3])
684 {
685         float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3];
686         float a, b, c, d, e, x, y, z, radius2=radius*radius;
687         float elen2,edotv,edotbv,nordotv;
688         float newLambda;
689         int found_by_sweep=0;
690
691         sub_v3_v3v3(e1,v1,v0);
692         sub_v3_v3v3(e2,v2,v0);
693         sub_v3_v3v3(vel,p2,p1);
694
695 /*---test plane of tri---*/
696         cross_v3_v3v3(nor,e1,e2);
697         normalize_v3(nor);
698
699         /* flip normal */
700         if(dot_v3v3(nor,vel)>0.0f) negate_v3(nor);
701         
702         a=dot_v3v3(p1,nor)-dot_v3v3(v0,nor);
703         nordotv=dot_v3v3(nor,vel);
704
705         if (fabsf(nordotv) < 0.000001f)
706         {
707                 if(fabsf(a) >= radius) {
708                         return 0;
709                 }
710         }
711         else
712         {
713                 float t0=(-a+radius)/nordotv;
714                 float t1=(-a-radius)/nordotv;
715
716                 if(t0>t1)
717                         SWAP(float, t0, t1);
718
719                 if(t0>1.0f || t1<0.0f) return 0;
720
721                 /* clamp to [0,1] */
722                 CLAMP(t0, 0.0f, 1.0f);
723                 CLAMP(t1, 0.0f, 1.0f);
724
725                 /*---test inside of tri---*/
726                 /* plane intersection point */
727
728                 point[0] = p1[0] + vel[0]*t0 - nor[0]*radius;
729                 point[1] = p1[1] + vel[1]*t0 - nor[1]*radius;
730                 point[2] = p1[2] + vel[2]*t0 - nor[2]*radius;
731
732
733                 /* is the point in the tri? */
734                 a=dot_v3v3(e1,e1);
735                 b=dot_v3v3(e1,e2);
736                 c=dot_v3v3(e2,e2);
737
738                 sub_v3_v3v3(temp,point,v0);
739                 d=dot_v3v3(temp,e1);
740                 e=dot_v3v3(temp,e2);
741                 
742                 x=d*c-e*b;
743                 y=e*a-d*b;
744                 z=x+y-(a*c-b*b);
745
746
747                 if(z <= 0.0f && (x >= 0.0f && y >= 0.0f))
748                 {
749                 //(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000){
750                         *lambda=t0;
751                         copy_v3_v3(ipoint,point);
752                         return 1;
753                 }
754         }
755
756
757         *lambda=1.0f;
758
759 /*---test points---*/
760         a=dot_v3v3(vel,vel);
761
762         /*v0*/
763         sub_v3_v3v3(temp,p1,v0);
764         b=2.0f*dot_v3v3(vel,temp);
765         c=dot_v3v3(temp,temp)-radius2;
766
767         if(getLowestRoot(a, b, c, *lambda, lambda))
768         {
769                 copy_v3_v3(ipoint,v0);
770                 found_by_sweep=1;
771         }
772
773         /*v1*/
774         sub_v3_v3v3(temp,p1,v1);
775         b=2.0f*dot_v3v3(vel,temp);
776         c=dot_v3v3(temp,temp)-radius2;
777
778         if(getLowestRoot(a, b, c, *lambda, lambda))
779         {
780                 copy_v3_v3(ipoint,v1);
781                 found_by_sweep=1;
782         }
783         
784         /*v2*/
785         sub_v3_v3v3(temp,p1,v2);
786         b=2.0f*dot_v3v3(vel,temp);
787         c=dot_v3v3(temp,temp)-radius2;
788
789         if(getLowestRoot(a, b, c, *lambda, lambda))
790         {
791                 copy_v3_v3(ipoint,v2);
792                 found_by_sweep=1;
793         }
794
795 /*---test edges---*/
796         sub_v3_v3v3(e3,v2,v1); //wasnt yet calculated
797
798
799         /*e1*/
800         sub_v3_v3v3(bv,v0,p1);
801
802         elen2 = dot_v3v3(e1,e1);
803         edotv = dot_v3v3(e1,vel);
804         edotbv = dot_v3v3(e1,bv);
805
806         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
807         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
808         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
809
810         if(getLowestRoot(a, b, c, *lambda, &newLambda))
811         {
812                 e=(edotv*newLambda-edotbv)/elen2;
813
814                 if(e >= 0.0f && e <= 1.0f)
815                 {
816                         *lambda = newLambda;
817                         copy_v3_v3(ipoint,e1);
818                         mul_v3_fl(ipoint,e);
819                         add_v3_v3(ipoint, v0);
820                         found_by_sweep=1;
821                 }
822         }
823
824         /*e2*/
825         /*bv is same*/
826         elen2 = dot_v3v3(e2,e2);
827         edotv = dot_v3v3(e2,vel);
828         edotbv = dot_v3v3(e2,bv);
829
830         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
831         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
832         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
833
834         if(getLowestRoot(a, b, c, *lambda, &newLambda))
835         {
836                 e=(edotv*newLambda-edotbv)/elen2;
837
838                 if(e >= 0.0f && e <= 1.0f)
839                 {
840                         *lambda = newLambda;
841                         copy_v3_v3(ipoint,e2);
842                         mul_v3_fl(ipoint,e);
843                         add_v3_v3(ipoint, v0);
844                         found_by_sweep=1;
845                 }
846         }
847
848         /*e3*/
849         /* sub_v3_v3v3(bv,v0,p1); */ /* UNUSED */
850         /* elen2 = dot_v3v3(e1,e1); */ /* UNUSED */
851         /* edotv = dot_v3v3(e1,vel); */ /* UNUSED */
852         /* edotbv = dot_v3v3(e1,bv); */ /* UNUSED */
853
854         sub_v3_v3v3(bv,v1,p1);
855         elen2 = dot_v3v3(e3,e3);
856         edotv = dot_v3v3(e3,vel);
857         edotbv = dot_v3v3(e3,bv);
858
859         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
860         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
861         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
862
863         if(getLowestRoot(a, b, c, *lambda, &newLambda))
864         {
865                 e=(edotv*newLambda-edotbv)/elen2;
866
867                 if(e >= 0.0f && e <= 1.0f)
868                 {
869                         *lambda = newLambda;
870                         copy_v3_v3(ipoint,e3);
871                         mul_v3_fl(ipoint,e);
872                         add_v3_v3(ipoint, v1);
873                         found_by_sweep=1;
874                 }
875         }
876
877
878         return found_by_sweep;
879 }
880 int isect_axial_line_tri_v3(const int axis, const float p1[3], const float p2[3], const float v0[3], const float v1[3], const float v2[3], float *lambda)
881 {
882         float p[3], e1[3], e2[3];
883         float u, v, f;
884         int a0=axis, a1=(axis+1)%3, a2=(axis+2)%3;
885
886         //return isect_line_tri_v3(p1,p2,v0,v1,v2,lambda);
887
888         ///* first a simple bounding box test */
889         //if(MIN3(v0[a1],v1[a1],v2[a1]) > p1[a1]) return 0;
890         //if(MIN3(v0[a2],v1[a2],v2[a2]) > p1[a2]) return 0;
891         //if(MAX3(v0[a1],v1[a1],v2[a1]) < p1[a1]) return 0;
892         //if(MAX3(v0[a2],v1[a2],v2[a2]) < p1[a2]) return 0;
893
894         ///* then a full intersection test */
895         
896         sub_v3_v3v3(e1,v1,v0);
897         sub_v3_v3v3(e2,v2,v0);
898         sub_v3_v3v3(p,v0,p1);
899
900         f= (e2[a1]*e1[a2]-e2[a2]*e1[a1]);
901         if ((f > -0.000001f) && (f < 0.000001f)) return 0;
902
903         v= (p[a2]*e1[a1]-p[a1]*e1[a2])/f;
904         if ((v < 0.0f)||(v > 1.0f)) return 0;
905         
906         f= e1[a1];
907         if((f > -0.000001f) && (f < 0.000001f)){
908                 f= e1[a2];
909                 if((f > -0.000001f) && (f < 0.000001f)) return 0;
910                 u= (-p[a2]-v*e2[a2])/f;
911         }
912         else
913                 u= (-p[a1]-v*e2[a1])/f;
914
915         if ((u < 0.0f) || ((u + v) > 1.0f)) return 0;
916
917         *lambda = (p[a0]+u*e1[a0]+v*e2[a0])/(p2[a0]-p1[a0]);
918
919         if ((*lambda < 0.0f) || (*lambda > 1.0f)) return 0;
920
921         return 1;
922 }
923
924 /* Returns the number of point of interests
925  * 0 - lines are colinear
926  * 1 - lines are coplanar, i1 is set to intersection
927  * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively 
928  * */
929 int isect_line_line_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3], float i1[3], float i2[3])
930 {
931         float a[3], b[3], c[3], ab[3], cb[3], dir1[3], dir2[3];
932         float d;
933         
934         sub_v3_v3v3(c, v3, v1);
935         sub_v3_v3v3(a, v2, v1);
936         sub_v3_v3v3(b, v4, v3);
937
938         normalize_v3_v3(dir1, a);
939         normalize_v3_v3(dir2, b);
940         d = dot_v3v3(dir1, dir2);
941         if (d == 1.0f || d == -1.0f) {
942                 /* colinear */
943                 return 0;
944         }
945
946         cross_v3_v3v3(ab, a, b);
947         d = dot_v3v3(c, ab);
948
949         /* test if the two lines are coplanar */
950         if (d > -0.000001f && d < 0.000001f) {
951                 cross_v3_v3v3(cb, c, b);
952
953                 mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
954                 add_v3_v3v3(i1, v1, a);
955                 copy_v3_v3(i2, i1);
956                 
957                 return 1; /* one intersection only */
958         }
959         /* if not */
960         else {
961                 float n[3], t[3];
962                 float v3t[3], v4t[3];
963                 sub_v3_v3v3(t, v1, v3);
964
965                 /* offset between both plane where the lines lies */
966                 cross_v3_v3v3(n, a, b);
967                 project_v3_v3v3(t, t, n);
968
969                 /* for the first line, offset the second line until it is coplanar */
970                 add_v3_v3v3(v3t, v3, t);
971                 add_v3_v3v3(v4t, v4, t);
972                 
973                 sub_v3_v3v3(c, v3t, v1);
974                 sub_v3_v3v3(a, v2, v1);
975                 sub_v3_v3v3(b, v4t, v3t);
976
977                 cross_v3_v3v3(ab, a, b);
978                 cross_v3_v3v3(cb, c, b);
979
980                 mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
981                 add_v3_v3v3(i1, v1, a);
982
983                 /* for the second line, just substract the offset from the first intersection point */
984                 sub_v3_v3v3(i2, i1, t);
985                 
986                 return 2; /* two nearest points */
987         }
988
989
990 /* Intersection point strictly between the two lines
991  * 0 when no intersection is found 
992  * */
993 int isect_line_line_strict_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3], float vi[3], float *lambda)
994 {
995         float a[3], b[3], c[3], ab[3], cb[3], ca[3], dir1[3], dir2[3];
996         float d;
997         
998         sub_v3_v3v3(c, v3, v1);
999         sub_v3_v3v3(a, v2, v1);
1000         sub_v3_v3v3(b, v4, v3);
1001
1002         normalize_v3_v3(dir1, a);
1003         normalize_v3_v3(dir2, b);
1004         d = dot_v3v3(dir1, dir2);
1005         if (d == 1.0f || d == -1.0f || d == 0) {
1006                 /* colinear or one vector is zero-length*/
1007                 return 0;
1008         }
1009
1010         cross_v3_v3v3(ab, a, b);
1011         d = dot_v3v3(c, ab);
1012
1013         /* test if the two lines are coplanar */
1014         if (d > -0.000001f && d < 0.000001f) {
1015                 float f1, f2;
1016                 cross_v3_v3v3(cb, c, b);
1017                 cross_v3_v3v3(ca, c, a);
1018
1019                 f1 = dot_v3v3(cb, ab) / dot_v3v3(ab, ab);
1020                 f2 = dot_v3v3(ca, ab) / dot_v3v3(ab, ab);
1021                 
1022                 if (f1 >= 0 && f1 <= 1 &&
1023                         f2 >= 0 && f2 <= 1)
1024                 {
1025                         mul_v3_fl(a, f1);
1026                         add_v3_v3v3(vi, v1, a);
1027                         
1028                         if (lambda != NULL)
1029                         {
1030                                 *lambda = f1;
1031                         }
1032                         
1033                         return 1; /* intersection found */
1034                 }
1035                 else
1036                 {
1037                         return 0;
1038                 }
1039         }
1040         else
1041         {
1042                 return 0;
1043         }
1044
1045
1046 int isect_aabb_aabb_v3(const float min1[3], const float max1[3], const float min2[3], const float max2[3])
1047 {
1048         return (min1[0]<max2[0] && min1[1]<max2[1] && min1[2]<max2[2] &&
1049                         min2[0]<max1[0] && min2[1]<max1[1] && min2[2]<max1[2]);
1050 }
1051
1052 /* find closest point to p on line through l1,l2 and return lambda,
1053  * where (0 <= lambda <= 1) when cp is in the line segement l1,l2
1054  */
1055 float closest_to_line_v3(float cp[3], const float p[3], const float l1[3], const float l2[3])
1056 {
1057         float h[3],u[3],lambda;
1058         sub_v3_v3v3(u, l2, l1);
1059         sub_v3_v3v3(h, p, l1);
1060         lambda =dot_v3v3(u,h)/dot_v3v3(u,u);
1061         cp[0] = l1[0] + u[0] * lambda;
1062         cp[1] = l1[1] + u[1] * lambda;
1063         cp[2] = l1[2] + u[2] * lambda;
1064         return lambda;
1065 }
1066
1067 float closest_to_line_v2(float cp[2],const float p[2], const float l1[2], const float l2[2])
1068 {
1069         float h[2],u[2],lambda;
1070         sub_v2_v2v2(u, l2, l1);
1071         sub_v2_v2v2(h, p, l1);
1072         lambda =dot_v2v2(u,h)/dot_v2v2(u,u);
1073         cp[0] = l1[0] + u[0] * lambda;
1074         cp[1] = l1[1] + u[1] * lambda;
1075         return lambda;
1076 }
1077
1078 #if 0
1079 /* little sister we only need to know lambda */
1080 static float lambda_cp_line(float p[3], float l1[3], float l2[3])
1081 {
1082         float h[3],u[3];
1083         sub_v3_v3v3(u, l2, l1);
1084         sub_v3_v3v3(h, p, l1);
1085         return(dot_v3v3(u,h)/dot_v3v3(u,u));
1086 }
1087 #endif
1088
1089 /* Similar to LineIntersectsTriangleUV, except it operates on a quad and in 2d, assumes point is in quad */
1090 void isect_point_quad_uv_v2(const float v0[2], const float v1[2], const float v2[2], const float v3[2], const float pt[2], float *uv)
1091 {
1092         float x0,y0, x1,y1, wtot, v2d[2], w1, w2;
1093         
1094         /* used for parallel lines */
1095         float pt3d[3], l1[3], l2[3], pt_on_line[3];
1096         
1097         /* compute 2 edges  of the quad  intersection point */
1098         if (IsectLLPt2Df(v0[0],v0[1],v1[0],v1[1],  v2[0],v2[1],v3[0],v3[1], &x0,&y0) == 1) {
1099                 /* the intersection point between the quad-edge intersection and the point in the quad we want the uv's for */
1100                 /* should never be paralle !! */
1101                 /*printf("\tnot parallel 1\n");*/
1102                 IsectLLPt2Df(pt[0],pt[1],x0,y0,  v0[0],v0[1],v3[0],v3[1], &x1,&y1);
1103                 
1104                 /* Get the weights from the new intersection point, to each edge */
1105                 v2d[0] = x1-v0[0];
1106                 v2d[1] = y1-v0[1];
1107                 w1 = len_v2(v2d);
1108                 
1109                 v2d[0] = x1-v3[0]; /* some but for the other vert */
1110                 v2d[1] = y1-v3[1];
1111                 w2 = len_v2(v2d);
1112                 wtot = w1+w2;
1113                 /*w1 = w1/wtot;*/
1114                 /*w2 = w2/wtot;*/
1115                 uv[0] = w1/wtot;
1116         } else {
1117                 /* lines are parallel, lambda_cp_line_ex is 3d grrr */
1118                 /*printf("\tparallel1\n");*/
1119                 pt3d[0] = pt[0];
1120                 pt3d[1] = pt[1];
1121                 pt3d[2] = l1[2] = l2[2] = 0.0f;
1122                 
1123                 l1[0] = v0[0]; l1[1] = v0[1];
1124                 l2[0] = v1[0]; l2[1] = v1[1];
1125                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1126                 v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */
1127                 v2d[1] = pt[1]-pt_on_line[1];
1128                 w1 = len_v2(v2d);
1129                 
1130                 l1[0] = v2[0]; l1[1] = v2[1];
1131                 l2[0] = v3[0]; l2[1] = v3[1];
1132                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1133                 v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */
1134                 v2d[1] = pt[1]-pt_on_line[1];
1135                 w2 = len_v2(v2d);
1136                 wtot = w1+w2;
1137                 uv[0] = w1/wtot;        
1138         }
1139         
1140         /* Same as above to calc the uv[1] value, alternate calculation */
1141         
1142         if (IsectLLPt2Df(v0[0],v0[1],v3[0],v3[1],  v1[0],v1[1],v2[0],v2[1], &x0,&y0) == 1) { /* was v0,v1  v2,v3  now v0,v3  v1,v2*/
1143                 /* never paralle if above was not */
1144                 /*printf("\tnot parallel2\n");*/
1145                 IsectLLPt2Df(pt[0],pt[1],x0,y0,  v0[0],v0[1],v1[0],v1[1], &x1,&y1);/* was v0,v3  now v0,v1*/
1146                 
1147                 v2d[0] = x1-v0[0];
1148                 v2d[1] = y1-v0[1];
1149                 w1 = len_v2(v2d);
1150                 
1151                 v2d[0] = x1-v1[0];
1152                 v2d[1] = y1-v1[1];
1153                 w2 = len_v2(v2d);
1154                 wtot = w1+w2;
1155                 uv[1] = w1/wtot;
1156         } else {
1157                 /* lines are parallel, lambda_cp_line_ex is 3d grrr */
1158                 /*printf("\tparallel2\n");*/
1159                 pt3d[0] = pt[0];
1160                 pt3d[1] = pt[1];
1161                 pt3d[2] = l1[2] = l2[2] = 0.0f;
1162                 
1163                 
1164                 l1[0] = v0[0]; l1[1] = v0[1];
1165                 l2[0] = v3[0]; l2[1] = v3[1];
1166                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1167                 v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */
1168                 v2d[1] = pt[1]-pt_on_line[1];
1169                 w1 = len_v2(v2d);
1170                 
1171                 l1[0] = v1[0]; l1[1] = v1[1];
1172                 l2[0] = v2[0]; l2[1] = v2[1];
1173                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1174                 v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */
1175                 v2d[1] = pt[1]-pt_on_line[1];
1176                 w2 = len_v2(v2d);
1177                 wtot = w1+w2;
1178                 uv[1] = w1/wtot;
1179         }
1180         /* may need to flip UV's here */
1181 }
1182
1183 /* same as above but does tri's and quads, tri's are a bit of a hack */
1184 void isect_point_face_uv_v2(const int isquad, const float v0[2], const float v1[2], const float v2[2], const float v3[2], const float pt[2], float *uv)
1185 {
1186         if (isquad) {
1187                 isect_point_quad_uv_v2(v0, v1, v2, v3, pt, uv);
1188         }
1189         else {
1190                 /* not for quads, use for our abuse of LineIntersectsTriangleUV */
1191                 float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3], lambda;
1192                         
1193                 p1_3d[0] = p2_3d[0] = uv[0];
1194                 p1_3d[1] = p2_3d[1] = uv[1];
1195                 p1_3d[2] = 1.0f;
1196                 p2_3d[2] = -1.0f;
1197                 v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0;
1198                 
1199                 /* generate a new fuv, (this is possibly a non optimal solution,
1200                  * since we only need 2d calculation but use 3d func's)
1201                  * 
1202                  * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face
1203                  * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV.
1204                  * This means the new values will be correct in relation to the derived meshes face. 
1205                  */
1206                 copy_v2_v2(v0_3d, v0);
1207                 copy_v2_v2(v1_3d, v1);
1208                 copy_v2_v2(v2_3d, v2);
1209                 
1210                 /* Doing this in 3D is not nice */
1211                 isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv);
1212         }
1213 }
1214
1215 #if 0 // XXX this version used to be used in isect_point_tri_v2_int() and was called IsPointInTri2D
1216 int isect_point_tri_v2(float pt[2], float v1[2], float v2[2], float v3[2])
1217 {
1218         float inp1, inp2, inp3;
1219         
1220         inp1= (v2[0]-v1[0])*(v1[1]-pt[1]) + (v1[1]-v2[1])*(v1[0]-pt[0]);
1221         inp2= (v3[0]-v2[0])*(v2[1]-pt[1]) + (v2[1]-v3[1])*(v2[0]-pt[0]);
1222         inp3= (v1[0]-v3[0])*(v3[1]-pt[1]) + (v3[1]-v1[1])*(v3[0]-pt[0]);
1223         
1224         if(inp1<=0.0f && inp2<=0.0f && inp3<=0.0f) return 1;
1225         if(inp1>=0.0f && inp2>=0.0f && inp3>=0.0f) return 1;
1226         
1227         return 0;
1228 }
1229 #endif
1230
1231 #if 0
1232 int isect_point_tri_v2(float v0[2], float v1[2], float v2[2], float pt[2])
1233 {
1234                 /* not for quads, use for our abuse of LineIntersectsTriangleUV */
1235                 float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3];
1236                 /* not used */
1237                 float lambda, uv[3];
1238                         
1239                 p1_3d[0] = p2_3d[0] = uv[0]= pt[0];
1240                 p1_3d[1] = p2_3d[1] = uv[1]= uv[2]= pt[1];
1241                 p1_3d[2] = 1.0f;
1242                 p2_3d[2] = -1.0f;
1243                 v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0;
1244                 
1245                 /* generate a new fuv, (this is possibly a non optimal solution,
1246                  * since we only need 2d calculation but use 3d func's)
1247                  * 
1248                  * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face
1249                  * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV.
1250                  * This means the new values will be correct in relation to the derived meshes face. 
1251                  */
1252                 copy_v2_v2(v0_3d, v0);
1253                 copy_v2_v2(v1_3d, v1);
1254                 copy_v2_v2(v2_3d, v2);
1255                 
1256                 /* Doing this in 3D is not nice */
1257                 return isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv);
1258 }
1259 #endif
1260
1261 /*
1262
1263         x1,y2
1264         |  \
1265         |   \     .(a,b)
1266         |    \
1267         x1,y1-- x2,y1
1268
1269 */
1270 int isect_point_tri_v2_int(const int x1, const int y1, const int x2, const int y2, const int a, const int b)
1271 {
1272         float v1[2], v2[2], v3[2], p[2];
1273         
1274         v1[0]= (float)x1;
1275         v1[1]= (float)y1;
1276         
1277         v2[0]= (float)x1;
1278         v2[1]= (float)y2;
1279         
1280         v3[0]= (float)x2;
1281         v3[1]= (float)y1;
1282         
1283         p[0]= (float)a;
1284         p[1]= (float)b;
1285         
1286         return isect_point_tri_v2(p, v1, v2, v3);
1287 }
1288
1289 static int point_in_slice(const float p[3], const float v1[3], const float l1[3], const float l2[3])
1290 {
1291 /* 
1292 what is a slice ?
1293 some maths:
1294 a line including l1,l2 and a point not on the line 
1295 define a subset of R3 delimeted by planes parallel to the line and orthogonal 
1296 to the (point --> line) distance vector,one plane on the line one on the point, 
1297 the room inside usually is rather small compared to R3 though still infinte
1298 useful for restricting (speeding up) searches 
1299 e.g. all points of triangular prism are within the intersection of 3 'slices'
1300 onother trivial case : cube 
1301 but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
1302 */
1303         float h,rp[3],cp[3],q[3];
1304
1305         closest_to_line_v3(cp,v1,l1,l2);
1306         sub_v3_v3v3(q,cp,v1);
1307
1308         sub_v3_v3v3(rp,p,v1);
1309         h=dot_v3v3(q,rp)/dot_v3v3(q,q);
1310         if (h < 0.0f || h > 1.0f) return 0;
1311         return 1;
1312 }
1313
1314 #if 0
1315 /*adult sister defining the slice planes by the origin and the normal  
1316 NOTE |normal| may not be 1 but defining the thickness of the slice*/
1317 static int point_in_slice_as(float p[3],float origin[3],float normal[3])
1318 {
1319         float h,rp[3];
1320         sub_v3_v3v3(rp,p,origin);
1321         h=dot_v3v3(normal,rp)/dot_v3v3(normal,normal);
1322         if (h < 0.0f || h > 1.0f) return 0;
1323         return 1;
1324 }
1325
1326 /*mama (knowing the squared length of the normal)*/
1327 static int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns)
1328 {
1329         float h,rp[3];
1330         sub_v3_v3v3(rp,p,origin);
1331         h=dot_v3v3(normal,rp)/lns;
1332         if (h < 0.0f || h > 1.0f) return 0;
1333         return 1;
1334 }
1335 #endif
1336
1337 int isect_point_tri_prism_v3(const float p[3], const float v1[3], const float v2[3], const float v3[3])
1338 {
1339         if(!point_in_slice(p,v1,v2,v3)) return 0;
1340         if(!point_in_slice(p,v2,v3,v1)) return 0;
1341         if(!point_in_slice(p,v3,v1,v2)) return 0;
1342         return 1;
1343 }
1344
1345 int clip_line_plane(float p1[3], float p2[3], const float plane[4])
1346 {
1347         float dp[3], n[3], div, t, pc[3];
1348
1349         copy_v3_v3(n, plane);
1350         sub_v3_v3v3(dp, p2, p1);
1351         div= dot_v3v3(dp, n);
1352
1353         if(div == 0.0f) /* parallel */
1354                 return 1;
1355
1356         t= -(dot_v3v3(p1, n) + plane[3])/div;
1357
1358         if(div > 0.0f) {
1359                 /* behind plane, completely clipped */
1360                 if(t >= 1.0f) {
1361                         zero_v3(p1);
1362                         zero_v3(p2);
1363                         return 0;
1364                 }
1365
1366                 /* intersect plane */
1367                 if(t > 0.0f) {
1368                         madd_v3_v3v3fl(pc, p1, dp, t);
1369                         copy_v3_v3(p1, pc);
1370                         return 1;
1371                 }
1372
1373                 return 1;
1374         }
1375         else {
1376                 /* behind plane, completely clipped */
1377                 if(t <= 0.0f) {
1378                         zero_v3(p1);
1379                         zero_v3(p2);
1380                         return 0;
1381                 }
1382
1383                 /* intersect plane */
1384                 if(t < 1.0f) {
1385                         madd_v3_v3v3fl(pc, p1, dp, t);
1386                         copy_v3_v3(p2, pc);
1387                         return 1;
1388                 }
1389
1390                 return 1;
1391         }
1392 }
1393
1394
1395 void plot_line_v2v2i(const int p1[2], const int p2[2], int (*callback)(int, int, void *), void *userData)
1396 {
1397         int x1= p1[0];
1398         int y1= p1[1];
1399         int x2= p2[0];
1400         int y2= p2[1];
1401
1402         signed char ix;
1403         signed char iy;
1404
1405         // if x1 == x2 or y1 == y2, then it does not matter what we set here
1406         int delta_x = (x2 > x1?(ix = 1, x2 - x1):(ix = -1, x1 - x2)) << 1;
1407         int delta_y = (y2 > y1?(iy = 1, y2 - y1):(iy = -1, y1 - y2)) << 1;
1408
1409         if(callback(x1, y1, userData) == 0) {
1410                 return;
1411         }
1412
1413         if (delta_x >= delta_y) {
1414                 // error may go below zero
1415                 int error = delta_y - (delta_x >> 1);
1416
1417                 while (x1 != x2) {
1418                         if (error >= 0) {
1419                                 if (error || (ix > 0)) {
1420                                         y1 += iy;
1421                                         error -= delta_x;
1422                                 }
1423                                 // else do nothing
1424                         }
1425                         // else do nothing
1426
1427                         x1 += ix;
1428                         error += delta_y;
1429
1430                         if(callback(x1, y1, userData) == 0) {
1431                                 return ;
1432                         }
1433                 }
1434         }
1435         else {
1436                 // error may go below zero
1437                 int error = delta_x - (delta_y >> 1);
1438
1439                 while (y1 != y2) {
1440                         if (error >= 0) {
1441                                 if (error || (iy > 0)) {
1442                                         x1 += ix;
1443                                         error -= delta_y;
1444                                 }
1445                                 // else do nothing
1446                         }
1447                         // else do nothing
1448
1449                         y1 += iy;
1450                         error += delta_x;
1451
1452                         if(callback(x1, y1, userData) == 0) {
1453                                 return;
1454                         }
1455                 }
1456         }
1457 }
1458
1459 /****************************** Interpolation ********************************/
1460
1461 static float tri_signed_area(const float v1[3], const float v2[3], const float v3[3], const int i, const int j)
1462 {
1463         return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
1464 }
1465
1466 static int barycentric_weights(const float v1[3], const float v2[3], const float v3[3], const float co[3], const float n[3], float w[3])
1467 {
1468         float xn, yn, zn, a1, a2, a3, asum;
1469         short i, j;
1470
1471         /* find best projection of face XY, XZ or YZ: barycentric weights of
1472            the 2d projected coords are the same and faster to compute */
1473         xn= (float)fabs(n[0]);
1474         yn= (float)fabs(n[1]);
1475         zn= (float)fabs(n[2]);
1476         if(zn>=xn && zn>=yn) {i= 0; j= 1;}
1477         else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
1478         else {i= 1; j= 2;} 
1479
1480         a1= tri_signed_area(v2, v3, co, i, j);
1481         a2= tri_signed_area(v3, v1, co, i, j);
1482         a3= tri_signed_area(v1, v2, co, i, j);
1483
1484         asum= a1 + a2 + a3;
1485
1486         if (fabsf(asum) < FLT_EPSILON) {
1487                 /* zero area triangle */
1488                 w[0]= w[1]= w[2]= 1.0f/3.0f;
1489                 return 1;
1490         }
1491
1492         asum= 1.0f/asum;
1493         w[0]= a1*asum;
1494         w[1]= a2*asum;
1495         w[2]= a3*asum;
1496
1497         return 0;
1498 }
1499
1500 void interp_weights_face_v3(float w[4], const float v1[3], const float v2[3], const float v3[3], const float v4[3], const float co[3])
1501 {
1502         float w2[3];
1503
1504         w[0]= w[1]= w[2]= w[3]= 0.0f;
1505
1506         /* first check for exact match */
1507         if(equals_v3v3(co, v1))
1508                 w[0]= 1.0f;
1509         else if(equals_v3v3(co, v2))
1510                 w[1]= 1.0f;
1511         else if(equals_v3v3(co, v3))
1512                 w[2]= 1.0f;
1513         else if(v4 && equals_v3v3(co, v4))
1514                 w[3]= 1.0f;
1515         else {
1516                 /* otherwise compute barycentric interpolation weights */
1517                 float n1[3], n2[3], n[3];
1518                 int degenerate;
1519
1520                 sub_v3_v3v3(n1, v1, v3);
1521                 if (v4) {
1522                         sub_v3_v3v3(n2, v2, v4);
1523                 }
1524                 else {
1525                         sub_v3_v3v3(n2, v2, v3);
1526                 }
1527                 cross_v3_v3v3(n, n1, n2);
1528
1529                 /* OpenGL seems to split this way, so we do too */
1530                 if (v4) {
1531                         degenerate= barycentric_weights(v1, v2, v4, co, n, w);
1532                         SWAP(float, w[2], w[3]);
1533
1534                         if(degenerate || (w[0] < 0.0f)) {
1535                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
1536                                    so we interpolate using the other triangle */
1537                                 degenerate= barycentric_weights(v2, v3, v4, co, n, w2);
1538
1539                                 if(!degenerate) {
1540                                         w[0]= 0.0f;
1541                                         w[1]= w2[0];
1542                                         w[2]= w2[1];
1543                                         w[3]= w2[2];
1544                                 }
1545                         }
1546                 }
1547                 else
1548                         barycentric_weights(v1, v2, v3, co, n, w);
1549         }
1550 }
1551
1552 /* used by projection painting
1553  * note: using area_tri_signed_v2 means locations outside the triangle are correctly weighted */
1554 void barycentric_weights_v2(const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
1555 {
1556         float wtot_inv, wtot;
1557
1558         w[0] = area_tri_signed_v2(v2, v3, co);
1559         w[1] = area_tri_signed_v2(v3, v1, co);
1560         w[2] = area_tri_signed_v2(v1, v2, co);
1561         wtot = w[0]+w[1]+w[2];
1562
1563         if (wtot != 0.0f) {
1564                 wtot_inv = 1.0f/wtot;
1565
1566                 w[0] = w[0]*wtot_inv;
1567                 w[1] = w[1]*wtot_inv;
1568                 w[2] = w[2]*wtot_inv;
1569         }
1570         else /* dummy values for zero area face */
1571                 w[0] = w[1] = w[2] = 1.0f/3.0f;
1572 }
1573
1574 /* given 2 triangles in 3D space, and a point in relation to the first triangle.
1575  * calculate the location of a point in relation to the second triangle.
1576  * Useful for finding relative positions with geometry */
1577 void barycentric_transform(float pt_tar[3], float const pt_src[3],
1578                 const float tri_tar_p1[3], const float tri_tar_p2[3], const float tri_tar_p3[3],
1579                 const float tri_src_p1[3], const float tri_src_p2[3], const float tri_src_p3[3])
1580 {
1581         /* this works by moving the source triangle so its normal is pointing on the Z
1582          * axis where its barycentric wights can be calculated in 2D and its Z offset can
1583          *  be re-applied. The weights are applied directly to the targets 3D points and the
1584          *  z-depth is used to scale the targets normal as an offset.
1585          * This saves transforming the target into its Z-Up orientation and back (which could also work) */
1586         const float z_up[3] = {0, 0, 1};
1587         float no_tar[3], no_src[3];
1588         float quat_src[4];
1589         float pt_src_xy[3];
1590         float  tri_xy_src[3][3];
1591         float w_src[3];
1592         float area_tar, area_src;
1593         float z_ofs_src;
1594
1595         normal_tri_v3(no_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3);
1596         normal_tri_v3(no_src, tri_src_p1, tri_src_p2, tri_src_p3);
1597
1598         rotation_between_vecs_to_quat(quat_src, no_src, z_up);
1599         normalize_qt(quat_src);
1600
1601         copy_v3_v3(pt_src_xy, pt_src);
1602         copy_v3_v3(tri_xy_src[0], tri_src_p1);
1603         copy_v3_v3(tri_xy_src[1], tri_src_p2);
1604         copy_v3_v3(tri_xy_src[2], tri_src_p3);
1605
1606         /* make the source tri xy space */
1607         mul_qt_v3(quat_src, pt_src_xy);
1608         mul_qt_v3(quat_src, tri_xy_src[0]);
1609         mul_qt_v3(quat_src, tri_xy_src[1]);
1610         mul_qt_v3(quat_src, tri_xy_src[2]);
1611
1612         barycentric_weights_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2], pt_src_xy, w_src);
1613         interp_v3_v3v3v3(pt_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3, w_src);
1614
1615         area_tar= sqrtf(area_tri_v3(tri_tar_p1, tri_tar_p2, tri_tar_p3));
1616         area_src= sqrtf(area_tri_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2]));
1617
1618         z_ofs_src= pt_src_xy[2] - tri_xy_src[0][2];
1619         madd_v3_v3v3fl(pt_tar, pt_tar, no_tar, (z_ofs_src / area_src) * area_tar);
1620 }
1621
1622 /* given an array with some invalid values this function interpolates valid values
1623  * replacing the invalid ones */
1624 int interp_sparse_array(float *array, int const list_size, const float skipval)
1625 {
1626         int found_invalid = 0;
1627         int found_valid = 0;
1628         int i;
1629
1630         for (i=0; i < list_size; i++) {
1631                 if(array[i] == skipval)
1632                         found_invalid= 1;
1633                 else
1634                         found_valid= 1;
1635         }
1636
1637         if(found_valid==0) {
1638                 return -1;
1639         }
1640         else if (found_invalid==0) {
1641                 return 0;
1642         }
1643         else {
1644                 /* found invalid depths, interpolate */
1645                 float valid_last= skipval;
1646                 int valid_ofs= 0;
1647
1648                 float *array_up= MEM_callocN(sizeof(float) * list_size, "interp_sparse_array up");
1649                 float *array_down= MEM_callocN(sizeof(float) * list_size, "interp_sparse_array up");
1650
1651                 int *ofs_tot_up= MEM_callocN(sizeof(int) * list_size, "interp_sparse_array tup");
1652                 int *ofs_tot_down= MEM_callocN(sizeof(int) * list_size, "interp_sparse_array tdown");
1653
1654                 for (i=0; i < list_size; i++) {
1655                         if(array[i] == skipval) {
1656                                 array_up[i]= valid_last;
1657                                 ofs_tot_up[i]= ++valid_ofs;
1658                         }
1659                         else {
1660                                 valid_last= array[i];
1661                                 valid_ofs= 0;
1662                         }
1663                 }
1664
1665                 valid_last= skipval;
1666                 valid_ofs= 0;
1667
1668                 for (i=list_size-1; i >= 0; i--) {
1669                         if(array[i] == skipval) {
1670                                 array_down[i]= valid_last;
1671                                 ofs_tot_down[i]= ++valid_ofs;
1672                         }
1673                         else {
1674                                 valid_last= array[i];
1675                                 valid_ofs= 0;
1676                         }
1677                 }
1678
1679                 /* now blend */
1680                 for (i=0; i < list_size; i++) {
1681                         if(array[i] == skipval) {
1682                                 if(array_up[i] != skipval && array_down[i] != skipval) {
1683                                         array[i]= ((array_up[i] * ofs_tot_down[i]) +  (array_down[i] * ofs_tot_up[i])) / (float)(ofs_tot_down[i] + ofs_tot_up[i]);
1684                                 } else if (array_up[i] != skipval) {
1685                                         array[i]= array_up[i];
1686                                 } else if (array_down[i] != skipval) {
1687                                         array[i]= array_down[i];
1688                                 }
1689                         }
1690                 }
1691
1692                 MEM_freeN(array_up);
1693                 MEM_freeN(array_down);
1694
1695                 MEM_freeN(ofs_tot_up);
1696                 MEM_freeN(ofs_tot_down);
1697         }
1698
1699         return 1;
1700 }
1701
1702 /* Mean value weights - smooth interpolation weights for polygons with
1703  * more than 3 vertices */
1704 static float mean_value_half_tan(const float v1[3], const float v2[3], const float v3[3])
1705 {
1706         float d2[3], d3[3], cross[3], area, dot, len;
1707
1708         sub_v3_v3v3(d2, v2, v1);
1709         sub_v3_v3v3(d3, v3, v1);
1710         cross_v3_v3v3(cross, d2, d3);
1711
1712         area= len_v3(cross);
1713         dot= dot_v3v3(d2, d3);
1714         len= len_v3(d2)*len_v3(d3);
1715
1716         if(area == 0.0f)
1717                 return 0.0f;
1718         else
1719                 return (len - dot)/area;
1720 }
1721
1722 void interp_weights_poly_v3(float *w, float v[][3], const int n, const float co[3])
1723 {
1724         float totweight, t1, t2, len, *vmid, *vprev, *vnext;
1725         int i;
1726
1727         totweight= 0.0f;
1728
1729         for(i=0; i<n; i++) {
1730                 vmid= v[i];
1731                 vprev= (i == 0)? v[n-1]: v[i-1];
1732                 vnext= (i == n-1)? v[0]: v[i+1];
1733
1734                 t1= mean_value_half_tan(co, vprev, vmid);
1735                 t2= mean_value_half_tan(co, vmid, vnext);
1736
1737                 len= len_v3v3(co, vmid);
1738                 w[i]= (t1+t2)/len;
1739                 totweight += w[i];
1740         }
1741
1742         if(totweight != 0.0f)
1743                 for(i=0; i<n; i++)
1744                         w[i] /= totweight;
1745 }
1746
1747 /* (x1,v1)(t1=0)------(x2,v2)(t2=1), 0<t<1 --> (x,v)(t) */
1748 void interp_cubic_v3(float x[3], float v[3], const float x1[3], const float v1[3], const float x2[3], const float v2[3], const float t)
1749 {
1750         float a[3],b[3];
1751         float t2= t*t;
1752         float t3= t2*t;
1753
1754         /* cubic interpolation */
1755         a[0]= v1[0] + v2[0] + 2*(x1[0] - x2[0]);
1756         a[1]= v1[1] + v2[1] + 2*(x1[1] - x2[1]);
1757         a[2]= v1[2] + v2[2] + 2*(x1[2] - x2[2]);
1758
1759         b[0]= -2*v1[0] - v2[0] - 3*(x1[0] - x2[0]);
1760         b[1]= -2*v1[1] - v2[1] - 3*(x1[1] - x2[1]);
1761         b[2]= -2*v1[2] - v2[2] - 3*(x1[2] - x2[2]);
1762
1763         x[0]= a[0]*t3 + b[0]*t2 + v1[0]*t + x1[0];
1764         x[1]= a[1]*t3 + b[1]*t2 + v1[1]*t + x1[1];
1765         x[2]= a[2]*t3 + b[2]*t2 + v1[2]*t + x1[2];
1766
1767         v[0]= 3*a[0]*t2 + 2*b[0]*t + v1[0];
1768         v[1]= 3*a[1]*t2 + 2*b[1]*t + v1[1];
1769         v[2]= 3*a[2]*t2 + 2*b[2]*t + v1[2];
1770 }
1771
1772 /***************************** View & Projection *****************************/
1773
1774 void orthographic_m4(float matrix[][4], const float left, const float right, const float bottom, const float top, const float nearClip, const float farClip)
1775 {
1776         float Xdelta, Ydelta, Zdelta;
1777  
1778         Xdelta = right - left;
1779         Ydelta = top - bottom;
1780         Zdelta = farClip - nearClip;
1781         if (Xdelta == 0.0f || Ydelta == 0.0f || Zdelta == 0.0f) {
1782                 return;
1783         }
1784         unit_m4(matrix);
1785         matrix[0][0] = 2.0f/Xdelta;
1786         matrix[3][0] = -(right + left)/Xdelta;
1787         matrix[1][1] = 2.0f/Ydelta;
1788         matrix[3][1] = -(top + bottom)/Ydelta;
1789         matrix[2][2] = -2.0f/Zdelta;            /* note: negate Z       */
1790         matrix[3][2] = -(farClip + nearClip)/Zdelta;
1791 }
1792
1793 void perspective_m4(float mat[4][4], const float left, const float right, const float bottom, const float top, const float nearClip, const float farClip)
1794 {
1795         float Xdelta, Ydelta, Zdelta;
1796
1797         Xdelta = right - left;
1798         Ydelta = top - bottom;
1799         Zdelta = farClip - nearClip;
1800
1801         if (Xdelta == 0.0f || Ydelta == 0.0f || Zdelta == 0.0f) {
1802                 return;
1803         }
1804         mat[0][0] = nearClip * 2.0f/Xdelta;
1805         mat[1][1] = nearClip * 2.0f/Ydelta;
1806         mat[2][0] = (right + left)/Xdelta;              /* note: negate Z       */
1807         mat[2][1] = (top + bottom)/Ydelta;
1808         mat[2][2] = -(farClip + nearClip)/Zdelta;
1809         mat[2][3] = -1.0f;
1810         mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
1811         mat[0][1] = mat[0][2] = mat[0][3] =
1812                 mat[1][0] = mat[1][2] = mat[1][3] =
1813                 mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
1814
1815 }
1816
1817 /* translate a matrix created by orthographic_m4 or perspective_m4 in XY coords (used to jitter the view) */
1818 void window_translate_m4(float winmat[][4], float perspmat[][4], const float x, const float y)
1819 {
1820         if(winmat[2][3] == -1.0f) {
1821                 /* in the case of a win-matrix, this means perspective always */
1822                 float v1[3];
1823                 float v2[3];
1824                 float len1, len2;
1825
1826                 v1[0]= perspmat[0][0];
1827                 v1[1]= perspmat[1][0];
1828                 v1[2]= perspmat[2][0];
1829
1830                 v2[0]= perspmat[0][1];
1831                 v2[1]= perspmat[1][1];
1832                 v2[2]= perspmat[2][1];
1833
1834                 len1= (1.0f / len_v3(v1));
1835                 len2= (1.0f / len_v3(v2));
1836
1837                 winmat[2][0] += len1 * winmat[0][0] * x;
1838                 winmat[2][1] += len2 * winmat[1][1] * y;
1839         }
1840         else {
1841                 winmat[3][0] += x;
1842                 winmat[3][1] += y;
1843         }
1844 }
1845
1846 static void i_multmatrix(float icand[][4], float Vm[][4])
1847 {
1848         int row, col;
1849         float temp[4][4];
1850
1851         for(row=0 ; row<4 ; row++) 
1852                 for(col=0 ; col<4 ; col++)
1853                         temp[row][col] = icand[row][0] * Vm[0][col]
1854                                                    + icand[row][1] * Vm[1][col]
1855                                                    + icand[row][2] * Vm[2][col]
1856                                                    + icand[row][3] * Vm[3][col];
1857         copy_m4_m4(Vm, temp);
1858 }
1859
1860
1861 void polarview_m4(float Vm[][4],float dist, float azimuth, float incidence, float twist)
1862 {
1863
1864         unit_m4(Vm);
1865
1866         translate_m4(Vm,0.0, 0.0, -dist);
1867         rotate_m4(Vm,'Z',-twist);
1868         rotate_m4(Vm,'X',-incidence);
1869         rotate_m4(Vm,'Z',-azimuth);
1870 }
1871
1872 void lookat_m4(float mat[][4],float vx, float vy, float vz, float px, float py, float pz, float twist)
1873 {
1874         float sine, cosine, hyp, hyp1, dx, dy, dz;
1875         float mat1[4][4]= MAT4_UNITY;
1876         
1877         unit_m4(mat);
1878
1879         rotate_m4(mat, 'Z', -twist);
1880
1881         dx = px - vx;
1882         dy = py - vy;
1883         dz = pz - vz;
1884         hyp = dx * dx + dz * dz;        /* hyp squared  */
1885         hyp1 = (float)sqrt(dy*dy + hyp);
1886         hyp = (float)sqrt(hyp);         /* the real hyp */
1887         
1888         if (hyp1 != 0.0f) {             /* rotate X     */
1889                 sine = -dy / hyp1;
1890                 cosine = hyp /hyp1;
1891         } else {
1892                 sine = 0;
1893                 cosine = 1.0f;
1894         }
1895         mat1[1][1] = cosine;
1896         mat1[1][2] = sine;
1897         mat1[2][1] = -sine;
1898         mat1[2][2] = cosine;
1899         
1900         i_multmatrix(mat1, mat);
1901
1902         mat1[1][1] = mat1[2][2] = 1.0f; /* be careful here to reinit    */
1903         mat1[1][2] = mat1[2][1] = 0.0;  /* those modified by the last   */
1904         
1905         /* paragraph    */
1906         if (hyp != 0.0f) {                      /* rotate Y     */
1907                 sine = dx / hyp;
1908                 cosine = -dz / hyp;
1909         } else {
1910                 sine = 0;
1911                 cosine = 1.0f;
1912         }
1913         mat1[0][0] = cosine;
1914         mat1[0][2] = -sine;
1915         mat1[2][0] = sine;
1916         mat1[2][2] = cosine;
1917         
1918         i_multmatrix(mat1, mat);
1919         translate_m4(mat,-vx,-vy,-vz);  /* translate viewpoint to origin */
1920 }
1921
1922 int box_clip_bounds_m4(float boundbox[2][3], const float bounds[4], float winmat[4][4])
1923 {
1924         float mat[4][4], vec[4];
1925         int a, fl, flag= -1;
1926
1927         copy_m4_m4(mat, winmat);
1928
1929         for(a=0; a<8; a++) {
1930                 vec[0]= (a & 1)? boundbox[0][0]: boundbox[1][0];
1931                 vec[1]= (a & 2)? boundbox[0][1]: boundbox[1][1];
1932                 vec[2]= (a & 4)? boundbox[0][2]: boundbox[1][2];
1933                 vec[3]= 1.0;
1934                 mul_m4_v4(mat, vec);
1935
1936                 fl= 0;
1937                 if(bounds) {
1938                         if(vec[0] > bounds[1]*vec[3]) fl |= 1;
1939                         if(vec[0]< bounds[0]*vec[3]) fl |= 2;
1940                         if(vec[1] > bounds[3]*vec[3]) fl |= 4;
1941                         if(vec[1]< bounds[2]*vec[3]) fl |= 8;
1942                 }
1943                 else {
1944                         if(vec[0] < -vec[3]) fl |= 1;
1945                         if(vec[0] > vec[3]) fl |= 2;
1946                         if(vec[1] < -vec[3]) fl |= 4;
1947                         if(vec[1] > vec[3]) fl |= 8;
1948                 }
1949                 if(vec[2] < -vec[3]) fl |= 16;
1950                 if(vec[2] > vec[3]) fl |= 32;
1951
1952                 flag &= fl;
1953                 if(flag==0) return 0;
1954         }
1955
1956         return flag;
1957 }
1958
1959 void box_minmax_bounds_m4(float min[3], float max[3], float boundbox[2][3], float mat[4][4])
1960 {
1961         float mn[3], mx[3], vec[3];
1962         int a;
1963
1964         copy_v3_v3(mn, min);
1965         copy_v3_v3(mx, max);
1966
1967         for(a=0; a<8; a++) {
1968                 vec[0]= (a & 1)? boundbox[0][0]: boundbox[1][0];
1969                 vec[1]= (a & 2)? boundbox[0][1]: boundbox[1][1];
1970                 vec[2]= (a & 4)? boundbox[0][2]: boundbox[1][2];
1971
1972                 mul_m4_v3(mat, vec);
1973                 DO_MINMAX(vec, mn, mx);
1974         }
1975
1976         copy_v3_v3(min, mn);
1977         copy_v3_v3(max, mx);
1978 }
1979
1980 /********************************** Mapping **********************************/
1981
1982 void map_to_tube(float *u, float *v, const float x, const float y, const float z)
1983 {
1984         float len;
1985         
1986         *v = (z + 1.0f) / 2.0f;
1987         
1988         len= (float)sqrt(x*x+y*y);
1989         if(len > 0.0f)
1990                 *u = (float)((1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0);
1991         else
1992                 *v = *u = 0.0f; /* to avoid un-initialized variables */
1993 }
1994
1995 void map_to_sphere(float *u, float *v, const float x, const float y, const float z)
1996 {
1997         float len;
1998         
1999         len= (float)sqrt(x*x+y*y+z*z);
2000         if(len > 0.0f) {
2001                 if(x==0.0f && y==0.0f) *u= 0.0f;        /* othwise domain error */
2002                 else *u = (1.0f - atan2f(x,y) / (float)M_PI) / 2.0f;
2003
2004                 *v = 1.0f - (float)saacos(z/len)/(float)M_PI;
2005         } else {
2006                 *v = *u = 0.0f; /* to avoid un-initialized variables */
2007         }
2008 }
2009
2010 /********************************* Normals **********************************/
2011
2012 void accumulate_vertex_normals(float n1[3], float n2[3], float n3[3],
2013         float n4[3], const float f_no[3], const float co1[3], const float co2[3],
2014         const float co3[3], const float co4[3])
2015 {
2016         float vdiffs[4][3];
2017         const int nverts= (n4!=NULL && co4!=NULL)? 4: 3;
2018
2019         /* compute normalized edge vectors */
2020         sub_v3_v3v3(vdiffs[0], co2, co1);
2021         sub_v3_v3v3(vdiffs[1], co3, co2);
2022
2023         if(nverts==3) {
2024                 sub_v3_v3v3(vdiffs[2], co1, co3);
2025         }
2026         else {
2027                 sub_v3_v3v3(vdiffs[2], co4, co3);
2028                 sub_v3_v3v3(vdiffs[3], co1, co4);
2029                 normalize_v3(vdiffs[3]);
2030         }
2031
2032         normalize_v3(vdiffs[0]);
2033         normalize_v3(vdiffs[1]);
2034         normalize_v3(vdiffs[2]);
2035
2036         /* accumulate angle weighted face normal */
2037         {
2038                 float *vn[]= {n1, n2, n3, n4};
2039                 const float *prev_edge = vdiffs[nverts-1];
2040                 int i;
2041
2042                 for(i=0; i<nverts; i++) {
2043                         const float *cur_edge= vdiffs[i];
2044                         const float fac= saacos(-dot_v3v3(cur_edge, prev_edge));
2045
2046                         // accumulate
2047                         madd_v3_v3fl(vn[i], f_no, fac);
2048                         prev_edge = cur_edge;
2049                 }
2050         }
2051 }
2052
2053 /********************************* Tangents **********************************/
2054
2055 /* For normal map tangents we need to detect uv boundaries, and only average
2056  * tangents in case the uvs are connected. Alternative would be to store 1 
2057  * tangent per face rather than 4 per face vertex, but that's not compatible
2058  * with games */
2059
2060
2061 /* from BKE_mesh.h */
2062 #define STD_UV_CONNECT_LIMIT    0.0001f
2063
2064 void sum_or_add_vertex_tangent(void *arena, VertexTangent **vtang, const float tang[3], const float uv[2])
2065 {
2066         VertexTangent *vt;
2067
2068         /* find a tangent with connected uvs */
2069         for(vt= *vtang; vt; vt=vt->next) {
2070                 if(fabsf(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabsf(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT) {
2071                         add_v3_v3(vt->tang, tang);
2072                         return;
2073                 }
2074         }
2075
2076         /* if not found, append a new one */
2077         vt= BLI_memarena_alloc((MemArena *)arena, sizeof(VertexTangent));
2078         copy_v3_v3(vt->tang, tang);
2079         vt->uv[0]= uv[0];
2080         vt->uv[1]= uv[1];
2081
2082         if(*vtang)
2083                 vt->next= *vtang;
2084         *vtang= vt;
2085 }
2086
2087 float *find_vertex_tangent(VertexTangent *vtang, const float uv[2])
2088 {
2089         VertexTangent *vt;
2090         static float nulltang[3] = {0.0f, 0.0f, 0.0f};
2091
2092         for(vt= vtang; vt; vt=vt->next)
2093                 if(fabsf(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabsf(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT)
2094                         return vt->tang;
2095
2096         return nulltang;        /* shouldn't happen, except for nan or so */
2097 }
2098
2099 void tangent_from_uv(float uv1[2], float uv2[2], float uv3[3], float co1[3], float co2[3], float co3[3], float n[3], float tang[3])
2100 {
2101         float s1= uv2[0] - uv1[0];
2102         float s2= uv3[0] - uv1[0];
2103         float t1= uv2[1] - uv1[1];
2104         float t2= uv3[1] - uv1[1];
2105         float det= (s1 * t2 - s2 * t1);
2106
2107         if(det != 0.0f) { /* otherwise 'tang' becomes nan */
2108                 float tangv[3], ct[3], e1[3], e2[3];
2109
2110                 det= 1.0f/det;
2111
2112                 /* normals in render are inversed... */
2113                 sub_v3_v3v3(e1, co1, co2);
2114                 sub_v3_v3v3(e2, co1, co3);
2115                 tang[0] = (t2*e1[0] - t1*e2[0])*det;
2116                 tang[1] = (t2*e1[1] - t1*e2[1])*det;
2117                 tang[2] = (t2*e1[2] - t1*e2[2])*det;
2118                 tangv[0] = (s1*e2[0] - s2*e1[0])*det;
2119                 tangv[1] = (s1*e2[1] - s2*e1[1])*det;
2120                 tangv[2] = (s1*e2[2] - s2*e1[2])*det;
2121                 cross_v3_v3v3(ct, tang, tangv);
2122         
2123                 /* check flip */
2124                 if ((ct[0]*n[0] + ct[1]*n[1] + ct[2]*n[2]) < 0.0f)
2125                         negate_v3(tang);
2126         }
2127         else {
2128                 tang[0]= tang[1]= tang[2]=  0.0;
2129         }
2130 }
2131
2132 /****************************** Vector Clouds ********************************/
2133
2134 /* vector clouds */
2135 /* void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
2136                                                                  float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3])
2137
2138 input
2139 (
2140 int list_size
2141 4 lists as pointer to array[list_size]
2142 1. current pos array of 'new' positions 
2143 2. current weight array of 'new'weights (may be NULL pointer if you have no weights )
2144 3. reference rpos array of 'old' positions
2145 4. reference rweight array of 'old'weights (may be NULL pointer if you have no weights )
2146 )
2147 output  
2148 (
2149 float lloc[3] center of mass pos
2150 float rloc[3] center of mass rpos
2151 float lrot[3][3] rotation matrix
2152 float lscale[3][3] scale matrix
2153 pointers may be NULL if not needed
2154 )
2155
2156 */
2157 /* can't believe there is none in math utils */
2158 static float _det_m3(float m2[3][3])
2159 {
2160         float det = 0.f;
2161         if (m2){
2162         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
2163                 -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
2164                 +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
2165         }
2166         return det;
2167 }
2168
2169
2170 void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
2171                                                         float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3])
2172 {
2173         float accu_com[3]= {0.0f,0.0f,0.0f}, accu_rcom[3]= {0.0f,0.0f,0.0f};
2174         float accu_weight = 0.0f,accu_rweight = 0.0f,eps = 0.000001f;
2175
2176         int a;
2177         /* first set up a nice default response */
2178         if (lloc) zero_v3(lloc);
2179         if (rloc) zero_v3(rloc);
2180         if (lrot) unit_m3(lrot);
2181         if (lscale) unit_m3(lscale);
2182         /* do com for both clouds */
2183         if (pos && rpos && (list_size > 0)) /* paranoya check */
2184         {
2185                 /* do com for both clouds */
2186                 for(a=0; a<list_size; a++){
2187                         if (weight){
2188                                 float v[3];
2189                                 copy_v3_v3(v,pos[a]);
2190                                 mul_v3_fl(v,weight[a]);
2191                                 add_v3_v3(accu_com, v);
2192                                 accu_weight +=weight[a]; 
2193                         }
2194                         else add_v3_v3(accu_com, pos[a]);
2195
2196                         if (rweight){
2197                                 float v[3];
2198                                 copy_v3_v3(v,rpos[a]);
2199                                 mul_v3_fl(v,rweight[a]);
2200                                 add_v3_v3(accu_rcom, v);
2201                                 accu_rweight +=rweight[a]; 
2202                         }
2203                         else add_v3_v3(accu_rcom, rpos[a]);
2204
2205                 }
2206                 if (!weight || !rweight){
2207                         accu_weight = accu_rweight = list_size;
2208                 }
2209
2210                 mul_v3_fl(accu_com,1.0f/accu_weight);
2211                 mul_v3_fl(accu_rcom,1.0f/accu_rweight);
2212                 if (lloc) copy_v3_v3(lloc,accu_com);
2213                 if (rloc) copy_v3_v3(rloc,accu_rcom);
2214                 if (lrot || lscale){ /* caller does not want rot nor scale, strange but legal */
2215                         /*so now do some reverse engeneering and see if we can split rotation from scale ->Polardecompose*/
2216                         /* build 'projection' matrix */
2217                         float m[3][3],mr[3][3],q[3][3],qi[3][3];
2218                         float va[3],vb[3],stunt[3];
2219                         float odet,ndet;
2220                         int i=0,imax=15;
2221                         zero_m3(m);
2222                         zero_m3(mr);
2223
2224                         /* build 'projection' matrix */
2225                         for(a=0; a<list_size; a++){
2226                                 sub_v3_v3v3(va,rpos[a],accu_rcom);
2227                                 /* mul_v3_fl(va,bp->mass);  mass needs renormalzation here ?? */
2228                                 sub_v3_v3v3(vb,pos[a],accu_com);
2229                                 /* mul_v3_fl(va,rp->mass); */
2230                                 m[0][0] += va[0] * vb[0];
2231                                 m[0][1] += va[0] * vb[1];
2232                                 m[0][2] += va[0] * vb[2];
2233
2234                                 m[1][0] += va[1] * vb[0];
2235                                 m[1][1] += va[1] * vb[1];
2236                                 m[1][2] += va[1] * vb[2];
2237
2238                                 m[2][0] += va[2] * vb[0];
2239                                 m[2][1] += va[2] * vb[1];
2240                                 m[2][2] += va[2] * vb[2];
2241
2242                                 /* building the referenc matrix on the fly
2243                                 needed to scale properly later*/
2244
2245                                 mr[0][0] += va[0] * va[0];
2246                                 mr[0][1] += va[0] * va[1];
2247                                 mr[0][2] += va[0] * va[2];
2248
2249                                 mr[1][0] += va[1] * va[0];
2250                                 mr[1][1] += va[1] * va[1];
2251                                 mr[1][2] += va[1] * va[2];
2252
2253                                 mr[2][0] += va[2] * va[0];
2254                                 mr[2][1] += va[2] * va[1];
2255                                 mr[2][2] += va[2] * va[2];
2256                         }
2257                         copy_m3_m3(q,m);
2258                         stunt[0] = q[0][0]; stunt[1] = q[1][1]; stunt[2] = q[2][2];
2259                         /* renormalizing for numeric stability */
2260                         mul_m3_fl(q,1.f/len_v3(stunt)); 
2261
2262                         /* this is pretty much Polardecompose 'inline' the algo based on Higham's thesis */
2263                         /* without the far case ... but seems to work here pretty neat                   */
2264                         odet = 0.f;
2265                         ndet = _det_m3(q);
2266                         while((odet-ndet)*(odet-ndet) > eps && i<imax){
2267                                 invert_m3_m3(qi,q);
2268                                 transpose_m3(qi);
2269                                 add_m3_m3m3(q,q,qi);
2270                                 mul_m3_fl(q,0.5f);
2271                                 odet =ndet;
2272                                 ndet =_det_m3(q);
2273                                 i++;
2274                         }
2275
2276                         if (i){
2277                                 float scale[3][3];
2278                                 float irot[3][3];
2279                                 if(lrot) copy_m3_m3(lrot,q);
2280                                 invert_m3_m3(irot,q);
2281                                 invert_m3_m3(qi,mr);
2282                                 mul_m3_m3m3(q,m,qi); 
2283                                 mul_m3_m3m3(scale,irot,q); 
2284                                 if(lscale) copy_m3_m3(lscale,scale);
2285
2286                         }
2287                 }
2288         }
2289 }
2290
2291 /******************************* Form Factor *********************************/
2292
2293 static void vec_add_dir(float r[3], const float v1[3], const float v2[3], const float fac)
2294 {
2295         r[0]= v1[0] + fac*(v2[0] - v1[0]);
2296         r[1]= v1[1] + fac*(v2[1] - v1[1]);
2297         r[2]= v1[2] + fac*(v2[2] - v1[2]);
2298 }
2299
2300 static int ff_visible_quad(const float p[3], const float n[3], const float v0[3], const float v1[3], const float v2[3], float q0[3], float q1[3], float q2[3], float q3[3])
2301 {
2302         static const float epsilon = 1e-6f;
2303         float c, sd[3];
2304         
2305         c= dot_v3v3(n, p);
2306
2307         /* signed distances from the vertices to the plane. */
2308         sd[0]= dot_v3v3(n, v0) - c;
2309         sd[1]= dot_v3v3(n, v1) - c;
2310         sd[2]= dot_v3v3(n, v2) - c;
2311
2312         if(fabsf(sd[0]) < epsilon) sd[0] = 0.0f;
2313         if(fabsf(sd[1]) < epsilon) sd[1] = 0.0f;
2314         if(fabsf(sd[2]) < epsilon) sd[2] = 0.0f;
2315
2316         if(sd[0] > 0) {
2317                 if(sd[1] > 0) {
2318                         if(sd[2] > 0) {
2319                                 // +++
2320                                 copy_v3_v3(q0, v0);
2321                                 copy_v3_v3(q1, v1);
2322                                 copy_v3_v3(q2, v2);
2323                                 copy_v3_v3(q3, q2);
2324                         }
2325                         else if(sd[2] < 0) {
2326                                 // ++-
2327                                 copy_v3_v3(q0, v0);
2328                                 copy_v3_v3(q1, v1);
2329                                 vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2])));
2330                                 vec_add_dir(q3, v0, v2, (sd[0]/(sd[0]-sd[2])));
2331                         }
2332                         else {
2333                                 // ++0
2334                                 copy_v3_v3(q0, v0);
2335                                 copy_v3_v3(q1, v1);
2336                                 copy_v3_v3(q2, v2);
2337                                 copy_v3_v3(q3, q2);
2338                         }
2339                 }
2340                 else if(sd[1] < 0) {
2341                         if(sd[2] > 0) {
2342                                 // +-+
2343                                 copy_v3_v3(q0, v0);
2344                                 vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1])));
2345                                 vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2])));
2346                                 copy_v3_v3(q3, v2);
2347                         }
2348                         else if(sd[2] < 0) {
2349                                 // +--
2350                                 copy_v3_v3(q0, v0);
2351                                 vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1])));
2352                                 vec_add_dir(q2, v0, v2, (sd[0]/(sd[0]-sd[2])));
2353                                 copy_v3_v3(q3, q2);
2354                         }
2355                         else {
2356                                 // +-0
2357                                 copy_v3_v3(q0, v0);
2358                                 vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1])));
2359                                 copy_v3_v3(q2, v2);
2360                                 copy_v3_v3(q3, q2);
2361                         }
2362                 }
2363                 else {
2364                         if(sd[2] > 0) {
2365                                 // +0+
2366                                 copy_v3_v3(q0, v0);
2367                                 copy_v3_v3(q1, v1);
2368                                 copy_v3_v3(q2, v2);
2369                                 copy_v3_v3(q3, q2);
2370                         }
2371                         else if(sd[2] < 0) {
2372                                 // +0-
2373                                 copy_v3_v3(q0, v0);
2374                                 copy_v3_v3(q1, v1);
2375                                 vec_add_dir(q2, v0, v2, (sd[0]/(sd[0]-sd[2])));
2376                                 copy_v3_v3(q3, q2);
2377                         }
2378                         else {
2379                                 // +00
2380                                 copy_v3_v3(q0, v0);
2381                                 copy_v3_v3(q1, v1);
2382                                 copy_v3_v3(q2, v2);
2383                                 copy_v3_v3(q3, q2);
2384                         }
2385                 }
2386         }
2387         else if(sd[0] < 0) {
2388                 if(sd[1] > 0) {
2389                         if(sd[2] > 0) {
2390                                 // -++
2391                                 vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1])));
2392                                 copy_v3_v3(q1, v1);
2393                                 copy_v3_v3(q2, v2);
2394                                 vec_add_dir(q3, v0, v2, (sd[0]/(sd[0]-sd[2])));
2395                         }
2396                         else if(sd[2] < 0) {
2397                                 // -+-
2398                                 vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1])));
2399                                 copy_v3_v3(q1, v1);
2400                                 vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2])));
2401                                 copy_v3_v3(q3, q2);
2402                         }
2403                         else {
2404                                 // -+0
2405                                 vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1])));
2406                                 copy_v3_v3(q1, v1);
2407                                 copy_v3_v3(q2, v2);
2408                                 copy_v3_v3(q3, q2);
2409                         }
2410                 }
2411                 else if(sd[1] < 0) {
2412                         if(sd[2] > 0) {
2413                                 // --+
2414                                 vec_add_dir(q0, v0, v2, (sd[0]/(sd[0]-sd[2])));
2415                                 vec_add_dir(q1, v1, v2, (sd[1]/(sd[1]-sd[2])));
2416                                 copy_v3_v3(q2, v2);
2417                                 copy_v3_v3(q3, q2);
2418                         }
2419                         else if(sd[2] < 0) {
2420                                 // ---
2421                                 return 0;
2422                         }
2423                         else {
2424                                 // --0
2425                                 return 0;
2426                         }
2427                 }
2428                 else {
2429                         if(sd[2] > 0) {
2430                                 // -0+
2431                                 vec_add_dir(q0, v0, v2, (sd[0]/(sd[0]-sd[2])));
2432                                 copy_v3_v3(q1, v1);
2433                                 copy_v3_v3(q2, v2);
2434                                 copy_v3_v3(q3, q2);
2435                         }
2436                         else if(sd[2] < 0) {
2437                                 // -0-
2438                                 return 0;
2439                         }
2440                         else {
2441                                 // -00
2442                                 return 0;
2443                         }
2444                 }
2445         }
2446         else {
2447                 if(sd[1] > 0) {
2448                         if(sd[2] > 0) {
2449                                 // 0++
2450                                 copy_v3_v3(q0, v0);
2451                                 copy_v3_v3(q1, v1);
2452                                 copy_v3_v3(q2, v2);
2453                                 copy_v3_v3(q3, q2);
2454                         }
2455                         else if(sd[2] < 0) {
2456                                 // 0+-
2457                                 copy_v3_v3(q0, v0);
2458                                 copy_v3_v3(q1, v1);
2459                                 vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2])));
2460                                 copy_v3_v3(q3, q2);
2461                         }
2462                         else {
2463                                 // 0+0
2464                                 copy_v3_v3(q0, v0);
2465                                 copy_v3_v3(q1, v1);
2466                                 copy_v3_v3(q2, v2);
2467                                 copy_v3_v3(q3, q2);
2468                         }
2469                 }
2470                 else if(sd[1] < 0) {
2471                         if(sd[2] > 0) {
2472                                 // 0-+
2473                                 copy_v3_v3(q0, v0);
2474                                 vec_add_dir(q1, v1, v2, (sd[1]/(sd[1]-sd[2])));
2475                                 copy_v3_v3(q2, v2);
2476                                 copy_v3_v3(q3, q2);
2477                         }
2478                         else if(sd[2] < 0) {
2479                                 // 0--
2480                                 return 0;
2481                         }
2482                         else {
2483                                 // 0-0
2484                                 return 0;
2485                         }
2486                 }
2487                 else {
2488                         if(sd[2] > 0) {
2489                                 // 00+
2490                                 copy_v3_v3(q0, v0);
2491                                 copy_v3_v3(q1, v1);
2492                                 copy_v3_v3(q2, v2);
2493                                 copy_v3_v3(q3, q2);
2494                         }
2495                         else if(sd[2] < 0) {
2496                                 // 00-
2497                                 return 0;
2498                         }
2499                         else {
2500                                 // 000
2501                                 return 0;
2502                         }
2503                 }
2504         }
2505
2506         return 1;
2507 }
2508
2509 /* altivec optimization, this works, but is unused */
2510
2511 #if 0
2512 #include <Accelerate/Accelerate.h>
2513
2514 typedef union {
2515         vFloat v;
2516         float f[4];
2517 } vFloatResult;
2518
2519 static vFloat vec_splat_float(float val)
2520 {
2521         return (vFloat){val, val, val, val};
2522 }
2523
2524 static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float *q2, float *q3)
2525 {
2526         vFloat vcos, rlen, vrx, vry, vrz, vsrx, vsry, vsrz, gx, gy, gz, vangle;
2527         vUInt8 rotate = (vUInt8){4,5,6,7,8,9,10,11,12,13,14,15,0,1,2,3};
2528         vFloatResult vresult;
2529         float result;
2530
2531         /* compute r* */
2532         vrx = (vFloat){q0[0], q1[0], q2[0], q3[0]} - vec_splat_float(p[0]);
2533         vry = (vFloat){q0[1], q1[1], q2[1], q3[1]} - vec_splat_float(p[1]);
2534         vrz = (vFloat){q0[2], q1[2], q2[2], q3[2]} - vec_splat_float(p[2]);
2535
2536         /* normalize r* */
2537         rlen = vec_rsqrte(vrx*vrx + vry*vry + vrz*vrz + vec_splat_float(1e-16f));
2538         vrx = vrx*rlen;
2539         vry = vry*rlen;
2540         vrz = vrz*rlen;
2541
2542         /* rotate r* for cross and dot */
2543         vsrx= vec_perm(vrx, vrx, rotate);
2544         vsry= vec_perm(vry, vry, rotate);
2545         vsrz= vec_perm(vrz, vrz, rotate);
2546
2547         /* cross product */
2548         gx = vsry*vrz - vsrz*vry;
2549         gy = vsrz*vrx - vsrx*vrz;
2550         gz = vsrx*vry - vsry*vrx;
2551
2552         /* normalize */
2553         rlen = vec_rsqrte(gx*gx + gy*gy + gz*gz + vec_splat_float(1e-16f));
2554         gx = gx*rlen;
2555         gy = gy*rlen;
2556         gz = gz*rlen;
2557
2558         /* angle */
2559         vcos = vrx*vsrx + vry*vsry + vrz*vsrz;
2560         vcos= vec_max(vec_min(vcos, vec_splat_float(1.0f)), vec_splat_float(-1.0f));
2561         vangle= vacosf(vcos);
2562
2563         /* dot */
2564         vresult.v = (vec_splat_float(n[0])*gx +
2565                      vec_splat_float(n[1])*gy +
2566                      vec_splat_float(n[2])*gz)*vangle;
2567
2568         result= (vresult.f[0] + vresult.f[1] + vresult.f[2] + vresult.f[3])*(0.5f/(float)M_PI);
2569         result= MAX2(result, 0.0f);
2570
2571         return result;
2572 }
2573
2574 #endif
2575
2576 /* SSE optimization, acos code doesn't work */
2577
2578 #if 0
2579
2580 #include <xmmintrin.h>
2581
2582 static __m128 sse_approx_acos(__m128 x)
2583 {
2584         /* needs a better approximation than taylor expansion of acos, since that
2585          * gives big erros for near 1.0 values, sqrt(2*x)*acos(1-x) should work
2586          * better, see http://www.tom.womack.net/projects/sse-fast-arctrig.html */
2587
2588         return _mm_set_ps1(1.0f);
2589 }
2590
2591 static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float *q2, float *q3)
2592 {
2593         float r0[3], r1[3], r2[3], r3[3], g0[3], g1[3], g2[3], g3[3];
2594         float a1, a2, a3, a4, dot1, dot2, dot3, dot4, result;
2595         float fresult[4] __attribute__((aligned(16)));
2596         __m128 qx, qy, qz, rx, ry, rz, rlen, srx, sry, srz, gx, gy, gz, glen, rcos, angle, aresult;
2597
2598         /* compute r */
2599         qx = _mm_set_ps(q3[0], q2[0], q1[0], q0[0]);
2600         qy = _mm_set_ps(q3[1], q2[1], q1[1], q0[1]);
2601         qz = _mm_set_ps(q3[2], q2[2], q1[2], q0[2]);
2602
2603         rx = qx - _mm_set_ps1(p[0]);
2604         ry = qy - _mm_set_ps1(p[1]);
2605         rz = qz - _mm_set_ps1(p[2]);
2606
2607         /* normalize r */
2608         rlen = _mm_rsqrt_ps(rx*rx + ry*ry + rz*rz + _mm_set_ps1(1e-16f));
2609         rx = rx*rlen;
2610         ry = ry*rlen;
2611         rz = rz*rlen;
2612
2613         /* cross product */
2614         srx = _mm_shuffle_ps(rx, rx, _MM_SHUFFLE(0,3,2,1));
2615         sry = _mm_shuffle_ps(ry, ry, _MM_SHUFFLE(0,3,2,1));
2616         srz = _mm_shuffle_ps(rz, rz, _MM_SHUFFLE(0,3,2,1));
2617
2618         gx = sry*rz - srz*ry;
2619         gy = srz*rx - srx*rz;
2620         gz = srx*ry - sry*rx;
2621
2622         /* normalize g */
2623         glen = _mm_rsqrt_ps(gx*gx + gy*gy + gz*gz + _mm_set_ps1(1e-16f));
2624         gx = gx*glen;
2625         gy = gy*glen;
2626         gz = gz*glen;
2627
2628         /* compute angle */
2629         rcos = rx*srx + ry*sry + rz*srz;
2630         rcos= _mm_max_ps(_mm_min_ps(rcos, _mm_set_ps1(1.0f)), _mm_set_ps1(-1.0f));
2631
2632         angle = sse_approx_cos(rcos);
2633         aresult = (_mm_set_ps1(n[0])*gx + _mm_set_ps1(n[1])*gy + _mm_set_ps1(n[2])*gz)*angle;
2634
2635         /* sum together */
2636         result= (fresult[0] + fresult[1] + fresult[2] + fresult[3])*(0.5f/(float)M_PI);
2637         result= MAX2(result, 0.0f);
2638
2639         return result;
2640 }
2641
2642 #endif
2643
2644 static void ff_normalize(float n[3])
2645 {
2646         float d;
2647         
2648         d= dot_v3v3(n, n);
2649
2650         if(d > 1.0e-35F) {
2651                 d= 1.0f/sqrtf(d);
2652
2653                 n[0] *= d; 
2654                 n[1] *= d; 
2655                 n[2] *= d;
2656         } 
2657 }
2658
2659 static float ff_quad_form_factor(const float p[3], const float n[3], const float q0[3], const float q1[3], const float q2[3], const float q3[3])
2660 {
2661         float r0[3], r1[3], r2[3], r3[3], g0[3], g1[3], g2[3], g3[3];
2662         float a1, a2, a3, a4, dot1, dot2, dot3, dot4, result;
2663
2664         sub_v3_v3v3(r0, q0, p);
2665         sub_v3_v3v3(r1, q1, p);
2666         sub_v3_v3v3(r2, q2, p);
2667         sub_v3_v3v3(r3, q3, p);
2668
2669         ff_normalize(r0);
2670         ff_normalize(r1);
2671         ff_normalize(r2);
2672         ff_normalize(r3);
2673
2674         cross_v3_v3v3(g0, r1, r0); ff_normalize(g0);
2675         cross_v3_v3v3(g1, r2, r1); ff_normalize(g1);
2676         cross_v3_v3v3(g2, r3, r2); ff_normalize(g2);
2677         cross_v3_v3v3(g3, r0, r3); ff_normalize(g3);
2678
2679         a1= saacosf(dot_v3v3(r0, r1));
2680         a2= saacosf(dot_v3v3(r1, r2));
2681         a3= saacosf(dot_v3v3(r2, r3));
2682         a4= saacosf(dot_v3v3(r3, r0));
2683
2684         dot1= dot_v3v3(n, g0);
2685         dot2= dot_v3v3(n, g1);
2686         dot3= dot_v3v3(n, g2);
2687         dot4= dot_v3v3(n, g3);
2688
2689         result= (a1*dot1 + a2*dot2 + a3*dot3 + a4*dot4)*0.5f/(float)M_PI;
2690         result= MAX2(result, 0.0f);
2691
2692         return result;
2693 }
2694
2695 float form_factor_hemi_poly(float p[3], float n[3], float v1[3], float v2[3], float v3[3], float v4[3])
2696 {
2697         /* computes how much hemisphere defined by point and normal is
2698            covered by a quad or triangle, cosine weighted */
2699         float q0[3], q1[3], q2[3], q3[3], contrib= 0.0f;
2700
2701         if(ff_visible_quad(p, n, v1, v2, v3, q0, q1, q2, q3))
2702                 contrib += ff_quad_form_factor(p, n, q0, q1, q2, q3);
2703         
2704         if(v4 && ff_visible_quad(p, n, v1, v3, v4, q0, q1, q2, q3))
2705                 contrib += ff_quad_form_factor(p, n, q0, q1, q2, q3);
2706
2707         return contrib;
2708 }
2709
2710 /* evaluate if entire quad is a proper convex quad */
2711  int is_quad_convex_v3(const float *v1, const float *v2, const float *v3, const float *v4)
2712  {
2713         float nor[3], nor1[3], nor2[3], vec[4][2];
2714         
2715         /* define projection, do both trias apart, quad is undefined! */
2716         normal_tri_v3( nor1,v1, v2, v3);
2717         normal_tri_v3( nor2,v1, v3, v4);
2718         nor[0]= ABS(nor1[0]) + ABS(nor2[0]);
2719         nor[1]= ABS(nor1[1]) + ABS(nor2[1]);
2720         nor[2]= ABS(nor1[2]) + ABS(nor2[2]);
2721
2722         if(nor[2] >= nor[0] && nor[2] >= nor[1]) {
2723                 vec[0][0]= v1[0]; vec[0][1]= v1[1];
2724                 vec[1][0]= v2[0]; vec[1][1]= v2[1];
2725                 vec[2][0]= v3[0]; vec[2][1]= v3[1];
2726                 vec[3][0]= v4[0]; vec[3][1]= v4[1];
2727         }
2728         else if(nor[1] >= nor[0] && nor[1]>= nor[2]) {
2729                 vec[0][0]= v1[0]; vec[0][1]= v1[2];
2730                 vec[1][0]= v2[0]; vec[1][1]= v2[2];
2731                 vec[2][0]= v3[0]; vec[2][1]= v3[2];
2732                 vec[3][0]= v4[0]; vec[3][1]= v4[2];
2733         }
2734         else {
2735                 vec[0][0]= v1[1]; vec[0][1]= v1[2];
2736                 vec[1][0]= v2[1]; vec[1][1]= v2[2];
2737                 vec[2][0]= v3[1]; vec[2][1]= v3[2];
2738                 vec[3][0]= v4[1]; vec[3][1]= v4[2];
2739         }
2740         
2741         /* linetests, the 2 diagonals have to instersect to be convex */
2742         if( isect_line_line_v2(vec[0], vec[2], vec[1], vec[3]) > 0 ) return 1;
2743         return 0;
2744 }