diff options
Diffstat (limited to 'core/math')
-rw-r--r-- | core/math/a_star.cpp | 91 | ||||
-rw-r--r-- | core/math/a_star.h | 22 | ||||
-rw-r--r-- | core/math/basis.cpp | 29 | ||||
-rw-r--r-- | core/math/expression.cpp | 7 | ||||
-rw-r--r-- | core/math/expression.h | 1 | ||||
-rw-r--r-- | core/math/geometry.cpp | 106 | ||||
-rw-r--r-- | core/math/geometry.h | 93 | ||||
-rw-r--r-- | core/math/math_funcs.h | 6 | ||||
-rw-r--r-- | core/math/rect2.h | 41 |
9 files changed, 308 insertions, 88 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index e1388ad2ac..3d71e66f80 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -54,7 +54,8 @@ void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { pt->pos = p_pos; pt->weight_scale = p_weight_scale; pt->prev_point = NULL; - pt->last_pass = 0; + pt->open_pass = 0; + pt->closed_pass = 0; pt->enabled = true; points[p_id] = pt; } else { @@ -246,86 +247,62 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { if (!end_point->enabled) return false; - SelfList<Point>::List open_list; - bool found_route = false; - for (Set<Point *>::Element *E = begin_point->neighbours.front(); E; E = E->next()) { - - Point *n = E->get(); + Vector<Point *> open_list; + SortArray<Point *, SortPoints> sorter; - if (!n->enabled) - continue; + begin_point->g_score = 0; + begin_point->f_score = _estimate_cost(begin_point->id, end_point->id); - 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); - } + open_list.push_back(begin_point); while (true) { - if (open_list.first() == NULL) { - // No path found + if (open_list.size() == 0) // No path found break; - } - // Check open list - - SelfList<Point> *least_cost_point = open_list.first(); - real_t least_cost = Math_INF; - // TODO: Cache previous results - for (SelfList<Point> *E = open_list.first(); E; E = E->next()) { - - Point *p = E->self(); - - real_t cost = p->distance; - cost += _estimate_cost(p->id, end_point->id); - - if (cost < least_cost) { - least_cost_point = E; - least_cost = cost; - } - } + Point *p = open_list[0]; // The currently processed point - Point *p = least_cost_point->self(); if (p == end_point) { found_route = true; break; } + 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 + for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) { - Point *e = E->get(); + Point *e = E->get(); // The neighbour point - if (!e->enabled) + if (!e->enabled || e->closed_pass == pass) continue; - real_t distance = _compute_cost(p->id, e->id) * e->weight_scale + p->distance; + real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale; + + 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 - if (e->distance > distance) { - e->prev_point = p; - e->distance = distance; - } - } else { - // Add to open neighbours + 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 - e->prev_point = p; - e->distance = distance; - e->last_pass = pass; // Mark as used - open_list.add(&e->list); + continue; } - } - open_list.remove(least_cost_point); - } + e->prev_point = p; + e->g_score = tentative_g_score; + e->f_score = e->g_score + _estimate_cost(e->id, end_point->id); - // Clear the openf list - while (open_list.first()) { - open_list.remove(open_list.first()); + 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 + sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptrw()); + } } return found_route; @@ -352,8 +329,6 @@ 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 = points[p_from_id]; Point *b = points[p_to_id]; @@ -403,8 +378,6 @@ 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 = points[p_from_id]; Point *b = points[p_to_id]; diff --git a/core/math/a_star.h b/core/math/a_star.h index c63e1aa4dc..fac8a9d312 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -48,26 +48,34 @@ class AStar : public Reference { struct Point { - SelfList<Point> list; - int id; Vector3 pos; real_t weight_scale; - uint64_t last_pass; bool enabled; Set<Point *> neighbours; // 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 { struct { diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 9fcecd1ba6..1540bc8fe1 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -813,21 +813,28 @@ void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) { ERR_FAIL_COND(!p_axis.is_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); - real_t sine = Math::sin(p_phi); - 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; - - 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; - - 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); + + real_t sine = Math::sin(p_phi); + real_t t = 1 - cosine; + + 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; + + xyzt = p_axis.x * p_axis.z * t; + zyxs = p_axis.y * sine; + elements[0][2] = xyzt + zyxs; + elements[2][0] = xyzt - zyxs; + + 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) { diff --git a/core/math/expression.cpp b/core/math/expression.cpp index 133dcc7ab9..079c9b524f 100644 --- a/core/math/expression.cpp +++ b/core/math/expression.cpp @@ -64,6 +64,7 @@ const char *Expression::func_name[Expression::FUNC_MAX] = { "is_inf", "ease", "decimals", + "step_decimals", "stepify", "lerp", "inverse_lerp", @@ -149,6 +150,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: @@ -365,6 +367,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); diff --git a/core/math/expression.h b/core/math/expression.h index f9075cb689..f20619f0b6 100644 --- a/core/math/expression.h +++ b/core/math/expression.h @@ -62,6 +62,7 @@ public: MATH_ISINF, MATH_EASE, MATH_DECIMALS, + MATH_STEP_DECIMALS, MATH_STEPIFY, MATH_LERP, MATH_INVERSE_LERP, diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp index 0ab8707d3a..8314cb827c 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry.cpp @@ -31,8 +31,11 @@ #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) { @@ -1134,3 +1137,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 f3a671aa9a..0e144e491f 100644 --- a/core/math/geometry.h +++ b/core/math/geometry.h @@ -31,6 +31,7 @@ #ifndef GEOMETRY_H #define GEOMETRY_H +#include "core/math/delaunay.h" #include "core/math/face3.h" #include "core/math/rect2.h" #include "core/math/triangulate.h" @@ -785,6 +786,91 @@ 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_EXPLAIN("Attempt to offset a polyline like a polygon (use offset_polygon_2d instead)."); + ERR_FAIL_COND_V(p_end_type == END_POLYGON, Vector<Vector<Point2> >()); + + return _polypath_offset(p_polygon, p_delta, p_join_type, p_end_type); + } + + static Vector<Point2> transform_points_2d(const Vector<Point2> &p_points, const Transform2D &p_mat) { + + Vector<Point2> points; + + for (int i = 0; i < p_points.size(); ++i) { + points.push_back(p_mat.xform(p_points[i])); + } + return points; + } + + 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; @@ -833,7 +919,7 @@ public: further_away_opposite.y = MIN(p[i].y, further_away_opposite.y); } - further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312); // make point outside that wont intersect with points in segment from p_point + further_away += (further_away - further_away_opposite) * Vector2(1.221313, 1.512312); // make point outside that won't intersect with points in segment from p_point int intersections = 0; for (int i = 0; i < c; i++) { @@ -951,7 +1037,6 @@ 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); @@ -961,6 +1046,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_funcs.h b/core/math/math_funcs.h index a75f2fb4ab..82b5b56c01 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -61,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); } diff --git a/core/math/rect2.h b/core/math/rect2.h index 901d372132..d636aa223f 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -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; |