diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_geom.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_geom.c | 112 |
1 files changed, 97 insertions, 15 deletions
diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c index 10a9a2df9f5..3527af365ec 100644 --- a/source/blender/blenlib/intern/math_geom.c +++ b/source/blender/blenlib/intern/math_geom.c @@ -296,6 +296,88 @@ float dist_to_line_segment_v3(const float v1[3], const float v2[3], const float return len_v3v3(closest, v1); } +/* Adapted from "Real-Time Collision Detection" by Christer Ericson, + * published by Morgan Kaufmann Publishers, copyright 2005 Elsevier Inc. + * + * Set 'r' to the point in triangle (a, b, c) closest to point 'p' */ +void closest_on_tri_to_point_v3(float r[3], const float p[3], + const float a[3], const float b[3], const float c[3]) +{ + float ab[3], ac[3], ap[3], d1, d2; + float bp[3], d3, d4, vc, cp[3], d5, d6, vb, va; + float denom, v, w; + + /* Check if P in vertex region outside A */ + sub_v3_v3v3(ab, b, a); + sub_v3_v3v3(ac, c, a); + sub_v3_v3v3(ap, p, a); + d1 = dot_v3v3(ab, ap); + d2 = dot_v3v3(ac, ap); + if (d1 <= 0.0f && d2 <= 0.0f) { + /* barycentric coordinates (1,0,0) */ + copy_v3_v3(r, a); + return; + } + + /* Check if P in vertex region outside B */ + sub_v3_v3v3(bp, p, b); + d3 = dot_v3v3(ab, bp); + d4 = dot_v3v3(ac, bp); + if (d3 >= 0.0f && d4 <= d3) { + /* barycentric coordinates (0,1,0) */ + copy_v3_v3(r, b); + return; + } + /* Check if P in edge region of AB, if so return projection of P onto AB */ + vc = d1*d4 - d3*d2; + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { + float v = d1 / (d1 - d3); + /* barycentric coordinates (1-v,v,0) */ + madd_v3_v3v3fl(r, a, ab, v); + return; + } + /* Check if P in vertex region outside C */ + sub_v3_v3v3(cp, p, c); + d5 = dot_v3v3(ab, cp); + d6 = dot_v3v3(ac, cp); + if (d6 >= 0.0f && d5 <= d6) { + /* barycentric coordinates (0,0,1) */ + copy_v3_v3(r, c); + return; + } + /* Check if P in edge region of AC, if so return projection of P onto AC */ + vb = d5*d2 - d1*d6; + if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { + float w = d2 / (d2 - d6); + /* barycentric coordinates (1-w,0,w) */ + madd_v3_v3v3fl(r, a, ac, w); + return; + } + /* Check if P in edge region of BC, if so return projection of P onto BC */ + va = d3*d6 - d5*d4; + if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + /* barycentric coordinates (0,1-w,w) */ + sub_v3_v3v3(r, c, b); + mul_v3_fl(r, w); + add_v3_v3(r, b); + return; + } + + /* P inside face region. Compute Q through its barycentric coordinates (u,v,w) */ + denom = 1.0f / (va + vb + vc); + v = vb * denom; + w = vc * denom; + + /* = u*a + v*b + w*c, u = va * denom = 1.0f - v - w */ + /* ac * w */ + mul_v3_fl(ac, w); + /* a + ab * v */ + madd_v3_v3v3fl(r, a, ab, v); + /* a + ab * v + ac * w */ + add_v3_v3(r, ac); +} + /******************************* Intersection ********************************/ /* intersect Line-Line, shorts */ @@ -1037,7 +1119,7 @@ int isect_sweeping_sphere_tri_v3(const float p1[3], const float p2[3], const flo if (t0 > 1.0f || t1 < 0.0f) return 0; - /* clamp to [0,1] */ + /* clamp to [0, 1] */ CLAMP(t0, 0.0f, 1.0f); CLAMP(t1, 0.0f, 1.0f); @@ -1157,10 +1239,10 @@ int isect_sweeping_sphere_tri_v3(const float p1[3], const float p2[3], const flo } /*e3*/ - /* sub_v3_v3v3(bv,v0,p1); */ /* UNUSED */ - /* elen2 = dot_v3v3(e1,e1); */ /* UNUSED */ - /* edotv = dot_v3v3(e1,vel); */ /* UNUSED */ - /* edotbv = dot_v3v3(e1,bv); */ /* UNUSED */ + /* sub_v3_v3v3(bv, v0, p1); */ /* UNUSED */ + /* elen2 = dot_v3v3(e1, e1); */ /* UNUSED */ + /* edotv = dot_v3v3(e1, vel); */ /* UNUSED */ + /* edotbv = dot_v3v3(e1, bv); */ /* UNUSED */ sub_v3_v3v3(bv, v1, p1); elen2 = dot_v3v3(e3, e3); @@ -1195,7 +1277,7 @@ int isect_axial_line_tri_v3(const int axis, const float p1[3], const float p2[3] int a0 = axis, a1 = (axis + 1) % 3, a2 = (axis + 2) % 3; #if 0 - return isect_line_tri_v3(p1,p2,v0,v1,v2,lambda); + return isect_line_tri_v3(p1, p2, v0, v1, v2, lambda); /* first a simple bounding box test */ if (min_fff(v0[a1], v1[a1], v2[a1]) > p1[a1]) return 0; @@ -1415,8 +1497,8 @@ int isect_ray_aabb(const IsectRayAABBData *data, const float bb_min[3], return TRUE; } -/* find closest point to p on line through l1,l2 and return lambda, - * where (0 <= lambda <= 1) when cp is in the line segment l1,l2 +/* find closest point to p on line through (l1, l2) and return lambda, + * where (0 <= lambda <= 1) when cp is in the line segment (l1, l2) */ float closest_to_line_v3(float cp[3], const float p[3], const float l1[3], const float l2[3]) { @@ -1702,9 +1784,9 @@ static int point_in_slice(const float p[3], const float v1[3], const float l1[3] /* * what is a slice ? * some maths: - * a line including l1,l2 and a point not on the line + * a line including (l1, l2) and a point not on the line * define a subset of R3 delimited by planes parallel to the line and orthogonal - * to the (point --> line) distance vector,one plane on the line one on the point, + * to the (point --> line) distance vector, one plane on the line one on the point, * the room inside usually is rather small compared to R3 though still infinite * useful for restricting (speeding up) searches * e.g. all points of triangular prism are within the intersection of 3 'slices' @@ -2304,7 +2386,7 @@ void interp_weights_poly_v2(float *w, float v[][2], const int n, const float co[ } } -/* (x1,v1)(t1=0)------(x2,v2)(t2=1), 0<t<1 --> (x,v)(t) */ +/* (x1, v1)(t1=0)------(x2, v2)(t2=1), 0<t<1 --> (x, v)(t) */ void interp_cubic_v3(float x[3], float v[3], const float x1[3], const float v1[3], const float x2[3], const float v2[3], const float t) { float a[3], b[3]; @@ -2791,8 +2873,8 @@ void tangent_from_uv(float uv1[2], float uv2[2], float uv3[3], float co1[3], flo /****************************** Vector Clouds ********************************/ /* vector clouds */ -/* void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight, - * float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3]) +/* void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight, float (*rpos)[3], float *rweight, + * float lloc[3], float rloc[3], float lrot[3][3], float lscale[3][3]) * * input * ( @@ -2881,9 +2963,9 @@ void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight, fl /* build 'projection' matrix */ for (a = 0; a < list_size; a++) { sub_v3_v3v3(va, rpos[a], accu_rcom); - /* mul_v3_fl(va,bp->mass); mass needs renormalzation here ?? */ + /* mul_v3_fl(va, bp->mass); mass needs renormalzation here ?? */ sub_v3_v3v3(vb, pos[a], accu_com); - /* mul_v3_fl(va,rp->mass); */ + /* mul_v3_fl(va, rp->mass); */ m[0][0] += va[0] * vb[0]; m[0][1] += va[0] * vb[1]; m[0][2] += va[0] * vb[2]; |