Merge pull request #18407 from danilo2205/get_closest_point

Add methods get_closest_point and get_closest_offset for both Curve2D and Curve3D

2 files changed, 170 insertions, 0 deletions

diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp
index 5fd6f6c74d..4ec1e8973d 100644
--- a/scene/resources/curve.cpp
+++ b/scene/resources/curve.cpp
@@ -806,6 +806,87 @@ float Curve2D::get_bake_interval() const {
 	return bake_interval;
 }
 
+Vector2 Curve2D::get_closest_point(const Vector2 &p_to_point) const {
+	// Brute force method
+
+	if (baked_cache_dirty)
+		_bake();
+
+	//validate//
+	int pc = baked_point_cache.size();
+	if (pc == 0) {
+		ERR_EXPLAIN("No points in Curve2D");
+		ERR_FAIL_COND_V(pc == 0, Vector2());
+	}
+
+	if (pc == 1)
+		return baked_point_cache.get(0);
+
+	PoolVector2Array::Read r = baked_point_cache.read();
+
+	Vector2 nearest;
+	float nearest_dist = -1.0f;
+
+	for (int i = 0; i < pc - 1; i++) {
+		Vector2 origin = r[i];
+		Vector2 direction = (r[i + 1] - origin) / bake_interval;
+
+		float d = CLAMP((p_to_point - origin).dot(direction), 0.0f, bake_interval);
+		Vector2 proj = origin + direction * d;
+
+		float dist = proj.distance_squared_to(p_to_point);
+
+		if (nearest_dist < 0.0f || dist < nearest_dist) {
+			nearest = proj;
+			nearest_dist = dist;
+		}
+	}
+
+	return nearest;
+}
+
+float Curve2D::get_closest_offset(const Vector2 &p_to_point) const {
+	// Brute force method
+
+	if (baked_cache_dirty)
+		_bake();
+
+	//validate//
+	int pc = baked_point_cache.size();
+	if (pc == 0) {
+		ERR_EXPLAIN("No points in Curve2D");
+		ERR_FAIL_COND_V(pc == 0, 0.0f);
+	}
+
+	if (pc == 1)
+		return 0.0f;
+
+	PoolVector2Array::Read r = baked_point_cache.read();
+
+	float nearest = 0.0f;
+	float nearest_dist = -1.0f;
+	float offset = 0.0f;
+
+	for (int i = 0; i < pc - 1; i++) {
+		Vector2 origin = r[i];
+		Vector2 direction = (r[i + 1] - origin) / bake_interval;
+
+		float d = CLAMP((p_to_point - origin).dot(direction), 0.0f, bake_interval);
+		Vector2 proj = origin + direction * d;
+
+		float dist = proj.distance_squared_to(p_to_point);
+
+		if (nearest_dist < 0.0f || dist < nearest_dist) {
+			nearest = offset + d;
+			nearest_dist = dist;
+		}
+
+		offset += bake_interval;
+	}
+
+	return nearest;
+}
+
 Dictionary Curve2D::_get_data() const {
 
 	Dictionary dc;
@@ -909,6 +990,8 @@ void Curve2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_baked_length"), &Curve2D::get_baked_length);
 	ClassDB::bind_method(D_METHOD("interpolate_baked", "offset", "cubic"), &Curve2D::interpolate_baked, DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("get_baked_points"), &Curve2D::get_baked_points);
+	ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Curve2D::get_closest_point);
+	ClassDB::bind_method(D_METHOD("get_closest_offset", "to_point"), &Curve2D::get_closest_offset);
 	ClassDB::bind_method(D_METHOD("tessellate", "max_stages", "tolerance_degrees"), &Curve2D::tessellate, DEFVAL(5), DEFVAL(4));
 
 	ClassDB::bind_method(D_METHOD("_get_data"), &Curve2D::_get_data);
@@ -1276,6 +1359,87 @@ PoolRealArray Curve3D::get_baked_tilts() const {
 	return baked_tilt_cache;
 }
 
+Vector3 Curve3D::get_closest_point(const Vector3 &p_to_point) const {
+	// Brute force method
+
+	if (baked_cache_dirty)
+		_bake();
+
+	//validate//
+	int pc = baked_point_cache.size();
+	if (pc == 0) {
+		ERR_EXPLAIN("No points in Curve3D");
+		ERR_FAIL_COND_V(pc == 0, Vector3());
+	}
+
+	if (pc == 1)
+		return baked_point_cache.get(0);
+
+	PoolVector3Array::Read r = baked_point_cache.read();
+
+	Vector3 nearest;
+	float nearest_dist = -1.0f;
+
+	for (int i = 0; i < pc - 1; i++) {
+		Vector3 origin = r[i];
+		Vector3 direction = (r[i + 1] - origin) / bake_interval;
+
+		float d = CLAMP((p_to_point - origin).dot(direction), 0.0f, bake_interval);
+		Vector3 proj = origin + direction * d;
+
+		float dist = proj.distance_squared_to(p_to_point);
+
+		if (nearest_dist < 0.0f || dist < nearest_dist) {
+			nearest = proj;
+			nearest_dist = dist;
+		}
+	}
+
+	return nearest;
+}
+
+float Curve3D::get_closest_offset(const Vector3 &p_to_point) const {
+	// Brute force method
+
+	if (baked_cache_dirty)
+		_bake();
+
+	//validate//
+	int pc = baked_point_cache.size();
+	if (pc == 0) {
+		ERR_EXPLAIN("No points in Curve3D");
+		ERR_FAIL_COND_V(pc == 0, 0.0f);
+	}
+
+	if (pc == 1)
+		return 0.0f;
+
+	PoolVector3Array::Read r = baked_point_cache.read();
+
+	float nearest = 0.0f;
+	float nearest_dist = -1.0f;
+	float offset = 0.0f;
+
+	for (int i = 0; i < pc - 1; i++) {
+		Vector3 origin = r[i];
+		Vector3 direction = (r[i + 1] - origin) / bake_interval;
+
+		float d = CLAMP((p_to_point - origin).dot(direction), 0.0f, bake_interval);
+		Vector3 proj = origin + direction * d;
+
+		float dist = proj.distance_squared_to(p_to_point);
+
+		if (nearest_dist < 0.0f || dist < nearest_dist) {
+			nearest = offset + d;
+			nearest_dist = dist;
+		}
+
+		offset += bake_interval;
+	}
+
+	return nearest;
+}
+
 void Curve3D::set_bake_interval(float p_tolerance) {
 
 	bake_interval = p_tolerance;
@@ -1404,6 +1568,8 @@ void Curve3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("interpolate_baked", "offset", "cubic"), &Curve3D::interpolate_baked, DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("get_baked_points"), &Curve3D::get_baked_points);
 	ClassDB::bind_method(D_METHOD("get_baked_tilts"), &Curve3D::get_baked_tilts);
+	ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Curve3D::get_closest_point);
+	ClassDB::bind_method(D_METHOD("get_closest_offset", "to_point"), &Curve3D::get_closest_offset);
 	ClassDB::bind_method(D_METHOD("tessellate", "max_stages", "tolerance_degrees"), &Curve3D::tessellate, DEFVAL(5), DEFVAL(4));
 
 	ClassDB::bind_method(D_METHOD("_get_data"), &Curve3D::_get_data);
diff --git a/scene/resources/curve.h b/scene/resources/curve.h
index 4f55e4055c..492eb05d1e 100644
--- a/scene/resources/curve.h
+++ b/scene/resources/curve.h
@@ -199,6 +199,8 @@ public:
 	float get_baked_length() const;
 	Vector2 interpolate_baked(float p_offset, bool p_cubic = false) const;
 	PoolVector2Array get_baked_points() const; //useful for going through
+	Vector2 get_closest_point(const Vector2 &p_to_point) const;
+	float get_closest_offset(const Vector2 &p_to_point) const;
 
 	PoolVector2Array tessellate(int p_max_stages = 5, float p_tolerance = 4) const; //useful for display
 
@@ -268,6 +270,8 @@ public:
 	float interpolate_baked_tilt(float p_offset) const;
 	PoolVector3Array get_baked_points() const; //useful for going through
 	PoolRealArray get_baked_tilts() const; //useful for going through
+	Vector3 get_closest_point(const Vector3 &p_to_point) const;
+	float get_closest_offset(const Vector3 &p_to_point) const;
 
 	PoolVector3Array tessellate(int p_max_stages = 5, float p_tolerance = 4) const; //useful for display