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