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