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