diff --git a/src/goal_marker.cpp b/src/goal_marker.cpp index 4a85d52..91dff02 100644 --- a/src/goal_marker.cpp +++ b/src/goal_marker.cpp @@ -1,7 +1,8 @@ #include "goal_marker.hpp" -#include "godot_cpp/classes/global_constants.hpp" +#include "godot_cpp/core/math.hpp" #include "planner.hpp" #include "utils/godot_macros.h" +#include namespace godot { void GoalMarker::_bind_methods() { @@ -9,11 +10,63 @@ void GoalMarker::_bind_methods() { GDPROPERTY_HINTED(goal, Variant::OBJECT, PROPERTY_HINT_RESOURCE_TYPE, "Goal"); } -Ref GoalMarker::get_goal() const { - return this->goal; +bool GoalMarker::is_point_on(Vector3 point) { + return point.distance_squared_to(this->get_global_position()) <= .5f * .5f; +} + +Vector3 GoalMarker::nearest_point_on(Vector3 nearest_to) { + return this->get_global_position(); } void GoalMarker::set_goal(Ref goal) { this->goal = goal; } + +Ref GoalMarker::get_goal() const { + return this->goal; +} + +void GoalMarker::set_radius(float radius) { + this->radius = radius; +} + +float GoalMarker::get_radius() const { + return this->radius; +} + +#undef CLASSNAME + +void LineGoalMarker::_bind_methods() { +#define CLASSNAME LineGoalMarker + GDPROPERTY(left, Variant::FLOAT); + GDPROPERTY(right, Variant::FLOAT); +} + +bool LineGoalMarker::is_point_on(Vector3 point) { + return this->nearest_point_on(point).distance_to(point) < this->radius * this->radius; +} + +Vector3 LineGoalMarker::nearest_point_on(Vector3 point) { + Basis const &basis{this->get_global_basis()}; + Vector3 const right_unit{basis.get_column(0).normalized()}; + Vector3 const leftvec{(right_unit * -left) + this->get_global_position()}; + float const length{Math::abs(right - left)}; + return leftvec + right_unit * Math::clamp(right_unit.dot(point - leftvec), 0.f, length); +} + +void LineGoalMarker::set_left(float left) { + this->left = left; +} + +float LineGoalMarker::get_left() const { + return this->left; +} + +void LineGoalMarker::set_right(float right) { + this->right = right; +} + +float LineGoalMarker::get_right() const { + return this->right; +} } diff --git a/src/goal_marker.hpp b/src/goal_marker.hpp index de37d6c..6ccb3fa 100644 --- a/src/goal_marker.hpp +++ b/src/goal_marker.hpp @@ -14,10 +14,31 @@ class GoalMarker : public Area3D { GDCLASS(GoalMarker, Area3D); static void _bind_methods(); public: - Ref get_goal() const; + virtual bool is_point_on(Vector3 point); + virtual Vector3 nearest_point_on(Vector3 near_to); + void set_goal(Ref goal); -private: + Ref get_goal() const; + void set_radius(float radius); + float get_radius() const; +protected: Ref goal{nullptr}; + float radius{0.2f}; +}; + +class LineGoalMarker : public GoalMarker { + GDCLASS(LineGoalMarker, Area3D); + static void _bind_methods(); +public: + virtual bool is_point_on(Vector3 point) override; + virtual Vector3 nearest_point_on(Vector3 near_to) override; + + void set_left(float left); + float get_left() const; + void set_right(float right); + float get_right() const; +private: + float left{0.5f}, right{0.5f}; }; } diff --git a/src/register_types.cpp b/src/register_types.cpp index f3ff105..0beb221 100644 --- a/src/register_types.cpp +++ b/src/register_types.cpp @@ -66,6 +66,7 @@ void initialize_gdextension_types(ModuleInitializationLevel p_level) ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); + ClassDB::register_class(); } extern "C"