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