diff --git a/source/blender/blenlib/BLI_arithb.h b/source/blender/blenlib/BLI_arithb.h index 4d277cf98e1..4fa880c36d1 100644 --- a/source/blender/blenlib/BLI_arithb.h +++ b/source/blender/blenlib/BLI_arithb.h @@ -374,6 +374,7 @@ void LocQuatSizeToMat4(float mat[][4], float loc[3], float quat[4], float size[3 void tubemap(float x, float y, float z, float *u, float *v); void spheremap(float x, float y, float z, float *u, float *v); +int LineIntersectLine(float v1[3], float v2[3], float v3[3], float v4[3], float i1[3], float i2[3]); int LineIntersectsTriangle(float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda, float *uv); int RayIntersectsTriangle(float p1[3], float d[3], float v0[3], float v1[3], float v2[3], float *lambda, float *uv); int SweepingSphereIntersectsTriangleUV(float p1[3], float p2[3], float radius, float v0[3], float v1[3], float v2[3], float *lambda, float *ipoint); diff --git a/source/blender/blenlib/intern/arithb.c b/source/blender/blenlib/intern/arithb.c index 322a9e6fd02..2084ab3da5f 100644 --- a/source/blender/blenlib/intern/arithb.c +++ b/source/blender/blenlib/intern/arithb.c @@ -4032,6 +4032,74 @@ int AxialLineIntersectsTriangle(int axis, float p1[3], float p2[3], float v0[3], return 1; } +/* Returns the number of point of interests + * 0 - lines are colinear + * 1 - lines are coplanar, i1 is set to intersection + * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively + * */ +int LineIntersectLine(float v1[3], float v2[3], float v3[3], float v4[3], float i1[3], float i2[3]) +{ + float a[3], b[3], c[3], ab[3], cb[3], dir1[3], dir2[3]; + float d; + + VecSubf(c, v3, v1); + VecSubf(a, v2, v1); + VecSubf(b, v4, v3); + + VecCopyf(dir1, a); + Normalize(dir1); + VecCopyf(dir2, b); + Normalize(dir2); + d = Inpf(dir1, dir2); + if (d == 1.0f || d == -1.0f) { + /* colinear */ + return 0; + } + + Crossf(ab, a, b); + d = Inpf(c, ab); + + /* test if the two lines are coplanar */ + if (d > -0.000001f && d < 0.000001f) { + Crossf(cb, c, b); + + VecMulf(a, Inpf(cb, ab) / Inpf(ab, ab)); + VecAddf(i1, v1, a); + VecCopyf(i2, i1); + + return 1; /* one intersection only */ + } + /* if not */ + else { + float n[3], t[3]; + float v3t[3], v4t[3]; + VecSubf(t, v1, v3); + + /* offset between both plane where the lines lies */ + Crossf(n, a, b); + Projf(t, t, n); + + /* for the first line, offset the second line until it is coplanar */ + VecAddf(v3t, v3, t); + VecAddf(v4t, v4, t); + + VecSubf(c, v3t, v1); + VecSubf(a, v2, v1); + VecSubf(b, v4t, v3); + + Crossf(ab, a, b); + Crossf(cb, c, b); + + VecMulf(a, Inpf(cb, ab) / Inpf(ab, ab)); + VecAddf(i1, v1, a); + + /* for the second line, just substract the offset from the first intersection point */ + VecSubf(i2, i1, t); + + return 2; /* two nearest points */ + } +} + int AabbIntersectAabb(float min1[3], float max1[3], float min2[3], float max2[3]) { return (min1[0]size == 3 || vec1->size == 2) { - float a[3], b[3], c[3], ab[3], cb[3], dir1[3], dir2[3]; - float d; + int result; + if (vec1->size == 3) { VECCOPY(v1, vec1->vec); VECCOPY(v2, vec2->vec); @@ -1477,63 +1477,19 @@ PyObject *M_Mathutils_LineIntersect( PyObject * self, PyObject * args ) v4[1] = vec4->vec[1]; v4[2] = 0.0f; } + + result = LineIntersectLine(v1, v2, v3, v4, i1, i2); - VecSubf(c, v3, v1); - VecSubf(a, v2, v1); - VecSubf(b, v4, v3); - - VECCOPY(dir1, a); - Normalize(dir1); - VECCOPY(dir2, b); - Normalize(dir2); - d = Inpf(dir1, dir2); - if (d == 1.0f || d == -1.0f) { + if (result == 0) { /* colinear */ return EXPP_incr_ret( Py_None ); } - - Crossf(ab, a, b); - d = Inpf(c, ab); - - /* test if the two lines are coplanar */ - if (d > -0.000001f && d < 0.000001f) { - Crossf(cb, c, b); - - VecMulf(a, Inpf(cb, ab) / Inpf(ab, ab)); - VecAddf(i1, v1, a); - VECCOPY(i2, i1); - } - /* if not */ else { - float n[3], t[3]; - VecSubf(t, v1, v3); - - /* offset between both plane where the lines lies */ - Crossf(n, a, b); - Projf(t, t, n); - - /* for the first line, offset the second line until it is coplanar */ - VecAddf(v3, v3, t); - VecAddf(v4, v4, t); - - VecSubf(c, v3, v1); - VecSubf(a, v2, v1); - VecSubf(b, v4, v3); - - Crossf(ab, a, b); - Crossf(cb, c, b); - - VecMulf(a, Inpf(cb, ab) / Inpf(ab, ab)); - VecAddf(i1, v1, a); - - /* for the second line, just substract the offset from the first intersection point */ - VecSubf(i2, i1, t); + tuple = PyTuple_New( 2 ); + PyTuple_SetItem( tuple, 0, newVectorObject(i1, vec1->size, Py_NEW) ); + PyTuple_SetItem( tuple, 1, newVectorObject(i2, vec1->size, Py_NEW) ); + return tuple; } - - tuple = PyTuple_New( 2 ); - PyTuple_SetItem( tuple, 0, newVectorObject(i1, vec1->size, Py_NEW) ); - PyTuple_SetItem( tuple, 1, newVectorObject(i2, vec1->size, Py_NEW) ); - return tuple; } else { return ( EXPP_ReturnPyObjError( PyExc_TypeError,