From 2224975e7ec43954e6598d0285820c2aaf679f94 Mon Sep 17 00:00:00 2001 From: danilo2205 Date: Tue, 24 Apr 2018 20:28:17 -0300 Subject: [PATCH] Added methods get_closest_point and get_closest_offset to both Curve2D and Curve3D --- scene/resources/curve.cpp | 166 ++++++++++++++++++++++++++++++++++++++ scene/resources/curve.h | 4 + 2 files changed, 170 insertions(+) diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp index 5fd6f6c74dd..4ec1e8973d1 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 4f55e4055c5..492eb05d1e9 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