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