diff options
Diffstat (limited to 'core/math')
33 files changed, 4629 insertions, 1443 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 6908d7831d..021391da83 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -96,11 +96,11 @@ void AStar::remove_point(int p_id) { Point *p = points[p_id]; - for (int i = 0; i < p->neighbours.size(); i++) { + for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) { - Segment s(p_id, p->neighbours[i]->id); + Segment s(p_id, E->get()->id); segments.erase(s); - p->neighbours[i]->neighbours.erase(p); + E->get()->neighbours.erase(p); } memdelete(p); @@ -115,10 +115,10 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { Point *a = points[p_id]; Point *b = points[p_with_id]; - a->neighbours.push_back(b); + a->neighbours.insert(b); if (bidirectional) - b->neighbours.push_back(a); + b->neighbours.insert(a); Segment s(p_id, p_with_id); if (s.from == p_id) { @@ -168,8 +168,8 @@ PoolVector<int> AStar::get_point_connections(int p_id) { Point *p = points[p_id]; - for (int i = 0; i < p->neighbours.size(); i++) { - point_list.push_back(p->neighbours[i]->id); + for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) { + point_list.push_back(E->get()->id); } return point_list; @@ -242,9 +242,9 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { bool found_route = false; - for (int i = 0; i < begin_point->neighbours.size(); i++) { + for (Set<Point *>::Element *E = begin_point->neighbours.front(); E; E = E->next()) { - Point *n = begin_point->neighbours[i]; + 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; @@ -283,12 +283,10 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { } Point *p = least_cost_point->self(); - // Open the neighbours for search - int es = p->neighbours.size(); - for (int i = 0; i < es; i++) { + for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) { - Point *e = p->neighbours[i]; + Point *e = E->get(); real_t distance = _compute_cost(p->id, e->id) * e->weight_scale + p->distance; diff --git a/core/math/a_star.h b/core/math/a_star.h index f89e17c7bb..8c1b5f64cb 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -54,7 +54,7 @@ class AStar : public Reference { real_t weight_scale; uint64_t last_pass; - Vector<Point *> neighbours; + Set<Point *> neighbours; // Used for pathfinding Point *prev_point; diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index cff19f990c..e2e71dda92 100644 --- a/core/math/aabb.cpp +++ b/core/math/aabb.cpp @@ -245,7 +245,6 @@ Vector3 AABB::get_longest_axis() const { if (size.z > max_size) { axis = Vector3(0, 0, 1); - max_size = size.z; } return axis; @@ -262,7 +261,6 @@ int AABB::get_longest_axis_index() const { if (size.z > max_size) { axis = 2; - max_size = size.z; } return axis; @@ -280,7 +278,6 @@ Vector3 AABB::get_shortest_axis() const { if (size.z < max_size) { axis = Vector3(0, 0, 1); - max_size = size.z; } return axis; @@ -297,7 +294,6 @@ int AABB::get_shortest_axis_index() const { if (size.z < max_size) { axis = 2; - max_size = size.z; } return axis; diff --git a/core/math/aabb.h b/core/math/aabb.h index 39b8f403e7..cdb8eb48a3 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -76,6 +76,7 @@ public: _FORCE_INLINE_ bool smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t t0, real_t t1) const; _FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_planes, int p_plane_count) const; + _FORCE_INLINE_ bool inside_convex_shape(const Plane *p_planes, int p_plane_count) const; bool intersects_plane(const Plane &p_plane) const; _FORCE_INLINE_ bool has_point(const Vector3 &p_point) const; @@ -207,6 +208,25 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count) con return true; } +bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const { + + Vector3 half_extents = size * 0.5; + Vector3 ofs = position + half_extents; + + for (int i = 0; i < p_plane_count; i++) { + const Plane &p = p_planes[i]; + Vector3 point( + (p.normal.x < 0) ? -half_extents.x : half_extents.x, + (p.normal.y < 0) ? -half_extents.y : half_extents.y, + (p.normal.z < 0) ? -half_extents.z : half_extents.z); + point += ofs; + if (p.is_point_over(point)) + return false; + } + + return true; +} + bool AABB::has_point(const Vector3 &p_point) const { if (p_point.x < position.x) diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp index b1424e1d78..24096de551 100644 --- a/core/math/bsp_tree.cpp +++ b/core/math/bsp_tree.cpp @@ -244,10 +244,8 @@ bool BSP_Tree::point_is_inside(const Vector3 &p_point) const { const Node *nodesptr = &nodes[0]; const Plane *planesptr = &planes[0]; - int plane_count = planes.size(); int idx = node_count - 1; - int steps = 0; while (true) { @@ -259,21 +257,19 @@ bool BSP_Tree::point_is_inside(const Vector3 &p_point) const { return true; } - uint16_t plane = nodesptr[idx].plane; #ifdef DEBUG_ENABLED - + int plane_count = planes.size(); + uint16_t plane = nodesptr[idx].plane; ERR_FAIL_INDEX_V(plane, plane_count, false); #endif + bool over = planesptr[nodesptr[idx].plane].is_point_over(p_point); idx = over ? nodes[idx].over : nodes[idx].under; #ifdef DEBUG_ENABLED - ERR_FAIL_COND_V(idx < MAX_NODES && idx >= node_count, false); #endif - - steps++; } return false; @@ -453,10 +449,10 @@ BSP_Tree::operator Variant() const { for (int i = 0; i < planes.size(); i++) { - plane_values[i * 4 + 0] = planes[i].normal.x; - plane_values[i * 4 + 1] = planes[i].normal.y; - plane_values[i * 4 + 2] = planes[i].normal.z; - plane_values[i * 4 + 3] = planes[i].d; + plane_values.write[i * 4 + 0] = planes[i].normal.x; + plane_values.write[i * 4 + 1] = planes[i].normal.y; + plane_values.write[i * 4 + 2] = planes[i].normal.z; + plane_values.write[i * 4 + 3] = planes[i].d; } d["planes"] = plane_values; @@ -502,10 +498,10 @@ BSP_Tree::BSP_Tree(const Variant &p_variant) { PoolVector<real_t>::Read r = src_planes.read(); for (int i = 0; i < plane_count / 4; i++) { - planes[i].normal.x = r[i * 4 + 0]; - planes[i].normal.y = r[i * 4 + 1]; - planes[i].normal.z = r[i * 4 + 2]; - planes[i].d = r[i * 4 + 3]; + planes.write[i].normal.x = r[i * 4 + 0]; + planes.write[i].normal.y = r[i * 4 + 1]; + planes.write[i].normal.z = r[i * 4 + 2]; + planes.write[i].d = r[i * 4 + 3]; } } @@ -524,9 +520,9 @@ BSP_Tree::BSP_Tree(const Variant &p_variant) { for (int i = 0; i < nodes.size(); i++) { - nodes[i].over = r[i * 3 + 0]; - nodes[i].under = r[i * 3 + 1]; - nodes[i].plane = r[i * 3 + 2]; + nodes.write[i].over = r[i * 3 + 0]; + nodes.write[i].under = r[i * 3 + 1]; + nodes.write[i].plane = r[i * 3 + 2]; } } diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h index 226b4d572b..a689c7238a 100644 --- a/core/math/camera_matrix.h +++ b/core/math/camera_matrix.h @@ -31,7 +31,7 @@ #ifndef CAMERA_MATRIX_H #define CAMERA_MATRIX_H -#include "math_2d.h" +#include "rect2.h" #include "transform.h" /** @author Juan Linietsky <reduzio@gmail.com> diff --git a/core/math/delaunay.cpp b/core/math/delaunay.cpp new file mode 100644 index 0000000000..8cae92b7c0 --- /dev/null +++ b/core/math/delaunay.cpp @@ -0,0 +1 @@ +#include "delaunay.h" diff --git a/core/math/delaunay.h b/core/math/delaunay.h new file mode 100644 index 0000000000..46535d5ce9 --- /dev/null +++ b/core/math/delaunay.h @@ -0,0 +1,145 @@ +#ifndef DELAUNAY_H +#define DELAUNAY_H + +#include "rect2.h" + +class Delaunay2D { +public: + struct Triangle { + + int points[3]; + bool bad; + Triangle() { bad = false; } + Triangle(int p_a, int p_b, int p_c) { + points[0] = p_a; + points[1] = p_b; + points[2] = p_c; + bad = false; + } + }; + + struct Edge { + int edge[2]; + bool bad; + Edge() { bad = false; } + Edge(int p_a, int p_b) { + bad = false; + edge[0] = p_a; + edge[1] = p_b; + } + }; + + static bool circum_circle_contains(const Vector<Vector2> &p_vertices, const Triangle &p_triangle, int p_vertex) { + + Vector2 p1 = p_vertices[p_triangle.points[0]]; + Vector2 p2 = p_vertices[p_triangle.points[1]]; + Vector2 p3 = p_vertices[p_triangle.points[2]]; + + real_t ab = p1.x * p1.x + p1.y * p1.y; + real_t cd = p2.x * p2.x + p2.y * p2.y; + real_t ef = p3.x * p3.x + p3.y * p3.y; + + Vector2 circum( + (ab * (p3.y - p2.y) + cd * (p1.y - p3.y) + ef * (p2.y - p1.y)) / (p1.x * (p3.y - p2.y) + p2.x * (p1.y - p3.y) + p3.x * (p2.y - p1.y)), + (ab * (p3.x - p2.x) + cd * (p1.x - p3.x) + ef * (p2.x - p1.x)) / (p1.y * (p3.x - p2.x) + p2.y * (p1.x - p3.x) + p3.y * (p2.x - p1.x))); + + circum *= 0.5; + float r = p1.distance_squared_to(circum); + float d = p_vertices[p_vertex].distance_squared_to(circum); + return d <= r; + } + + 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) { + 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) { + return true; + } + + return false; + } + + static Vector<Triangle> triangulate(const Vector<Vector2> &p_points) { + + Vector<Vector2> points = p_points; + Vector<Triangle> triangles; + + Rect2 rect; + for (int i = 0; i < p_points.size(); i++) { + if (i == 0) { + rect.position = p_points[i]; + } else { + rect.expand_to(p_points[i]); + } + } + + float delta_max = MAX(rect.size.width, rect.size.height); + Vector2 center = rect.position + rect.size * 0.5; + + points.push_back(Vector2(center.x - 20 * delta_max, center.y - delta_max)); + points.push_back(Vector2(center.x, center.y + 20 * delta_max)); + points.push_back(Vector2(center.x + 20 * delta_max, center.y - delta_max)); + + triangles.push_back(Triangle(p_points.size() + 0, p_points.size() + 1, p_points.size() + 2)); + + for (int i = 0; i < p_points.size(); i++) { + //std::cout << "Traitement du point " << *p << std::endl; + //std::cout << "_triangles contains " << _triangles.size() << " elements" << std::endl; + + Vector<Edge> polygon; + + for (int j = 0; j < triangles.size(); j++) { + if (circum_circle_contains(points, triangles[j], i)) { + triangles.write[j].bad = true; + polygon.push_back(Edge(triangles[j].points[0], triangles[j].points[1])); + polygon.push_back(Edge(triangles[j].points[1], triangles[j].points[2])); + polygon.push_back(Edge(triangles[j].points[2], triangles[j].points[0])); + } + } + + for (int j = 0; j < triangles.size(); j++) { + if (triangles[j].bad) { + triangles.remove(j); + j--; + } + } + + for (int j = 0; j < polygon.size(); j++) { + for (int k = j + 1; k < polygon.size(); k++) { + if (edge_compare(points, polygon[j], polygon[k])) { + polygon.write[j].bad = true; + polygon.write[k].bad = true; + } + } + } + + for (int j = 0; j < polygon.size(); j++) { + + if (polygon[j].bad) { + continue; + } + triangles.push_back(Triangle(polygon[j].edge[0], polygon[j].edge[1], i)); + } + } + + for (int i = 0; i < triangles.size(); i++) { + bool invalid = false; + for (int j = 0; j < 3; j++) { + if (triangles[i].points[j] >= p_points.size()) { + invalid = true; + break; + } + } + if (invalid) { + triangles.remove(i); + i--; + } + } + + return triangles; + } +}; + +#endif // DELAUNAY_H diff --git a/core/math/expression.cpp b/core/math/expression.cpp new file mode 100644 index 0000000000..ba40cb4586 --- /dev/null +++ b/core/math/expression.cpp @@ -0,0 +1,2133 @@ +#include "expression.h" + +#include "class_db.h" +#include "func_ref.h" +#include "io/marshalls.h" +#include "math_funcs.h" +#include "os/os.h" +#include "reference.h" +#include "variant_parser.h" + +const char *Expression::func_name[Expression::FUNC_MAX] = { + "sin", + "cos", + "tan", + "sinh", + "cosh", + "tanh", + "asin", + "acos", + "atan", + "atan2", + "sqrt", + "fmod", + "fposmod", + "floor", + "ceil", + "round", + "abs", + "sign", + "pow", + "log", + "exp", + "is_nan", + "is_inf", + "ease", + "decimals", + "stepify", + "lerp", + "inverse_lerp", + "range_lerp", + "dectime", + "randomize", + "randi", + "randf", + "rand_range", + "seed", + "rand_seed", + "deg2rad", + "rad2deg", + "linear2db", + "db2linear", + "polar2cartesian", + "cartesian2polar", + "wrapi", + "wrapf", + "max", + "min", + "clamp", + "nearest_po2", + "weakref", + "funcref", + "convert", + "typeof", + "type_exists", + "char", + "str", + "print", + "printerr", + "printraw", + "var2str", + "str2var", + "var2bytes", + "bytes2var", + "color_named", +}; + +Expression::BuiltinFunc Expression::find_function(const String &p_string) { + + for (int i = 0; i < FUNC_MAX; i++) { + if (p_string == func_name[i]) + return BuiltinFunc(i); + } + + return FUNC_MAX; +} + +String Expression::get_func_name(BuiltinFunc p_func) { + + ERR_FAIL_INDEX_V(p_func, FUNC_MAX, String()); + return func_name[p_func]; +} + +int Expression::get_func_argument_count(BuiltinFunc p_func) { + + switch (p_func) { + + case MATH_RANDOMIZE: + case MATH_RAND: + case MATH_RANDF: + return 0; + case MATH_SIN: + case MATH_COS: + case MATH_TAN: + case MATH_SINH: + case MATH_COSH: + case MATH_TANH: + case MATH_ASIN: + case MATH_ACOS: + case MATH_ATAN: + case MATH_SQRT: + case MATH_FLOOR: + case MATH_CEIL: + case MATH_ROUND: + case MATH_ABS: + case MATH_SIGN: + case MATH_LOG: + case MATH_EXP: + case MATH_ISNAN: + case MATH_ISINF: + case MATH_DECIMALS: + case MATH_SEED: + case MATH_RANDSEED: + case MATH_DEG2RAD: + case MATH_RAD2DEG: + case MATH_LINEAR2DB: + case MATH_DB2LINEAR: + case LOGIC_NEAREST_PO2: + case OBJ_WEAKREF: + case TYPE_OF: + case TEXT_CHAR: + case TEXT_STR: + case TEXT_PRINT: + case TEXT_PRINTERR: + 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 MATH_ATAN2: + case MATH_FMOD: + case MATH_FPOSMOD: + case MATH_POW: + case MATH_EASE: + case MATH_STEPIFY: + case MATH_RANDOM: + case MATH_POLAR2CARTESIAN: + case MATH_CARTESIAN2POLAR: + case LOGIC_MAX: + case LOGIC_MIN: + case FUNC_FUNCREF: + case TYPE_CONVERT: + case COLORN: + return 2; + case MATH_LERP: + case MATH_INVERSE_LERP: + case MATH_DECTIME: + case MATH_WRAP: + case MATH_WRAPF: + case LOGIC_CLAMP: + return 3; + case MATH_RANGE_LERP: + return 5; + case FUNC_MAX: { + } + } + return 0; +} + +#define VALIDATE_ARG_NUM(m_arg) \ + if (!p_inputs[m_arg]->is_num()) { \ + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; \ + r_error.argument = m_arg; \ + r_error.expected = Variant::REAL; \ + return; \ + } + +void Expression::exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return, Variant::CallError &r_error, String &r_error_str) { + r_error.error = Variant::CallError::CALL_OK; + switch (p_func) { + case MATH_SIN: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::sin((double)*p_inputs[0]); + } break; + case MATH_COS: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::cos((double)*p_inputs[0]); + } break; + case MATH_TAN: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::tan((double)*p_inputs[0]); + } break; + case MATH_SINH: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::sinh((double)*p_inputs[0]); + } break; + case MATH_COSH: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::cosh((double)*p_inputs[0]); + } break; + case MATH_TANH: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::tanh((double)*p_inputs[0]); + } break; + case MATH_ASIN: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::asin((double)*p_inputs[0]); + } break; + case MATH_ACOS: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::acos((double)*p_inputs[0]); + } break; + case MATH_ATAN: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::atan((double)*p_inputs[0]); + } break; + case MATH_ATAN2: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + *r_return = Math::atan2((double)*p_inputs[0], (double)*p_inputs[1]); + } break; + case MATH_SQRT: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::sqrt((double)*p_inputs[0]); + } break; + case MATH_FMOD: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + *r_return = Math::fmod((double)*p_inputs[0], (double)*p_inputs[1]); + } break; + case MATH_FPOSMOD: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + *r_return = Math::fposmod((double)*p_inputs[0], (double)*p_inputs[1]); + } break; + case MATH_FLOOR: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::floor((double)*p_inputs[0]); + } break; + case MATH_CEIL: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::ceil((double)*p_inputs[0]); + } break; + case MATH_ROUND: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::round((double)*p_inputs[0]); + } break; + case MATH_ABS: { + + if (p_inputs[0]->get_type() == Variant::INT) { + + int64_t i = *p_inputs[0]; + *r_return = ABS(i); + } else if (p_inputs[0]->get_type() == Variant::REAL) { + + real_t r = *p_inputs[0]; + *r_return = Math::abs(r); + } else { + + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::REAL; + } + } break; + case MATH_SIGN: { + + if (p_inputs[0]->get_type() == Variant::INT) { + + int64_t i = *p_inputs[0]; + *r_return = i < 0 ? -1 : (i > 0 ? +1 : 0); + } else if (p_inputs[0]->get_type() == Variant::REAL) { + + real_t r = *p_inputs[0]; + *r_return = r < 0.0 ? -1.0 : (r > 0.0 ? +1.0 : 0.0); + } else { + + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::REAL; + } + } break; + case MATH_POW: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + *r_return = Math::pow((double)*p_inputs[0], (double)*p_inputs[1]); + } break; + case MATH_LOG: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::log((double)*p_inputs[0]); + } break; + case MATH_EXP: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::exp((double)*p_inputs[0]); + } break; + case MATH_ISNAN: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::is_nan((double)*p_inputs[0]); + } break; + case MATH_ISINF: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::is_inf((double)*p_inputs[0]); + } break; + case MATH_EASE: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + *r_return = Math::ease((double)*p_inputs[0], (double)*p_inputs[1]); + } break; + case MATH_DECIMALS: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::step_decimals((double)*p_inputs[0]); + } break; + case MATH_STEPIFY: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + *r_return = Math::stepify((double)*p_inputs[0], (double)*p_inputs[1]); + } break; + case MATH_LERP: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + *r_return = Math::lerp((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); + } break; + case MATH_INVERSE_LERP: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + *r_return = Math::inverse_lerp((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); + } break; + case MATH_RANGE_LERP: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + VALIDATE_ARG_NUM(3); + 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_DECTIME: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + *r_return = Math::dectime((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); + } break; + case MATH_RANDOMIZE: { + Math::randomize(); + + } break; + case MATH_RAND: { + *r_return = Math::rand(); + } break; + case MATH_RANDF: { + *r_return = Math::randf(); + } break; + case MATH_RANDOM: { + + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + *r_return = Math::random((double)*p_inputs[0], (double)*p_inputs[1]); + } break; + case MATH_SEED: { + + VALIDATE_ARG_NUM(0); + uint64_t seed = *p_inputs[0]; + Math::seed(seed); + + } break; + case MATH_RANDSEED: { + + VALIDATE_ARG_NUM(0); + uint64_t seed = *p_inputs[0]; + int ret = Math::rand_from_seed(&seed); + Array reta; + reta.push_back(ret); + reta.push_back(seed); + *r_return = reta; + + } break; + case MATH_DEG2RAD: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::deg2rad((double)*p_inputs[0]); + } break; + case MATH_RAD2DEG: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::rad2deg((double)*p_inputs[0]); + } break; + case MATH_LINEAR2DB: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::linear2db((double)*p_inputs[0]); + } break; + case MATH_DB2LINEAR: { + + VALIDATE_ARG_NUM(0); + *r_return = Math::db2linear((double)*p_inputs[0]); + } break; + case MATH_POLAR2CARTESIAN: { + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + double r = *p_inputs[0]; + double th = *p_inputs[1]; + *r_return = Vector2(r * Math::cos(th), r * Math::sin(th)); + } break; + case MATH_CARTESIAN2POLAR: { + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + double x = *p_inputs[0]; + double y = *p_inputs[1]; + *r_return = Vector2(Math::sqrt(x * x + y * y), Math::atan2(y, x)); + } break; + case MATH_WRAP: { + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + *r_return = Math::wrapi((int64_t)*p_inputs[0], (int64_t)*p_inputs[1], (int64_t)*p_inputs[2]); + } break; + case MATH_WRAPF: { + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + *r_return = Math::wrapf((double)*p_inputs[0], (double)*p_inputs[1], (double)*p_inputs[2]); + } break; + case LOGIC_MAX: { + + if (p_inputs[0]->get_type() == Variant::INT && p_inputs[1]->get_type() == Variant::INT) { + + int64_t a = *p_inputs[0]; + int64_t b = *p_inputs[1]; + *r_return = MAX(a, b); + } else { + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + + real_t a = *p_inputs[0]; + real_t b = *p_inputs[1]; + + *r_return = MAX(a, b); + } + + } break; + case LOGIC_MIN: { + + if (p_inputs[0]->get_type() == Variant::INT && p_inputs[1]->get_type() == Variant::INT) { + + int64_t a = *p_inputs[0]; + int64_t b = *p_inputs[1]; + *r_return = MIN(a, b); + } else { + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + + real_t a = *p_inputs[0]; + real_t b = *p_inputs[1]; + + *r_return = MIN(a, b); + } + } break; + case LOGIC_CLAMP: { + + if (p_inputs[0]->get_type() == Variant::INT && p_inputs[1]->get_type() == Variant::INT && p_inputs[2]->get_type() == Variant::INT) { + + int64_t a = *p_inputs[0]; + int64_t b = *p_inputs[1]; + int64_t c = *p_inputs[2]; + *r_return = CLAMP(a, b, c); + } else { + VALIDATE_ARG_NUM(0); + VALIDATE_ARG_NUM(1); + VALIDATE_ARG_NUM(2); + + real_t a = *p_inputs[0]; + real_t b = *p_inputs[1]; + real_t c = *p_inputs[2]; + + *r_return = CLAMP(a, b, c); + } + } break; + case LOGIC_NEAREST_PO2: { + + VALIDATE_ARG_NUM(0); + int64_t num = *p_inputs[0]; + *r_return = next_power_of_2(num); + } break; + case OBJ_WEAKREF: { + + if (p_inputs[0]->get_type() != Variant::OBJECT) { + + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::OBJECT; + + return; + } + + if (p_inputs[0]->is_ref()) { + + REF r = *p_inputs[0]; + if (!r.is_valid()) { + + return; + } + + Ref<WeakRef> wref = memnew(WeakRef); + wref->set_ref(r); + *r_return = wref; + } else { + Object *obj = *p_inputs[0]; + if (!obj) { + + return; + } + Ref<WeakRef> wref = memnew(WeakRef); + wref->set_obj(obj); + *r_return = wref; + } + + } break; + case FUNC_FUNCREF: { + + if (p_inputs[0]->get_type() != Variant::OBJECT) { + + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::OBJECT; + + return; + } + if (p_inputs[1]->get_type() != Variant::STRING && p_inputs[1]->get_type() != Variant::NODE_PATH) { + + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 1; + r_error.expected = Variant::STRING; + + return; + } + + Ref<FuncRef> fr = memnew(FuncRef); + + fr->set_instance(*p_inputs[0]); + fr->set_function(*p_inputs[1]); + + *r_return = fr; + + } break; + case TYPE_CONVERT: { + + VALIDATE_ARG_NUM(1); + int type = *p_inputs[1]; + if (type < 0 || type >= Variant::VARIANT_MAX) { + + r_error_str = RTR("Invalid type argument to convert(), use TYPE_* constants."); + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::INT; + return; + + } else { + + *r_return = Variant::construct(Variant::Type(type), p_inputs, 1, r_error); + } + } break; + case TYPE_OF: { + + *r_return = p_inputs[0]->get_type(); + + } break; + case TYPE_EXISTS: { + + *r_return = ClassDB::class_exists(*p_inputs[0]); + + } break; + case TEXT_CHAR: { + + CharType result[2] = { *p_inputs[0], 0 }; + + *r_return = String(result); + + } break; + case TEXT_STR: { + + String str = *p_inputs[0]; + + *r_return = str; + + } break; + case TEXT_PRINT: { + + String str = *p_inputs[0]; + print_line(str); + + } break; + + case TEXT_PRINTERR: { + + String str = *p_inputs[0]; + print_error(str); + + } break; + case TEXT_PRINTRAW: { + + String str = *p_inputs[0]; + OS::get_singleton()->print("%s", str.utf8().get_data()); + + } break; + case VAR_TO_STR: { + + String vars; + VariantWriter::write_to_string(*p_inputs[0], vars); + *r_return = vars; + } break; + case STR_TO_VAR: { + + if (p_inputs[0]->get_type() != Variant::STRING) { + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::STRING; + + return; + } + + VariantParser::StreamString ss; + ss.s = *p_inputs[0]; + + String errs; + int line; + Error err = VariantParser::parse(&ss, *r_return, errs, line); + + if (err != OK) { + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::STRING; + *r_return = "Parse error at line " + itos(line) + ": " + errs; + return; + } + + } break; + case VAR_TO_BYTES: { + + PoolByteArray barr; + int len; + Error err = encode_variant(*p_inputs[0], NULL, len); + if (err) { + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::NIL; + r_error_str = "Unexpected error encoding variable to bytes, likely unserializable type found (Object or RID)."; + return; + } + + barr.resize(len); + { + PoolByteArray::Write w = barr.write(); + encode_variant(*p_inputs[0], w.ptr(), len); + } + *r_return = barr; + } break; + case BYTES_TO_VAR: { + + if (p_inputs[0]->get_type() != Variant::POOL_BYTE_ARRAY) { + r_error.error = Variant::CallError::CALL_ERROR_INVALID_ARGUMENT; + r_error.argument = 0; + r_error.expected = Variant::POOL_BYTE_ARRAY; + + return; + } + + PoolByteArray varr = *p_inputs[0]; + Variant ret; + { + PoolByteArray::Read r = varr.read(); + Error err = decode_variant(ret, r.ptr(), varr.size(), NULL); + 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; + r_error.argument = 0; + r_error.expected = Variant::POOL_BYTE_ARRAY; + return; + } + } + + *r_return = ret; + + } break; + case COLORN: { + + VALIDATE_ARG_NUM(1); + + Color color = Color::named(*p_inputs[0]); + color.a = *p_inputs[1]; + + *r_return = String(color); + + } break; + default: {} + } +} + +//////// + +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; + return OK; + }; + case '}': { + + r_token.type = TK_CURLY_BRACKET_CLOSE; + return OK; + }; + case '[': { + + r_token.type = TK_BRACKET_OPEN; + return OK; + }; + case ']': { + + r_token.type = TK_BRACKET_CLOSE; + return OK; + }; + case '(': { + + r_token.type = TK_PARENTHESIS_OPEN; + return OK; + }; + case ')': { + + r_token.type = TK_PARENTHESIS_CLOSE; + return OK; + }; + case ',': { + + r_token.type = TK_COMMA; + return OK; + }; + case ':': { + + 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') { + _set_error("Expected number after '$'"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } + index *= 10; + index += expression[str_ofs] - '0'; + str_ofs++; + + } while (expression[str_ofs] >= '0' && expression[str_ofs] <= '9'); + + r_token.value = index; + return OK; + }; + case '=': { + + cchar = GET_CHAR(); + if (cchar == '=') { + r_token.type = TK_OP_EQUAL; + } else { + _set_error("Expected '='"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } + return OK; + }; + case '!': { + + if (expression[str_ofs] == '=') { + r_token.type = TK_OP_NOT_EQUAL; + str_ofs++; + } else { + r_token.type = TK_OP_NOT; + } + return OK; + }; + case '>': { + + if (expression[str_ofs] == '=') { + r_token.type = TK_OP_GREATER_EQUAL; + str_ofs++; + } else if (expression[str_ofs] == '>') { + r_token.type = TK_OP_SHIFT_RIGHT; + str_ofs++; + } else { + r_token.type = TK_OP_GREATER; + } + return OK; + }; + case '<': { + + if (expression[str_ofs] == '=') { + r_token.type = TK_OP_LESS_EQUAL; + str_ofs++; + } else if (expression[str_ofs] == '<') { + r_token.type = TK_OP_SHIFT_LEFT; + str_ofs++; + } else { + r_token.type = TK_OP_LESS; + } + return OK; + }; + case '+': { + r_token.type = TK_OP_ADD; + return OK; + }; + case '-': { + r_token.type = TK_OP_SUB; + return OK; + }; + case '/': { + r_token.type = TK_OP_DIV; + return OK; + }; + case '*': { + r_token.type = TK_OP_MUL; + return OK; + }; + case '%': { + r_token.type = TK_OP_MOD; + return OK; + }; + case '&': { + + if (expression[str_ofs] == '&') { + r_token.type = TK_OP_AND; + str_ofs++; + } else { + r_token.type = TK_OP_BIT_AND; + } + return OK; + }; + case '|': { + + if (expression[str_ofs] == '|') { + r_token.type = TK_OP_OR; + str_ofs++; + } else { + r_token.type = TK_OP_BIT_OR; + } + return OK; + }; + case '^': { + + r_token.type = TK_OP_BIT_XOR; + + return OK; + }; + case '~': { + + r_token.type = TK_OP_BIT_INVERT; + + return OK; + }; + case '"': { + + String str; + while (true) { + + CharType ch = GET_CHAR(); + + if (ch == 0) { + _set_error("Unterminated String"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } else if (ch == '"') { + break; + } else if (ch == '\\') { + //escaped characters... + + CharType next = GET_CHAR(); + if (next == 0) { + _set_error("Unterminated String"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } + CharType res = 0; + + switch (next) { + + case 'b': res = 8; break; + case 't': res = 9; break; + case 'n': res = 10; break; + case 'f': res = 12; break; + case 'r': res = 13; break; + case 'u': { + //hexnumbarh - oct is deprecated + + for (int j = 0; j < 4; j++) { + CharType c = GET_CHAR(); + + if (c == 0) { + _set_error("Unterminated String"); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } + if (!((c >= '0' && c <= '9') || (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') { + v = c - '0'; + } else if (c >= 'a' && c <= 'f') { + v = c - 'a'; + v += 10; + } else if (c >= 'A' && c <= 'F') { + v = c - 'A'; + v += 10; + } else { + ERR_PRINT("BUG"); + v = 0; + } + + res <<= 4; + res |= v; + } + + } break; + //case '\"': res='\"'; break; + //case '\\': res='\\'; break; + //case '/': res='/'; break; + default: { + res = next; + //r_err_str="Invalid escape sequence"; + //return ERR_PARSE_ERROR; + } break; + } + + str += res; + + } else { + str += ch; + } + } + + r_token.type = TK_CONSTANT; + r_token.value = str; + return OK; + + } break; + default: { + + if (cchar <= 32) { + break; + } + + if (cchar >= '0' && cchar <= '9') { + //a number + + String num; +#define READING_SIGN 0 +#define READING_INT 1 +#define READING_DEC 2 +#define READING_EXP 3 +#define READING_DONE 4 + int reading = READING_INT; + + CharType c = cchar; + bool exp_sign = false; + bool exp_beg = false; + bool is_float = false; + + while (true) { + + switch (reading) { + case READING_INT: { + + if (c >= '0' && c <= '9') { + //pass + } else if (c == '.') { + reading = READING_DEC; + is_float = true; + } else if (c == 'e') { + reading = READING_EXP; + } else { + reading = READING_DONE; + } + + } break; + case READING_DEC: { + + if (c >= '0' && c <= '9') { + + } else if (c == 'e') { + reading = READING_EXP; + + } else { + reading = READING_DONE; + } + + } break; + case READING_EXP: { + + if (c >= '0' && c <= '9') { + exp_beg = true; + + } else if ((c == '-' || c == '+') && !exp_sign && !exp_beg) { + if (c == '-') + is_float = true; + exp_sign = true; + + } else { + reading = READING_DONE; + } + } break; + } + + if (reading == READING_DONE) + break; + num += String::chr(c); + c = GET_CHAR(); + } + + str_ofs--; + + r_token.type = TK_CONSTANT; + + if (is_float) + r_token.value = num.to_double(); + else + r_token.value = num.to_int(); + return OK; + + } else if ((cchar >= 'A' && cchar <= 'Z') || (cchar >= 'a' && cchar <= 'z') || cchar == '_') { + + String id; + bool first = true; + + while ((cchar >= 'A' && cchar <= 'Z') || (cchar >= 'a' && cchar <= 'z') || cchar == '_' || (!first && cchar >= '0' && cchar <= '9')) { + + id += String::chr(cchar); + cchar = GET_CHAR(); + first = false; + } + + str_ofs--; //go back one + + if (id == "in") { + r_token.type = TK_OP_IN; + } else if (id == "null") { + r_token.type = TK_CONSTANT; + r_token.value = Variant(); + } else if (id == "true") { + r_token.type = TK_CONSTANT; + r_token.value = true; + } else if (id == "false") { + r_token.type = TK_CONSTANT; + r_token.value = false; + } else if (id == "PI") { + r_token.type = TK_CONSTANT; + r_token.value = Math_PI; + } else if (id == "TAU") { + r_token.type = TK_CONSTANT; + r_token.value = Math_TAU; + } else if (id == "INF") { + r_token.type = TK_CONSTANT; + r_token.value = Math_INF; + } else if (id == "NAN") { + r_token.type = TK_CONSTANT; + r_token.value = Math_NAN; + } else if (id == "not") { + r_token.type = TK_OP_NOT; + } else if (id == "or") { + r_token.type = TK_OP_OR; + } else if (id == "and") { + r_token.type = TK_OP_AND; + } else if (id == "self") { + r_token.type = TK_SELF; + } else { + + for (int i = 0; i < Variant::VARIANT_MAX; i++) { + if (id == Variant::get_type_name(Variant::Type(i))) { + r_token.type = TK_BASIC_TYPE; + r_token.value = i; + return OK; + } + } + + BuiltinFunc bifunc = find_function(id); + if (bifunc != FUNC_MAX) { + r_token.type = TK_BUILTIN_FUNC; + r_token.value = bifunc; + return OK; + } + + r_token.type = TK_IDENTIFIER; + r_token.value = id; + } + + return OK; + } else { + _set_error("Unexpected character."); + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; + } + } + } + } + + r_token.type = TK_ERROR; + return ERR_PARSE_ERROR; +} + +const char *Expression::token_name[TK_MAX] = { + "CURLY BRACKET OPEN", + "CURLY BRACKET CLOSE", + "BRACKET OPEN", + "BRACKET CLOSE", + "PARENTHESIS OPEN", + "PARENTHESIS CLOSE", + "IDENTIFIER", + "BUILTIN FUNC", + "SELF", + "CONSTANT", + "BASIC TYPE", + "COLON", + "COMMA", + "PERIOD", + "OP IN", + "OP EQUAL", + "OP NOT EQUAL", + "OP LESS", + "OP LESS EQUAL", + "OP GREATER", + "OP GREATER EQUAL", + "OP AND", + "OP OR", + "OP NOT", + "OP ADD", + "OP SUB", + "OP MUL", + "OP DIV", + "OP MOD", + "OP SHIFT LEFT", + "OP SHIFT RIGHT", + "OP BIT AND", + "OP BIT OR", + "OP BIT XOR", + "OP BIT INVERT", + "OP INPUT", + "EOF", + "ERROR" +}; + +Expression::ENode *Expression::_parse_expression() { + + Vector<ExpressionNode> expression; + + while (true) { + //keep appending stuff to expression + ENode *expr = NULL; + + Token tk; + _get_token(tk); + if (error_set) + return NULL; + + switch (tk.type) { + case TK_CURLY_BRACKET_OPEN: { + //a dictionary + DictionaryNode *dn = alloc_node<DictionaryNode>(); + + while (true) { + + int cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_CURLY_BRACKET_CLOSE) { + break; + } + str_ofs = cofs; //revert + //parse an expression + ENode *expr = _parse_expression(); + if (!expr) + return NULL; + dn->dict.push_back(expr); + + _get_token(tk); + if (tk.type != TK_COLON) { + _set_error("Expected ':'"); + return NULL; + } + + expr = _parse_expression(); + if (!expr) + return NULL; + + dn->dict.push_back(expr); + + cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_COMMA) { + //all good + } else if (tk.type == TK_CURLY_BRACKET_CLOSE) { + str_ofs = cofs; + } else { + _set_error("Expected ',' or '}'"); + } + } + + expr = dn; + } break; + case TK_BRACKET_OPEN: { + //an array + + ArrayNode *an = alloc_node<ArrayNode>(); + + while (true) { + + int cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_BRACKET_CLOSE) { + break; + } + str_ofs = cofs; //revert + //parse an expression + ENode *expr = _parse_expression(); + if (!expr) + return NULL; + an->array.push_back(expr); + + cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_COMMA) { + //all good + } else if (tk.type == TK_BRACKET_CLOSE) { + str_ofs = cofs; + } else { + _set_error("Expected ',' or ']'"); + } + } + + expr = an; + } break; + case TK_PARENTHESIS_OPEN: { + //a suexpression + ENode *e = _parse_expression(); + if (error_set) + return NULL; + _get_token(tk); + if (tk.type != TK_PARENTHESIS_CLOSE) { + _set_error("Expected ')'"); + return NULL; + } + + expr = e; + + } break; + case TK_IDENTIFIER: { + + String identifier = tk.value; + + int cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_PARENTHESIS_OPEN) { + //function call + CallNode *func_call = alloc_node<CallNode>(); + func_call->method = identifier; + SelfNode *self_node = alloc_node<SelfNode>(); + func_call->base = self_node; + + while (true) { + + int cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_PARENTHESIS_CLOSE) { + break; + } + str_ofs = cofs; //revert + //parse an expression + ENode *expr = _parse_expression(); + if (!expr) + return NULL; + + func_call->arguments.push_back(expr); + + cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_COMMA) { + //all good + } else if (tk.type == TK_PARENTHESIS_CLOSE) { + str_ofs = cofs; + } else { + _set_error("Expected ',' or ')'"); + } + } + + expr = func_call; + } else { + //named indexing + str_ofs = cofs; + + int input_index = -1; + for (int i = 0; i < input_names.size(); i++) { + if (input_names[i] == identifier) { + input_index = i; + break; + } + } + + if (input_index != -1) { + InputNode *input = alloc_node<InputNode>(); + input->index = input_index; + expr = input; + } else { + + NamedIndexNode *index = alloc_node<NamedIndexNode>(); + SelfNode *self_node = alloc_node<SelfNode>(); + index->base = self_node; + index->name = identifier; + expr = index; + } + } + } break; + case TK_INPUT: { + + InputNode *input = alloc_node<InputNode>(); + input->index = tk.value; + expr = input; + } break; + case TK_SELF: { + + SelfNode *self = alloc_node<SelfNode>(); + expr = self; + } break; + case TK_CONSTANT: { + ConstantNode *constant = alloc_node<ConstantNode>(); + constant->value = tk.value; + expr = constant; + } break; + case TK_BASIC_TYPE: { + //constructor.. + + Variant::Type bt = Variant::Type(int(tk.value)); + _get_token(tk); + if (tk.type != TK_PARENTHESIS_OPEN) { + _set_error("Expected '('"); + return NULL; + } + + ConstructorNode *constructor = alloc_node<ConstructorNode>(); + constructor->data_type = bt; + + while (true) { + + int cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_PARENTHESIS_CLOSE) { + break; + } + str_ofs = cofs; //revert + //parse an expression + ENode *expr = _parse_expression(); + if (!expr) + return NULL; + + constructor->arguments.push_back(expr); + + cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_COMMA) { + //all good + } else if (tk.type == TK_PARENTHESIS_CLOSE) { + str_ofs = cofs; + } else { + _set_error("Expected ',' or ')'"); + } + } + + expr = constructor; + + } break; + case TK_BUILTIN_FUNC: { + //builtin function + + _get_token(tk); + if (tk.type != TK_PARENTHESIS_OPEN) { + _set_error("Expected '('"); + return NULL; + } + + BuiltinFuncNode *bifunc = alloc_node<BuiltinFuncNode>(); + bifunc->func = BuiltinFunc(int(tk.value)); + + while (true) { + + int cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_PARENTHESIS_CLOSE) { + break; + } + str_ofs = cofs; //revert + //parse an expression + ENode *expr = _parse_expression(); + if (!expr) + return NULL; + + bifunc->arguments.push_back(expr); + + cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_COMMA) { + //all good + } else if (tk.type == TK_PARENTHESIS_CLOSE) { + str_ofs = cofs; + } else { + _set_error("Expected ',' or ')'"); + } + } + + int expected_args = get_func_argument_count(bifunc->func); + if (bifunc->arguments.size() != expected_args) { + _set_error("Builtin func '" + get_func_name(bifunc->func) + "' expects " + itos(expected_args) + " arguments."); + } + + expr = bifunc; + + } break; + case TK_OP_SUB: { + + ExpressionNode e; + e.is_op = true; + e.op = Variant::OP_NEGATE; + expression.push_back(e); + continue; + } break; + case TK_OP_NOT: { + + ExpressionNode e; + e.is_op = true; + e.op = Variant::OP_NOT; + expression.push_back(e); + continue; + } break; + + default: { + _set_error("Expected expression."); + return NULL; + } break; + } + + //before going to operators, must check indexing! + + while (true) { + int cofs2 = str_ofs; + _get_token(tk); + if (error_set) + return NULL; + + bool done = false; + + switch (tk.type) { + case TK_BRACKET_OPEN: { + //value indexing + + IndexNode *index = alloc_node<IndexNode>(); + index->base = expr; + + ENode *what = _parse_expression(); + if (!what) + return NULL; + + index->index = what; + + _get_token(tk); + if (tk.type != TK_BRACKET_CLOSE) { + _set_error("Expected ']' at end of index."); + return NULL; + } + expr = index; + + } break; + case TK_PERIOD: { + //named indexing or function call + _get_token(tk); + if (tk.type != TK_IDENTIFIER) { + _set_error("Expected identifier after '.'"); + return NULL; + } + + StringName identifier = tk.value; + + int cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_PARENTHESIS_OPEN) { + //function call + CallNode *func_call = alloc_node<CallNode>(); + func_call->method = identifier; + func_call->base = expr; + + while (true) { + + int cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_PARENTHESIS_CLOSE) { + break; + } + str_ofs = cofs; //revert + //parse an expression + ENode *expr = _parse_expression(); + if (!expr) + return NULL; + + func_call->arguments.push_back(expr); + + cofs = str_ofs; + _get_token(tk); + if (tk.type == TK_COMMA) { + //all good + } else if (tk.type == TK_PARENTHESIS_CLOSE) { + str_ofs = cofs; + } else { + _set_error("Expected ',' or ')'"); + } + } + + expr = func_call; + } else { + //named indexing + str_ofs = cofs; + + NamedIndexNode *index = alloc_node<NamedIndexNode>(); + index->base = expr; + index->name = identifier; + expr = index; + } + + } break; + default: { + str_ofs = cofs2; + done = true; + } break; + } + + if (done) + break; + } + + //push expression + { + ExpressionNode e; + e.is_op = false; + e.node = expr; + expression.push_back(e); + } + + //ok finally look for an operator + + int cofs = str_ofs; + _get_token(tk); + if (error_set) + return NULL; + + Variant::Operator op = Variant::OP_MAX; + + switch (tk.type) { + case TK_OP_IN: op = Variant::OP_IN; break; + case TK_OP_EQUAL: op = Variant::OP_EQUAL; break; + case TK_OP_NOT_EQUAL: op = Variant::OP_NOT_EQUAL; break; + case TK_OP_LESS: op = Variant::OP_LESS; break; + case TK_OP_LESS_EQUAL: op = Variant::OP_LESS_EQUAL; break; + case TK_OP_GREATER: op = Variant::OP_GREATER; break; + case TK_OP_GREATER_EQUAL: op = Variant::OP_GREATER_EQUAL; break; + case TK_OP_AND: op = Variant::OP_AND; break; + case TK_OP_OR: op = Variant::OP_OR; break; + case TK_OP_NOT: op = Variant::OP_NOT; break; + case TK_OP_ADD: op = Variant::OP_ADD; break; + case TK_OP_SUB: op = Variant::OP_SUBTRACT; break; + case TK_OP_MUL: op = Variant::OP_MULTIPLY; break; + case TK_OP_DIV: op = Variant::OP_DIVIDE; break; + case TK_OP_MOD: op = Variant::OP_MODULE; break; + case TK_OP_SHIFT_LEFT: op = Variant::OP_SHIFT_LEFT; break; + case TK_OP_SHIFT_RIGHT: op = Variant::OP_SHIFT_RIGHT; break; + case TK_OP_BIT_AND: op = Variant::OP_BIT_AND; break; + 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: {}; + } + + if (op == Variant::OP_MAX) { //stop appending stuff + str_ofs = cofs; + break; + } + + //push operator and go on + { + ExpressionNode e; + e.is_op = true; + e.op = op; + expression.push_back(e); + } + } + + /* Reduce the set set of expressions and place them in an operator tree, respecting precedence */ + + while (expression.size() > 1) { + + int next_op = -1; + int min_priority = 0xFFFFF; + bool is_unary = false; + + for (int i = 0; i < expression.size(); i++) { + + if (!expression[i].is_op) { + + continue; + } + + int priority; + + bool unary = false; + + switch (expression[i].op) { + + case Variant::OP_BIT_NEGATE: + priority = 0; + unary = true; + break; + case Variant::OP_NEGATE: + priority = 1; + unary = true; + break; + + case Variant::OP_MULTIPLY: priority = 2; break; + case Variant::OP_DIVIDE: priority = 2; break; + case Variant::OP_MODULE: priority = 2; break; + + case Variant::OP_ADD: priority = 3; break; + case Variant::OP_SUBTRACT: priority = 3; break; + + case Variant::OP_SHIFT_LEFT: priority = 4; break; + case Variant::OP_SHIFT_RIGHT: priority = 4; break; + + case Variant::OP_BIT_AND: priority = 5; break; + case Variant::OP_BIT_XOR: priority = 6; break; + case Variant::OP_BIT_OR: priority = 7; break; + + case Variant::OP_LESS: priority = 8; break; + case Variant::OP_LESS_EQUAL: priority = 8; break; + case Variant::OP_GREATER: priority = 8; break; + case Variant::OP_GREATER_EQUAL: priority = 8; break; + + case Variant::OP_EQUAL: priority = 8; break; + case Variant::OP_NOT_EQUAL: priority = 8; break; + + case Variant::OP_IN: priority = 10; break; + + case Variant::OP_NOT: + priority = 11; + unary = true; + break; + case Variant::OP_AND: priority = 12; break; + case Variant::OP_OR: priority = 13; break; + + default: { + _set_error("Parser bug, invalid operator in expression: " + itos(expression[i].op)); + return NULL; + } + } + + if (priority < min_priority) { + // < is used for left to right (default) + // <= is used for right to left + + next_op = i; + min_priority = priority; + is_unary = unary; + } + } + + if (next_op == -1) { + + _set_error("Yet another parser bug...."); + ERR_FAIL_COND_V(next_op == -1, NULL); + } + + // OK! create operator.. + if (is_unary) { + + int expr_pos = next_op; + while (expression[expr_pos].is_op) { + + expr_pos++; + if (expr_pos == expression.size()) { + //can happen.. + _set_error("Unexpected end of expression..."); + return NULL; + } + } + + //consecutively do unary opeators + for (int i = expr_pos - 1; i >= next_op; i--) { + + OperatorNode *op = alloc_node<OperatorNode>(); + op->op = expression[i].op; + op->nodes[0] = expression[i + 1].node; + op->nodes[1] = NULL; + expression.write[i].is_op = false; + expression.write[i].node = op; + expression.remove(i + 1); + } + + } else { + + if (next_op < 1 || next_op >= (expression.size() - 1)) { + _set_error("Parser bug..."); + ERR_FAIL_V(NULL); + } + + OperatorNode *op = alloc_node<OperatorNode>(); + op->op = expression[next_op].op; + + if (expression[next_op - 1].is_op) { + + _set_error("Parser bug..."); + ERR_FAIL_V(NULL); + } + + if (expression[next_op + 1].is_op) { + // this is not invalid and can really appear + // but it becomes invalid anyway because no binary op + // can be followed by a unary op in a valid combination, + // due to how precedence works, unaries will always disappear first + + _set_error("Unexpected two consecutive operators."); + return NULL; + } + + op->nodes[0] = expression[next_op - 1].node; //expression goes as left + op->nodes[1] = expression[next_op + 1].node; //next expression goes as right + + //replace all 3 nodes by this operator and make it an expression + expression.write[next_op - 1].node = op; + expression.remove(next_op); + expression.remove(next_op); + } + } + + return expression[0].node; +} + +bool Expression::_compile_expression() { + + if (!expression_dirty) + return error_set; + + if (nodes) { + memdelete(nodes); + nodes = NULL; + root = NULL; + } + + error_str = String(); + error_set = false; + str_ofs = 0; + + root = _parse_expression(); + + if (error_set) { + root = NULL; + if (nodes) { + memdelete(nodes); + } + nodes = NULL; + return true; + } + + expression_dirty = false; + return false; +} + +bool Expression::_execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, String &r_error_str) { + + switch (p_node->type) { + case Expression::ENode::TYPE_INPUT: { + + const Expression::InputNode *in = static_cast<const Expression::InputNode *>(p_node); + if (in->index < 0 || in->index >= p_inputs.size()) { + r_error_str = vformat(RTR("Invalid input %i (not passed) in expression"), in->index); + return true; + } + r_ret = p_inputs[in->index]; + } break; + case Expression::ENode::TYPE_CONSTANT: { + + const Expression::ConstantNode *c = static_cast<const Expression::ConstantNode *>(p_node); + r_ret = c->value; + + } break; + case Expression::ENode::TYPE_SELF: { + + if (!p_instance) { + r_error_str = RTR("self can't be used because instance is null (not passed)"); + return true; + } + r_ret = p_instance; + } break; + case Expression::ENode::TYPE_OPERATOR: { + + const Expression::OperatorNode *op = static_cast<const Expression::OperatorNode *>(p_node); + + Variant a; + bool ret = _execute(p_inputs, p_instance, op->nodes[0], a, r_error_str); + if (ret) + return true; + + Variant b; + + if (op->nodes[1]) { + bool ret = _execute(p_inputs, p_instance, op->nodes[1], b, r_error_str); + if (ret) + return true; + } + + bool valid = true; + Variant::evaluate(op->op, a, b, r_ret, valid); + if (!valid) { + r_error_str = vformat(RTR("Invalid operands to operator %s, %s and %s."), Variant::get_operator_name(op->op), Variant::get_type_name(a.get_type()), Variant::get_type_name(b.get_type())); + return true; + } + + } break; + case Expression::ENode::TYPE_INDEX: { + + const Expression::IndexNode *index = static_cast<const Expression::IndexNode *>(p_node); + + Variant base; + bool ret = _execute(p_inputs, p_instance, index->base, base, r_error_str); + if (ret) + return true; + + Variant idx; + + ret = _execute(p_inputs, p_instance, index->index, idx, r_error_str); + if (ret) + return true; + + bool valid; + r_ret = base.get(idx, &valid); + if (!valid) { + r_error_str = vformat(RTR("Invalid index of type %s for base type %s"), Variant::get_type_name(idx.get_type()), Variant::get_type_name(base.get_type())); + return true; + } + + } break; + case Expression::ENode::TYPE_NAMED_INDEX: { + + const Expression::NamedIndexNode *index = static_cast<const Expression::NamedIndexNode *>(p_node); + + Variant base; + bool ret = _execute(p_inputs, p_instance, index->base, base, r_error_str); + if (ret) + return true; + + bool valid; + r_ret = base.get_named(index->name, &valid); + if (!valid) { + r_error_str = vformat(RTR("Invalid named index '%s' for base type %s"), String(index->name), Variant::get_type_name(base.get_type())); + return true; + } + + } break; + case Expression::ENode::TYPE_ARRAY: { + const Expression::ArrayNode *array = static_cast<const Expression::ArrayNode *>(p_node); + + Array arr; + arr.resize(array->array.size()); + for (int i = 0; i < array->array.size(); i++) { + + Variant value; + bool ret = _execute(p_inputs, p_instance, array->array[i], value, r_error_str); + + if (ret) + return true; + arr[i] = value; + } + + r_ret = arr; + + } break; + case Expression::ENode::TYPE_DICTIONARY: { + const Expression::DictionaryNode *dictionary = static_cast<const Expression::DictionaryNode *>(p_node); + + Dictionary d; + for (int i = 0; i < dictionary->dict.size(); i += 2) { + + Variant key; + bool ret = _execute(p_inputs, p_instance, dictionary->dict[i + 0], key, r_error_str); + + if (ret) + return true; + + Variant value; + ret = _execute(p_inputs, p_instance, dictionary->dict[i + 1], value, r_error_str); + if (ret) + return true; + + d[key] = value; + } + + r_ret = d; + } break; + case Expression::ENode::TYPE_CONSTRUCTOR: { + + const Expression::ConstructorNode *constructor = static_cast<const Expression::ConstructorNode *>(p_node); + + Vector<Variant> arr; + Vector<const Variant *> argp; + arr.resize(constructor->arguments.size()); + argp.resize(constructor->arguments.size()); + + for (int i = 0; i < constructor->arguments.size(); i++) { + + Variant value; + bool ret = _execute(p_inputs, p_instance, constructor->arguments[i], value, r_error_str); + + if (ret) + return true; + arr.write[i] = value; + argp.write[i] = &arr[i]; + } + + Variant::CallError ce; + r_ret = Variant::construct(constructor->data_type, (const Variant **)argp.ptr(), argp.size(), ce); + + if (ce.error != Variant::CallError::CALL_OK) { + r_error_str = vformat(RTR("Invalid arguments to construct '%s'"), Variant::get_type_name(constructor->data_type)); + return true; + } + + } break; + case Expression::ENode::TYPE_BUILTIN_FUNC: { + + const Expression::BuiltinFuncNode *bifunc = static_cast<const Expression::BuiltinFuncNode *>(p_node); + + Vector<Variant> arr; + Vector<const Variant *> argp; + arr.resize(bifunc->arguments.size()); + argp.resize(bifunc->arguments.size()); + + for (int i = 0; i < bifunc->arguments.size(); i++) { + + Variant value; + bool ret = _execute(p_inputs, p_instance, bifunc->arguments[i], value, r_error_str); + if (ret) + return true; + arr.write[i] = value; + argp.write[i] = &arr[i]; + } + + Variant::CallError ce; + exec_func(bifunc->func, (const Variant **)argp.ptr(), &r_ret, ce, r_error_str); + + if (ce.error != Variant::CallError::CALL_OK) { + r_error_str = "Builtin Call Failed. " + r_error_str; + return true; + } + + } break; + case Expression::ENode::TYPE_CALL: { + + const Expression::CallNode *call = static_cast<const Expression::CallNode *>(p_node); + + Variant base; + bool ret = _execute(p_inputs, p_instance, call->base, base, r_error_str); + + if (ret) + return true; + + Vector<Variant> arr; + Vector<const Variant *> argp; + arr.resize(call->arguments.size()); + argp.resize(call->arguments.size()); + + 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); + + if (ret) + return true; + arr.write[i] = value; + argp.write[i] = &arr[i]; + } + + Variant::CallError ce; + r_ret = base.call(call->method, (const Variant **)argp.ptr(), argp.size(), ce); + + if (ce.error != Variant::CallError::CALL_OK) { + r_error_str = vformat(RTR("On call to '%s':"), String(call->method)); + return true; + } + + } break; + } + return false; +} + +Error Expression::parse(const String &p_expression, const Vector<String> &p_input_names) { + + if (nodes) { + memdelete(nodes); + nodes = NULL; + root = NULL; + } + + error_str = String(); + error_set = false; + str_ofs = 0; + input_names = p_input_names; + + expression = p_expression; + root = _parse_expression(); + + if (error_set) { + root = NULL; + if (nodes) { + memdelete(nodes); + } + nodes = NULL; + return ERR_INVALID_PARAMETER; + } + + return OK; +} + +Variant Expression::execute(Array p_inputs, Object *p_base, bool p_show_error) { + + execution_error = false; + Variant output; + String error_txt; + bool err = _execute(p_inputs, p_base, root, output, error_txt); + if (err) { + execution_error = true; + error_str = error_txt; + if (p_show_error) { + ERR_EXPLAIN(error_str); + ERR_FAIL_V(Variant()); + } + } + + return output; +} + +bool Expression::has_execute_failed() const { + return execution_error; +} + +String Expression::get_error_text() const { + return error_str; +} + +void Expression::_bind_methods() { + + ClassDB::bind_method(D_METHOD("parse", "expression", "input_names"), &Expression::parse, DEFVAL(Vector<String>())); + ClassDB::bind_method(D_METHOD("execute", "inputs", "base_instance", "show_error"), &Expression::execute, DEFVAL(Array()), DEFVAL(Variant()), DEFVAL(true)); + ClassDB::bind_method(D_METHOD("has_execute_failed"), &Expression::has_execute_failed); + 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() { + + if (nodes) { + memdelete(nodes); + } +} diff --git a/core/math/expression.h b/core/math/expression.h new file mode 100644 index 0000000000..7a7639cf0b --- /dev/null +++ b/core/math/expression.h @@ -0,0 +1,325 @@ +#ifndef EXPRESSION_H +#define EXPRESSION_H + +#include "core/reference.h" + +class Expression : public Reference { + GDCLASS(Expression, Reference) +public: + enum BuiltinFunc { + MATH_SIN, + MATH_COS, + MATH_TAN, + MATH_SINH, + MATH_COSH, + MATH_TANH, + MATH_ASIN, + MATH_ACOS, + MATH_ATAN, + MATH_ATAN2, + MATH_SQRT, + MATH_FMOD, + MATH_FPOSMOD, + MATH_FLOOR, + MATH_CEIL, + MATH_ROUND, + MATH_ABS, + MATH_SIGN, + MATH_POW, + MATH_LOG, + MATH_EXP, + MATH_ISNAN, + MATH_ISINF, + MATH_EASE, + MATH_DECIMALS, + MATH_STEPIFY, + MATH_LERP, + MATH_INVERSE_LERP, + MATH_RANGE_LERP, + MATH_DECTIME, + MATH_RANDOMIZE, + MATH_RAND, + MATH_RANDF, + MATH_RANDOM, + MATH_SEED, + MATH_RANDSEED, + MATH_DEG2RAD, + MATH_RAD2DEG, + MATH_LINEAR2DB, + MATH_DB2LINEAR, + MATH_POLAR2CARTESIAN, + MATH_CARTESIAN2POLAR, + MATH_WRAP, + MATH_WRAPF, + LOGIC_MAX, + LOGIC_MIN, + LOGIC_CLAMP, + LOGIC_NEAREST_PO2, + OBJ_WEAKREF, + FUNC_FUNCREF, + TYPE_CONVERT, + TYPE_OF, + TYPE_EXISTS, + TEXT_CHAR, + TEXT_STR, + TEXT_PRINT, + TEXT_PRINTERR, + TEXT_PRINTRAW, + VAR_TO_STR, + STR_TO_VAR, + VAR_TO_BYTES, + BYTES_TO_VAR, + COLORN, + FUNC_MAX + }; + + static int get_func_argument_count(BuiltinFunc p_func); + static String get_func_name(BuiltinFunc p_func); + static void exec_func(BuiltinFunc p_func, const Variant **p_inputs, Variant *r_return, Variant::CallError &r_error, String &r_error_str); + static BuiltinFunc find_function(const String &p_string); + +private: + static const char *func_name[FUNC_MAX]; + + struct Input { + + Variant::Type type; + String name; + + Input() { type = Variant::NIL; } + }; + + Vector<Input> inputs; + Variant::Type output_type; + + String expression; + + bool sequenced; + int str_ofs; + bool expression_dirty; + + bool _compile_expression(); + + enum TokenType { + TK_CURLY_BRACKET_OPEN, + TK_CURLY_BRACKET_CLOSE, + TK_BRACKET_OPEN, + TK_BRACKET_CLOSE, + TK_PARENTHESIS_OPEN, + TK_PARENTHESIS_CLOSE, + TK_IDENTIFIER, + TK_BUILTIN_FUNC, + TK_SELF, + TK_CONSTANT, + TK_BASIC_TYPE, + TK_COLON, + TK_COMMA, + TK_PERIOD, + TK_OP_IN, + TK_OP_EQUAL, + TK_OP_NOT_EQUAL, + TK_OP_LESS, + TK_OP_LESS_EQUAL, + TK_OP_GREATER, + TK_OP_GREATER_EQUAL, + TK_OP_AND, + TK_OP_OR, + TK_OP_NOT, + TK_OP_ADD, + TK_OP_SUB, + TK_OP_MUL, + TK_OP_DIV, + TK_OP_MOD, + TK_OP_SHIFT_LEFT, + TK_OP_SHIFT_RIGHT, + TK_OP_BIT_AND, + TK_OP_BIT_OR, + TK_OP_BIT_XOR, + TK_OP_BIT_INVERT, + TK_INPUT, + TK_EOF, + TK_ERROR, + TK_MAX + }; + + static const char *token_name[TK_MAX]; + struct Token { + + TokenType type; + Variant value; + }; + + void _set_error(const String &p_err) { + if (error_set) + return; + error_str = p_err; + error_set = true; + } + + Error _get_token(Token &r_token); + + String error_str; + bool error_set; + + struct ENode { + + enum Type { + TYPE_INPUT, + TYPE_CONSTANT, + TYPE_SELF, + TYPE_OPERATOR, + TYPE_INDEX, + TYPE_NAMED_INDEX, + TYPE_ARRAY, + TYPE_DICTIONARY, + TYPE_CONSTRUCTOR, + TYPE_BUILTIN_FUNC, + TYPE_CALL + }; + + ENode *next; + + Type type; + + ENode() { next = NULL; } + virtual ~ENode() { + if (next) { + memdelete(next); + } + } + }; + + struct ExpressionNode { + + bool is_op; + union { + Variant::Operator op; + ENode *node; + }; + }; + + ENode *_parse_expression(); + + struct InputNode : public ENode { + + int index; + InputNode() { + type = TYPE_INPUT; + } + }; + + struct ConstantNode : public ENode { + + Variant value; + ConstantNode() { + type = TYPE_CONSTANT; + } + }; + + struct OperatorNode : public ENode { + + Variant::Operator op; + + ENode *nodes[2]; + + OperatorNode() { + type = TYPE_OPERATOR; + } + }; + + struct SelfNode : public ENode { + + SelfNode() { + type = TYPE_SELF; + } + }; + + struct IndexNode : public ENode { + ENode *base; + ENode *index; + + IndexNode() { + type = TYPE_INDEX; + } + }; + + struct NamedIndexNode : public ENode { + ENode *base; + StringName name; + + NamedIndexNode() { + type = TYPE_NAMED_INDEX; + } + }; + + struct ConstructorNode : public ENode { + Variant::Type data_type; + Vector<ENode *> arguments; + + ConstructorNode() { + type = TYPE_CONSTRUCTOR; + } + }; + + struct CallNode : public ENode { + ENode *base; + StringName method; + Vector<ENode *> arguments; + + CallNode() { + type = TYPE_CALL; + } + }; + + struct ArrayNode : public ENode { + Vector<ENode *> array; + ArrayNode() { + type = TYPE_ARRAY; + } + }; + + struct DictionaryNode : public ENode { + Vector<ENode *> dict; + DictionaryNode() { + type = TYPE_DICTIONARY; + } + }; + + struct BuiltinFuncNode : public ENode { + BuiltinFunc func; + Vector<ENode *> arguments; + BuiltinFuncNode() { + type = TYPE_BUILTIN_FUNC; + } + }; + + template <class T> + T *alloc_node() { + T *node = memnew(T); + node->next = nodes; + nodes = node; + return node; + } + + ENode *root; + ENode *nodes; + + Vector<String> input_names; + + bool execution_error; + bool _execute(const Array &p_inputs, Object *p_instance, Expression::ENode *p_node, Variant &r_ret, String &r_error_str); + +protected: + static void _bind_methods(); + +public: + Error parse(const String &p_expression, const Vector<String> &p_input_names = Vector<String>()); + Variant execute(Array p_inputs, Object *p_base = NULL, bool p_show_error = true); + bool has_execute_failed() const; + String get_error_text() const; + + Expression(); + ~Expression(); +}; + +#endif // EXPRESSION_H diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp index 24f077a4ca..d8cb657b5e 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry.cpp @@ -56,7 +56,7 @@ void Geometry::MeshData::optimize_vertices() { vtx_remap[idx] = ni; } - faces[i].indices[j] = vtx_remap[idx]; + faces.write[i].indices.write[j] = vtx_remap[idx]; } } @@ -74,8 +74,8 @@ void Geometry::MeshData::optimize_vertices() { vtx_remap[b] = ni; } - edges[i].a = vtx_remap[a]; - edges[i].b = vtx_remap[b]; + edges.write[i].a = vtx_remap[a]; + edges.write[i].b = vtx_remap[b]; } Vector<Vector3> new_vertices; @@ -84,7 +84,7 @@ void Geometry::MeshData::optimize_vertices() { for (int i = 0; i < vertices.size(); i++) { if (vtx_remap.has(i)) - new_vertices[vtx_remap[i]] = vertices[i]; + new_vertices.write[vtx_remap[i]] = vertices[i]; } vertices = new_vertices; } @@ -626,7 +626,6 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e voxelsize.z /= div_z; // create and initialize cells to zero - //print_line("Wrapper: Initializing Cells"); uint8_t ***cell_status = memnew_arr(uint8_t **, div_x); for (int i = 0; i < div_x; i++) { @@ -645,7 +644,6 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e } // plot faces into cells - //print_line("Wrapper (1/6): Plotting Faces"); for (int i = 0; i < face_count; i++) { @@ -659,8 +657,6 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e // determine which cells connect to the outside by traversing the outside and recursively flood-fill marking - //print_line("Wrapper (2/6): Flood Filling"); - for (int i = 0; i < div_x; i++) { for (int j = 0; j < div_y; j++) { @@ -690,8 +686,6 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e // build faces for the inside-outside cell divisors - //print_line("Wrapper (3/6): Building Faces"); - PoolVector<Face3> wrapped_faces; for (int i = 0; i < div_x; i++) { @@ -705,8 +699,6 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e } } - //print_line("Wrapper (4/6): Transforming Back Vertices"); - // transform face vertices to global coords int wrapped_faces_count = wrapped_faces.size(); @@ -724,7 +716,6 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e } // clean up grid - //print_line("Wrapper (5/6): Grid Cleanup"); for (int i = 0; i < div_x; i++) { @@ -740,7 +731,6 @@ PoolVector<Face3> Geometry::wrap_geometry(PoolVector<Face3> p_array, real_t *p_e if (p_error) *p_error = voxelsize.length(); - //print_line("Wrapper (6/6): Finished."); return wrapped_faces; } @@ -1014,8 +1004,8 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu Vector<_AtlasWorkRect> wrects; wrects.resize(p_rects.size()); for (int i = 0; i < p_rects.size(); i++) { - wrects[i].s = p_rects[i]; - wrects[i].idx = i; + wrects.write[i].s = p_rects[i]; + wrects.write[i].idx = i; } wrects.sort(); int widest = wrects[0].s.width; @@ -1033,7 +1023,7 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu Vector<int> hmax; hmax.resize(w); for (int j = 0; j < w; j++) - hmax[j] = 0; + hmax.write[j] = 0; //place them int ofs = 0; @@ -1052,8 +1042,8 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu from_y = hmax[ofs + k]; } - wrects[j].p.x = ofs; - wrects[j].p.y = from_y; + wrects.write[j].p.x = ofs; + wrects.write[j].p.y = from_y; int end_h = from_y + wrects[j].s.height; int end_w = ofs + wrects[j].s.width; if (ofs == 0) @@ -1061,7 +1051,7 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu for (int k = 0; k < wrects[j].s.width; k++) { - hmax[ofs + k] = end_h; + hmax.write[ofs + k] = end_h; } if (end_h > max_h) @@ -1101,7 +1091,7 @@ void Geometry::make_atlas(const Vector<Size2i> &p_rects, Vector<Point2i> &r_resu for (int i = 0; i < p_rects.size(); i++) { - r_result[results[best].result[i].idx] = results[best].result[i].p; + r_result.write[results[best].result[i].idx] = results[best].result[i].p; } r_size = Size2(results[best].max_w, results[best].max_h); diff --git a/core/math/geometry.h b/core/math/geometry.h index ca4363e129..83b9467a30 100644 --- a/core/math/geometry.h +++ b/core/math/geometry.h @@ -33,9 +33,9 @@ #include "dvector.h" #include "face3.h" -#include "math_2d.h" #include "object.h" #include "print_string.h" +#include "rect2.h" #include "triangulate.h" #include "vector.h" #include "vector3.h" @@ -502,16 +502,15 @@ public: } static bool is_point_in_triangle(const Vector2 &s, const Vector2 &a, const Vector2 &b, const Vector2 &c) { - int as_x = s.x - a.x; - int as_y = s.y - a.y; + Vector2 an = a - s; + Vector2 bn = b - s; + Vector2 cn = c - s; - bool s_ab = (b.x - a.x) * as_y - (b.y - a.y) * as_x > 0; + bool orientation = an.cross(bn) > 0; - if (((c.x - a.x) * as_y - (c.y - a.y) * as_x > 0) == s_ab) return false; + if ((bn.cross(cn) > 0) != orientation) return false; - if (((c.x - b.x) * (s.y - b.y) - (c.y - b.y) * (s.x - b.x) > 0) != s_ab) return false; - - return true; + return (cn.cross(an) > 0) == orientation; } static bool is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon); @@ -530,6 +529,21 @@ public: 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/ + + 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? + return false; + } + + const Vector2 v = p_from_a - p_from_b; + const real_t t = (p_dir_b.x * v.y - p_dir_b.y * v.x) / denom; + r_result = p_from_a + t * p_dir_a; + return true; + } + static bool segment_intersects_segment_2d(const Vector2 &p_from_a, const Vector2 &p_to_a, const Vector2 &p_from_b, const Vector2 &p_to_b, Vector2 *r_result) { Vector2 B = p_to_a - p_from_a; @@ -876,14 +890,14 @@ public: for (int i = 0; i < n; ++i) { while (k >= 2 && vec2_cross(H[k - 2], H[k - 1], P[i]) <= 0) k--; - H[k++] = P[i]; + H.write[k++] = P[i]; } // 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--; - H[k++] = P[i]; + H.write[k++] = P[i]; } H.resize(k); diff --git a/core/math/math_2d.h b/core/math/math_2d.h deleted file mode 100644 index 8928349a44..0000000000 --- a/core/math/math_2d.h +++ /dev/null @@ -1,992 +0,0 @@ -/*************************************************************************/ -/* math_2d.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) */ -/* */ -/* 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 MATH_2D_H -#define MATH_2D_H - -#include "math_funcs.h" -#include "ustring.h" -/** - @author Juan Linietsky <reduzio@gmail.com> -*/ -enum Margin { - - MARGIN_LEFT, - MARGIN_TOP, - MARGIN_RIGHT, - MARGIN_BOTTOM -}; - -enum Corner { - - CORNER_TOP_LEFT, - CORNER_TOP_RIGHT, - CORNER_BOTTOM_RIGHT, - CORNER_BOTTOM_LEFT -}; - -enum Orientation { - - HORIZONTAL, - VERTICAL -}; - -enum HAlign { - - HALIGN_LEFT, - HALIGN_CENTER, - HALIGN_RIGHT -}; - -enum VAlign { - - VALIGN_TOP, - VALIGN_CENTER, - VALIGN_BOTTOM -}; - -struct Vector2 { - - union { - real_t x; - real_t width; - }; - union { - real_t y; - real_t height; - }; - - _FORCE_INLINE_ real_t &operator[](int p_idx) { - return p_idx ? y : x; - } - _FORCE_INLINE_ const real_t &operator[](int p_idx) const { - return p_idx ? y : x; - } - - void normalize(); - Vector2 normalized() const; - bool is_normalized() const; - - real_t length() const; - real_t length_squared() const; - - real_t distance_to(const Vector2 &p_vector2) const; - 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; - - real_t dot(const Vector2 &p_other) const; - real_t cross(const Vector2 &p_other) const; - Vector2 cross(real_t p_other) const; - Vector2 project(const Vector2 &p_vec) const; - - Vector2 plane_project(real_t p_d, const Vector2 &p_vec) const; - - Vector2 clamped(real_t p_len) const; - - _FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t); - _FORCE_INLINE_ Vector2 linear_interpolate(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 slide(const Vector2 &p_normal) const; - Vector2 bounce(const Vector2 &p_normal) const; - Vector2 reflect(const Vector2 &p_normal) const; - - Vector2 operator+(const Vector2 &p_v) const; - void operator+=(const Vector2 &p_v); - Vector2 operator-(const Vector2 &p_v) const; - void operator-=(const Vector2 &p_v); - Vector2 operator*(const Vector2 &p_v1) const; - - Vector2 operator*(const real_t &rvalue) const; - void operator*=(const real_t &rvalue); - void operator*=(const Vector2 &rvalue) { *this = *this * rvalue; } - - Vector2 operator/(const Vector2 &p_v1) const; - - Vector2 operator/(const real_t &rvalue) const; - - void operator/=(const real_t &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); } - - real_t angle() const; - - void set_rotation(real_t p_radians) { - - x = Math::cos(p_radians); - y = Math::sin(p_radians); - } - - _FORCE_INLINE_ Vector2 abs() const { - - return Vector2(Math::abs(x), Math::abs(y)); - } - - Vector2 rotated(real_t p_by) const; - Vector2 tangent() const { - - return Vector2(y, -x); - } - - Vector2 floor() const; - Vector2 snapped(const Vector2 &p_by) const; - real_t aspect() const { return width / height; } - - operator String() const { return String::num(x) + ", " + String::num(y); } - - _FORCE_INLINE_ Vector2(real_t p_x, real_t p_y) { - x = p_x; - y = p_y; - } - _FORCE_INLINE_ Vector2() { - x = 0; - y = 0; - } -}; - -_FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2 &p_vec) const { - - return p_vec - *this * (dot(p_vec) - p_d); -} - -_FORCE_INLINE_ Vector2 operator*(real_t p_scalar, const Vector2 &p_vec) { - - return p_vec * p_scalar; -} - -_FORCE_INLINE_ Vector2 Vector2::operator+(const Vector2 &p_v) const { - - return Vector2(x + p_v.x, y + p_v.y); -} -_FORCE_INLINE_ void Vector2::operator+=(const Vector2 &p_v) { - - x += p_v.x; - y += p_v.y; -} -_FORCE_INLINE_ Vector2 Vector2::operator-(const Vector2 &p_v) const { - - return Vector2(x - p_v.x, y - p_v.y); -} -_FORCE_INLINE_ void Vector2::operator-=(const Vector2 &p_v) { - - x -= p_v.x; - y -= p_v.y; -} - -_FORCE_INLINE_ Vector2 Vector2::operator*(const Vector2 &p_v1) const { - - return Vector2(x * p_v1.x, y * p_v1.y); -}; - -_FORCE_INLINE_ Vector2 Vector2::operator*(const real_t &rvalue) const { - - return Vector2(x * rvalue, y * rvalue); -}; -_FORCE_INLINE_ void Vector2::operator*=(const real_t &rvalue) { - - x *= rvalue; - y *= rvalue; -}; - -_FORCE_INLINE_ Vector2 Vector2::operator/(const Vector2 &p_v1) const { - - return Vector2(x / p_v1.x, y / p_v1.y); -}; - -_FORCE_INLINE_ Vector2 Vector2::operator/(const real_t &rvalue) const { - - return Vector2(x / rvalue, y / rvalue); -}; - -_FORCE_INLINE_ void Vector2::operator/=(const real_t &rvalue) { - - x /= rvalue; - y /= rvalue; -}; - -_FORCE_INLINE_ Vector2 Vector2::operator-() const { - - return Vector2(-x, -y); -} - -_FORCE_INLINE_ bool Vector2::operator==(const Vector2 &p_vec2) const { - - return x == p_vec2.x && y == p_vec2.y; -} -_FORCE_INLINE_ bool Vector2::operator!=(const Vector2 &p_vec2) const { - - return x != p_vec2.x || y != p_vec2.y; -} - -Vector2 Vector2::linear_interpolate(const Vector2 &p_b, real_t p_t) const { - - Vector2 res = *this; - - res.x += (p_t * (p_b.x - x)); - res.y += (p_t * (p_b.y - y)); - - return res; -} - -Vector2 Vector2::linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t) { - - Vector2 res = p_a; - - res.x += (p_t * (p_b.x - p_a.x)); - res.y += (p_t * (p_b.y - p_a.y)); - - return res; -} - -typedef Vector2 Size2; -typedef Vector2 Point2; - -struct Transform2D; - -struct Rect2 { - - Point2 position; - Size2 size; - - const Vector2 &get_position() const { return position; } - void set_position(const Vector2 &p_pos) { position = p_pos; } - const Vector2 &get_size() const { return size; } - void set_size(const Vector2 &p_size) { size = p_size; } - - real_t get_area() const { return size.width * size.height; } - - inline bool intersects(const Rect2 &p_rect) const { - if (position.x >= (p_rect.position.x + p_rect.size.width)) - return false; - if ((position.x + size.width) <= p_rect.position.x) - return false; - if (position.y >= (p_rect.position.y + p_rect.size.height)) - return false; - if ((position.y + size.height) <= p_rect.position.y) - return false; - - return true; - } - - inline real_t distance_to(const Vector2 &p_point) const { - - real_t dist; - bool inside = true; - - if (p_point.x < position.x) { - real_t d = position.x - p_point.x; - dist = inside ? d : MIN(dist, d); - inside = false; - } - if (p_point.y < position.y) { - real_t d = position.y - p_point.y; - dist = inside ? d : MIN(dist, d); - inside = false; - } - if (p_point.x >= (position.x + size.x)) { - real_t d = p_point.x - (position.x + size.x); - dist = inside ? d : MIN(dist, d); - inside = false; - } - if (p_point.y >= (position.y + size.y)) { - real_t d = p_point.y - (position.y + size.y); - dist = inside ? d : MIN(dist, d); - inside = false; - } - - if (inside) - return 0; - else - return dist; - } - - _FORCE_INLINE_ bool intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const; - - bool intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos = NULL, Point2 *r_normal = NULL) const; - - inline bool encloses(const Rect2 &p_rect) const { - - return (p_rect.position.x >= position.x) && (p_rect.position.y >= position.y) && - ((p_rect.position.x + p_rect.size.x) < (position.x + size.x)) && - ((p_rect.position.y + p_rect.size.y) < (position.y + size.y)); - } - - inline bool has_no_area() const { - - return (size.x <= 0 || size.y <= 0); - } - inline Rect2 clip(const Rect2 &p_rect) const { /// return a clipped rect - - Rect2 new_rect = p_rect; - - if (!intersects(new_rect)) - return Rect2(); - - new_rect.position.x = MAX(p_rect.position.x, position.x); - new_rect.position.y = MAX(p_rect.position.y, position.y); - - Point2 p_rect_end = p_rect.position + p_rect.size; - Point2 end = position + size; - - new_rect.size.x = MIN(p_rect_end.x, end.x) - new_rect.position.x; - new_rect.size.y = MIN(p_rect_end.y, end.y) - new_rect.position.y; - - return new_rect; - } - - inline Rect2 merge(const Rect2 &p_rect) const { ///< return a merged rect - - Rect2 new_rect; - - new_rect.position.x = MIN(p_rect.position.x, position.x); - new_rect.position.y = MIN(p_rect.position.y, position.y); - - new_rect.size.x = MAX(p_rect.position.x + p_rect.size.x, position.x + size.x); - new_rect.size.y = MAX(p_rect.position.y + p_rect.size.y, position.y + size.y); - - new_rect.size = new_rect.size - new_rect.position; //make relative again - - return new_rect; - }; - inline bool has_point(const Point2 &p_point) const { - if (p_point.x < position.x) - return false; - if (p_point.y < position.y) - return false; - - if (p_point.x >= (position.x + size.x)) - return false; - if (p_point.y >= (position.y + size.y)) - return false; - - 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; } - - inline Rect2 grow(real_t p_by) const { - - Rect2 g = *this; - g.position.x -= p_by; - g.position.y -= p_by; - g.size.width += p_by * 2; - g.size.height += p_by * 2; - return g; - } - - inline Rect2 grow_margin(Margin p_margin, real_t p_amount) const { - Rect2 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 Rect2 grow_individual(real_t p_left, real_t p_top, real_t p_right, real_t p_bottom) const { - - Rect2 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; - } - - inline Rect2 expand(const Vector2 &p_vector) const { - - Rect2 r = *this; - r.expand_to(p_vector); - return r; - } - - inline void expand_to(const Vector2 &p_vector) { //in place function for speed - - Vector2 begin = position; - Vector2 end = position + size; - - if (p_vector.x < begin.x) - begin.x = p_vector.x; - if (p_vector.y < begin.y) - begin.y = p_vector.y; - - if (p_vector.x > end.x) - end.x = p_vector.x; - if (p_vector.y > end.y) - end.y = p_vector.y; - - position = begin; - size = end - begin; - } - - inline Rect2 abs() const { - - return Rect2(Point2(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); - } - - operator String() const { return String(position) + ", " + String(size); } - - Rect2() {} - Rect2(real_t p_x, real_t p_y, real_t p_width, real_t p_height) : - position(Point2(p_x, p_y)), - size(Size2(p_width, p_height)) { - } - Rect2(const Point2 &p_pos, const Size2 &p_size) : - position(p_pos), - size(p_size) { - } -}; - -/* INTEGER STUFF */ - -struct Point2i { - - union { - int x; - int width; - }; - union { - int y; - int height; - }; - - _FORCE_INLINE_ int &operator[](int p_idx) { - return p_idx ? y : x; - } - _FORCE_INLINE_ const int &operator[](int p_idx) const { - return p_idx ? y : x; - } - - Point2i operator+(const Point2i &p_v) const; - void operator+=(const Point2i &p_v); - Point2i operator-(const Point2i &p_v) const; - void operator-=(const Point2i &p_v); - Point2i operator*(const Point2i &p_v1) const; - - Point2i operator*(const int &rvalue) const; - void operator*=(const int &rvalue); - - Point2i operator/(const Point2i &p_v1) const; - - Point2i operator/(const int &rvalue) const; - - void operator/=(const int &rvalue); - - Point2i operator-() const; - bool operator<(const Point2i &p_vec2) const { return (x == p_vec2.x) ? (y < p_vec2.y) : (x < p_vec2.x); } - bool operator>(const Point2i &p_vec2) const { return (x == p_vec2.x) ? (y > p_vec2.y) : (x > p_vec2.x); } - - bool operator==(const Point2i &p_vec2) const; - bool operator!=(const Point2i &p_vec2) const; - - real_t get_aspect() const { return width / (real_t)height; } - - operator String() const { return String::num(x) + ", " + String::num(y); } - - operator Vector2() const { return Vector2(x, y); } - inline Point2i(const Vector2 &p_vec2) { - x = (int)p_vec2.x; - y = (int)p_vec2.y; - } - inline Point2i(int p_x, int p_y) { - x = p_x; - y = p_y; - } - inline Point2i() { - x = 0; - y = 0; - } -}; - -typedef Point2i Size2i; - -struct Rect2i { - - Point2i position; - Size2i size; - - const Point2i &get_position() const { return position; } - void set_position(const Point2i &p_pos) { position = p_pos; } - const Point2i &get_size() const { return size; } - void set_size(const Point2i &p_size) { size = p_size; } - - int get_area() const { return size.width * size.height; } - - inline bool intersects(const Rect2i &p_rect) const { - if (position.x > (p_rect.position.x + p_rect.size.width)) - return false; - if ((position.x + size.width) < p_rect.position.x) - return false; - if (position.y > (p_rect.position.y + p_rect.size.height)) - return false; - if ((position.y + size.height) < p_rect.position.y) - return false; - - return true; - } - - inline bool encloses(const Rect2i &p_rect) const { - - return (p_rect.position.x >= position.x) && (p_rect.position.y >= position.y) && - ((p_rect.position.x + p_rect.size.x) < (position.x + size.x)) && - ((p_rect.position.y + p_rect.size.y) < (position.y + size.y)); - } - - inline bool has_no_area() const { - - return (size.x <= 0 || size.y <= 0); - } - inline Rect2i clip(const Rect2i &p_rect) const { /// return a clipped rect - - Rect2i new_rect = p_rect; - - if (!intersects(new_rect)) - return Rect2i(); - - new_rect.position.x = MAX(p_rect.position.x, position.x); - new_rect.position.y = MAX(p_rect.position.y, position.y); - - Point2 p_rect_end = p_rect.position + p_rect.size; - Point2 end = position + size; - - new_rect.size.x = (int)(MIN(p_rect_end.x, end.x) - new_rect.position.x); - new_rect.size.y = (int)(MIN(p_rect_end.y, end.y) - new_rect.position.y); - - return new_rect; - } - - inline Rect2i merge(const Rect2i &p_rect) const { ///< return a merged rect - - Rect2i new_rect; - - new_rect.position.x = MIN(p_rect.position.x, position.x); - new_rect.position.y = MIN(p_rect.position.y, position.y); - - new_rect.size.x = MAX(p_rect.position.x + p_rect.size.x, position.x + size.x); - new_rect.size.y = MAX(p_rect.position.y + p_rect.size.y, position.y + size.y); - - new_rect.size = new_rect.size - new_rect.position; //make relative again - - return new_rect; - }; - bool has_point(const Point2 &p_point) const { - if (p_point.x < position.x) - return false; - if (p_point.y < position.y) - return false; - - if (p_point.x >= (position.x + size.x)) - return false; - if (p_point.y >= (position.y + size.y)) - return false; - - 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; } - - Rect2i grow(int p_by) const { - - Rect2i g = *this; - g.position.x -= p_by; - g.position.y -= p_by; - g.size.width += p_by * 2; - g.size.height += p_by * 2; - return g; - } - - inline void expand_to(const Point2i &p_vector) { - - Point2i begin = position; - Point2i end = position + size; - - if (p_vector.x < begin.x) - begin.x = p_vector.x; - if (p_vector.y < begin.y) - begin.y = p_vector.y; - - if (p_vector.x > end.x) - end.x = p_vector.x; - if (p_vector.y > end.y) - end.y = p_vector.y; - - position = begin; - size = end - begin; - } - - operator String() const { return String(position) + ", " + String(size); } - - operator Rect2() const { return Rect2(position, size); } - Rect2i(const Rect2 &p_r2) : - position(p_r2.position), - size(p_r2.size) { - } - Rect2i() {} - Rect2i(int p_x, int p_y, int p_width, int p_height) : - position(Point2(p_x, p_y)), - size(Size2(p_width, p_height)) { - } - Rect2i(const Point2 &p_pos, const Size2 &p_size) : - position(p_pos), - size(p_size) { - } -}; - -struct Transform2D { - // Warning #1: basis of Transform2D is stored differently from Basis. In terms of elements array, the basis matrix looks like "on paper": - // M = (elements[0][0] elements[1][0]) - // (elements[0][1] elements[1][1]) - // This is such that the columns, which can be interpreted as basis vectors of the coordinate system "painted" on the object, can be accessed as elements[i]. - // Note that this is the opposite of the indices in mathematical texts, meaning: $M_{12}$ in a math book corresponds to elements[1][0] here. - // This requires additional care when working with explicit indices. - // See https://en.wikipedia.org/wiki/Row-_and_column-major_order for further reading. - - // Warning #2: 2D be aware that unlike 3D code, 2D code uses a left-handed coordinate system: Y-axis points down, - // and angle is measure from +X to +Y in a clockwise-fashion. - - Vector2 elements[3]; - - _FORCE_INLINE_ real_t tdotx(const Vector2 &v) const { return elements[0][0] * v.x + elements[1][0] * v.y; } - _FORCE_INLINE_ real_t tdoty(const Vector2 &v) const { return elements[0][1] * v.x + elements[1][1] * v.y; } - - const Vector2 &operator[](int p_idx) const { return elements[p_idx]; } - Vector2 &operator[](int p_idx) { return elements[p_idx]; } - - _FORCE_INLINE_ Vector2 get_axis(int p_axis) const { - ERR_FAIL_INDEX_V(p_axis, 3, Vector2()); - return elements[p_axis]; - } - _FORCE_INLINE_ void set_axis(int p_axis, const Vector2 &p_vec) { - ERR_FAIL_INDEX(p_axis, 3); - elements[p_axis] = p_vec; - } - - void invert(); - Transform2D inverse() const; - - void affine_invert(); - Transform2D affine_inverse() const; - - void set_rotation(real_t p_rot); - real_t get_rotation() const; - _FORCE_INLINE_ void set_rotation_and_scale(real_t p_rot, const Size2 &p_scale); - void rotate(real_t p_phi); - - void scale(const Size2 &p_scale); - void scale_basis(const Size2 &p_scale); - void translate(real_t p_tx, real_t p_ty); - void translate(const Vector2 &p_translation); - - real_t basis_determinant() const; - - Size2 get_scale() const; - - _FORCE_INLINE_ const Vector2 &get_origin() const { return elements[2]; } - _FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { elements[2] = p_origin; } - - Transform2D scaled(const Size2 &p_scale) const; - Transform2D basis_scaled(const Size2 &p_scale) const; - Transform2D translated(const Vector2 &p_offset) const; - Transform2D rotated(real_t p_phi) const; - - Transform2D untranslated() const; - - void orthonormalize(); - Transform2D orthonormalized() const; - - bool operator==(const Transform2D &p_transform) const; - bool operator!=(const Transform2D &p_transform) const; - - void operator*=(const Transform2D &p_transform); - Transform2D operator*(const Transform2D &p_transform) const; - - Transform2D interpolate_with(const Transform2D &p_transform, real_t p_c) const; - - _FORCE_INLINE_ Vector2 basis_xform(const Vector2 &p_vec) const; - _FORCE_INLINE_ Vector2 basis_xform_inv(const Vector2 &p_vec) const; - _FORCE_INLINE_ Vector2 xform(const Vector2 &p_vec) const; - _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; - - operator String() const; - - Transform2D(real_t xx, real_t xy, real_t yx, real_t yy, real_t ox, real_t oy) { - - elements[0][0] = xx; - elements[0][1] = xy; - elements[1][0] = yx; - elements[1][1] = yy; - elements[2][0] = ox; - elements[2][1] = oy; - } - - Transform2D(real_t p_rot, const Vector2 &p_pos); - Transform2D() { - elements[0][0] = 1.0; - elements[1][1] = 1.0; - } -}; - -bool Rect2::intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const { - - //SAT intersection between local and transformed rect2 - - Vector2 xf_points[4] = { - p_xform.xform(p_rect.position), - p_xform.xform(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y)), - p_xform.xform(Vector2(p_rect.position.x, p_rect.position.y + p_rect.size.y)), - p_xform.xform(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y + p_rect.size.y)), - }; - - real_t low_limit; - - //base rect2 first (faster) - - if (xf_points[0].y > position.y) - goto next1; - if (xf_points[1].y > position.y) - goto next1; - if (xf_points[2].y > position.y) - goto next1; - if (xf_points[3].y > position.y) - goto next1; - - return false; - -next1: - - low_limit = position.y + size.y; - - if (xf_points[0].y < low_limit) - goto next2; - if (xf_points[1].y < low_limit) - goto next2; - if (xf_points[2].y < low_limit) - goto next2; - if (xf_points[3].y < low_limit) - goto next2; - - return false; - -next2: - - if (xf_points[0].x > position.x) - goto next3; - if (xf_points[1].x > position.x) - goto next3; - if (xf_points[2].x > position.x) - goto next3; - if (xf_points[3].x > position.x) - goto next3; - - return false; - -next3: - - low_limit = position.x + size.x; - - if (xf_points[0].x < low_limit) - goto next4; - if (xf_points[1].x < low_limit) - goto next4; - if (xf_points[2].x < low_limit) - goto next4; - if (xf_points[3].x < low_limit) - goto next4; - - return false; - -next4: - - Vector2 xf_points2[4] = { - position, - Vector2(position.x + size.x, position.y), - Vector2(position.x, position.y + size.y), - Vector2(position.x + size.x, position.y + size.y), - }; - - real_t maxa = p_xform.elements[0].dot(xf_points2[0]); - real_t mina = maxa; - - real_t dp = p_xform.elements[0].dot(xf_points2[1]); - maxa = MAX(dp, maxa); - mina = MIN(dp, mina); - - dp = p_xform.elements[0].dot(xf_points2[2]); - maxa = MAX(dp, maxa); - mina = MIN(dp, mina); - - dp = p_xform.elements[0].dot(xf_points2[3]); - maxa = MAX(dp, maxa); - mina = MIN(dp, mina); - - real_t maxb = p_xform.elements[0].dot(xf_points[0]); - real_t minb = maxb; - - dp = p_xform.elements[0].dot(xf_points[1]); - maxb = MAX(dp, maxb); - minb = MIN(dp, minb); - - dp = p_xform.elements[0].dot(xf_points[2]); - maxb = MAX(dp, maxb); - minb = MIN(dp, minb); - - dp = p_xform.elements[0].dot(xf_points[3]); - maxb = MAX(dp, maxb); - minb = MIN(dp, minb); - - if (mina > maxb) - return false; - if (minb > maxa) - return false; - - maxa = p_xform.elements[1].dot(xf_points2[0]); - mina = maxa; - - dp = p_xform.elements[1].dot(xf_points2[1]); - maxa = MAX(dp, maxa); - mina = MIN(dp, mina); - - dp = p_xform.elements[1].dot(xf_points2[2]); - maxa = MAX(dp, maxa); - mina = MIN(dp, mina); - - dp = p_xform.elements[1].dot(xf_points2[3]); - maxa = MAX(dp, maxa); - mina = MIN(dp, mina); - - maxb = p_xform.elements[1].dot(xf_points[0]); - minb = maxb; - - dp = p_xform.elements[1].dot(xf_points[1]); - maxb = MAX(dp, maxb); - minb = MIN(dp, minb); - - dp = p_xform.elements[1].dot(xf_points[2]); - maxb = MAX(dp, maxb); - minb = MIN(dp, minb); - - dp = p_xform.elements[1].dot(xf_points[3]); - maxb = MAX(dp, maxb); - minb = MIN(dp, minb); - - if (mina > maxb) - return false; - if (minb > maxa) - return false; - - return true; -} - -Vector2 Transform2D::basis_xform(const Vector2 &p_vec) const { - - return Vector2( - tdotx(p_vec), - tdoty(p_vec)); -} - -Vector2 Transform2D::basis_xform_inv(const Vector2 &p_vec) const { - - return Vector2( - elements[0].dot(p_vec), - elements[1].dot(p_vec)); -} - -Vector2 Transform2D::xform(const Vector2 &p_vec) const { - - return Vector2( - tdotx(p_vec), - tdoty(p_vec)) + - elements[2]; -} -Vector2 Transform2D::xform_inv(const Vector2 &p_vec) const { - - Vector2 v = p_vec - elements[2]; - - return Vector2( - elements[0].dot(v), - elements[1].dot(v)); -} -Rect2 Transform2D::xform(const Rect2 &p_rect) const { - - Vector2 x = elements[0] * p_rect.size.x; - Vector2 y = elements[1] * p_rect.size.y; - Vector2 pos = xform(p_rect.position); - - Rect2 new_rect; - new_rect.position = pos; - new_rect.expand_to(pos + x); - new_rect.expand_to(pos + y); - new_rect.expand_to(pos + x + y); - return new_rect; -} - -void Transform2D::set_rotation_and_scale(real_t p_rot, const Size2 &p_scale) { - - elements[0][0] = Math::cos(p_rot) * p_scale.x; - elements[1][1] = Math::cos(p_rot) * p_scale.y; - elements[1][0] = -Math::sin(p_rot) * p_scale.y; - elements[0][1] = Math::sin(p_rot) * p_scale.x; -} - -Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const { - - Vector2 ends[4] = { - xform_inv(p_rect.position), - xform_inv(Vector2(p_rect.position.x, p_rect.position.y + p_rect.size.y)), - xform_inv(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y + p_rect.size.y)), - xform_inv(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y)) - }; - - Rect2 new_rect; - new_rect.position = ends[0]; - new_rect.expand_to(ends[1]); - new_rect.expand_to(ends[2]); - new_rect.expand_to(ends[3]); - - return new_rect; -} - -#endif diff --git a/core/math/math_defs.h b/core/math/math_defs.h index d3484d8d02..a5feee6eb5 100644 --- a/core/math/math_defs.h +++ b/core/math/math_defs.h @@ -36,30 +36,71 @@ #define CMP_NORMALIZE_TOLERANCE 0.000001 #define CMP_POINT_IN_PLANE_EPSILON 0.00001 +#define Math_SQRT12 0.7071067811865475244008443621048490 +#define Math_SQRT2 1.4142135623730950488016887242 +#define Math_LN2 0.6931471805599453094172321215 +#define Math_TAU 6.2831853071795864769252867666 +#define Math_PI 3.1415926535897932384626433833 +#define Math_E 2.7182818284590452353602874714 +#define Math_INF INFINITY +#define Math_NAN NAN + #ifdef DEBUG_ENABLED #define MATH_CHECKS #endif #define USEC_TO_SEC(m_usec) ((m_usec) / 1000000.0) -/** - * "Real" is a type that will be translated to either floats or fixed depending - * on the compilation setting - */ enum ClockDirection { - CLOCKWISE, COUNTERCLOCKWISE }; -#ifdef REAL_T_IS_DOUBLE +enum Orientation { -typedef double real_t; + HORIZONTAL, + VERTICAL +}; -#else +enum HAlign { -typedef float real_t; + HALIGN_LEFT, + HALIGN_CENTER, + HALIGN_RIGHT +}; + +enum VAlign { + + VALIGN_TOP, + VALIGN_CENTER, + VALIGN_BOTTOM +}; +enum Margin { + + MARGIN_LEFT, + MARGIN_TOP, + MARGIN_RIGHT, + MARGIN_BOTTOM +}; + +enum Corner { + + CORNER_TOP_LEFT, + CORNER_TOP_RIGHT, + CORNER_BOTTOM_RIGHT, + CORNER_BOTTOM_LEFT +}; + +/** + * 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. + */ +#ifdef REAL_T_IS_DOUBLE +typedef double real_t; +#else +typedef float real_t; #endif #endif // MATH_DEFS_H diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp index f060a8e4ab..5c8512d8bd 100644 --- a/core/math/math_funcs.cpp +++ b/core/math/math_funcs.cpp @@ -177,18 +177,3 @@ float Math::random(float from, float to) { float ret = (float)r / (float)RANDOM_MAX; return (ret) * (to - from) + from; } - -int Math::wrapi(int value, int min, int max) { - --max; - int rng = max - min + 1; - value = ((value - min) % rng); - if (value < 0) - return max + 1 + value; - else - return min + value; -} - -float Math::wrapf(float value, float min, float max) { - float rng = max - min; - return min + (value - min) - (rng * floor((value - min) / rng)); -} diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index e15abc6b50..992084a653 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -39,13 +39,6 @@ #include <float.h> #include <math.h> -#define Math_PI 3.14159265358979323846 -#define Math_TAU 6.28318530717958647692 -#define Math_SQRT12 0.7071067811865475244008443621048490 -#define Math_LN2 0.693147180559945309417 -#define Math_INF INFINITY -#define Math_NAN NAN - class Math { static pcg32_random_t default_pcg; @@ -182,8 +175,22 @@ public: static _ALWAYS_INLINE_ float abs(float g) { return absf(g); } static _ALWAYS_INLINE_ int abs(int g) { return g > 0 ? g : -g; } - static _ALWAYS_INLINE_ double fposmod(double p_x, double p_y) { return (p_x >= 0) ? Math::fmod(p_x, p_y) : p_y - Math::fmod(-p_x, p_y); } - static _ALWAYS_INLINE_ float fposmod(float p_x, float p_y) { return (p_x >= 0) ? Math::fmod(p_x, p_y) : p_y - Math::fmod(-p_x, p_y); } + static _ALWAYS_INLINE_ double fposmod(double p_x, double p_y) { + double value = Math::fmod(p_x, p_y); + if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { + value += p_y; + } + value += 0.0; + return value; + } + static _ALWAYS_INLINE_ float fposmod(float p_x, float p_y) { + float value = Math::fmod(p_x, p_y); + if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) { + value += p_y; + } + value += 0.0; + 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; } @@ -209,8 +216,18 @@ 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 int wrapi(int value, int min, int max); - static float wrapf(float value, float min, float max); + 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_ double wrapf(double value, double min, double max) { + double rng = max - min; + return value - (rng * Math::floor((value - min) / rng)); + } + static _ALWAYS_INLINE_ float wrapf(float value, float min, float max) { + float rng = max - min; + return value - (rng * Math::floor((value - min) / rng)); + } // double only, as these functions are mainly used by the editor and not performance-critical, static double ease(double p_x, double p_c); diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp index 189b1ef9b3..7db41756ed 100644 --- a/core/math/matrix3.cpp +++ b/core/math/matrix3.cpp @@ -242,19 +242,12 @@ void Basis::scale_local(const Vector3 &p_scale) { Basis Basis::scaled_local(const Vector3 &p_scale) const { Basis b; - b.set_scale(p_scale); + b.set_diagonal(p_scale); return (*this) * b; } -void Basis::set_scale(const Vector3 &p_scale) { - - set_axis(0, get_axis(0).normalized() * p_scale.x); - set_axis(1, get_axis(1).normalized() * p_scale.y); - set_axis(2, get_axis(2).normalized() * p_scale.z); -} - -Vector3 Basis::get_scale() const { +Vector3 Basis::get_scale_abs() const { return Vector3( Vector3(elements[0][0], elements[1][0], elements[2][0]).length(), @@ -262,7 +255,13 @@ Vector3 Basis::get_scale() const { Vector3(elements[0][2], elements[1][2], elements[2][2]).length()); } -Vector3 Basis::get_signed_scale() const { +Vector3 Basis::get_scale_local() const { + real_t det_sign = determinant() > 0 ? 1 : -1; + return det_sign * Vector3(elements[0].length(), elements[1].length(), elements[2].length()); +} + +// get_scale works with get_rotation, use get_scale_abs if you need to enforce positive signature. +Vector3 Basis::get_scale() const { // FIXME: We are assuming M = R.S (R is rotation and S is scaling), and use polar decomposition to extract R and S. // A polar decomposition is M = O.P, where O is an orthogonal matrix (meaning rotation and reflection) and // P is a positive semi-definite matrix (meaning it contains absolute values of scaling along its diagonal). @@ -342,8 +341,15 @@ void Basis::rotate(const Vector3 &p_euler) { *this = rotated(p_euler); } -// TODO: rename this to get_rotation_euler -Vector3 Basis::get_rotation() const { +Basis Basis::rotated(const Quat &p_quat) const { + return Basis(p_quat) * (*this); +} + +void Basis::rotate(const Quat &p_quat) { + *this = rotated(p_quat); +} + +Vector3 Basis::get_rotation_euler() const { // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). // See the comment in get_scale() for further information. @@ -357,6 +363,20 @@ Vector3 Basis::get_rotation() const { return m.get_euler(); } +Quat Basis::get_rotation_quat() const { + // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, + // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). + // See the comment in get_scale() for further information. + Basis m = orthonormalized(); + real_t det = m.determinant(); + if (det < 0) { + // Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles. + m.scale(Vector3(-1, -1, -1)); + } + + return m.get_quat(); +} + void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const { // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). @@ -371,6 +391,22 @@ void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const { m.get_axis_angle(p_axis, p_angle); } +void Basis::get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const { + // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, + // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). + // See the comment in get_scale() for further information. + Basis m = transposed(); + m.orthonormalize(); + real_t det = m.determinant(); + if (det < 0) { + // Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles. + m.scale(Vector3(-1, -1, -1)); + } + + m.get_axis_angle(p_axis, p_angle); + p_angle = -p_angle; +} + // get_euler_xyz returns a vector containing the Euler angles in the format // (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last // (following the convention they are commonly defined in the literature). @@ -561,10 +597,9 @@ Basis::operator String() const { } Quat Basis::get_quat() const { - //commenting this check because precision issues cause it to fail when it shouldn't - //#ifdef MATH_CHECKS - //ERR_FAIL_COND_V(is_rotation() == false, Quat()); - //#endif +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(is_rotation() == false, Quat()); +#endif real_t trace = elements[0][0] + elements[1][1] + elements[2][2]; real_t temp[4]; @@ -767,3 +802,45 @@ void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) { 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); } + +void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { + set_diagonal(p_scale); + rotate(p_axis, p_phi); +} + +void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale) { + set_diagonal(p_scale); + rotate(p_euler); +} + +void Basis::set_quat_scale(const Quat &p_quat, const Vector3 &p_scale) { + set_diagonal(p_scale); + rotate(p_quat); +} + +void Basis::set_diagonal(const Vector3 p_diag) { + elements[0][0] = p_diag.x; + elements[0][1] = 0; + elements[0][2] = 0; + + elements[1][0] = 0; + elements[1][1] = p_diag.y; + elements[1][2] = 0; + + elements[2][0] = 0; + elements[2][1] = 0; + elements[2][2] = p_diag.z; +} + +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 + + Quat from(*this); + Quat to(target); + + return Basis(from.slerp(to, t)); +} diff --git a/core/math/matrix3.h b/core/math/matrix3.h index c426435729..9ff1a97dc9 100644 --- a/core/math/matrix3.h +++ b/core/math/matrix3.h @@ -81,8 +81,14 @@ public: void rotate(const Vector3 &p_euler); Basis rotated(const Vector3 &p_euler) const; - Vector3 get_rotation() const; + void rotate(const Quat &p_quat); + Basis rotated(const Quat &p_quat) const; + + Vector3 get_rotation_euler() const; void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const; + void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const; + Quat get_rotation_quat() const; + Vector3 get_rotation() const { return get_rotation_euler(); }; Vector3 rotref_posscale_decomposition(Basis &rotref) const; @@ -106,9 +112,13 @@ public: void scale_local(const Vector3 &p_scale); Basis scaled_local(const Vector3 &p_scale) const; - void set_scale(const Vector3 &p_scale); Vector3 get_scale() const; - Vector3 get_signed_scale() const; + Vector3 get_scale_abs() const; + Vector3 get_scale_local() const; + + void set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale); + void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale); + void set_quat_scale(const Quat &p_quat, const Vector3 &p_scale); // transposed dot products _FORCE_INLINE_ real_t tdotx(const Vector3 &v) const { @@ -140,10 +150,14 @@ public: int get_orthogonal_index() const; void set_orthogonal_index(int p_index); + void set_diagonal(const Vector3 p_diag); + bool is_orthogonal() const; bool is_diagonal() const; bool is_rotation() const; + Basis slerp(const Basis &target, const real_t &t) const; + operator String() const; /* create / set */ @@ -217,8 +231,13 @@ public: operator Quat() const { return get_quat(); } Basis(const Quat &p_quat) { set_quat(p_quat); }; + Basis(const Quat &p_quat, const Vector3 &p_scale) { set_quat_scale(p_quat, p_scale); } + Basis(const Vector3 &p_euler) { set_euler(p_euler); } + Basis(const Vector3 &p_euler, const Vector3 &p_scale) { set_euler_scale(p_euler, p_scale); } + Basis(const Vector3 &p_axis, real_t p_phi) { set_axis_angle(p_axis, p_phi); } + Basis(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_phi, p_scale); } _FORCE_INLINE_ Basis(const Vector3 &row0, const Vector3 &row1, const Vector3 &row2) { elements[0] = row0; diff --git a/core/math/quat.cpp b/core/math/quat.cpp index 9aa8b537d2..67c9048a41 100644 --- a/core/math/quat.cpp +++ b/core/math/quat.cpp @@ -89,7 +89,7 @@ void Quat::set_euler_yxz(const Vector3 &p_euler) { set(sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3, sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3, - -sin_a1 * sin_a2 * cos_a3 + cos_a1 * sin_a2 * sin_a3, + -sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3, sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3); } @@ -98,6 +98,9 @@ void Quat::set_euler_yxz(const Vector3 &p_euler) { // and similar for other axes. // 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)); +#endif Basis m(*this); return m.get_euler_yxz(); } @@ -135,11 +138,17 @@ bool Quat::is_normalized() const { } Quat Quat::inverse() const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(is_normalized() == false, 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()); +#endif Quat to1; real_t omega, cosom, sinom, scale0, scale1; @@ -183,7 +192,10 @@ 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()); +#endif const Quat &from = *this; real_t dot = from.dot(q); @@ -202,7 +214,10 @@ 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()); +#endif //the only way to do slerp :| real_t t2 = (1.0 - t) * t * 2; Quat sp = this->slerp(q, t); @@ -215,7 +230,10 @@ Quat::operator String() const { return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w); } -Quat::Quat(const Vector3 &axis, const real_t &angle) { +void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) { +#ifdef MATH_CHECKS + ERR_FAIL_COND(axis.is_normalized() == false); +#endif real_t d = axis.length(); if (d == 0) set(0, 0, 0, 0); diff --git a/core/math/quat.h b/core/math/quat.h index ebc924504b..6dc8d66f60 100644 --- a/core/math/quat.h +++ b/core/math/quat.h @@ -64,11 +64,13 @@ public: Quat slerpni(const Quat &q, const real_t &t) const; Quat cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const; + void set_axis_angle(const Vector3 &axis, const real_t &angle); _FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { r_angle = 2 * Math::acos(w); - r_axis.x = x / Math::sqrt(1 - w * w); - r_axis.y = y / Math::sqrt(1 - w * w); - r_axis.z = z / Math::sqrt(1 - w * w); + real_t r = ((real_t)1) / Math::sqrt(1 - w * w); + r_axis.x = x * r; + r_axis.y = y * r; + r_axis.z = z * r; } void operator*=(const Quat &q); @@ -82,10 +84,12 @@ public: } _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const { - - Quat q = *this * v; - q *= this->inverse(); - return Vector3(q.x, q.y, q.z); +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(is_normalized() == false, v); +#endif + Vector3 u(x, y, z); + Vector3 uv = u.cross(v); + return v + ((uv * w) + u.cross(uv)) * ((real_t)2); } _FORCE_INLINE_ void operator+=(const Quat &q); @@ -115,7 +119,15 @@ public: z = p_z; w = p_w; } - Quat(const Vector3 &axis, const real_t &angle); + Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); } + + Quat(const Vector3 &euler) { set_euler(euler); } + Quat(const Quat &q) { + x = q.x; + y = q.y; + z = q.z; + w = q.w; + } Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc { diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index 102e454e02..9d4f4f66b7 100644 --- a/core/math/quick_hull.cpp +++ b/core/math/quick_hull.cpp @@ -61,10 +61,9 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Vector3 sp = p_points[i].snapped(Vector3(0.0001, 0.0001, 0.0001)); if (valid_cache.has(sp)) { - valid_points[i] = false; - //print_line("INVALIDATED: "+itos(i)); + valid_points.write[i] = false; } else { - valid_points[i] = true; + valid_points.write[i] = true; valid_cache.insert(sp); } } @@ -74,7 +73,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me int longest_axis = aabb.get_longest_axis_index(); //first two vertices are the most distant - int simplex[4]; + int simplex[4] = { 0 }; { real_t max = 0, min = 0; @@ -397,7 +396,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Map<Edge, RetFaceConnect>::Element *F = ret_edges.find(e); ERR_CONTINUE(!F); - List<Geometry::MeshData::Face>::Element *O = F->get().left == E ? F->get().right : F->get().left; ERR_CONTINUE(O == E); ERR_CONTINUE(O == NULL); @@ -426,7 +424,6 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Edge e2(idx, idxn); Map<Edge, RetFaceConnect>::Element *F2 = ret_edges.find(e2); - ERR_CONTINUE(!F2); //change faceconnect, point to this face instead if (F2->get().left == O) @@ -439,6 +436,15 @@ 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; + + if (E->get().right == O) + E->get().right = NULL; + } + ret_edges.erase(F); //remove the edge ret_faces.erase(O); //remove the face } @@ -448,11 +454,10 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me //fill mesh r_mesh.faces.clear(); r_mesh.faces.resize(ret_faces.size()); - //print_line("FACECOUNT: "+itos(r_mesh.faces.size())); int idx = 0; for (List<Geometry::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) { - r_mesh.faces[idx++] = E->get(); + r_mesh.faces.write[idx++] = E->get(); } r_mesh.edges.resize(ret_edges.size()); idx = 0; @@ -461,17 +466,10 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry::MeshData &r_me Geometry::MeshData::Edge e; e.a = E->key().vertices[0]; e.b = E->key().vertices[1]; - r_mesh.edges[idx++] = e; + r_mesh.edges.write[idx++] = e; } r_mesh.vertices = p_points; - //r_mesh.optimize_vertices(); - /* - print_line("FACES: "+itos(r_mesh.faces.size())); - print_line("EDGES: "+itos(r_mesh.edges.size())); - print_line("VERTICES: "+itos(r_mesh.vertices.size())); -*/ - return OK; } diff --git a/core/math/rect2.cpp b/core/math/rect2.cpp new file mode 100644 index 0000000000..480bccdff1 --- /dev/null +++ b/core/math/rect2.cpp @@ -0,0 +1,240 @@ +/*************************************************************************/ +/* rect2.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) */ +/* */ +/* 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 "transform_2d.h" // Includes rect2.h but Rect2 needs Transform2D + +bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos, Point2 *r_normal) const { + + real_t min = 0, max = 1; + int axis = 0; + real_t sign = 0; + + for (int i = 0; i < 2; i++) { + real_t seg_from = p_from[i]; + real_t seg_to = p_to[i]; + real_t box_begin = position[i]; + real_t box_end = box_begin + size[i]; + real_t cmin, cmax; + real_t csign; + + if (seg_from < seg_to) { + + if (seg_from > box_end || seg_to < box_begin) + return false; + real_t length = seg_to - seg_from; + cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0; + cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1; + csign = -1.0; + + } else { + + if (seg_to > box_end || seg_from < box_begin) + return false; + real_t length = seg_to - seg_from; + cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0; + cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1; + csign = 1.0; + } + + if (cmin > min) { + min = cmin; + axis = i; + sign = csign; + } + if (cmax < max) + max = cmax; + if (max < min) + return false; + } + + Vector2 rel = p_to - p_from; + + if (r_normal) { + Vector2 normal; + normal[axis] = sign; + *r_normal = normal; + } + + if (r_pos) + *r_pos = p_from + rel * min; + + return true; +} + +bool Rect2::intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const { + + //SAT intersection between local and transformed rect2 + + Vector2 xf_points[4] = { + p_xform.xform(p_rect.position), + p_xform.xform(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y)), + p_xform.xform(Vector2(p_rect.position.x, p_rect.position.y + p_rect.size.y)), + p_xform.xform(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y + p_rect.size.y)), + }; + + real_t low_limit; + + //base rect2 first (faster) + + if (xf_points[0].y > position.y) + goto next1; + if (xf_points[1].y > position.y) + goto next1; + if (xf_points[2].y > position.y) + goto next1; + if (xf_points[3].y > position.y) + goto next1; + + return false; + +next1: + + low_limit = position.y + size.y; + + if (xf_points[0].y < low_limit) + goto next2; + if (xf_points[1].y < low_limit) + goto next2; + if (xf_points[2].y < low_limit) + goto next2; + if (xf_points[3].y < low_limit) + goto next2; + + return false; + +next2: + + if (xf_points[0].x > position.x) + goto next3; + if (xf_points[1].x > position.x) + goto next3; + if (xf_points[2].x > position.x) + goto next3; + if (xf_points[3].x > position.x) + goto next3; + + return false; + +next3: + + low_limit = position.x + size.x; + + if (xf_points[0].x < low_limit) + goto next4; + if (xf_points[1].x < low_limit) + goto next4; + if (xf_points[2].x < low_limit) + goto next4; + if (xf_points[3].x < low_limit) + goto next4; + + return false; + +next4: + + Vector2 xf_points2[4] = { + position, + Vector2(position.x + size.x, position.y), + Vector2(position.x, position.y + size.y), + Vector2(position.x + size.x, position.y + size.y), + }; + + real_t maxa = p_xform.elements[0].dot(xf_points2[0]); + real_t mina = maxa; + + real_t dp = p_xform.elements[0].dot(xf_points2[1]); + maxa = MAX(dp, maxa); + mina = MIN(dp, mina); + + dp = p_xform.elements[0].dot(xf_points2[2]); + maxa = MAX(dp, maxa); + mina = MIN(dp, mina); + + dp = p_xform.elements[0].dot(xf_points2[3]); + maxa = MAX(dp, maxa); + mina = MIN(dp, mina); + + real_t maxb = p_xform.elements[0].dot(xf_points[0]); + real_t minb = maxb; + + dp = p_xform.elements[0].dot(xf_points[1]); + maxb = MAX(dp, maxb); + minb = MIN(dp, minb); + + dp = p_xform.elements[0].dot(xf_points[2]); + maxb = MAX(dp, maxb); + minb = MIN(dp, minb); + + dp = p_xform.elements[0].dot(xf_points[3]); + maxb = MAX(dp, maxb); + minb = MIN(dp, minb); + + if (mina > maxb) + return false; + if (minb > maxa) + return false; + + maxa = p_xform.elements[1].dot(xf_points2[0]); + mina = maxa; + + dp = p_xform.elements[1].dot(xf_points2[1]); + maxa = MAX(dp, maxa); + mina = MIN(dp, mina); + + dp = p_xform.elements[1].dot(xf_points2[2]); + maxa = MAX(dp, maxa); + mina = MIN(dp, mina); + + dp = p_xform.elements[1].dot(xf_points2[3]); + maxa = MAX(dp, maxa); + mina = MIN(dp, mina); + + maxb = p_xform.elements[1].dot(xf_points[0]); + minb = maxb; + + dp = p_xform.elements[1].dot(xf_points[1]); + maxb = MAX(dp, maxb); + minb = MIN(dp, minb); + + dp = p_xform.elements[1].dot(xf_points[2]); + maxb = MAX(dp, maxb); + minb = MIN(dp, minb); + + dp = p_xform.elements[1].dot(xf_points[3]); + maxb = MAX(dp, maxb); + minb = MIN(dp, minb); + + if (mina > maxb) + return false; + if (minb > maxa) + return false; + + return true; +} diff --git a/core/math/rect2.h b/core/math/rect2.h new file mode 100644 index 0000000000..20329bee0d --- /dev/null +++ b/core/math/rect2.h @@ -0,0 +1,371 @@ +/*************************************************************************/ +/* rect2.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) */ +/* */ +/* 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 RECT2_H +#define RECT2_H + +#include "vector2.h" // also includes math_funcs and ustring + +struct Transform2D; + +struct Rect2 { + + Point2 position; + Size2 size; + + const Vector2 &get_position() const { return position; } + void set_position(const Vector2 &p_pos) { position = p_pos; } + const Vector2 &get_size() const { return size; } + void set_size(const Vector2 &p_size) { size = p_size; } + + real_t get_area() const { return size.width * size.height; } + + inline bool intersects(const Rect2 &p_rect) const { + if (position.x >= (p_rect.position.x + p_rect.size.width)) + return false; + if ((position.x + size.width) <= p_rect.position.x) + return false; + if (position.y >= (p_rect.position.y + p_rect.size.height)) + return false; + if ((position.y + size.height) <= p_rect.position.y) + return false; + + return true; + } + + inline real_t distance_to(const Vector2 &p_point) const { + + real_t dist = 0.0; + bool inside = true; + + if (p_point.x < position.x) { + real_t d = position.x - p_point.x; + dist = inside ? d : MIN(dist, d); + inside = false; + } + if (p_point.y < position.y) { + real_t d = position.y - p_point.y; + dist = inside ? d : MIN(dist, d); + inside = false; + } + if (p_point.x >= (position.x + size.x)) { + real_t d = p_point.x - (position.x + size.x); + dist = inside ? d : MIN(dist, d); + inside = false; + } + if (p_point.y >= (position.y + size.y)) { + real_t d = p_point.y - (position.y + size.y); + dist = inside ? d : MIN(dist, d); + inside = false; + } + + if (inside) + return 0; + else + return dist; + } + + bool intersects_transformed(const Transform2D &p_xform, const Rect2 &p_rect) const; + + bool intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos = NULL, Point2 *r_normal = NULL) const; + + inline bool encloses(const Rect2 &p_rect) const { + + return (p_rect.position.x >= position.x) && (p_rect.position.y >= position.y) && + ((p_rect.position.x + p_rect.size.x) < (position.x + size.x)) && + ((p_rect.position.y + p_rect.size.y) < (position.y + size.y)); + } + + inline bool has_no_area() const { + + return (size.x <= 0 || size.y <= 0); + } + inline Rect2 clip(const Rect2 &p_rect) const { /// return a clipped rect + + Rect2 new_rect = p_rect; + + if (!intersects(new_rect)) + return Rect2(); + + new_rect.position.x = MAX(p_rect.position.x, position.x); + new_rect.position.y = MAX(p_rect.position.y, position.y); + + Point2 p_rect_end = p_rect.position + p_rect.size; + Point2 end = position + size; + + new_rect.size.x = MIN(p_rect_end.x, end.x) - new_rect.position.x; + new_rect.size.y = MIN(p_rect_end.y, end.y) - new_rect.position.y; + + return new_rect; + } + + inline Rect2 merge(const Rect2 &p_rect) const { ///< return a merged rect + + Rect2 new_rect; + + new_rect.position.x = MIN(p_rect.position.x, position.x); + new_rect.position.y = MIN(p_rect.position.y, position.y); + + new_rect.size.x = MAX(p_rect.position.x + p_rect.size.x, position.x + size.x); + new_rect.size.y = MAX(p_rect.position.y + p_rect.size.y, position.y + size.y); + + new_rect.size = new_rect.size - new_rect.position; //make relative again + + return new_rect; + }; + inline bool has_point(const Point2 &p_point) const { + if (p_point.x < position.x) + return false; + if (p_point.y < position.y) + return false; + + if (p_point.x >= (position.x + size.x)) + return false; + if (p_point.y >= (position.y + size.y)) + return false; + + 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; } + + inline Rect2 grow(real_t p_by) const { + + Rect2 g = *this; + g.position.x -= p_by; + g.position.y -= p_by; + g.size.width += p_by * 2; + g.size.height += p_by * 2; + return g; + } + + inline Rect2 grow_margin(Margin p_margin, real_t p_amount) const { + Rect2 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 Rect2 grow_individual(real_t p_left, real_t p_top, real_t p_right, real_t p_bottom) const { + + Rect2 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; + } + + inline Rect2 expand(const Vector2 &p_vector) const { + + Rect2 r = *this; + r.expand_to(p_vector); + return r; + } + + inline void expand_to(const Vector2 &p_vector) { //in place function for speed + + Vector2 begin = position; + Vector2 end = position + size; + + if (p_vector.x < begin.x) + begin.x = p_vector.x; + if (p_vector.y < begin.y) + begin.y = p_vector.y; + + if (p_vector.x > end.x) + end.x = p_vector.x; + if (p_vector.y > end.y) + end.y = p_vector.y; + + position = begin; + size = end - begin; + } + + inline Rect2 abs() const { + + return Rect2(Point2(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0)), size.abs()); + } + + operator String() const { return String(position) + ", " + String(size); } + + Rect2() {} + Rect2(real_t p_x, real_t p_y, real_t p_width, real_t p_height) : + position(Point2(p_x, p_y)), + size(Size2(p_width, p_height)) { + } + Rect2(const Point2 &p_pos, const Size2 &p_size) : + position(p_pos), + size(p_size) { + } +}; + +struct Rect2i { + + Point2i position; + Size2i size; + + const Point2i &get_position() const { return position; } + void set_position(const Point2i &p_position) { position = p_position; } + const Size2i &get_size() const { return size; } + void set_size(const Size2i &p_size) { size = p_size; } + + int get_area() const { return size.width * size.height; } + + inline bool intersects(const Rect2i &p_rect) const { + if (position.x > (p_rect.position.x + p_rect.size.width)) + return false; + if ((position.x + size.width) < p_rect.position.x) + return false; + if (position.y > (p_rect.position.y + p_rect.size.height)) + return false; + if ((position.y + size.height) < p_rect.position.y) + return false; + + return true; + } + + inline bool encloses(const Rect2i &p_rect) const { + + return (p_rect.position.x >= position.x) && (p_rect.position.y >= position.y) && + ((p_rect.position.x + p_rect.size.x) < (position.x + size.x)) && + ((p_rect.position.y + p_rect.size.y) < (position.y + size.y)); + } + + inline bool has_no_area() const { + + return (size.x <= 0 || size.y <= 0); + } + inline Rect2i clip(const Rect2i &p_rect) const { /// return a clipped rect + + Rect2i new_rect = p_rect; + + if (!intersects(new_rect)) + return Rect2i(); + + new_rect.position.x = MAX(p_rect.position.x, position.x); + new_rect.position.y = MAX(p_rect.position.y, position.y); + + Point2 p_rect_end = p_rect.position + p_rect.size; + Point2 end = position + size; + + new_rect.size.x = (int)(MIN(p_rect_end.x, end.x) - new_rect.position.x); + new_rect.size.y = (int)(MIN(p_rect_end.y, end.y) - new_rect.position.y); + + return new_rect; + } + + inline Rect2i merge(const Rect2i &p_rect) const { ///< return a merged rect + + Rect2i new_rect; + + new_rect.position.x = MIN(p_rect.position.x, position.x); + new_rect.position.y = MIN(p_rect.position.y, position.y); + + new_rect.size.x = MAX(p_rect.position.x + p_rect.size.x, position.x + size.x); + new_rect.size.y = MAX(p_rect.position.y + p_rect.size.y, position.y + size.y); + + new_rect.size = new_rect.size - new_rect.position; //make relative again + + return new_rect; + }; + bool has_point(const Point2 &p_point) const { + if (p_point.x < position.x) + return false; + if (p_point.y < position.y) + return false; + + if (p_point.x >= (position.x + size.x)) + return false; + if (p_point.y >= (position.y + size.y)) + return false; + + 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; } + + Rect2i grow(int p_by) const { + + Rect2i g = *this; + g.position.x -= p_by; + g.position.y -= p_by; + g.size.width += p_by * 2; + g.size.height += p_by * 2; + return g; + } + + inline void expand_to(const Point2i &p_vector) { + + Point2i begin = position; + Point2i end = position + size; + + if (p_vector.x < begin.x) + begin.x = p_vector.x; + if (p_vector.y < begin.y) + begin.y = p_vector.y; + + if (p_vector.x > end.x) + end.x = p_vector.x; + if (p_vector.y > end.y) + end.y = p_vector.y; + + position = begin; + size = end - begin; + } + + operator String() const { return String(position) + ", " + String(size); } + + operator Rect2() const { return Rect2(position, size); } + Rect2i(const Rect2 &p_r2) : + position(p_r2.position), + size(p_r2.size) { + } + Rect2i() {} + Rect2i(int p_x, int p_y, int p_width, int p_height) : + position(Point2(p_x, p_y)), + size(Size2(p_width, p_height)) { + } + Rect2i(const Point2 &p_pos, const Size2 &p_size) : + position(p_pos), + size(p_size) { + } +}; + +#endif // RECT2_H diff --git a/core/math/transform.cpp b/core/math/transform.cpp index f727d00e30..976e0f174e 100644 --- a/core/math/transform.cpp +++ b/core/math/transform.cpp @@ -119,20 +119,19 @@ Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c) /* not sure if very "efficient" but good enough? */ - Vector3 src_scale = basis.get_signed_scale(); - Quat src_rot = basis.orthonormalized(); + Vector3 src_scale = basis.get_scale(); + Quat src_rot = basis.get_rotation_quat(); Vector3 src_loc = origin; - Vector3 dst_scale = p_transform.basis.get_signed_scale(); - Quat dst_rot = p_transform.basis; + Vector3 dst_scale = p_transform.basis.get_scale(); + Quat dst_rot = p_transform.basis.get_rotation_quat(); Vector3 dst_loc = p_transform.origin; - Transform dst; //this could be made faster by using a single function in Basis.. - dst.basis = src_rot.slerp(dst_rot, p_c).normalized(); - dst.basis.set_scale(src_scale.linear_interpolate(dst_scale, p_c)); - dst.origin = src_loc.linear_interpolate(dst_loc, p_c); + Transform interp; + interp.basis.set_quat_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.linear_interpolate(dst_scale, p_c)); + interp.origin = src_loc.linear_interpolate(dst_loc, p_c); - return dst; + return interp; } void Transform::scale(const Vector3 &p_scale) { diff --git a/core/math/math_2d.cpp b/core/math/transform_2d.cpp index d2e4101999..4bb763c879 100644 --- a/core/math/math_2d.cpp +++ b/core/math/transform_2d.cpp @@ -1,5 +1,5 @@ /*************************************************************************/ -/* math_2d.cpp */ +/* transform_2d.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,282 +28,7 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#include "math_2d.h" - -real_t Vector2::angle() const { - - return Math::atan2(y, x); -} - -real_t Vector2::length() const { - - return Math::sqrt(x * x + y * y); -} - -real_t Vector2::length_squared() const { - - return x * x + y * y; -} - -void Vector2::normalize() { - - real_t l = x * x + y * y; - if (l != 0) { - - l = Math::sqrt(l); - x /= l; - y /= l; - } -} - -Vector2 Vector2::normalized() const { - - Vector2 v = *this; - v.normalize(); - return v; -} - -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); -} - -real_t Vector2::distance_to(const Vector2 &p_vector2) const { - - return Math::sqrt((x - p_vector2.x) * (x - p_vector2.x) + (y - p_vector2.y) * (y - p_vector2.y)); -} - -real_t Vector2::distance_squared_to(const Vector2 &p_vector2) const { - - return (x - p_vector2.x) * (x - p_vector2.x) + (y - p_vector2.y) * (y - p_vector2.y); -} - -real_t Vector2::angle_to(const Vector2 &p_vector2) const { - - return Math::atan2(cross(p_vector2), dot(p_vector2)); -} - -real_t Vector2::angle_to_point(const Vector2 &p_vector2) const { - - return Math::atan2(y - p_vector2.y, x - p_vector2.x); -} - -real_t Vector2::dot(const Vector2 &p_other) const { - - return x * p_other.x + y * p_other.y; -} - -real_t Vector2::cross(const Vector2 &p_other) const { - - return x * p_other.y - y * p_other.x; -} - -Vector2 Vector2::cross(real_t p_other) const { - - return Vector2(p_other * y, -p_other * x); -} - -Vector2 Vector2::floor() const { - - return Vector2(Math::floor(x), Math::floor(y)); -} - -Vector2 Vector2::rotated(real_t p_by) const { - - Vector2 v; - v.set_rotation(angle() + p_by); - v *= length(); - return v; -} - -Vector2 Vector2::project(const Vector2 &p_vec) const { - - Vector2 v1 = p_vec; - Vector2 v2 = *this; - return v2 * (v1.dot(v2) / v2.dot(v2)); -} - -Vector2 Vector2::snapped(const Vector2 &p_by) const { - - return Vector2( - Math::stepify(x, p_by.x), - Math::stepify(y, p_by.y)); -} - -Vector2 Vector2::clamped(real_t p_len) const { - - real_t l = length(); - Vector2 v = *this; - if (l > 0 && p_len < l) { - - v /= l; - v *= p_len; - } - - return v; -} - -Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_t) const { - - Vector2 p0 = p_pre_a; - Vector2 p1 = *this; - Vector2 p2 = p_b; - Vector2 p3 = p_post_b; - - real_t t = p_t; - real_t t2 = t * t; - real_t t3 = t2 * t; - - Vector2 out; - out = 0.5 * ((p1 * 2.0) + - (-p0 + p2) * t + - (2.0 * p0 - 5.0 * p1 + 4 * p2 - p3) * t2 + - (-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t3); - return out; -} - -// 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()); -#endif - return *this - p_normal * this->dot(p_normal); -} - -Vector2 Vector2::bounce(const Vector2 &p_normal) const { - return -reflect(p_normal); -} - -Vector2 Vector2::reflect(const Vector2 &p_normal) const { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V(p_normal.is_normalized() == false, Vector2()); -#endif - return 2.0 * p_normal * this->dot(p_normal) - *this; -} - -bool Rect2::intersects_segment(const Point2 &p_from, const Point2 &p_to, Point2 *r_pos, Point2 *r_normal) const { - - real_t min = 0, max = 1; - int axis = 0; - real_t sign = 0; - - for (int i = 0; i < 2; i++) { - real_t seg_from = p_from[i]; - real_t seg_to = p_to[i]; - real_t box_begin = position[i]; - real_t box_end = box_begin + size[i]; - real_t cmin, cmax; - real_t csign; - - if (seg_from < seg_to) { - - if (seg_from > box_end || seg_to < box_begin) - return false; - real_t length = seg_to - seg_from; - cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0; - cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1; - csign = -1.0; - - } else { - - if (seg_to > box_end || seg_from < box_begin) - return false; - real_t length = seg_to - seg_from; - cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0; - cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1; - csign = 1.0; - } - - if (cmin > min) { - min = cmin; - axis = i; - sign = csign; - } - if (cmax < max) - max = cmax; - if (max < min) - return false; - } - - Vector2 rel = p_to - p_from; - - if (r_normal) { - Vector2 normal; - normal[axis] = sign; - *r_normal = normal; - } - - if (r_pos) - *r_pos = p_from + rel * min; - - return true; -} - -/* Point2i */ - -Point2i Point2i::operator+(const Point2i &p_v) const { - - return Point2i(x + p_v.x, y + p_v.y); -} -void Point2i::operator+=(const Point2i &p_v) { - - x += p_v.x; - y += p_v.y; -} -Point2i Point2i::operator-(const Point2i &p_v) const { - - return Point2i(x - p_v.x, y - p_v.y); -} -void Point2i::operator-=(const Point2i &p_v) { - - x -= p_v.x; - y -= p_v.y; -} - -Point2i Point2i::operator*(const Point2i &p_v1) const { - - return Point2i(x * p_v1.x, y * p_v1.y); -}; - -Point2i Point2i::operator*(const int &rvalue) const { - - return Point2i(x * rvalue, y * rvalue); -}; -void Point2i::operator*=(const int &rvalue) { - - x *= rvalue; - y *= rvalue; -}; - -Point2i Point2i::operator/(const Point2i &p_v1) const { - - return Point2i(x / p_v1.x, y / p_v1.y); -}; - -Point2i Point2i::operator/(const int &rvalue) const { - - return Point2i(x / rvalue, y / rvalue); -}; - -void Point2i::operator/=(const int &rvalue) { - - x /= rvalue; - y /= rvalue; -}; - -Point2i Point2i::operator-() const { - - return Point2i(-x, -y); -} - -bool Point2i::operator==(const Point2i &p_vec2) const { - - return x == p_vec2.x && y == p_vec2.y; -} -bool Point2i::operator!=(const Point2i &p_vec2) const { - - return x != p_vec2.x || y != p_vec2.y; -} +#include "transform_2d.h" void Transform2D::invert() { // FIXME: this function assumes the basis is a rotation matrix, with no scaling. diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h new file mode 100644 index 0000000000..bf73755f0d --- /dev/null +++ b/core/math/transform_2d.h @@ -0,0 +1,201 @@ +/*************************************************************************/ +/* transform_2d.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) */ +/* */ +/* 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 TRANSFORM_2D_H +#define TRANSFORM_2D_H + +#include "rect2.h" // also includes vector2, math_funcs, and ustring + +struct Transform2D { + // Warning #1: basis of Transform2D is stored differently from Basis. In terms of elements array, the basis matrix looks like "on paper": + // M = (elements[0][0] elements[1][0]) + // (elements[0][1] elements[1][1]) + // This is such that the columns, which can be interpreted as basis vectors of the coordinate system "painted" on the object, can be accessed as elements[i]. + // Note that this is the opposite of the indices in mathematical texts, meaning: $M_{12}$ in a math book corresponds to elements[1][0] here. + // This requires additional care when working with explicit indices. + // See https://en.wikipedia.org/wiki/Row-_and_column-major_order for further reading. + + // Warning #2: 2D be aware that unlike 3D code, 2D code uses a left-handed coordinate system: Y-axis points down, + // and angle is measure from +X to +Y in a clockwise-fashion. + + Vector2 elements[3]; + + _FORCE_INLINE_ real_t tdotx(const Vector2 &v) const { return elements[0][0] * v.x + elements[1][0] * v.y; } + _FORCE_INLINE_ real_t tdoty(const Vector2 &v) const { return elements[0][1] * v.x + elements[1][1] * v.y; } + + const Vector2 &operator[](int p_idx) const { return elements[p_idx]; } + Vector2 &operator[](int p_idx) { return elements[p_idx]; } + + _FORCE_INLINE_ Vector2 get_axis(int p_axis) const { + ERR_FAIL_INDEX_V(p_axis, 3, Vector2()); + return elements[p_axis]; + } + _FORCE_INLINE_ void set_axis(int p_axis, const Vector2 &p_vec) { + ERR_FAIL_INDEX(p_axis, 3); + elements[p_axis] = p_vec; + } + + void invert(); + Transform2D inverse() const; + + void affine_invert(); + Transform2D affine_inverse() const; + + void set_rotation(real_t p_rot); + real_t get_rotation() const; + _FORCE_INLINE_ void set_rotation_and_scale(real_t p_rot, const Size2 &p_scale); + void rotate(real_t p_phi); + + void scale(const Size2 &p_scale); + void scale_basis(const Size2 &p_scale); + void translate(real_t p_tx, real_t p_ty); + void translate(const Vector2 &p_translation); + + real_t basis_determinant() const; + + Size2 get_scale() const; + + _FORCE_INLINE_ const Vector2 &get_origin() const { return elements[2]; } + _FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { elements[2] = p_origin; } + + Transform2D scaled(const Size2 &p_scale) const; + Transform2D basis_scaled(const Size2 &p_scale) const; + Transform2D translated(const Vector2 &p_offset) const; + Transform2D rotated(real_t p_phi) const; + + Transform2D untranslated() const; + + void orthonormalize(); + Transform2D orthonormalized() const; + + bool operator==(const Transform2D &p_transform) const; + bool operator!=(const Transform2D &p_transform) const; + + void operator*=(const Transform2D &p_transform); + Transform2D operator*(const Transform2D &p_transform) const; + + Transform2D interpolate_with(const Transform2D &p_transform, real_t p_c) const; + + _FORCE_INLINE_ Vector2 basis_xform(const Vector2 &p_vec) const; + _FORCE_INLINE_ Vector2 basis_xform_inv(const Vector2 &p_vec) const; + _FORCE_INLINE_ Vector2 xform(const Vector2 &p_vec) const; + _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; + + operator String() const; + + Transform2D(real_t xx, real_t xy, real_t yx, real_t yy, real_t ox, real_t oy) { + + elements[0][0] = xx; + elements[0][1] = xy; + elements[1][0] = yx; + elements[1][1] = yy; + elements[2][0] = ox; + elements[2][1] = oy; + } + + Transform2D(real_t p_rot, const Vector2 &p_pos); + Transform2D() { + elements[0][0] = 1.0; + elements[1][1] = 1.0; + } +}; + +Vector2 Transform2D::basis_xform(const Vector2 &p_vec) const { + + return Vector2( + tdotx(p_vec), + tdoty(p_vec)); +} + +Vector2 Transform2D::basis_xform_inv(const Vector2 &p_vec) const { + + return Vector2( + elements[0].dot(p_vec), + elements[1].dot(p_vec)); +} + +Vector2 Transform2D::xform(const Vector2 &p_vec) const { + + return Vector2( + tdotx(p_vec), + tdoty(p_vec)) + + elements[2]; +} +Vector2 Transform2D::xform_inv(const Vector2 &p_vec) const { + + Vector2 v = p_vec - elements[2]; + + return Vector2( + elements[0].dot(v), + elements[1].dot(v)); +} +Rect2 Transform2D::xform(const Rect2 &p_rect) const { + + Vector2 x = elements[0] * p_rect.size.x; + Vector2 y = elements[1] * p_rect.size.y; + Vector2 pos = xform(p_rect.position); + + Rect2 new_rect; + new_rect.position = pos; + new_rect.expand_to(pos + x); + new_rect.expand_to(pos + y); + new_rect.expand_to(pos + x + y); + return new_rect; +} + +void Transform2D::set_rotation_and_scale(real_t p_rot, const Size2 &p_scale) { + + elements[0][0] = Math::cos(p_rot) * p_scale.x; + elements[1][1] = Math::cos(p_rot) * p_scale.y; + elements[1][0] = -Math::sin(p_rot) * p_scale.y; + elements[0][1] = Math::sin(p_rot) * p_scale.x; +} + +Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const { + + Vector2 ends[4] = { + xform_inv(p_rect.position), + xform_inv(Vector2(p_rect.position.x, p_rect.position.y + p_rect.size.y)), + xform_inv(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y + p_rect.size.y)), + xform_inv(Vector2(p_rect.position.x + p_rect.size.x, p_rect.position.y)) + }; + + Rect2 new_rect; + new_rect.position = ends[0]; + new_rect.expand_to(ends[1]); + new_rect.expand_to(ends[2]); + new_rect.expand_to(ends[3]); + + return new_rect; +} + +#endif // TRANSFORM_2D_H diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index edd4ad3441..5475f733c3 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -88,6 +88,26 @@ int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, in return index; } +void TriangleMesh::get_indices(PoolVector<int> *r_triangles_indices) const { + + if (!valid) + return; + + const int triangles_num = triangles.size(); + + // Parse vertices indices + PoolVector<Triangle>::Read triangles_read = triangles.read(); + + r_triangles_indices->resize(triangles_num * 3); + PoolVector<int>::Write r_indices_write = r_triangles_indices->write(); + + for (int i = 0; i < triangles_num; ++i) { + r_indices_write[3 * i + 0] = triangles_read[i].indices[0]; + r_indices_write[3 * i + 1] = triangles_read[i].indices[1]; + r_indices_write[3 * i + 2] = triangles_read[i].indices[2]; + } +} + void TriangleMesh::create(const PoolVector<Vector3> &p_faces) { valid = false; @@ -490,6 +510,222 @@ bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, V return inters; } +bool TriangleMesh::intersect_convex_shape(const Plane *p_planes, int p_plane_count) const { + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); + + //p_fully_inside = true; + + enum { + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, + + }; + + int level = 0; + + PoolVector<Triangle>::Read trianglesr = triangles.read(); + PoolVector<Vector3>::Read verticesr = vertices.read(); + PoolVector<BVH>::Read bvhr = bvh.read(); + + const Triangle *triangleptr = trianglesr.ptr(); + const Vector3 *vertexptr = verticesr.ptr(); + int pos = bvh.size() - 1; + const BVH *bvhptr = bvhr.ptr(); + + stack[0] = pos; + while (true) { + + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &b = bvhptr[node]; + bool done = false; + + switch (stack[level] >> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + bool valid = b.aabb.intersects_convex_shape(p_planes, p_plane_count); + if (!valid) { + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + + if (b.face_index >= 0) { + + const Triangle &s = triangleptr[b.face_index]; + + for (int j = 0; j < 3; ++j) { + const Vector3 &point = vertexptr[s.indices[j]]; + const Vector3 &next_point = vertexptr[s.indices[(j + 1) % 3]]; + Vector3 res; + bool over = true; + for (int i = 0; i < p_plane_count; i++) { + const Plane &p = p_planes[i]; + + if (p.intersects_segment(point, next_point, &res)) { + bool inisde = true; + for (int k = 0; k < p_plane_count; k++) { + if (k == i) continue; + const Plane &pp = p_planes[k]; + if (pp.is_point_over(res)) { + inisde = false; + break; + } + } + if (inisde) return true; + } + + if (p.is_point_over(point)) { + over = false; + break; + } + } + if (over) return true; + } + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; + } + } + continue; + } + case VISIT_LEFT_BIT: { + + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.left | TEST_AABB_BIT; + level++; + continue; + } + case VISIT_RIGHT_BIT: { + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.right | TEST_AABB_BIT; + level++; + continue; + } + case VISIT_DONE_BIT: { + + if (level == 0) { + done = true; + break; + } else + level--; + continue; + } + } + + if (done) + break; + } + + return false; +} + +bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, Vector3 p_scale) const { + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth); + + enum { + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, + + }; + + int level = 0; + + PoolVector<Triangle>::Read trianglesr = triangles.read(); + PoolVector<Vector3>::Read verticesr = vertices.read(); + PoolVector<BVH>::Read bvhr = bvh.read(); + + Transform scale(Basis().scaled(p_scale)); + + const Triangle *triangleptr = trianglesr.ptr(); + const Vector3 *vertexptr = verticesr.ptr(); + int pos = bvh.size() - 1; + const BVH *bvhptr = bvhr.ptr(); + + stack[0] = pos; + while (true) { + + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &b = bvhptr[node]; + bool done = false; + + switch (stack[level] >> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + bool intersects = scale.xform(b.aabb).intersects_convex_shape(p_planes, p_plane_count); + if (!intersects) return false; + + bool inside = scale.xform(b.aabb).inside_convex_shape(p_planes, p_plane_count); + if (inside) { + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + + if (b.face_index >= 0) { + const Triangle &s = triangleptr[b.face_index]; + for (int j = 0; j < 3; ++j) { + Vector3 point = scale.xform(vertexptr[s.indices[j]]); + for (int i = 0; i < p_plane_count; i++) { + const Plane &p = p_planes[i]; + if (p.is_point_over(point)) return false; + } + } + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; + } + } + continue; + } + case VISIT_LEFT_BIT: { + + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.left | TEST_AABB_BIT; + level++; + continue; + } + case VISIT_RIGHT_BIT: { + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = b.right | TEST_AABB_BIT; + level++; + continue; + } + case VISIT_DONE_BIT: { + + if (level == 0) { + done = true; + break; + } else + level--; + continue; + } + } + + if (done) + break; + } + + return true; +} + bool TriangleMesh::is_valid() const { return valid; diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index 9f145f2afb..bf793fc50f 100644 --- a/core/math/triangle_mesh.h +++ b/core/math/triangle_mesh.h @@ -89,9 +89,15 @@ public: bool is_valid() const; bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const; bool intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal) const; + bool intersect_convex_shape(const Plane *p_planes, int p_plane_count) const; + bool inside_convex_shape(const Plane *p_planes, int p_plane_count, Vector3 p_scale = Vector3(1, 1, 1)) const; Vector3 get_area_normal(const AABB &p_aabb) const; PoolVector<Face3> get_faces() const; + PoolVector<Triangle> get_triangles() const { return triangles; } + PoolVector<Vector3> get_vertices() const { return vertices; } + void get_indices(PoolVector<int> *p_triangles_indices) const; + void create(const PoolVector<Vector3> &p_faces); TriangleMesh(); }; diff --git a/core/math/triangulate.cpp b/core/math/triangulate.cpp index 5bae74ac7e..0edc0ea039 100644 --- a/core/math/triangulate.cpp +++ b/core/math/triangulate.cpp @@ -51,7 +51,8 @@ real_t Triangulate::get_area(const Vector<Vector2> &contour) { bool Triangulate::is_inside_triangle(real_t Ax, real_t Ay, real_t Bx, real_t By, real_t Cx, real_t Cy, - real_t Px, real_t Py) + real_t Px, real_t Py, + bool include_edges) { real_t ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy; @@ -74,10 +75,14 @@ bool Triangulate::is_inside_triangle(real_t Ax, real_t Ay, cCROSSap = cx * apy - cy * apx; bCROSScp = bx * cpy - by * cpx; - return ((aCROSSbp > 0.0) && (bCROSScp > 0.0) && (cCROSSap > 0.0)); + if (include_edges) { + return ((aCROSSbp > 0.0) && (bCROSScp > 0.0) && (cCROSSap > 0.0)); + } else { + return ((aCROSSbp >= 0.0) && (bCROSScp >= 0.0) && (cCROSSap >= 0.0)); + } }; -bool Triangulate::snip(const Vector<Vector2> &p_contour, int u, int v, int w, int n, const Vector<int> &V) { +bool Triangulate::snip(const Vector<Vector2> &p_contour, int u, int v, int w, int n, const Vector<int> &V, bool relaxed) { int p; real_t Ax, Ay, Bx, By, Cx, Cy, Px, Py; const Vector2 *contour = &p_contour[0]; @@ -91,13 +96,20 @@ bool Triangulate::snip(const Vector<Vector2> &p_contour, int u, int v, int w, in Cx = contour[V[w]].x; Cy = contour[V[w]].y; - if (CMP_EPSILON > (((Bx - Ax) * (Cy - Ay)) - ((By - Ay) * (Cx - Ax)))) return false; + // It can happen that the triangulation ends up with three aligned vertices to deal with. + // In this scenario, making the check below strict may reject the possibility of + // forming a last triangle with these aligned vertices, preventing the triangulatiom + // from completing. + // To avoid that we allow zero-area triangles if all else failed. + float threshold = relaxed ? -CMP_EPSILON : CMP_EPSILON; + + if (threshold > (((Bx - Ax) * (Cy - Ay)) - ((By - Ay) * (Cx - Ax)))) return false; for (p = 0; p < n; p++) { if ((p == u) || (p == v) || (p == w)) continue; Px = contour[V[p]].x; Py = contour[V[p]].y; - if (is_inside_triangle(Ax, Ay, Bx, By, Cx, Cy, Px, Py)) return false; + if (is_inside_triangle(Ax, Ay, Bx, By, Cx, Cy, Px, Py, relaxed)) return false; } return true; @@ -116,10 +128,12 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &resul if (0.0 < get_area(contour)) for (int v = 0; v < n; v++) - V[v] = v; + V.write[v] = v; else for (int v = 0; v < n; v++) - V[v] = (n - 1) - v; + V.write[v] = (n - 1) - v; + + bool relaxed = false; int nv = n; @@ -129,8 +143,20 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &resul for (int v = nv - 1; nv > 2;) { /* if we loop, it is probably a non-simple polygon */ if (0 >= (count--)) { - //** Triangulate: ERROR - probable bad polygon! - return false; + if (relaxed) { + //** Triangulate: ERROR - probable bad polygon! + return false; + } else { + // There may be aligned vertices that the strict + // checks prevent from triangulating. In this situation + // we are better off adding flat triangles than + // failing, so we relax the checks and try one last + // round. + // Only relaxing the constraints as a last resort avoids + // degenerate triangles when they aren't necessary. + count = 2 * nv; + relaxed = true; + } } /* three consecutive vertices in current polygon, <u,v,w> */ @@ -141,7 +167,7 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &resul int w = v + 1; if (nv <= w) w = 0; /* next */ - if (snip(contour, u, v, w, nv, V)) { + if (snip(contour, u, v, w, nv, V, relaxed)) { int a, b, c, s, t; /* true names of the vertices */ @@ -156,7 +182,7 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour, Vector<int> &resul /* remove v from remaining polygon */ for (s = v, t = v + 1; t < nv; s++, t++) - V[s] = V[t]; + V.write[s] = V[t]; nv--; diff --git a/core/math/triangulate.h b/core/math/triangulate.h index e336dc5756..a0f56f5f27 100644 --- a/core/math/triangulate.h +++ b/core/math/triangulate.h @@ -31,7 +31,7 @@ #ifndef TRIANGULATE_H #define TRIANGULATE_H -#include "math_2d.h" +#include "vector2.h" /* http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml @@ -51,10 +51,11 @@ public: static bool is_inside_triangle(real_t Ax, real_t Ay, real_t Bx, real_t By, real_t Cx, real_t Cy, - real_t Px, real_t Py); + real_t Px, real_t Py, + bool include_edges); private: - static bool snip(const Vector<Vector2> &p_contour, int u, int v, int w, int n, const Vector<int> &V); + static bool snip(const Vector<Vector2> &p_contour, int u, int v, int w, int n, const Vector<int> &V, bool relaxed); }; #endif diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp new file mode 100644 index 0000000000..84c9f0fca6 --- /dev/null +++ b/core/math/vector2.cpp @@ -0,0 +1,250 @@ +/*************************************************************************/ +/* vector2.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) */ +/* */ +/* 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 "vector2.h" + +real_t Vector2::angle() const { + + return Math::atan2(y, x); +} + +real_t Vector2::length() const { + + return Math::sqrt(x * x + y * y); +} + +real_t Vector2::length_squared() const { + + return x * x + y * y; +} + +void Vector2::normalize() { + + real_t l = x * x + y * y; + if (l != 0) { + + l = Math::sqrt(l); + x /= l; + y /= l; + } +} + +Vector2 Vector2::normalized() const { + + Vector2 v = *this; + v.normalize(); + return v; +} + +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); +} + +real_t Vector2::distance_to(const Vector2 &p_vector2) const { + + return Math::sqrt((x - p_vector2.x) * (x - p_vector2.x) + (y - p_vector2.y) * (y - p_vector2.y)); +} + +real_t Vector2::distance_squared_to(const Vector2 &p_vector2) const { + + return (x - p_vector2.x) * (x - p_vector2.x) + (y - p_vector2.y) * (y - p_vector2.y); +} + +real_t Vector2::angle_to(const Vector2 &p_vector2) const { + + return Math::atan2(cross(p_vector2), dot(p_vector2)); +} + +real_t Vector2::angle_to_point(const Vector2 &p_vector2) const { + + return Math::atan2(y - p_vector2.y, x - p_vector2.x); +} + +real_t Vector2::dot(const Vector2 &p_other) const { + + return x * p_other.x + y * p_other.y; +} + +real_t Vector2::cross(const Vector2 &p_other) const { + + return x * p_other.y - y * p_other.x; +} + +Vector2 Vector2::floor() const { + + return Vector2(Math::floor(x), Math::floor(y)); +} + +Vector2 Vector2::ceil() const { + + return Vector2(Math::ceil(x), Math::ceil(y)); +} + +Vector2 Vector2::round() const { + + return Vector2(Math::round(x), Math::round(y)); +} + +Vector2 Vector2::rotated(real_t p_by) const { + + Vector2 v; + v.set_rotation(angle() + p_by); + v *= length(); + return v; +} + +Vector2 Vector2::project(const Vector2 &p_b) const { + return p_b * (dot(p_b) / p_b.length_squared()); +} + +Vector2 Vector2::snapped(const Vector2 &p_by) const { + + return Vector2( + Math::stepify(x, p_by.x), + Math::stepify(y, p_by.y)); +} + +Vector2 Vector2::clamped(real_t p_len) const { + + real_t l = length(); + Vector2 v = *this; + if (l > 0 && p_len < l) { + + v /= l; + v *= p_len; + } + + return v; +} + +Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_t) const { + + Vector2 p0 = p_pre_a; + Vector2 p1 = *this; + Vector2 p2 = p_b; + Vector2 p3 = p_post_b; + + real_t t = p_t; + real_t t2 = t * t; + real_t t3 = t2 * t; + + Vector2 out; + out = 0.5 * ((p1 * 2.0) + + (-p0 + p2) * t + + (2.0 * p0 - 5.0 * p1 + 4 * p2 - p3) * t2 + + (-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t3); + return out; +} + +// 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()); +#endif + return *this - p_normal * this->dot(p_normal); +} + +Vector2 Vector2::bounce(const Vector2 &p_normal) const { + return -reflect(p_normal); +} + +Vector2 Vector2::reflect(const Vector2 &p_normal) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(p_normal.is_normalized() == false, Vector2()); +#endif + return 2.0 * p_normal * this->dot(p_normal) - *this; +} + +/* Vector2i */ + +Vector2i Vector2i::operator+(const Vector2i &p_v) const { + + return Vector2i(x + p_v.x, y + p_v.y); +} +void Vector2i::operator+=(const Vector2i &p_v) { + + x += p_v.x; + y += p_v.y; +} +Vector2i Vector2i::operator-(const Vector2i &p_v) const { + + return Vector2i(x - p_v.x, y - p_v.y); +} +void Vector2i::operator-=(const Vector2i &p_v) { + + x -= p_v.x; + y -= p_v.y; +} + +Vector2i Vector2i::operator*(const Vector2i &p_v1) const { + + return Vector2i(x * p_v1.x, y * p_v1.y); +}; + +Vector2i Vector2i::operator*(const int &rvalue) const { + + return Vector2i(x * rvalue, y * rvalue); +}; +void Vector2i::operator*=(const int &rvalue) { + + x *= rvalue; + y *= rvalue; +}; + +Vector2i Vector2i::operator/(const Vector2i &p_v1) const { + + return Vector2i(x / p_v1.x, y / p_v1.y); +}; + +Vector2i Vector2i::operator/(const int &rvalue) const { + + return Vector2i(x / rvalue, y / rvalue); +}; + +void Vector2i::operator/=(const int &rvalue) { + + x /= rvalue; + y /= rvalue; +}; + +Vector2i Vector2i::operator-() const { + + return Vector2i(-x, -y); +} + +bool Vector2i::operator==(const Vector2i &p_vec2) const { + + return x == p_vec2.x && y == p_vec2.y; +} +bool Vector2i::operator!=(const Vector2i &p_vec2) const { + + return x != p_vec2.x || y != p_vec2.y; +} diff --git a/core/math/vector2.h b/core/math/vector2.h new file mode 100644 index 0000000000..fbcdc80b60 --- /dev/null +++ b/core/math/vector2.h @@ -0,0 +1,316 @@ +/*************************************************************************/ +/* vector2.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) */ +/* */ +/* 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 VECTOR2_H +#define VECTOR2_H + +#include "math_funcs.h" +#include "ustring.h" + +struct Vector2i; + +struct Vector2 { + + union { + real_t x; + real_t width; + }; + union { + real_t y; + real_t height; + }; + + _FORCE_INLINE_ real_t &operator[](int p_idx) { + return p_idx ? y : x; + } + _FORCE_INLINE_ const real_t &operator[](int p_idx) const { + return p_idx ? y : x; + } + + void normalize(); + Vector2 normalized() const; + bool is_normalized() const; + + real_t length() const; + real_t length_squared() const; + + real_t distance_to(const Vector2 &p_vector2) const; + 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; + + real_t dot(const Vector2 &p_other) const; + real_t cross(const Vector2 &p_other) const; + Vector2 project(const Vector2 &p_b) const; + + Vector2 plane_project(real_t p_d, const Vector2 &p_vec) const; + + Vector2 clamped(real_t p_len) const; + + _FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t); + _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 slide(const Vector2 &p_normal) const; + Vector2 bounce(const Vector2 &p_normal) const; + Vector2 reflect(const Vector2 &p_normal) const; + + Vector2 operator+(const Vector2 &p_v) const; + void operator+=(const Vector2 &p_v); + Vector2 operator-(const Vector2 &p_v) const; + void operator-=(const Vector2 &p_v); + Vector2 operator*(const Vector2 &p_v1) const; + + Vector2 operator*(const real_t &rvalue) const; + void operator*=(const real_t &rvalue); + void operator*=(const Vector2 &rvalue) { *this = *this * rvalue; } + + Vector2 operator/(const Vector2 &p_v1) const; + + Vector2 operator/(const real_t &rvalue) const; + + void operator/=(const real_t &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); } + + real_t angle() const; + + void set_rotation(real_t p_radians) { + + x = Math::cos(p_radians); + y = Math::sin(p_radians); + } + + _FORCE_INLINE_ Vector2 abs() const { + + return Vector2(Math::abs(x), Math::abs(y)); + } + + Vector2 rotated(real_t p_by) const; + Vector2 tangent() const { + + return Vector2(y, -x); + } + + Vector2 floor() const; + Vector2 ceil() const; + Vector2 round() const; + Vector2 snapped(const Vector2 &p_by) const; + real_t aspect() const { return width / height; } + + operator String() const { return String::num(x) + ", " + String::num(y); } + + _FORCE_INLINE_ Vector2(real_t p_x, real_t p_y) { + x = p_x; + y = p_y; + } + _FORCE_INLINE_ Vector2() { + x = 0; + y = 0; + } +}; + +_FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2 &p_vec) const { + + return p_vec - *this * (dot(p_vec) - p_d); +} + +_FORCE_INLINE_ Vector2 operator*(real_t p_scalar, const Vector2 &p_vec) { + + return p_vec * p_scalar; +} + +_FORCE_INLINE_ Vector2 Vector2::operator+(const Vector2 &p_v) const { + + return Vector2(x + p_v.x, y + p_v.y); +} +_FORCE_INLINE_ void Vector2::operator+=(const Vector2 &p_v) { + + x += p_v.x; + y += p_v.y; +} +_FORCE_INLINE_ Vector2 Vector2::operator-(const Vector2 &p_v) const { + + return Vector2(x - p_v.x, y - p_v.y); +} +_FORCE_INLINE_ void Vector2::operator-=(const Vector2 &p_v) { + + x -= p_v.x; + y -= p_v.y; +} + +_FORCE_INLINE_ Vector2 Vector2::operator*(const Vector2 &p_v1) const { + + return Vector2(x * p_v1.x, y * p_v1.y); +}; + +_FORCE_INLINE_ Vector2 Vector2::operator*(const real_t &rvalue) const { + + return Vector2(x * rvalue, y * rvalue); +}; +_FORCE_INLINE_ void Vector2::operator*=(const real_t &rvalue) { + + x *= rvalue; + y *= rvalue; +}; + +_FORCE_INLINE_ Vector2 Vector2::operator/(const Vector2 &p_v1) const { + + return Vector2(x / p_v1.x, y / p_v1.y); +}; + +_FORCE_INLINE_ Vector2 Vector2::operator/(const real_t &rvalue) const { + + return Vector2(x / rvalue, y / rvalue); +}; + +_FORCE_INLINE_ void Vector2::operator/=(const real_t &rvalue) { + + x /= rvalue; + y /= rvalue; +}; + +_FORCE_INLINE_ Vector2 Vector2::operator-() const { + + return Vector2(-x, -y); +} + +_FORCE_INLINE_ bool Vector2::operator==(const Vector2 &p_vec2) const { + + return x == p_vec2.x && y == p_vec2.y; +} +_FORCE_INLINE_ bool Vector2::operator!=(const Vector2 &p_vec2) const { + + return x != p_vec2.x || y != p_vec2.y; +} + +Vector2 Vector2::linear_interpolate(const Vector2 &p_b, real_t p_t) const { + + Vector2 res = *this; + + res.x += (p_t * (p_b.x - x)); + res.y += (p_t * (p_b.y - y)); + + return res; +} + +Vector2 Vector2::slerp(const Vector2 &p_b, real_t p_t) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(is_normalized() == false, Vector2()); +#endif + real_t theta = angle_to(p_b); + return rotated(theta * p_t); +} + +Vector2 Vector2::linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t) { + + Vector2 res = p_a; + + res.x += (p_t * (p_b.x - p_a.x)); + res.y += (p_t * (p_b.y - p_a.y)); + + return res; +} + +typedef Vector2 Size2; +typedef Vector2 Point2; + +/* INTEGER STUFF */ + +struct Vector2i { + + union { + int x; + int width; + }; + union { + int y; + int height; + }; + + _FORCE_INLINE_ int &operator[](int p_idx) { + return p_idx ? y : x; + } + _FORCE_INLINE_ const int &operator[](int p_idx) const { + return p_idx ? y : x; + } + + Vector2i operator+(const Vector2i &p_v) const; + void operator+=(const Vector2i &p_v); + Vector2i operator-(const Vector2i &p_v) const; + void operator-=(const Vector2i &p_v); + Vector2i operator*(const Vector2i &p_v1) const; + + Vector2i operator*(const int &rvalue) const; + void operator*=(const int &rvalue); + + Vector2i operator/(const Vector2i &p_v1) const; + + Vector2i operator/(const int &rvalue) const; + + void operator/=(const int &rvalue); + + Vector2i operator-() const; + bool operator<(const Vector2i &p_vec2) const { return (x == p_vec2.x) ? (y < p_vec2.y) : (x < p_vec2.x); } + bool operator>(const Vector2i &p_vec2) const { return (x == p_vec2.x) ? (y > p_vec2.y) : (x > p_vec2.x); } + + bool operator==(const Vector2i &p_vec2) const; + bool operator!=(const Vector2i &p_vec2) const; + + real_t get_aspect() const { return width / (real_t)height; } + + operator String() const { return String::num(x) + ", " + String::num(y); } + + operator Vector2() const { return Vector2(x, y); } + inline Vector2i(const Vector2 &p_vec2) { + x = (int)p_vec2.x; + y = (int)p_vec2.y; + } + inline Vector2i(int p_x, int p_y) { + x = p_x; + y = p_y; + } + inline Vector2i() { + x = 0; + y = 0; + } +}; + +typedef Vector2i Size2i; +typedef Vector2i Point2i; + +#endif // VECTOR2_H diff --git a/core/math/vector3.h b/core/math/vector3.h index 10ec4f5641..5f0e8919ff 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -91,6 +91,7 @@ struct Vector3 { /* Static Methods between 2 vector3s */ _FORCE_INLINE_ Vector3 linear_interpolate(const Vector3 &p_b, real_t p_t) const; + _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; @@ -103,10 +104,13 @@ struct Vector3 { _FORCE_INLINE_ Vector3 floor() const; _FORCE_INLINE_ Vector3 sign() const; _FORCE_INLINE_ Vector3 ceil() const; + _FORCE_INLINE_ Vector3 round() const; _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 project(const Vector3 &p_b) const; + _FORCE_INLINE_ real_t angle_to(const Vector3 &p_b) const; _FORCE_INLINE_ Vector3 slide(const Vector3 &p_normal) const; @@ -204,6 +208,11 @@ Vector3 Vector3::ceil() const { return Vector3(Math::ceil(x), Math::ceil(y), Math::ceil(z)); } +Vector3 Vector3::round() const { + + return Vector3(Math::round(x), Math::round(y), Math::round(z)); +} + Vector3 Vector3::linear_interpolate(const Vector3 &p_b, real_t p_t) const { return Vector3( @@ -212,6 +221,15 @@ Vector3 Vector3::linear_interpolate(const Vector3 &p_b, real_t p_t) const { z + (p_t * (p_b.z - z))); } +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); +} + real_t Vector3::distance_to(const Vector3 &p_b) const { return (p_b - *this).length(); @@ -222,6 +240,10 @@ real_t Vector3::distance_squared_to(const Vector3 &p_b) const { return (p_b - *this).length_squared(); } +Vector3 Vector3::project(const Vector3 &p_b) const { + return p_b * (dot(p_b) / p_b.length_squared()); +} + real_t Vector3::angle_to(const Vector3 &p_b) const { return Math::atan2(cross(p_b).length(), dot(p_b)); |