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