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