diff options
Diffstat (limited to 'core/math')
52 files changed, 2162 insertions, 833 deletions
diff --git a/core/math/SCsub b/core/math/SCsub index 4efc902717..be438fcfbe 100644 --- a/core/math/SCsub +++ b/core/math/SCsub @@ -2,6 +2,6 @@ Import('env') -env.add_source_files(env.core_sources, "*.cpp") +env_math = env.Clone() -Export('env') +env_math.add_source_files(env.core_sources, "*.cpp") diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index e4f93289e9..ae2b56e7b7 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -40,7 +40,17 @@ int AStar::get_available_point_id() const { return 1; } - return points.back()->key() + 1; + // calculate our new next available point id if bigger than before or next id already contained in set of points. + if (points.has(last_free_id)) { + int cur_new_id = last_free_id; + while (points.has(cur_new_id)) { + cur_new_id++; + } + int &non_const = const_cast<int &>(last_free_id); + non_const = cur_new_id; + } + + return last_free_id; } void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { @@ -48,78 +58,110 @@ void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { ERR_FAIL_COND(p_id < 0); ERR_FAIL_COND(p_weight_scale < 1); - if (!points.has(p_id)) { + Point *found_pt; + bool p_exists = points.lookup(p_id, found_pt); + + if (!p_exists) { Point *pt = memnew(Point); pt->id = p_id; pt->pos = p_pos; pt->weight_scale = p_weight_scale; pt->prev_point = NULL; - pt->last_pass = 0; - points[p_id] = pt; + pt->open_pass = 0; + pt->closed_pass = 0; + pt->enabled = true; + points.set(p_id, pt); } else { - points[p_id]->pos = p_pos; - points[p_id]->weight_scale = p_weight_scale; + found_pt->pos = p_pos; + found_pt->weight_scale = p_weight_scale; } } Vector3 AStar::get_point_position(int p_id) const { - ERR_FAIL_COND_V(!points.has(p_id), Vector3()); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND_V(!p_exists, Vector3()); - return points[p_id]->pos; + return p->pos; } void AStar::set_point_position(int p_id, const Vector3 &p_pos) { - ERR_FAIL_COND(!points.has(p_id)); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND(!p_exists); - points[p_id]->pos = p_pos; + p->pos = p_pos; } real_t AStar::get_point_weight_scale(int p_id) const { - ERR_FAIL_COND_V(!points.has(p_id), 0); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND_V(!p_exists, 0); - return points[p_id]->weight_scale; + return p->weight_scale; } void AStar::set_point_weight_scale(int p_id, real_t p_weight_scale) { - ERR_FAIL_COND(!points.has(p_id)); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND(!p_exists); ERR_FAIL_COND(p_weight_scale < 1); - points[p_id]->weight_scale = p_weight_scale; + p->weight_scale = p_weight_scale; } void AStar::remove_point(int p_id) { - ERR_FAIL_COND(!points.has(p_id)); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND(!p_exists); - Point *p = points[p_id]; + for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { - for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) { + Segment s(p_id, (*it.key)); + segments.erase(s); - Segment s(p_id, E->get()->id); + (*it.value)->neighbours.remove(p->id); + (*it.value)->unlinked_neighbours.remove(p->id); + } + + for (OAHashMap<int, Point *>::Iterator it = p->unlinked_neighbours.iter(); it.valid; it = p->unlinked_neighbours.next_iter(it)) { + + Segment s(p_id, (*it.key)); segments.erase(s); - E->get()->neighbours.erase(p); + + (*it.value)->neighbours.remove(p->id); + (*it.value)->unlinked_neighbours.remove(p->id); } memdelete(p); - points.erase(p_id); + points.remove(p_id); + last_free_id = p_id; } void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { - ERR_FAIL_COND(!points.has(p_id)); - ERR_FAIL_COND(!points.has(p_with_id)); ERR_FAIL_COND(p_id == p_with_id); - Point *a = points[p_id]; - Point *b = points[p_with_id]; - a->neighbours.insert(b); + Point *a; + bool from_exists = points.lookup(p_id, a); + ERR_FAIL_COND(!from_exists); - if (bidirectional) - b->neighbours.insert(a); + Point *b; + bool to_exists = points.lookup(p_with_id, b); + ERR_FAIL_COND(!to_exists); + + a->neighbours.set(b->id, b); + + if (bidirectional) { + b->neighbours.set(a->id, a); + } else { + b->unlinked_neighbours.set(a->id, a); + } Segment s(p_id, p_with_id); if (s.from == p_id) { @@ -132,6 +174,7 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { segments.insert(s); } + void AStar::disconnect_points(int p_id, int p_with_id) { Segment s(p_id, p_with_id); @@ -139,10 +182,18 @@ void AStar::disconnect_points(int p_id, int p_with_id) { segments.erase(s); - Point *a = points[p_id]; - Point *b = points[p_with_id]; - a->neighbours.erase(b); - b->neighbours.erase(a); + Point *a; + bool a_exists = points.lookup(p_id, a); + CRASH_COND(!a_exists); + + Point *b; + bool b_exists = points.lookup(p_with_id, b); + CRASH_COND(!b_exists); + + a->neighbours.remove(b->id); + a->unlinked_neighbours.remove(b->id); + b->neighbours.remove(a->id); + b->unlinked_neighbours.remove(a->id); } bool AStar::has_point(int p_id) const { @@ -154,8 +205,8 @@ Array AStar::get_points() { Array point_list; - for (const Map<int, Point *>::Element *E = points.front(); E; E = E->next()) { - point_list.push_back(E->key()); + for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { + point_list.push_back(*(it.key)); } return point_list; @@ -163,14 +214,14 @@ Array AStar::get_points() { PoolVector<int> AStar::get_point_connections(int p_id) { - ERR_FAIL_COND_V(!points.has(p_id), PoolVector<int>()); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND_V(!p_exists, PoolVector<int>()); PoolVector<int> point_list; - Point *p = points[p_id]; - - for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) { - point_list.push_back(E->get()->id); + for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { + point_list.push_back((*it.key)); } return point_list; @@ -184,25 +235,41 @@ bool AStar::are_points_connected(int p_id, int p_with_id) const { void AStar::clear() { - for (const Map<int, Point *>::Element *E = points.front(); E; E = E->next()) { - - memdelete(E->get()); + last_free_id = 0; + for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { + memdelete(*(it.value)); } segments.clear(); points.clear(); } -int AStar::get_closest_point(const Vector3 &p_point) const { +int AStar::get_point_count() const { + return points.get_num_elements(); +} + +int AStar::get_point_capacity() const { + return points.get_capacity(); +} + +void AStar::reserve_space(int p_num_nodes) { + ERR_FAIL_COND_MSG(p_num_nodes <= 0, "New capacity must be greater than 0, was: " + itos(p_num_nodes) + "."); + ERR_FAIL_COND_MSG((uint32_t)p_num_nodes < points.get_capacity(), "New capacity must be greater than current capacity: " + itos(points.get_capacity()) + ", new was: " + itos(p_num_nodes) + "."); + points.reserve(p_num_nodes); +} + +int AStar::get_closest_point(const Vector3 &p_point, bool p_include_disabled) const { int closest_id = -1; real_t closest_dist = 1e20; - for (const Map<int, Point *>::Element *E = points.front(); E; E = E->next()) { + for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { - real_t d = p_point.distance_squared_to(E->get()->pos); + if (!p_include_disabled && !(*it.value)->enabled) continue; // Disabled points should not be considered. + + real_t d = p_point.distance_squared_to((*it.value)->pos); if (closest_id < 0 || d < closest_dist) { closest_dist = d; - closest_id = E->key(); + closest_id = *(it.key); } } @@ -211,12 +278,16 @@ int AStar::get_closest_point(const Vector3 &p_point) const { Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const { - real_t closest_dist = 1e20; bool found = false; + real_t closest_dist = 1e20; Vector3 closest_point; for (const Set<Segment>::Element *E = segments.front(); E; E = E->next()) { + if (!(E->get().from_point->enabled && E->get().to_point->enabled)) { + continue; + } + Vector3 segment[2] = { E->get().from_point->pos, E->get().to_point->pos, @@ -239,91 +310,60 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { pass++; - SelfList<Point>::List open_list; + if (!end_point->enabled) return false; bool found_route = false; - for (Set<Point *>::Element *E = begin_point->neighbours.front(); E; E = E->next()) { + Vector<Point *> open_list; + SortArray<Point *, SortPoints> sorter; - Point *n = E->get(); - n->prev_point = begin_point; - n->distance = _compute_cost(begin_point->id, n->id) * n->weight_scale; - n->last_pass = pass; - open_list.add(&n->list); + begin_point->g_score = 0; + begin_point->f_score = _estimate_cost(begin_point->id, end_point->id); + open_list.push_back(begin_point); - if (end_point == n) { - found_route = true; - break; - } - } + while (!open_list.empty()) { - while (!found_route) { + Point *p = open_list[0]; // The currently processed point - if (open_list.first() == NULL) { - // No path found + if (p == end_point) { + found_route = true; break; } - // Check open list - SelfList<Point> *least_cost_point = NULL; - real_t least_cost = 1e30; + sorter.pop_heap(0, open_list.size(), open_list.ptrw()); // Remove the current point from the open list + open_list.remove(open_list.size() - 1); + p->closed_pass = pass; // Mark the point as closed - // TODO: Cache previous results - for (SelfList<Point> *E = open_list.first(); E; E = E->next()) { + for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { - Point *p = E->self(); + Point *e = *(it.value); // The neighbour point - real_t cost = p->distance; - cost += _estimate_cost(p->id, end_point->id); - - if (cost < least_cost) { - - least_cost_point = E; - least_cost = cost; + if (!e->enabled || e->closed_pass == pass) { + continue; } - } - - Point *p = least_cost_point->self(); - - for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) { - Point *e = E->get(); + real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale; - real_t distance = _compute_cost(p->id, e->id) * e->weight_scale + p->distance; + bool new_point = false; - if (e->last_pass == pass) { - // Already visited, is this cheaper? + if (e->open_pass != pass) { // The point wasn't inside the open list. + e->open_pass = pass; + open_list.push_back(e); + new_point = true; + } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous. + continue; + } - if (e->distance > distance) { + e->prev_point = p; + e->g_score = tentative_g_score; + e->f_score = e->g_score + _estimate_cost(e->id, end_point->id); - e->prev_point = p; - e->distance = distance; - } + if (new_point) { // The position of the new points is already known. + sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptrw()); } else { - // Add to open neighbours - - e->prev_point = p; - e->distance = distance; - e->last_pass = pass; // Mark as used - open_list.add(&e->list); - - if (e == end_point) { - // End reached; stop algorithm - found_route = true; - break; - } + sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptrw()); } } - - if (found_route) - break; - - open_list.remove(least_cost_point); - } - - // Clear the openf list - while (open_list.first()) { - open_list.remove(open_list.first()); } return found_route; @@ -334,7 +374,15 @@ float AStar::_estimate_cost(int p_from_id, int p_to_id) { if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost)) return get_script_instance()->call(SceneStringNames::get_singleton()->_estimate_cost, p_from_id, p_to_id); - return points[p_from_id]->pos.distance_to(points[p_to_id]->pos); + Point *from_point; + bool from_exists = points.lookup(p_from_id, from_point); + CRASH_COND(!from_exists); + + Point *to_point; + bool to_exists = points.lookup(p_to_id, to_point); + CRASH_COND(!to_exists); + + return from_point->pos.distance_to(to_point->pos); } float AStar::_compute_cost(int p_from_id, int p_to_id) { @@ -342,18 +390,26 @@ float AStar::_compute_cost(int p_from_id, int p_to_id) { if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost)) return get_script_instance()->call(SceneStringNames::get_singleton()->_compute_cost, p_from_id, p_to_id); - return points[p_from_id]->pos.distance_to(points[p_to_id]->pos); + Point *from_point; + bool from_exists = points.lookup(p_from_id, from_point); + CRASH_COND(!from_exists); + + Point *to_point; + bool to_exists = points.lookup(p_to_id, to_point); + CRASH_COND(!to_exists); + + return from_point->pos.distance_to(to_point->pos); } PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { - ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector<Vector3>()); - ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector<Vector3>()); - - pass++; + Point *a; + bool from_exists = points.lookup(p_from_id, a); + ERR_FAIL_COND_V(!from_exists, PoolVector<Vector3>()); - Point *a = points[p_from_id]; - Point *b = points[p_to_id]; + Point *b; + bool to_exists = points.lookup(p_to_id, b); + ERR_FAIL_COND_V(!to_exists, PoolVector<Vector3>()); if (a == b) { PoolVector<Vector3> ret; @@ -365,11 +421,8 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { Point *end_point = b; bool found_route = _solve(begin_point, end_point); + if (!found_route) return PoolVector<Vector3>(); - if (!found_route) - return PoolVector<Vector3>(); - - // Midpoints Point *p = end_point; int pc = 1; // Begin point while (p != begin_point) { @@ -383,14 +436,14 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { { PoolVector<Vector3>::Write w = path.write(); - Point *p = end_point; + Point *p2 = end_point; int idx = pc - 1; - while (p != begin_point) { - w[idx--] = p->pos; - p = p->prev_point; + while (p2 != begin_point) { + w[idx--] = p2->pos; + p2 = p2->prev_point; } - w[0] = p->pos; // Assign first + w[0] = p2->pos; // Assign first } return path; @@ -398,13 +451,13 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { - ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector<int>()); - ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector<int>()); - - pass++; + Point *a; + bool from_exists = points.lookup(p_from_id, a); + ERR_FAIL_COND_V(!from_exists, PoolVector<int>()); - Point *a = points[p_from_id]; - Point *b = points[p_to_id]; + Point *b; + bool to_exists = points.lookup(p_to_id, b); + ERR_FAIL_COND_V(!to_exists, PoolVector<int>()); if (a == b) { PoolVector<int> ret; @@ -416,11 +469,8 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { Point *end_point = b; bool found_route = _solve(begin_point, end_point); + if (!found_route) return PoolVector<int>(); - if (!found_route) - return PoolVector<int>(); - - // Midpoints Point *p = end_point; int pc = 1; // Begin point while (p != begin_point) { @@ -447,6 +497,24 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { return path; } +void AStar::set_point_disabled(int p_id, bool p_disabled) { + + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND(!p_exists); + + p->enabled = !p_disabled; +} + +bool AStar::is_point_disabled(int p_id) const { + + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND_V(!p_exists, false); + + return !p->enabled; +} + void AStar::_bind_methods() { ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar::get_available_point_id); @@ -457,17 +525,22 @@ void AStar::_bind_methods() { ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar::set_point_weight_scale); ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar::remove_point); ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar::has_point); + ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar::get_point_connections); ClassDB::bind_method(D_METHOD("get_points"), &AStar::get_points); - ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar::get_point_connections); + ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar::set_point_disabled, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar::is_point_disabled); ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar::connect_points, DEFVAL(true)); ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar::disconnect_points); ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar::are_points_connected); + ClassDB::bind_method(D_METHOD("get_point_count"), &AStar::get_point_count); + ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar::get_point_capacity); + ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar::reserve_space); ClassDB::bind_method(D_METHOD("clear"), &AStar::clear); - ClassDB::bind_method(D_METHOD("get_closest_point", "to_position"), &AStar::get_closest_point); + ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar::get_closest_point, DEFVAL(false)); ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar::get_closest_position_in_segment); ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar::get_point_path); @@ -478,12 +551,157 @@ void AStar::_bind_methods() { } AStar::AStar() { - + last_free_id = 0; pass = 1; } AStar::~AStar() { - - pass = 1; clear(); } + +///////////////////////////////////////////////////////////// + +int AStar2D::get_available_point_id() const { + return astar.get_available_point_id(); +} + +void AStar2D::add_point(int p_id, const Vector2 &p_pos, real_t p_weight_scale) { + astar.add_point(p_id, Vector3(p_pos.x, p_pos.y, 0), p_weight_scale); +} + +Vector2 AStar2D::get_point_position(int p_id) const { + Vector3 p = astar.get_point_position(p_id); + return Vector2(p.x, p.y); +} + +void AStar2D::set_point_position(int p_id, const Vector2 &p_pos) { + astar.set_point_position(p_id, Vector3(p_pos.x, p_pos.y, 0)); +} + +real_t AStar2D::get_point_weight_scale(int p_id) const { + return astar.get_point_weight_scale(p_id); +} + +void AStar2D::set_point_weight_scale(int p_id, real_t p_weight_scale) { + astar.set_point_weight_scale(p_id, p_weight_scale); +} + +void AStar2D::remove_point(int p_id) { + astar.remove_point(p_id); +} + +bool AStar2D::has_point(int p_id) const { + return astar.has_point(p_id); +} + +PoolVector<int> AStar2D::get_point_connections(int p_id) { + return astar.get_point_connections(p_id); +} + +Array AStar2D::get_points() { + return astar.get_points(); +} + +void AStar2D::set_point_disabled(int p_id, bool p_disabled) { + astar.set_point_disabled(p_id, p_disabled); +} + +bool AStar2D::is_point_disabled(int p_id) const { + return astar.is_point_disabled(p_id); +} + +void AStar2D::connect_points(int p_id, int p_with_id, bool p_bidirectional) { + astar.connect_points(p_id, p_with_id, p_bidirectional); +} + +void AStar2D::disconnect_points(int p_id, int p_with_id) { + astar.disconnect_points(p_id, p_with_id); +} + +bool AStar2D::are_points_connected(int p_id, int p_with_id) const { + return astar.are_points_connected(p_id, p_with_id); +} + +int AStar2D::get_point_count() const { + return astar.get_point_count(); +} + +int AStar2D::get_point_capacity() const { + return astar.get_point_capacity(); +} + +void AStar2D::clear() { + astar.clear(); +} + +void AStar2D::reserve_space(int p_num_nodes) { + astar.reserve_space(p_num_nodes); +} + +int AStar2D::get_closest_point(const Vector2 &p_point, bool p_include_disabled) const { + return astar.get_closest_point(Vector3(p_point.x, p_point.y, 0), p_include_disabled); +} + +Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const { + Vector3 p = astar.get_closest_position_in_segment(Vector3(p_point.x, p_point.y, 0)); + return Vector2(p.x, p.y); +} + +PoolVector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) { + + PoolVector3Array pv = astar.get_point_path(p_from_id, p_to_id); + int size = pv.size(); + PoolVector2Array path; + path.resize(size); + { + PoolVector<Vector3>::Read r = pv.read(); + PoolVector<Vector2>::Write w = path.write(); + for (int i = 0; i < size; i++) { + Vector3 p = r[i]; + w[i] = Vector2(p.x, p.y); + } + } + return path; +} + +PoolVector<int> AStar2D::get_id_path(int p_from_id, int p_to_id) { + return astar.get_id_path(p_from_id, p_to_id); +} + +void AStar2D::_bind_methods() { + + ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar2D::get_available_point_id); + ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar2D::add_point, DEFVAL(1.0)); + ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar2D::get_point_position); + ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar2D::set_point_position); + ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar2D::get_point_weight_scale); + ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar2D::set_point_weight_scale); + ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar2D::remove_point); + ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar2D::has_point); + ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar2D::get_point_connections); + ClassDB::bind_method(D_METHOD("get_points"), &AStar2D::get_points); + + ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar2D::set_point_disabled, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar2D::is_point_disabled); + + ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar2D::connect_points, DEFVAL(true)); + ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar2D::disconnect_points); + ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar2D::are_points_connected); + + ClassDB::bind_method(D_METHOD("get_point_count"), &AStar2D::get_point_count); + ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar2D::get_point_capacity); + ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar2D::reserve_space); + ClassDB::bind_method(D_METHOD("clear"), &AStar2D::clear); + + ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar2D::get_closest_point, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar2D::get_closest_position_in_segment); + + ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar2D::get_point_path); + ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar2D::get_id_path); +} + +AStar2D::AStar2D() { +} + +AStar2D::~AStar2D() { +} diff --git a/core/math/a_star.h b/core/math/a_star.h index d2ef765006..0a5d3e992c 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,8 +31,8 @@ #ifndef ASTAR_H #define ASTAR_H +#include "core/oa_hash_map.h" #include "core/reference.h" -#include "core/self_list.h" /** A* pathfinding algorithm @@ -42,30 +42,41 @@ class AStar : public Reference { - GDCLASS(AStar, Reference) - - uint64_t pass; + GDCLASS(AStar, Reference); struct Point { - SelfList<Point> list; + Point() : + neighbours(4u), + unlinked_neighbours(4u) {} int id; Vector3 pos; real_t weight_scale; - uint64_t last_pass; + bool enabled; - Set<Point *> neighbours; + OAHashMap<int, Point *> neighbours; + OAHashMap<int, Point *> unlinked_neighbours; - // Used for pathfinding + // Used for pathfinding. Point *prev_point; - real_t distance; - - Point() : - list(this) {} + real_t g_score; + real_t f_score; + uint64_t open_pass; + uint64_t closed_pass; }; - Map<int, Point *> points; + struct SortPoints { + _FORCE_INLINE_ bool operator()(const Point *A, const Point *B) const { // Returns true when the Point A is worse than Point B. + if (A->f_score > B->f_score) { + return true; + } else if (A->f_score < B->f_score) { + return false; + } else { + return A->g_score < B->g_score; // If the f_costs are the same then prioritize the points that are further away from the start. + } + } + }; struct Segment { union { @@ -91,6 +102,10 @@ class AStar : public Reference { } }; + int last_free_id; + uint64_t pass; + + OAHashMap<int, Point *> points; Set<Segment> segments; bool _solve(Point *begin_point, Point *end_point); @@ -114,13 +129,19 @@ public: PoolVector<int> get_point_connections(int p_id); Array get_points(); + void set_point_disabled(int p_id, bool p_disabled = true); + bool is_point_disabled(int p_id) const; + void connect_points(int p_id, int p_with_id, bool bidirectional = true); void disconnect_points(int p_id, int p_with_id); bool are_points_connected(int p_id, int p_with_id) const; + int get_point_count() const; + int get_point_capacity() const; + void reserve_space(int p_num_nodes); void clear(); - int get_closest_point(const Vector3 &p_point) const; + int get_closest_point(const Vector3 &p_point, bool p_include_disabled = false) const; Vector3 get_closest_position_in_segment(const Vector3 &p_point) const; PoolVector<Vector3> get_point_path(int p_from_id, int p_to_id); @@ -130,4 +151,46 @@ public: ~AStar(); }; +class AStar2D : public Reference { + GDCLASS(AStar2D, Reference); + AStar astar; + +protected: + static void _bind_methods(); + +public: + int get_available_point_id() const; + + void add_point(int p_id, const Vector2 &p_pos, real_t p_weight_scale = 1); + Vector2 get_point_position(int p_id) const; + void set_point_position(int p_id, const Vector2 &p_pos); + real_t get_point_weight_scale(int p_id) const; + void set_point_weight_scale(int p_id, real_t p_weight_scale); + void remove_point(int p_id); + bool has_point(int p_id) const; + PoolVector<int> get_point_connections(int p_id); + Array get_points(); + + void set_point_disabled(int p_id, bool p_disabled = true); + bool is_point_disabled(int p_id) const; + + void connect_points(int p_id, int p_with_id, bool p_bidirectional = true); + void disconnect_points(int p_id, int p_with_id); + bool are_points_connected(int p_id, int p_with_id) const; + + int get_point_count() const; + int get_point_capacity() const; + void reserve_space(int p_num_nodes); + void clear(); + + int get_closest_point(const Vector2 &p_point, bool p_include_disabled = false) const; + Vector2 get_closest_position_in_segment(const Vector2 &p_point) const; + + PoolVector<Vector2> get_point_path(int p_from_id, int p_to_id); + PoolVector<int> get_id_path(int p_from_id, int p_to_id); + + AStar2D(); + ~AStar2D(); +}; + #endif // ASTAR_H diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index d0cb2b5195..a4eb1fe2a5 100644 --- a/core/math/aabb.cpp +++ b/core/math/aabb.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/aabb.h b/core/math/aabb.h index 0b03b7d314..52e5ed3626 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/audio_frame.cpp b/core/math/audio_frame.cpp index eff817bbaa..2496a70890 100644 --- a/core/math/audio_frame.cpp +++ b/core/math/audio_frame.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/audio_frame.h b/core/math/audio_frame.h index fde26e8056..98e4e33021 100644 --- a/core/math/audio_frame.h +++ b/core/math/audio_frame.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,6 +31,7 @@ #ifndef AUDIOFRAME_H #define AUDIOFRAME_H +#include "core/math/vector2.h" #include "core/typedefs.h" static inline float undenormalise(volatile float f) { @@ -122,6 +123,20 @@ struct AudioFrame { r = p_frame.r; } + _ALWAYS_INLINE_ AudioFrame operator=(const AudioFrame &p_frame) { + l = p_frame.l; + r = p_frame.r; + return *this; + } + + _ALWAYS_INLINE_ operator Vector2() const { + return Vector2(l, r); + } + + _ALWAYS_INLINE_ AudioFrame(const Vector2 &p_v2) { + l = p_v2.x; + r = p_v2.y; + } _ALWAYS_INLINE_ AudioFrame() {} }; diff --git a/core/math/matrix3.cpp b/core/math/basis.cpp index fca54b1556..0a491010e2 100644 --- a/core/math/matrix3.cpp +++ b/core/math/basis.cpp @@ -1,12 +1,12 @@ /*************************************************************************/ -/* matrix3.cpp */ +/* basis.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -28,7 +28,7 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "matrix3.h" +#include "basis.h" #include "core/math/math_funcs.h" #include "core/os/copymem.h" @@ -76,9 +76,11 @@ void Basis::invert() { } void Basis::orthonormalize() { + #ifdef MATH_CHECKS ERR_FAIL_COND(determinant() == 0); #endif + // Gram-Schmidt Process Vector3 x = get_axis(0); @@ -118,16 +120,16 @@ bool Basis::is_diagonal() const { } bool Basis::is_rotation() const { - return Math::is_equal_approx(determinant(), 1) && is_orthogonal(); + return Math::is_equal_approx(determinant(), 1, UNIT_EPSILON) && is_orthogonal(); } bool Basis::is_symmetric() const { - if (!Math::is_equal_approx(elements[0][1], elements[1][0])) + if (!Math::is_equal_approx_ratio(elements[0][1], elements[1][0], UNIT_EPSILON)) return false; - if (!Math::is_equal_approx(elements[0][2], elements[2][0])) + if (!Math::is_equal_approx_ratio(elements[0][2], elements[2][0], UNIT_EPSILON)) return false; - if (!Math::is_equal_approx(elements[1][2], elements[2][1])) + if (!Math::is_equal_approx_ratio(elements[1][2], elements[2][1], UNIT_EPSILON)) return false; return true; @@ -258,7 +260,7 @@ Vector3 Basis::get_scale_abs() const { } Vector3 Basis::get_scale_local() const { - real_t det_sign = determinant() > 0 ? 1 : -1; + real_t det_sign = SGN(determinant()); return det_sign * Vector3(elements[0].length(), elements[1].length(), elements[2].length()); } @@ -284,7 +286,7 @@ Vector3 Basis::get_scale() const { // matrix elements. // // The rotation part of this decomposition is returned by get_rotation* functions. - real_t det_sign = determinant() > 0 ? 1 : -1; + real_t det_sign = SGN(determinant()); return det_sign * Vector3( Vector3(elements[0][0], elements[1][0], elements[2][0]).length(), Vector3(elements[0][1], elements[1][1], elements[2][1]).length(), @@ -299,14 +301,14 @@ Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const { ERR_FAIL_COND_V(determinant() == 0, Vector3()); Basis m = transposed() * (*this); - ERR_FAIL_COND_V(m.is_diagonal() == false, Vector3()); + ERR_FAIL_COND_V(!m.is_diagonal(), Vector3()); #endif Vector3 scale = get_scale(); Basis inv_scale = Basis().scaled(scale.inverse()); // this will also absorb the sign of scale rotref = (*this) * inv_scale; #ifdef MATH_CHECKS - ERR_FAIL_COND_V(rotref.is_orthogonal() == false, Vector3()); + ERR_FAIL_COND_V(!rotref.is_orthogonal(), Vector3()); #endif return scale.abs(); } @@ -430,7 +432,7 @@ Vector3 Basis::get_euler_xyz() const { Vector3 euler; #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_rotation() == false, euler); + ERR_FAIL_COND_V(!is_rotation(), euler); #endif real_t sy = elements[0][2]; if (sy < 1.0) { @@ -488,6 +490,11 @@ void Basis::set_euler_xyz(const Vector3 &p_euler) { // as the x, y, and z components of a Vector3 respectively. Vector3 Basis::get_euler_yxz() const { + /* checking this is a bad idea, because obtaining from scaled transform is a valid use case +#ifdef MATH_CHECKS + ERR_FAIL_COND(!is_rotation()); +#endif +*/ // Euler angles in YXZ convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -496,9 +503,7 @@ Vector3 Basis::get_euler_yxz() const { // cy*sx*sz-cz*sy cy*cz*sx+sy*sz cy*cx Vector3 euler; -#ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_rotation() == false, euler); -#endif + real_t m12 = elements[1][2]; if (m12 < 1) { @@ -552,11 +557,23 @@ void Basis::set_euler_yxz(const Vector3 &p_euler) { *this = ymat * xmat * zmat; } -bool Basis::is_equal_approx(const Basis &a, const Basis &b) const { +bool Basis::is_equal_approx(const Basis &a, const Basis &b, real_t p_epsilon) const { + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (!Math::is_equal_approx(a.elements[i][j], b.elements[i][j], p_epsilon)) + return false; + } + } + + return true; +} + +bool Basis::is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon) const { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - if (Math::is_equal_approx(a.elements[i][j], b.elements[i][j]) == false) + if (!Math::is_equal_approx_ratio(a.elements[i][j], b.elements[i][j], p_epsilon)) return false; } } @@ -599,10 +616,13 @@ Basis::operator String() const { } Quat Basis::get_quat() const { + #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_rotation() == false, Quat()); + ERR_FAIL_COND_V_MSG(!is_rotation(), Quat(), "Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() instead."); #endif - real_t trace = elements[0][0] + elements[1][1] + elements[2][2]; + /* Allow getting a quaternion from an unnormalized transform */ + Basis m = *this; + real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; real_t temp[4]; if (trace > 0.0) { @@ -610,23 +630,23 @@ Quat Basis::get_quat() const { temp[3] = (s * 0.5); s = 0.5 / s; - temp[0] = ((elements[2][1] - elements[1][2]) * s); - temp[1] = ((elements[0][2] - elements[2][0]) * s); - temp[2] = ((elements[1][0] - elements[0][1]) * s); + temp[0] = ((m.elements[2][1] - m.elements[1][2]) * s); + temp[1] = ((m.elements[0][2] - m.elements[2][0]) * s); + temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s); } else { - int i = elements[0][0] < elements[1][1] ? - (elements[1][1] < elements[2][2] ? 2 : 1) : - (elements[0][0] < elements[2][2] ? 2 : 0); + int i = m.elements[0][0] < m.elements[1][1] ? + (m.elements[1][1] < m.elements[2][2] ? 2 : 1) : + (m.elements[0][0] < m.elements[2][2] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; - real_t s = Math::sqrt(elements[i][i] - elements[j][j] - elements[k][k] + 1.0); + real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0); temp[i] = s * 0.5; s = 0.5 / s; - temp[3] = (elements[k][j] - elements[j][k]) * s; - temp[j] = (elements[j][i] + elements[i][j]) * s; - temp[k] = (elements[k][i] + elements[i][k]) * s; + temp[3] = (m.elements[k][j] - m.elements[j][k]) * s; + temp[j] = (m.elements[j][i] + m.elements[i][j]) * s; + temp[k] = (m.elements[k][i] + m.elements[i][k]) * s; } return Quat(temp[0], temp[1], temp[2], temp[3]); @@ -696,9 +716,11 @@ void Basis::set_orthogonal_index(int p_index) { } void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { + /* checking this is a bad idea, because obtaining from scaled transform is a valid use case #ifdef MATH_CHECKS - ERR_FAIL_COND(is_rotation() == false); + ERR_FAIL_COND(!is_rotation()); #endif +*/ real_t angle, x, y, z; // variables for result real_t epsilon = 0.01; // margin to allow for rounding errors real_t epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees @@ -785,24 +807,31 @@ void Basis::set_quat(const Quat &p_quat) { void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) { // Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_angle #ifdef MATH_CHECKS - ERR_FAIL_COND(p_axis.is_normalized() == false); + ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "Axis must be normalized."); #endif Vector3 axis_sq(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z); - real_t cosine = Math::cos(p_phi); + elements[0][0] = axis_sq.x + cosine * (1.0 - axis_sq.x); + elements[1][1] = axis_sq.y + cosine * (1.0 - axis_sq.y); + elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z); + real_t sine = Math::sin(p_phi); + real_t t = 1 - cosine; - elements[0][0] = axis_sq.x + cosine * (1.0 - axis_sq.x); - elements[0][1] = p_axis.x * p_axis.y * (1.0 - cosine) - p_axis.z * sine; - elements[0][2] = p_axis.z * p_axis.x * (1.0 - cosine) + p_axis.y * sine; + real_t xyzt = p_axis.x * p_axis.y * t; + real_t zyxs = p_axis.z * sine; + elements[0][1] = xyzt - zyxs; + elements[1][0] = xyzt + zyxs; - elements[1][0] = p_axis.x * p_axis.y * (1.0 - cosine) + p_axis.z * sine; - elements[1][1] = axis_sq.y + cosine * (1.0 - axis_sq.y); - elements[1][2] = p_axis.y * p_axis.z * (1.0 - cosine) - p_axis.x * sine; + xyzt = p_axis.x * p_axis.z * t; + zyxs = p_axis.y * sine; + elements[0][2] = xyzt + zyxs; + elements[2][0] = xyzt - zyxs; - elements[2][0] = p_axis.z * p_axis.x * (1.0 - cosine) - p_axis.y * sine; - elements[2][1] = p_axis.y * p_axis.z * (1.0 - cosine) + p_axis.x * sine; - elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z); + xyzt = p_axis.y * p_axis.z * t; + zyxs = p_axis.x * sine; + elements[1][2] = xyzt - zyxs; + elements[2][1] = xyzt + zyxs; } void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { @@ -820,7 +849,7 @@ void Basis::set_quat_scale(const Quat &p_quat, const Vector3 &p_scale) { rotate(p_quat); } -void Basis::set_diagonal(const Vector3 p_diag) { +void Basis::set_diagonal(const Vector3 &p_diag) { elements[0][0] = p_diag.x; elements[0][1] = 0; elements[0][2] = 0; @@ -835,14 +864,15 @@ void Basis::set_diagonal(const Vector3 p_diag) { } Basis Basis::slerp(const Basis &target, const real_t &t) const { -// TODO: implement this directly without using quaternions to make it more efficient -#ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_rotation() == false, Basis()); - ERR_FAIL_COND_V(target.is_rotation() == false, Basis()); -#endif + //consider scale Quat from(*this); Quat to(target); - return Basis(from.slerp(to, t)); + Basis b(from.slerp(to, t)); + b.elements[0] *= Math::lerp(elements[0].length(), target.elements[0].length(), t); + b.elements[1] *= Math::lerp(elements[1].length(), target.elements[1].length(), t); + b.elements[2] *= Math::lerp(elements[2].length(), target.elements[2].length(), t); + + return b; } diff --git a/core/math/matrix3.h b/core/math/basis.h index 35bf75bbe4..053effda69 100644 --- a/core/math/matrix3.h +++ b/core/math/basis.h @@ -1,12 +1,12 @@ /*************************************************************************/ -/* matrix3.h */ +/* basis.h */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,15 +31,11 @@ // Circular dependency between Vector3 and Basis :/ #include "core/math/vector3.h" -#ifndef MATRIX3_H -#define MATRIX3_H +#ifndef BASIS_H +#define BASIS_H #include "core/math/quat.h" -/** - @author Juan Linietsky <reduzio@gmail.com> -*/ - class Basis { public: Vector3 elements[3]; @@ -133,7 +129,8 @@ public: return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2]; } - bool is_equal_approx(const Basis &a, const Basis &b) const; + bool is_equal_approx(const Basis &a, const Basis &b, real_t p_epsilon = CMP_EPSILON) const; + bool is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon = UNIT_EPSILON) const; bool operator==(const Basis &p_matrix) const; bool operator!=(const Basis &p_matrix) const; @@ -152,7 +149,7 @@ public: int get_orthogonal_index() const; void set_orthogonal_index(int p_index); - void set_diagonal(const Vector3 p_diag); + void set_diagonal(const Vector3 &p_diag); bool is_orthogonal() const; bool is_diagonal() const; @@ -341,4 +338,4 @@ real_t Basis::determinant() const { elements[1][0] * (elements[0][1] * elements[2][2] - elements[2][1] * elements[0][2]) + elements[2][0] * (elements[0][1] * elements[1][2] - elements[1][1] * elements[0][2]); } -#endif +#endif // BASIS_H diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp index 6e51c56357..cfa698282e 100644 --- a/core/math/bsp_tree.cpp +++ b/core/math/bsp_tree.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -142,7 +142,7 @@ int BSP_Tree::_get_points_inside(int p_node, const Vector3 *p_points, int *p_ind } return _get_points_inside(node->over, p_points, p_indices, p_center, p_half_extents, p_indices_count); - } else if (dist_min <= 0) { //all points behind plane + } else { //all points behind plane if (node->under == UNDER_LEAF) { @@ -150,8 +150,6 @@ int BSP_Tree::_get_points_inside(int p_node, const Vector3 *p_points, int *p_ind } return _get_points_inside(node->under, p_points, p_indices, p_center, p_half_extents, p_indices_count); } - - return 0; } int BSP_Tree::get_points_inside(const Vector3 *p_points, int p_point_count) const { @@ -165,7 +163,6 @@ int BSP_Tree::get_points_inside(const Vector3 *p_points, int p_point_count) cons int pass_count = 0; const Node *nodesptr = &nodes[0]; const Plane *planesptr = &planes[0]; - int plane_count = planes.size(); int node_count = nodes.size(); if (node_count == 0) // no nodes! @@ -192,10 +189,10 @@ int BSP_Tree::get_points_inside(const Vector3 *p_points, int p_point_count) cons break; } - uint16_t plane = nodesptr[idx].plane; #ifdef DEBUG_ENABLED - - ERR_FAIL_INDEX_V(plane, plane_count, false); + int plane_count = planes.size(); + uint16_t plane = nodesptr[idx].plane; + ERR_FAIL_UNSIGNED_INDEX_V(plane, plane_count, false); #endif idx = planesptr[nodesptr[idx].plane].is_point_over(point) ? nodes[idx].over : nodes[idx].under; @@ -261,7 +258,7 @@ bool BSP_Tree::point_is_inside(const Vector3 &p_point) const { #ifdef DEBUG_ENABLED int plane_count = planes.size(); uint16_t plane = nodesptr[idx].plane; - ERR_FAIL_INDEX_V(plane, plane_count, false); + ERR_FAIL_UNSIGNED_INDEX_V(plane, plane_count, false); #endif bool over = planesptr[nodesptr[idx].plane].is_point_over(p_point); @@ -272,8 +269,6 @@ bool BSP_Tree::point_is_inside(const Vector3 &p_point) const { ERR_FAIL_COND_V(idx < MAX_NODES && idx >= node_count, false); #endif } - - return false; } static int _bsp_find_best_half_plane(const Face3 *p_faces, const Vector<int> &p_indices, real_t p_tolerance) { diff --git a/core/math/bsp_tree.h b/core/math/bsp_tree.h index b06e6b8539..90b5e8322a 100644 --- a/core/math/bsp_tree.h +++ b/core/math/bsp_tree.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,16 +31,14 @@ #ifndef BSP_TREE_H #define BSP_TREE_H -#include "core/dvector.h" #include "core/math/aabb.h" #include "core/math/face3.h" #include "core/math/plane.h" #include "core/method_ptrcall.h" +#include "core/pool_vector.h" #include "core/variant.h" #include "core/vector.h" -/** - @author Juan Linietsky <reduzio@gmail.com> -*/ + class BSP_Tree { public: enum { diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp index 3a082d5720..30c0cab909 100644 --- a/core/math/camera_matrix.cpp +++ b/core/math/camera_matrix.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -210,6 +210,14 @@ void CameraMatrix::set_frustum(real_t p_left, real_t p_right, real_t p_bottom, r te[15] = 0; } +void CameraMatrix::set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov) { + if (!p_flip_fov) { + p_size *= p_aspect; + } + + set_frustum(-p_size / 2 + p_offset.x, +p_size / 2 + p_offset.x, -p_size / p_aspect / 2 + p_offset.y, +p_size / p_aspect / 2 + p_offset.y, p_near, p_far); +} + real_t CameraMatrix::get_z_far() const { const real_t *matrix = (const real_t *)this->matrix; @@ -294,8 +302,8 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform &p_transform) /** Fast Plane Extraction from combined modelview/projection matrices. * References: - * http://www.markmorley.com/opengl/frustumculling.html - * http://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf + * https://web.archive.org/web/20011221205252/http://www.markmorley.com/opengl/frustumculling.html + * https://web.archive.org/web/20061020020112/http://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf */ Vector<Plane> planes; @@ -499,21 +507,21 @@ void CameraMatrix::set_light_bias() { real_t *m = &matrix[0][0]; - m[0] = 0.5, - m[1] = 0.0, - m[2] = 0.0, - m[3] = 0.0, - m[4] = 0.0, - m[5] = 0.5, - m[6] = 0.0, - m[7] = 0.0, - m[8] = 0.0, - m[9] = 0.0, - m[10] = 0.5, - m[11] = 0.0, - m[12] = 0.5, - m[13] = 0.5, - m[14] = 0.5, + m[0] = 0.5; + m[1] = 0.0; + m[2] = 0.0; + m[3] = 0.0; + m[4] = 0.0; + m[5] = 0.5; + m[6] = 0.0; + m[7] = 0.0; + m[8] = 0.0; + m[9] = 0.0; + m[10] = 0.5; + m[11] = 0.0; + m[12] = 0.5; + m[13] = 0.5; + m[14] = 0.5; m[15] = 1.0; } @@ -521,21 +529,21 @@ void CameraMatrix::set_light_atlas_rect(const Rect2 &p_rect) { real_t *m = &matrix[0][0]; - m[0] = p_rect.size.width, - m[1] = 0.0, - m[2] = 0.0, - m[3] = 0.0, - m[4] = 0.0, - m[5] = p_rect.size.height, - m[6] = 0.0, - m[7] = 0.0, - m[8] = 0.0, - m[9] = 0.0, - m[10] = 1.0, - m[11] = 0.0, - m[12] = p_rect.position.x, - m[13] = p_rect.position.y, - m[14] = 0.0, + m[0] = p_rect.size.width; + m[1] = 0.0; + m[2] = 0.0; + m[3] = 0.0; + m[4] = 0.0; + m[5] = p_rect.size.height; + m[6] = 0.0; + m[7] = 0.0; + m[8] = 0.0; + m[9] = 0.0; + m[10] = 1.0; + m[11] = 0.0; + m[12] = p_rect.position.x; + m[13] = p_rect.position.y; + m[14] = 0.0; m[15] = 1.0; } diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h index bd20908ad9..63cc88553d 100644 --- a/core/math/camera_matrix.h +++ b/core/math/camera_matrix.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -34,10 +34,6 @@ #include "core/math/rect2.h" #include "core/math/transform.h" -/** - @author Juan Linietsky <reduzio@gmail.com> -*/ - struct CameraMatrix { enum Planes { @@ -61,6 +57,7 @@ struct CameraMatrix { void set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar); void set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov = false); void set_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far); + void set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov = false); static real_t get_fovy(real_t p_fovx, real_t p_aspect) { diff --git a/core/math/delaunay.h b/core/math/delaunay.h index 9c5eef9069..3f8013a3e6 100644 --- a/core/math/delaunay.h +++ b/core/math/delaunay.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -80,11 +80,11 @@ public: } static bool edge_compare(const Vector<Vector2> &p_vertices, const Edge &p_a, const Edge &p_b) { - if (p_vertices[p_a.edge[0]].distance_to(p_vertices[p_b.edge[0]]) < CMP_EPSILON && p_vertices[p_a.edge[1]].distance_to(p_vertices[p_b.edge[1]]) < CMP_EPSILON) { + if (p_vertices[p_a.edge[0]] == p_vertices[p_b.edge[0]] && p_vertices[p_a.edge[1]] == p_vertices[p_b.edge[1]]) { return true; } - if (p_vertices[p_a.edge[0]].distance_to(p_vertices[p_b.edge[1]]) < CMP_EPSILON && p_vertices[p_a.edge[1]].distance_to(p_vertices[p_b.edge[0]]) < CMP_EPSILON) { + if (p_vertices[p_a.edge[0]] == p_vertices[p_b.edge[1]] && p_vertices[p_a.edge[1]] == p_vertices[p_b.edge[0]]) { return true; } diff --git a/core/math/disjoint_set.cpp b/core/math/disjoint_set.cpp new file mode 100644 index 0000000000..c9d47aa0ae --- /dev/null +++ b/core/math/disjoint_set.cpp @@ -0,0 +1,31 @@ +/*************************************************************************/ +/* disjoint_set.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "disjoint_set.h" diff --git a/core/math/disjoint_set.h b/core/math/disjoint_set.h new file mode 100644 index 0000000000..c9b3d0b65d --- /dev/null +++ b/core/math/disjoint_set.h @@ -0,0 +1,155 @@ +/*************************************************************************/ +/* disjoint_set.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef DISJOINT_SET_H +#define DISJOINT_SET_H + +#include "core/map.h" +#include "core/vector.h" + +/** + @author Marios Staikopoulos <marios@staik.net> +*/ + +/* This DisjointSet class uses Find with path compression and Union by rank */ +template <typename T, class C = Comparator<T>, class AL = DefaultAllocator> +class DisjointSet { + + struct Element { + T object; + Element *parent = nullptr; + int rank = 0; + }; + + typedef Map<T, Element *, C, AL> MapT; + + MapT elements; + + Element *get_parent(Element *element); + + _FORCE_INLINE_ Element *insert_or_get(T object); + +public: + ~DisjointSet(); + + _FORCE_INLINE_ void insert(T object) { (void)insert_or_get(object); } + + void create_union(T a, T b); + + void get_representatives(Vector<T> &out_roots); + + void get_members(Vector<T> &out_members, T representative); +}; + +/* FUNCTIONS */ + +template <typename T, class C, class AL> +DisjointSet<T, C, AL>::~DisjointSet() { + for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) { + memdelete_allocator<Element, AL>(itr->value()); + } +} + +template <typename T, class C, class AL> +typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::get_parent(Element *element) { + if (element->parent != element) { + element->parent = get_parent(element->parent); + } + + return element->parent; +} + +template <typename T, class C, class AL> +typename DisjointSet<T, C, AL>::Element *DisjointSet<T, C, AL>::insert_or_get(T object) { + typename MapT::Element *itr = elements.find(object); + if (itr != nullptr) { + return itr->value(); + } + + Element *new_element = memnew_allocator(Element, AL); + new_element->object = object; + new_element->parent = new_element; + elements.insert(object, new_element); + + return new_element; +} + +template <typename T, class C, class AL> +void DisjointSet<T, C, AL>::create_union(T a, T b) { + + Element *x = insert_or_get(a); + Element *y = insert_or_get(b); + + Element *x_root = get_parent(x); + Element *y_root = get_parent(y); + + // Already in the same set + if (x_root == y_root) + return; + + // Not in the same set, merge + if (x_root->rank < y_root->rank) { + SWAP(x_root, y_root); + } + + // Merge y_root into x_root + y_root->parent = x_root; + if (x_root->rank == y_root->rank) { + ++x_root->rank; + } +} + +template <typename T, class C, class AL> +void DisjointSet<T, C, AL>::get_representatives(Vector<T> &out_representatives) { + for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) { + Element *element = itr->value(); + if (element->parent == element) { + out_representatives.push_back(element->object); + } + } +} + +template <typename T, class C, class AL> +void DisjointSet<T, C, AL>::get_members(Vector<T> &out_members, T representative) { + typename MapT::Element *rep_itr = elements.find(representative); + ERR_FAIL_COND(rep_itr == nullptr); + + Element *rep_element = rep_itr->value(); + ERR_FAIL_COND(rep_element->parent != rep_element); + + for (typename MapT::Element *itr = elements.front(); itr != nullptr; itr = itr->next()) { + Element *parent = get_parent(itr->value()); + if (parent == rep_element) { + out_members.push_back(itr->key()); + } + } +} + +#endif diff --git a/core/math/expression.cpp b/core/math/expression.cpp index c0d7f874d2..46f81ce5c3 100644 --- a/core/math/expression.cpp +++ b/core/math/expression.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -52,6 +52,7 @@ const char *Expression::func_name[Expression::FUNC_MAX] = { "sqrt", "fmod", "fposmod", + "posmod", "floor", "ceil", "round", @@ -64,10 +65,14 @@ const char *Expression::func_name[Expression::FUNC_MAX] = { "is_inf", "ease", "decimals", + "step_decimals", "stepify", "lerp", + "lerp_angle", "inverse_lerp", "range_lerp", + "smoothstep", + "move_toward", "dectime", "randomize", "randi", @@ -148,6 +153,7 @@ int Expression::get_func_argument_count(BuiltinFunc p_func) { case MATH_ISNAN: case MATH_ISINF: case MATH_DECIMALS: + case MATH_STEP_DECIMALS: case MATH_SEED: case MATH_RANDSEED: case MATH_DEG2RAD: @@ -164,13 +170,14 @@ int Expression::get_func_argument_count(BuiltinFunc p_func) { case TEXT_PRINTRAW: case VAR_TO_STR: case STR_TO_VAR: - case VAR_TO_BYTES: - case BYTES_TO_VAR: case TYPE_EXISTS: return 1; + case VAR_TO_BYTES: + case BYTES_TO_VAR: case MATH_ATAN2: case MATH_FMOD: case MATH_FPOSMOD: + case MATH_POSMOD: case MATH_POW: case MATH_EASE: case MATH_STEPIFY: @@ -184,7 +191,10 @@ int Expression::get_func_argument_count(BuiltinFunc p_func) { case COLORN: return 2; case MATH_LERP: + case MATH_LERP_ANGLE: case MATH_INVERSE_LERP: + case MATH_SMOOTHSTEP: + case MATH_MOVE_TOWARD: case MATH_DECTIME: case MATH_WRAP: case MATH_WRAPF: @@ -277,6 +287,12 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant VALIDATE_ARG_NUM(1); *r_return = Math::fposmod((double)*p_inputs[0], (double)*p_inputs[1]); } break; + case MATH_POSMOD: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + *r_return = Math::posmod((int)*p_inputs[0], (int)*p_inputs[1]); + } break; case MATH_FLOOR: { VALIDATE_ARG_NUM(0); @@ -363,6 +379,11 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant VALIDATE_ARG_NUM(0); *r_return = Math::step_decimals((double)*p_inputs[0]); } break; + case MATH_STEP_DECIMALS: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::step_decimals((double)*p_inputs[0]); + } break; case MATH_STEPIFY: { VALIDATE_ARG_NUM(0); @@ -376,6 +397,13 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant VALIDATE_ARG_NUM(2); *r_return = Math::lerp((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); } break; + case MATH_LERP_ANGLE: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + *r_return = Math::lerp_angle((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); + } break; case MATH_INVERSE_LERP: { VALIDATE_ARG_NUM(0); @@ -392,6 +420,19 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant VALIDATE_ARG_NUM(4); *r_return = Math::range_lerp((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2], (double)*p_inputs[3], (double)*p_inputs[4]); } break; + case MATH_SMOOTHSTEP: { + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + *r_return = Math::smoothstep((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); + } break; + case MATH_MOVE_TOWARD: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + *r_return = Math::move_toward((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); + } break; case MATH_DECTIME: { VALIDATE_ARG_NUM(0); @@ -696,8 +737,9 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant case VAR_TO_BYTES: { PoolByteArray barr; + bool full_objects = *p_inputs[1]; int len; - Error err = encode_variant(*p_inputs[0], NULL, len); + Error err = encode_variant(*p_inputs[0], NULL, len, full_objects); if (err) { r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; r_error.argument = 0; @@ -709,7 +751,7 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant barr.resize(len); { PoolByteArray::Write w = barr.write(); - encode_variant(*p_inputs[0], w.ptr(), len); + encode_variant(*p_inputs[0], w.ptr(), len, full_objects); } *r_return = barr; } break; @@ -724,10 +766,11 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant } PoolByteArray varr = *p_inputs[0]; + bool allow_objects = *p_inputs[1]; Variant ret; { PoolByteArray::Read r = varr.read(); - Error err = decode_variant(ret, r.ptr(), varr.size(), NULL); + Error err = decode_variant(ret, r.ptr(), varr.size(), NULL, allow_objects); if (err != OK) { r_error_str = RTR("Not enough bytes for decoding bytes, or invalid format."); r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; @@ -750,29 +793,30 @@ void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return = String(color); } break; - default: {} + default: { + } } } //////// +static bool _is_number(CharType c) { + return (c >= '0' && c <= '9'); +} + Error Expression::_get_token(Token &r_token) { while (true) { #define GET_CHAR() (str_ofs >= expression.length() ? 0 : expression[str_ofs++]) CharType cchar = GET_CHAR(); - if (cchar == 0) { - r_token.type = TK_EOF; - return OK; - } switch (cchar) { case 0: { r_token.type = TK_EOF; return OK; - } break; + }; case '{': { r_token.type = TK_CURLY_BRACKET_OPEN; @@ -813,17 +857,12 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_COLON; return OK; }; - case '.': { - - r_token.type = TK_PERIOD; - return OK; - }; case '$': { r_token.type = TK_INPUT; int index = 0; do { - if (expression[str_ofs] < '0' || expression[str_ofs] > '9') { + if (!_is_number(expression[str_ofs])) { _set_error("Expected number after '$'"); r_token.type = TK_ERROR; return ERR_PARSE_ERROR; @@ -832,7 +871,7 @@ Error Expression::_get_token(Token &r_token) { index += expression[str_ofs] - '0'; str_ofs++; - } while (expression[str_ofs] >= '0' && expression[str_ofs] <= '9'); + } while (_is_number(expression[str_ofs])); r_token.value = index; return OK; @@ -979,14 +1018,14 @@ Error Expression::_get_token(Token &r_token) { r_token.type = TK_ERROR; return ERR_PARSE_ERROR; } - if (!((c >= '0' && c <= '9') || (c >= 'a' && c <= 'f') || (c >= 'A' && c <= 'F'))) { + if (!(_is_number(c) || (c >= 'a' && c <= 'f') || (c >= 'A' && c <= 'F'))) { _set_error("Malformed hex constant in string"); r_token.type = TK_ERROR; return ERR_PARSE_ERROR; } CharType v; - if (c >= '0' && c <= '9') { + if (_is_number(c)) { v = c - '0'; } else if (c >= 'a' && c <= 'f') { v = c - 'a'; @@ -1032,7 +1071,8 @@ Error Expression::_get_token(Token &r_token) { break; } - if (cchar >= '0' && cchar <= '9') { + CharType next_char = (str_ofs >= expression.length()) ? 0 : expression[str_ofs]; + if (_is_number(cchar) || (cchar == '.' && _is_number(next_char))) { //a number String num; @@ -1053,7 +1093,7 @@ Error Expression::_get_token(Token &r_token) { switch (reading) { case READING_INT: { - if (c >= '0' && c <= '9') { + if (_is_number(c)) { //pass } else if (c == '.') { reading = READING_DEC; @@ -1067,7 +1107,7 @@ Error Expression::_get_token(Token &r_token) { } break; case READING_DEC: { - if (c >= '0' && c <= '9') { + if (_is_number(c)) { } else if (c == 'e') { reading = READING_EXP; @@ -1079,7 +1119,7 @@ Error Expression::_get_token(Token &r_token) { } break; case READING_EXP: { - if (c >= '0' && c <= '9') { + if (_is_number(c)) { exp_beg = true; } else if ((c == '-' || c == '+') && !exp_sign && !exp_beg) { @@ -1114,7 +1154,7 @@ Error Expression::_get_token(Token &r_token) { String id; bool first = true; - while ((cchar >= 'A' && cchar <= 'Z') || (cchar >= 'a' && cchar <= 'z') || cchar == '_' || (!first && cchar >= '0' && cchar <= '9')) { + while ((cchar >= 'A' && cchar <= 'Z') || (cchar >= 'a' && cchar <= 'z') || cchar == '_' || (!first && _is_number(cchar))) { id += String::chr(cchar); cchar = GET_CHAR(); @@ -1176,6 +1216,12 @@ Error Expression::_get_token(Token &r_token) { } return OK; + + } else if (cchar == '.') { + // Handled down there as we support '.[0-9]' as numbers above + r_token.type = TK_PERIOD; + return OK; + } else { _set_error("Unexpected character."); r_token.type = TK_ERROR; @@ -1183,6 +1229,7 @@ Error Expression::_get_token(Token &r_token) { } } } +#undef GET_CHAR } r_token.type = TK_ERROR; @@ -1257,10 +1304,10 @@ Expression::ENode *Expression::_parse_expression() { } str_ofs = cofs; //revert //parse an expression - ENode *expr = _parse_expression(); - if (!expr) + ENode *subexpr = _parse_expression(); + if (!subexpr) return NULL; - dn->dict.push_back(expr); + dn->dict.push_back(subexpr); _get_token(tk); if (tk.type != TK_COLON) { @@ -1268,11 +1315,11 @@ Expression::ENode *Expression::_parse_expression() { return NULL; } - expr = _parse_expression(); - if (!expr) + subexpr = _parse_expression(); + if (!subexpr) return NULL; - dn->dict.push_back(expr); + dn->dict.push_back(subexpr); cofs = str_ofs; _get_token(tk); @@ -1301,10 +1348,10 @@ Expression::ENode *Expression::_parse_expression() { } str_ofs = cofs; //revert //parse an expression - ENode *expr = _parse_expression(); - if (!expr) + ENode *subexpr = _parse_expression(); + if (!subexpr) return NULL; - an->array.push_back(expr); + an->array.push_back(subexpr); cofs = str_ofs; _get_token(tk); @@ -1348,25 +1395,25 @@ Expression::ENode *Expression::_parse_expression() { while (true) { - int cofs = str_ofs; + int cofs2 = str_ofs; _get_token(tk); if (tk.type == TK_PARENTHESIS_CLOSE) { break; } - str_ofs = cofs; //revert + str_ofs = cofs2; //revert //parse an expression - ENode *expr = _parse_expression(); - if (!expr) + ENode *subexpr = _parse_expression(); + if (!subexpr) return NULL; - func_call->arguments.push_back(expr); + func_call->arguments.push_back(subexpr); - cofs = str_ofs; + cofs2 = str_ofs; _get_token(tk); if (tk.type == TK_COMMA) { //all good } else if (tk.type == TK_PARENTHESIS_CLOSE) { - str_ofs = cofs; + str_ofs = cofs2; } else { _set_error("Expected ',' or ')'"); } @@ -1437,11 +1484,11 @@ Expression::ENode *Expression::_parse_expression() { } str_ofs = cofs; //revert //parse an expression - ENode *expr = _parse_expression(); - if (!expr) + ENode *subexpr = _parse_expression(); + if (!subexpr) return NULL; - constructor->arguments.push_back(expr); + constructor->arguments.push_back(subexpr); cofs = str_ofs; _get_token(tk); @@ -1478,11 +1525,11 @@ Expression::ENode *Expression::_parse_expression() { } str_ofs = cofs; //revert //parse an expression - ENode *expr = _parse_expression(); - if (!expr) + ENode *subexpr = _parse_expression(); + if (!subexpr) return NULL; - bifunc->arguments.push_back(expr); + bifunc->arguments.push_back(subexpr); cofs = str_ofs; _get_token(tk); @@ -1577,25 +1624,25 @@ Expression::ENode *Expression::_parse_expression() { while (true) { - int cofs = str_ofs; + int cofs3 = str_ofs; _get_token(tk); if (tk.type == TK_PARENTHESIS_CLOSE) { break; } - str_ofs = cofs; //revert + str_ofs = cofs3; //revert //parse an expression - ENode *expr = _parse_expression(); - if (!expr) + ENode *subexpr = _parse_expression(); + if (!subexpr) return NULL; - func_call->arguments.push_back(expr); + func_call->arguments.push_back(subexpr); - cofs = str_ofs; + cofs3 = str_ofs; _get_token(tk); if (tk.type == TK_COMMA) { //all good } else if (tk.type == TK_PARENTHESIS_CLOSE) { - str_ofs = cofs; + str_ofs = cofs3; } else { _set_error("Expected ',' or ')'"); } @@ -1662,7 +1709,8 @@ Expression::ENode *Expression::_parse_expression() { case TK_OP_BIT_OR: op = Variant::OP_BIT_OR; break; case TK_OP_BIT_XOR: op = Variant::OP_BIT_XOR; break; case TK_OP_BIT_INVERT: op = Variant::OP_BIT_NEGATE; break; - default: {}; + default: { + }; } if (op == Variant::OP_MAX) { //stop appending stuff @@ -1759,7 +1807,7 @@ Expression::ENode *Expression::_parse_expression() { if (next_op == -1) { _set_error("Yet another parser bug...."); - ERR_FAIL_COND_V(next_op == -1, NULL); + ERR_FAIL_V(NULL); } // OK! create operator.. @@ -1895,7 +1943,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: Variant b; if (op->nodes[1]) { - bool ret = _execute(p_inputs, p_instance, op->nodes[1], b, r_error_str); + ret = _execute(p_inputs, p_instance, op->nodes[1], b, r_error_str); if (ret) return true; } @@ -2063,7 +2111,7 @@ bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression: for (int i = 0; i < call->arguments.size(); i++) { Variant value; - bool ret = _execute(p_inputs, p_instance, call->arguments[i], value, r_error_str); + ret = _execute(p_inputs, p_instance, call->arguments[i], value, r_error_str); if (ret) return true; @@ -2114,6 +2162,8 @@ Error Expression::parse(const String &p_expression, const Vector<String> &p_inpu Variant Expression::execute(Array p_inputs, Object *p_base, bool p_show_error) { + ERR_FAIL_COND_V_MSG(error_set, Variant(), "There was previously a parse error: " + error_str + "."); + execution_error = false; Variant output; String error_txt; @@ -2121,10 +2171,7 @@ Variant Expression::execute(Array p_inputs, Object *p_base, bool p_show_error) { if (err) { execution_error = true; error_str = error_txt; - if (p_show_error) { - ERR_EXPLAIN(error_str); - ERR_FAIL_V(Variant()); - } + ERR_FAIL_COND_V_MSG(p_show_error, Variant(), error_str); } return output; @@ -2146,13 +2193,13 @@ void Expression::_bind_methods() { ClassDB::bind_method(D_METHOD("get_error_text"), &Expression::get_error_text); } -Expression::Expression() { - output_type = Variant::NIL; - error_set = true; - root = NULL; - nodes = NULL; - sequenced = false; - execution_error = false; +Expression::Expression() : + output_type(Variant::NIL), + sequenced(false), + error_set(true), + root(NULL), + nodes(NULL), + execution_error(false) { } Expression::~Expression() { diff --git a/core/math/expression.h b/core/math/expression.h index ac2416d0dd..833220592c 100644 --- a/core/math/expression.h +++ b/core/math/expression.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -34,7 +34,8 @@ #include "core/reference.h" class Expression : public Reference { - GDCLASS(Expression, Reference) + GDCLASS(Expression, Reference); + public: enum BuiltinFunc { MATH_SIN, @@ -50,6 +51,7 @@ public: MATH_SQRT, MATH_FMOD, MATH_FPOSMOD, + MATH_POSMOD, MATH_FLOOR, MATH_CEIL, MATH_ROUND, @@ -62,10 +64,14 @@ public: MATH_ISINF, MATH_EASE, MATH_DECIMALS, + MATH_STEP_DECIMALS, MATH_STEPIFY, MATH_LERP, + MATH_LERP_ANGLE, MATH_INVERSE_LERP, MATH_RANGE_LERP, + MATH_SMOOTHSTEP, + MATH_MOVE_TOWARD, MATH_DECTIME, MATH_RANDOMIZE, MATH_RAND, @@ -116,7 +122,9 @@ private: Variant::Type type; String name; - Input() { type = Variant::NIL; } + Input() : + type(Variant::NIL) { + } }; Vector<Input> inputs; diff --git a/core/math/face3.cpp b/core/math/face3.cpp index aa46fde7f7..ab09142b2d 100644 --- a/core/math/face3.cpp +++ b/core/math/face3.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -202,11 +202,12 @@ bool Face3::intersects_aabb(const AABB &p_aabb) const { { \ real_t aabb_min = p_aabb.position.m_ax; \ real_t aabb_max = p_aabb.position.m_ax + p_aabb.size.m_ax; \ - real_t tri_min, tri_max; \ - for (int i = 0; i < 3; i++) { \ - if (i == 0 || vertex[i].m_ax > tri_max) \ + real_t tri_min = vertex[0].m_ax; \ + real_t tri_max = vertex[0].m_ax; \ + for (int i = 1; i < 3; i++) { \ + if (vertex[i].m_ax > tri_max) \ tri_max = vertex[i].m_ax; \ - if (i == 0 || vertex[i].m_ax < tri_min) \ + if (vertex[i].m_ax < tri_min) \ tri_min = vertex[i].m_ax; \ } \ \ diff --git a/core/math/face3.h b/core/math/face3.h index b41daf04d4..184e80ff77 100644 --- a/core/math/face3.h +++ b/core/math/face3.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -241,13 +241,13 @@ bool Face3::intersects_aabb2(const AABB &p_aabb) const { real_t minT = 1e20, maxT = -1e20; for (int k = 0; k < 3; k++) { - real_t d = axis.dot(vertex[k]); + real_t vert_d = axis.dot(vertex[k]); - if (d > maxT) - maxT = d; + if (vert_d > maxT) + maxT = vert_d; - if (d < minT) - minT = d; + if (vert_d < minT) + minT = vert_d; } if (maxB < minT || maxT < minB) diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp index be5e40e4e6..e0ead8446f 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,7 +31,13 @@ #include "geometry.h" #include "core/print_string.h" +#include "thirdparty/misc/clipper.hpp" +#include "thirdparty/misc/triangulator.h" +#define SCALE_FACTOR 100000.0 // Based on CMP_EPSILON. + +// This implementation is very inefficient, commenting unless bugs happen. See the other one. +/* bool Geometry::is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon) { Vector<int> indices = Geometry::triangulate_polygon(p_polygon); @@ -42,6 +48,7 @@ bool Geometry::is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> } return false; } +*/ void Geometry::MeshData::optimize_vertices() { @@ -118,8 +125,8 @@ struct _FaceClassify { }; static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { - /* connect faces, error will occur if an edge is shared between more than 2 faces */ - /* clear connections */ + // Connect faces, error will occur if an edge is shared between more than 2 faces. + // Clear connections. bool error = false; @@ -189,13 +196,6 @@ static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { if (p_faces[i].links[j].face == -1) p_faces[i].valid = false; } - /*printf("face %i is valid: %i, group %i. connected to %i:%i,%i:%i,%i:%i\n",i,p_faces[i].valid,p_faces[i].group, - p_faces[i].links[0].face, - p_faces[i].links[0].edge, - p_faces[i].links[1].face, - p_faces[i].links[1].edge, - p_faces[i].links[2].face, - p_faces[i].links[2].edge);*/ } return error; } @@ -241,12 +241,9 @@ PoolVector<PoolVector<Face3> > Geometry::separate_objects(PoolVector<Face3> p_ar bool error = _connect_faces(_fcptr, len, -1); - if (error) { - - ERR_FAIL_COND_V(error, PoolVector<PoolVector<Face3> >()); // invalid geometry - } + ERR_FAIL_COND_V_MSG(error, PoolVector<PoolVector<Face3> >(), "Invalid geometry."); - /* group connected faces in separate objects */ + // Group connected faces in separate objects. int group = 0; for (int i = 0; i < len; i++) { @@ -258,7 +255,7 @@ PoolVector<PoolVector<Face3> > Geometry::separate_objects(PoolVector<Face3> p_ar } } - /* group connected faces in separate objects */ + // Group connected faces in separate objects. for (int i = 0; i < len; i++) { @@ -370,7 +367,7 @@ static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z) { if (p_cell_status[x][y][z] & 3) - return; // nothing to do, already used and/or visited + return; // Nothing to do, already used and/or visited. p_cell_status[x][y][z] = _CELL_PREV_FIRST; @@ -378,29 +375,20 @@ static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, uint8_t &c = p_cell_status[x][y][z]; - //printf("at %i,%i,%i\n",x,y,z); - if ((c & _CELL_STEP_MASK) == _CELL_STEP_NONE) { - /* Haven't been in here, mark as outside */ + // Haven't been in here, mark as outside. p_cell_status[x][y][z] |= _CELL_EXTERIOR; - //printf("not marked as anything, marking exterior\n"); } - //printf("cell step is %i\n",(c&_CELL_STEP_MASK)); - if ((c & _CELL_STEP_MASK) != _CELL_STEP_DONE) { - /* if not done, increase step */ + // If not done, increase step. c += 1 << 2; - //printf("incrementing cell step\n"); } if ((c & _CELL_STEP_MASK) == _CELL_STEP_DONE) { - /* Go back */ - //printf("done, going back a cell\n"); - + // Go back. switch (c & _CELL_PREV_MASK) { case _CELL_PREV_FIRST: { - //printf("at end, finished marking\n"); return; } break; case _CELL_PREV_Y_POS: { @@ -434,8 +422,6 @@ static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, continue; } - //printf("attempting new cell!\n"); - int next_x = x, next_y = y, next_z = z; uint8_t prev = 0; @@ -469,8 +455,6 @@ static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, default: ERR_FAIL(); } - //printf("testing if new cell will be ok...!\n"); - if (next_x < 0 || next_x >= len_x) continue; if (next_y < 0 || next_y >= len_y) @@ -478,13 +462,9 @@ static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, if (next_z < 0 || next_z >= len_z) continue; - //printf("testing if new cell is traversable\n"); - if (p_cell_status[next_x][next_y][next_z] & 3) continue; - //printf("move to it\n"); - x = next_x; y = next_y; z = next_z; @@ -501,18 +481,7 @@ static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, i if (p_cell_status[x][y][z] & _CELL_EXTERIOR) return; -/* static const Vector3 vertices[8]={ - Vector3(0,0,0), - Vector3(0,0,1), - Vector3(0,1,0), - Vector3(0,1,1), - Vector3(1,0,0), - Vector3(1,0,1), - Vector3(1,1,0), - Vector3(1,1,1), - }; -*/ -#define vert(m_idx) Vector3((m_idx & 4) >> 2, (m_idx & 2) >> 1, m_idx & 1) +#define vert(m_idx) Vector3(((m_idx)&4) >> 2, ((m_idx)&2) >> 1, (m_idx)&1) static const uint8_t indices[6][4] = { { 7, 6, 4, 5 }, @@ -523,22 +492,6 @@ static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, i { 0, 4, 6, 2 }, }; - /* - - {0,1,2,3}, - {0,1,4,5}, - {0,2,4,6}, - {4,5,6,7}, - {2,3,7,6}, - {1,3,5,7}, - - {0,2,3,1}, - {0,1,5,4}, - {0,4,6,2}, - {7,6,4,5}, - {7,3,2,6}, - {7,5,1,3}, -*/ for (int i = 0; i < 6; i++) { @@ -601,9 +554,9 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e } } - global_aabb.grow_by(0.01); // avoid numerical error + global_aabb.grow_by(0.01); // Avoid numerical error. - // determine amount of cells in grid axis + // Determine amount of cells in grid axis. int div_x, div_y, div_z; if (global_aabb.size.x / _MIN_SIZE < _MAX_LENGTH) @@ -626,7 +579,7 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e voxelsize.y /= div_y; voxelsize.z /= div_z; - // create and initialize cells to zero + // Create and initialize cells to zero. uint8_t ***cell_status = memnew_arr(uint8_t **, div_x); for (int i = 0; i < div_x; i++) { @@ -644,7 +597,7 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e } } - // plot faces into cells + // Plot faces into cells. for (int i = 0; i < face_count; i++) { @@ -656,7 +609,7 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e _plot_face(cell_status, 0, 0, 0, div_x, div_y, div_z, voxelsize, f); } - // determine which cells connect to the outside by traversing the outside and recursively flood-fill marking + // Determine which cells connect to the outside by traversing the outside and recursively flood-fill marking. for (int i = 0; i < div_x; i++) { @@ -685,7 +638,7 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e } } - // build faces for the inside-outside cell divisors + // Build faces for the inside-outside cell divisors. PoolVector<Face3> wrapped_faces; @@ -700,7 +653,7 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e } } - // transform face vertices to global coords + // Transform face vertices to global coords. int wrapped_faces_count = wrapped_faces.size(); PoolVector<Face3>::Write wrapped_facesw = wrapped_faces.write(); @@ -735,13 +688,47 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e return wrapped_faces; } +Vector<Vector<Vector2> > Geometry::decompose_polygon_in_convex(Vector<Point2> polygon) { + Vector<Vector<Vector2> > decomp; + List<TriangulatorPoly> in_poly, out_poly; + + TriangulatorPoly inp; + inp.Init(polygon.size()); + for (int i = 0; i < polygon.size(); i++) { + inp.GetPoint(i) = polygon[i]; + } + inp.SetOrientation(TRIANGULATOR_CCW); + in_poly.push_back(inp); + TriangulatorPartition tpart; + if (tpart.ConvexPartition_HM(&in_poly, &out_poly) == 0) { // Failed. + ERR_PRINT("Convex decomposing failed!"); + return decomp; + } + + decomp.resize(out_poly.size()); + int idx = 0; + for (List<TriangulatorPoly>::Element *I = out_poly.front(); I; I = I->next()) { + TriangulatorPoly &tp = I->get(); + + decomp.write[idx].resize(tp.GetNumPoints()); + + for (int64_t i = 0; i < tp.GetNumPoints(); i++) { + decomp.write[idx].write[i] = tp.GetPoint(i); + } + + idx++; + } + + return decomp; +} + Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes) { MeshData mesh; #define SUBPLANE_SIZE 1024.0 - real_t subplane_size = 1024.0; // should compute this from the actual plane + real_t subplane_size = 1024.0; // Should compute this from the actual plane. for (int i = 0; i < p_planes.size(); i++) { Plane p = p_planes[i]; @@ -749,7 +736,7 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes Vector3 ref = Vector3(0.0, 1.0, 0.0); if (ABS(p.normal.dot(ref)) > 0.95) - ref = Vector3(0.0, 0.0, 1.0); // change axis + ref = Vector3(0.0, 0.0, 1.0); // Change axis. Vector3 right = p.normal.cross(ref).normalized(); Vector3 up = p.normal.cross(right).normalized(); @@ -787,20 +774,20 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes real_t dist0 = clip.distance_to(edge0_A); real_t dist1 = clip.distance_to(edge1_A); - if (dist0 <= 0) { // behind plane + if (dist0 <= 0) { // Behind plane. new_vertices.push_back(vertices[k]); } - // check for different sides and non coplanar + // Check for different sides and non coplanar. if ((dist0 * dist1) < 0) { - // calculate intersection + // Calculate intersection. Vector3 rel = edge1_A - edge0_A; real_t den = clip.normal.dot(rel); - if (Math::abs(den) < CMP_EPSILON) - continue; // point too short + if (Math::is_zero_approx(den)) + continue; // Point too short. real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; Vector3 inters = edge0_A + rel * dist; @@ -814,11 +801,11 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes if (vertices.size() < 3) continue; - //result is a clockwise face + // Result is a clockwise face. MeshData::Face face; - // add face indices + // Add face indices. for (int j = 0; j < vertices.size(); j++) { int idx = -1; @@ -842,7 +829,7 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes face.plane = p; mesh.faces.push_back(face); - //add edge + // Add edge. for (int j = 0; j < face.indices.size(); j++) { @@ -932,7 +919,7 @@ PoolVector<Plane> Geometry::build_sphere_planes(real_t p_radius, int p_lats, int for (int j = 1; j <= p_lats; j++) { - //todo this is stupid, fix + // FIXME: This is stupid. Vector3 angle = normal.linear_interpolate(axis, j / (real_t)p_lats).normalized(); Vector3 pos = angle * p_radius; planes.push_back(Plane(pos, angle)); @@ -992,12 +979,12 @@ struct _AtlasWorkRectResult { void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size) { - //super simple, almost brute force scanline stacking fitter - //it's pretty basic for now, but it tries to make sure that the aspect ratio of the - //resulting atlas is somehow square. This is necessary because video cards have limits - //on texture size (usually 2048 or 4096), so the more square a texture, the more chances - //it will work in every hardware. - // for example, it will prioritize a 1024x1024 atlas (works everywhere) instead of a + // Super simple, almost brute force scanline stacking fitter. + // It's pretty basic for now, but it tries to make sure that the aspect ratio of the + // resulting atlas is somehow square. This is necessary because video cards have limits. + // On texture size (usually 2048 or 4096), so the more square a texture, the more chances. + // It will work in every hardware. + // For example, it will prioritize a 1024x1024 atlas (works everywhere) instead of a // 256x8192 atlas (won't work anywhere). ERR_FAIL_COND(p_rects.size() == 0); @@ -1026,7 +1013,7 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu for (int j = 0; j < w; j++) hmax.write[j] = 0; - //place them + // Place them. int ofs = 0; int limit_h = 0; for (int j = 0; j < wrects.size(); j++) { @@ -1061,7 +1048,7 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu if (end_w > max_w) max_w = end_w; - if (ofs == 0 || end_h > limit_h) //while h limit not reached, keep stacking + if (ofs == 0 || end_h > limit_h) // While h limit not reached, keep stacking. ofs += wrects[j].s.width; } @@ -1072,7 +1059,7 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu results.push_back(result); } - //find the result with the best aspect ratio + // Find the result with the best aspect ratio. int best = -1; real_t best_aspect = 1e20; @@ -1097,3 +1084,106 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu r_size = Size2(results[best].max_w, results[best].max_h); } + +Vector<Vector<Point2> > Geometry::_polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open) { + + using namespace ClipperLib; + + ClipType op = ctUnion; + + switch (p_op) { + case OPERATION_UNION: op = ctUnion; break; + case OPERATION_DIFFERENCE: op = ctDifference; break; + case OPERATION_INTERSECTION: op = ctIntersection; break; + case OPERATION_XOR: op = ctXor; break; + } + Path path_a, path_b; + + // Need to scale points (Clipper's requirement for robust computation). + for (int i = 0; i != p_polypath_a.size(); ++i) { + path_a << IntPoint(p_polypath_a[i].x * SCALE_FACTOR, p_polypath_a[i].y * SCALE_FACTOR); + } + for (int i = 0; i != p_polypath_b.size(); ++i) { + path_b << IntPoint(p_polypath_b[i].x * SCALE_FACTOR, p_polypath_b[i].y * SCALE_FACTOR); + } + Clipper clp; + clp.AddPath(path_a, ptSubject, !is_a_open); // Forward compatible with Clipper 10.0.0. + clp.AddPath(path_b, ptClip, true); // Polylines cannot be set as clip. + + Paths paths; + + if (is_a_open) { + PolyTree tree; // Needed to populate polylines. + clp.Execute(op, tree); + OpenPathsFromPolyTree(tree, paths); + } else { + clp.Execute(op, paths); // Works on closed polygons only. + } + // Have to scale points down now. + Vector<Vector<Point2> > polypaths; + + for (Paths::size_type i = 0; i < paths.size(); ++i) { + Vector<Vector2> polypath; + + const Path &scaled_path = paths[i]; + + for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { + polypath.push_back(Point2( + static_cast<real_t>(scaled_path[j].X) / SCALE_FACTOR, + static_cast<real_t>(scaled_path[j].Y) / SCALE_FACTOR)); + } + polypaths.push_back(polypath); + } + return polypaths; +} + +Vector<Vector<Point2> > Geometry::_polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { + + using namespace ClipperLib; + + JoinType jt = jtSquare; + + switch (p_join_type) { + case JOIN_SQUARE: jt = jtSquare; break; + case JOIN_ROUND: jt = jtRound; break; + case JOIN_MITER: jt = jtMiter; break; + } + + EndType et = etClosedPolygon; + + switch (p_end_type) { + case END_POLYGON: et = etClosedPolygon; break; + case END_JOINED: et = etClosedLine; break; + case END_BUTT: et = etOpenButt; break; + case END_SQUARE: et = etOpenSquare; break; + case END_ROUND: et = etOpenRound; break; + } + ClipperOffset co; + Path path; + + // Need to scale points (Clipper's requirement for robust computation). + for (int i = 0; i != p_polypath.size(); ++i) { + path << IntPoint(p_polypath[i].x * SCALE_FACTOR, p_polypath[i].y * SCALE_FACTOR); + } + co.AddPath(path, jt, et); + + Paths paths; + co.Execute(paths, p_delta * SCALE_FACTOR); // Inflate/deflate. + + // Have to scale points down now. + Vector<Vector<Point2> > polypaths; + + for (Paths::size_type i = 0; i < paths.size(); ++i) { + Vector<Vector2> polypath; + + const Path &scaled_path = paths[i]; + + for (Paths::size_type j = 0; j < scaled_path.size(); ++j) { + polypath.push_back(Point2( + static_cast<real_t>(scaled_path[j].X) / SCALE_FACTOR, + static_cast<real_t>(scaled_path[j].Y) / SCALE_FACTOR)); + } + polypaths.push_back(polypath); + } + return polypaths; +} diff --git a/core/math/geometry.h b/core/math/geometry.h index a813a90774..8b0a51c651 100644 --- a/core/math/geometry.h +++ b/core/math/geometry.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,56 +31,53 @@ #ifndef GEOMETRY_H #define GEOMETRY_H -#include "core/dvector.h" +#include "core/math/delaunay.h" #include "core/math/face3.h" #include "core/math/rect2.h" #include "core/math/triangulate.h" #include "core/math/vector3.h" #include "core/object.h" +#include "core/pool_vector.h" #include "core/print_string.h" #include "core/vector.h" -/** - @author Juan Linietsky <reduzio@gmail.com> -*/ - class Geometry { Geometry(); public: static real_t get_closest_points_between_segments(const Vector2 &p1, const Vector2 &q1, const Vector2 &p2, const Vector2 &q2, Vector2 &c1, Vector2 &c2) { - Vector2 d1 = q1 - p1; // Direction vector of segment S1 - Vector2 d2 = q2 - p2; // Direction vector of segment S2 + Vector2 d1 = q1 - p1; // Direction vector of segment S1. + Vector2 d2 = q2 - p2; // Direction vector of segment S2. Vector2 r = p1 - p2; - real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative - real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative + real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative. + real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative. real_t f = d2.dot(r); real_t s, t; - // Check if either or both segments degenerate into points + // Check if either or both segments degenerate into points. if (a <= CMP_EPSILON && e <= CMP_EPSILON) { - // Both segments degenerate into points + // Both segments degenerate into points. c1 = p1; c2 = p2; return Math::sqrt((c1 - c2).dot(c1 - c2)); } if (a <= CMP_EPSILON) { - // First segment degenerates into a point + // First segment degenerates into a point. s = 0.0; t = f / e; // s = 0 => t = (b*s + f) / e = f / e t = CLAMP(t, 0.0, 1.0); } else { real_t c = d1.dot(r); if (e <= CMP_EPSILON) { - // Second segment degenerates into a point + // Second segment degenerates into a point. t = 0.0; s = CLAMP(-c / a, 0.0, 1.0); // t = 0 => s = (b*t - c) / a = -c / a } else { - // The general nondegenerate case starts here + // The general nondegenerate case starts here. real_t b = d1.dot(d2); - real_t denom = a * e - b * b; // Always nonnegative + real_t denom = a * e - b * b; // Always nonnegative. // If segments not parallel, compute closest point on L1 to L2 and - // clamp to segment S1. Else pick arbitrary s (here 0) + // clamp to segment S1. Else pick arbitrary s (here 0). if (denom != 0.0) { s = CLAMP((b * f - c * e) / denom, 0.0, 1.0); } else @@ -91,7 +88,7 @@ public: //If t in [0,1] done. Else clamp t, recompute s for the new value // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a - // and clamp s to [0, 1] + // and clamp s to [0, 1]. if (t < 0.0) { t = 0.0; s = CLAMP(-c / a, 0.0, 1.0); @@ -108,14 +105,14 @@ public: static void get_closest_points_between_segments(const Vector3 &p1, const Vector3 &p2, const Vector3 &q1, const Vector3 &q2, Vector3 &c1, Vector3 &c2) { -//do the function 'd' as defined by pb. I think is is dot product of some sort +// Do the function 'd' as defined by pb. I think is is dot product of some sort. #define d_of(m, n, o, p) ((m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z)) - //calculate the parametric position on the 2 curves, mua and mub + // Calculate the parametric position on the 2 curves, mua and mub. real_t mua = (d_of(p1, q1, q2, q1) * d_of(q2, q1, p2, p1) - d_of(p1, q1, p2, p1) * d_of(q2, q1, q2, q1)) / (d_of(p2, p1, p2, p1) * d_of(q2, q1, q2, q1) - d_of(q2, q1, p2, p1) * d_of(q2, q1, p2, p1)); real_t mub = (d_of(p1, q1, q2, q1) + mua * d_of(q2, q1, p2, p1)) / d_of(q2, q1, q2, q1); - //clip the value between [0..1] constraining the solution to lie on the original curves + // Clip the value between [0..1] constraining the solution to lie on the original curves. if (mua < 0) mua = 0; if (mub < 0) mub = 0; if (mua > 1) mua = 1; @@ -128,38 +125,38 @@ public: Vector3 u = p_to_a - p_from_a; Vector3 v = p_to_b - p_from_b; Vector3 w = p_from_a - p_to_a; - real_t a = u.dot(u); // always >= 0 + real_t a = u.dot(u); // Always >= 0 real_t b = u.dot(v); - real_t c = v.dot(v); // always >= 0 + real_t c = v.dot(v); // Always >= 0 real_t d = u.dot(w); real_t e = v.dot(w); - real_t D = a * c - b * b; // always >= 0 + real_t D = a * c - b * b; // Always >= 0 real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 - // compute the line parameters of the two closest points - if (D < CMP_EPSILON) { // the lines are almost parallel - sN = 0.0; // force using point P0 on segment S1 - sD = 1.0; // to prevent possible division by 0.0 later + // Compute the line parameters of the two closest points. + if (D < CMP_EPSILON) { // The lines are almost parallel. + sN = 0.0; // Force using point P0 on segment S1 + sD = 1.0; // to prevent possible division by 0.0 later. tN = e; tD = c; - } else { // get the closest points on the infinite lines + } else { // Get the closest points on the infinite lines sN = (b * e - c * d); tN = (a * e - b * d); - if (sN < 0.0) { // sc < 0 => the s=0 edge is visible + if (sN < 0.0) { // sc < 0 => the s=0 edge is visible. sN = 0.0; tN = e; tD = c; - } else if (sN > sD) { // sc > 1 => the s=1 edge is visible + } else if (sN > sD) { // sc > 1 => the s=1 edge is visible. sN = sD; tN = e + b; tD = c; } } - if (tN < 0.0) { // tc < 0 => the t=0 edge is visible + if (tN < 0.0) { // tc < 0 => the t=0 edge is visible. tN = 0.0; - // recompute sc for this edge + // Recompute sc for this edge. if (-d < 0.0) sN = 0.0; else if (-d > a) @@ -168,9 +165,9 @@ public: sN = -d; sD = a; } - } else if (tN > tD) { // tc > 1 => the t=1 edge is visible + } else if (tN > tD) { // tc > 1 => the t=1 edge is visible. tN = tD; - // recompute sc for this edge + // Recompute sc for this edge. if ((-d + b) < 0.0) sN = 0; else if ((-d + b) > a) @@ -180,14 +177,14 @@ public: sD = a; } } - // finally do the division to get sc and tc - sc = (Math::abs(sN) < CMP_EPSILON ? 0.0 : sN / sD); - tc = (Math::abs(tN) < CMP_EPSILON ? 0.0 : tN / tD); + // Finally do the division to get sc and tc. + sc = (Math::is_zero_approx(sN) ? 0.0 : sN / sD); + tc = (Math::is_zero_approx(tN) ? 0.0 : tN / tD); - // get the difference of the two closest points + // Get the difference of the two closest points. Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) - return dP.length(); // return the closest distance + return dP.length(); // Return the closest distance. } static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = 0) { @@ -195,7 +192,7 @@ public: Vector3 e2 = p_v2 - p_v0; Vector3 h = p_dir.cross(e2); real_t a = e1.dot(h); - if (a > -CMP_EPSILON && a < CMP_EPSILON) // parallel test + if (Math::is_zero_approx(a)) // Parallel test. return false; real_t f = 1.0 / a; @@ -213,16 +210,15 @@ public: if (v < 0.0 || u + v > 1.0) return false; - // at this stage we can compute t to find out where - // the intersection point is on the line + // At this stage we can compute t to find out where + // the intersection point is on the line. real_t t = f * e2.dot(q); if (t > 0.00001) { // ray intersection if (r_res) *r_res = p_from + p_dir * t; return true; - } else // this means that there is a line intersection - // but not a ray intersection + } else // This means that there is a line intersection but not a ray intersection. return false; } @@ -233,7 +229,7 @@ public: Vector3 e2 = p_v2 - p_v0; Vector3 h = rel.cross(e2); real_t a = e1.dot(h); - if (a > -CMP_EPSILON && a < CMP_EPSILON) // parallel test + if (Math::is_zero_approx(a)) // Parallel test. return false; real_t f = 1.0 / a; @@ -251,16 +247,15 @@ public: if (v < 0.0 || u + v > 1.0) return false; - // at this stage we can compute t to find out where - // the intersection point is on the line + // At this stage we can compute t to find out where + // the intersection point is on the line. real_t t = f * e2.dot(q); - if (t > CMP_EPSILON && t <= 1.0) { // ray intersection + if (t > CMP_EPSILON && t <= 1.0) { // Ray intersection. if (r_res) *r_res = p_from + rel * t; return true; - } else // this means that there is a line intersection - // but not a ray intersection + } else // This means that there is a line intersection but not a ray intersection. return false; } @@ -270,13 +265,11 @@ public: Vector3 rel = (p_to - p_from); real_t rel_l = rel.length(); if (rel_l < CMP_EPSILON) - return false; // both points are the same + return false; // Both points are the same. Vector3 normal = rel / rel_l; real_t sphere_d = normal.dot(sphere_pos); - //Vector3 ray_closest=normal*sphere_d; - real_t ray_distance = sphere_pos.distance_to(normal * sphere_d); if (ray_distance >= p_sphere_radius) @@ -288,7 +281,7 @@ public: if (inters_d2 >= CMP_EPSILON) inters_d -= Math::sqrt(inters_d2); - // check in segment + // Check in segment. if (inters_d < 0 || inters_d > rel_l) return false; @@ -307,9 +300,9 @@ public: Vector3 rel = (p_to - p_from); real_t rel_l = rel.length(); if (rel_l < CMP_EPSILON) - return false; // both points are the same + return false; // Both points are the same. - // first check if they are parallel + // First check if they are parallel. Vector3 normal = (rel / rel_l); Vector3 crs = normal.cross(Vector3(0, 0, 1)); real_t crs_l = crs.length(); @@ -317,8 +310,7 @@ public: Vector3 z_dir; if (crs_l < CMP_EPSILON) { - //blahblah parallel - z_dir = Vector3(1, 0, 0); //any x/y vector ok + z_dir = Vector3(1, 0, 0); // Any x/y vector OK. } else { z_dir = crs / crs_l; } @@ -326,12 +318,12 @@ public: real_t dist = z_dir.dot(p_from); if (dist >= p_radius) - return false; // too far away + return false; // Too far away. - // convert to 2D + // Convert to 2D. real_t w2 = p_radius * p_radius - dist * dist; if (w2 < CMP_EPSILON) - return false; //avoid numerical error + return false; // Avoid numerical error. Size2 size(Math::sqrt(w2), p_height * 0.5); Vector3 x_dir = z_dir.cross(Vector3(0, 0, 1)).normalized(); @@ -378,7 +370,7 @@ public: return false; } - // convert to 3D again + // Convert to 3D again. Vector3 result = p_from + (rel * min); Vector3 res_normal = result; @@ -419,19 +411,18 @@ public: real_t den = p.normal.dot(dir); - //printf("den is %i\n",den); if (Math::abs(den) <= CMP_EPSILON) - continue; // ignore parallel plane + continue; // Ignore parallel plane. real_t dist = -p.distance_to(p_from) / den; if (den > 0) { - //backwards facing plane + // Backwards facing plane. if (dist < max) max = dist; } else { - //front facing plane + // Front facing plane. if (dist > min) { min = dist; min_index = i; @@ -439,8 +430,8 @@ public: } } - if (max <= min || min < 0 || min > rel_l || min_index == -1) // exit conditions - return false; // no intersection + if (max <= min || min < 0 || min > rel_l || min_index == -1) // Exit conditions. + return false; // No intersection. if (p_res) *p_res = p_from + dir * min; @@ -454,52 +445,49 @@ public: Vector3 p = p_point - p_segment[0]; Vector3 n = p_segment[1] - p_segment[0]; - real_t l = n.length(); - if (l < 1e-10) - return p_segment[0]; // both points are the same, just give any - n /= l; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) + return p_segment[0]; // Both points are the same, just give any. - real_t d = n.dot(p); + real_t d = n.dot(p) / l2; if (d <= 0.0) - return p_segment[0]; // before first point - else if (d >= l) - return p_segment[1]; // after first point + return p_segment[0]; // Before first point. + else if (d >= 1.0) + return p_segment[1]; // After first point. else - return p_segment[0] + n * d; // inside + return p_segment[0] + n * d; // Inside. } static Vector3 get_closest_point_to_segment_uncapped(const Vector3 &p_point, const Vector3 *p_segment) { Vector3 p = p_point - p_segment[0]; Vector3 n = p_segment[1] - p_segment[0]; - real_t l = n.length(); - if (l < 1e-10) - return p_segment[0]; // both points are the same, just give any - n /= l; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) + return p_segment[0]; // Both points are the same, just give any. - real_t d = n.dot(p); + real_t d = n.dot(p) / l2; - return p_segment[0] + n * d; // inside + return p_segment[0] + n * d; // Inside. } static Vector2 get_closest_point_to_segment_2d(const Vector2 &p_point, const Vector2 *p_segment) { Vector2 p = p_point - p_segment[0]; Vector2 n = p_segment[1] - p_segment[0]; - real_t l = n.length(); - if (l < 1e-10) - return p_segment[0]; // both points are the same, just give any - n /= l; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) + return p_segment[0]; // Both points are the same, just give any. - real_t d = n.dot(p); + real_t d = n.dot(p) / l2; if (d <= 0.0) - return p_segment[0]; // before first point - else if (d >= l) - return p_segment[1]; // after first point + return p_segment[0]; // Before first point. + else if (d >= 1.0) + return p_segment[1]; // After first point. else - return p_segment[0] + n * d; // inside + return p_segment[0] + n * d; // Inside. } static bool is_point_in_triangle(const Vector2 &s, const Vector2 &a, const Vector2 &b, const Vector2 &c) { @@ -514,28 +502,25 @@ public: return (cn.cross(an) > 0) == orientation; } - static bool is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon); - static Vector2 get_closest_point_to_segment_uncapped_2d(const Vector2 &p_point, const Vector2 *p_segment) { Vector2 p = p_point - p_segment[0]; Vector2 n = p_segment[1] - p_segment[0]; - real_t l = n.length(); - if (l < 1e-10) - return p_segment[0]; // both points are the same, just give any - n /= l; + real_t l2 = n.length_squared(); + if (l2 < 1e-20) + return p_segment[0]; // Both points are the same, just give any. - real_t d = n.dot(p); + real_t d = n.dot(p) / l2; - return p_segment[0] + n * d; // inside + return p_segment[0] + n * d; // Inside. } static bool line_intersects_line_2d(const Vector2 &p_from_a, const Vector2 &p_dir_a, const Vector2 &p_from_b, const Vector2 &p_dir_b, Vector2 &r_result) { - // see http://paulbourke.net/geometry/pointlineplane/ + // See http://paulbourke.net/geometry/pointlineplane/ const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y; - if (Math::abs(denom) < CMP_EPSILON) { // parallel? + if (Math::is_zero_approx(denom)) { // Parallel? return false; } @@ -563,11 +548,11 @@ public: real_t ABpos = D.x + (C.x - D.x) * D.y / (D.y - C.y); - // Fail if segment C-D crosses line A-B outside of segment A-B. + // Fail if segment C-D crosses line A-B outside of segment A-B. if (ABpos < 0 || ABpos > 1.0) return false; - // (4) Apply the discovered position to line A-B in the original coordinate system. + // (4) Apply the discovered position to line A-B in the original coordinate system. if (r_result) *r_result = p_from_a + B * ABpos; @@ -600,7 +585,7 @@ public: real_t d = p_normal.dot(p_sphere_pos) - p_normal.dot(p_triangle[0]); - if (d > p_sphere_radius || d < -p_sphere_radius) // not touching the plane of the face, return + if (d > p_sphere_radius || d < -p_sphere_radius) // Not touching the plane of the face, return. return false; Vector3 contact = p_sphere_pos - (p_normal * d); @@ -620,25 +605,25 @@ public: for (int i = 0; i < 3; i++) { - // check edge cylinder + // Check edge cylinder. Vector3 n1 = verts[i] - verts[i + 1]; Vector3 n2 = p_sphere_pos - verts[i + 1]; - ///@TODO i could discard by range here to make the algorithm quicker? dunno.. + ///@TODO Maybe discard by range here to make the algorithm quicker. - // check point within cylinder radius + // Check point within cylinder radius. Vector3 axis = n1.cross(n2).cross(n1); - axis.normalize(); // ugh + axis.normalize(); real_t ad = axis.dot(n2); if (ABS(ad) > p_sphere_radius) { - // no chance with this edge, too far away + // No chance with this edge, too far away. continue; } - // check point within edge capsule cylinder + // Check point within edge capsule cylinder. /** 4th TEST INSIDE EDGE POINTS **/ real_t sphere_at = n1.dot(n2); @@ -647,8 +632,7 @@ public: r_triangle_contact = p_sphere_pos - axis * (axis.dot(n2)); r_sphere_contact = p_sphere_pos - axis * p_sphere_radius; - // point inside here - //printf("solved inside edge\n"); + // Point inside here. return true; } @@ -658,53 +642,58 @@ public: Vector3 n = (p_sphere_pos - verts[i + 1]).normalized(); - //r_triangle_contact=verts[i+1]+n*p_sphere_radius;p_sphere_pos+axis*(p_sphere_radius-axis.dot(n2)); r_triangle_contact = verts[i + 1]; r_sphere_contact = p_sphere_pos - n * p_sphere_radius; - //printf("solved inside point segment 1\n"); return true; } if (n2.distance_squared_to(n1) < r2) { Vector3 n = (p_sphere_pos - verts[i]).normalized(); - //r_triangle_contact=verts[i]+n*p_sphere_radius;p_sphere_pos+axis*(p_sphere_radius-axis.dot(n2)); r_triangle_contact = verts[i]; r_sphere_contact = p_sphere_pos - n * p_sphere_radius; - //printf("solved inside point segment 1\n"); return true; } - break; // It's pointless to continue at this point, so save some cpu cycles + break; // It's pointless to continue at this point, so save some CPU cycles. } return false; } + static inline bool is_point_in_circle(const Vector2 &p_point, const Vector2 &p_circle_pos, real_t p_circle_radius) { + + return p_point.distance_squared_to(p_circle_pos) <= p_circle_radius * p_circle_radius; + } + static real_t segment_intersects_circle(const Vector2 &p_from, const Vector2 &p_to, const Vector2 &p_circle_pos, real_t p_circle_radius) { Vector2 line_vec = p_to - p_from; Vector2 vec_to_line = p_from - p_circle_pos; - /* create a quadratic formula of the form ax^2 + bx + c = 0 */ + // Create a quadratic formula of the form ax^2 + bx + c = 0 real_t a, b, c; a = line_vec.dot(line_vec); b = 2 * vec_to_line.dot(line_vec); c = vec_to_line.dot(vec_to_line) - p_circle_radius * p_circle_radius; - /* solve for t */ + // Solve for t. real_t sqrtterm = b * b - 4 * a * c; - /* if the term we intend to square root is less than 0 then the answer won't be real, so it definitely won't be t in the range 0 to 1 */ + // If the term we intend to square root is less than 0 then the answer won't be real, + // so it definitely won't be t in the range 0 to 1. if (sqrtterm < 0) return -1; - /* if we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection) then the following can be skipped and we can just return the equivalent of res1 */ + // If we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection) + // then the following can be skipped and we can just return the equivalent of res1. sqrtterm = Math::sqrt(sqrtterm); real_t res1 = (-b - sqrtterm) / (2 * a); - //real_t res2 = ( -b + sqrtterm ) / (2 * a); + real_t res2 = (-b + sqrtterm) / (2 * a); - return (res1 >= 0 && res1 <= 1) ? res1 : -1; + if (res1 >= 0 && res1 <= 1) return res1; + if (res2 >= 0 && res2 <= 1) return res2; + return -1; } static inline Vector<Vector3> clip_polygon(const Vector<Vector3> &polygon, const Plane &p_plane) { @@ -723,7 +712,6 @@ public: int outside_count = 0; for (int a = 0; a < polygon.size(); a++) { - //real_t p_plane.d = (*this) * polygon[a]; real_t dist = p_plane.distance_to(polygon[a]); if (dist < -CMP_POINT_IN_PLANE_EPSILON) { location_cache[a] = LOC_INSIDE; @@ -740,11 +728,11 @@ public: if (outside_count == 0) { - return polygon; // no changes + return polygon; // No changes. } else if (inside_count == 0) { - return Vector<Vector3>(); //empty + return Vector<Vector3>(); // Empty. } long previous = polygon.size() - 1; @@ -783,6 +771,80 @@ public: return clipped; } + enum PolyBooleanOperation { + OPERATION_UNION, + OPERATION_DIFFERENCE, + OPERATION_INTERSECTION, + OPERATION_XOR + }; + enum PolyJoinType { + JOIN_SQUARE, + JOIN_ROUND, + JOIN_MITER + }; + enum PolyEndType { + END_POLYGON, + END_JOINED, + END_BUTT, + END_SQUARE, + END_ROUND + }; + + static Vector<Vector<Point2> > merge_polygons_2d(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { + + return _polypaths_do_operation(OPERATION_UNION, p_polygon_a, p_polygon_b); + } + + static Vector<Vector<Point2> > clip_polygons_2d(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { + + return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polygon_a, p_polygon_b); + } + + static Vector<Vector<Point2> > intersect_polygons_2d(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { + + return _polypaths_do_operation(OPERATION_INTERSECTION, p_polygon_a, p_polygon_b); + } + + static Vector<Vector<Point2> > exclude_polygons_2d(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) { + + return _polypaths_do_operation(OPERATION_XOR, p_polygon_a, p_polygon_b); + } + + static Vector<Vector<Point2> > clip_polyline_with_polygon_2d(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) { + + return _polypaths_do_operation(OPERATION_DIFFERENCE, p_polyline, p_polygon, true); + } + + static Vector<Vector<Point2> > intersect_polyline_with_polygon_2d(const Vector<Vector2> &p_polyline, const Vector<Vector2> &p_polygon) { + + return _polypaths_do_operation(OPERATION_INTERSECTION, p_polyline, p_polygon, true); + } + + static Vector<Vector<Point2> > offset_polygon_2d(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type) { + + return _polypath_offset(p_polygon, p_delta, p_join_type, END_POLYGON); + } + + static Vector<Vector<Point2> > offset_polyline_2d(const Vector<Vector2> &p_polygon, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type) { + + ERR_FAIL_COND_V_MSG(p_end_type == END_POLYGON, Vector<Vector<Point2> >(), "Attempt to offset a polyline like a polygon (use offset_polygon_2d instead)."); + + return _polypath_offset(p_polygon, p_delta, p_join_type, p_end_type); + } + + static Vector<int> triangulate_delaunay_2d(const Vector<Vector2> &p_points) { + + Vector<Delaunay2D::Triangle> tr = Delaunay2D::triangulate(p_points); + Vector<int> triangles; + + for (int i = 0; i < tr.size(); i++) { + triangles.push_back(tr[i].points[0]); + triangles.push_back(tr[i].points[1]); + triangles.push_back(tr[i].points[2]); + } + return triangles; + } + static Vector<int> triangulate_polygon(const Vector<Vector2> &p_polygon) { Vector<int> triangles; @@ -800,9 +862,56 @@ public: return Vector<Vector<Vector2> >(); } + static bool is_polygon_clockwise(const Vector<Vector2> &p_polygon) { + int c = p_polygon.size(); + if (c < 3) + return false; + const Vector2 *p = p_polygon.ptr(); + real_t sum = 0; + for (int i = 0; i < c; i++) { + const Vector2 &v1 = p[i]; + const Vector2 &v2 = p[(i + 1) % c]; + sum += (v2.x - v1.x) * (v2.y + v1.y); + } + + return sum > 0.0f; + } + + // Alternate implementation that should be faster. + static bool is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon) { + int c = p_polygon.size(); + if (c < 3) + return false; + const Vector2 *p = p_polygon.ptr(); + Vector2 further_away(-1e20, -1e20); + Vector2 further_away_opposite(1e20, 1e20); + + for (int i = 0; i < c; i++) { + further_away.x = MAX(p[i].x, further_away.x); + further_away.y = MAX(p[i].y, further_away.y); + further_away_opposite.x = MIN(p[i].x, further_away_opposite.x); + further_away_opposite.y = MIN(p[i].y, further_away_opposite.y); + } + + // Make point outside that won't intersect with points in segment from p_point. + further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312); + + int intersections = 0; + for (int i = 0; i < c; i++) { + const Vector2 &v1 = p[i]; + const Vector2 &v2 = p[(i + 1) % c]; + if (segment_intersects_segment_2d(v1, v2, p_point, further_away, NULL)) { + intersections++; + } + } + + return (intersections & 1); + } + static PoolVector<PoolVector<Face3> > separate_objects(PoolVector<Face3> p_array); - static PoolVector<Face3> wrap_geometry(PoolVector<Face3> p_array, real_t *p_error = NULL); ///< create a "wrap" that encloses the given geometry + // Create a "wrap" that encloses the given geometry. + static PoolVector<Face3> wrap_geometry(PoolVector<Face3> p_array, real_t *p_error = NULL); struct MeshData { @@ -884,17 +993,17 @@ public: Vector<Point2> H; H.resize(2 * n); - // Sort points lexicographically + // Sort points lexicographically. P.sort(); - // Build lower hull + // Build lower hull. for (int i = 0; i < n; ++i) { while (k >= 2 && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) k--; H.write[k++] = P[i]; } - // Build upper hull + // Build upper hull. for (int i = n - 2, t = k + 1; i >= 0; i--) { while (k >= t && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) k--; @@ -904,6 +1013,7 @@ public: H.resize(k); return H; } + static Vector<Vector<Vector2> > decompose_polygon_in_convex(Vector<Point2> polygon); static MeshData build_convex_mesh(const PoolVector<Plane> &p_planes); static PoolVector<Plane> build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis = Vector3::AXIS_Z); @@ -912,6 +1022,10 @@ public: static PoolVector<Plane> build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis = Vector3::AXIS_Z); static void make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_result, Size2i &r_size); + +private: + static Vector<Vector<Point2> > _polypaths_do_operation(PolyBooleanOperation p_op, const Vector<Point2> &p_polypath_a, const Vector<Point2> &p_polypath_b, bool is_a_open = false); + static Vector<Vector<Point2> > _polypath_offset(const Vector<Point2> &p_polypath, real_t p_delta, PolyJoinType p_join_type, PolyEndType p_end_type); }; #endif diff --git a/core/math/math_defs.h b/core/math/math_defs.h index a5feee6eb5..c54d3cc96f 100644 --- a/core/math/math_defs.h +++ b/core/math/math_defs.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,6 +33,7 @@ #define CMP_EPSILON 0.00001 #define CMP_EPSILON2 (CMP_EPSILON * CMP_EPSILON) + #define CMP_NORMALIZE_TOLERANCE 0.000001 #define CMP_POINT_IN_PLANE_EPSILON 0.00001 @@ -49,6 +50,14 @@ #define MATH_CHECKS #endif +//this epsilon is for values related to a unit size (scalar or vector len) +#ifdef PRECISE_MATH_CHECKS +#define UNIT_EPSILON 0.00001 +#else +//tolerate some more floating point error normally +#define UNIT_EPSILON 0.001 +#endif + #define USEC_TO_SEC(m_usec) ((m_usec) / 1000000.0) enum ClockDirection { @@ -93,9 +102,9 @@ enum Corner { }; /** - * The "Real" type is an abstract type used for real numbers, such as 1.5, + * The "Real" type is an abstract type used for real numbers, such as 1.5, * in contrast to integer numbers. Precision can be controlled with the - * presence or absence of the REAL_T_IS_DOUBLE define. + * presence or absence of the REAL_T_IS_DOUBLE define. */ #ifdef REAL_T_IS_DOUBLE typedef double real_t; diff --git a/core/math/math_fieldwise.cpp b/core/math/math_fieldwise.cpp index 20b2341ab0..f65f504f4c 100644 --- a/core/math/math_fieldwise.cpp +++ b/core/math/math_fieldwise.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/math_fieldwise.h b/core/math/math_fieldwise.h index 0e7cc3ea4a..c245928f56 100644 --- a/core/math/math_fieldwise.h +++ b/core/math/math_fieldwise.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp index 0c06d2a2b5..f04e40cb6c 100644 --- a/core/math/math_funcs.cpp +++ b/core/math/math_funcs.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,30 +30,27 @@ #include "math_funcs.h" -#include "core/os/os.h" - -pcg32_random_t Math::default_pcg = { 12047754176567800795ULL, PCG_DEFAULT_INC_64 }; +RandomPCG Math::default_rand(RandomPCG::DEFAULT_SEED, RandomPCG::DEFAULT_INC); #define PHI 0x9e3779b9 -// TODO: we should eventually expose pcg.inc too uint32_t Math::rand_from_seed(uint64_t *seed) { - pcg32_random_t pcg = { *seed, PCG_DEFAULT_INC_64 }; - uint32_t r = pcg32_random_r(&pcg); - *seed = pcg.state; + RandomPCG rng = RandomPCG(*seed, RandomPCG::DEFAULT_INC); + uint32_t r = rng.rand(); + *seed = rng.get_seed(); return r; } void Math::seed(uint64_t x) { - default_pcg.state = x; + default_rand.seed(x); } void Math::randomize() { - seed(OS::get_singleton()->get_ticks_usec() * default_pcg.state + PCG_DEFAULT_INC_64); + default_rand.randomize(); } uint32_t Math::rand() { - return pcg32_random_r(&default_pcg); + return default_rand.rand(); } int Math::step_decimals(double p_step) { @@ -82,6 +79,15 @@ int Math::step_decimals(double p_step) { return 0; } +// Only meant for editor usage in float ranges, where a step of 0 +// means that decimal digits should not be limited in String::num. +int Math::range_step_decimals(double p_step) { + if (p_step < 0.0000000000001) { + return 16; // Max value hardcoded in String::num + } + return step_decimals(p_step); +} + double Math::dectime(double p_value, double p_amount, double p_step) { double sgn = p_value < 0 ? -1.0 : 1.0; double val = Math::abs(p_value); @@ -164,18 +170,12 @@ uint32_t Math::larger_prime(uint32_t p_val) { return primes[idx]; idx++; } - - return 0; } double Math::random(double from, double to) { - unsigned int r = Math::rand(); - double ret = (double)r / (double)RANDOM_MAX; - return (ret) * (to - from) + from; + return default_rand.random(from, to); } float Math::random(float from, float to) { - unsigned int r = Math::rand(); - float ret = (float)r / (float)RANDOM_MAX; - return (ret) * (to - from) + from; + return default_rand.random(from, to); } diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 472baf0484..9078abea68 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,6 +32,7 @@ #define MATH_FUNCS_H #include "core/math/math_defs.h" +#include "core/math/random_pcg.h" #include "core/typedefs.h" #include "thirdparty/misc/pcg.h" @@ -41,12 +42,12 @@ class Math { - static pcg32_random_t default_pcg; + static RandomPCG default_rand; public: Math() {} // useless to instance - static const uint64_t RANDOM_MAX = 4294967295; + static const uint64_t RANDOM_MAX = 0xFFFFFFFF; static _ALWAYS_INLINE_ double sin(double p_x) { return ::sin(p_x); } static _ALWAYS_INLINE_ float sin(float p_x) { return ::sinf(p_x); } @@ -60,6 +61,12 @@ public: static _ALWAYS_INLINE_ double sinh(double p_x) { return ::sinh(p_x); } static _ALWAYS_INLINE_ float sinh(float p_x) { return ::sinhf(p_x); } + static _ALWAYS_INLINE_ float sinc(float p_x) { return p_x == 0 ? 1 : ::sin(p_x) / p_x; } + static _ALWAYS_INLINE_ double sinc(double p_x) { return p_x == 0 ? 1 : ::sin(p_x) / p_x; } + + static _ALWAYS_INLINE_ float sincn(float p_x) { return sinc(Math_PI * p_x); } + static _ALWAYS_INLINE_ double sincn(double p_x) { return sinc(Math_PI * p_x); } + static _ALWAYS_INLINE_ double cosh(double p_x) { return ::cosh(p_x); } static _ALWAYS_INLINE_ float cosh(float p_x) { return ::coshf(p_x); } @@ -191,6 +198,13 @@ public: value += 0.0; return value; } + static _ALWAYS_INLINE_ int posmod(int p_x, int p_y) { + int value = p_x % p_y; + if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { + value += p_y; + } + return value; + } static _ALWAYS_INLINE_ double deg2rad(double p_y) { return p_y * Math_PI / 180.0; } static _ALWAYS_INLINE_ float deg2rad(float p_y) { return p_y * Math_PI / 180.0; } @@ -201,12 +215,36 @@ public: static _ALWAYS_INLINE_ double lerp(double p_from, double p_to, double p_weight) { return p_from + (p_to - p_from) * p_weight; } static _ALWAYS_INLINE_ float lerp(float p_from, float p_to, float p_weight) { return p_from + (p_to - p_from) * p_weight; } + static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) { + double difference = fmod(p_to - p_from, Math_TAU); + double distance = fmod(2.0 * difference, Math_TAU) - difference; + return p_from + distance * p_weight; + } + static _ALWAYS_INLINE_ float lerp_angle(float p_from, float p_to, float p_weight) { + float difference = fmod(p_to - p_from, (float)Math_TAU); + float distance = fmod(2.0f * difference, (float)Math_TAU) - difference; + return p_from + distance * p_weight; + } + static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) { return (p_value - p_from) / (p_to - p_from); } static _ALWAYS_INLINE_ float inverse_lerp(float p_from, float p_to, float p_value) { return (p_value - p_from) / (p_to - p_from); } static _ALWAYS_INLINE_ double range_lerp(double p_value, double p_istart, double p_istop, double p_ostart, double p_ostop) { return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); } static _ALWAYS_INLINE_ float range_lerp(float p_value, float p_istart, float p_istop, float p_ostart, float p_ostop) { return Math::lerp(p_ostart, p_ostop, Math::inverse_lerp(p_istart, p_istop, p_value)); } + static _ALWAYS_INLINE_ double smoothstep(double p_from, double p_to, double p_weight) { + if (is_equal_approx(p_from, p_to)) return p_from; + double x = CLAMP((p_weight - p_from) / (p_to - p_from), 0.0, 1.0); + return x * x * (3.0 - 2.0 * x); + } + static _ALWAYS_INLINE_ float smoothstep(float p_from, float p_to, float p_weight) { + if (is_equal_approx(p_from, p_to)) return p_from; + float x = CLAMP((p_weight - p_from) / (p_to - p_from), 0.0f, 1.0f); + return x * x * (3.0f - 2.0f * x); + } + static _ALWAYS_INLINE_ double move_toward(double p_from, double p_to, double p_delta) { return abs(p_to - p_from) <= p_delta ? p_to : p_from + SGN(p_to - p_from) * p_delta; } + static _ALWAYS_INLINE_ float move_toward(float p_from, float p_to, float p_delta) { return abs(p_to - p_from) <= p_delta ? p_to : p_from + SGN(p_to - p_from) * p_delta; } + static _ALWAYS_INLINE_ double linear2db(double p_linear) { return Math::log(p_linear) * 8.6858896380650365530225783783321; } static _ALWAYS_INLINE_ float linear2db(float p_linear) { return Math::log(p_linear) * 8.6858896380650365530225783783321; } @@ -216,22 +254,23 @@ public: static _ALWAYS_INLINE_ double round(double p_val) { return (p_val >= 0) ? Math::floor(p_val + 0.5) : -Math::floor(-p_val + 0.5); } static _ALWAYS_INLINE_ float round(float p_val) { return (p_val >= 0) ? Math::floor(p_val + 0.5) : -Math::floor(-p_val + 0.5); } - static _ALWAYS_INLINE_ int wrapi(int value, int min, int max) { - int rng = max - min; - return min + ((((value - min) % rng) + rng) % rng); + static _ALWAYS_INLINE_ int64_t wrapi(int64_t value, int64_t min, int64_t max) { + int64_t range = max - min; + return range == 0 ? min : min + ((((value - min) % range) + range) % range); } static _ALWAYS_INLINE_ double wrapf(double value, double min, double max) { - double rng = max - min; - return value - (rng * Math::floor((value - min) / rng)); + double range = max - min; + return is_zero_approx(range) ? min : value - (range * Math::floor((value - min) / range)); } static _ALWAYS_INLINE_ float wrapf(float value, float min, float max) { - float rng = max - min; - return value - (rng * Math::floor((value - min) / rng)); + float range = max - min; + return is_zero_approx(range) ? min : value - (range * Math::floor((value - min) / range)); } // double only, as these functions are mainly used by the editor and not performance-critical, static double ease(double p_x, double p_c); static int step_decimals(double p_step); + static int range_step_decimals(double p_step); static double stepify(double p_value, double p_step); static double dectime(double p_value, double p_amount, double p_step); @@ -241,20 +280,49 @@ public: static void randomize(); static uint32_t rand_from_seed(uint64_t *seed); static uint32_t rand(); - static _ALWAYS_INLINE_ double randf() { return (double)rand() / (double)Math::RANDOM_MAX; } - static _ALWAYS_INLINE_ float randd() { return (float)rand() / (float)Math::RANDOM_MAX; } + static _ALWAYS_INLINE_ double randd() { return (double)rand() / (double)Math::RANDOM_MAX; } + static _ALWAYS_INLINE_ float randf() { return (float)rand() / (float)Math::RANDOM_MAX; } static double random(double from, double to); static float random(float from, float to); static real_t random(int from, int to) { return (real_t)random((real_t)from, (real_t)to); } + static _ALWAYS_INLINE_ bool is_equal_approx_ratio(real_t a, real_t b, real_t epsilon = CMP_EPSILON, real_t min_epsilon = CMP_EPSILON) { + // this is an approximate way to check that numbers are close, as a ratio of their average size + // helps compare approximate numbers that may be very big or very small + real_t diff = abs(a - b); + if (diff == 0.0 || diff < min_epsilon) { + return true; + } + real_t avg_size = (abs(a) + abs(b)) / 2.0; + diff /= avg_size; + return diff < epsilon; + } + static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b) { - // TODO: Comparing floats for approximate-equality is non-trivial. - // Using epsilon should cover the typical cases in Godot (where a == b is used to compare two reals), such as matrix and vector comparison operators. - // A proper implementation in terms of ULPs should eventually replace the contents of this function. - // See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ for details. + // Check for exact equality first, required to handle "infinity" values. + if (a == b) { + return true; + } + // Then check for approximate equality. + real_t tolerance = CMP_EPSILON * abs(a); + if (tolerance < CMP_EPSILON) { + tolerance = CMP_EPSILON; + } + return abs(a - b) < tolerance; + } - return abs(a - b) < CMP_EPSILON; + static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b, real_t tolerance) { + // Check for exact equality first, required to handle "infinity" values. + if (a == b) { + return true; + } + // Then check for approximate equality. + return abs(a - b) < tolerance; + } + + static _ALWAYS_INLINE_ bool is_zero_approx(real_t s) { + return abs(s) < CMP_EPSILON; } static _ALWAYS_INLINE_ float absf(float g) { @@ -305,16 +373,6 @@ public: return b; } -#if defined(__GNUC__) - - static _ALWAYS_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE - static _ALWAYS_INLINE_ int64_t dtoll(float p_float) { return (int64_t)p_float; } ///@TODO OPTIMIZE and rename -#else - - static _ALWAYS_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE - static _ALWAYS_INLINE_ int64_t dtoll(float p_float) { return (int64_t)p_float; } ///@TODO OPTIMIZE and rename -#endif - static _ALWAYS_INLINE_ uint32_t halfbits_to_floatbits(uint16_t h) { uint16_t h_exp, h_sig; uint32_t f_sgn, f_exp, f_sig; diff --git a/core/math/octree.h b/core/math/octree.h index cd89743a5a..db15c8a1f8 100644 --- a/core/math/octree.h +++ b/core/math/octree.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -38,10 +38,6 @@ #include "core/print_string.h" #include "core/variant.h" -/** - @author Juan Linietsky <reduzio@gmail.com> -*/ - typedef uint32_t OctreeElementID; #define OCTREE_ELEMENT_INVALID_ID 0 @@ -568,10 +564,7 @@ void Octree<T, use_pairs, AL>::_ensure_valid_root(const AABB &p_aabb) { while (!base.encloses(p_aabb)) { - if (base.size.x > OCTREE_SIZE_LIMIT) { - ERR_EXPLAIN("Octree upper size limit reeached, does the AABB supplied contain NAN?"); - ERR_FAIL(); - } + ERR_FAIL_COND_MSG(base.size.x > OCTREE_SIZE_LIMIT, "Octree upper size limit reached, does the AABB supplied contain NAN?"); Octant *gp = memnew_allocator(Octant, AL); octant_count++; @@ -916,34 +909,34 @@ void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) { pass++; - for (typename List<typename Element::OctantOwner, AL>::Element *E = owners.front(); E;) { + for (typename List<typename Element::OctantOwner, AL>::Element *F = owners.front(); F;) { - Octant *o = E->get().octant; - typename List<typename Element::OctantOwner, AL>::Element *N = E->next(); + Octant *o = F->get().octant; + typename List<typename Element::OctantOwner, AL>::Element *N = F->next(); /* if (!use_pairs) - o->elements.erase( E->get().E ); + o->elements.erase( F->get().E ); */ if (use_pairs && e.pairable) - o->pairable_elements.erase(E->get().E); + o->pairable_elements.erase(F->get().E); else - o->elements.erase(E->get().E); + o->elements.erase(F->get().E); if (_remove_element_from_octant(&e, o, common_parent->parent)) { - owners.erase(E); + owners.erase(F); } - E = N; + F = N; } if (use_pairs) { //unpair child elements in anything that survived - for (typename List<typename Element::OctantOwner, AL>::Element *E = owners.front(); E; E = E->next()) { + for (typename List<typename Element::OctantOwner, AL>::Element *F = owners.front(); F; F = F->next()) { - Octant *o = E->get().octant; + Octant *o = F->get().octant; // erase children pairs, unref ONCE pass++; diff --git a/core/math/plane.cpp b/core/math/plane.cpp index 3c597d57f8..b01853c4ac 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -110,7 +110,7 @@ bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 real_t den = normal.dot(segment); //printf("den is %i\n",den); - if (Math::abs(den) <= CMP_EPSILON) { + if (Math::is_zero_approx(den)) { return false; } @@ -135,7 +135,7 @@ bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vec real_t den = normal.dot(segment); //printf("den is %i\n",den); - if (Math::abs(den) <= CMP_EPSILON) { + if (Math::is_zero_approx(den)) { return false; } diff --git a/core/math/plane.h b/core/math/plane.h index 4eedebb79e..ec817edd2c 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -74,10 +74,11 @@ public: _FORCE_INLINE_ bool operator!=(const Plane &p_plane) const; operator String() const; - _FORCE_INLINE_ Plane() { d = 0; } + _FORCE_INLINE_ Plane() : + d(0) {} _FORCE_INLINE_ Plane(real_t p_a, real_t p_b, real_t p_c, real_t p_d) : normal(p_a, p_b, p_c), - d(p_d){}; + d(p_d) {} _FORCE_INLINE_ Plane(const Vector3 &p_normal, real_t p_d); _FORCE_INLINE_ Plane(const Vector3 &p_point, const Vector3 &p_normal); @@ -124,12 +125,12 @@ Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_ bool Plane::operator==(const Plane &p_plane) const { - return normal == p_plane.normal && d == p_plane.d; + return normal == p_plane.normal && Math::is_equal_approx(d, p_plane.d); } bool Plane::operator!=(const Plane &p_plane) const { - return normal != p_plane.normal || d != p_plane.d; + return normal != p_plane.normal || !Math::is_equal_approx(d, p_plane.d); } #endif // PLANE_H diff --git a/core/math/quat.cpp b/core/math/quat.cpp index d660ce4553..1a67be7384 100644 --- a/core/math/quat.cpp +++ b/core/math/quat.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,7 +30,7 @@ #include "quat.h" -#include "core/math/matrix3.h" +#include "core/math/basis.h" #include "core/print_string.h" // set_euler_xyz expects a vector containing the Euler angles in the format @@ -100,7 +100,7 @@ void Quat::set_euler_yxz(const Vector3 &p_euler) { // This implementation uses YXZ convention (Z is the first rotation). Vector3 Quat::get_euler_yxz() const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Vector3(0, 0, 0)); + ERR_FAIL_COND_V(!is_normalized(), Vector3(0, 0, 0)); #endif Basis m(*this); return m.get_euler_yxz(); @@ -135,20 +135,20 @@ Quat Quat::normalized() const { } bool Quat::is_normalized() const { - return Math::is_equal_approx(length_squared(), 1.0); + return Math::is_equal_approx(length_squared(), 1.0, UNIT_EPSILON); //use less epsilon } Quat Quat::inverse() const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Quat()); + ERR_FAIL_COND_V(!is_normalized(), Quat()); #endif return Quat(-x, -y, -z, w); } Quat Quat::slerp(const Quat &q, const real_t &t) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Quat()); - ERR_FAIL_COND_V(q.is_normalized() == false, Quat()); + ERR_FAIL_COND_V(!is_normalized(), Quat()); + ERR_FAIL_COND_V(!q.is_normalized(), Quat()); #endif Quat to1; real_t omega, cosom, sinom, scale0, scale1; @@ -194,8 +194,8 @@ Quat Quat::slerp(const Quat &q, const real_t &t) const { Quat Quat::slerpni(const Quat &q, const real_t &t) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Quat()); - ERR_FAIL_COND_V(q.is_normalized() == false, Quat()); + ERR_FAIL_COND_V(!is_normalized(), Quat()); + ERR_FAIL_COND_V(!q.is_normalized(), Quat()); #endif const Quat &from = *this; @@ -216,8 +216,8 @@ Quat Quat::slerpni(const Quat &q, const real_t &t) const { Quat Quat::cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Quat()); - ERR_FAIL_COND_V(q.is_normalized() == false, Quat()); + ERR_FAIL_COND_V(!is_normalized(), Quat()); + ERR_FAIL_COND_V(!q.is_normalized(), Quat()); #endif //the only way to do slerp :| real_t t2 = (1.0 - t) * t * 2; @@ -233,7 +233,7 @@ Quat::operator String() const { void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) { #ifdef MATH_CHECKS - ERR_FAIL_COND(axis.is_normalized() == false); + ERR_FAIL_COND(!axis.is_normalized()); #endif real_t d = axis.length(); if (d == 0) diff --git a/core/math/quat.h b/core/math/quat.h index 10d3846c87..3d6602e466 100644 --- a/core/math/quat.h +++ b/core/math/quat.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -38,10 +38,6 @@ #include "core/math/math_funcs.h" #include "core/ustring.h" -/** - @author Juan Linietsky <reduzio@gmail.com> -*/ - class Quat { public: real_t x, y, z, w; @@ -87,7 +83,7 @@ public: _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, v); + ERR_FAIL_COND_V(!is_normalized(), v); #endif Vector3 u(x, y, z); Vector3 uv = u.cross(v); @@ -115,20 +111,28 @@ public: z = p_z; w = p_w; } - inline Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) { - x = p_x; - y = p_y; - z = p_z; - w = p_w; + inline Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) : + x(p_x), + y(p_y), + z(p_z), + w(p_w) { } Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); } Quat(const Vector3 &euler) { set_euler(euler); } - Quat(const Quat &q) { + Quat(const Quat &q) : + x(q.x), + y(q.y), + z(q.z), + w(q.w) { + } + + Quat operator=(const Quat &q) { x = q.x; y = q.y; z = q.z; w = q.w; + return *this; } Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc @@ -153,9 +157,11 @@ public: } } - inline Quat() { - x = y = z = 0; - w = 1; + inline Quat() : + x(0), + y(0), + z(0), + w(1) { } }; diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index 23823b339a..fc2eb1454d 100644 --- a/core/math/quick_hull.cpp +++ b/core/math/quick_hull.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -36,8 +36,6 @@ uint32_t QuickHull::debug_stop_after = 0xFFFFFFFF; Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_mesh) { - static const real_t over_tolerance = 0.0001; - /* CREATE AABB VOLUME */ AABB aabb; @@ -180,6 +178,8 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me faces.push_back(f); } + real_t over_tolerance = 3 * UNIT_EPSILON * (aabb.size.x + aabb.size.y + aabb.size.z); + /* COMPUTE AVAILABLE VERTICES */ for (int i = 0; i < p_points.size(); i++) { @@ -438,12 +438,12 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me } // remove all edge connections to this face - for (Map<Edge, RetFaceConnect>::Element *E = ret_edges.front(); E; E = E->next()) { - if (E->get().left == O) - E->get().left = NULL; + for (Map<Edge, RetFaceConnect>::Element *G = ret_edges.front(); G; G = G->next()) { + if (G->get().left == O) + G->get().left = NULL; - if (E->get().right == O) - E->get().right = NULL; + if (G->get().right == O) + G->get().right = NULL; } ret_edges.erase(F); //remove the edge diff --git a/core/math/quick_hull.h b/core/math/quick_hull.h index 0ac2758323..a445a47cbe 100644 --- a/core/math/quick_hull.h +++ b/core/math/quick_hull.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -64,7 +64,7 @@ public: struct Face { Plane plane; - int vertices[3]; + uint32_t vertices[3]; Vector<int> points_over; bool operator<(const Face &p_face) const { diff --git a/core/math/random_number_generator.cpp b/core/math/random_number_generator.cpp new file mode 100644 index 0000000000..54a88d5cd8 --- /dev/null +++ b/core/math/random_number_generator.cpp @@ -0,0 +1,46 @@ +/*************************************************************************/ +/* random_number_generator.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "random_number_generator.h" + +RandomNumberGenerator::RandomNumberGenerator() {} + +void RandomNumberGenerator::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_seed", "seed"), &RandomNumberGenerator::set_seed); + ClassDB::bind_method(D_METHOD("get_seed"), &RandomNumberGenerator::get_seed); + ADD_PROPERTY(PropertyInfo(Variant::INT, "seed"), "set_seed", "get_seed"); + + ClassDB::bind_method(D_METHOD("randi"), &RandomNumberGenerator::randi); + ClassDB::bind_method(D_METHOD("randf"), &RandomNumberGenerator::randf); + ClassDB::bind_method(D_METHOD("randfn", "mean", "deviation"), &RandomNumberGenerator::randfn, DEFVAL(0.0), DEFVAL(1.0)); + ClassDB::bind_method(D_METHOD("randf_range", "from", "to"), &RandomNumberGenerator::randf_range); + ClassDB::bind_method(D_METHOD("randi_range", "from", "to"), &RandomNumberGenerator::randi_range); + ClassDB::bind_method(D_METHOD("randomize"), &RandomNumberGenerator::randomize); +} diff --git a/core/math/random_number_generator.h b/core/math/random_number_generator.h new file mode 100644 index 0000000000..9b54ea9b2e --- /dev/null +++ b/core/math/random_number_generator.h @@ -0,0 +1,71 @@ +/*************************************************************************/ +/* random_number_generator.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef RANDOM_NUMBER_GENERATOR_H +#define RANDOM_NUMBER_GENERATOR_H + +#include "core/math/random_pcg.h" +#include "core/reference.h" + +class RandomNumberGenerator : public Reference { + GDCLASS(RandomNumberGenerator, Reference); + + RandomPCG randbase; + +protected: + static void _bind_methods(); + +public: + _FORCE_INLINE_ void set_seed(uint64_t seed) { randbase.seed(seed); } + + _FORCE_INLINE_ uint64_t get_seed() { return randbase.get_seed(); } + + _FORCE_INLINE_ void randomize() { randbase.randomize(); } + + _FORCE_INLINE_ uint32_t randi() { return randbase.rand(); } + + _FORCE_INLINE_ real_t randf() { return randbase.randf(); } + + _FORCE_INLINE_ real_t randf_range(real_t from, real_t to) { return randbase.random(from, to); } + + _FORCE_INLINE_ real_t randfn(real_t mean = 0.0, real_t deviation = 1.0) { return randbase.randfn(mean, deviation); } + + _FORCE_INLINE_ int randi_range(int from, int to) { + unsigned int ret = randbase.rand(); + if (to < from) + return ret % (from - to + 1) + to; + else + return ret % (to - from + 1) + from; + } + + RandomNumberGenerator(); +}; + +#endif // RANDOM_NUMBER_GENERATOR_H diff --git a/core/math/random_pcg.cpp b/core/math/random_pcg.cpp new file mode 100644 index 0000000000..00c0af515d --- /dev/null +++ b/core/math/random_pcg.cpp @@ -0,0 +1,51 @@ +/*************************************************************************/ +/* random_pcg.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "random_pcg.h" + +#include "core/os/os.h" + +RandomPCG::RandomPCG(uint64_t p_seed, uint64_t p_inc) : + pcg(), + current_inc(p_inc) { + seed(p_seed); +} + +void RandomPCG::randomize() { + seed(OS::get_singleton()->get_ticks_usec() * pcg.state + PCG_DEFAULT_INC_64); +} + +double RandomPCG::random(double p_from, double p_to) { + return randd() * (p_to - p_from) + p_from; +} + +float RandomPCG::random(float p_from, float p_to) { + return randf() * (p_to - p_from) + p_from; +} diff --git a/core/math/random_pcg.h b/core/math/random_pcg.h new file mode 100644 index 0000000000..aa25914638 --- /dev/null +++ b/core/math/random_pcg.h @@ -0,0 +1,136 @@ +/*************************************************************************/ +/* random_pcg.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef RANDOM_PCG_H +#define RANDOM_PCG_H + +#include <math.h> + +#include "core/math/math_defs.h" + +#include "thirdparty/misc/pcg.h" + +#if defined(__GNUC__) || (_llvm_has_builtin(__builtin_clz)) +#define CLZ32(x) __builtin_clz(x) +#elif defined(_MSC_VER) +#include "intrin.h" +static int __bsr_clz32(uint32_t x) { + unsigned long index; + _BitScanReverse(&index, x); + return 31 - index; +} +#define CLZ32(x) __bsr_clz32(x) +#else +#endif + +#if defined(__GNUC__) || (_llvm_has_builtin(__builtin_ldexp) && _llvm_has_builtin(__builtin_ldexpf)) +#define LDEXP(s, e) __builtin_ldexp(s, e) +#define LDEXPF(s, e) __builtin_ldexpf(s, e) +#else +#include "math.h" +#define LDEXP(s, e) ldexp(s, e) +#define LDEXPF(s, e) ldexp(s, e) +#endif + +class RandomPCG { + pcg32_random_t pcg; + uint64_t current_seed; // seed with this to get the same state + uint64_t current_inc; + +public: + static const uint64_t DEFAULT_SEED = 12047754176567800795U; + static const uint64_t DEFAULT_INC = PCG_DEFAULT_INC_64; + static const uint64_t RANDOM_MAX = 0xFFFFFFFF; + + RandomPCG(uint64_t p_seed = DEFAULT_SEED, uint64_t p_inc = DEFAULT_INC); + + _FORCE_INLINE_ void seed(uint64_t p_seed) { + current_seed = p_seed; + pcg32_srandom_r(&pcg, current_seed, current_inc); + } + _FORCE_INLINE_ uint64_t get_seed() { return current_seed; } + + void randomize(); + _FORCE_INLINE_ uint32_t rand() { + current_seed = pcg.state; + return pcg32_random_r(&pcg); + } + + // Obtaining floating point numbers in [0, 1] range with "good enough" uniformity. + // These functions sample the output of rand() as the fraction part of an infinite binary number, + // with some tricks applied to reduce ops and branching: + // 1. Instead of shifting to the first 1 and connecting random bits, we simply set the MSB and LSB to 1. + // Provided that the RNG is actually uniform bit by bit, this should have the exact same effect. + // 2. In order to compensate for exponent info loss, we count zeros from another random number, + // and just add that to the initial offset. + // This has the same probability as counting and shifting an actual bit stream: 2^-n for n zeroes. + // For all numbers above 2^-96 (2^-64 for floats), the functions should be uniform. + // However, all numbers below that threshold are floored to 0. + // The thresholds are chosen to minimize rand() calls while keeping the numbers within a totally subjective quality standard. + // If clz or ldexp isn't available, fall back to bit truncation for performance, sacrificing uniformity. + _FORCE_INLINE_ double randd() { +#if defined(CLZ32) + uint32_t proto_exp_offset = rand(); + if (unlikely(proto_exp_offset == 0)) { + return 0; + } + uint64_t significand = (((uint64_t)rand()) << 32) | rand() | 0x8000000000000001U; + return LDEXP((double)significand, -64 - CLZ32(proto_exp_offset)); +#else +#pragma message("RandomPCG::randd - intrinsic clz is not available, falling back to bit truncation") + return (double)(((((uint64_t)rand()) << 32) | rand()) & 0x1FFFFFFFFFFFFFU) / (double)0x1FFFFFFFFFFFFFU; +#endif + } + _FORCE_INLINE_ float randf() { +#if defined(CLZ32) + uint32_t proto_exp_offset = rand(); + if (unlikely(proto_exp_offset == 0)) { + return 0; + } + return LDEXPF((float)(rand() | 0x80000001), -32 - CLZ32(proto_exp_offset)); +#else +#pragma message("RandomPCG::randf - intrinsic clz is not available, falling back to bit truncation") + return (float)(rand() & 0xFFFFFF) / (float)0xFFFFFF; +#endif + } + + _FORCE_INLINE_ double randfn(double p_mean, double p_deviation) { + return p_mean + p_deviation * (cos(Math_TAU * randd()) * sqrt(-2.0 * log(randd()))); // Box-Muller transform + } + _FORCE_INLINE_ float randfn(float p_mean, float p_deviation) { + return p_mean + p_deviation * (cos(Math_TAU * randf()) * sqrt(-2.0 * log(randf()))); // Box-Muller transform + } + + double random(double p_from, double p_to); + float random(float p_from, float p_to); + real_t random(int p_from, int p_to) { return (real_t)random((real_t)p_from, (real_t)p_to); } +}; + +#endif // RANDOM_PCG_H diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp index 24c1c8c984..fea128afbd 100644 --- a/core/math/rect2.cpp +++ b/core/math/rect2.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/rect2.h b/core/math/rect2.h index 96c0e177d3..d636aa223f 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -67,7 +67,7 @@ struct Rect2 { if (p_point.x < position.x) { real_t d = position.x - p_point.x; - dist = inside ? d : MIN(dist, d); + dist = d; inside = false; } if (p_point.y < position.y) { @@ -103,7 +103,7 @@ struct Rect2 { ((p_rect.position.y + p_rect.size.y) < (position.y + size.y)); } - inline bool has_no_area() const { + _FORCE_INLINE_ bool has_no_area() const { return (size.x <= 0 || size.y <= 0); } @@ -154,8 +154,6 @@ struct Rect2 { return true; } - inline bool no_area() const { return (size.width <= 0 || size.height <= 0); } - bool operator==(const Rect2 &p_rect) const { return position == p_rect.position && size == p_rect.size; } bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; } @@ -189,7 +187,7 @@ struct Rect2 { return g; } - inline Rect2 expand(const Vector2 &p_vector) const { + _FORCE_INLINE_ Rect2 expand(const Vector2 &p_vector) const { Rect2 r = *this; r.expand_to(p_vector); @@ -215,7 +213,7 @@ struct Rect2 { size = end - begin; } - inline Rect2 abs() const { + _FORCE_INLINE_ Rect2 abs() const { return Rect2(Point2(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); } @@ -265,7 +263,7 @@ struct Rect2i { ((p_rect.position.y + p_rect.size.y) < (position.y + size.y)); } - inline bool has_no_area() const { + _FORCE_INLINE_ bool has_no_area() const { return (size.x <= 0 || size.y <= 0); } @@ -316,8 +314,6 @@ struct Rect2i { return true; } - bool no_area() { return (size.width <= 0 || size.height <= 0); } - bool operator==(const Rect2i &p_rect) const { return position == p_rect.position && size == p_rect.size; } bool operator!=(const Rect2i &p_rect) const { return position != p_rect.position || size != p_rect.size; } @@ -331,6 +327,33 @@ struct Rect2i { return g; } + inline Rect2i grow_margin(Margin p_margin, int p_amount) const { + Rect2i g = *this; + g = g.grow_individual((MARGIN_LEFT == p_margin) ? p_amount : 0, + (MARGIN_TOP == p_margin) ? p_amount : 0, + (MARGIN_RIGHT == p_margin) ? p_amount : 0, + (MARGIN_BOTTOM == p_margin) ? p_amount : 0); + return g; + } + + inline Rect2i grow_individual(int p_left, int p_top, int p_right, int p_bottom) const { + + Rect2i g = *this; + g.position.x -= p_left; + g.position.y -= p_top; + g.size.width += p_left + p_right; + g.size.height += p_top + p_bottom; + + return g; + } + + _FORCE_INLINE_ Rect2i expand(const Vector2i &p_vector) const { + + Rect2i r = *this; + r.expand_to(p_vector); + return r; + } + inline void expand_to(const Point2i &p_vector) { Point2i begin = position; diff --git a/core/math/transform.cpp b/core/math/transform.cpp index 75257a6e60..4056975da8 100644 --- a/core/math/transform.cpp +++ b/core/math/transform.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -213,3 +213,8 @@ Transform::Transform(const Basis &p_basis, const Vector3 &p_origin) : basis(p_basis), origin(p_origin) { } + +Transform::Transform(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t ox, real_t oy, real_t oz) { + basis = Basis(xx, xy, xz, yx, yy, yz, zx, zy, zz); + origin = Vector3(ox, oy, oz); +} diff --git a/core/math/transform.h b/core/math/transform.h index 97c8bf9ab0..90e2b07583 100644 --- a/core/math/transform.h +++ b/core/math/transform.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,12 +32,9 @@ #define TRANSFORM_H #include "core/math/aabb.h" -#include "core/math/matrix3.h" +#include "core/math/basis.h" #include "core/math/plane.h" - -/** - @author Juan Linietsky <reduzio@gmail.com> -*/ +#include "core/pool_vector.h" class Transform { public: @@ -86,6 +83,9 @@ public: _FORCE_INLINE_ AABB xform(const AABB &p_aabb) const; _FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const; + _FORCE_INLINE_ PoolVector<Vector3> xform(const PoolVector<Vector3> &p_array) const; + _FORCE_INLINE_ PoolVector<Vector3> xform_inv(const PoolVector<Vector3> &p_array) const; + void operator*=(const Transform &p_transform); Transform operator*(const Transform &p_transform) const; @@ -108,6 +108,7 @@ public: operator String() const; + Transform(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t ox, real_t oy, real_t oz); Transform(const Basis &p_basis, const Vector3 &p_origin = Vector3()); Transform() {} }; @@ -157,22 +158,29 @@ _FORCE_INLINE_ Plane Transform::xform_inv(const Plane &p_plane) const { } _FORCE_INLINE_ AABB Transform::xform(const AABB &p_aabb) const { - /* define vertices */ - Vector3 x = basis.get_axis(0) * p_aabb.size.x; - Vector3 y = basis.get_axis(1) * p_aabb.size.y; - Vector3 z = basis.get_axis(2) * p_aabb.size.z; - Vector3 pos = xform(p_aabb.position); - //could be even further optimized - AABB new_aabb; - new_aabb.position = pos; - new_aabb.expand_to(pos + x); - new_aabb.expand_to(pos + y); - new_aabb.expand_to(pos + z); - new_aabb.expand_to(pos + x + y); - new_aabb.expand_to(pos + x + z); - new_aabb.expand_to(pos + y + z); - new_aabb.expand_to(pos + x + y + z); - return new_aabb; + + /* http://dev.theomader.com/transform-bounding-boxes/ */ + Vector3 min = p_aabb.position; + Vector3 max = p_aabb.position + p_aabb.size; + Vector3 tmin, tmax; + for (int i = 0; i < 3; i++) { + tmin[i] = tmax[i] = origin[i]; + for (int j = 0; j < 3; j++) { + real_t e = basis[i][j] * min[j]; + real_t f = basis[i][j] * max[j]; + if (e < f) { + tmin[i] += e; + tmax[i] += f; + } else { + tmin[i] += f; + tmax[i] += e; + } + } + } + AABB r_aabb; + r_aabb.position = tmin; + r_aabb.size = tmax - tmin; + return r_aabb; } _FORCE_INLINE_ AABB Transform::xform_inv(const AABB &p_aabb) const { @@ -201,4 +209,32 @@ _FORCE_INLINE_ AABB Transform::xform_inv(const AABB &p_aabb) const { return ret; } +PoolVector<Vector3> Transform::xform(const PoolVector<Vector3> &p_array) const { + + PoolVector<Vector3> array; + array.resize(p_array.size()); + + PoolVector<Vector3>::Read r = p_array.read(); + PoolVector<Vector3>::Write w = array.write(); + + for (int i = 0; i < p_array.size(); ++i) { + w[i] = xform(r[i]); + } + return array; +} + +PoolVector<Vector3> Transform::xform_inv(const PoolVector<Vector3> &p_array) const { + + PoolVector<Vector3> array; + array.resize(p_array.size()); + + PoolVector<Vector3>::Read r = p_array.read(); + PoolVector<Vector3>::Write w = array.write(); + + for (int i = 0; i < p_array.size(); ++i) { + w[i] = xform_inv(r[i]); + } + return array; +} + #endif // TRANSFORM_H diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp index 4bb763c879..1d0387bd45 100644 --- a/core/math/transform_2d.cpp +++ b/core/math/transform_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -80,13 +80,14 @@ real_t Transform2D::get_rotation() const { } void Transform2D::set_rotation(real_t p_rot) { - + Size2 scale = get_scale(); real_t cr = Math::cos(p_rot); real_t sr = Math::sin(p_rot); elements[0][0] = cr; elements[0][1] = sr; elements[1][0] = -sr; elements[1][1] = cr; + set_scale(scale); } Transform2D::Transform2D(real_t p_rot, const Vector2 &p_pos) { @@ -101,10 +102,17 @@ Transform2D::Transform2D(real_t p_rot, const Vector2 &p_pos) { } Size2 Transform2D::get_scale() const { - real_t det_sign = basis_determinant() > 0 ? 1 : -1; + real_t det_sign = SGN(basis_determinant()); return Size2(elements[0].length(), det_sign * elements[1].length()); } +void Transform2D::set_scale(const Size2 &p_scale) { + elements[0].normalize(); + elements[1].normalize(); + elements[0] *= p_scale.x; + elements[1] *= p_scale.y; +} + void Transform2D::scale(const Size2 &p_scale) { scale_basis(p_scale); elements[2] *= p_scale; diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h index c8fc3c39e3..e8b44ab197 100644 --- a/core/math/transform_2d.h +++ b/core/math/transform_2d.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -32,6 +32,7 @@ #define TRANSFORM_2D_H #include "core/math/rect2.h" // also includes vector2, math_funcs, and ustring +#include "core/pool_vector.h" struct Transform2D { // Warning #1: basis of Transform2D is stored differently from Basis. In terms of elements array, the basis matrix looks like "on paper": @@ -81,6 +82,7 @@ struct Transform2D { real_t basis_determinant() const; Size2 get_scale() const; + void set_scale(const Size2 &p_scale); _FORCE_INLINE_ const Vector2 &get_origin() const { return elements[2]; } _FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { elements[2] = p_origin; } @@ -109,6 +111,8 @@ struct Transform2D { _FORCE_INLINE_ Vector2 xform_inv(const Vector2 &p_vec) const; _FORCE_INLINE_ Rect2 xform(const Rect2 &p_rect) const; _FORCE_INLINE_ Rect2 xform_inv(const Rect2 &p_rect) const; + _FORCE_INLINE_ PoolVector<Vector2> xform(const PoolVector<Vector2> &p_array) const; + _FORCE_INLINE_ PoolVector<Vector2> xform_inv(const PoolVector<Vector2> &p_array) const; operator String() const; @@ -198,4 +202,32 @@ Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const { return new_rect; } +PoolVector<Vector2> Transform2D::xform(const PoolVector<Vector2> &p_array) const { + + PoolVector<Vector2> array; + array.resize(p_array.size()); + + PoolVector<Vector2>::Read r = p_array.read(); + PoolVector<Vector2>::Write w = array.write(); + + for (int i = 0; i < p_array.size(); ++i) { + w[i] = xform(r[i]); + } + return array; +} + +PoolVector<Vector2> Transform2D::xform_inv(const PoolVector<Vector2> &p_array) const { + + PoolVector<Vector2> array; + array.resize(p_array.size()); + + PoolVector<Vector2>::Read r = p_array.read(); + PoolVector<Vector2>::Write w = array.write(); + + for (int i = 0; i < p_array.size(); ++i) { + w[i] = xform_inv(r[i]); + } + return array; +} + #endif // TRANSFORM_2D_H diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 6b8dc5eeb3..546981be44 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,7 +30,7 @@ #include "triangle_mesh.h" -#include "core/sort.h" +#include "core/sort_array.h" int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &max_depth, int &max_alloc) { @@ -182,7 +182,7 @@ void TriangleMesh::create(const PoolVector<Vector3> &p_faces) { int max_alloc = fc; _create_bvh(bw.ptr(), bwp.ptr(), 0, fc, 1, max_depth, max_alloc); - bw = PoolVector<BVH>::Write(); //clearup + bw.release(); //clearup bvh.resize(max_alloc); //resize back valid = true; @@ -751,7 +751,7 @@ PoolVector<Face3> TriangleMesh::get_faces() const { } } - w = PoolVector<Face3>::Write(); + w.release(); return faces; } diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index e5f181fba7..8b01080852 100644 --- a/core/math/triangle_mesh.h +++ b/core/math/triangle_mesh.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -97,7 +97,7 @@ public: PoolVector<Triangle> get_triangles() const { return triangles; } PoolVector<Vector3> get_vertices() const { return vertices; } - void get_indices(PoolVector<int> *p_triangles_indices) const; + void get_indices(PoolVector<int> *r_triangles_indices) const; void create(const PoolVector<Vector3> &p_faces); TriangleMesh(); diff --git a/core/math/triangulate.cpp b/core/math/triangulate.cpp index 69ffc95946..be409e62a7 100644 --- a/core/math/triangulate.cpp +++ b/core/math/triangulate.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/triangulate.h b/core/math/triangulate.h index 2b0557ee55..2437e2b0f0 100644 --- a/core/math/triangulate.h +++ b/core/math/triangulate.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp index 84c9f0fca6..972bccc0ac 100644 --- a/core/math/vector2.cpp +++ b/core/math/vector2.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -65,7 +65,7 @@ Vector2 Vector2::normalized() const { bool Vector2::is_normalized() const { // use length_squared() instead of length() to avoid sqrt(), makes it more stringent. - return Math::is_equal_approx(length_squared(), 1.0); + return Math::is_equal_approx(length_squared(), 1.0, UNIT_EPSILON); } real_t Vector2::distance_to(const Vector2 &p_vector2) const { @@ -98,6 +98,11 @@ real_t Vector2::cross(const Vector2 &p_other) const { return x * p_other.y - y * p_other.x; } +Vector2 Vector2::sign() const { + + return Vector2(SGN(x), SGN(y)); +} + Vector2 Vector2::floor() const { return Vector2(Math::floor(x), Math::floor(y)); @@ -121,6 +126,14 @@ Vector2 Vector2::rotated(real_t p_by) const { return v; } +Vector2 Vector2::posmod(const real_t p_mod) const { + return Vector2(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod)); +} + +Vector2 Vector2::posmodv(const Vector2 &p_modv) const { + return Vector2(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y)); +} + Vector2 Vector2::project(const Vector2 &p_b) const { return p_b * (dot(p_b) / p_b.length_squared()); } @@ -164,10 +177,17 @@ Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, c return out; } +Vector2 Vector2::move_toward(const Vector2 &p_to, const real_t p_delta) const { + Vector2 v = *this; + Vector2 vd = p_to - v; + real_t len = vd.length(); + return len <= p_delta || len < CMP_EPSILON ? p_to : v + vd / len * p_delta; +} + // slide returns the component of the vector along the given plane, specified by its normal vector. Vector2 Vector2::slide(const Vector2 &p_normal) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(p_normal.is_normalized() == false, Vector2()); + ERR_FAIL_COND_V(!p_normal.is_normalized(), Vector2()); #endif return *this - p_normal * this->dot(p_normal); } @@ -178,7 +198,7 @@ Vector2 Vector2::bounce(const Vector2 &p_normal) const { Vector2 Vector2::reflect(const Vector2 &p_normal) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(p_normal.is_normalized() == false, Vector2()); + ERR_FAIL_COND_V(!p_normal.is_normalized(), Vector2()); #endif return 2.0 * p_normal * this->dot(p_normal) - *this; } diff --git a/core/math/vector2.h b/core/math/vector2.h index df49484aaf..1a73831891 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -38,6 +38,11 @@ struct Vector2i; struct Vector2 { + enum Axis { + AXIS_X, + AXIS_Y, + }; + union { real_t x; real_t width; @@ -65,9 +70,12 @@ struct Vector2 { real_t distance_squared_to(const Vector2 &p_vector2) const; real_t angle_to(const Vector2 &p_vector2) const; real_t angle_to_point(const Vector2 &p_vector2) const; + _FORCE_INLINE_ Vector2 direction_to(const Vector2 &p_b) const; real_t dot(const Vector2 &p_other) const; real_t cross(const Vector2 &p_other) const; + Vector2 posmod(const real_t p_mod) const; + Vector2 posmodv(const Vector2 &p_modv) const; Vector2 project(const Vector2 &p_b) const; Vector2 plane_project(real_t p_d, const Vector2 &p_vec) const; @@ -78,6 +86,7 @@ struct Vector2 { _FORCE_INLINE_ Vector2 linear_interpolate(const Vector2 &p_b, real_t p_t) const; _FORCE_INLINE_ Vector2 slerp(const Vector2 &p_b, real_t p_t) const; Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_t) const; + Vector2 move_toward(const Vector2 &p_to, const real_t p_delta) const; Vector2 slide(const Vector2 &p_normal) const; Vector2 bounce(const Vector2 &p_normal) const; @@ -98,14 +107,17 @@ struct Vector2 { Vector2 operator/(const real_t &rvalue) const; void operator/=(const real_t &rvalue); + void operator/=(const Vector2 &rvalue) { *this = *this / rvalue; } Vector2 operator-() const; bool operator==(const Vector2 &p_vec2) const; bool operator!=(const Vector2 &p_vec2) const; - bool operator<(const Vector2 &p_vec2) const { return (x == p_vec2.x) ? (y < p_vec2.y) : (x < p_vec2.x); } - bool operator<=(const Vector2 &p_vec2) const { return (x == p_vec2.x) ? (y <= p_vec2.y) : (x <= p_vec2.x); } + bool operator<(const Vector2 &p_vec2) const { return Math::is_equal_approx(x, p_vec2.x) ? (y < p_vec2.y) : (x < p_vec2.x); } + bool operator>(const Vector2 &p_vec2) const { return Math::is_equal_approx(x, p_vec2.x) ? (y > p_vec2.y) : (x > p_vec2.x); } + bool operator<=(const Vector2 &p_vec2) const { return Math::is_equal_approx(x, p_vec2.x) ? (y <= p_vec2.y) : (x < p_vec2.x); } + bool operator>=(const Vector2 &p_vec2) const { return Math::is_equal_approx(x, p_vec2.x) ? (y >= p_vec2.y) : (x > p_vec2.x); } real_t angle() const; @@ -126,6 +138,7 @@ struct Vector2 { return Vector2(y, -x); } + Vector2 sign() const; Vector2 floor() const; Vector2 ceil() const; Vector2 round() const; @@ -138,10 +151,7 @@ struct Vector2 { x = p_x; y = p_y; } - _FORCE_INLINE_ Vector2() { - x = 0; - y = 0; - } + _FORCE_INLINE_ Vector2() { x = y = 0; } }; _FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2 &p_vec) const { @@ -211,11 +221,11 @@ _FORCE_INLINE_ Vector2 Vector2::operator-() const { _FORCE_INLINE_ bool Vector2::operator==(const Vector2 &p_vec2) const { - return x == p_vec2.x && y == p_vec2.y; + return Math::is_equal_approx(x, p_vec2.x) && Math::is_equal_approx(y, p_vec2.y); } _FORCE_INLINE_ bool Vector2::operator!=(const Vector2 &p_vec2) const { - return x != p_vec2.x || y != p_vec2.y; + return !Math::is_equal_approx(x, p_vec2.x) || !Math::is_equal_approx(y, p_vec2.y); } Vector2 Vector2::linear_interpolate(const Vector2 &p_b, real_t p_t) const { @@ -230,12 +240,18 @@ Vector2 Vector2::linear_interpolate(const Vector2 &p_b, real_t p_t) const { Vector2 Vector2::slerp(const Vector2 &p_b, real_t p_t) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Vector2()); + ERR_FAIL_COND_V(!is_normalized(), Vector2()); #endif real_t theta = angle_to(p_b); return rotated(theta * p_t); } +Vector2 Vector2::direction_to(const Vector2 &p_b) const { + Vector2 ret(p_b.x - x, p_b.y - y); + ret.normalize(); + return ret; +} + Vector2 Vector2::linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t) { Vector2 res = p_a; @@ -253,6 +269,11 @@ typedef Vector2 Point2; struct Vector2i { + enum Axis { + AXIS_X, + AXIS_Y, + }; + union { int x; int width; diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index 5dbb01493d..73927821cf 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,7 +30,7 @@ #include "vector3.h" -#include "core/math/matrix3.h" +#include "core/math/basis.h" void Vector3::rotate(const Vector3 &p_axis, real_t p_phi) { @@ -127,6 +127,13 @@ Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, c return out; } +Vector3 Vector3::move_toward(const Vector3 &p_to, const real_t p_delta) const { + Vector3 v = *this; + Vector3 vd = p_to - v; + real_t len = vd.length(); + return len <= p_delta || len < CMP_EPSILON ? p_to : v + vd / len * p_delta; +} + Vector3::operator String() const { return (rtos(x) + ", " + rtos(y) + ", " + rtos(z)); diff --git a/core/math/vector3.h b/core/math/vector3.h index 5302832eeb..c68b075613 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2018 Godot Engine contributors (cf. AUTHORS.md) */ +/* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,9 +31,7 @@ #ifndef VECTOR3_H #define VECTOR3_H -#include "core/math/math_defs.h" #include "core/math/math_funcs.h" -#include "core/typedefs.h" #include "core/ustring.h" class Basis; @@ -94,6 +92,7 @@ struct Vector3 { _FORCE_INLINE_ Vector3 slerp(const Vector3 &p_b, real_t p_t) const; Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const; Vector3 cubic_interpolaten(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const; + Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const; _FORCE_INLINE_ Vector3 cross(const Vector3 &p_b) const; _FORCE_INLINE_ real_t dot(const Vector3 &p_b) const; @@ -109,9 +108,12 @@ struct Vector3 { _FORCE_INLINE_ real_t distance_to(const Vector3 &p_b) const; _FORCE_INLINE_ real_t distance_squared_to(const Vector3 &p_b) const; + _FORCE_INLINE_ Vector3 posmod(const real_t p_mod) const; + _FORCE_INLINE_ Vector3 posmodv(const Vector3 &p_modv) const; _FORCE_INLINE_ Vector3 project(const Vector3 &p_b) const; _FORCE_INLINE_ real_t angle_to(const Vector3 &p_b) const; + _FORCE_INLINE_ Vector3 direction_to(const Vector3 &p_b) const; _FORCE_INLINE_ Vector3 slide(const Vector3 &p_normal) const; _FORCE_INLINE_ Vector3 bounce(const Vector3 &p_normal) const; @@ -139,19 +141,21 @@ struct Vector3 { _FORCE_INLINE_ bool operator!=(const Vector3 &p_v) const; _FORCE_INLINE_ bool operator<(const Vector3 &p_v) const; _FORCE_INLINE_ bool operator<=(const Vector3 &p_v) const; + _FORCE_INLINE_ bool operator>(const Vector3 &p_v) const; + _FORCE_INLINE_ bool operator>=(const Vector3 &p_v) const; operator String() const; - _FORCE_INLINE_ Vector3() { x = y = z = 0; } _FORCE_INLINE_ Vector3(real_t p_x, real_t p_y, real_t p_z) { x = p_x; y = p_y; z = p_z; } + _FORCE_INLINE_ Vector3() { x = y = z = 0; } }; // Should be included after class definition, otherwise we get circular refs -#include "core/math/matrix3.h" +#include "core/math/basis.h" Vector3 Vector3::cross(const Vector3 &p_b) const { @@ -217,12 +221,8 @@ Vector3 Vector3::linear_interpolate(const Vector3 &p_b, real_t p_t) const { } Vector3 Vector3::slerp(const Vector3 &p_b, real_t p_t) const { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V(is_normalized() == false, Vector3()); -#endif - real_t theta = angle_to(p_b); - return rotated(cross(p_b), theta * p_t); + return rotated(cross(p_b).normalized(), theta * p_t); } real_t Vector3::distance_to(const Vector3 &p_b) const { @@ -235,6 +235,14 @@ real_t Vector3::distance_squared_to(const Vector3 &p_b) const { return (p_b - *this).length_squared(); } +Vector3 Vector3::posmod(const real_t p_mod) const { + return Vector3(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod), Math::fposmod(z, p_mod)); +} + +Vector3 Vector3::posmodv(const Vector3 &p_modv) const { + return Vector3(Math::fposmod(x, p_modv.x), Math::fposmod(y, p_modv.y), Math::fposmod(z, p_modv.z)); +} + Vector3 Vector3::project(const Vector3 &p_b) const { return p_b * (dot(p_b) / p_b.length_squared()); } @@ -244,6 +252,12 @@ real_t Vector3::angle_to(const Vector3 &p_b) const { return Math::atan2(cross(p_b).length(), dot(p_b)); } +Vector3 Vector3::direction_to(const Vector3 &p_b) const { + Vector3 ret(p_b.x - x, p_b.y - y, p_b.z - z); + ret.normalize(); + return ret; +} + /* Operators */ Vector3 &Vector3::operator+=(const Vector3 &p_v) { @@ -334,17 +348,17 @@ Vector3 Vector3::operator-() const { bool Vector3::operator==(const Vector3 &p_v) const { - return (x == p_v.x && y == p_v.y && z == p_v.z); + return (Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y) && Math::is_equal_approx(z, p_v.z)); } bool Vector3::operator!=(const Vector3 &p_v) const { - return (x != p_v.x || y != p_v.y || z != p_v.z); + return (!Math::is_equal_approx(x, p_v.x) || !Math::is_equal_approx(y, p_v.y) || !Math::is_equal_approx(z, p_v.z)); } bool Vector3::operator<(const Vector3 &p_v) const { - if (x == p_v.x) { - if (y == p_v.y) + if (Math::is_equal_approx(x, p_v.x)) { + if (Math::is_equal_approx(y, p_v.y)) return z < p_v.z; else return y < p_v.y; @@ -353,10 +367,22 @@ bool Vector3::operator<(const Vector3 &p_v) const { } } +bool Vector3::operator>(const Vector3 &p_v) const { + + if (Math::is_equal_approx(x, p_v.x)) { + if (Math::is_equal_approx(y, p_v.y)) + return z > p_v.z; + else + return y > p_v.y; + } else { + return x > p_v.x; + } +} + bool Vector3::operator<=(const Vector3 &p_v) const { - if (x == p_v.x) { - if (y == p_v.y) + if (Math::is_equal_approx(x, p_v.x)) { + if (Math::is_equal_approx(y, p_v.y)) return z <= p_v.z; else return y < p_v.y; @@ -365,6 +391,18 @@ bool Vector3::operator<=(const Vector3 &p_v) const { } } +bool Vector3::operator>=(const Vector3 &p_v) const { + + if (Math::is_equal_approx(x, p_v.x)) { + if (Math::is_equal_approx(y, p_v.y)) + return z >= p_v.z; + else + return y > p_v.y; + } else { + return x > p_v.x; + } +} + _FORCE_INLINE_ Vector3 vec3_cross(const Vector3 &p_a, const Vector3 &p_b) { return p_a.cross(p_b); @@ -395,13 +433,14 @@ real_t Vector3::length_squared() const { void Vector3::normalize() { - real_t l = length(); - if (l == 0) { + real_t lengthsq = length_squared(); + if (lengthsq == 0) { x = y = z = 0; } else { - x /= l; - y /= l; - z /= l; + real_t length = Math::sqrt(lengthsq); + x /= length; + y /= length; + z /= length; } } @@ -414,7 +453,7 @@ Vector3 Vector3::normalized() const { bool Vector3::is_normalized() const { // use length_squared() instead of length() to avoid sqrt(), makes it more stringent. - return Math::is_equal_approx(length_squared(), 1.0); + return Math::is_equal_approx(length_squared(), 1.0, UNIT_EPSILON); } Vector3 Vector3::inverse() const { @@ -430,7 +469,7 @@ void Vector3::zero() { // slide returns the component of the vector along the given plane, specified by its normal vector. Vector3 Vector3::slide(const Vector3 &p_normal) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(p_normal.is_normalized() == false, Vector3()); + ERR_FAIL_COND_V(!p_normal.is_normalized(), Vector3()); #endif return *this - p_normal * this->dot(p_normal); } @@ -441,7 +480,7 @@ Vector3 Vector3::bounce(const Vector3 &p_normal) const { Vector3 Vector3::reflect(const Vector3 &p_normal) const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V(p_normal.is_normalized() == false, Vector3()); + ERR_FAIL_COND_V(!p_normal.is_normalized(), Vector3()); #endif return 2.0 * p_normal * this->dot(p_normal) - *this; } |