5979a24c807af73649f1ae54283b0e876a829311
[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 static float lambda_cp_line(const float p[3], const float l1[3], const float l2[3]);
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 int isect_line_plane_v3(float out[3], const float l1[3], const float l2[3], const float plane_co[3], const float plane_no[3], const short no_flip)
644 {
645         float l_vec[3]; /* l1 -> l2 normalized vector */
646         float p_no[3]; /* 'plane_no' normalized */
647         float dot;
648
649         sub_v3_v3v3(l_vec, l2, l1);
650
651         normalize_v3(l_vec);
652         normalize_v3_v3(p_no, plane_no);
653
654         dot= dot_v3v3(l_vec, p_no);
655         if(dot == 0.0f) {
656                 return 0;
657         }
658         else {
659                 float l1_plane[3]; /* line point aligned with the plane */
660                 float dist; /* 'plane_no' aligned distance to the 'plane_co' */
661
662                 /* for pradictable flipping since the plane is only used to
663                  * define a direction, ignore its flipping and aligned with 'l_vec' */
664                 if(dot < 0.0f) {
665                         dot= -dot;
666                         negate_v3(p_no);
667                 }
668
669                 add_v3_v3v3(l1_plane, l1, p_no);
670
671                 dist = lambda_cp_line(plane_co, l1, l1_plane);
672
673                 /* treat line like a ray, when 'no_flip' is set */
674                 if(no_flip && dist < 0.0f) {
675                         dist= -dist;
676                 }
677
678                 mul_v3_fl(l_vec, dist / dot);
679
680                 add_v3_v3v3(out, l1, l_vec);
681
682                 return 1;
683         }
684 }
685
686 /* Adapted from the paper by Kasper Fauerby */
687 /* "Improved Collision detection and Response" */
688 static int getLowestRoot(const float a, const float b, const float c, const float maxR, float *root)
689 {
690         // Check if a solution exists
691         float determinant = b*b - 4.0f*a*c;
692
693         // If determinant is negative it means no solutions.
694         if (determinant >= 0.0f)
695         {
696                 // calculate the two roots: (if determinant == 0 then
697                 // x1==x2 but lets disregard that slight optimization)
698                 float sqrtD = (float)sqrt(determinant);
699                 float r1 = (-b - sqrtD) / (2.0f*a);
700                 float r2 = (-b + sqrtD) / (2.0f*a);
701                 
702                 // Sort so x1 <= x2
703                 if (r1 > r2)
704                         SWAP(float, r1, r2);
705
706                 // Get lowest root:
707                 if (r1 > 0.0f && r1 < maxR)
708                 {
709                         *root = r1;
710                         return 1;
711                 }
712
713                 // It is possible that we want x2 - this can happen
714                 // if x1 < 0
715                 if (r2 > 0.0f && r2 < maxR)
716                 {
717                         *root = r2;
718                         return 1;
719                 }
720         }
721         // No (valid) solutions
722         return 0;
723 }
724
725 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])
726 {
727         float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3];
728         float a, b, c, d, e, x, y, z, radius2=radius*radius;
729         float elen2,edotv,edotbv,nordotv;
730         float newLambda;
731         int found_by_sweep=0;
732
733         sub_v3_v3v3(e1,v1,v0);
734         sub_v3_v3v3(e2,v2,v0);
735         sub_v3_v3v3(vel,p2,p1);
736
737 /*---test plane of tri---*/
738         cross_v3_v3v3(nor,e1,e2);
739         normalize_v3(nor);
740
741         /* flip normal */
742         if(dot_v3v3(nor,vel)>0.0f) negate_v3(nor);
743         
744         a=dot_v3v3(p1,nor)-dot_v3v3(v0,nor);
745         nordotv=dot_v3v3(nor,vel);
746
747         if (fabsf(nordotv) < 0.000001f)
748         {
749                 if(fabsf(a) >= radius) {
750                         return 0;
751                 }
752         }
753         else
754         {
755                 float t0=(-a+radius)/nordotv;
756                 float t1=(-a-radius)/nordotv;
757
758                 if(t0>t1)
759                         SWAP(float, t0, t1);
760
761                 if(t0>1.0f || t1<0.0f) return 0;
762
763                 /* clamp to [0,1] */
764                 CLAMP(t0, 0.0f, 1.0f);
765                 CLAMP(t1, 0.0f, 1.0f);
766
767                 /*---test inside of tri---*/
768                 /* plane intersection point */
769
770                 point[0] = p1[0] + vel[0]*t0 - nor[0]*radius;
771                 point[1] = p1[1] + vel[1]*t0 - nor[1]*radius;
772                 point[2] = p1[2] + vel[2]*t0 - nor[2]*radius;
773
774
775                 /* is the point in the tri? */
776                 a=dot_v3v3(e1,e1);
777                 b=dot_v3v3(e1,e2);
778                 c=dot_v3v3(e2,e2);
779
780                 sub_v3_v3v3(temp,point,v0);
781                 d=dot_v3v3(temp,e1);
782                 e=dot_v3v3(temp,e2);
783                 
784                 x=d*c-e*b;
785                 y=e*a-d*b;
786                 z=x+y-(a*c-b*b);
787
788
789                 if(z <= 0.0f && (x >= 0.0f && y >= 0.0f))
790                 {
791                 //(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000){
792                         *lambda=t0;
793                         copy_v3_v3(ipoint,point);
794                         return 1;
795                 }
796         }
797
798
799         *lambda=1.0f;
800
801 /*---test points---*/
802         a=dot_v3v3(vel,vel);
803
804         /*v0*/
805         sub_v3_v3v3(temp,p1,v0);
806         b=2.0f*dot_v3v3(vel,temp);
807         c=dot_v3v3(temp,temp)-radius2;
808
809         if(getLowestRoot(a, b, c, *lambda, lambda))
810         {
811                 copy_v3_v3(ipoint,v0);
812                 found_by_sweep=1;
813         }
814
815         /*v1*/
816         sub_v3_v3v3(temp,p1,v1);
817         b=2.0f*dot_v3v3(vel,temp);
818         c=dot_v3v3(temp,temp)-radius2;
819
820         if(getLowestRoot(a, b, c, *lambda, lambda))
821         {
822                 copy_v3_v3(ipoint,v1);
823                 found_by_sweep=1;
824         }
825         
826         /*v2*/
827         sub_v3_v3v3(temp,p1,v2);
828         b=2.0f*dot_v3v3(vel,temp);
829         c=dot_v3v3(temp,temp)-radius2;
830
831         if(getLowestRoot(a, b, c, *lambda, lambda))
832         {
833                 copy_v3_v3(ipoint,v2);
834                 found_by_sweep=1;
835         }
836
837 /*---test edges---*/
838         sub_v3_v3v3(e3,v2,v1); //wasnt yet calculated
839
840
841         /*e1*/
842         sub_v3_v3v3(bv,v0,p1);
843
844         elen2 = dot_v3v3(e1,e1);
845         edotv = dot_v3v3(e1,vel);
846         edotbv = dot_v3v3(e1,bv);
847
848         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
849         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
850         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
851
852         if(getLowestRoot(a, b, c, *lambda, &newLambda))
853         {
854                 e=(edotv*newLambda-edotbv)/elen2;
855
856                 if(e >= 0.0f && e <= 1.0f)
857                 {
858                         *lambda = newLambda;
859                         copy_v3_v3(ipoint,e1);
860                         mul_v3_fl(ipoint,e);
861                         add_v3_v3(ipoint, v0);
862                         found_by_sweep=1;
863                 }
864         }
865
866         /*e2*/
867         /*bv is same*/
868         elen2 = dot_v3v3(e2,e2);
869         edotv = dot_v3v3(e2,vel);
870         edotbv = dot_v3v3(e2,bv);
871
872         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
873         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
874         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
875
876         if(getLowestRoot(a, b, c, *lambda, &newLambda))
877         {
878                 e=(edotv*newLambda-edotbv)/elen2;
879
880                 if(e >= 0.0f && e <= 1.0f)
881                 {
882                         *lambda = newLambda;
883                         copy_v3_v3(ipoint,e2);
884                         mul_v3_fl(ipoint,e);
885                         add_v3_v3(ipoint, v0);
886                         found_by_sweep=1;
887                 }
888         }
889
890         /*e3*/
891         /* sub_v3_v3v3(bv,v0,p1); */ /* UNUSED */
892         /* elen2 = dot_v3v3(e1,e1); */ /* UNUSED */
893         /* edotv = dot_v3v3(e1,vel); */ /* UNUSED */
894         /* edotbv = dot_v3v3(e1,bv); */ /* UNUSED */
895
896         sub_v3_v3v3(bv,v1,p1);
897         elen2 = dot_v3v3(e3,e3);
898         edotv = dot_v3v3(e3,vel);
899         edotbv = dot_v3v3(e3,bv);
900
901         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
902         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
903         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
904
905         if(getLowestRoot(a, b, c, *lambda, &newLambda))
906         {
907                 e=(edotv*newLambda-edotbv)/elen2;
908
909                 if(e >= 0.0f && e <= 1.0f)
910                 {
911                         *lambda = newLambda;
912                         copy_v3_v3(ipoint,e3);
913                         mul_v3_fl(ipoint,e);
914                         add_v3_v3(ipoint, v1);
915                         found_by_sweep=1;
916                 }
917         }
918
919
920         return found_by_sweep;
921 }
922 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)
923 {
924         float p[3], e1[3], e2[3];
925         float u, v, f;
926         int a0=axis, a1=(axis+1)%3, a2=(axis+2)%3;
927
928         //return isect_line_tri_v3(p1,p2,v0,v1,v2,lambda);
929
930         ///* first a simple bounding box test */
931         //if(MIN3(v0[a1],v1[a1],v2[a1]) > p1[a1]) return 0;
932         //if(MIN3(v0[a2],v1[a2],v2[a2]) > p1[a2]) return 0;
933         //if(MAX3(v0[a1],v1[a1],v2[a1]) < p1[a1]) return 0;
934         //if(MAX3(v0[a2],v1[a2],v2[a2]) < p1[a2]) return 0;
935
936         ///* then a full intersection test */
937         
938         sub_v3_v3v3(e1,v1,v0);
939         sub_v3_v3v3(e2,v2,v0);
940         sub_v3_v3v3(p,v0,p1);
941
942         f= (e2[a1]*e1[a2]-e2[a2]*e1[a1]);
943         if ((f > -0.000001f) && (f < 0.000001f)) return 0;
944
945         v= (p[a2]*e1[a1]-p[a1]*e1[a2])/f;
946         if ((v < 0.0f)||(v > 1.0f)) return 0;
947         
948         f= e1[a1];
949         if((f > -0.000001f) && (f < 0.000001f)){
950                 f= e1[a2];
951                 if((f > -0.000001f) && (f < 0.000001f)) return 0;
952                 u= (-p[a2]-v*e2[a2])/f;
953         }
954         else
955                 u= (-p[a1]-v*e2[a1])/f;
956
957         if ((u < 0.0f) || ((u + v) > 1.0f)) return 0;
958
959         *lambda = (p[a0]+u*e1[a0]+v*e2[a0])/(p2[a0]-p1[a0]);
960
961         if ((*lambda < 0.0f) || (*lambda > 1.0f)) return 0;
962
963         return 1;
964 }
965
966 /* Returns the number of point of interests
967  * 0 - lines are colinear
968  * 1 - lines are coplanar, i1 is set to intersection
969  * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively 
970  * */
971 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])
972 {
973         float a[3], b[3], c[3], ab[3], cb[3], dir1[3], dir2[3];
974         float d;
975         
976         sub_v3_v3v3(c, v3, v1);
977         sub_v3_v3v3(a, v2, v1);
978         sub_v3_v3v3(b, v4, v3);
979
980         normalize_v3_v3(dir1, a);
981         normalize_v3_v3(dir2, b);
982         d = dot_v3v3(dir1, dir2);
983         if (d == 1.0f || d == -1.0f) {
984                 /* colinear */
985                 return 0;
986         }
987
988         cross_v3_v3v3(ab, a, b);
989         d = dot_v3v3(c, ab);
990
991         /* test if the two lines are coplanar */
992         if (d > -0.000001f && d < 0.000001f) {
993                 cross_v3_v3v3(cb, c, b);
994
995                 mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
996                 add_v3_v3v3(i1, v1, a);
997                 copy_v3_v3(i2, i1);
998                 
999                 return 1; /* one intersection only */
1000         }
1001         /* if not */
1002         else {
1003                 float n[3], t[3];
1004                 float v3t[3], v4t[3];
1005                 sub_v3_v3v3(t, v1, v3);
1006
1007                 /* offset between both plane where the lines lies */
1008                 cross_v3_v3v3(n, a, b);
1009                 project_v3_v3v3(t, t, n);
1010
1011                 /* for the first line, offset the second line until it is coplanar */
1012                 add_v3_v3v3(v3t, v3, t);
1013                 add_v3_v3v3(v4t, v4, t);
1014                 
1015                 sub_v3_v3v3(c, v3t, v1);
1016                 sub_v3_v3v3(a, v2, v1);
1017                 sub_v3_v3v3(b, v4t, v3t);
1018
1019                 cross_v3_v3v3(ab, a, b);
1020                 cross_v3_v3v3(cb, c, b);
1021
1022                 mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
1023                 add_v3_v3v3(i1, v1, a);
1024
1025                 /* for the second line, just substract the offset from the first intersection point */
1026                 sub_v3_v3v3(i2, i1, t);
1027                 
1028                 return 2; /* two nearest points */
1029         }
1030
1031
1032 /* Intersection point strictly between the two lines
1033  * 0 when no intersection is found 
1034  * */
1035 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)
1036 {
1037         float a[3], b[3], c[3], ab[3], cb[3], ca[3], dir1[3], dir2[3];
1038         float d;
1039         
1040         sub_v3_v3v3(c, v3, v1);
1041         sub_v3_v3v3(a, v2, v1);
1042         sub_v3_v3v3(b, v4, v3);
1043
1044         normalize_v3_v3(dir1, a);
1045         normalize_v3_v3(dir2, b);
1046         d = dot_v3v3(dir1, dir2);
1047         if (d == 1.0f || d == -1.0f || d == 0) {
1048                 /* colinear or one vector is zero-length*/
1049                 return 0;
1050         }
1051
1052         cross_v3_v3v3(ab, a, b);
1053         d = dot_v3v3(c, ab);
1054
1055         /* test if the two lines are coplanar */
1056         if (d > -0.000001f && d < 0.000001f) {
1057                 float f1, f2;
1058                 cross_v3_v3v3(cb, c, b);
1059                 cross_v3_v3v3(ca, c, a);
1060
1061                 f1 = dot_v3v3(cb, ab) / dot_v3v3(ab, ab);
1062                 f2 = dot_v3v3(ca, ab) / dot_v3v3(ab, ab);
1063                 
1064                 if (f1 >= 0 && f1 <= 1 &&
1065                         f2 >= 0 && f2 <= 1)
1066                 {
1067                         mul_v3_fl(a, f1);
1068                         add_v3_v3v3(vi, v1, a);
1069                         
1070                         if (lambda != NULL)
1071                         {
1072                                 *lambda = f1;
1073                         }
1074                         
1075                         return 1; /* intersection found */
1076                 }
1077                 else
1078                 {
1079                         return 0;
1080                 }
1081         }
1082         else
1083         {
1084                 return 0;
1085         }
1086
1087
1088 int isect_aabb_aabb_v3(const float min1[3], const float max1[3], const float min2[3], const float max2[3])
1089 {
1090         return (min1[0]<max2[0] && min1[1]<max2[1] && min1[2]<max2[2] &&
1091                         min2[0]<max1[0] && min2[1]<max1[1] && min2[2]<max1[2]);
1092 }
1093
1094 /* find closest point to p on line through l1,l2 and return lambda,
1095  * where (0 <= lambda <= 1) when cp is in the line segement l1,l2
1096  */
1097 float closest_to_line_v3(float cp[3], const float p[3], const float l1[3], const float l2[3])
1098 {
1099         float h[3],u[3],lambda;
1100         sub_v3_v3v3(u, l2, l1);
1101         sub_v3_v3v3(h, p, l1);
1102         lambda =dot_v3v3(u,h)/dot_v3v3(u,u);
1103         cp[0] = l1[0] + u[0] * lambda;
1104         cp[1] = l1[1] + u[1] * lambda;
1105         cp[2] = l1[2] + u[2] * lambda;
1106         return lambda;
1107 }
1108
1109 float closest_to_line_v2(float cp[2],const float p[2], const float l1[2], const float l2[2])
1110 {
1111         float h[2],u[2],lambda;
1112         sub_v2_v2v2(u, l2, l1);
1113         sub_v2_v2v2(h, p, l1);
1114         lambda =dot_v2v2(u,h)/dot_v2v2(u,u);
1115         cp[0] = l1[0] + u[0] * lambda;
1116         cp[1] = l1[1] + u[1] * lambda;
1117         return lambda;
1118 }
1119
1120 /* little sister we only need to know lambda */
1121 static float lambda_cp_line(const float p[3], const float l1[3], const float l2[3])
1122 {
1123         float h[3],u[3];
1124         sub_v3_v3v3(u, l2, l1);
1125         sub_v3_v3v3(h, p, l1);
1126         return(dot_v3v3(u,h)/dot_v3v3(u,u));
1127 }
1128
1129 /* Similar to LineIntersectsTriangleUV, except it operates on a quad and in 2d, assumes point is in quad */
1130 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)
1131 {
1132         float x0,y0, x1,y1, wtot, v2d[2], w1, w2;
1133         
1134         /* used for parallel lines */
1135         float pt3d[3], l1[3], l2[3], pt_on_line[3];
1136         
1137         /* compute 2 edges  of the quad  intersection point */
1138         if (IsectLLPt2Df(v0[0],v0[1],v1[0],v1[1],  v2[0],v2[1],v3[0],v3[1], &x0,&y0) == 1) {
1139                 /* the intersection point between the quad-edge intersection and the point in the quad we want the uv's for */
1140                 /* should never be paralle !! */
1141                 /*printf("\tnot parallel 1\n");*/
1142                 IsectLLPt2Df(pt[0],pt[1],x0,y0,  v0[0],v0[1],v3[0],v3[1], &x1,&y1);
1143                 
1144                 /* Get the weights from the new intersection point, to each edge */
1145                 v2d[0] = x1-v0[0];
1146                 v2d[1] = y1-v0[1];
1147                 w1 = len_v2(v2d);
1148                 
1149                 v2d[0] = x1-v3[0]; /* some but for the other vert */
1150                 v2d[1] = y1-v3[1];
1151                 w2 = len_v2(v2d);
1152                 wtot = w1+w2;
1153                 /*w1 = w1/wtot;*/
1154                 /*w2 = w2/wtot;*/
1155                 uv[0] = w1/wtot;
1156         } else {
1157                 /* lines are parallel, lambda_cp_line_ex is 3d grrr */
1158                 /*printf("\tparallel1\n");*/
1159                 pt3d[0] = pt[0];
1160                 pt3d[1] = pt[1];
1161                 pt3d[2] = l1[2] = l2[2] = 0.0f;
1162                 
1163                 l1[0] = v0[0]; l1[1] = v0[1];
1164                 l2[0] = v1[0]; l2[1] = v1[1];
1165                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1166                 v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */
1167                 v2d[1] = pt[1]-pt_on_line[1];
1168                 w1 = len_v2(v2d);
1169                 
1170                 l1[0] = v2[0]; l1[1] = v2[1];
1171                 l2[0] = v3[0]; l2[1] = v3[1];
1172                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1173                 v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */
1174                 v2d[1] = pt[1]-pt_on_line[1];
1175                 w2 = len_v2(v2d);
1176                 wtot = w1+w2;
1177                 uv[0] = w1/wtot;        
1178         }
1179         
1180         /* Same as above to calc the uv[1] value, alternate calculation */
1181         
1182         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*/
1183                 /* never paralle if above was not */
1184                 /*printf("\tnot parallel2\n");*/
1185                 IsectLLPt2Df(pt[0],pt[1],x0,y0,  v0[0],v0[1],v1[0],v1[1], &x1,&y1);/* was v0,v3  now v0,v1*/
1186                 
1187                 v2d[0] = x1-v0[0];
1188                 v2d[1] = y1-v0[1];
1189                 w1 = len_v2(v2d);
1190                 
1191                 v2d[0] = x1-v1[0];
1192                 v2d[1] = y1-v1[1];
1193                 w2 = len_v2(v2d);
1194                 wtot = w1+w2;
1195                 uv[1] = w1/wtot;
1196         } else {
1197                 /* lines are parallel, lambda_cp_line_ex is 3d grrr */
1198                 /*printf("\tparallel2\n");*/
1199                 pt3d[0] = pt[0];
1200                 pt3d[1] = pt[1];
1201                 pt3d[2] = l1[2] = l2[2] = 0.0f;
1202                 
1203                 
1204                 l1[0] = v0[0]; l1[1] = v0[1];
1205                 l2[0] = v3[0]; l2[1] = v3[1];
1206                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1207                 v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */
1208                 v2d[1] = pt[1]-pt_on_line[1];
1209                 w1 = len_v2(v2d);
1210                 
1211                 l1[0] = v1[0]; l1[1] = v1[1];
1212                 l2[0] = v2[0]; l2[1] = v2[1];
1213                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1214                 v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */
1215                 v2d[1] = pt[1]-pt_on_line[1];
1216                 w2 = len_v2(v2d);
1217                 wtot = w1+w2;
1218                 uv[1] = w1/wtot;
1219         }
1220         /* may need to flip UV's here */
1221 }
1222
1223 /* same as above but does tri's and quads, tri's are a bit of a hack */
1224 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)
1225 {
1226         if (isquad) {
1227                 isect_point_quad_uv_v2(v0, v1, v2, v3, pt, uv);
1228         }
1229         else {
1230                 /* not for quads, use for our abuse of LineIntersectsTriangleUV */
1231                 float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3], lambda;
1232                         
1233                 p1_3d[0] = p2_3d[0] = uv[0];
1234                 p1_3d[1] = p2_3d[1] = uv[1];
1235                 p1_3d[2] = 1.0f;
1236                 p2_3d[2] = -1.0f;
1237                 v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0;
1238                 
1239                 /* generate a new fuv, (this is possibly a non optimal solution,
1240                  * since we only need 2d calculation but use 3d func's)
1241                  * 
1242                  * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face
1243                  * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV.
1244                  * This means the new values will be correct in relation to the derived meshes face. 
1245                  */
1246                 copy_v2_v2(v0_3d, v0);
1247                 copy_v2_v2(v1_3d, v1);
1248                 copy_v2_v2(v2_3d, v2);
1249                 
1250                 /* Doing this in 3D is not nice */
1251                 isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv);
1252         }
1253 }
1254
1255 #if 0 // XXX this version used to be used in isect_point_tri_v2_int() and was called IsPointInTri2D
1256 int isect_point_tri_v2(float pt[2], float v1[2], float v2[2], float v3[2])
1257 {
1258         float inp1, inp2, inp3;
1259         
1260         inp1= (v2[0]-v1[0])*(v1[1]-pt[1]) + (v1[1]-v2[1])*(v1[0]-pt[0]);
1261         inp2= (v3[0]-v2[0])*(v2[1]-pt[1]) + (v2[1]-v3[1])*(v2[0]-pt[0]);
1262         inp3= (v1[0]-v3[0])*(v3[1]-pt[1]) + (v3[1]-v1[1])*(v3[0]-pt[0]);
1263         
1264         if(inp1<=0.0f && inp2<=0.0f && inp3<=0.0f) return 1;
1265         if(inp1>=0.0f && inp2>=0.0f && inp3>=0.0f) return 1;
1266         
1267         return 0;
1268 }
1269 #endif
1270
1271 #if 0
1272 int isect_point_tri_v2(float v0[2], float v1[2], float v2[2], float pt[2])
1273 {
1274                 /* not for quads, use for our abuse of LineIntersectsTriangleUV */
1275                 float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3];
1276                 /* not used */
1277                 float lambda, uv[3];
1278                         
1279                 p1_3d[0] = p2_3d[0] = uv[0]= pt[0];
1280                 p1_3d[1] = p2_3d[1] = uv[1]= uv[2]= pt[1];
1281                 p1_3d[2] = 1.0f;
1282                 p2_3d[2] = -1.0f;
1283                 v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0;
1284                 
1285                 /* generate a new fuv, (this is possibly a non optimal solution,
1286                  * since we only need 2d calculation but use 3d func's)
1287                  * 
1288                  * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face
1289                  * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV.
1290                  * This means the new values will be correct in relation to the derived meshes face. 
1291                  */
1292                 copy_v2_v2(v0_3d, v0);
1293                 copy_v2_v2(v1_3d, v1);
1294                 copy_v2_v2(v2_3d, v2);
1295                 
1296                 /* Doing this in 3D is not nice */
1297                 return isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv);
1298 }
1299 #endif
1300
1301 /*
1302
1303         x1,y2
1304         |  \
1305         |   \     .(a,b)
1306         |    \
1307         x1,y1-- x2,y1
1308
1309 */
1310 int isect_point_tri_v2_int(const int x1, const int y1, const int x2, const int y2, const int a, const int b)
1311 {
1312         float v1[2], v2[2], v3[2], p[2];
1313         
1314         v1[0]= (float)x1;
1315         v1[1]= (float)y1;
1316         
1317         v2[0]= (float)x1;
1318         v2[1]= (float)y2;
1319         
1320         v3[0]= (float)x2;
1321         v3[1]= (float)y1;
1322         
1323         p[0]= (float)a;
1324         p[1]= (float)b;
1325         
1326         return isect_point_tri_v2(p, v1, v2, v3);
1327 }
1328
1329 static int point_in_slice(const float p[3], const float v1[3], const float l1[3], const float l2[3])
1330 {
1331 /* 
1332 what is a slice ?
1333 some maths:
1334 a line including l1,l2 and a point not on the line 
1335 define a subset of R3 delimeted by planes parallel to the line and orthogonal 
1336 to the (point --> line) distance vector,one plane on the line one on the point, 
1337 the room inside usually is rather small compared to R3 though still infinte
1338 useful for restricting (speeding up) searches 
1339 e.g. all points of triangular prism are within the intersection of 3 'slices'
1340 onother trivial case : cube 
1341 but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
1342 */
1343         float h,rp[3],cp[3],q[3];
1344
1345         closest_to_line_v3(cp,v1,l1,l2);
1346         sub_v3_v3v3(q,cp,v1);
1347
1348         sub_v3_v3v3(rp,p,v1);
1349         h=dot_v3v3(q,rp)/dot_v3v3(q,q);
1350         if (h < 0.0f || h > 1.0f) return 0;
1351         return 1;
1352 }
1353
1354 #if 0
1355 /*adult sister defining the slice planes by the origin and the normal  
1356 NOTE |normal| may not be 1 but defining the thickness of the slice*/
1357 static int point_in_slice_as(float p[3],float origin[3],float normal[3])
1358 {
1359         float h,rp[3];
1360         sub_v3_v3v3(rp,p,origin);
1361         h=dot_v3v3(normal,rp)/dot_v3v3(normal,normal);
1362         if (h < 0.0f || h > 1.0f) return 0;
1363         return 1;
1364 }
1365
1366 /*mama (knowing the squared length of the normal)*/
1367 static int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns)
1368 {
1369         float h,rp[3];
1370         sub_v3_v3v3(rp,p,origin);
1371         h=dot_v3v3(normal,rp)/lns;
1372         if (h < 0.0f || h > 1.0f) return 0;
1373         return 1;
1374 }
1375 #endif
1376
1377 int isect_point_tri_prism_v3(const float p[3], const float v1[3], const float v2[3], const float v3[3])
1378 {
1379         if(!point_in_slice(p,v1,v2,v3)) return 0;
1380         if(!point_in_slice(p,v2,v3,v1)) return 0;
1381         if(!point_in_slice(p,v3,v1,v2)) return 0;
1382         return 1;
1383 }
1384
1385 int clip_line_plane(float p1[3], float p2[3], const float plane[4])
1386 {
1387         float dp[3], n[3], div, t, pc[3];
1388
1389         copy_v3_v3(n, plane);
1390         sub_v3_v3v3(dp, p2, p1);
1391         div= dot_v3v3(dp, n);
1392
1393         if(div == 0.0f) /* parallel */
1394                 return 1;
1395
1396         t= -(dot_v3v3(p1, n) + plane[3])/div;
1397
1398         if(div > 0.0f) {
1399                 /* behind plane, completely clipped */
1400                 if(t >= 1.0f) {
1401                         zero_v3(p1);
1402                         zero_v3(p2);
1403                         return 0;
1404                 }
1405
1406                 /* intersect plane */
1407                 if(t > 0.0f) {
1408                         madd_v3_v3v3fl(pc, p1, dp, t);
1409                         copy_v3_v3(p1, pc);
1410                         return 1;
1411                 }
1412
1413                 return 1;
1414         }
1415         else {
1416                 /* behind plane, completely clipped */
1417                 if(t <= 0.0f) {
1418                         zero_v3(p1);
1419                         zero_v3(p2);
1420                         return 0;
1421                 }
1422
1423                 /* intersect plane */
1424                 if(t < 1.0f) {
1425                         madd_v3_v3v3fl(pc, p1, dp, t);
1426                         copy_v3_v3(p2, pc);
1427                         return 1;
1428                 }
1429
1430                 return 1;
1431         }
1432 }
1433
1434
1435 void plot_line_v2v2i(const int p1[2], const int p2[2], int (*callback)(int, int, void *), void *userData)
1436 {
1437         int x1= p1[0];
1438         int y1= p1[1];
1439         int x2= p2[0];
1440         int y2= p2[1];
1441
1442         signed char ix;
1443         signed char iy;
1444
1445         // if x1 == x2 or y1 == y2, then it does not matter what we set here
1446         int delta_x = (x2 > x1?(ix = 1, x2 - x1):(ix = -1, x1 - x2)) << 1;
1447         int delta_y = (y2 > y1?(iy = 1, y2 - y1):(iy = -1, y1 - y2)) << 1;
1448
1449         if(callback(x1, y1, userData) == 0) {
1450                 return;
1451         }
1452
1453         if (delta_x >= delta_y) {
1454                 // error may go below zero
1455                 int error = delta_y - (delta_x >> 1);
1456
1457                 while (x1 != x2) {
1458                         if (error >= 0) {
1459                                 if (error || (ix > 0)) {
1460                                         y1 += iy;
1461                                         error -= delta_x;
1462                                 }
1463                                 // else do nothing
1464                         }
1465                         // else do nothing
1466
1467                         x1 += ix;
1468                         error += delta_y;
1469
1470                         if(callback(x1, y1, userData) == 0) {
1471                                 return ;
1472                         }
1473                 }
1474         }
1475         else {
1476                 // error may go below zero
1477                 int error = delta_x - (delta_y >> 1);
1478
1479                 while (y1 != y2) {
1480                         if (error >= 0) {
1481                                 if (error || (iy > 0)) {
1482                                         x1 += ix;
1483                                         error -= delta_y;
1484                                 }
1485                                 // else do nothing
1486                         }
1487                         // else do nothing
1488
1489                         y1 += iy;
1490                         error += delta_x;
1491
1492                         if(callback(x1, y1, userData) == 0) {
1493                                 return;
1494                         }
1495                 }
1496         }
1497 }
1498
1499 /****************************** Interpolation ********************************/
1500
1501 static float tri_signed_area(const float v1[3], const float v2[3], const float v3[3], const int i, const int j)
1502 {
1503         return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
1504 }
1505
1506 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])
1507 {
1508         float xn, yn, zn, a1, a2, a3, asum;
1509         short i, j;
1510
1511         /* find best projection of face XY, XZ or YZ: barycentric weights of
1512            the 2d projected coords are the same and faster to compute */
1513         xn= (float)fabs(n[0]);
1514         yn= (float)fabs(n[1]);
1515         zn= (float)fabs(n[2]);
1516         if(zn>=xn && zn>=yn) {i= 0; j= 1;}
1517         else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
1518         else {i= 1; j= 2;} 
1519
1520         a1= tri_signed_area(v2, v3, co, i, j);
1521         a2= tri_signed_area(v3, v1, co, i, j);
1522         a3= tri_signed_area(v1, v2, co, i, j);
1523
1524         asum= a1 + a2 + a3;
1525
1526         if (fabsf(asum) < FLT_EPSILON) {
1527                 /* zero area triangle */
1528                 w[0]= w[1]= w[2]= 1.0f/3.0f;
1529                 return 1;
1530         }
1531
1532         asum= 1.0f/asum;
1533         w[0]= a1*asum;
1534         w[1]= a2*asum;
1535         w[2]= a3*asum;
1536
1537         return 0;
1538 }
1539
1540 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])
1541 {
1542         float w2[3];
1543
1544         w[0]= w[1]= w[2]= w[3]= 0.0f;
1545
1546         /* first check for exact match */
1547         if(equals_v3v3(co, v1))
1548                 w[0]= 1.0f;
1549         else if(equals_v3v3(co, v2))
1550                 w[1]= 1.0f;
1551         else if(equals_v3v3(co, v3))
1552                 w[2]= 1.0f;
1553         else if(v4 && equals_v3v3(co, v4))
1554                 w[3]= 1.0f;
1555         else {
1556                 /* otherwise compute barycentric interpolation weights */
1557                 float n1[3], n2[3], n[3];
1558                 int degenerate;
1559
1560                 sub_v3_v3v3(n1, v1, v3);
1561                 if (v4) {
1562                         sub_v3_v3v3(n2, v2, v4);
1563                 }
1564                 else {
1565                         sub_v3_v3v3(n2, v2, v3);
1566                 }
1567                 cross_v3_v3v3(n, n1, n2);
1568
1569                 /* OpenGL seems to split this way, so we do too */
1570                 if (v4) {
1571                         degenerate= barycentric_weights(v1, v2, v4, co, n, w);
1572                         SWAP(float, w[2], w[3]);
1573
1574                         if(degenerate || (w[0] < 0.0f)) {
1575                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
1576                                    so we interpolate using the other triangle */
1577                                 degenerate= barycentric_weights(v2, v3, v4, co, n, w2);
1578
1579                                 if(!degenerate) {
1580                                         w[0]= 0.0f;
1581                                         w[1]= w2[0];
1582                                         w[2]= w2[1];
1583                                         w[3]= w2[2];
1584                                 }
1585                         }
1586                 }
1587                 else
1588                         barycentric_weights(v1, v2, v3, co, n, w);
1589         }
1590 }
1591
1592 /* used by projection painting
1593  * note: using area_tri_signed_v2 means locations outside the triangle are correctly weighted */
1594 void barycentric_weights_v2(const float v1[2], const float v2[2], const float v3[2], const float co[2], float w[3])
1595 {
1596         float wtot_inv, wtot;
1597
1598         w[0] = area_tri_signed_v2(v2, v3, co);
1599         w[1] = area_tri_signed_v2(v3, v1, co);
1600         w[2] = area_tri_signed_v2(v1, v2, co);
1601         wtot = w[0]+w[1]+w[2];
1602
1603         if (wtot != 0.0f) {
1604                 wtot_inv = 1.0f/wtot;
1605
1606                 w[0] = w[0]*wtot_inv;
1607                 w[1] = w[1]*wtot_inv;
1608                 w[2] = w[2]*wtot_inv;
1609         }
1610         else /* dummy values for zero area face */
1611                 w[0] = w[1] = w[2] = 1.0f/3.0f;
1612 }
1613
1614 /* given 2 triangles in 3D space, and a point in relation to the first triangle.
1615  * calculate the location of a point in relation to the second triangle.
1616  * Useful for finding relative positions with geometry */
1617 void barycentric_transform(float pt_tar[3], float const pt_src[3],
1618                 const float tri_tar_p1[3], const float tri_tar_p2[3], const float tri_tar_p3[3],
1619                 const float tri_src_p1[3], const float tri_src_p2[3], const float tri_src_p3[3])
1620 {
1621         /* this works by moving the source triangle so its normal is pointing on the Z
1622          * axis where its barycentric wights can be calculated in 2D and its Z offset can
1623          *  be re-applied. The weights are applied directly to the targets 3D points and the
1624          *  z-depth is used to scale the targets normal as an offset.
1625          * This saves transforming the target into its Z-Up orientation and back (which could also work) */
1626         const float z_up[3] = {0, 0, 1};
1627         float no_tar[3], no_src[3];
1628         float quat_src[4];
1629         float pt_src_xy[3];
1630         float  tri_xy_src[3][3];
1631         float w_src[3];
1632         float area_tar, area_src;
1633         float z_ofs_src;
1634
1635         normal_tri_v3(no_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3);
1636         normal_tri_v3(no_src, tri_src_p1, tri_src_p2, tri_src_p3);
1637
1638         rotation_between_vecs_to_quat(quat_src, no_src, z_up);
1639         normalize_qt(quat_src);
1640
1641         copy_v3_v3(pt_src_xy, pt_src);
1642         copy_v3_v3(tri_xy_src[0], tri_src_p1);
1643         copy_v3_v3(tri_xy_src[1], tri_src_p2);
1644         copy_v3_v3(tri_xy_src[2], tri_src_p3);
1645
1646         /* make the source tri xy space */
1647         mul_qt_v3(quat_src, pt_src_xy);
1648         mul_qt_v3(quat_src, tri_xy_src[0]);
1649         mul_qt_v3(quat_src, tri_xy_src[1]);
1650         mul_qt_v3(quat_src, tri_xy_src[2]);
1651
1652         barycentric_weights_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2], pt_src_xy, w_src);
1653         interp_v3_v3v3v3(pt_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3, w_src);
1654
1655         area_tar= sqrtf(area_tri_v3(tri_tar_p1, tri_tar_p2, tri_tar_p3));
1656         area_src= sqrtf(area_tri_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2]));
1657
1658         z_ofs_src= pt_src_xy[2] - tri_xy_src[0][2];
1659         madd_v3_v3v3fl(pt_tar, pt_tar, no_tar, (z_ofs_src / area_src) * area_tar);
1660 }
1661
1662 /* given an array with some invalid values this function interpolates valid values
1663  * replacing the invalid ones */
1664 int interp_sparse_array(float *array, int const list_size, const float skipval)
1665 {
1666         int found_invalid = 0;
1667         int found_valid = 0;
1668         int i;
1669
1670         for (i=0; i < list_size; i++) {
1671                 if(array[i] == skipval)
1672                         found_invalid= 1;
1673                 else
1674                         found_valid= 1;
1675         }
1676
1677         if(found_valid==0) {
1678                 return -1;
1679         }
1680         else if (found_invalid==0) {
1681                 return 0;
1682         }
1683         else {
1684                 /* found invalid depths, interpolate */
1685                 float valid_last= skipval;
1686                 int valid_ofs= 0;
1687
1688                 float *array_up= MEM_callocN(sizeof(float) * list_size, "interp_sparse_array up");
1689                 float *array_down= MEM_callocN(sizeof(float) * list_size, "interp_sparse_array up");
1690
1691                 int *ofs_tot_up= MEM_callocN(sizeof(int) * list_size, "interp_sparse_array tup");
1692                 int *ofs_tot_down= MEM_callocN(sizeof(int) * list_size, "interp_sparse_array tdown");
1693
1694                 for (i=0; i < list_size; i++) {
1695                         if(array[i] == skipval) {
1696                                 array_up[i]= valid_last;
1697                                 ofs_tot_up[i]= ++valid_ofs;
1698                         }
1699                         else {
1700                                 valid_last= array[i];
1701                                 valid_ofs= 0;
1702                         }
1703                 }
1704
1705                 valid_last= skipval;
1706                 valid_ofs= 0;
1707
1708                 for (i=list_size-1; i >= 0; i--) {
1709                         if(array[i] == skipval) {
1710                                 array_down[i]= valid_last;
1711                                 ofs_tot_down[i]= ++valid_ofs;
1712                         }
1713                         else {
1714                                 valid_last= array[i];
1715                                 valid_ofs= 0;
1716                         }
1717                 }
1718
1719                 /* now blend */
1720                 for (i=0; i < list_size; i++) {
1721                         if(array[i] == skipval) {
1722                                 if(array_up[i] != skipval && array_down[i] != skipval) {
1723                                         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]);
1724                                 } else if (array_up[i] != skipval) {
1725                                         array[i]= array_up[i];
1726                                 } else if (array_down[i] != skipval) {
1727                                         array[i]= array_down[i];
1728                                 }
1729                         }
1730                 }
1731
1732                 MEM_freeN(array_up);
1733                 MEM_freeN(array_down);
1734
1735                 MEM_freeN(ofs_tot_up);
1736                 MEM_freeN(ofs_tot_down);
1737         }
1738
1739         return 1;
1740 }
1741
1742 /* Mean value weights - smooth interpolation weights for polygons with
1743  * more than 3 vertices */
1744 static float mean_value_half_tan(const float v1[3], const float v2[3], const float v3[3])
1745 {
1746         float d2[3], d3[3], cross[3], area, dot, len;
1747
1748         sub_v3_v3v3(d2, v2, v1);
1749         sub_v3_v3v3(d3, v3, v1);
1750         cross_v3_v3v3(cross, d2, d3);
1751
1752         area= len_v3(cross);
1753         dot= dot_v3v3(d2, d3);
1754         len= len_v3(d2)*len_v3(d3);
1755
1756         if(area == 0.0f)
1757                 return 0.0f;
1758         else
1759                 return (len - dot)/area;
1760 }
1761
1762 void interp_weights_poly_v3(float *w, float v[][3], const int n, const float co[3])
1763 {
1764         float totweight, t1, t2, len, *vmid, *vprev, *vnext;
1765         int i;
1766
1767         totweight= 0.0f;
1768
1769         for(i=0; i<n; i++) {
1770                 vmid= v[i];
1771                 vprev= (i == 0)? v[n-1]: v[i-1];
1772                 vnext= (i == n-1)? v[0]: v[i+1];
1773
1774                 t1= mean_value_half_tan(co, vprev, vmid);
1775                 t2= mean_value_half_tan(co, vmid, vnext);
1776
1777                 len= len_v3v3(co, vmid);
1778                 w[i]= (t1+t2)/len;
1779                 totweight += w[i];
1780         }
1781
1782         if(totweight != 0.0f)
1783                 for(i=0; i<n; i++)
1784                         w[i] /= totweight;
1785 }
1786
1787 /* (x1,v1)(t1=0)------(x2,v2)(t2=1), 0<t<1 --> (x,v)(t) */
1788 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)
1789 {
1790         float a[3],b[3];
1791         float t2= t*t;
1792         float t3= t2*t;
1793
1794         /* cubic interpolation */
1795         a[0]= v1[0] + v2[0] + 2*(x1[0] - x2[0]);
1796         a[1]= v1[1] + v2[1] + 2*(x1[1] - x2[1]);
1797         a[2]= v1[2] + v2[2] + 2*(x1[2] - x2[2]);
1798
1799         b[0]= -2*v1[0] - v2[0] - 3*(x1[0] - x2[0]);
1800         b[1]= -2*v1[1] - v2[1] - 3*(x1[1] - x2[1]);
1801         b[2]= -2*v1[2] - v2[2] - 3*(x1[2] - x2[2]);
1802
1803         x[0]= a[0]*t3 + b[0]*t2 + v1[0]*t + x1[0];
1804         x[1]= a[1]*t3 + b[1]*t2 + v1[1]*t + x1[1];
1805         x[2]= a[2]*t3 + b[2]*t2 + v1[2]*t + x1[2];
1806
1807         v[0]= 3*a[0]*t2 + 2*b[0]*t + v1[0];
1808         v[1]= 3*a[1]*t2 + 2*b[1]*t + v1[1];
1809         v[2]= 3*a[2]*t2 + 2*b[2]*t + v1[2];
1810 }
1811
1812 /***************************** View & Projection *****************************/
1813
1814 void orthographic_m4(float matrix[][4], const float left, const float right, const float bottom, const float top, const float nearClip, const float farClip)
1815 {
1816         float Xdelta, Ydelta, Zdelta;
1817  
1818         Xdelta = right - left;
1819         Ydelta = top - bottom;
1820         Zdelta = farClip - nearClip;
1821         if (Xdelta == 0.0f || Ydelta == 0.0f || Zdelta == 0.0f) {
1822                 return;
1823         }
1824         unit_m4(matrix);
1825         matrix[0][0] = 2.0f/Xdelta;
1826         matrix[3][0] = -(right + left)/Xdelta;
1827         matrix[1][1] = 2.0f/Ydelta;
1828         matrix[3][1] = -(top + bottom)/Ydelta;
1829         matrix[2][2] = -2.0f/Zdelta;            /* note: negate Z       */
1830         matrix[3][2] = -(farClip + nearClip)/Zdelta;
1831 }
1832
1833 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)
1834 {
1835         float Xdelta, Ydelta, Zdelta;
1836
1837         Xdelta = right - left;
1838         Ydelta = top - bottom;
1839         Zdelta = farClip - nearClip;
1840
1841         if (Xdelta == 0.0f || Ydelta == 0.0f || Zdelta == 0.0f) {
1842                 return;
1843         }
1844         mat[0][0] = nearClip * 2.0f/Xdelta;
1845         mat[1][1] = nearClip * 2.0f/Ydelta;
1846         mat[2][0] = (right + left)/Xdelta;              /* note: negate Z       */
1847         mat[2][1] = (top + bottom)/Ydelta;
1848         mat[2][2] = -(farClip + nearClip)/Zdelta;
1849         mat[2][3] = -1.0f;
1850         mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
1851         mat[0][1] = mat[0][2] = mat[0][3] =
1852                 mat[1][0] = mat[1][2] = mat[1][3] =
1853                 mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
1854
1855 }
1856
1857 /* translate a matrix created by orthographic_m4 or perspective_m4 in XY coords (used to jitter the view) */
1858 void window_translate_m4(float winmat[][4], float perspmat[][4], const float x, const float y)
1859 {
1860         if(winmat[2][3] == -1.0f) {
1861                 /* in the case of a win-matrix, this means perspective always */
1862                 float v1[3];
1863                 float v2[3];
1864                 float len1, len2;
1865
1866                 v1[0]= perspmat[0][0];
1867                 v1[1]= perspmat[1][0];
1868                 v1[2]= perspmat[2][0];
1869
1870                 v2[0]= perspmat[0][1];
1871                 v2[1]= perspmat[1][1];
1872                 v2[2]= perspmat[2][1];
1873
1874                 len1= (1.0f / len_v3(v1));
1875                 len2= (1.0f / len_v3(v2));
1876
1877                 winmat[2][0] += len1 * winmat[0][0] * x;
1878                 winmat[2][1] += len2 * winmat[1][1] * y;
1879         }
1880         else {
1881                 winmat[3][0] += x;
1882                 winmat[3][1] += y;
1883         }
1884 }
1885
1886 static void i_multmatrix(float icand[][4], float Vm[][4])
1887 {
1888         int row, col;
1889         float temp[4][4];
1890
1891         for(row=0 ; row<4 ; row++) 
1892                 for(col=0 ; col<4 ; col++)
1893                         temp[row][col] = icand[row][0] * Vm[0][col]
1894                                                    + icand[row][1] * Vm[1][col]
1895                                                    + icand[row][2] * Vm[2][col]
1896                                                    + icand[row][3] * Vm[3][col];
1897         copy_m4_m4(Vm, temp);
1898 }
1899
1900
1901 void polarview_m4(float Vm[][4],float dist, float azimuth, float incidence, float twist)
1902 {
1903
1904         unit_m4(Vm);
1905
1906         translate_m4(Vm,0.0, 0.0, -dist);
1907         rotate_m4(Vm,'Z',-twist);
1908         rotate_m4(Vm,'X',-incidence);
1909         rotate_m4(Vm,'Z',-azimuth);
1910 }
1911
1912 void lookat_m4(float mat[][4],float vx, float vy, float vz, float px, float py, float pz, float twist)
1913 {
1914         float sine, cosine, hyp, hyp1, dx, dy, dz;
1915         float mat1[4][4]= MAT4_UNITY;
1916         
1917         unit_m4(mat);
1918
1919         rotate_m4(mat, 'Z', -twist);
1920
1921         dx = px - vx;
1922         dy = py - vy;
1923         dz = pz - vz;
1924         hyp = dx * dx + dz * dz;        /* hyp squared  */
1925         hyp1 = (float)sqrt(dy*dy + hyp);
1926         hyp = (float)sqrt(hyp);         /* the real hyp */
1927         
1928         if (hyp1 != 0.0f) {             /* rotate X     */
1929                 sine = -dy / hyp1;
1930                 cosine = hyp /hyp1;
1931         } else {
1932                 sine = 0;
1933                 cosine = 1.0f;
1934         }
1935         mat1[1][1] = cosine;
1936         mat1[1][2] = sine;
1937         mat1[2][1] = -sine;
1938         mat1[2][2] = cosine;
1939         
1940         i_multmatrix(mat1, mat);
1941
1942         mat1[1][1] = mat1[2][2] = 1.0f; /* be careful here to reinit    */
1943         mat1[1][2] = mat1[2][1] = 0.0;  /* those modified by the last   */
1944         
1945         /* paragraph    */
1946         if (hyp != 0.0f) {                      /* rotate Y     */
1947                 sine = dx / hyp;
1948                 cosine = -dz / hyp;
1949         } else {
1950                 sine = 0;
1951                 cosine = 1.0f;
1952         }
1953         mat1[0][0] = cosine;
1954         mat1[0][2] = -sine;
1955         mat1[2][0] = sine;
1956         mat1[2][2] = cosine;
1957         
1958         i_multmatrix(mat1, mat);
1959         translate_m4(mat,-vx,-vy,-vz);  /* translate viewpoint to origin */
1960 }
1961
1962 int box_clip_bounds_m4(float boundbox[2][3], const float bounds[4], float winmat[4][4])
1963 {
1964         float mat[4][4], vec[4];
1965         int a, fl, flag= -1;
1966
1967         copy_m4_m4(mat, winmat);
1968
1969         for(a=0; a<8; a++) {
1970                 vec[0]= (a & 1)? boundbox[0][0]: boundbox[1][0];
1971                 vec[1]= (a & 2)? boundbox[0][1]: boundbox[1][1];
1972                 vec[2]= (a & 4)? boundbox[0][2]: boundbox[1][2];
1973                 vec[3]= 1.0;
1974                 mul_m4_v4(mat, vec);
1975
1976                 fl= 0;
1977                 if(bounds) {
1978                         if(vec[0] > bounds[1]*vec[3]) fl |= 1;
1979                         if(vec[0]< bounds[0]*vec[3]) fl |= 2;
1980                         if(vec[1] > bounds[3]*vec[3]) fl |= 4;
1981                         if(vec[1]< bounds[2]*vec[3]) fl |= 8;
1982                 }
1983                 else {
1984                         if(vec[0] < -vec[3]) fl |= 1;
1985                         if(vec[0] > vec[3]) fl |= 2;
1986                         if(vec[1] < -vec[3]) fl |= 4;
1987                         if(vec[1] > vec[3]) fl |= 8;
1988                 }
1989                 if(vec[2] < -vec[3]) fl |= 16;
1990                 if(vec[2] > vec[3]) fl |= 32;
1991
1992                 flag &= fl;
1993                 if(flag==0) return 0;
1994         }
1995
1996         return flag;
1997 }
1998
1999 void box_minmax_bounds_m4(float min[3], float max[3], float boundbox[2][3], float mat[4][4])
2000 {
2001         float mn[3], mx[3], vec[3];
2002         int a;
2003
2004         copy_v3_v3(mn, min);
2005         copy_v3_v3(mx, max);
2006
2007         for(a=0; a<8; a++) {
2008                 vec[0]= (a & 1)? boundbox[0][0]: boundbox[1][0];
2009                 vec[1]= (a & 2)? boundbox[0][1]: boundbox[1][1];
2010                 vec[2]= (a & 4)? boundbox[0][2]: boundbox[1][2];
2011
2012                 mul_m4_v3(mat, vec);
2013                 DO_MINMAX(vec, mn, mx);
2014         }
2015
2016         copy_v3_v3(min, mn);
2017         copy_v3_v3(max, mx);
2018 }
2019
2020 /********************************** Mapping **********************************/
2021
2022 void map_to_tube(float *u, float *v, const float x, const float y, const float z)
2023 {
2024         float len;
2025         
2026         *v = (z + 1.0f) / 2.0f;
2027         
2028         len= (float)sqrt(x*x+y*y);
2029         if(len > 0.0f)
2030                 *u = (float)((1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0);
2031         else
2032                 *v = *u = 0.0f; /* to avoid un-initialized variables */
2033 }
2034
2035 void map_to_sphere(float *u, float *v, const float x, const float y, const float z)
2036 {
2037         float len;
2038         
2039         len= (float)sqrt(x*x+y*y+z*z);
2040         if(len > 0.0f) {
2041                 if(x==0.0f && y==0.0f) *u= 0.0f;        /* othwise domain error */
2042                 else *u = (1.0f - atan2f(x,y) / (float)M_PI) / 2.0f;
2043
2044                 *v = 1.0f - (float)saacos(z/len)/(float)M_PI;
2045         } else {
2046                 *v = *u = 0.0f; /* to avoid un-initialized variables */
2047         }
2048 }
2049
2050 /********************************* Normals **********************************/
2051
2052 void accumulate_vertex_normals(float n1[3], float n2[3], float n3[3],
2053         float n4[3], const float f_no[3], const float co1[3], const float co2[3],
2054         const float co3[3], const float co4[3])
2055 {
2056         float vdiffs[4][3];
2057         const int nverts= (n4!=NULL && co4!=NULL)? 4: 3;
2058
2059         /* compute normalized edge vectors */
2060         sub_v3_v3v3(vdiffs[0], co2, co1);
2061         sub_v3_v3v3(vdiffs[1], co3, co2);
2062
2063         if(nverts==3) {
2064                 sub_v3_v3v3(vdiffs[2], co1, co3);
2065         }
2066         else {
2067                 sub_v3_v3v3(vdiffs[2], co4, co3);
2068                 sub_v3_v3v3(vdiffs[3], co1, co4);
2069                 normalize_v3(vdiffs[3]);
2070         }
2071
2072         normalize_v3(vdiffs[0]);
2073         normalize_v3(vdiffs[1]);
2074         normalize_v3(vdiffs[2]);
2075
2076         /* accumulate angle weighted face normal */
2077         {
2078                 float *vn[]= {n1, n2, n3, n4};
2079                 const float *prev_edge = vdiffs[nverts-1];
2080                 int i;
2081
2082                 for(i=0; i<nverts; i++) {
2083                         const float *cur_edge= vdiffs[i];
2084                         const float fac= saacos(-dot_v3v3(cur_edge, prev_edge));
2085
2086                         // accumulate
2087                         madd_v3_v3fl(vn[i], f_no, fac);
2088                         prev_edge = cur_edge;
2089                 }
2090         }
2091 }
2092
2093 /********************************* Tangents **********************************/
2094
2095 /* For normal map tangents we need to detect uv boundaries, and only average
2096  * tangents in case the uvs are connected. Alternative would be to store 1 
2097  * tangent per face rather than 4 per face vertex, but that's not compatible
2098  * with games */
2099
2100
2101 /* from BKE_mesh.h */
2102 #define STD_UV_CONNECT_LIMIT    0.0001f
2103
2104 void sum_or_add_vertex_tangent(void *arena, VertexTangent **vtang, const float tang[3], const float uv[2])
2105 {
2106         VertexTangent *vt;
2107
2108         /* find a tangent with connected uvs */
2109         for(vt= *vtang; vt; vt=vt->next) {
2110                 if(fabsf(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabsf(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT) {
2111                         add_v3_v3(vt->tang, tang);
2112                         return;
2113                 }
2114         }
2115
2116         /* if not found, append a new one */
2117         vt= BLI_memarena_alloc((MemArena *)arena, sizeof(VertexTangent));
2118         copy_v3_v3(vt->tang, tang);
2119         vt->uv[0]= uv[0];
2120         vt->uv[1]= uv[1];
2121
2122         if(*vtang)
2123                 vt->next= *vtang;
2124         *vtang= vt;
2125 }
2126
2127 float *find_vertex_tangent(VertexTangent *vtang, const float uv[2])
2128 {
2129         VertexTangent *vt;
2130         static float nulltang[3] = {0.0f, 0.0f, 0.0f};
2131
2132         for(vt= vtang; vt; vt=vt->next)
2133                 if(fabsf(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabsf(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT)
2134                         return vt->tang;
2135
2136         return nulltang;        /* shouldn't happen, except for nan or so */
2137 }
2138
2139 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])
2140 {
2141         float s1= uv2[0] - uv1[0];
2142         float s2= uv3[0] - uv1[0];
2143         float t1= uv2[1] - uv1[1];
2144         float t2= uv3[1] - uv1[1];
2145         float det= (s1 * t2 - s2 * t1);
2146
2147         if(det != 0.0f) { /* otherwise 'tang' becomes nan */
2148                 float tangv[3], ct[3], e1[3], e2[3];
2149
2150                 det= 1.0f/det;
2151
2152                 /* normals in render are inversed... */
2153                 sub_v3_v3v3(e1, co1, co2);
2154                 sub_v3_v3v3(e2, co1, co3);
2155                 tang[0] = (t2*e1[0] - t1*e2[0])*det;
2156                 tang[1] = (t2*e1[1] - t1*e2[1])*det;
2157                 tang[2] = (t2*e1[2] - t1*e2[2])*det;
2158                 tangv[0] = (s1*e2[0] - s2*e1[0])*det;
2159                 tangv[1] = (s1*e2[1] - s2*e1[1])*det;
2160                 tangv[2] = (s1*e2[2] - s2*e1[2])*det;
2161                 cross_v3_v3v3(ct, tang, tangv);
2162         
2163                 /* check flip */
2164                 if ((ct[0]*n[0] + ct[1]*n[1] + ct[2]*n[2]) < 0.0f)
2165                         negate_v3(tang);
2166         }
2167         else {
2168                 tang[0]= tang[1]= tang[2]=  0.0;
2169         }
2170 }
2171
2172 /****************************** Vector Clouds ********************************/
2173
2174 /* vector clouds */
2175 /* void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
2176                                                                  float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3])
2177
2178 input
2179 (
2180 int list_size
2181 4 lists as pointer to array[list_size]
2182 1. current pos array of 'new' positions 
2183 2. current weight array of 'new'weights (may be NULL pointer if you have no weights )
2184 3. reference rpos array of 'old' positions
2185 4. reference rweight array of 'old'weights (may be NULL pointer if you have no weights )
2186 )
2187 output  
2188 (
2189 float lloc[3] center of mass pos
2190 float rloc[3] center of mass rpos
2191 float lrot[3][3] rotation matrix
2192 float lscale[3][3] scale matrix
2193 pointers may be NULL if not needed
2194 )
2195
2196 */
2197 /* can't believe there is none in math utils */
2198 static float _det_m3(float m2[3][3])
2199 {
2200         float det = 0.f;
2201         if (m2){
2202         det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
2203                 -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
2204                 +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
2205         }
2206         return det;
2207 }
2208
2209
2210 void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
2211                                                         float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3])
2212 {
2213         float accu_com[3]= {0.0f,0.0f,0.0f}, accu_rcom[3]= {0.0f,0.0f,0.0f};
2214         float accu_weight = 0.0f,accu_rweight = 0.0f,eps = 0.000001f;
2215
2216         int a;
2217         /* first set up a nice default response */
2218         if (lloc) zero_v3(lloc);
2219         if (rloc) zero_v3(rloc);
2220         if (lrot) unit_m3(lrot);
2221         if (lscale) unit_m3(lscale);
2222         /* do com for both clouds */
2223         if (pos && rpos && (list_size > 0)) /* paranoya check */
2224         {
2225                 /* do com for both clouds */
2226                 for(a=0; a<list_size; a++){
2227                         if (weight){
2228                                 float v[3];
2229                                 copy_v3_v3(v,pos[a]);
2230                                 mul_v3_fl(v,weight[a]);
2231                                 add_v3_v3(accu_com, v);
2232                                 accu_weight +=weight[a]; 
2233                         }
2234                         else add_v3_v3(accu_com, pos[a]);
2235
2236                         if (rweight){
2237                                 float v[3];
2238                                 copy_v3_v3(v,rpos[a]);
2239                                 mul_v3_fl(v,rweight[a]);
2240                                 add_v3_v3(accu_rcom, v);
2241                                 accu_rweight +=rweight[a]; 
2242                         }
2243                         else add_v3_v3(accu_rcom, rpos[a]);
2244
2245                 }
2246                 if (!weight || !rweight){
2247                         accu_weight = accu_rweight = list_size;
2248                 }
2249
2250                 mul_v3_fl(accu_com,1.0f/accu_weight);
2251                 mul_v3_fl(accu_rcom,1.0f/accu_rweight);
2252                 if (lloc) copy_v3_v3(lloc,accu_com);
2253                 if (rloc) copy_v3_v3(rloc,accu_rcom);
2254                 if (lrot || lscale){ /* caller does not want rot nor scale, strange but legal */
2255                         /*so now do some reverse engeneering and see if we can split rotation from scale ->Polardecompose*/
2256                         /* build 'projection' matrix */
2257                         float m[3][3],mr[3][3],q[3][3],qi[3][3];
2258                         float va[3],vb[3],stunt[3];
2259                         float odet,ndet;
2260                         int i=0,imax=15;
2261                         zero_m3(m);
2262                         zero_m3(mr);
2263
2264                         /* build 'projection' matrix */
2265                         for(a=0; a<list_size; a++){
2266                                 sub_v3_v3v3(va,rpos[a],accu_rcom);
2267                                 /* mul_v3_fl(va,bp->mass);  mass needs renormalzation here ?? */
2268                                 sub_v3_v3v3(vb,pos[a],accu_com);
2269                                 /* mul_v3_fl(va,rp->mass); */
2270                                 m[0][0] += va[0] * vb[0];
2271                                 m[0][1] += va[0] * vb[1];
2272                                 m[0][2] += va[0] * vb[2];
2273
2274                                 m[1][0] += va[1] * vb[0];
2275                                 m[1][1] += va[1] * vb[1];
2276                                 m[1][2] += va[1] * vb[2];
2277
2278                                 m[2][0] += va[2] * vb[0];
2279                                 m[2][1] += va[2] * vb[1];
2280                                 m[2][2] += va[2] * vb[2];
2281
2282                                 /* building the referenc matrix on the fly
2283                                 needed to scale properly later*/
2284
2285                                 mr[0][0] += va[0] * va[0];
2286                                 mr[0][1] += va[0] * va[1];
2287                                 mr[0][2] += va[0] * va[2];
2288
2289                                 mr[1][0] += va[1] * va[0];
2290                                 mr[1][1] += va[1] * va[1];
2291                                 mr[1][2] += va[1] * va[2];
2292
2293                                 mr[2][0] += va[2] * va[0];
2294                                 mr[2][1] += va[2] * va[1];
2295                                 mr[2][2] += va[2] * va[2];
2296                         }
2297                         copy_m3_m3(q,m);
2298                         stunt[0] = q[0][0]; stunt[1] = q[1][1]; stunt[2] = q[2][2];
2299                         /* renormalizing for numeric stability */
2300                         mul_m3_fl(q,1.f/len_v3(stunt)); 
2301
2302                         /* this is pretty much Polardecompose 'inline' the algo based on Higham's thesis */
2303                         /* without the far case ... but seems to work here pretty neat                   */
2304                         odet = 0.f;
2305                         ndet = _det_m3(q);
2306                         while((odet-ndet)*(odet-ndet) > eps && i<imax){
2307                                 invert_m3_m3(qi,q);
2308                                 transpose_m3(qi);
2309                                 add_m3_m3m3(q,q,qi);
2310                                 mul_m3_fl(q,0.5f);
2311                                 odet =ndet;
2312                                 ndet =_det_m3(q);
2313                                 i++;
2314                         }
2315
2316                         if (i){
2317                                 float scale[3][3];
2318                                 float irot[3][3];
2319                                 if(lrot) copy_m3_m3(lrot,q);
2320                                 invert_m3_m3(irot,q);
2321                                 invert_m3_m3(qi,mr);
2322                                 mul_m3_m3m3(q,m,qi); 
2323                                 mul_m3_m3m3(scale,irot,q); 
2324                                 if(lscale) copy_m3_m3(lscale,scale);
2325
2326                         }
2327                 }
2328         }
2329 }
2330
2331 /******************************* Form Factor *********************************/
2332
2333 static void vec_add_dir(float r[3], const float v1[3], const float v2[3], const float fac)
2334 {
2335         r[0]= v1[0] + fac*(v2[0] - v1[0]);
2336         r[1]= v1[1] + fac*(v2[1] - v1[1]);
2337         r[2]= v1[2] + fac*(v2[2] - v1[2]);
2338 }
2339
2340 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])
2341 {
2342         static const float epsilon = 1e-6f;
2343         float c, sd[3];
2344         
2345         c= dot_v3v3(n, p);
2346
2347         /* signed distances from the vertices to the plane. */
2348         sd[0]= dot_v3v3(n, v0) - c;
2349         sd[1]= dot_v3v3(n, v1) - c;
2350         sd[2]= dot_v3v3(n, v2) - c;
2351
2352         if(fabsf(sd[0]) < epsilon) sd[0] = 0.0f;
2353         if(fabsf(sd[1]) < epsilon) sd[1] = 0.0f;
2354         if(fabsf(sd[2]) < epsilon) sd[2] = 0.0f;
2355
2356         if(sd[0] > 0) {
2357                 if(sd[1] > 0) {
2358                         if(sd[2] > 0) {
2359                                 // +++
2360                                 copy_v3_v3(q0, v0);
2361                                 copy_v3_v3(q1, v1);
2362                                 copy_v3_v3(q2, v2);
2363                                 copy_v3_v3(q3, q2);
2364                         }
2365                         else if(sd[2] < 0) {
2366                                 // ++-
2367                                 copy_v3_v3(q0, v0);
2368                                 copy_v3_v3(q1, v1);
2369                                 vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2])));
2370                                 vec_add_dir(q3, v0, v2, (sd[0]/(sd[0]-sd[2])));
2371                         }
2372                         else {
2373                                 // ++0
2374                                 copy_v3_v3(q0, v0);
2375                                 copy_v3_v3(q1, v1);
2376                                 copy_v3_v3(q2, v2);
2377                                 copy_v3_v3(q3, q2);
2378                         }
2379                 }
2380                 else if(sd[1] < 0) {
2381                         if(sd[2] > 0) {
2382                                 // +-+
2383                                 copy_v3_v3(q0, v0);
2384                                 vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1])));
2385                                 vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2])));
2386                                 copy_v3_v3(q3, v2);
2387                         }
2388                         else if(sd[2] < 0) {
2389                                 // +--
2390                                 copy_v3_v3(q0, v0);
2391                                 vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1])));
2392                                 vec_add_dir(q2, v0, v2, (sd[0]/(sd[0]-sd[2])));
2393                                 copy_v3_v3(q3, q2);
2394                         }
2395                         else {
2396                                 // +-0
2397                                 copy_v3_v3(q0, v0);
2398                                 vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1])));
2399                                 copy_v3_v3(q2, v2);
2400                                 copy_v3_v3(q3, q2);
2401                         }
2402                 }
2403                 else {
2404                         if(sd[2] > 0) {
2405                                 // +0+
2406                                 copy_v3_v3(q0, v0);
2407                                 copy_v3_v3(q1, v1);
2408                                 copy_v3_v3(q2, v2);
2409                                 copy_v3_v3(q3, q2);
2410                         }
2411                         else if(sd[2] < 0) {
2412                                 // +0-
2413                                 copy_v3_v3(q0, v0);
2414                                 copy_v3_v3(q1, v1);
2415                                 vec_add_dir(q2, v0, v2, (sd[0]/(sd[0]-sd[2])));
2416                                 copy_v3_v3(q3, q2);
2417                         }
2418                         else {
2419                                 // +00
2420                                 copy_v3_v3(q0, v0);
2421                                 copy_v3_v3(q1, v1);
2422                                 copy_v3_v3(q2, v2);
2423                                 copy_v3_v3(q3, q2);
2424                         }
2425                 }
2426         }
2427         else if(sd[0] < 0) {
2428                 if(sd[1] > 0) {
2429                         if(sd[2] > 0) {
2430                                 // -++
2431                                 vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1])));
2432                                 copy_v3_v3(q1, v1);
2433                                 copy_v3_v3(q2, v2);
2434                                 vec_add_dir(q3, v0, v2, (sd[0]/(sd[0]-sd[2])));
2435                         }
2436                         else if(sd[2] < 0) {
2437                                 // -+-
2438                                 vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1])));
2439                                 copy_v3_v3(q1, v1);
2440                                 vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2])));
2441                                 copy_v3_v3(q3, q2);
2442                         }
2443                         else {
2444                                 // -+0
2445                                 vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1])));
2446                                 copy_v3_v3(q1, v1);
2447                                 copy_v3_v3(q2, v2);
2448                                 copy_v3_v3(q3, q2);
2449                         }
2450                 }
2451                 else if(sd[1] < 0) {
2452                         if(sd[2] > 0) {
2453                                 // --+
2454                                 vec_add_dir(q0, v0, v2, (sd[0]/(sd[0]-sd[2])));
2455                                 vec_add_dir(q1, v1, v2, (sd[1]/(sd[1]-sd[2])));
2456                                 copy_v3_v3(q2, v2);
2457                                 copy_v3_v3(q3, q2);
2458                         }
2459                         else if(sd[2] < 0) {
2460                                 // ---
2461                                 return 0;
2462                         }
2463                         else {
2464                                 // --0
2465                                 return 0;
2466                         }
2467                 }
2468                 else {
2469                         if(sd[2] > 0) {
2470                                 // -0+
2471                                 vec_add_dir(q0, v0, v2, (sd[0]/(sd[0]-sd[2])));
2472                                 copy_v3_v3(q1, v1);
2473                                 copy_v3_v3(q2, v2);
2474                                 copy_v3_v3(q3, q2);
2475                         }
2476                         else if(sd[2] < 0) {
2477                                 // -0-
2478                                 return 0;
2479                         }
2480                         else {
2481                                 // -00
2482                                 return 0;
2483                         }
2484                 }
2485         }
2486         else {
2487                 if(sd[1] > 0) {
2488                         if(sd[2] > 0) {
2489                                 // 0++
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                                 // 0+-
2497                                 copy_v3_v3(q0, v0);
2498                                 copy_v3_v3(q1, v1);
2499                                 vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2])));
2500                                 copy_v3_v3(q3, q2);
2501                         }
2502                         else {
2503                                 // 0+0
2504                                 copy_v3_v3(q0, v0);
2505                                 copy_v3_v3(q1, v1);
2506                                 copy_v3_v3(q2, v2);
2507                                 copy_v3_v3(q3, q2);
2508                         }
2509                 }
2510                 else if(sd[1] < 0) {
2511                         if(sd[2] > 0) {
2512                                 // 0-+
2513                                 copy_v3_v3(q0, v0);
2514                                 vec_add_dir(q1, v1, v2, (sd[1]/(sd[1]-sd[2])));
2515                                 copy_v3_v3(q2, v2);
2516                                 copy_v3_v3(q3, q2);
2517                         }
2518                         else if(sd[2] < 0) {
2519                                 // 0--
2520                                 return 0;
2521                         }
2522                         else {
2523                                 // 0-0
2524                                 return 0;
2525                         }
2526                 }
2527                 else {
2528                         if(sd[2] > 0) {
2529                                 // 00+
2530                                 copy_v3_v3(q0, v0);
2531                                 copy_v3_v3(q1, v1);
2532                                 copy_v3_v3(q2, v2);
2533                                 copy_v3_v3(q3, q2);
2534                         }
2535                         else if(sd[2] < 0) {
2536                                 // 00-
2537                                 return 0;
2538                         }
2539                         else {
2540                                 // 000
2541                                 return 0;
2542                         }
2543                 }
2544         }
2545
2546         return 1;
2547 }
2548
2549 /* altivec optimization, this works, but is unused */
2550
2551 #if 0
2552 #include <Accelerate/Accelerate.h>
2553
2554 typedef union {
2555         vFloat v;
2556         float f[4];
2557 } vFloatResult;
2558
2559 static vFloat vec_splat_float(float val)
2560 {
2561         return (vFloat){val, val, val, val};
2562 }
2563
2564 static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float *q2, float *q3)
2565 {
2566         vFloat vcos, rlen, vrx, vry, vrz, vsrx, vsry, vsrz, gx, gy, gz, vangle;
2567         vUInt8 rotate = (vUInt8){4,5,6,7,8,9,10,11,12,13,14,15,0,1,2,3};
2568         vFloatResult vresult;
2569         float result;
2570
2571         /* compute r* */
2572         vrx = (vFloat){q0[0], q1[0], q2[0], q3[0]} - vec_splat_float(p[0]);
2573         vry = (vFloat){q0[1], q1[1], q2[1], q3[1]} - vec_splat_float(p[1]);
2574         vrz = (vFloat){q0[2], q1[2], q2[2], q3[2]} - vec_splat_float(p[2]);
2575
2576         /* normalize r* */
2577         rlen = vec_rsqrte(vrx*vrx + vry*vry + vrz*vrz + vec_splat_float(1e-16f));
2578         vrx = vrx*rlen;
2579         vry = vry*rlen;
2580         vrz = vrz*rlen;
2581
2582         /* rotate r* for cross and dot */
2583         vsrx= vec_perm(vrx, vrx, rotate);
2584         vsry= vec_perm(vry, vry, rotate);
2585         vsrz= vec_perm(vrz, vrz, rotate);
2586
2587         /* cross product */
2588         gx = vsry*vrz - vsrz*vry;
2589         gy = vsrz*vrx - vsrx*vrz;
2590         gz = vsrx*vry - vsry*vrx;
2591
2592         /* normalize */
2593         rlen = vec_rsqrte(gx*gx + gy*gy + gz*gz + vec_splat_float(1e-16f));
2594         gx = gx*rlen;
2595         gy = gy*rlen;
2596         gz = gz*rlen;
2597
2598         /* angle */
2599         vcos = vrx*vsrx + vry*vsry + vrz*vsrz;
2600         vcos= vec_max(vec_min(vcos, vec_splat_float(1.0f)), vec_splat_float(-1.0f));
2601         vangle= vacosf(vcos);
2602
2603         /* dot */
2604         vresult.v = (vec_splat_float(n[0])*gx +
2605                      vec_splat_float(n[1])*gy +
2606                      vec_splat_float(n[2])*gz)*vangle;
2607
2608         result= (vresult.f[0] + vresult.f[1] + vresult.f[2] + vresult.f[3])*(0.5f/(float)M_PI);
2609         result= MAX2(result, 0.0f);
2610
2611         return result;
2612 }
2613
2614 #endif
2615
2616 /* SSE optimization, acos code doesn't work */
2617
2618 #if 0
2619
2620 #include <xmmintrin.h>
2621
2622 static __m128 sse_approx_acos(__m128 x)
2623 {
2624         /* needs a better approximation than taylor expansion of acos, since that
2625          * gives big erros for near 1.0 values, sqrt(2*x)*acos(1-x) should work
2626          * better, see http://www.tom.womack.net/projects/sse-fast-arctrig.html */
2627
2628         return _mm_set_ps1(1.0f);
2629 }
2630
2631 static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float *q2, float *q3)
2632 {
2633         float r0[3], r1[3], r2[3], r3[3], g0[3], g1[3], g2[3], g3[3];
2634         float a1, a2, a3, a4, dot1, dot2, dot3, dot4, result;
2635         float fresult[4] __attribute__((aligned(16)));
2636         __m128 qx, qy, qz, rx, ry, rz, rlen, srx, sry, srz, gx, gy, gz, glen, rcos, angle, aresult;
2637
2638         /* compute r */
2639         qx = _mm_set_ps(q3[0], q2[0], q1[0], q0[0]);
2640         qy = _mm_set_ps(q3[1], q2[1], q1[1], q0[1]);
2641         qz = _mm_set_ps(q3[2], q2[2], q1[2], q0[2]);
2642
2643         rx = qx - _mm_set_ps1(p[0]);
2644         ry = qy - _mm_set_ps1(p[1]);
2645         rz = qz - _mm_set_ps1(p[2]);
2646
2647         /* normalize r */
2648         rlen = _mm_rsqrt_ps(rx*rx + ry*ry + rz*rz + _mm_set_ps1(1e-16f));
2649         rx = rx*rlen;
2650         ry = ry*rlen;
2651         rz = rz*rlen;
2652
2653         /* cross product */
2654         srx = _mm_shuffle_ps(rx, rx, _MM_SHUFFLE(0,3,2,1));
2655         sry = _mm_shuffle_ps(ry, ry, _MM_SHUFFLE(0,3,2,1));
2656         srz = _mm_shuffle_ps(rz, rz, _MM_SHUFFLE(0,3,2,1));
2657
2658         gx = sry*rz - srz*ry;
2659         gy = srz*rx - srx*rz;
2660         gz = srx*ry - sry*rx;
2661
2662         /* normalize g */
2663         glen = _mm_rsqrt_ps(gx*gx + gy*gy + gz*gz + _mm_set_ps1(1e-16f));
2664         gx = gx*glen;
2665         gy = gy*glen;
2666         gz = gz*glen;
2667
2668         /* compute angle */
2669         rcos = rx*srx + ry*sry + rz*srz;
2670         rcos= _mm_max_ps(_mm_min_ps(rcos, _mm_set_ps1(1.0f)), _mm_set_ps1(-1.0f));
2671
2672         angle = sse_approx_cos(rcos);
2673         aresult = (_mm_set_ps1(n[0])*gx + _mm_set_ps1(n[1])*gy + _mm_set_ps1(n[2])*gz)*angle;
2674
2675         /* sum together */
2676         result= (fresult[0] + fresult[1] + fresult[2] + fresult[3])*(0.5f/(float)M_PI);
2677         result= MAX2(result, 0.0f);
2678
2679         return result;
2680 }
2681
2682 #endif
2683
2684 static void ff_normalize(float n[3])
2685 {
2686         float d;
2687         
2688         d= dot_v3v3(n, n);
2689
2690         if(d > 1.0e-35F) {
2691                 d= 1.0f/sqrtf(d);
2692
2693                 n[0] *= d; 
2694                 n[1] *= d; 
2695                 n[2] *= d;
2696         } 
2697 }
2698
2699 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])
2700 {
2701         float r0[3], r1[3], r2[3], r3[3], g0[3], g1[3], g2[3], g3[3];
2702         float a1, a2, a3, a4, dot1, dot2, dot3, dot4, result;
2703
2704         sub_v3_v3v3(r0, q0, p);
2705         sub_v3_v3v3(r1, q1, p);
2706         sub_v3_v3v3(r2, q2, p);
2707         sub_v3_v3v3(r3, q3, p);
2708
2709         ff_normalize(r0);
2710         ff_normalize(r1);
2711         ff_normalize(r2);
2712         ff_normalize(r3);
2713
2714         cross_v3_v3v3(g0, r1, r0); ff_normalize(g0);
2715         cross_v3_v3v3(g1, r2, r1); ff_normalize(g1);
2716         cross_v3_v3v3(g2, r3, r2); ff_normalize(g2);
2717         cross_v3_v3v3(g3, r0, r3); ff_normalize(g3);
2718
2719         a1= saacosf(dot_v3v3(r0, r1));
2720         a2= saacosf(dot_v3v3(r1, r2));
2721         a3= saacosf(dot_v3v3(r2, r3));
2722         a4= saacosf(dot_v3v3(r3, r0));
2723
2724         dot1= dot_v3v3(n, g0);
2725         dot2= dot_v3v3(n, g1);
2726         dot3= dot_v3v3(n, g2);
2727         dot4= dot_v3v3(n, g3);
2728
2729         result= (a1*dot1 + a2*dot2 + a3*dot3 + a4*dot4)*0.5f/(float)M_PI;
2730         result= MAX2(result, 0.0f);
2731
2732         return result;
2733 }
2734
2735 float form_factor_hemi_poly(float p[3], float n[3], float v1[3], float v2[3], float v3[3], float v4[3])
2736 {
2737         /* computes how much hemisphere defined by point and normal is
2738            covered by a quad or triangle, cosine weighted */
2739         float q0[3], q1[3], q2[3], q3[3], contrib= 0.0f;
2740
2741         if(ff_visible_quad(p, n, v1, v2, v3, q0, q1, q2, q3))
2742                 contrib += ff_quad_form_factor(p, n, q0, q1, q2, q3);
2743         
2744         if(v4 && ff_visible_quad(p, n, v1, v3, v4, q0, q1, q2, q3))
2745                 contrib += ff_quad_form_factor(p, n, q0, q1, q2, q3);
2746
2747         return contrib;
2748 }