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