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