Sculpt:
[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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, 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 #include <float.h>
29 #include <stdio.h>
30 #include <stdlib.h>
31 #include <string.h>
32
33 #include "BLI_math.h"
34 #include "BLI_memarena.h"
35
36 /********************************** Polygons *********************************/
37
38 void cent_tri_v3(float *cent, float *v1, float *v2, float *v3)
39 {
40         cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
41         cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
42         cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
43 }
44
45 void cent_quad_v3(float *cent, float *v1, float *v2, float *v3, float *v4)
46 {
47         cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
48         cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
49         cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
50 }
51
52 float normal_tri_v3(float *n, float *v1, float *v2, float *v3)
53 {
54         float n1[3],n2[3];
55
56         n1[0]= v1[0]-v2[0];
57         n2[0]= v2[0]-v3[0];
58         n1[1]= v1[1]-v2[1];
59         n2[1]= v2[1]-v3[1];
60         n1[2]= v1[2]-v2[2];
61         n2[2]= v2[2]-v3[2];
62         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
63         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
64         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
65         return normalize_v3(n);
66 }
67
68 float normal_quad_v3(float *n, float *v1, float *v2, float *v3, float *v4)
69 {
70         /* real cross! */
71         float n1[3],n2[3];
72
73         n1[0]= v1[0]-v3[0];
74         n1[1]= v1[1]-v3[1];
75         n1[2]= v1[2]-v3[2];
76
77         n2[0]= v2[0]-v4[0];
78         n2[1]= v2[1]-v4[1];
79         n2[2]= v2[2]-v4[2];
80
81         n[0]= n1[1]*n2[2]-n1[2]*n2[1];
82         n[1]= n1[2]*n2[0]-n1[0]*n2[2];
83         n[2]= n1[0]*n2[1]-n1[1]*n2[0];
84
85         return normalize_v3(n);
86 }
87
88 float area_tri_v2(float *v1, float *v2, float *v3)
89 {
90         return (float)(0.5*fabs((v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0])));
91 }
92
93
94 float area_quad_v3(float *v1, float *v2, float *v3,  float *v4)  /* only convex Quadrilaterals */
95 {
96         float len, vec1[3], vec2[3], n[3];
97
98         sub_v3_v3v3(vec1, v2, v1);
99         sub_v3_v3v3(vec2, v4, v1);
100         cross_v3_v3v3(n, vec1, vec2);
101         len= normalize_v3(n);
102
103         sub_v3_v3v3(vec1, v4, v3);
104         sub_v3_v3v3(vec2, v2, v3);
105         cross_v3_v3v3(n, vec1, vec2);
106         len+= normalize_v3(n);
107
108         return (len/2.0f);
109 }
110
111 float area_tri_v3(float *v1, float *v2, float *v3)  /* Triangles */
112 {
113         float len, vec1[3], vec2[3], n[3];
114
115         sub_v3_v3v3(vec1, v3, v2);
116         sub_v3_v3v3(vec2, v1, v2);
117         cross_v3_v3v3(n, vec1, vec2);
118         len= normalize_v3(n);
119
120         return (len/2.0f);
121 }
122
123 #define MAX2(x,y)               ((x)>(y) ? (x) : (y))
124 #define MAX3(x,y,z)             MAX2(MAX2((x),(y)) , (z))
125
126
127 float area_poly_v3(int nr, float verts[][3], float *normal)
128 {
129         float x, y, z, area, max;
130         float *cur, *prev;
131         int a, px=0, py=1;
132
133         /* first: find dominant axis: 0==X, 1==Y, 2==Z */
134         x= (float)fabs(normal[0]);
135         y= (float)fabs(normal[1]);
136         z= (float)fabs(normal[2]);
137         max = MAX3(x, y, z);
138         if(max==y) py=2;
139         else if(max==x) {
140                 px=1; 
141                 py= 2;
142         }
143
144         /* The Trapezium Area Rule */
145         prev= verts[nr-1];
146         cur= verts[0];
147         area= 0;
148         for(a=0; a<nr; a++) {
149                 area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
150                 prev= verts[a];
151                 cur= verts[a+1];
152         }
153
154         return (float)fabs(0.5*area/max);
155 }
156
157 /********************************* Distance **********************************/
158
159 /* distance v1 to line v2-v3 */
160 /* using Hesse formula, NO LINE PIECE! */
161 float dist_to_line_v2(float *v1, float *v2, float *v3)
162 {
163         float a[2],deler;
164
165         a[0]= v2[1]-v3[1];
166         a[1]= v3[0]-v2[0];
167         deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
168         if(deler== 0.0f) return 0;
169
170         return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
171
172 }
173
174 /* distance v1 to line-piece v2-v3 */
175 float dist_to_line_segment_v2(float *v1, float *v2, float *v3) 
176 {
177         float labda, rc[2], pt[2], len;
178         
179         rc[0]= v3[0]-v2[0];
180         rc[1]= v3[1]-v2[1];
181         len= rc[0]*rc[0]+ rc[1]*rc[1];
182         if(len==0.0) {
183                 rc[0]= v1[0]-v2[0];
184                 rc[1]= v1[1]-v2[1];
185                 return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
186         }
187         
188         labda= (rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]))/len;
189         if(labda<=0.0) {
190                 pt[0]= v2[0];
191                 pt[1]= v2[1];
192         }
193         else if(labda>=1.0) {
194                 pt[0]= v3[0];
195                 pt[1]= v3[1];
196         }
197         else {
198                 pt[0]= labda*rc[0]+v2[0];
199                 pt[1]= labda*rc[1]+v2[1];
200         }
201
202         rc[0]= pt[0]-v1[0];
203         rc[1]= pt[1]-v1[1];
204         return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
205 }
206
207 /* point closest to v1 on line v2-v3 in 3D */
208 void closest_to_line_segment_v3(float *closest, float v1[3], float v2[3], float v3[3])
209 {
210         float lambda, cp[3];
211
212         lambda= closest_to_line_v3(cp,v1, v2, v3);
213
214         if(lambda <= 0.0f)
215                 copy_v3_v3(closest, v2);
216         else if(lambda >= 1.0f)
217                 copy_v3_v3(closest, v3);
218         else
219                 copy_v3_v3(closest, cp);
220 }
221
222 /* distance v1 to line-piece v2-v3 in 3D */
223 float dist_to_line_segment_v3(float *v1, float *v2, float *v3) 
224 {
225         float closest[3];
226
227         closest_to_line_segment_v3(closest, v1, v2, v3);
228
229         return len_v3v3(closest, v1);
230 }
231
232 /******************************* Intersection ********************************/
233
234 /* intersect Line-Line, shorts */
235 int isect_line_line_v2_short(short *v1, short *v2, short *v3, short *v4)
236 {
237         /* return:
238         -1: colliniar
239          0: no intersection of segments
240          1: exact intersection of segments
241          2: cross-intersection of segments
242         */
243         float div, labda, mu;
244         
245         div= (float)((v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]));
246         if(div==0.0f) return -1;
247         
248         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
249         
250         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
251         
252         if(labda>=0.0f && labda<=1.0f && mu>=0.0f && mu<=1.0f) {
253                 if(labda==0.0f || labda==1.0f || mu==0.0f || mu==1.0f) return 1;
254                 return 2;
255         }
256         return 0;
257 }
258
259 /* intersect Line-Line, floats */
260 int isect_line_line_v2(float *v1, float *v2, float *v3, float *v4)
261 {
262         /* return:
263         -1: colliniar
264 0: no intersection of segments
265 1: exact intersection of segments
266 2: cross-intersection of segments
267         */
268         float div, labda, mu;
269         
270         div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
271         if(div==0.0) return -1;
272         
273         labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
274         
275         mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
276         
277         if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
278                 if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
279                 return 2;
280         }
281         return 0;
282 }
283
284 /*
285 -1: colliniar
286  1: intersection
287
288 */
289 static short IsectLLPt2Df(float x0,float y0,float x1,float y1,
290                                          float x2,float y2,float x3,float y3, float *xi,float *yi)
291
292 {
293         /*
294         this function computes the intersection of the sent lines
295         and returns the intersection point, note that the function assumes
296         the lines intersect. the function can handle vertical as well
297         as horizontal lines. note the function isn't very clever, it simply
298         applies the math, but we don't need speed since this is a
299         pre-processing step
300         */
301         float c1,c2, // constants of linear equations
302         det_inv,  // the inverse of the determinant of the coefficient
303         m1,m2;    // the slopes of each line
304         /*
305         compute slopes, note the cludge for infinity, however, this will
306         be close enough
307         */
308         if (fabs(x1-x0) > 0.000001)
309                 m1 = (y1-y0) / (x1-x0);
310         else
311                 return -1; /*m1 = (float) 1e+10;*/   // close enough to infinity
312
313         if (fabs(x3-x2) > 0.000001)
314                 m2 = (y3-y2) / (x3-x2);
315         else
316                 return -1; /*m2 = (float) 1e+10;*/   // close enough to infinity
317
318         if (fabs(m1-m2) < 0.000001)
319                 return -1; /* paralelle lines */
320         
321 // compute constants
322
323         c1 = (y0-m1*x0);
324         c2 = (y2-m2*x2);
325
326 // compute the inverse of the determinate
327
328         det_inv = 1.0f / (-m1 + m2);
329
330 // use Kramers rule to compute xi and yi
331
332         *xi= ((-c2 + c1) *det_inv);
333         *yi= ((m2*c1 - m1*c2) *det_inv);
334         
335         return 1; 
336 } // end Intersect_Lines
337
338 #define SIDE_OF_LINE(pa,pb,pp)  ((pa[0]-pp[0])*(pb[1]-pp[1]))-((pb[0]-pp[0])*(pa[1]-pp[1]))
339 /* point in tri */
340 // XXX was called IsectPT2Df
341 int isect_point_tri_v2(float pt[2], float v1[2], float v2[2], float v3[2])
342 {
343         if (SIDE_OF_LINE(v1,v2,pt)>=0.0) {
344                 if (SIDE_OF_LINE(v2,v3,pt)>=0.0) {
345                         if (SIDE_OF_LINE(v3,v1,pt)>=0.0) {
346                                 return 1;
347                         }
348                 }
349         } else {
350                 if (! (SIDE_OF_LINE(v2,v3,pt)>=0.0)) {
351                         if (! (SIDE_OF_LINE(v3,v1,pt)>=0.0)) {
352                                 return -1;
353                         }
354                 }
355         }
356         
357         return 0;
358 }
359 /* point in quad - only convex quads */
360 int isect_point_quad_v2(float pt[2], float v1[2], float v2[2], float v3[2], float v4[2])
361 {
362         if (SIDE_OF_LINE(v1,v2,pt)>=0.0) {
363                 if (SIDE_OF_LINE(v2,v3,pt)>=0.0) {
364                         if (SIDE_OF_LINE(v3,v4,pt)>=0.0) {
365                                 if (SIDE_OF_LINE(v4,v1,pt)>=0.0) {
366                                         return 1;
367                                 }
368                         }
369                 }
370         } else {
371                 if (! (SIDE_OF_LINE(v2,v3,pt)>=0.0)) {
372                         if (! (SIDE_OF_LINE(v3,v4,pt)>=0.0)) {
373                                 if (! (SIDE_OF_LINE(v4,v1,pt)>=0.0)) {
374                                         return -1;
375                                 }
376                         }
377                 }
378         }
379         
380         return 0;
381 }
382
383 /* moved from effect.c
384    test if the line starting at p1 ending at p2 intersects the triangle v0..v2
385    return non zero if it does 
386 */
387 int isect_line_tri_v3(float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda, float *uv)
388 {
389
390         float p[3], s[3], d[3], e1[3], e2[3], q[3];
391         float a, f, u, v;
392         
393         sub_v3_v3v3(e1, v1, v0);
394         sub_v3_v3v3(e2, v2, v0);
395         sub_v3_v3v3(d, p2, p1);
396         
397         cross_v3_v3v3(p, d, e2);
398         a = dot_v3v3(e1, p);
399         if ((a > -0.000001) && (a < 0.000001)) return 0;
400         f = 1.0f/a;
401         
402         sub_v3_v3v3(s, p1, v0);
403         
404         cross_v3_v3v3(q, s, e1);
405         *lambda = f * dot_v3v3(e2, q);
406         if ((*lambda < 0.0)||(*lambda > 1.0)) return 0;
407         
408         u = f * dot_v3v3(s, p);
409         if ((u < 0.0)||(u > 1.0)) return 0;
410         
411         v = f * dot_v3v3(d, q);
412         if ((v < 0.0)||((u + v) > 1.0)) return 0;
413
414         if(uv) {
415                 uv[0]= u;
416                 uv[1]= v;
417         }
418         
419         return 1;
420 }
421
422 /* moved from effect.c
423    test if the ray starting at p1 going in d direction intersects the triangle v0..v2
424    return non zero if it does 
425 */
426 int isect_ray_tri_v3(float p1[3], float d[3], float v0[3], float v1[3], float v2[3], float *lambda, float *uv)
427 {
428         float p[3], s[3], e1[3], e2[3], q[3];
429         float a, f, u, v;
430         
431         sub_v3_v3v3(e1, v1, v0);
432         sub_v3_v3v3(e2, v2, v0);
433         
434         cross_v3_v3v3(p, d, e2);
435         a = dot_v3v3(e1, p);
436         /* note: these values were 0.000001 in 2.4x but for projection snapping on
437          * a human head (1BU==1m), subsurf level 2, this gave many errors - campbell */
438         if ((a > -0.00000001) && (a < 0.00000001)) return 0;
439         f = 1.0f/a;
440         
441         sub_v3_v3v3(s, p1, v0);
442         
443         cross_v3_v3v3(q, s, e1);
444         *lambda = f * dot_v3v3(e2, q);
445         if ((*lambda < 0.0)) return 0;
446         
447         u = f * dot_v3v3(s, p);
448         if ((u < 0.0)||(u > 1.0)) return 0;
449         
450         v = f * dot_v3v3(d, q);
451         if ((v < 0.0)||((u + v) > 1.0)) return 0;
452
453         if(uv) {
454                 uv[0]= u;
455                 uv[1]= v;
456         }
457         
458         return 1;
459 }
460
461 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)
462 {
463         float p[3], s[3], e1[3], e2[3], q[3];
464         float a, f, u, v;
465         
466         sub_v3_v3v3(e1, v1, v0);
467         sub_v3_v3v3(e2, v2, v0);
468         
469         cross_v3_v3v3(p, d, e2);
470         a = dot_v3v3(e1, p);
471         if (a == 0.0f) return 0;
472         f = 1.0f/a;
473         
474         sub_v3_v3v3(s, p1, v0);
475         
476         cross_v3_v3v3(q, s, e1);
477         *lambda = f * dot_v3v3(e2, q);
478         if ((*lambda < 0.0)) return 0;
479         
480         u = f * dot_v3v3(s, p);
481         if ((u < -epsilon)||(u > 1.0+epsilon)) return 0;
482         
483         v = f * dot_v3v3(d, q);
484         if ((v < -epsilon)||((u + v) > 1.0+epsilon)) return 0;
485
486         if(uv) {
487                 uv[0]= u;
488                 uv[1]= v;
489         }
490         
491         return 1;
492 }
493
494 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)
495 {
496         float p[3], s[3], e1[3], e2[3], q[3];
497         float a, f, u, v;
498         float du = 0, dv = 0;
499         
500         sub_v3_v3v3(e1, v1, v0);
501         sub_v3_v3v3(e2, v2, v0);
502         
503         cross_v3_v3v3(p, d, e2);
504         a = dot_v3v3(e1, p);
505         if ((a > -0.000001) && (a < 0.000001)) return 0;
506         f = 1.0f/a;
507         
508         sub_v3_v3v3(s, p1, v0);
509         
510         cross_v3_v3v3(q, s, e1);
511         *lambda = f * dot_v3v3(e2, q);
512         if ((*lambda < 0.0)) return 0;
513         
514         u = f * dot_v3v3(s, p);
515         v = f * dot_v3v3(d, q);
516         
517         if (u < 0) du = u;
518         if (u > 1) du = u - 1;
519         if (v < 0) dv = v;
520         if (v > 1) dv = v - 1;
521         if (u > 0 && v > 0 && u + v > 1)
522         {
523                 float t = u + v - 1;
524                 du = u - t/2;
525                 dv = v - t/2;
526         }
527
528         mul_v3_fl(e1, du);
529         mul_v3_fl(e2, dv);
530         
531         if (dot_v3v3(e1, e1) + dot_v3v3(e2, e2) > threshold * threshold)
532         {
533                 return 0;
534         }
535
536         if(uv) {
537                 uv[0]= u;
538                 uv[1]= v;
539         }
540         
541         return 1;
542 }
543
544
545 /* Adapted from the paper by Kasper Fauerby */
546 /* "Improved Collision detection and Response" */
547 static int getLowestRoot(float a, float b, float c, float maxR, float* root)
548 {
549         // Check if a solution exists
550         float determinant = b*b - 4.0f*a*c;
551
552         // If determinant is negative it means no solutions.
553         if (determinant >= 0.0f)
554         {
555                 // calculate the two roots: (if determinant == 0 then
556                 // x1==x2 but let’s disregard that slight optimization)
557                 float sqrtD = (float)sqrt(determinant);
558                 float r1 = (-b - sqrtD) / (2.0f*a);
559                 float r2 = (-b + sqrtD) / (2.0f*a);
560                 
561                 // Sort so x1 <= x2
562                 if (r1 > r2)
563                         SWAP(float, r1, r2);
564
565                 // Get lowest root:
566                 if (r1 > 0.0f && r1 < maxR)
567                 {
568                         *root = r1;
569                         return 1;
570                 }
571
572                 // It is possible that we want x2 - this can happen
573                 // if x1 < 0
574                 if (r2 > 0.0f && r2 < maxR)
575                 {
576                         *root = r2;
577                         return 1;
578                 }
579         }
580         // No (valid) solutions
581         return 0;
582 }
583
584 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)
585 {
586         float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3];
587         float a, b, c, d, e, x, y, z, radius2=radius*radius;
588         float elen2,edotv,edotbv,nordotv,vel2;
589         float newLambda;
590         int found_by_sweep=0;
591
592         sub_v3_v3v3(e1,v1,v0);
593         sub_v3_v3v3(e2,v2,v0);
594         sub_v3_v3v3(vel,p2,p1);
595
596 /*---test plane of tri---*/
597         cross_v3_v3v3(nor,e1,e2);
598         normalize_v3(nor);
599
600         /* flip normal */
601         if(dot_v3v3(nor,vel)>0.0f) negate_v3(nor);
602         
603         a=dot_v3v3(p1,nor)-dot_v3v3(v0,nor);
604         nordotv=dot_v3v3(nor,vel);
605
606         if (fabs(nordotv) < 0.000001)
607         {
608                 if(fabs(a)>=radius)
609                 {
610                         return 0;
611                 }
612         }
613         else
614         {
615                 float t0=(-a+radius)/nordotv;
616                 float t1=(-a-radius)/nordotv;
617
618                 if(t0>t1)
619                         SWAP(float, t0, t1);
620
621                 if(t0>1.0f || t1<0.0f) return 0;
622
623                 /* clamp to [0,1] */
624                 CLAMP(t0, 0.0f, 1.0f);
625                 CLAMP(t1, 0.0f, 1.0f);
626
627                 /*---test inside of tri---*/
628                 /* plane intersection point */
629
630                 point[0] = p1[0] + vel[0]*t0 - nor[0]*radius;
631                 point[1] = p1[1] + vel[1]*t0 - nor[1]*radius;
632                 point[2] = p1[2] + vel[2]*t0 - nor[2]*radius;
633
634
635                 /* is the point in the tri? */
636                 a=dot_v3v3(e1,e1);
637                 b=dot_v3v3(e1,e2);
638                 c=dot_v3v3(e2,e2);
639
640                 sub_v3_v3v3(temp,point,v0);
641                 d=dot_v3v3(temp,e1);
642                 e=dot_v3v3(temp,e2);
643                 
644                 x=d*c-e*b;
645                 y=e*a-d*b;
646                 z=x+y-(a*c-b*b);
647
648
649                 if(z <= 0.0f && (x >= 0.0f && y >= 0.0f))
650                 {
651                 //(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000){
652                         *lambda=t0;
653                         copy_v3_v3(ipoint,point);
654                         return 1;
655                 }
656         }
657
658
659         *lambda=1.0f;
660
661 /*---test points---*/
662         a=vel2=dot_v3v3(vel,vel);
663
664         /*v0*/
665         sub_v3_v3v3(temp,p1,v0);
666         b=2.0f*dot_v3v3(vel,temp);
667         c=dot_v3v3(temp,temp)-radius2;
668
669         if(getLowestRoot(a, b, c, *lambda, lambda))
670         {
671                 copy_v3_v3(ipoint,v0);
672                 found_by_sweep=1;
673         }
674
675         /*v1*/
676         sub_v3_v3v3(temp,p1,v1);
677         b=2.0f*dot_v3v3(vel,temp);
678         c=dot_v3v3(temp,temp)-radius2;
679
680         if(getLowestRoot(a, b, c, *lambda, lambda))
681         {
682                 copy_v3_v3(ipoint,v1);
683                 found_by_sweep=1;
684         }
685         
686         /*v2*/
687         sub_v3_v3v3(temp,p1,v2);
688         b=2.0f*dot_v3v3(vel,temp);
689         c=dot_v3v3(temp,temp)-radius2;
690
691         if(getLowestRoot(a, b, c, *lambda, lambda))
692         {
693                 copy_v3_v3(ipoint,v2);
694                 found_by_sweep=1;
695         }
696
697 /*---test edges---*/
698         sub_v3_v3v3(e3,v2,v1); //wasnt yet calculated
699
700
701         /*e1*/
702         sub_v3_v3v3(bv,v0,p1);
703
704         elen2 = dot_v3v3(e1,e1);
705         edotv = dot_v3v3(e1,vel);
706         edotbv = dot_v3v3(e1,bv);
707
708         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
709         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
710         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
711
712         if(getLowestRoot(a, b, c, *lambda, &newLambda))
713         {
714                 e=(edotv*newLambda-edotbv)/elen2;
715
716                 if(e >= 0.0f && e <= 1.0f)
717                 {
718                         *lambda = newLambda;
719                         copy_v3_v3(ipoint,e1);
720                         mul_v3_fl(ipoint,e);
721                         add_v3_v3v3(ipoint,ipoint,v0);
722                         found_by_sweep=1;
723                 }
724         }
725
726         /*e2*/
727         /*bv is same*/
728         elen2 = dot_v3v3(e2,e2);
729         edotv = dot_v3v3(e2,vel);
730         edotbv = dot_v3v3(e2,bv);
731
732         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
733         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
734         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
735
736         if(getLowestRoot(a, b, c, *lambda, &newLambda))
737         {
738                 e=(edotv*newLambda-edotbv)/elen2;
739
740                 if(e >= 0.0f && e <= 1.0f)
741                 {
742                         *lambda = newLambda;
743                         copy_v3_v3(ipoint,e2);
744                         mul_v3_fl(ipoint,e);
745                         add_v3_v3v3(ipoint,ipoint,v0);
746                         found_by_sweep=1;
747                 }
748         }
749
750         /*e3*/
751         sub_v3_v3v3(bv,v0,p1);
752         elen2 = dot_v3v3(e1,e1);
753         edotv = dot_v3v3(e1,vel);
754         edotbv = dot_v3v3(e1,bv);
755
756         sub_v3_v3v3(bv,v1,p1);
757         elen2 = dot_v3v3(e3,e3);
758         edotv = dot_v3v3(e3,vel);
759         edotbv = dot_v3v3(e3,bv);
760
761         a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
762         b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
763         c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
764
765         if(getLowestRoot(a, b, c, *lambda, &newLambda))
766         {
767                 e=(edotv*newLambda-edotbv)/elen2;
768
769                 if(e >= 0.0f && e <= 1.0f)
770                 {
771                         *lambda = newLambda;
772                         copy_v3_v3(ipoint,e3);
773                         mul_v3_fl(ipoint,e);
774                         add_v3_v3v3(ipoint,ipoint,v1);
775                         found_by_sweep=1;
776                 }
777         }
778
779
780         return found_by_sweep;
781 }
782 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)
783 {
784         float p[3], e1[3], e2[3];
785         float u, v, f;
786         int a0=axis, a1=(axis+1)%3, a2=(axis+2)%3;
787
788         //return isect_line_tri_v3(p1,p2,v0,v1,v2,lambda);
789
790         ///* first a simple bounding box test */
791         //if(MIN3(v0[a1],v1[a1],v2[a1]) > p1[a1]) return 0;
792         //if(MIN3(v0[a2],v1[a2],v2[a2]) > p1[a2]) return 0;
793         //if(MAX3(v0[a1],v1[a1],v2[a1]) < p1[a1]) return 0;
794         //if(MAX3(v0[a2],v1[a2],v2[a2]) < p1[a2]) return 0;
795
796         ///* then a full intersection test */
797         
798         sub_v3_v3v3(e1,v1,v0);
799         sub_v3_v3v3(e2,v2,v0);
800         sub_v3_v3v3(p,v0,p1);
801
802         f= (e2[a1]*e1[a2]-e2[a2]*e1[a1]);
803         if ((f > -0.000001) && (f < 0.000001)) return 0;
804
805         v= (p[a2]*e1[a1]-p[a1]*e1[a2])/f;
806         if ((v < 0.0)||(v > 1.0)) return 0;
807         
808         f= e1[a1];
809         if((f > -0.000001) && (f < 0.000001)){
810                 f= e1[a2];
811                 if((f > -0.000001) && (f < 0.000001)) return 0;
812                 u= (-p[a2]-v*e2[a2])/f;
813         }
814         else
815                 u= (-p[a1]-v*e2[a1])/f;
816
817         if ((u < 0.0)||((u + v) > 1.0)) return 0;
818
819         *lambda = (p[a0]+u*e1[a0]+v*e2[a0])/(p2[a0]-p1[a0]);
820
821         if ((*lambda < 0.0)||(*lambda > 1.0)) return 0;
822
823         return 1;
824 }
825
826 /* Returns the number of point of interests
827  * 0 - lines are colinear
828  * 1 - lines are coplanar, i1 is set to intersection
829  * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively 
830  * */
831 int isect_line_line_v3(float v1[3], float v2[3], float v3[3], float v4[3], float i1[3], float i2[3])
832 {
833         float a[3], b[3], c[3], ab[3], cb[3], dir1[3], dir2[3];
834         float d;
835         
836         sub_v3_v3v3(c, v3, v1);
837         sub_v3_v3v3(a, v2, v1);
838         sub_v3_v3v3(b, v4, v3);
839
840         copy_v3_v3(dir1, a);
841         normalize_v3(dir1);
842         copy_v3_v3(dir2, b);
843         normalize_v3(dir2);
844         d = dot_v3v3(dir1, dir2);
845         if (d == 1.0f || d == -1.0f) {
846                 /* colinear */
847                 return 0;
848         }
849
850         cross_v3_v3v3(ab, a, b);
851         d = dot_v3v3(c, ab);
852
853         /* test if the two lines are coplanar */
854         if (d > -0.000001f && d < 0.000001f) {
855                 cross_v3_v3v3(cb, c, b);
856
857                 mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
858                 add_v3_v3v3(i1, v1, a);
859                 copy_v3_v3(i2, i1);
860                 
861                 return 1; /* one intersection only */
862         }
863         /* if not */
864         else {
865                 float n[3], t[3];
866                 float v3t[3], v4t[3];
867                 sub_v3_v3v3(t, v1, v3);
868
869                 /* offset between both plane where the lines lies */
870                 cross_v3_v3v3(n, a, b);
871                 project_v3_v3v3(t, t, n);
872
873                 /* for the first line, offset the second line until it is coplanar */
874                 add_v3_v3v3(v3t, v3, t);
875                 add_v3_v3v3(v4t, v4, t);
876                 
877                 sub_v3_v3v3(c, v3t, v1);
878                 sub_v3_v3v3(a, v2, v1);
879                 sub_v3_v3v3(b, v4t, v3t);
880
881                 cross_v3_v3v3(ab, a, b);
882                 cross_v3_v3v3(cb, c, b);
883
884                 mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
885                 add_v3_v3v3(i1, v1, a);
886
887                 /* for the second line, just substract the offset from the first intersection point */
888                 sub_v3_v3v3(i2, i1, t);
889                 
890                 return 2; /* two nearest points */
891         }
892
893
894 /* Intersection point strictly between the two lines
895  * 0 when no intersection is found 
896  * */
897 int isect_line_line_strict_v3(float v1[3], float v2[3], float v3[3], float v4[3], float vi[3], float *lambda)
898 {
899         float a[3], b[3], c[3], ab[3], cb[3], ca[3], dir1[3], dir2[3];
900         float d;
901         float d1;
902         
903         sub_v3_v3v3(c, v3, v1);
904         sub_v3_v3v3(a, v2, v1);
905         sub_v3_v3v3(b, v4, v3);
906
907         copy_v3_v3(dir1, a);
908         normalize_v3(dir1);
909         copy_v3_v3(dir2, b);
910         normalize_v3(dir2);
911         d = dot_v3v3(dir1, dir2);
912         if (d == 1.0f || d == -1.0f || d == 0) {
913                 /* colinear or one vector is zero-length*/
914                 return 0;
915         }
916         
917         d1 = d;
918
919         cross_v3_v3v3(ab, a, b);
920         d = dot_v3v3(c, ab);
921
922         /* test if the two lines are coplanar */
923         if (d > -0.000001f && d < 0.000001f) {
924                 float f1, f2;
925                 cross_v3_v3v3(cb, c, b);
926                 cross_v3_v3v3(ca, c, a);
927
928                 f1 = dot_v3v3(cb, ab) / dot_v3v3(ab, ab);
929                 f2 = dot_v3v3(ca, ab) / dot_v3v3(ab, ab);
930                 
931                 if (f1 >= 0 && f1 <= 1 &&
932                         f2 >= 0 && f2 <= 1)
933                 {
934                         mul_v3_fl(a, f1);
935                         add_v3_v3v3(vi, v1, a);
936                         
937                         if (lambda != NULL)
938                         {
939                                 *lambda = f1;
940                         }
941                         
942                         return 1; /* intersection found */
943                 }
944                 else
945                 {
946                         return 0;
947                 }
948         }
949         else
950         {
951                 return 0;
952         }
953
954
955 int isect_aabb_aabb_v3(float min1[3], float max1[3], float min2[3], float max2[3])
956 {
957         return (min1[0]<max2[0] && min1[1]<max2[1] && min1[2]<max2[2] &&
958                 min2[0]<max1[0] && min2[1]<max1[1] && min2[2]<max1[2]);
959 }
960
961 /* find closest point to p on line through l1,l2 and return lambda,
962  * where (0 <= lambda <= 1) when cp is in the line segement l1,l2
963  */
964 float closest_to_line_v3(float cp[3],float p[3], float l1[3], float l2[3])
965 {
966         float h[3],u[3],lambda;
967         sub_v3_v3v3(u, l2, l1);
968         sub_v3_v3v3(h, p, l1);
969         lambda =dot_v3v3(u,h)/dot_v3v3(u,u);
970         cp[0] = l1[0] + u[0] * lambda;
971         cp[1] = l1[1] + u[1] * lambda;
972         cp[2] = l1[2] + u[2] * lambda;
973         return lambda;
974 }
975
976 #if 0
977 /* little sister we only need to know lambda */
978 static float lambda_cp_line(float p[3], float l1[3], float l2[3])
979 {
980         float h[3],u[3];
981         sub_v3_v3v3(u, l2, l1);
982         sub_v3_v3v3(h, p, l1);
983         return(dot_v3v3(u,h)/dot_v3v3(u,u));
984 }
985 #endif
986
987 /* Similar to LineIntersectsTriangleUV, except it operates on a quad and in 2d, assumes point is in quad */
988 void isect_point_quad_uv_v2(float v0[2], float v1[2], float v2[2], float v3[2], float pt[2], float *uv)
989 {
990         float x0,y0, x1,y1, wtot, v2d[2], w1, w2;
991         
992         /* used for paralelle lines */
993         float pt3d[3], l1[3], l2[3], pt_on_line[3];
994         
995         /* compute 2 edges  of the quad  intersection point */
996         if (IsectLLPt2Df(v0[0],v0[1],v1[0],v1[1],  v2[0],v2[1],v3[0],v3[1], &x0,&y0) == 1) {
997                 /* the intersection point between the quad-edge intersection and the point in the quad we want the uv's for */
998                 /* should never be paralle !! */
999                 /*printf("\tnot paralelle 1\n");*/
1000                 IsectLLPt2Df(pt[0],pt[1],x0,y0,  v0[0],v0[1],v3[0],v3[1], &x1,&y1);
1001                 
1002                 /* Get the weights from the new intersection point, to each edge */
1003                 v2d[0] = x1-v0[0];
1004                 v2d[1] = y1-v0[1];
1005                 w1 = len_v2(v2d);
1006                 
1007                 v2d[0] = x1-v3[0]; /* some but for the other vert */
1008                 v2d[1] = y1-v3[1];
1009                 w2 = len_v2(v2d);
1010                 wtot = w1+w2;
1011                 /*w1 = w1/wtot;*/
1012                 /*w2 = w2/wtot;*/
1013                 uv[0] = w1/wtot;
1014         } else {
1015                 /* lines are paralelle, lambda_cp_line_ex is 3d grrr */
1016                 /*printf("\tparalelle1\n");*/
1017                 pt3d[0] = pt[0];
1018                 pt3d[1] = pt[1];
1019                 pt3d[2] = l1[2] = l2[2] = 0.0f;
1020                 
1021                 l1[0] = v0[0]; l1[1] = v0[1];
1022                 l2[0] = v1[0]; l2[1] = v1[1];
1023                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1024                 v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */
1025                 v2d[1] = pt[1]-pt_on_line[1];
1026                 w1 = len_v2(v2d);
1027                 
1028                 l1[0] = v2[0]; l1[1] = v2[1];
1029                 l2[0] = v3[0]; l2[1] = v3[1];
1030                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1031                 v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */
1032                 v2d[1] = pt[1]-pt_on_line[1];
1033                 w2 = len_v2(v2d);
1034                 wtot = w1+w2;
1035                 uv[0] = w1/wtot;        
1036         }
1037         
1038         /* Same as above to calc the uv[1] value, alternate calculation */
1039         
1040         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*/
1041                 /* never paralle if above was not */
1042                 /*printf("\tnot paralelle2\n");*/
1043                 IsectLLPt2Df(pt[0],pt[1],x0,y0,  v0[0],v0[1],v1[0],v1[1], &x1,&y1);/* was v0,v3  now v0,v1*/
1044                 
1045                 v2d[0] = x1-v0[0];
1046                 v2d[1] = y1-v0[1];
1047                 w1 = len_v2(v2d);
1048                 
1049                 v2d[0] = x1-v1[0];
1050                 v2d[1] = y1-v1[1];
1051                 w2 = len_v2(v2d);
1052                 wtot = w1+w2;
1053                 uv[1] = w1/wtot;
1054         } else {
1055                 /* lines are paralelle, lambda_cp_line_ex is 3d grrr */
1056                 /*printf("\tparalelle2\n");*/
1057                 pt3d[0] = pt[0];
1058                 pt3d[1] = pt[1];
1059                 pt3d[2] = l1[2] = l2[2] = 0.0f;
1060                 
1061                 
1062                 l1[0] = v0[0]; l1[1] = v0[1];
1063                 l2[0] = v3[0]; l2[1] = v3[1];
1064                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1065                 v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */
1066                 v2d[1] = pt[1]-pt_on_line[1];
1067                 w1 = len_v2(v2d);
1068                 
1069                 l1[0] = v1[0]; l1[1] = v1[1];
1070                 l2[0] = v2[0]; l2[1] = v2[1];
1071                 closest_to_line_v3(pt_on_line,pt3d, l1, l2);
1072                 v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */
1073                 v2d[1] = pt[1]-pt_on_line[1];
1074                 w2 = len_v2(v2d);
1075                 wtot = w1+w2;
1076                 uv[1] = w1/wtot;
1077         }
1078         /* may need to flip UV's here */
1079 }
1080
1081 /* same as above but does tri's and quads, tri's are a bit of a hack */
1082 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)
1083 {
1084         if (isquad) {
1085                 isect_point_quad_uv_v2(v0, v1, v2, v3, pt, uv);
1086         }
1087         else {
1088                 /* not for quads, use for our abuse of LineIntersectsTriangleUV */
1089                 float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3], lambda;
1090                         
1091                 p1_3d[0] = p2_3d[0] = uv[0];
1092                 p1_3d[1] = p2_3d[1] = uv[1];
1093                 p1_3d[2] = 1.0f;
1094                 p2_3d[2] = -1.0f;
1095                 v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0;
1096                 
1097                 /* generate a new fuv, (this is possibly a non optimal solution,
1098                  * since we only need 2d calculation but use 3d func's)
1099                  * 
1100                  * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face
1101                  * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV.
1102                  * This means the new values will be correct in relation to the derived meshes face. 
1103                  */
1104                 copy_v2_v2(v0_3d, v0);
1105                 copy_v2_v2(v1_3d, v1);
1106                 copy_v2_v2(v2_3d, v2);
1107                 
1108                 /* Doing this in 3D is not nice */
1109                 isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv);
1110         }
1111 }
1112
1113 #if 0 // XXX this version used to be used in isect_point_tri_v2_int() and was called IsPointInTri2D
1114 int isect_point_tri_v2(float pt[2], float v1[2], float v2[2], float v3[2])
1115 {
1116         float inp1, inp2, inp3;
1117         
1118         inp1= (v2[0]-v1[0])*(v1[1]-pt[1]) + (v1[1]-v2[1])*(v1[0]-pt[0]);
1119         inp2= (v3[0]-v2[0])*(v2[1]-pt[1]) + (v2[1]-v3[1])*(v2[0]-pt[0]);
1120         inp3= (v1[0]-v3[0])*(v3[1]-pt[1]) + (v3[1]-v1[1])*(v3[0]-pt[0]);
1121         
1122         if(inp1<=0.0f && inp2<=0.0f && inp3<=0.0f) return 1;
1123         if(inp1>=0.0f && inp2>=0.0f && inp3>=0.0f) return 1;
1124         
1125         return 0;
1126 }
1127 #endif
1128
1129 #if 0
1130 int isect_point_tri_v2(float v0[2], float v1[2], float v2[2], float pt[2])
1131 {
1132                 /* not for quads, use for our abuse of LineIntersectsTriangleUV */
1133                 float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3];
1134                 /* not used */
1135                 float lambda, uv[3];
1136                         
1137                 p1_3d[0] = p2_3d[0] = uv[0]= pt[0];
1138                 p1_3d[1] = p2_3d[1] = uv[1]= uv[2]= pt[1];
1139                 p1_3d[2] = 1.0f;
1140                 p2_3d[2] = -1.0f;
1141                 v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0;
1142                 
1143                 /* generate a new fuv, (this is possibly a non optimal solution,
1144                  * since we only need 2d calculation but use 3d func's)
1145                  * 
1146                  * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face
1147                  * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV.
1148                  * This means the new values will be correct in relation to the derived meshes face. 
1149                  */
1150                 copy_v2_v2(v0_3d, v0);
1151                 copy_v2_v2(v1_3d, v1);
1152                 copy_v2_v2(v2_3d, v2);
1153                 
1154                 /* Doing this in 3D is not nice */
1155                 return isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv);
1156 }
1157 #endif
1158
1159 /*
1160
1161         x1,y2
1162         |  \
1163         |   \     .(a,b)
1164         |    \
1165         x1,y1-- x2,y1
1166
1167 */
1168 int isect_point_tri_v2_int(int x1, int y1, int x2, int y2, int a, int b)
1169 {
1170         float v1[2], v2[2], v3[2], p[2];
1171         
1172         v1[0]= (float)x1;
1173         v1[1]= (float)y1;
1174         
1175         v2[0]= (float)x1;
1176         v2[1]= (float)y2;
1177         
1178         v3[0]= (float)x2;
1179         v3[1]= (float)y1;
1180         
1181         p[0]= (float)a;
1182         p[1]= (float)b;
1183         
1184         return isect_point_tri_v2(p, v1, v2, v3);
1185 }
1186
1187 static int point_in_slice(float p[3], float v1[3], float l1[3], float l2[3])
1188 {
1189 /* 
1190 what is a slice ?
1191 some maths:
1192 a line including l1,l2 and a point not on the line 
1193 define a subset of R3 delimeted by planes parallel to the line and orthogonal 
1194 to the (point --> line) distance vector,one plane on the line one on the point, 
1195 the room inside usually is rather small compared to R3 though still infinte
1196 useful for restricting (speeding up) searches 
1197 e.g. all points of triangular prism are within the intersection of 3 'slices'
1198 onother trivial case : cube 
1199 but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
1200 */
1201         float h,rp[3],cp[3],q[3];
1202
1203         closest_to_line_v3(cp,v1,l1,l2);
1204         sub_v3_v3v3(q,cp,v1);
1205
1206         sub_v3_v3v3(rp,p,v1);
1207         h=dot_v3v3(q,rp)/dot_v3v3(q,q);
1208         if (h < 0.0f || h > 1.0f) return 0;
1209         return 1;
1210 }
1211
1212 #if 0
1213 /*adult sister defining the slice planes by the origin and the normal  
1214 NOTE |normal| may not be 1 but defining the thickness of the slice*/
1215 static int point_in_slice_as(float p[3],float origin[3],float normal[3])
1216 {
1217         float h,rp[3];
1218         sub_v3_v3v3(rp,p,origin);
1219         h=dot_v3v3(normal,rp)/dot_v3v3(normal,normal);
1220         if (h < 0.0f || h > 1.0f) return 0;
1221         return 1;
1222 }
1223
1224 /*mama (knowing the squared lenght of the normal)*/
1225 static int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns)
1226 {
1227         float h,rp[3];
1228         sub_v3_v3v3(rp,p,origin);
1229         h=dot_v3v3(normal,rp)/lns;
1230         if (h < 0.0f || h > 1.0f) return 0;
1231         return 1;
1232 }
1233 #endif
1234
1235 int isect_point_tri_prism_v3(float p[3], float v1[3], float v2[3], float v3[3])
1236 {
1237         if(!point_in_slice(p,v1,v2,v3)) return 0;
1238         if(!point_in_slice(p,v2,v3,v1)) return 0;
1239         if(!point_in_slice(p,v3,v1,v2)) return 0;
1240         return 1;
1241 }
1242
1243 /****************************** Interpolation ********************************/
1244
1245 static float tri_signed_area(float *v1, float *v2, float *v3, int i, int j)
1246 {
1247         return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
1248 }
1249
1250 static int barycentric_weights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
1251 {
1252         float xn, yn, zn, a1, a2, a3, asum;
1253         short i, j;
1254
1255         /* find best projection of face XY, XZ or YZ: barycentric weights of
1256            the 2d projected coords are the same and faster to compute */
1257         xn= (float)fabs(n[0]);
1258         yn= (float)fabs(n[1]);
1259         zn= (float)fabs(n[2]);
1260         if(zn>=xn && zn>=yn) {i= 0; j= 1;}
1261         else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
1262         else {i= 1; j= 2;} 
1263
1264         a1= tri_signed_area(v2, v3, co, i, j);
1265         a2= tri_signed_area(v3, v1, co, i, j);
1266         a3= tri_signed_area(v1, v2, co, i, j);
1267
1268         asum= a1 + a2 + a3;
1269
1270         if (fabs(asum) < FLT_EPSILON) {
1271                 /* zero area triangle */
1272                 w[0]= w[1]= w[2]= 1.0f/3.0f;
1273                 return 1;
1274         }
1275
1276         asum= 1.0f/asum;
1277         w[0]= a1*asum;
1278         w[1]= a2*asum;
1279         w[2]= a3*asum;
1280
1281         return 0;
1282 }
1283
1284 void interp_weights_face_v3(float *w,float *v1, float *v2, float *v3, float *v4, float *co)
1285 {
1286         float w2[3];
1287
1288         w[0]= w[1]= w[2]= w[3]= 0.0f;
1289
1290         /* first check for exact match */
1291         if(equals_v3v3(co, v1))
1292                 w[0]= 1.0f;
1293         else if(equals_v3v3(co, v2))
1294                 w[1]= 1.0f;
1295         else if(equals_v3v3(co, v3))
1296                 w[2]= 1.0f;
1297         else if(v4 && equals_v3v3(co, v4))
1298                 w[3]= 1.0f;
1299         else {
1300                 /* otherwise compute barycentric interpolation weights */
1301                 float n1[3], n2[3], n[3];
1302                 int degenerate;
1303
1304                 sub_v3_v3v3(n1, v1, v3);
1305                 if (v4) {
1306                         sub_v3_v3v3(n2, v2, v4);
1307                 }
1308                 else {
1309                         sub_v3_v3v3(n2, v2, v3);
1310                 }
1311                 cross_v3_v3v3(n, n1, n2);
1312
1313                 /* OpenGL seems to split this way, so we do too */
1314                 if (v4) {
1315                         degenerate= barycentric_weights(v1, v2, v4, co, n, w);
1316                         SWAP(float, w[2], w[3]);
1317
1318                         if(degenerate || (w[0] < 0.0f)) {
1319                                 /* if w[1] is negative, co is on the other side of the v1-v3 edge,
1320                                    so we interpolate using the other triangle */
1321                                 degenerate= barycentric_weights(v2, v3, v4, co, n, w2);
1322
1323                                 if(!degenerate) {
1324                                         w[0]= 0.0f;
1325                                         w[1]= w2[0];
1326                                         w[2]= w2[1];
1327                                         w[3]= w2[2];
1328                                 }
1329                         }
1330                 }
1331                 else
1332                         barycentric_weights(v1, v2, v3, co, n, w);
1333         }
1334 }
1335
1336 /* Mean value weights - smooth interpolation weights for polygons with
1337  * more than 3 vertices */
1338 static float mean_value_half_tan(float *v1, float *v2, float *v3)
1339 {
1340         float d2[3], d3[3], cross[3], area, dot, len;
1341
1342         sub_v3_v3v3(d2, v2, v1);
1343         sub_v3_v3v3(d3, v3, v1);
1344         cross_v3_v3v3(cross, d2, d3);
1345
1346         area= len_v3(cross);
1347         dot= dot_v3v3(d2, d3);
1348         len= len_v3(d2)*len_v3(d3);
1349
1350         if(area == 0.0f)
1351                 return 0.0f;
1352         else
1353                 return (len - dot)/area;
1354 }
1355
1356 void interp_weights_poly_v3(float *w,float v[][3], int n, float *co)
1357 {
1358         float totweight, t1, t2, len, *vmid, *vprev, *vnext;
1359         int i;
1360
1361         totweight= 0.0f;
1362
1363         for(i=0; i<n; i++) {
1364                 vmid= v[i];
1365                 vprev= (i == 0)? v[n-1]: v[i-1];
1366                 vnext= (i == n-1)? v[0]: v[i+1];
1367
1368                 t1= mean_value_half_tan(co, vprev, vmid);
1369                 t2= mean_value_half_tan(co, vmid, vnext);
1370
1371                 len= len_v3v3(co, vmid);
1372                 w[i]= (t1+t2)/len;
1373                 totweight += w[i];
1374         }
1375
1376         if(totweight != 0.0f)
1377                 for(i=0; i<n; i++)
1378                         w[i] /= totweight;
1379 }
1380
1381 /* (x1,v1)(t1=0)------(x2,v2)(t2=1), 0<t<1 --> (x,v)(t) */
1382 void interp_cubic_v3(float *x, float *v,float *x1, float *v1, float *x2, float *v2, float t)
1383 {
1384         float a[3],b[3];
1385         float t2= t*t;
1386         float t3= t2*t;
1387
1388         /* cubic interpolation */
1389         a[0]= v1[0] + v2[0] + 2*(x1[0] - x2[0]);
1390         a[1]= v1[1] + v2[1] + 2*(x1[1] - x2[1]);
1391         a[2]= v1[2] + v2[2] + 2*(x1[2] - x2[2]);
1392
1393         b[0]= -2*v1[0] - v2[0] - 3*(x1[0] - x2[0]);
1394         b[1]= -2*v1[1] - v2[1] - 3*(x1[1] - x2[1]);
1395         b[2]= -2*v1[2] - v2[2] - 3*(x1[2] - x2[2]);
1396
1397         x[0]= a[0]*t3 + b[0]*t2 + v1[0]*t + x1[0];
1398         x[1]= a[1]*t3 + b[1]*t2 + v1[1]*t + x1[1];
1399         x[2]= a[2]*t3 + b[2]*t2 + v1[2]*t + x1[2];
1400
1401         v[0]= 3*a[0]*t2 + 2*b[0]*t + v1[0];
1402         v[1]= 3*a[1]*t2 + 2*b[1]*t + v1[1];
1403         v[2]= 3*a[2]*t2 + 2*b[2]*t + v1[2];
1404 }
1405
1406 /***************************** View & Projection *****************************/
1407
1408 void orthographic_m4(float matrix[][4],float left, float right, float bottom, float top, float nearClip, float farClip)
1409 {
1410     float Xdelta, Ydelta, Zdelta;
1411  
1412     Xdelta = right - left;
1413     Ydelta = top - bottom;
1414     Zdelta = farClip - nearClip;
1415     if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1416                 return;
1417     }
1418     unit_m4(matrix);
1419     matrix[0][0] = 2.0f/Xdelta;
1420     matrix[3][0] = -(right + left)/Xdelta;
1421     matrix[1][1] = 2.0f/Ydelta;
1422     matrix[3][1] = -(top + bottom)/Ydelta;
1423     matrix[2][2] = -2.0f/Zdelta;                /* note: negate Z       */
1424     matrix[3][2] = -(farClip + nearClip)/Zdelta;
1425 }
1426
1427 void perspective_m4(float mat[][4],float left, float right, float bottom, float top, float nearClip, float farClip)
1428 {
1429         float Xdelta, Ydelta, Zdelta;
1430
1431         Xdelta = right - left;
1432         Ydelta = top - bottom;
1433         Zdelta = farClip - nearClip;
1434
1435         if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
1436                 return;
1437         }
1438         mat[0][0] = nearClip * 2.0f/Xdelta;
1439         mat[1][1] = nearClip * 2.0f/Ydelta;
1440         mat[2][0] = (right + left)/Xdelta;              /* note: negate Z       */
1441         mat[2][1] = (top + bottom)/Ydelta;
1442         mat[2][2] = -(farClip + nearClip)/Zdelta;
1443         mat[2][3] = -1.0f;
1444         mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
1445         mat[0][1] = mat[0][2] = mat[0][3] =
1446             mat[1][0] = mat[1][2] = mat[1][3] =
1447             mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
1448
1449 }
1450
1451 static void i_multmatrix(float icand[][4], float Vm[][4])
1452 {
1453     int row, col;
1454     float temp[4][4];
1455
1456     for(row=0 ; row<4 ; row++) 
1457         for(col=0 ; col<4 ; col++)
1458             temp[row][col] = icand[row][0] * Vm[0][col]
1459                            + icand[row][1] * Vm[1][col]
1460                            + icand[row][2] * Vm[2][col]
1461                            + icand[row][3] * Vm[3][col];
1462         copy_m4_m4(Vm, temp);
1463 }
1464
1465
1466 void polarview_m4(float Vm[][4],float dist, float azimuth, float incidence, float twist)
1467 {
1468
1469         unit_m4(Vm);
1470
1471     translate_m4(Vm,0.0, 0.0, -dist);
1472     rotate_m4(Vm,'z',-twist);   
1473     rotate_m4(Vm,'x',-incidence);
1474     rotate_m4(Vm,'z',-azimuth);
1475 }
1476
1477 void lookat_m4(float mat[][4],float vx, float vy, float vz, float px, float py, float pz, float twist)
1478 {
1479         float sine, cosine, hyp, hyp1, dx, dy, dz;
1480         float mat1[4][4];
1481         
1482         unit_m4(mat);
1483         unit_m4(mat1);
1484
1485         rotate_m4(mat,'z',-twist);
1486
1487         dx = px - vx;
1488         dy = py - vy;
1489         dz = pz - vz;
1490         hyp = dx * dx + dz * dz;        /* hyp squared  */
1491         hyp1 = (float)sqrt(dy*dy + hyp);
1492         hyp = (float)sqrt(hyp);         /* the real hyp */
1493         
1494         if (hyp1 != 0.0) {              /* rotate X     */
1495                 sine = -dy / hyp1;
1496                 cosine = hyp /hyp1;
1497         } else {
1498                 sine = 0;
1499                 cosine = 1.0f;
1500         }
1501         mat1[1][1] = cosine;
1502         mat1[1][2] = sine;
1503         mat1[2][1] = -sine;
1504         mat1[2][2] = cosine;
1505         
1506         i_multmatrix(mat1, mat);
1507
1508     mat1[1][1] = mat1[2][2] = 1.0f;     /* be careful here to reinit    */
1509     mat1[1][2] = mat1[2][1] = 0.0;      /* those modified by the last   */
1510         
1511         /* paragraph    */
1512         if (hyp != 0.0f) {                      /* rotate Y     */
1513                 sine = dx / hyp;
1514                 cosine = -dz / hyp;
1515         } else {
1516                 sine = 0;
1517                 cosine = 1.0f;
1518         }
1519         mat1[0][0] = cosine;
1520         mat1[0][2] = -sine;
1521         mat1[2][0] = sine;
1522         mat1[2][2] = cosine;
1523         
1524         i_multmatrix(mat1, mat);
1525         translate_m4(mat,-vx,-vy,-vz);  /* translate viewpoint to origin */
1526 }
1527
1528 /********************************** Mapping **********************************/
1529
1530 void map_to_tube(float *u, float *v,float x, float y, float z)
1531 {
1532         float len;
1533         
1534         *v = (z + 1.0f) / 2.0f;
1535         
1536         len= (float)sqrt(x*x+y*y);
1537         if(len > 0.0f)
1538                 *u = (float)((1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0);
1539         else
1540                 *v = *u = 0.0f; /* to avoid un-initialized variables */
1541 }
1542
1543 void map_to_sphere(float *u, float *v,float x, float y, float z)
1544 {
1545         float len;
1546         
1547         len= (float)sqrt(x*x+y*y+z*z);
1548         if(len > 0.0f) {
1549                 if(x==0.0f && y==0.0f) *u= 0.0f;        /* othwise domain error */
1550                 else *u = (float)((1.0 - (float)atan2(x,y) / M_PI) / 2.0);
1551                 
1552                 z/=len;
1553                 *v = 1.0f - (float)saacos(z)/(float)M_PI;
1554         } else {
1555                 *v = *u = 0.0f; /* to avoid un-initialized variables */
1556         }
1557 }
1558
1559 /********************************************************/
1560
1561 /* Tangents */
1562
1563 /* For normal map tangents we need to detect uv boundaries, and only average
1564  * tangents in case the uvs are connected. Alternative would be to store 1 
1565  * tangent per face rather than 4 per face vertex, but that's not compatible
1566  * with games */
1567
1568
1569 /* from BKE_mesh.h */
1570 #define STD_UV_CONNECT_LIMIT    0.0001f
1571
1572 void sum_or_add_vertex_tangent(void *arena, VertexTangent **vtang, float *tang, float *uv)
1573 {
1574         VertexTangent *vt;
1575
1576         /* find a tangent with connected uvs */
1577         for(vt= *vtang; vt; vt=vt->next) {
1578                 if(fabs(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabs(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT) {
1579                         add_v3_v3v3(vt->tang, vt->tang, tang);
1580                         return;
1581                 }
1582         }
1583
1584         /* if not found, append a new one */
1585         vt= BLI_memarena_alloc((MemArena *)arena, sizeof(VertexTangent));
1586         copy_v3_v3(vt->tang, tang);
1587         vt->uv[0]= uv[0];
1588         vt->uv[1]= uv[1];
1589
1590         if(*vtang)
1591                 vt->next= *vtang;
1592         *vtang= vt;
1593 }
1594
1595 float *find_vertex_tangent(VertexTangent *vtang, float *uv)
1596 {
1597         VertexTangent *vt;
1598         static float nulltang[3] = {0.0f, 0.0f, 0.0f};
1599
1600         for(vt= vtang; vt; vt=vt->next)
1601                 if(fabs(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabs(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT)
1602                         return vt->tang;
1603
1604         return nulltang;        /* shouldn't happen, except for nan or so */
1605 }
1606
1607 void tangent_from_uv(float *uv1, float *uv2, float *uv3, float *co1, float *co2, float *co3, float *n, float *tang)
1608 {
1609         float tangv[3], ct[3], e1[3], e2[3], s1, t1, s2, t2, det;
1610
1611         s1= uv2[0] - uv1[0];
1612         s2= uv3[0] - uv1[0];
1613         t1= uv2[1] - uv1[1];
1614         t2= uv3[1] - uv1[1];
1615         det= 1.0f / (s1 * t2 - s2 * t1);
1616         
1617         /* normals in render are inversed... */
1618         sub_v3_v3v3(e1, co1, co2);
1619         sub_v3_v3v3(e2, co1, co3);
1620         tang[0] = (t2*e1[0] - t1*e2[0])*det;
1621         tang[1] = (t2*e1[1] - t1*e2[1])*det;
1622         tang[2] = (t2*e1[2] - t1*e2[2])*det;
1623         tangv[0] = (s1*e2[0] - s2*e1[0])*det;
1624         tangv[1] = (s1*e2[1] - s2*e1[1])*det;
1625         tangv[2] = (s1*e2[2] - s2*e1[2])*det;
1626         cross_v3_v3v3(ct, tang, tangv);
1627
1628         /* check flip */
1629         if ((ct[0]*n[0] + ct[1]*n[1] + ct[2]*n[2]) < 0.0f)
1630                 negate_v3(tang);
1631 }
1632
1633 /********************************************************/
1634
1635 /* vector clouds */
1636 /* void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
1637                                                            float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3])
1638
1639 input
1640 (
1641 int list_size
1642 4 lists as pointer to array[list_size]
1643 1. current pos array of 'new' positions 
1644 2. current weight array of 'new'weights (may be NULL pointer if you have no weights )
1645 3. reference rpos array of 'old' positions
1646 4. reference rweight array of 'old'weights (may be NULL pointer if you have no weights )
1647 )
1648 output  
1649 (
1650 float lloc[3] center of mass pos
1651 float rloc[3] center of mass rpos
1652 float lrot[3][3] rotation matrix
1653 float lscale[3][3] scale matrix
1654 pointers may be NULL if not needed
1655 )
1656
1657 */
1658 /* can't believe there is none in math utils */
1659 float _det_m3(float m2[3][3])
1660 {
1661     float det = 0.f;
1662     if (m2){
1663     det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
1664         -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
1665         +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
1666     }
1667     return det;
1668 }
1669
1670
1671 void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight,
1672                                                         float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3])
1673 {
1674         float accu_com[3]= {0.0f,0.0f,0.0f}, accu_rcom[3]= {0.0f,0.0f,0.0f};
1675         float accu_weight = 0.0f,accu_rweight = 0.0f,eps = 0.000001f;
1676
1677         int a;
1678         /* first set up a nice default response */
1679         if (lloc) zero_v3(lloc);
1680         if (rloc) zero_v3(rloc);
1681         if (lrot) unit_m3(lrot);
1682         if (lscale) unit_m3(lscale);
1683         /* do com for both clouds */
1684         if (pos && rpos && (list_size > 0)) /* paranoya check */
1685         {
1686                 /* do com for both clouds */
1687                 for(a=0; a<list_size; a++){
1688                         if (weight){
1689                                 float v[3];
1690                                 copy_v3_v3(v,pos[a]);
1691                                 mul_v3_fl(v,weight[a]);
1692                                 add_v3_v3v3(accu_com,accu_com,v);
1693                                 accu_weight +=weight[a]; 
1694                         }
1695                         else add_v3_v3v3(accu_com,accu_com,pos[a]);
1696
1697                         if (rweight){
1698                                 float v[3];
1699                                 copy_v3_v3(v,rpos[a]);
1700                                 mul_v3_fl(v,rweight[a]);
1701                                 add_v3_v3v3(accu_rcom,accu_rcom,v);
1702                                 accu_rweight +=rweight[a]; 
1703                         }
1704                         else add_v3_v3v3(accu_rcom,accu_rcom,rpos[a]);
1705
1706                 }
1707                 if (!weight || !rweight){
1708                         accu_weight = accu_rweight = list_size;
1709                 }
1710
1711                 mul_v3_fl(accu_com,1.0f/accu_weight);
1712                 mul_v3_fl(accu_rcom,1.0f/accu_rweight);
1713                 if (lloc) copy_v3_v3(lloc,accu_com);
1714                 if (rloc) copy_v3_v3(rloc,accu_rcom);
1715                 if (lrot || lscale){ /* caller does not want rot nor scale, strange but legal */
1716                         /*so now do some reverse engeneering and see if we can split rotation from scale ->Polardecompose*/
1717                         /* build 'projection' matrix */
1718                         float m[3][3],mr[3][3],q[3][3],qi[3][3];
1719                         float va[3],vb[3],stunt[3];
1720                         float odet,ndet;
1721                         int i=0,imax=15;
1722                         zero_m3(m);
1723                         zero_m3(mr);
1724
1725                         /* build 'projection' matrix */
1726                         for(a=0; a<list_size; a++){
1727                                 sub_v3_v3v3(va,rpos[a],accu_rcom);
1728                                 /* mul_v3_fl(va,bp->mass);  mass needs renormalzation here ?? */
1729                                 sub_v3_v3v3(vb,pos[a],accu_com);
1730                                 /* mul_v3_fl(va,rp->mass); */
1731                                 m[0][0] += va[0] * vb[0];
1732                                 m[0][1] += va[0] * vb[1];
1733                                 m[0][2] += va[0] * vb[2];
1734
1735                                 m[1][0] += va[1] * vb[0];
1736                                 m[1][1] += va[1] * vb[1];
1737                                 m[1][2] += va[1] * vb[2];
1738
1739                                 m[2][0] += va[2] * vb[0];
1740                                 m[2][1] += va[2] * vb[1];
1741                                 m[2][2] += va[2] * vb[2];
1742
1743                                 /* building the referenc matrix on the fly
1744                                 needed to scale properly later*/
1745
1746                                 mr[0][0] += va[0] * va[0];
1747                                 mr[0][1] += va[0] * va[1];
1748                                 mr[0][2] += va[0] * va[2];
1749
1750                                 mr[1][0] += va[1] * va[0];
1751                                 mr[1][1] += va[1] * va[1];
1752                                 mr[1][2] += va[1] * va[2];
1753
1754                                 mr[2][0] += va[2] * va[0];
1755                                 mr[2][1] += va[2] * va[1];
1756                                 mr[2][2] += va[2] * va[2];
1757                         }
1758                         copy_m3_m3(q,m);
1759                         stunt[0] = q[0][0]; stunt[1] = q[1][1]; stunt[2] = q[2][2];
1760                         /* renormalizing for numeric stability */
1761                         mul_m3_fl(q,1.f/len_v3(stunt)); 
1762
1763                         /* this is pretty much Polardecompose 'inline' the algo based on Higham's thesis */
1764                         /* without the far case ... but seems to work here pretty neat                   */
1765                         odet = 0.f;
1766                         ndet = _det_m3(q);
1767                         while((odet-ndet)*(odet-ndet) > eps && i<imax){
1768                                 invert_m3_m3(qi,q);
1769                                 transpose_m3(qi);
1770                                 add_m3_m3m3(q,q,qi);
1771                                 mul_m3_fl(q,0.5f);
1772                                 odet =ndet;
1773                                 ndet =_det_m3(q);
1774                                 i++;
1775                         }
1776
1777                         if (i){
1778                                 float scale[3][3];
1779                                 float irot[3][3];
1780                                 if(lrot) copy_m3_m3(lrot,q);
1781                                 invert_m3_m3(irot,q);
1782                                 invert_m3_m3(qi,mr);
1783                                 mul_m3_m3m3(q,m,qi); 
1784                                 mul_m3_m3m3(scale,irot,q); 
1785                                 if(lscale) copy_m3_m3(lscale,scale);
1786
1787                         }
1788                 }
1789         }
1790 }