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