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