Transform Snap: Optimize edge-snap using BVH tree

changes in BLI_kdopbvh:

- `BLI_bvhtree_find_nearest_to_ray` now takes is_ray_normalized and scale argument.
- `BLI_bvhtree_find_nearest_to_ray_angle` has been added (use for perspective view).

changes in BLI_bvhutils:

- `bvhtree_from_editmesh_edges_ex` was added.

changes in math_geom:

- `dist_squared_ray_to_seg_v3` was added.

other changes:

- `do_ray_start_correction` is no longer necessary to snap to verts.
- the way in which the test of depth was done before is being simulated in callbacks.

source/blender/blenkernel/BKE_bvhutils.h

@@ -122,6 +122,14 @@ BVHTree *bvhtree_from_mesh_verts_ex(
         const bool vert_allocated, const BLI_bitmap *mask, int verts_num_active,
         float epsilon, int tree_type, int axis);
 
+BVHTree *bvhtree_from_editmesh_edges(
+        BVHTreeFromEditMesh *data, struct BMEditMesh *em,
+        float epsilon, int tree_type, int axis);
+BVHTree *bvhtree_from_editmesh_edges_ex(
+        BVHTreeFromEditMesh *data, struct BMEditMesh *em,
+        const BLI_bitmap *edges_mask, int edges_num_active,
+        float epsilon, int tree_type, int axis);
+
 BVHTree *bvhtree_from_mesh_edges(
         struct BVHTreeFromMesh *data, struct DerivedMesh *mesh,
         float epsilon, int tree_type, int axis);

source/blender/blenkernel/intern/bvhutils.c

@@ -590,6 +590,77 @@ BVHTree *bvhtree_from_mesh_verts_ex(
 /** \name Edge Builder
  * \{ */
 
+static BVHTree *bvhtree_from_editmesh_edges_create_tree(
+        float epsilon, int tree_type, int axis,
+        BMEditMesh *em, const int edges_num,
+        const BLI_bitmap *edges_mask, int edges_num_active)
+{
+	BVHTree *tree = NULL;
+	int i;
+	BM_mesh_elem_table_ensure(em->bm, BM_EDGE);
+	if (edges_mask) {
+		BLI_assert(IN_RANGE_INCL(edges_num_active, 0, edges_num));
+	}
+	else {
+		edges_num_active = edges_num;
+	}
+
+	tree = BLI_bvhtree_new(edges_num_active, epsilon, tree_type, axis);
+
+	if (tree) {
+		BMIter iter;
+		BMEdge *eed;
+		BM_ITER_MESH_INDEX (eed, &iter, em->bm, BM_EDGES_OF_MESH, i) {
+			if (edges_mask && !BLI_BITMAP_TEST_BOOL(edges_mask, i)) {
+				continue;
+			}
+			float co[2][3];
+			copy_v3_v3(co[0], eed->v1->co);
+			copy_v3_v3(co[1], eed->v2->co);
+
+			BLI_bvhtree_insert(tree, i, co[0], 2);
+		}
+		BLI_assert(BLI_bvhtree_get_size(tree) == edges_num_active);
+		BLI_bvhtree_balance(tree);
+	}
+
+	return tree;
+}
+
+/* Builds a bvh tree where nodes are the edges of the given em */
+BVHTree *bvhtree_from_editmesh_edges_ex(
+        BVHTreeFromEditMesh *data, BMEditMesh *em,
+        const BLI_bitmap *edges_mask, int edges_num_active,
+        float epsilon, int tree_type, int axis)
+{
+	int edge_num = em->bm->totedge;
+
+	BVHTree *tree = bvhtree_from_editmesh_edges_create_tree(
+	        epsilon, tree_type, axis,
+	        em, edge_num, edges_mask, edges_num_active);
+
+	if (tree) {
+		memset(data, 0, sizeof(*data));
+		data->tree = tree;
+		data->em = em;
+		data->nearest_callback = NULL;  /* TODO */
+		data->raycast_callback = NULL;  /* TODO */
+		/* TODO: not urgent however since users currently define own callbacks */
+		data->nearest_to_ray_callback = NULL;
+	}
+
+	return tree;
+}
+BVHTree *bvhtree_from_editmesh_edges(
+        BVHTreeFromEditMesh *data, BMEditMesh *em,
+        float epsilon, int tree_type, int axis)
+{
+	return bvhtree_from_editmesh_edges_ex(
+	        data, em,
+	        NULL, -1,
+	        epsilon, tree_type, axis);
+}
+
 /* Builds a bvh tree where nodes are the edges of the given dm */
 BVHTree *bvhtree_from_mesh_edges(
         BVHTreeFromMesh *data, DerivedMesh *dm,

source/blender/blenlib/BLI_kdopbvh.h

@@ -96,7 +96,8 @@ typedef void (*BVHTree_NearestPointCallback)(void *userdata, int index, const fl
 typedef void (*BVHTree_RayCastCallback)(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit);
 
 /* callback must update nearest in case it finds a nearest result */
-typedef void (*BVHTree_NearestToRayCallback)(void *userdata, int index, const BVHTreeRay *ray, BVHTreeNearest *nearest);
+typedef void (*BVHTree_NearestToRayCallback)(void *userdata, const float ray_co[3], const float ray_dir[3],
+                                             const float scale[3], int index, BVHTreeNearest *nearest);
 
 /* callback to check if 2 nodes overlap (use thread if intersection results need to be stored) */
 typedef bool (*BVHTree_OverlapCallback)(void *userdata, int index_a, int index_b, int thread);
@@ -142,8 +143,16 @@ int BLI_bvhtree_find_nearest(
         BVHTree *tree, const float co[3], BVHTreeNearest *nearest,
         BVHTree_NearestPointCallback callback, void *userdata);
 
+int BLI_bvhtree_find_nearest_to_ray_angle(
+        BVHTree *tree, const float co[3], const float dir[3],
+        const bool ray_is_normalized, const float scale[3],
+        BVHTreeNearest *nearest,
+        BVHTree_NearestToRayCallback callback, void *userdata);
+
 int BLI_bvhtree_find_nearest_to_ray(
-        BVHTree *tree, const float co[3], const float dir[3], BVHTreeNearest *nearest,
+        BVHTree *tree, const float co[3], const float dir[3],
+        const bool ray_is_normalized, const float scale[3],
+        BVHTreeNearest *nearest,
         BVHTree_NearestToRayCallback callback, void *userdata);
 
 int BLI_bvhtree_ray_cast_ex(

source/blender/blenlib/BLI_math_geom.h

@@ -115,6 +115,13 @@ float dist_signed_squared_to_corner_v3v3v3(
         const float p[3],
         const float v1[3], const float v2[3], const float v3[3],
         const float axis_ref[3]);
+float dist_squared_to_ray_v3(
+        const float ray_origin[3], const float ray_direction[3],
+        const float co[3], float *r_depth);
+float dist_squared_ray_to_seg_v3(
+        const float ray_origin[3], const float ray_direction[3],
+        const float v0[3], const float v1[3],
+        float r_point[3], float *r_depth);
 float closest_to_line_v2(float r_close[2], const float p[2], const float l1[2], const float l2[2]);
 float closest_to_line_v3(float r_close[3], const float p[3], const float l1[3], const float l2[3]);
 void closest_to_line_segment_v2(float r_close[2], const float p[2], const float l1[2], const float l2[2]);

source/blender/blenlib/intern/BLI_kdopbvh.c

@@ -163,12 +163,23 @@ typedef struct BVHNearestRayData {
 	BVHTree *tree;
 	BVHTree_NearestToRayCallback callback;
 	void    *userdata;
-	BVHTreeRay ray;
 
-	struct NearestRayToAABB_Precalc nearest_precalc;
+	struct {
+		bool sign[3];
+		float origin[3];
+		float direction[3];
+
+		float direction_scaled_square[3];
+		float inv_dir[3];
+
+		float cdot_axis[3];
+	} ray;
 
 	bool pick_smallest[3];
+
 	BVHTreeNearest nearest;
+
+	float scale[3];
 } BVHNearestRayData;
 
 /** \} */
@@ -1889,32 +1900,310 @@ void BLI_bvhtree_ray_cast_all(
 
 /* -------------------------------------------------------------------- */
 
-/** \name BLI_bvhtree_find_nearest_to_ray
+/** \name BLI_bvhtree_find_nearest_to_ray functions
  *
  * \{ */
 
-static float calc_dist_sq_to_ray(BVHNearestRayData *data, BVHNode *node)
+static void dist_squared_ray_to_aabb_scaled_v3_precalc(
+        BVHNearestRayData *data,
+        const float ray_origin[3], const float ray_direction[3],
+        const bool ray_is_normalized, const float scale[3])
 {
-	const float *bv = node->bv;
-	const float bb_min[3] = {bv[0], bv[2], bv[4]};
-	const float bb_max[3] = {bv[1], bv[3], bv[5]};
-	return dist_squared_ray_to_aabb_v3(&data->nearest_precalc, bb_min, bb_max, data->pick_smallest);
+	if (scale) {
+		copy_v3_v3(data->scale, scale);
+	}
+	else {
+		copy_v3_fl(data->scale, 1.0f);
+	}
+	/* un-normalize ray */
+	if (ray_is_normalized && scale &&
+	   (data->scale[0] != 1.0f || data->scale[1] != 1.0f || data->scale[2] != 1.0f))
+	{
+		data->ray.direction[0] = ray_direction[0] * data->scale[0];
+		data->ray.direction[1] = ray_direction[1] * data->scale[1];
+		data->ray.direction[2] = ray_direction[2] * data->scale[2];
+
+		mul_v3_v3fl(data->ray.direction, ray_direction, 1 / len_v3(data->ray.direction));
+	}
+	else {
+		copy_v3_v3(data->ray.direction, ray_direction);
+	}
+
+	float dir_sq[3];
+
+	for (int i = 0; i < 3; i++) {
+		data->ray.origin[i] = ray_origin[i];
+		data->ray.inv_dir[i] = (data->ray.direction[i] != 0.0f) ?
+		                       (1.0f / data->ray.direction[i]) : FLT_MAX;
+		/* It has to be in function of `ray.inv_dir`,
+		 * since the division of 1 by 0.0f, can be -inf or +inf */
+		data->ray.sign[i] = (data->ray.inv_dir[i] < 0.0f);
+
+		data->ray.direction_scaled_square[i] = data->ray.direction[i] * data->scale[i];
+
+		dir_sq[i] = SQUARE(data->ray.direction_scaled_square[i]);
+
+		data->ray.direction_scaled_square[i] *= data->scale[i];
+	}
+
+	/* `diag_sq` Length square of each face diagonal */
+	float diag_sq[3] = {
+		dir_sq[1] + dir_sq[2],
+		dir_sq[0] + dir_sq[2],
+		dir_sq[0] + dir_sq[1],
+	};
+
+	data->ray.cdot_axis[0] = (diag_sq[0] != 0.0f) ? data->ray.direction[0] / diag_sq[0] : FLT_MAX;
+	data->ray.cdot_axis[1] = (diag_sq[1] != 0.0f) ? data->ray.direction[1] / diag_sq[1] : FLT_MAX;
+	data->ray.cdot_axis[2] = (diag_sq[2] != 0.0f) ? data->ray.direction[2] / diag_sq[2] : FLT_MAX;
 }
 
-static void dfs_find_nearest_to_ray_dfs(BVHNearestRayData *data, BVHNode *node)
+/**
+ * Returns the squared distance from a ray to a bound-box `AABB`.
+ * It is based on `fast_ray_nearest_hit` solution to obtain
+ * the coordinates of the nearest edge of Bound Box to the ray
+ */
+MINLINE float dist_squared_ray_to_aabb_scaled_v3__impl(
+        const BVHNearestRayData *data,
+        const float bv[6], float *r_depth_sq, bool r_axis_closest[3])
+{
+
+	/* `tmin` is a vector that has the smaller distances to each of the
+	 * infinite planes of the `AABB` faces (hit in nearest face X plane,
+	 * nearest face Y plane and nearest face Z plane) */
+	float local_bvmin[3], local_bvmax[3];
+
+	if (data->ray.sign[0]) {
+		local_bvmin[0] = bv[1];
+		local_bvmax[0] = bv[0];
+	}
+	else {
+		local_bvmin[0] = bv[0];
+		local_bvmax[0] = bv[1];
+	}
+
+	if (data->ray.sign[1]) {
+		local_bvmin[1] = bv[3];
+		local_bvmax[1] = bv[2];
+	}
+	else {
+		local_bvmin[1] = bv[2];
+		local_bvmax[1] = bv[3];
+	}
+
+	if (data->ray.sign[2]) {
+		local_bvmin[2] = bv[5];
+		local_bvmax[2] = bv[4];
+	}
+	else {
+		local_bvmin[2] = bv[4];
+		local_bvmax[2] = bv[5];
+	}
+
+	sub_v3_v3(local_bvmin, data->ray.origin);
+	sub_v3_v3(local_bvmax, data->ray.origin);
+
+	const float tmin[3] = {
+		local_bvmin[0] * data->ray.inv_dir[0],
+		local_bvmin[1] * data->ray.inv_dir[1],
+		local_bvmin[2] * data->ray.inv_dir[2],
+	};
+
+	/* `tmax` is a vector that has the longer distances to each of the
+	 * infinite planes of the `AABB` faces (hit in farthest face X plane,
+	 * farthest face Y plane and farthest face Z plane) */
+	const float tmax[3] = {
+		local_bvmax[0] * data->ray.inv_dir[0],
+		local_bvmax[1] * data->ray.inv_dir[1],
+		local_bvmax[2] * data->ray.inv_dir[2],
+	};
+	/* `v1` and `v3` is be the coordinates of the nearest `AABB` edge to the ray*/
+	float v1[3], v2[3];
+	/* `rtmin` is the highest value of the smaller distances. == max_axis_v3(tmin)
+	* `rtmax` is the lowest value of longer distances. == min_axis_v3(tmax)*/
+	float rtmin, rtmax, mul;
+	/* `main_axis` is the axis equivalent to edge close to the ray */
+	int main_axis;
+
+	r_axis_closest[0] = false;
+	r_axis_closest[1] = false;
+	r_axis_closest[2] = false;
+
+	/* *** min_axis_v3(tmax) *** */
+	if ((tmax[0] <= tmax[1]) && (tmax[0] <= tmax[2])) {
+		// printf("# Hit in X %s\n", data->sign[0] ? "min", "max");
+		rtmax = tmax[0];
+		v1[0] = v2[0] = local_bvmax[0];
+		mul = local_bvmax[0] * data->ray.direction_scaled_square[0];
+		main_axis = 3;
+		r_axis_closest[0] = data->ray.sign[0];
+	}
+	else if ((tmax[1] <= tmax[0]) && (tmax[1] <= tmax[2])) {
+		// printf("# Hit in Y %s\n", data->sign[1] ? "min", "max");
+		rtmax = tmax[1];
+		v1[1] = v2[1] = local_bvmax[1];
+		mul = local_bvmax[1] * data->ray.direction_scaled_square[1];
+		main_axis = 2;
+		r_axis_closest[1] = data->ray.sign[1];
+	}
+	else {
+		// printf("# Hit in Z %s\n", data->sign[2] ? "min", "max");
+		rtmax = tmax[2];
+		v1[2] = v2[2] = local_bvmax[2];
+		mul = local_bvmax[2] * data->ray.direction_scaled_square[2];
+		main_axis = 1;
+		r_axis_closest[2] = data->ray.sign[2];
+	}
+
+	/* *** max_axis_v3(tmin) *** */
+	if ((tmin[0] >= tmin[1]) && (tmin[0] >= tmin[2])) {
+		// printf("# To X %s\n", data->sign[0] ? "max", "min");
+		rtmin = tmin[0];
+		v1[0] = v2[0] = local_bvmin[0];
+		mul += local_bvmin[0] * data->ray.direction_scaled_square[0];
+		main_axis -= 3;
+		r_axis_closest[0] = !data->ray.sign[0];
+	}
+	else if ((tmin[1] >= tmin[0]) && (tmin[1] >= tmin[2])) {
+		// printf("# To Y %s\n", data->sign[1] ? "max", "min");
+		rtmin = tmin[1];
+		v1[1] = v2[1] = local_bvmin[1];
+		mul += local_bvmin[1] * data->ray.direction_scaled_square[1];
+		main_axis -= 1;
+		r_axis_closest[1] = !data->ray.sign[1];
+	}
+	else {
+		// printf("# To Z %s\n", data->sign[2] ? "max", "min");
+		rtmin = tmin[2];
+		v1[2] = v2[2] = local_bvmin[2];
+		mul += local_bvmin[2] * data->ray.direction_scaled_square[2];
+		main_axis -= 2;
+		r_axis_closest[2] = !data->ray.sign[2];
+	}
+	/* *** end min/max axis *** */
+
+	if (main_axis < 0)
+		main_axis += 3;
+
+	/* if rtmin < rtmax, ray intersect `AABB` */
+	if (rtmin <= rtmax) {
+#ifdef IGNORE_BEHIND_RAY
+		/* `if rtmax < depth_min`, the whole `AABB` is behind us */
+		if (rtmax < min_depth) {
+			return fallback;
+		}
+#endif
+		const float proj = rtmin * data->ray.direction[main_axis];
+
+		if (data->ray.sign[main_axis])
+			r_axis_closest[main_axis] = (proj - local_bvmax[main_axis]) < (local_bvmin[main_axis] - proj);
+		else
+			r_axis_closest[main_axis] = (proj - local_bvmin[main_axis]) < (local_bvmax[main_axis] - proj);
+
+		//if (r_depth_sq)
+		//	*r_depth_sq = SQUARE(rtmin);
+
+		return 0.0f;
+	}
+#ifdef IGNORE_BEHIND_RAY
+	/* `if rtmin < depth_min`, the whole `AABB` is behing us */
+	else if (rtmin < min_depth) {
+		return fallback;
+	}
+#endif
+
+	if (data->ray.sign[main_axis]) {
+		v1[main_axis] = local_bvmax[main_axis];
+		v2[main_axis] = local_bvmin[main_axis];
+	}
+	else {
+		v1[main_axis] = local_bvmin[main_axis];
+		v2[main_axis] = local_bvmax[main_axis];
+	}
+	{
+		/* `proj` equals to nearest point on the ray closest to the edge `v1 v2` of the `AABB`. */
+		const float proj = mul * data->ray.cdot_axis[main_axis];
+		float depth_sq, r_point[3];
+		if (v1[main_axis] > proj) { /* the nearest point to the ray is the point v1 */
+			r_axis_closest[main_axis] = true;
+			/* `depth` is equivalent the distance of the the projection of v1 on the ray */
+			depth_sq = mul + data->ray.direction_scaled_square[main_axis] * v1[main_axis];
+
+			copy_v3_v3(r_point, v1);
+		}
+		else if (v2[main_axis] < proj) { /* the nearest point of the ray is the point v2 */
+			r_axis_closest[main_axis] = false;
+
+			depth_sq = mul + data->ray.direction_scaled_square[main_axis] * v2[main_axis];
+
+			copy_v3_v3(r_point, v2);
+		}
+		else {  /* the nearest point of the ray is on the edge of the `AABB`. */
+			r_axis_closest[main_axis] = (proj - v1[main_axis]) < (v2[main_axis] - proj);
+
+			depth_sq = mul + data->ray.direction_scaled_square[main_axis] * proj;
+#if 0
+			r_point[0] = main_axis == 0 ? proj : v2[0];
+			r_point[1] = main_axis == 1 ? proj : v2[1];
+			r_point[2] = main_axis == 2 ? proj : v2[2];
+#else
+			v2[main_axis] = proj;
+			copy_v3_v3(r_point, v2);
+#endif
+		}
+		depth_sq *= depth_sq;
+
+		if (r_depth_sq)
+			*r_depth_sq = depth_sq;
+
+		/* TODO: scale can be optional */
+		r_point[0] *= data->scale[0];
+		r_point[1] *= data->scale[1];
+		r_point[2] *= data->scale[2];
+
+		return len_squared_v3(r_point) - depth_sq;
+	}
+}
+
+/**
+ * <pre>
+ *  + r_point
+ *  |
+ *  | dist
+ *  |
+ *  +----depth----+orig <-- dir
+ *
+ * tangent = dist/depth
+ * </pre>
+ */
+static float calc_tangent_sq(BVHNearestRayData *data, BVHNode *node)
+{
+	float depth_sq;
+	const float dist_sq = dist_squared_ray_to_aabb_scaled_v3__impl(
+	        data, node->bv, &depth_sq, data->pick_smallest);
+
+	return (dist_sq != 0.0f) ? (dist_sq / depth_sq) : 0.0f;
+}
+
+static float calc_dist_sq_to_ray(BVHNearestRayData *data, BVHNode *node)
+{
+	return dist_squared_ray_to_aabb_scaled_v3__impl(
+	        data, node->bv, NULL,
+	        data->pick_smallest);
+}
+
+static void dfs_find_lowest_tangent_dfs(BVHNearestRayData *data, BVHNode *node)
 {
 	if (node->totnode == 0) {
 		if (data->callback) {
-			data->callback(data->userdata, node->index, &data->ray, &data->nearest);
+			data->callback(data->userdata, data->ray.origin, data->ray.direction,
+			               data->scale, node->index, &data->nearest);
 		}
 		else {
-			const float dist_sq = calc_dist_sq_to_ray(data, node);
-			if (dist_sq != FLT_MAX) {  /* not an invalid ray */
-				data->nearest.index = node->index;
-				data->nearest.dist_sq = dist_sq;
-				/* TODO: return a value to the data->nearest.co
-				 * not urgent however since users currently define own callbacks */
-			}
+			data->nearest.index = node->index;
+			data->nearest.dist_sq = calc_tangent_sq(data, node);
+			/* TODO: return a value to the data->nearest.co
+			 * not urgent however since users currently define own callbacks */
 		}
 	}
 	else {
@@ -1922,25 +2211,63 @@ static void dfs_find_nearest_to_ray_dfs(BVHNearestRayData *data, BVHNode *node)
 		/* First pick the closest node to dive on */
 		if (data->pick_smallest[node->main_axis]) {
 			for (i = 0; i != node->totnode; i++) {
-				if (calc_dist_sq_to_ray(data, node->children[i]) >= data->nearest.dist_sq) {
-					continue;
+				if (calc_tangent_sq(data, node->children[i]) < data->nearest.dist_sq) {
+					dfs_find_lowest_tangent_dfs(data, node->children[i]);
 				}
-				dfs_find_nearest_to_ray_dfs(data, node->children[i]);
 			}
 		}
 		else {
 			for (i = node->totnode - 1; i >= 0; i--) {
-				if (calc_dist_sq_to_ray(data, node->children[i]) >= data->nearest.dist_sq) {
-					continue;
+				if (calc_tangent_sq(data, node->children[i]) < data->nearest.dist_sq) {
+					dfs_find_lowest_tangent_dfs(data, node->children[i]);
 				}
-				dfs_find_nearest_to_ray_dfs(data, node->children[i]);
 			}
 		}
 	}
 }
 
-int BLI_bvhtree_find_nearest_to_ray(
-        BVHTree *tree, const float co[3], const float dir[3], BVHTreeNearest *nearest,
+static void dfs_find_nearest_to_ray_dfs(BVHNearestRayData *data, BVHNode *node)
+{
+	if (node->totnode == 0) {
+		if (data->callback) {
+			data->callback(data->userdata, data->ray.origin, data->ray.direction,
+			               data->scale, node->index, &data->nearest);
+		}
+		else {
+			data->nearest.index = node->index;
+			data->nearest.dist_sq = calc_dist_sq_to_ray(data, node);
+			/* TODO: return a value to the data->nearest.co
+			 * not urgent however since users currently define own callbacks */
+		}
+	}
+	else {
+		int i;
+		/* First pick the closest node to dive on */
+		if (data->pick_smallest[node->main_axis]) {
+			for (i = 0; i != node->totnode; i++) {
+				if (calc_dist_sq_to_ray(data, node->children[i]) < data->nearest.dist_sq) {
+					dfs_find_nearest_to_ray_dfs(data, node->children[i]);
+				}
+			}
+		}
+		else {
+			for (i = node->totnode - 1; i >= 0; i--) {
+				if (calc_dist_sq_to_ray(data, node->children[i]) < data->nearest.dist_sq) {
+					dfs_find_nearest_to_ray_dfs(data, node->children[i]);
+				}
+			}
+		}
+	}
+}
+
+/**
+ * Returns the point whose tangent defined by the angle between the point and ray is the lowest
+ * nearest.dist_sq returns the angle's tangent
+ */
+int BLI_bvhtree_find_nearest_to_ray_angle(
+        BVHTree *tree, const float co[3], const float dir[3],
+        const bool ray_is_normalized, const float scale[3],
+        BVHTreeNearest *nearest,
         BVHTree_NearestToRayCallback callback, void *userdata)
 {
 	BVHNearestRayData data;
@@ -1951,11 +2278,46 @@ int BLI_bvhtree_find_nearest_to_ray(
 	data.callback = callback;
 	data.userdata = userdata;
 
-	copy_v3_v3(data.ray.origin, co);
-	copy_v3_v3(data.ray.direction, dir);
-	data.ray.radius = 0.0f;  /* unused here */
+	dist_squared_ray_to_aabb_scaled_v3_precalc(&data, co, dir, ray_is_normalized, scale);
 
-	dist_squared_ray_to_aabb_v3_precalc(&data.nearest_precalc, co, dir);
+	if (nearest) {
+		memcpy(&data.nearest, nearest, sizeof(*nearest));
+	}
+	else {
+		data.nearest.index = -1;
+		data.nearest.dist_sq = FLT_MAX;
+	}
+
+	/* dfs search */
+	if (root) {
+		if (calc_tangent_sq(&data, root) < data.nearest.dist_sq)
+			dfs_find_lowest_tangent_dfs(&data, root);
+	}
+
+	/* copy back results */
+	if (nearest) {
+		memcpy(nearest, &data.nearest, sizeof(*nearest));
+	}
+
+	return data.nearest.index;
+}
+
+/* return the nearest point to ray */
+int BLI_bvhtree_find_nearest_to_ray(
+        BVHTree *tree, const float co[3], const float dir[3],
+        const bool ray_is_normalized, const float scale[3],
+        BVHTreeNearest *nearest,
+        BVHTree_NearestToRayCallback callback, void *userdata)
+{
+	BVHNearestRayData data;
+	BVHNode *root = tree->nodes[tree->totleaf];
+
+	data.tree = tree;
+
+	data.callback = callback;
+	data.userdata = userdata;
+
+	dist_squared_ray_to_aabb_scaled_v3_precalc(&data, co, dir, ray_is_normalized, scale);
 
 	if (nearest) {
 		memcpy(&data.nearest, nearest, sizeof(*nearest));

source/blender/blenlib/intern/math_geom.c

@@ -572,6 +572,67 @@ float dist_signed_squared_to_corner_v3v3v3(
 	}
 }
 
+/**
+ * return the distance squared of a point to a ray.
+ */
+float dist_squared_to_ray_v3(
+        const float ray_origin[3], const float ray_direction[3],
+        const float co[3], float *r_depth)
+{
+	float dvec[3];
+	sub_v3_v3v3(dvec, co, ray_origin);
+	*r_depth = dot_v3v3(dvec, ray_direction);
+	return len_squared_v3(dvec) - SQUARE(*r_depth);
+}
+/**
+ * Find the closest point in a seg to a ray and return the distance squared.
+ * \param r_point : Is the point on segment closest to ray (or to ray_origin if the ray and the segment are parallel).
+ * \param depth: the distance of r_point projection on ray to the ray_origin.
+ */
+float dist_squared_ray_to_seg_v3(
+        const float ray_origin[3], const float ray_direction[3],
+        const float v0[3], const float v1[3],
+        float r_point[3], float *r_depth)
+{
+	float a[3], t[3], n[3], lambda;
+	sub_v3_v3v3(a, v1, v0);
+	sub_v3_v3v3(t, v0, ray_origin);
+	cross_v3_v3v3(n, a, ray_direction);
+	const float nlen = len_squared_v3(n);
+
+	/* if (nlen == 0.0f) the lines are parallel,
+	 * has no nearest point, only distance squared.*/
+	if (nlen == 0.0f) {
+		/* Calculate the distance to the point v0 then */
+		copy_v3_v3(r_point, v0);
+		*r_depth = dot_v3v3(t, ray_direction);
+	}
+	else {
+		float c[3], cray[3];
+		sub_v3_v3v3(c, n, t);
+		cross_v3_v3v3(cray, c, ray_direction);
+		lambda = dot_v3v3(cray, n) / nlen;
+		if (lambda <= 0) {
+			copy_v3_v3(r_point, v0);
+
+			*r_depth = dot_v3v3(t, ray_direction);
+		}
+		else if (lambda >= 1) {
+			copy_v3_v3(r_point, v1);
+
+			sub_v3_v3v3(t, v1, ray_origin);
+			*r_depth = dot_v3v3(t, ray_direction);
+		}
+		else {
+			madd_v3_v3v3fl(r_point, v0, a, lambda);
+
+			sub_v3_v3v3(t, r_point, ray_origin);
+			*r_depth = dot_v3v3(t, ray_direction);
+		}
+	}
+	return len_squared_v3(t) - SQUARE(*r_depth);
+}
+
 /* Adapted from "Real-Time Collision Detection" by Christer Ericson,
  * published by Morgan Kaufmann Publishers, copyright 2005 Elsevier Inc.
  *