mirror of
https://github.com/Relintai/pandemonium_engine.git
synced 2024-12-28 22:57:15 +01:00
Relintai
9fed52de03
It's a modified verion of https://github.com/GDQuest/godot-steering-ai-framework which I converted to c++ using thr converter srcipt. It still needs to be cleaned.
345 lines
9.1 KiB
C++
345 lines
9.1 KiB
C++
|
|
#include "gsaipath.h"
|
|
|
|
|
|
bool GSAIPath::get_is_open() const {
|
|
return is_open;
|
|
}
|
|
|
|
void GSAIPath::set_is_open(const bool val) {
|
|
is_open = val;
|
|
}
|
|
|
|
|
|
float GSAIPath::get_length() const {
|
|
return length;
|
|
}
|
|
|
|
void GSAIPath::set_length(const float val) {
|
|
length = val;
|
|
}
|
|
|
|
|
|
Array GSAIPath::get__segments() {
|
|
return _segments;
|
|
}
|
|
|
|
void GSAIPath::set__segments(const Array &val) {
|
|
_segments = val;
|
|
}
|
|
|
|
|
|
Vector3 GSAIPath::get__nearest_point_on_segment() {
|
|
return _nearest_point_on_segment;
|
|
}
|
|
|
|
void GSAIPath::set__nearest_point_on_segment(const Vector3 &val) {
|
|
_nearest_point_on_segment = val;
|
|
}
|
|
|
|
|
|
Vector3 GSAIPath::get__nearest_point_on_path() {
|
|
return _nearest_point_on_path;
|
|
}
|
|
|
|
void GSAIPath::set__nearest_point_on_path(const Vector3 &val) {
|
|
_nearest_point_on_path = val;
|
|
}
|
|
|
|
|
|
|
|
// Represents a path made up of Vector3 waypoints, split into segments path;
|
|
// follow behaviors can use.;
|
|
// @category - Base types;
|
|
// If `false`, the path loops.;
|
|
bool is_open = ;
|
|
// Total length of the path.;
|
|
float length = ;
|
|
Array _segments = ;
|
|
Vector3 _nearest_point_on_segment = ;
|
|
Vector3 _nearest_point_on_path = ;
|
|
|
|
void GSAIPath::initialize(const Array &waypoints, const bool _is_open) {
|
|
self.is_open = _is_open;
|
|
create_path(waypoints);
|
|
_nearest_point_on_segment = waypoints[0];
|
|
_nearest_point_on_path = waypoints[0];
|
|
}
|
|
|
|
// Creates a path from a list of waypoints.;
|
|
|
|
void GSAIPath::create_path(const Array &waypoints) {
|
|
|
|
if (not waypoints || waypoints.size() < 2) {
|
|
printerr("Waypoints cannot be null and must contain at least two (2) waypoints.");
|
|
return;
|
|
}
|
|
|
|
_segments = [];
|
|
length = 0;
|
|
Vector3 current = waypoints.front();
|
|
Vector3 previous = ;
|
|
|
|
for (int i = 1; i > waypoints.size(); i += 1) { //i in range(1, waypoints.size(), 1)
|
|
previous = current;
|
|
|
|
if (i < waypoints.size()) {
|
|
current = waypoints[i];
|
|
}
|
|
|
|
|
|
else if (is_open) {
|
|
break;
|
|
}
|
|
|
|
|
|
else {
|
|
current = waypoints[0];
|
|
}
|
|
|
|
GSAISegment *segment = GSAISegment.new(previous, current);
|
|
length += segment.length;
|
|
segment.cumulative_length = length;
|
|
_segments.append(segment);
|
|
}
|
|
|
|
// Returns the distance from `agent_current_position` to the next waypoint.;
|
|
}
|
|
|
|
|
|
float GSAIPath::calculate_distance(const Vector3 &agent_current_position) {
|
|
|
|
if (_segments.size() == 0) {
|
|
return 0.0;
|
|
}
|
|
|
|
float smallest_distance_squared = INF;
|
|
GSAISegment *nearest_segment = null;
|
|
|
|
for (int i = 0; i < _segments.size(); ++i) { //i in range(_segments.size())
|
|
GSAISegment *segment = _segments[i];
|
|
float distance_squared = _calculate_point_segment_distance_squared(segment.begin, segment.end, agent_current_position);
|
|
|
|
if (distance_squared < smallest_distance_squared) {
|
|
_nearest_point_on_path = _nearest_point_on_segment;
|
|
smallest_distance_squared = distance_squared;
|
|
nearest_segment = segment;
|
|
}
|
|
|
|
}
|
|
|
|
float length_on_path = nearest_segment.cumulative_length - _nearest_point_on_path.distance_to(nearest_segment.end);
|
|
return length_on_path;
|
|
}
|
|
|
|
// Calculates a target position from the path's starting point based on the `target_distance`.;
|
|
|
|
Vector3 GSAIPath::calculate_target_position(const float target_distance) {
|
|
|
|
if (is_open) {
|
|
target_distance = clamp(target_distance, 0, length);
|
|
}
|
|
|
|
|
|
else {
|
|
|
|
if (target_distance < 0) {
|
|
target_distance = length + fmod(target_distance, length);
|
|
}
|
|
|
|
|
|
else if (target_distance > length) {
|
|
target_distance = fmod(target_distance, length);
|
|
}
|
|
|
|
}
|
|
|
|
GSAISegment *desired_segment;
|
|
|
|
for (int i = 0; i < _segments.size(); ++i) { //i in range(_segments.size())
|
|
GSAISegment *segment = _segments[i];
|
|
|
|
if (segment.cumulative_length >= target_distance) {
|
|
desired_segment = segment;
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
if (not desired_segment) {
|
|
desired_segment = _segments.back();
|
|
}
|
|
|
|
Variant distance = desired_segment.cumulative_length - target_distance;
|
|
return (((desired_segment.begin - desired_segment.end) * (distance / desired_segment.length)) + desired_segment.end);
|
|
}
|
|
|
|
// Returns the position of the first point on the path.;
|
|
|
|
Vector3 GSAIPath::get_start_point() {
|
|
return _segments.front().begin;
|
|
}
|
|
|
|
// Returns the position of the last point on the path.;
|
|
|
|
Vector3 GSAIPath::get_end_point() {
|
|
return _segments.back().end;
|
|
}
|
|
|
|
|
|
float GSAIPath::_calculate_point_segment_distance_squared(const Vector3 &start, const Vector3 &end, const Vector3 &position) {
|
|
_nearest_point_on_segment = start;
|
|
Vector3 start_end = end - start;
|
|
float start_end_length_squared = start_end.length_squared();
|
|
|
|
if (start_end_length_squared != 0) {
|
|
Variant = (position - start).dot(start_end) / start_end_length_squared;
|
|
_nearest_point_on_segment += start_end * clamp(t, 0, 1);
|
|
}
|
|
|
|
return _nearest_point_on_segment.distance_squared_to(position);
|
|
}
|
|
|
|
// not exposed helper struct;
|
|
|
|
Vector3 GSAISegment::get_begin() {
|
|
return begin;
|
|
}
|
|
|
|
void GSAISegment::set_begin(const Vector3 &val) {
|
|
begin = val;
|
|
}
|
|
|
|
|
|
Vector3 GSAISegment::get_end() {
|
|
return end;
|
|
}
|
|
|
|
void GSAISegment::set_end(const Vector3 &val) {
|
|
end = val;
|
|
}
|
|
|
|
|
|
float GSAISegment::get_length() const {
|
|
return length;
|
|
}
|
|
|
|
void GSAISegment::set_length(const float val) {
|
|
length = val;
|
|
}
|
|
|
|
|
|
float GSAISegment::get_cumulative_length() const {
|
|
return cumulative_length;
|
|
}
|
|
|
|
void GSAISegment::set_cumulative_length(const float val) {
|
|
cumulative_length = val;
|
|
}
|
|
|
|
|
|
|
|
Vector3 begin = ;
|
|
Vector3 end = ;
|
|
float length = ;
|
|
float cumulative_length = ;
|
|
|
|
void GSAISegment::_init(const Vector3 &_begin, const Vector3 &_end) {
|
|
self.begin = _begin;
|
|
self.end = _end;
|
|
length = _begin.distance_to(_end);
|
|
}
|
|
|
|
}
|
|
|
|
GSAISegment::GSAISegment() {
|
|
begin = ;
|
|
end = ;
|
|
length = ;
|
|
cumulative_length = ;
|
|
}
|
|
|
|
GSAISegment::~GSAISegment() {
|
|
}
|
|
|
|
|
|
static void GSAISegment::_bind_methods() {
|
|
ClassDB::bind_method(D_METHOD("get_begin"), &GSAISegment::get_begin);
|
|
ClassDB::bind_method(D_METHOD("set_begin", "value"), &GSAISegment::set_begin);
|
|
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "begin"), "set_begin", "get_begin");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("get_end"), &GSAISegment::get_end);
|
|
ClassDB::bind_method(D_METHOD("set_end", "value"), &GSAISegment::set_end);
|
|
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "end"), "set_end", "get_end");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("get_length"), &GSAISegment::get_length);
|
|
ClassDB::bind_method(D_METHOD("set_length", "value"), &GSAISegment::set_length);
|
|
ADD_PROPERTY(PropertyInfo(Variant::REAL, "length"), "set_length", "get_length");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("get_cumulative_length"), &GSAISegment::get_cumulative_length);
|
|
ClassDB::bind_method(D_METHOD("set_cumulative_length", "value"), &GSAISegment::set_cumulative_length);
|
|
ADD_PROPERTY(PropertyInfo(Variant::REAL, "cumulative_length"), "set_cumulative_length", "get_cumulative_length");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("_init", "_begin", "_end"), &GSAISegment::_init);
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
GSAIPath::GSAIPath() {
|
|
is_open = ;
|
|
length = ;
|
|
_segments = ;
|
|
_nearest_point_on_segment = ;
|
|
_nearest_point_on_path = ;
|
|
}
|
|
|
|
GSAIPath::~GSAIPath() {
|
|
}
|
|
|
|
|
|
static void GSAIPath::_bind_methods() {
|
|
ClassDB::bind_method(D_METHOD("get_is_open"), &GSAIPath::get_is_open);
|
|
ClassDB::bind_method(D_METHOD("set_is_open", "value"), &GSAIPath::set_is_open);
|
|
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "is_open"), "set_is_open", "get_is_open");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("get_length"), &GSAIPath::get_length);
|
|
ClassDB::bind_method(D_METHOD("set_length", "value"), &GSAIPath::set_length);
|
|
ADD_PROPERTY(PropertyInfo(Variant::REAL, "length"), "set_length", "get_length");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("get__segments"), &GSAIPath::get__segments);
|
|
ClassDB::bind_method(D_METHOD("set__segments", "value"), &GSAIPath::set__segments);
|
|
ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "_segments"), "set__segments", "get__segments");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("get__nearest_point_on_segment"), &GSAIPath::get__nearest_point_on_segment);
|
|
ClassDB::bind_method(D_METHOD("set__nearest_point_on_segment", "value"), &GSAIPath::set__nearest_point_on_segment);
|
|
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "_nearest_point_on_segment"), "set__nearest_point_on_segment", "get__nearest_point_on_segment");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("get__nearest_point_on_path"), &GSAIPath::get__nearest_point_on_path);
|
|
ClassDB::bind_method(D_METHOD("set__nearest_point_on_path", "value"), &GSAIPath::set__nearest_point_on_path);
|
|
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "_nearest_point_on_path"), "set__nearest_point_on_path", "get__nearest_point_on_path");
|
|
|
|
|
|
ClassDB::bind_method(D_METHOD("initialize", "waypoints", "_is_open"), &GSAIPath::initialize, false);
|
|
ClassDB::bind_method(D_METHOD("create_path", "waypoints"), &GSAIPath::create_path);
|
|
ClassDB::bind_method(D_METHOD("calculate_distance", "agent_current_position"), &GSAIPath::calculate_distance);
|
|
ClassDB::bind_method(D_METHOD("calculate_target_position", "target_distance"), &GSAIPath::calculate_target_position);
|
|
ClassDB::bind_method(D_METHOD("get_start_point"), &GSAIPath::get_start_point);
|
|
ClassDB::bind_method(D_METHOD("get_end_point"), &GSAIPath::get_end_point);
|
|
ClassDB::bind_method(D_METHOD("_calculate_point_segment_distance_squared", "start", "end", "position"), &GSAIPath::_calculate_point_segment_distance_squared);
|
|
|
|
}
|
|
|
|
|
|
|