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