Math Lib: Use plane intersect from graphics-gems

4 files changed, 103 insertions, 34 deletions

diff --git a/source/blender/blenkernel/intern/camera.c b/source/blender/blenkernel/intern/camera.c
index 88e089d9960..b67f553b3b0 100644
--- a/source/blender/blenkernel/intern/camera.c
+++ b/source/blender/blenkernel/intern/camera.c
@@ -565,7 +565,7 @@ static void camera_frame_fit_data_init(
 static bool camera_frame_fit_calc_from_data(
         CameraParams *params, CameraViewFrameData *data, float r_co[3], float *r_scale)
 {
-	float plane_tx[CAMERA_VIEWFRAME_NUM_PLANES][3];
+	float plane_tx[CAMERA_VIEWFRAME_NUM_PLANES][4];
 	unsigned int i;
 
 	if (data->tot <= 1) {
@@ -609,15 +609,13 @@ static bool camera_frame_fit_calc_from_data(
 
 		/* apply the dist-from-plane's to the transformed plane points */
 		for (i = 0; i < CAMERA_VIEWFRAME_NUM_PLANES; i++) {
-			mul_v3_v3fl(plane_tx[i], data->normal_tx[i], sqrtf_signed(data->dist_vals_sq[i]));
+			float co[3];
+			mul_v3_v3fl(co, data->normal_tx[i], sqrtf_signed(data->dist_vals_sq[i]));
+			plane_from_point_normal_v3(plane_tx[i], co, data->normal_tx[i]);
 		}
 
-		if ((!isect_plane_plane_v3(plane_isect_1, plane_isect_1_no,
-		                           plane_tx[0], data->normal_tx[0],
-		                           plane_tx[2], data->normal_tx[2])) ||
-		    (!isect_plane_plane_v3(plane_isect_2, plane_isect_2_no,
-		                           plane_tx[1], data->normal_tx[1],
-		                           plane_tx[3], data->normal_tx[3])))
+		if ((!isect_plane_plane_v3(plane_tx[0], plane_tx[2], plane_isect_1, plane_isect_1_no)) ||
+		    (!isect_plane_plane_v3(plane_tx[1], plane_tx[3], plane_isect_2, plane_isect_2_no)))
 		{
 			return false;
 		}
diff --git a/source/blender/blenlib/BLI_math_geom.h b/source/blender/blenlib/BLI_math_geom.h
index 0f389fb7045..b3526b6fc60 100644
--- a/source/blender/blenlib/BLI_math_geom.h
+++ b/source/blender/blenlib/BLI_math_geom.h
@@ -183,9 +183,12 @@ bool isect_point_planes_v3(float (*planes)[4], int totplane, const float p[3]);
 bool isect_line_plane_v3(float out[3], const float l1[3], const float l2[3],
                          const float plane_co[3], const float plane_no[3]) ATTR_WARN_UNUSED_RESULT;
 
-bool isect_plane_plane_v3(float r_isect_co[3], float r_isect_no[3],
-                          const float plane_a_co[3], const float plane_a_no[3],
-                          const float plane_b_co[3], const float plane_b_no[3]) ATTR_WARN_UNUSED_RESULT;
+bool isect_plane_plane_plane_v3(
+        const float plane_a[4], const float plane_b[4], const float plane_c[4],
+        float r_isect_co[3]) ATTR_WARN_UNUSED_RESULT;
+bool isect_plane_plane_v3(
+        const float plane_a[4], const float plane_b[4],
+        float r_isect_co[3], float r_isect_no[3]) ATTR_WARN_UNUSED_RESULT;
 
 /* line/ray triangle */
 bool isect_line_tri_v3(
diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c
index dca54f51a5f..c644e04a9bb 100644
--- a/source/blender/blenlib/intern/math_geom.c
+++ b/source/blender/blenlib/intern/math_geom.c
@@ -1556,28 +1556,89 @@ bool isect_line_plane_v3(float out[3],
 }
 
 /**
+ * Intersect three planes, return the point where all 3 meet.
+ * See Graphics Gems 1 pg 305
+ *
+ * \param plane_a, plane_b, plane_c: Planes.
+ * \param r_isect_co: The resulting intersection point.
+ */
+bool isect_plane_plane_plane_v3(
+        const float plane_a[4], const float plane_b[4], const float plane_c[4],
+        float r_isect_co[3])
+{
+	float det;
+
+	det = determinant_m3(UNPACK3(plane_a), UNPACK3(plane_b), UNPACK3(plane_c));
+
+	if (det != 0.0f) {
+		float tmp[3];
+
+		/* (plane_b.xyz.cross(plane_c.xyz) * -plane_a[3] +
+		 *  plane_c.xyz.cross(plane_a.xyz) * -plane_b[3] +
+		 *  plane_a.xyz.cross(plane_b.xyz) * -plane_c[3]) / det; */
+
+		cross_v3_v3v3(tmp, plane_b, plane_c);
+		mul_v3_v3fl(r_isect_co, tmp, -plane_a[3]);
+
+		cross_v3_v3v3(tmp, plane_c, plane_a);
+		madd_v3_v3fl(r_isect_co, tmp, -plane_b[3]);
+
+		cross_v3_v3v3(tmp, plane_a, plane_b);
+		madd_v3_v3fl(r_isect_co, tmp, -plane_c[3]);
+
+		mul_v3_fl(r_isect_co, 1.0f / det);
+
+		return true;
+	}
+	else {
+		return false;
+	}
+}
+
+/**
  * Intersect two planes, return a point on the intersection and a vector
  * that runs on the direction of the intersection.
- * Return error code is the same as 'isect_line_line_v3'.
  *
- * \param r_isect_co The resulting intersection point.
- * \param r_isect_no The resulting vector of the intersection.
- * \param plane_a_co The point on the first plane.
- * \param plane_a_no The normal of the first plane.
- * \param plane_b_co The point on the second plane.
- * \param plane_b_no The normal of the second plane.
  *
- * \note return normal isn't unit length
+ * \note this is a slightly reduced version of #isect_plane_plane_plane_v3
+ *
+ * \param plane_a, plane_b: Planes.
+ * \param r_isect_co: The resulting intersection point.
+ * \param r_isect_no: The resulting vector of the intersection.
+ *
+ * \note \a r_isect_no isn't unit length.
  */
-bool isect_plane_plane_v3(float r_isect_co[3], float r_isect_no[3],
-                          const float plane_a_co[3], const float plane_a_no[3],
-                          const float plane_b_co[3], const float plane_b_no[3])
+bool isect_plane_plane_v3(
+        const float plane_a[4], const float plane_b[4],
+        float r_isect_co[3], float r_isect_no[3])
 {
-	float plane_a_co_other[3];
-	cross_v3_v3v3(r_isect_no, plane_a_no, plane_b_no); /* direction is simply the cross product */
-	cross_v3_v3v3(plane_a_co_other, plane_a_no, r_isect_no);
-	add_v3_v3(plane_a_co_other, plane_a_co);
-	return isect_line_plane_v3(r_isect_co, plane_a_co, plane_a_co_other, plane_b_co, plane_b_no);
+	float det, plane_c[3];
+
+	/* direction is simply the cross product */
+	cross_v3_v3v3(plane_c, plane_a, plane_b);
+
+	det = determinant_m3(UNPACK3(plane_a), UNPACK3(plane_b), UNPACK3(plane_c));
+
+	if (det != 0.0f) {
+		float tmp[3];
+
+		/* (plane_b.xyz.cross(plane_c.xyz) * -plane_a[3] +
+		 *  plane_c.xyz.cross(plane_a.xyz) * -plane_b[3]) / det; */
+		cross_v3_v3v3(tmp, plane_b, plane_c);
+		mul_v3_v3fl(r_isect_co, tmp, -plane_a[3]);
+
+		cross_v3_v3v3(tmp, plane_c, plane_a);
+		madd_v3_v3fl(r_isect_co, tmp, -plane_b[3]);
+
+		mul_v3_fl(r_isect_co, 1.0f / det);
+
+		copy_v3_v3(r_isect_no, plane_c);
+
+		return true;
+	}
+	else {
+		return false;
+	}
 }
 
 /**
@@ -1595,16 +1656,19 @@ bool isect_tri_tri_epsilon_v3(
         const float epsilon)
 {
 	const float *tri_pair[2][3] = {{t_a0, t_a1, t_a2}, {t_b0, t_b1, t_b2}};
-	float no_a[3], no_b[3];
+	float plane_a[4], plane_b[4];
 	float plane_co[3], plane_no[3];
 
 	BLI_assert((r_i1 != NULL) == (r_i2 != NULL));
 
 	/* normalizing is needed for small triangles T46007 */
-	normal_tri_v3(no_a, UNPACK3(tri_pair[0]));
-	normal_tri_v3(no_b, UNPACK3(tri_pair[1]));
+	normal_tri_v3(plane_a, UNPACK3(tri_pair[0]));
+	normal_tri_v3(plane_b, UNPACK3(tri_pair[1]));
+
+	plane_a[3] = -dot_v3v3(plane_a, t_a0);
+	plane_b[3] = -dot_v3v3(plane_b, t_b0);
 
-	if (isect_plane_plane_v3(plane_co, plane_no, t_a0, no_a, t_b0, no_b)) {
+	if (isect_plane_plane_v3(plane_a, plane_b, plane_co, plane_no)) {
 		/**
 		 * Implementation note: its simpler to project the triangles onto the intersection plane
 		 * before intersecting their edges with the ray, defined by 'isect_plane_plane_v3'.
diff --git a/source/blender/python/mathutils/mathutils_geometry.c b/source/blender/python/mathutils/mathutils_geometry.c
index 81d000991d0..02247986558 100644
--- a/source/blender/python/mathutils/mathutils_geometry.c
+++ b/source/blender/python/mathutils/mathutils_geometry.c
@@ -530,6 +530,7 @@ static PyObject *M_Geometry_intersect_plane_plane(PyObject *UNUSED(self), PyObje
 	PyObject *ret, *ret_co, *ret_no;
 	PyObject *py_plane_a_co, *py_plane_a_no, *py_plane_b_co, *py_plane_b_no;
 	float plane_a_co[3], plane_a_no[3], plane_b_co[3], plane_b_no[3];
+	float plane_a[4], plane_b[4];
 
 	float isect_co[3];
 	float isect_no[3];
@@ -549,9 +550,12 @@ static PyObject *M_Geometry_intersect_plane_plane(PyObject *UNUSED(self), PyObje
 		return NULL;
 	}
 
-	if (isect_plane_plane_v3(isect_co, isect_no,
-	                         plane_a_co, plane_a_no,
-	                         plane_b_co, plane_b_no))
+	plane_from_point_normal_v3(plane_a, plane_a_co, plane_a_no);
+	plane_from_point_normal_v3(plane_b, plane_b_co, plane_b_no);
+
+	if (isect_plane_plane_v3(
+	        plane_a, plane_b,
+	        isect_co, isect_no))
 	{
 		normalize_v3(isect_no);