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