diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp new file mode 100644 index 0000000..8a1fe14 --- /dev/null +++ b/core/math/aabb.cpp @@ -0,0 +1,413 @@ +/*************************************************************************/ +/* aabb.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "aabb.h" + +#include "core/math/math.h" + +real_t AABB::get_area() const { + return size.x * size.y * size.z; +} + +bool AABB::operator==(const AABB &p_rval) const { + return ((position == p_rval.position) && (size == p_rval.size)); +} +bool AABB::operator!=(const AABB &p_rval) const { + return ((position != p_rval.position) || (size != p_rval.size)); +} + +bool AABB::create_from_points(const Vector &p_points) { + if (!p_points.size()) { + return false; + } + + Vector3 begin = p_points[0]; + Vector3 end = begin; + + for (int n = 1; n < p_points.size(); n++) { + const Vector3 &pt = p_points[n]; + + if (pt.x < begin.x) { + begin.x = pt.x; + } + if (pt.y < begin.y) { + begin.y = pt.y; + } + if (pt.z < begin.z) { + begin.z = pt.z; + } + + if (pt.x > end.x) { + end.x = pt.x; + } + if (pt.y > end.y) { + end.y = pt.y; + } + if (pt.z > end.z) { + end.z = pt.z; + } + } + + position = begin; + size = end - begin; + + return true; +} + +void AABB::merge_with(const AABB &p_aabb) { + Vector3 beg_1, beg_2; + Vector3 end_1, end_2; + Vector3 min, max; + + beg_1 = position; + beg_2 = p_aabb.position; + end_1 = Vector3(size.x, size.y, size.z) + beg_1; + end_2 = Vector3(p_aabb.size.x, p_aabb.size.y, p_aabb.size.z) + beg_2; + + min.x = (beg_1.x < beg_2.x) ? beg_1.x : beg_2.x; + min.y = (beg_1.y < beg_2.y) ? beg_1.y : beg_2.y; + min.z = (beg_1.z < beg_2.z) ? beg_1.z : beg_2.z; + + max.x = (end_1.x > end_2.x) ? end_1.x : end_2.x; + max.y = (end_1.y > end_2.y) ? end_1.y : end_2.y; + max.z = (end_1.z > end_2.z) ? end_1.z : end_2.z; + + position = min; + size = max - min; +} + +bool AABB::is_equal_approx(const AABB &p_aabb) const { + return position.is_equal_approx(p_aabb.position) && size.is_equal_approx(p_aabb.size); +} + +AABB AABB::intersection(const AABB &p_aabb) const { + Vector3 src_min = position; + Vector3 src_max = position + size; + Vector3 dst_min = p_aabb.position; + Vector3 dst_max = p_aabb.position + p_aabb.size; + + Vector3 min, max; + + if (src_min.x > dst_max.x || src_max.x < dst_min.x) { + return AABB(); + } else { + min.x = (src_min.x > dst_min.x) ? src_min.x : dst_min.x; + max.x = (src_max.x < dst_max.x) ? src_max.x : dst_max.x; + } + + if (src_min.y > dst_max.y || src_max.y < dst_min.y) { + return AABB(); + } else { + min.y = (src_min.y > dst_min.y) ? src_min.y : dst_min.y; + max.y = (src_max.y < dst_max.y) ? src_max.y : dst_max.y; + } + + if (src_min.z > dst_max.z || src_max.z < dst_min.z) { + return AABB(); + } else { + min.z = (src_min.z > dst_min.z) ? src_min.z : dst_min.z; + max.z = (src_max.z < dst_max.z) ? src_max.z : dst_max.z; + } + + return AABB(min, max - min); +} + +bool AABB::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip, Vector3 *r_normal) const { + Vector3 c1, c2; + Vector3 end = position + size; + real_t near = -1e20; + real_t far = 1e20; + int axis = 0; + + for (int i = 0; i < 3; i++) { + if (p_dir[i] == 0) { + if ((p_from[i] < position[i]) || (p_from[i] > end[i])) { + return false; + } + } else { // ray not parallel to planes in this direction + c1[i] = (position[i] - p_from[i]) / p_dir[i]; + c2[i] = (end[i] - p_from[i]) / p_dir[i]; + + if (c1[i] > c2[i]) { + SWAP(c1, c2); + } + if (c1[i] > near) { + near = c1[i]; + axis = i; + } + if (c2[i] < far) { + far = c2[i]; + } + if ((near > far) || (far < 0)) { + return false; + } + } + } + + if (r_clip) { + *r_clip = c1; + } + if (r_normal) { + *r_normal = Vector3(); + (*r_normal)[axis] = p_dir[axis] ? -1 : 1; + } + + return true; +} + +bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip, Vector3 *r_normal) const { + real_t min = 0, max = 1; + int axis = 0; + real_t sign = 0; + + for (int i = 0; i < 3; 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; + } + } + + Vector3 rel = p_to - p_from; + + if (r_normal) { + Vector3 normal; + normal[axis] = sign; + *r_normal = normal; + } + + if (r_clip) { + *r_clip = p_from + rel * min; + } + + return true; +} + +bool AABB::intersects_plane(const Plane &p_plane) const { + Vector3 points[8] = { + Vector3(position.x, position.y, position.z), + Vector3(position.x, position.y, position.z + size.z), + Vector3(position.x, position.y + size.y, position.z), + Vector3(position.x, position.y + size.y, position.z + size.z), + Vector3(position.x + size.x, position.y, position.z), + Vector3(position.x + size.x, position.y, position.z + size.z), + Vector3(position.x + size.x, position.y + size.y, position.z), + Vector3(position.x + size.x, position.y + size.y, position.z + size.z), + }; + + bool over = false; + bool under = false; + + for (int i = 0; i < 8; i++) { + if (p_plane.distance_to(points[i]) > 0) { + over = true; + } else { + under = true; + } + } + + return under && over; +} + +Vector3 AABB::get_longest_axis() const { + Vector3 axis(1, 0, 0); + real_t max_size = size.x; + + if (size.y > max_size) { + axis = Vector3(0, 1, 0); + max_size = size.y; + } + + if (size.z > max_size) { + axis = Vector3(0, 0, 1); + } + + return axis; +} +int AABB::get_longest_axis_index() const { + int axis = 0; + real_t max_size = size.x; + + if (size.y > max_size) { + axis = 1; + max_size = size.y; + } + + if (size.z > max_size) { + axis = 2; + } + + return axis; +} + +Vector3 AABB::get_shortest_axis() const { + Vector3 axis(1, 0, 0); + real_t max_size = size.x; + + if (size.y < max_size) { + axis = Vector3(0, 1, 0); + max_size = size.y; + } + + if (size.z < max_size) { + axis = Vector3(0, 0, 1); + } + + return axis; +} +int AABB::get_shortest_axis_index() const { + int axis = 0; + real_t max_size = size.x; + + if (size.y < max_size) { + axis = 1; + max_size = size.y; + } + + if (size.z < max_size) { + axis = 2; + } + + return axis; +} + +AABB AABB::merge(const AABB &p_with) const { + AABB aabb = *this; + aabb.merge_with(p_with); + return aabb; +} +AABB AABB::expand(const Vector3 &p_vector) const { + AABB aabb = *this; + aabb.expand_to(p_vector); + return aabb; +} +AABB AABB::grow(real_t p_by) const { + AABB aabb = *this; + aabb.grow_by(p_by); + return aabb; +} + +void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const { + ERR_FAIL_INDEX(p_edge, 12); + switch (p_edge) { + case 0: { + r_from = Vector3(position.x + size.x, position.y, position.z); + r_to = Vector3(position.x, position.y, position.z); + } break; + case 1: { + r_from = Vector3(position.x + size.x, position.y, position.z + size.z); + r_to = Vector3(position.x + size.x, position.y, position.z); + } break; + case 2: { + r_from = Vector3(position.x, position.y, position.z + size.z); + r_to = Vector3(position.x + size.x, position.y, position.z + size.z); + + } break; + case 3: { + r_from = Vector3(position.x, position.y, position.z); + r_to = Vector3(position.x, position.y, position.z + size.z); + + } break; + case 4: { + r_from = Vector3(position.x, position.y + size.y, position.z); + r_to = Vector3(position.x + size.x, position.y + size.y, position.z); + } break; + case 5: { + r_from = Vector3(position.x + size.x, position.y + size.y, position.z); + r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z); + } break; + case 6: { + r_from = Vector3(position.x + size.x, position.y + size.y, position.z + size.z); + r_to = Vector3(position.x, position.y + size.y, position.z + size.z); + + } break; + case 7: { + r_from = Vector3(position.x, position.y + size.y, position.z + size.z); + r_to = Vector3(position.x, position.y + size.y, position.z); + + } break; + case 8: { + r_from = Vector3(position.x, position.y, position.z + size.z); + r_to = Vector3(position.x, position.y + size.y, position.z + size.z); + + } break; + case 9: { + r_from = Vector3(position.x, position.y, position.z); + r_to = Vector3(position.x, position.y + size.y, position.z); + + } break; + case 10: { + r_from = Vector3(position.x + size.x, position.y, position.z); + r_to = Vector3(position.x + size.x, position.y + size.y, position.z); + + } break; + case 11: { + r_from = Vector3(position.x + size.x, position.y, position.z + size.z); + r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z); + + } break; + } +} + +AABB::operator String() const { + return String() + position + " - " + size; +} diff --git a/core/math/aabb.h b/core/math/aabb.h new file mode 100644 index 0000000..a0b1eed --- /dev/null +++ b/core/math/aabb.h @@ -0,0 +1,425 @@ +/*************************************************************************/ +/* aabb.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 AABB_H +#define AABB_H + +#include "core/math/math_defs.h" +#include "core/math/plane.h" +#include "core/math/vector3.h" +#include "core/typedefs.h" +#include "core/containers/vector.h" +#include "core/string.h" + +/** + * AABB / AABB (Axis Aligned Bounding Box) + * This is implemented by a point (position) and the box size + */ + +class AABB { +public: + Vector3 position; + Vector3 size; + + real_t get_area() const; /// get area + _FORCE_INLINE_ bool has_no_area() const { + return (size.x <= 0 || size.y <= 0 || size.z <= 0); + } + + _FORCE_INLINE_ bool has_no_surface() const { + return (size.x <= 0 && size.y <= 0 && size.z <= 0); + } + + const Vector3 &get_position() const { return position; } + void set_position(const Vector3 &p_pos) { position = p_pos; } + const Vector3 &get_size() const { return size; } + void set_size(const Vector3 &p_size) { size = p_size; } + Vector3 get_center() const { return position + (size * 0.5); } + + bool operator==(const AABB &p_rval) const; + bool operator!=(const AABB &p_rval) const; + + bool is_equal_approx(const AABB &p_aabb) const; + _FORCE_INLINE_ bool intersects(const AABB &p_aabb) const; /// Both AABBs overlap + _FORCE_INLINE_ bool intersects_inclusive(const AABB &p_aabb) const; /// Both AABBs (or their faces) overlap + _FORCE_INLINE_ bool encloses(const AABB &p_aabb) const; /// p_aabb is completely inside this + + AABB merge(const AABB &p_with) const; + void merge_with(const AABB &p_aabb); ///merge with another AABB + AABB intersection(const AABB &p_aabb) const; ///get box where two intersect, empty if no intersection occurs + bool intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip = nullptr, Vector3 *r_normal = nullptr) const; + bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip = nullptr, Vector3 *r_normal = nullptr) const; + _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 Vector3 *p_points, int p_point_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; + _FORCE_INLINE_ Vector3 get_support(const Vector3 &p_normal) const; + + Vector3 get_longest_axis() const; + int get_longest_axis_index() const; + _FORCE_INLINE_ real_t get_longest_axis_size() const; + + Vector3 get_shortest_axis() const; + int get_shortest_axis_index() const; + _FORCE_INLINE_ real_t get_shortest_axis_size() const; + + AABB grow(real_t p_by) const; + _FORCE_INLINE_ void grow_by(real_t p_amount); + + void get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const; + _FORCE_INLINE_ Vector3 get_endpoint(int p_point) const; + + AABB expand(const Vector3 &p_vector) const; + _FORCE_INLINE_ void project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const; + _FORCE_INLINE_ void expand_to(const Vector3 &p_vector); /** expand to contain a point if necessary */ + bool create_from_points(const Vector &p_points); + + _FORCE_INLINE_ AABB abs() const { + return AABB(Vector3(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0), position.z + MIN(size.z, 0)), size.abs()); + } + + operator String() const; + + _FORCE_INLINE_ AABB() {} + inline AABB(const Vector3 &p_pos, const Vector3 &p_size) : + position(p_pos), + size(p_size) { + } +}; + +inline bool AABB::intersects(const AABB &p_aabb) const { + if (position.x >= (p_aabb.position.x + p_aabb.size.x)) { + return false; + } + if ((position.x + size.x) <= p_aabb.position.x) { + return false; + } + if (position.y >= (p_aabb.position.y + p_aabb.size.y)) { + return false; + } + if ((position.y + size.y) <= p_aabb.position.y) { + return false; + } + if (position.z >= (p_aabb.position.z + p_aabb.size.z)) { + return false; + } + if ((position.z + size.z) <= p_aabb.position.z) { + return false; + } + + return true; +} + +inline bool AABB::intersects_inclusive(const AABB &p_aabb) const { + if (position.x > (p_aabb.position.x + p_aabb.size.x)) { + return false; + } + if ((position.x + size.x) < p_aabb.position.x) { + return false; + } + if (position.y > (p_aabb.position.y + p_aabb.size.y)) { + return false; + } + if ((position.y + size.y) < p_aabb.position.y) { + return false; + } + if (position.z > (p_aabb.position.z + p_aabb.size.z)) { + return false; + } + if ((position.z + size.z) < p_aabb.position.z) { + return false; + } + + return true; +} + +inline bool AABB::encloses(const AABB &p_aabb) const { + Vector3 src_min = position; + Vector3 src_max = position + size; + Vector3 dst_min = p_aabb.position; + Vector3 dst_max = p_aabb.position + p_aabb.size; + + return ( + (src_min.x <= dst_min.x) && + (src_max.x > dst_max.x) && + (src_min.y <= dst_min.y) && + (src_max.y > dst_max.y) && + (src_min.z <= dst_min.z) && + (src_max.z > dst_max.z)); +} + +Vector3 AABB::get_support(const Vector3 &p_normal) const { + Vector3 half_extents = size * 0.5; + Vector3 ofs = position + half_extents; + + return Vector3( + (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) + + ofs; +} + +Vector3 AABB::get_endpoint(int p_point) const { + switch (p_point) { + case 0: + return Vector3(position.x, position.y, position.z); + case 1: + return Vector3(position.x, position.y, position.z + size.z); + case 2: + return Vector3(position.x, position.y + size.y, position.z); + case 3: + return Vector3(position.x, position.y + size.y, position.z + size.z); + case 4: + return Vector3(position.x + size.x, position.y, position.z); + case 5: + return Vector3(position.x + size.x, position.y, position.z + size.z); + case 6: + return Vector3(position.x + size.x, position.y + size.y, position.z); + case 7: + return Vector3(position.x + size.x, position.y + size.y, position.z + size.z); + }; + + //ERR_FAIL_V(Vector3()); + + return Vector3(); +} + +bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_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; + } + } + + // Make sure all points in the shape aren't fully separated from the AABB on + // each axis. + int bad_point_counts_positive[3] = { 0 }; + int bad_point_counts_negative[3] = { 0 }; + + for (int k = 0; k < 3; k++) { + for (int i = 0; i < p_point_count; i++) { + if (p_points[i].coordinates[k] > ofs.coordinates[k] + half_extents.coordinates[k]) { + bad_point_counts_positive[k]++; + } + if (p_points[i].coordinates[k] < ofs.coordinates[k] - half_extents.coordinates[k]) { + bad_point_counts_negative[k]++; + } + } + + if (bad_point_counts_negative[k] == p_point_count) { + return false; + } + if (bad_point_counts_positive[k] == p_point_count) { + return false; + } + } + + 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) { + return false; + } + if (p_point.y < position.y) { + return false; + } + if (p_point.z < position.z) { + return false; + } + if (p_point.x > position.x + size.x) { + return false; + } + if (p_point.y > position.y + size.y) { + return false; + } + if (p_point.z > position.z + size.z) { + return false; + } + + return true; +} + +inline void AABB::expand_to(const Vector3 &p_vector) { + Vector3 begin = position; + Vector3 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.z < begin.z) { + begin.z = p_vector.z; + } + + if (p_vector.x > end.x) { + end.x = p_vector.x; + } + if (p_vector.y > end.y) { + end.y = p_vector.y; + } + if (p_vector.z > end.z) { + end.z = p_vector.z; + } + + position = begin; + size = end - begin; +} + +void AABB::project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const { + Vector3 half_extents(size.x * 0.5, size.y * 0.5, size.z * 0.5); + Vector3 center(position.x + half_extents.x, position.y + half_extents.y, position.z + half_extents.z); + + real_t length = p_plane.normal.abs().dot(half_extents); + real_t distance = p_plane.distance_to(center); + r_min = distance - length; + r_max = distance + length; +} + +inline real_t AABB::get_longest_axis_size() const { + real_t max_size = size.x; + + if (size.y > max_size) { + max_size = size.y; + } + + if (size.z > max_size) { + max_size = size.z; + } + + return max_size; +} + +inline real_t AABB::get_shortest_axis_size() const { + real_t max_size = size.x; + + if (size.y < max_size) { + max_size = size.y; + } + + if (size.z < max_size) { + max_size = size.z; + } + + return max_size; +} + +bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t t0, real_t t1) const { + real_t divx = 1.0 / p_dir.x; + real_t divy = 1.0 / p_dir.y; + real_t divz = 1.0 / p_dir.z; + + Vector3 upbound = position + size; + real_t tmin, tmax, tymin, tymax, tzmin, tzmax; + if (p_dir.x >= 0) { + tmin = (position.x - p_from.x) * divx; + tmax = (upbound.x - p_from.x) * divx; + } else { + tmin = (upbound.x - p_from.x) * divx; + tmax = (position.x - p_from.x) * divx; + } + if (p_dir.y >= 0) { + tymin = (position.y - p_from.y) * divy; + tymax = (upbound.y - p_from.y) * divy; + } else { + tymin = (upbound.y - p_from.y) * divy; + tymax = (position.y - p_from.y) * divy; + } + if ((tmin > tymax) || (tymin > tmax)) { + return false; + } + if (tymin > tmin) { + tmin = tymin; + } + if (tymax < tmax) { + tmax = tymax; + } + if (p_dir.z >= 0) { + tzmin = (position.z - p_from.z) * divz; + tzmax = (upbound.z - p_from.z) * divz; + } else { + tzmin = (upbound.z - p_from.z) * divz; + tzmax = (position.z - p_from.z) * divz; + } + if ((tmin > tzmax) || (tzmin > tmax)) { + return false; + } + if (tzmin > tmin) { + tmin = tzmin; + } + if (tzmax < tmax) { + tmax = tzmax; + } + return ((tmin < t1) && (tmax > t0)); +} + +void AABB::grow_by(real_t p_amount) { + position.x -= p_amount; + position.y -= p_amount; + position.z -= p_amount; + size.x += 2.0 * p_amount; + size.y += 2.0 * p_amount; + size.z += 2.0 * p_amount; +} + +#endif // AABB_H diff --git a/core/math/basis.cpp b/core/math/basis.cpp new file mode 100644 index 0000000..0e15a4c --- /dev/null +++ b/core/math/basis.cpp @@ -0,0 +1,1023 @@ +/*************************************************************************/ +/* basis.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "basis.h" + +#include "core/math/math.h" +#include "core/error_macros.h" + +#define cofac(row1, col1, row2, col2) \ + (elements[row1][col1] * elements[row2][col2] - elements[row1][col2] * elements[row2][col1]) + +void Basis::from_z(const Vector3 &p_z) { + if (Math::abs(p_z.z) > Math_SQRT12) { + // choose p in y-z plane + real_t a = p_z[1] * p_z[1] + p_z[2] * p_z[2]; + real_t k = 1.0 / Math::sqrt(a); + elements[0] = Vector3(0, -p_z[2] * k, p_z[1] * k); + elements[1] = Vector3(a * k, -p_z[0] * elements[0][2], p_z[0] * elements[0][1]); + } else { + // choose p in x-y plane + real_t a = p_z.x * p_z.x + p_z.y * p_z.y; + real_t k = 1.0 / Math::sqrt(a); + elements[0] = Vector3(-p_z.y * k, p_z.x * k, 0); + elements[1] = Vector3(-p_z.z * elements[0].y, p_z.z * elements[0].x, a * k); + } + elements[2] = p_z; +} + +void Basis::invert() { + real_t co[3] = { + cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1) + }; + real_t det = elements[0][0] * co[0] + + elements[0][1] * co[1] + + elements[0][2] * co[2]; +#ifdef MATH_CHECKS + ERR_FAIL_COND(det == 0); +#endif + real_t s = 1.0 / det; + + set(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); +} + +void Basis::orthonormalize() { + // Gram-Schmidt Process + + Vector3 x = get_axis(0); + Vector3 y = get_axis(1); + Vector3 z = get_axis(2); + + x.normalize(); + y = (y - x * (x.dot(y))); + y.normalize(); + z = (z - x * (x.dot(z)) - y * (y.dot(z))); + z.normalize(); + + set_axis(0, x); + set_axis(1, y); + set_axis(2, z); +} + +Basis Basis::orthonormalized() const { + Basis c = *this; + c.orthonormalize(); + return c; +} + +bool Basis::is_orthogonal() const { + Basis identity; + Basis m = (*this) * transposed(); + + return m.is_equal_approx(identity); +} + +bool Basis::is_diagonal() const { + return ( + Math::is_zero_approx(elements[0][1]) && Math::is_zero_approx(elements[0][2]) && + Math::is_zero_approx(elements[1][0]) && Math::is_zero_approx(elements[1][2]) && + Math::is_zero_approx(elements[2][0]) && Math::is_zero_approx(elements[2][1])); +} + +bool Basis::is_rotation() const { + return Math::is_equal_approx(determinant(), 1)/*, (real_t)UNIT_EPSILON)*/ && is_orthogonal(); +} + +bool Basis::is_symmetric() const { + if (!Math::is_equal_approx_ratio(elements[0][1], elements[1][0], UNIT_EPSILON)) { + return false; + } + if (!Math::is_equal_approx_ratio(elements[0][2], elements[2][0], UNIT_EPSILON)) { + return false; + } + if (!Math::is_equal_approx_ratio(elements[1][2], elements[2][1], UNIT_EPSILON)) { + return false; + } + + return true; +} + +Basis Basis::diagonalize() { +//NOTE: only implemented for symmetric matrices +//with the Jacobi iterative method method +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(!is_symmetric(), Basis()); +#endif + const int ite_max = 1024; + + real_t off_matrix_norm_2 = elements[0][1] * elements[0][1] + elements[0][2] * elements[0][2] + elements[1][2] * elements[1][2]; + + int ite = 0; + Basis acc_rot; + while (off_matrix_norm_2 > CMP_EPSILON2 && ite++ < ite_max) { + real_t el01_2 = elements[0][1] * elements[0][1]; + real_t el02_2 = elements[0][2] * elements[0][2]; + real_t el12_2 = elements[1][2] * elements[1][2]; + // Find the pivot element + int i, j; + if (el01_2 > el02_2) { + if (el12_2 > el01_2) { + i = 1; + j = 2; + } else { + i = 0; + j = 1; + } + } else { + if (el12_2 > el02_2) { + i = 1; + j = 2; + } else { + i = 0; + j = 2; + } + } + + // Compute the rotation angle + real_t angle; + if (Math::is_equal_approx(elements[j][j], elements[i][i])) { + angle = Math_PI / 4; + } else { + angle = 0.5 * Math::atan(2 * elements[i][j] / (elements[j][j] - elements[i][i])); + } + + // Compute the rotation matrix + Basis rot; + rot.elements[i][i] = rot.elements[j][j] = Math::cos(angle); + rot.elements[i][j] = -(rot.elements[j][i] = Math::sin(angle)); + + // Update the off matrix norm + off_matrix_norm_2 -= elements[i][j] * elements[i][j]; + + // Apply the rotation + *this = rot * *this * rot.transposed(); + acc_rot = rot * acc_rot; + } + + return acc_rot; +} + +Basis Basis::inverse() const { + Basis inv = *this; + inv.invert(); + return inv; +} + +void Basis::transpose() { + SWAP(elements[0][1], elements[1][0]); + SWAP(elements[0][2], elements[2][0]); + SWAP(elements[1][2], elements[2][1]); +} + +Basis Basis::transposed() const { + Basis tr = *this; + tr.transpose(); + return tr; +} + +// Multiplies the matrix from left by the scaling matrix: M -> S.M +// See the comment for Basis::rotated for further explanation. +void Basis::scale(const Vector3 &p_scale) { + elements[0][0] *= p_scale.x; + elements[0][1] *= p_scale.x; + elements[0][2] *= p_scale.x; + elements[1][0] *= p_scale.y; + elements[1][1] *= p_scale.y; + elements[1][2] *= p_scale.y; + elements[2][0] *= p_scale.z; + elements[2][1] *= p_scale.z; + elements[2][2] *= p_scale.z; +} + +Basis Basis::scaled(const Vector3 &p_scale) const { + Basis m = *this; + m.scale(p_scale); + return m; +} + +void Basis::scale_local(const Vector3 &p_scale) { + // performs a scaling in object-local coordinate system: + // M -> (M.S.Minv).M = M.S. + *this = scaled_local(p_scale); +} + +Basis Basis::scaled_local(const Vector3 &p_scale) const { + Basis b; + b.set_diagonal(p_scale); + + return (*this) * b; +} + +Vector3 Basis::get_scale_abs() const { + return Vector3( + Vector3(elements[0][0], elements[1][0], elements[2][0]).length(), + Vector3(elements[0][1], elements[1][1], elements[2][1]).length(), + Vector3(elements[0][2], elements[1][2], elements[2][2]).length()); +} + +Vector3 Basis::get_scale_local() const { + real_t det_sign = SGN(determinant()); + return Vector3(elements[0].length(), elements[1].length(), elements[2].length()) * det_sign; +} + +// 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). + // + // Despite being different from what we want to achieve, we can nevertheless make use of polar decomposition + // here as follows. We can split O into a rotation and a reflection as O = R.Q, and obtain M = R.S where + // we defined S = Q.P. Now, R is a proper rotation matrix and S is a (signed) scaling matrix, + // which can involve negative scalings. However, there is a catch: unlike the polar decomposition of M = O.P, + // the decomposition of O into a rotation and reflection matrix as O = R.Q is not unique. + // Therefore, we are going to do this decomposition by sticking to a particular convention. + // This may lead to confusion for some users though. + // + // The convention we use here is to absorb the sign flip into the scaling matrix. + // The same convention is also used in other similar functions such as get_rotation_axis_angle, get_rotation, ... + // + // A proper way to get rid of this issue would be to store the scaling values (or at least their signs) + // as a part of Basis. However, if we go that path, we need to disable direct (write) access to the + // matrix elements. + // + // The rotation part of this decomposition is returned by get_rotation* functions. + real_t det_sign = SGN(determinant()); + return get_scale_abs() * det_sign; +} + +// Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S. +// Returns the rotation-reflection matrix via reference argument, and scaling information is returned as a Vector3. +// This (internal) function is too specific and named too ugly to expose to users, and probably there's no need to do so. +Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(determinant() == 0, Vector3()); + + Basis m = transposed() * (*this); + ERR_FAIL_COND_V(!m.is_diagonal(), Vector3()); +#endif + Vector3 scale = get_scale(); + Basis inv_scale = Basis().scaled(scale.inverse()); // this will also absorb the sign of scale + rotref = (*this) * inv_scale; + +#ifdef MATH_CHECKS + ERR_FAIL_COND_V(!rotref.is_orthogonal(), Vector3()); +#endif + return scale.abs(); +} + +// Multiplies the matrix from left by the rotation matrix: M -> R.M +// Note that this does *not* rotate the matrix itself. +// +// The main use of Basis is as Transform.basis, which is used a the transformation matrix +// of 3D object. Rotate here refers to rotation of the object (which is R * (*this)), +// not the matrix itself (which is R * (*this) * R.transposed()). +Basis Basis::rotated(const Vector3 &p_axis, real_t p_phi) const { + return Basis(p_axis, p_phi) * (*this); +} + +void Basis::rotate(const Vector3 &p_axis, real_t p_phi) { + *this = rotated(p_axis, p_phi); +} + +void Basis::rotate_local(const Vector3 &p_axis, real_t p_phi) { + // performs a rotation in object-local coordinate system: + // M -> (M.R.Minv).M = M.R. + *this = rotated_local(p_axis, p_phi); +} +Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_phi) const { + return (*this) * Basis(p_axis, p_phi); +} + +Basis Basis::rotated(const Vector3 &p_euler) const { + return Basis(p_euler) * (*this); +} + +void Basis::rotate(const Vector3 &p_euler) { + *this = rotated(p_euler); +} + +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. + 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_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(). + // 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)); + } + + 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). +// +// The current implementation uses XYZ convention (Z is the first rotation), +// so euler.z is the angle of the (first) rotation around Z axis and so on, +// +// And thus, assuming the matrix is a rotation matrix, this function returns +// the angles in the decomposition R = X(a1).Y(a2).Z(a3) where Z(a) rotates +// around the z-axis by a and so on. +Vector3 Basis::get_euler_xyz() const { + // Euler angles in XYZ convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + + Vector3 euler; + real_t sy = elements[0][2]; + if (sy < (1.0 - CMP_EPSILON)) { + if (sy > -(1.0 - CMP_EPSILON)) { + // is this a pure Y rotation? + if (elements[1][0] == 0.0 && elements[0][1] == 0.0 && elements[1][2] == 0 && elements[2][1] == 0 && elements[1][1] == 1) { + // return the simplest form (human friendlier in editor and scripts) + euler.x = 0; + euler.y = atan2(elements[0][2], elements[0][0]); + euler.z = 0; + } else { + euler.x = Math::atan2(-elements[1][2], elements[2][2]); + euler.y = Math::asin(sy); + euler.z = Math::atan2(-elements[0][1], elements[0][0]); + } + } else { + euler.x = Math::atan2(elements[2][1], elements[1][1]); + euler.y = -Math_PI / 2.0; + euler.z = 0.0; + } + } else { + euler.x = Math::atan2(elements[2][1], elements[1][1]); + euler.y = Math_PI / 2.0; + euler.z = 0.0; + } + return euler; +} + +// set_euler_xyz expects a vector containing the Euler angles in the format +// (ax,ay,az), where ax is the angle of rotation around x axis, +// and similar for other axes. +// The current implementation uses XYZ convention (Z is the first rotation). +void Basis::set_euler_xyz(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + //optimizer will optimize away all this anyway + *this = xmat * (ymat * zmat); +} + +Vector3 Basis::get_euler_xzy() const { + // Euler angles in XZY convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy -sz cz*sy + // sx*sy+cx*cy*sz cx*cz cx*sz*sy-cy*sx + // cy*sx*sz cz*sx cx*cy+sx*sz*sy + + Vector3 euler; + real_t sz = elements[0][1]; + if (sz < (1.0 - CMP_EPSILON)) { + if (sz > -(1.0 - CMP_EPSILON)) { + euler.x = Math::atan2(elements[2][1], elements[1][1]); + euler.y = Math::atan2(elements[0][2], elements[0][0]); + euler.z = Math::asin(-sz); + } else { + // It's -1 + euler.x = -Math::atan2(elements[1][2], elements[2][2]); + euler.y = 0.0; + euler.z = Math_PI / 2.0; + } + } else { + // It's 1 + euler.x = -Math::atan2(elements[1][2], elements[2][2]); + euler.y = 0.0; + euler.z = -Math_PI / 2.0; + } + return euler; +} + +void Basis::set_euler_xzy(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + *this = xmat * zmat * ymat; +} + +Vector3 Basis::get_euler_yzx() const { + // Euler angles in YZX convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx + // sz cz*cx -cz*sx + // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx + + Vector3 euler; + real_t sz = elements[1][0]; + if (sz < (1.0 - CMP_EPSILON)) { + if (sz > -(1.0 - CMP_EPSILON)) { + euler.x = Math::atan2(-elements[1][2], elements[1][1]); + euler.y = Math::atan2(-elements[2][0], elements[0][0]); + euler.z = Math::asin(sz); + } else { + // It's -1 + euler.x = Math::atan2(elements[2][1], elements[2][2]); + euler.y = 0.0; + euler.z = -Math_PI / 2.0; + } + } else { + // It's 1 + euler.x = Math::atan2(elements[2][1], elements[2][2]); + euler.y = 0.0; + euler.z = Math_PI / 2.0; + } + return euler; +} + +void Basis::set_euler_yzx(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + *this = ymat * zmat * xmat; +} + +// get_euler_yxz returns a vector containing the Euler angles in the YXZ convention, +// as in first-Z, then-X, last-Y. The angles for X, Y, and Z rotations are returned +// as the x, y, and z components of a Vector3 respectively. +Vector3 Basis::get_euler_yxz() const { + // Euler angles in YXZ convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy + // cx*sz cx*cz -sx + // cy*sx*sz-cz*sy cy*cz*sx+sy*sz cy*cx + + Vector3 euler; + + real_t m12 = elements[1][2]; + + if (m12 < (1 - CMP_EPSILON)) { + if (m12 > -(1 - CMP_EPSILON)) { + // is this a pure X rotation? + if (elements[1][0] == 0 && elements[0][1] == 0 && elements[0][2] == 0 && elements[2][0] == 0 && elements[0][0] == 1) { + // return the simplest form (human friendlier in editor and scripts) + euler.x = atan2(-m12, elements[1][1]); + euler.y = 0; + euler.z = 0; + } else { + euler.x = asin(-m12); + euler.y = atan2(elements[0][2], elements[2][2]); + euler.z = atan2(elements[1][0], elements[1][1]); + } + } else { // m12 == -1 + euler.x = Math_PI * 0.5; + euler.y = atan2(elements[0][1], elements[0][0]); + euler.z = 0; + } + } else { // m12 == 1 + euler.x = -Math_PI * 0.5; + euler.y = -atan2(elements[0][1], elements[0][0]); + euler.z = 0; + } + + return euler; +} + +// set_euler_yxz expects a vector containing the Euler angles in the format +// (ax,ay,az), where ax is the angle of rotation around x axis, +// and similar for other axes. +// The current implementation uses YXZ convention (Z is the first rotation). +void Basis::set_euler_yxz(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + //optimizer will optimize away all this anyway + *this = ymat * xmat * zmat; +} + +Vector3 Basis::get_euler_zxy() const { + // Euler angles in ZXY convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx + // cy*sz+cz*sx*sy cz*cx sz*sy-cz*cy*sx + // -cx*sy sx cx*cy + Vector3 euler; + real_t sx = elements[2][1]; + if (sx < (1.0 - CMP_EPSILON)) { + if (sx > -(1.0 - CMP_EPSILON)) { + euler.x = Math::asin(sx); + euler.y = Math::atan2(-elements[2][0], elements[2][2]); + euler.z = Math::atan2(-elements[0][1], elements[1][1]); + } else { + // It's -1 + euler.x = -Math_PI / 2.0; + euler.y = Math::atan2(elements[0][2], elements[0][0]); + euler.z = 0; + } + } else { + // It's 1 + euler.x = Math_PI / 2.0; + euler.y = Math::atan2(elements[0][2], elements[0][0]); + euler.z = 0; + } + return euler; +} + +void Basis::set_euler_zxy(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + *this = zmat * xmat * ymat; +} + +Vector3 Basis::get_euler_zyx() const { + // Euler angles in ZYX convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*cy + // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx + // -sy cy*sx cy*cx + Vector3 euler; + real_t sy = elements[2][0]; + if (sy < (1.0 - CMP_EPSILON)) { + if (sy > -(1.0 - CMP_EPSILON)) { + euler.x = Math::atan2(elements[2][1], elements[2][2]); + euler.y = Math::asin(-sy); + euler.z = Math::atan2(elements[1][0], elements[0][0]); + } else { + // It's -1 + euler.x = 0; + euler.y = Math_PI / 2.0; + euler.z = -Math::atan2(elements[0][1], elements[1][1]); + } + } else { + // It's 1 + euler.x = 0; + euler.y = -Math_PI / 2.0; + euler.z = -Math::atan2(elements[0][1], elements[1][1]); + } + return euler; +} + +void Basis::set_euler_zyx(const Vector3 &p_euler) { + real_t c, s; + + c = Math::cos(p_euler.x); + s = Math::sin(p_euler.x); + Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); + + c = Math::cos(p_euler.y); + s = Math::sin(p_euler.y); + Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); + + c = Math::cos(p_euler.z); + s = Math::sin(p_euler.z); + Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); + + *this = zmat * ymat * xmat; +} + +bool Basis::is_equal_approx(const Basis &p_basis) const { + return elements[0].is_equal_approx(p_basis.elements[0]) && elements[1].is_equal_approx(p_basis.elements[1]) && elements[2].is_equal_approx(p_basis.elements[2]); +} + +bool Basis::is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon) const { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (!Math::is_equal_approx_ratio(a.elements[i][j], b.elements[i][j], p_epsilon)) { + return false; + } + } + } + + return true; +} + +bool Basis::operator==(const Basis &p_matrix) const { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (elements[i][j] != p_matrix.elements[i][j]) { + return false; + } + } + } + + return true; +} + +bool Basis::operator!=(const Basis &p_matrix) const { + return (!(*this == p_matrix)); +} + +Basis::operator String() const { + String mtx; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (i != 0 || j != 0) { + mtx += ", "; + } + + mtx += String::num(elements[i][j]); + } + } + + return mtx; +} + +Quat Basis::get_quat() const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_rotation(), Quat(), "Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() if the Basis contains linearly independent vectors."); +#endif + /* Allow getting a quaternion from an unnormalized transform */ + Basis m = *this; + real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; + real_t temp[4]; + + if (trace > 0.0) { + real_t s = Math::sqrt(trace + 1.0); + temp[3] = (s * 0.5); + s = 0.5 / s; + + temp[0] = ((m.elements[2][1] - m.elements[1][2]) * s); + temp[1] = ((m.elements[0][2] - m.elements[2][0]) * s); + temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s); + } else { + int i = m.elements[0][0] < m.elements[1][1] + ? (m.elements[1][1] < m.elements[2][2] ? 2 : 1) + : (m.elements[0][0] < m.elements[2][2] ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0); + temp[i] = s * 0.5; + s = 0.5 / s; + + temp[3] = (m.elements[k][j] - m.elements[j][k]) * s; + temp[j] = (m.elements[j][i] + m.elements[i][j]) * s; + temp[k] = (m.elements[k][i] + m.elements[i][k]) * s; + } + + return Quat(temp[0], temp[1], temp[2], temp[3]); +} + +static const Basis _ortho_bases[24] = { + Basis(1, 0, 0, 0, 1, 0, 0, 0, 1), + Basis(0, -1, 0, 1, 0, 0, 0, 0, 1), + Basis(-1, 0, 0, 0, -1, 0, 0, 0, 1), + Basis(0, 1, 0, -1, 0, 0, 0, 0, 1), + Basis(1, 0, 0, 0, 0, -1, 0, 1, 0), + Basis(0, 0, 1, 1, 0, 0, 0, 1, 0), + Basis(-1, 0, 0, 0, 0, 1, 0, 1, 0), + Basis(0, 0, -1, -1, 0, 0, 0, 1, 0), + Basis(1, 0, 0, 0, -1, 0, 0, 0, -1), + Basis(0, 1, 0, 1, 0, 0, 0, 0, -1), + Basis(-1, 0, 0, 0, 1, 0, 0, 0, -1), + Basis(0, -1, 0, -1, 0, 0, 0, 0, -1), + Basis(1, 0, 0, 0, 0, 1, 0, -1, 0), + Basis(0, 0, -1, 1, 0, 0, 0, -1, 0), + Basis(-1, 0, 0, 0, 0, -1, 0, -1, 0), + Basis(0, 0, 1, -1, 0, 0, 0, -1, 0), + Basis(0, 0, 1, 0, 1, 0, -1, 0, 0), + Basis(0, -1, 0, 0, 0, 1, -1, 0, 0), + Basis(0, 0, -1, 0, -1, 0, -1, 0, 0), + Basis(0, 1, 0, 0, 0, -1, -1, 0, 0), + Basis(0, 0, 1, 0, -1, 0, 1, 0, 0), + Basis(0, 1, 0, 0, 0, 1, 1, 0, 0), + Basis(0, 0, -1, 0, 1, 0, 1, 0, 0), + Basis(0, -1, 0, 0, 0, -1, 1, 0, 0) +}; + +int Basis::get_orthogonal_index() const { + //could be sped up if i come up with a way + Basis orth = *this; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + real_t v = orth[i][j]; + if (v > 0.5) { + v = 1.0; + } else if (v < -0.5) { + v = -1.0; + } else { + v = 0; + } + + orth[i][j] = v; + } + } + + for (int i = 0; i < 24; i++) { + if (_ortho_bases[i] == orth) { + return i; + } + } + + return 0; +} + +void Basis::set_orthogonal_index(int p_index) { + //there only exist 24 orthogonal bases in r3 + ERR_FAIL_INDEX(p_index, 24); + + *this = _ortho_bases[p_index]; +} + +void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { + /* checking this is a bad idea, because obtaining from scaled transform is a valid use case +#ifdef MATH_CHECKS + ERR_FAIL_COND(!is_rotation()); +#endif +*/ + real_t angle, x, y, z; // variables for result + real_t epsilon = 0.01; // margin to allow for rounding errors + real_t epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees + + if ((Math::abs(elements[1][0] - elements[0][1]) < epsilon) && (Math::abs(elements[2][0] - elements[0][2]) < epsilon) && (Math::abs(elements[2][1] - elements[1][2]) < epsilon)) { + // singularity found + // first check for identity matrix which must have +1 for all terms + // in leading diagonaland zero in other terms + if ((Math::abs(elements[1][0] + elements[0][1]) < epsilon2) && (Math::abs(elements[2][0] + elements[0][2]) < epsilon2) && (Math::abs(elements[2][1] + elements[1][2]) < epsilon2) && (Math::abs(elements[0][0] + elements[1][1] + elements[2][2] - 3) < epsilon2)) { + // this singularity is identity matrix so angle = 0 + r_axis = Vector3(0, 1, 0); + r_angle = 0; + return; + } + // otherwise this singularity is angle = 180 + angle = Math_PI; + real_t xx = (elements[0][0] + 1) / 2; + real_t yy = (elements[1][1] + 1) / 2; + real_t zz = (elements[2][2] + 1) / 2; + real_t xy = (elements[1][0] + elements[0][1]) / 4; + real_t xz = (elements[2][0] + elements[0][2]) / 4; + real_t yz = (elements[2][1] + elements[1][2]) / 4; + if ((xx > yy) && (xx > zz)) { // elements[0][0] is the largest diagonal term + if (xx < epsilon) { + x = 0; + y = Math_SQRT12; + z = Math_SQRT12; + } else { + x = Math::sqrt(xx); + y = xy / x; + z = xz / x; + } + } else if (yy > zz) { // elements[1][1] is the largest diagonal term + if (yy < epsilon) { + x = Math_SQRT12; + y = 0; + z = Math_SQRT12; + } else { + y = Math::sqrt(yy); + x = xy / y; + z = yz / y; + } + } else { // elements[2][2] is the largest diagonal term so base result on this + if (zz < epsilon) { + x = Math_SQRT12; + y = Math_SQRT12; + z = 0; + } else { + z = Math::sqrt(zz); + x = xz / z; + y = yz / z; + } + } + r_axis = Vector3(x, y, z); + r_angle = angle; + return; + } + // as we have reached here there are no singularities so we can handle normally + real_t s = Math::sqrt((elements[1][2] - elements[2][1]) * (elements[1][2] - elements[2][1]) + (elements[2][0] - elements[0][2]) * (elements[2][0] - elements[0][2]) + (elements[0][1] - elements[1][0]) * (elements[0][1] - elements[1][0])); // s=|axis||sin(angle)|, used to normalise + + angle = Math::acos((elements[0][0] + elements[1][1] + elements[2][2] - 1) / 2); + if (angle < 0) { + s = -s; + } + x = (elements[2][1] - elements[1][2]) / s; + y = (elements[0][2] - elements[2][0]) / s; + z = (elements[1][0] - elements[0][1]) / s; + + r_axis = Vector3(x, y, z); + r_angle = angle; +} + +void Basis::set_quat(const Quat &p_quat) { + real_t d = p_quat.length_squared(); + real_t s = 2.0 / d; + real_t xs = p_quat.x * s, ys = p_quat.y * s, zs = p_quat.z * s; + real_t wx = p_quat.w * xs, wy = p_quat.w * ys, wz = p_quat.w * zs; + real_t xx = p_quat.x * xs, xy = p_quat.x * ys, xz = p_quat.x * zs; + real_t yy = p_quat.y * ys, yz = p_quat.y * zs, zz = p_quat.z * zs; + set(1.0 - (yy + zz), xy - wz, xz + wy, + xy + wz, 1.0 - (xx + zz), yz - wx, + xz - wy, yz + wx, 1.0 - (xx + yy)); +} + +void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) { +// Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_angle +#ifdef MATH_CHECKS + ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "The axis Vector3 must be normalized."); +#endif + Vector3 axis_sq(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z); + real_t cosine = Math::cos(p_phi); + elements[0][0] = axis_sq.x + cosine * (1.0 - axis_sq.x); + elements[1][1] = axis_sq.y + cosine * (1.0 - axis_sq.y); + elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z); + + real_t sine = Math::sin(p_phi); + real_t t = 1 - cosine; + + real_t xyzt = p_axis.x * p_axis.y * t; + real_t zyxs = p_axis.z * sine; + elements[0][1] = xyzt - zyxs; + elements[1][0] = xyzt + zyxs; + + xyzt = p_axis.x * p_axis.z * t; + zyxs = p_axis.y * sine; + elements[0][2] = xyzt + zyxs; + elements[2][0] = xyzt - zyxs; + + xyzt = p_axis.y * p_axis.z * t; + zyxs = p_axis.x * sine; + elements[1][2] = xyzt - zyxs; + elements[2][1] = xyzt + zyxs; +} + +void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { + 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 &p_to, const real_t &p_weight) const { + //consider scale + Quat from(*this); + Quat to(p_to); + + Basis b(from.slerp(to, p_weight)); + b.elements[0] *= Math::lerp(elements[0].length(), p_to.elements[0].length(), p_weight); + b.elements[1] *= Math::lerp(elements[1].length(), p_to.elements[1].length(), p_weight); + b.elements[2] *= Math::lerp(elements[2].length(), p_to.elements[2].length(), p_weight); + + return b; +} diff --git a/core/math/basis.h b/core/math/basis.h new file mode 100644 index 0000000..ebd9e77 --- /dev/null +++ b/core/math/basis.h @@ -0,0 +1,329 @@ +/*************************************************************************/ +/* basis.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 BASIS_H +#define BASIS_H + +#include "core/math/quat.h" +#include "core/math/vector3.h" + +class Basis { +public: + Vector3 elements[3] = { + Vector3(1, 0, 0), + Vector3(0, 1, 0), + Vector3(0, 0, 1) + }; + + _FORCE_INLINE_ const Vector3 &operator[](int axis) const { + return elements[axis]; + } + _FORCE_INLINE_ Vector3 &operator[](int axis) { + return elements[axis]; + } + + void invert(); + void transpose(); + + Basis inverse() const; + Basis transposed() const; + + _FORCE_INLINE_ real_t determinant() const; + + void from_z(const Vector3 &p_z); + + _FORCE_INLINE_ Vector3 get_axis(int p_axis) const { + // get actual basis axis (elements is transposed for performance) + return Vector3(elements[0][p_axis], elements[1][p_axis], elements[2][p_axis]); + } + _FORCE_INLINE_ void set_axis(int p_axis, const Vector3 &p_value) { + // get actual basis axis (elements is transposed for performance) + elements[0][p_axis] = p_value.x; + elements[1][p_axis] = p_value.y; + elements[2][p_axis] = p_value.z; + } + + void rotate(const Vector3 &p_axis, real_t p_phi); + Basis rotated(const Vector3 &p_axis, real_t p_phi) const; + + void rotate_local(const Vector3 &p_axis, real_t p_phi); + Basis rotated_local(const Vector3 &p_axis, real_t p_phi) const; + + void rotate(const Vector3 &p_euler); + Basis rotated(const Vector3 &p_euler) 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; + + Vector3 get_euler_xyz() const; + void set_euler_xyz(const Vector3 &p_euler); + + Vector3 get_euler_xzy() const; + void set_euler_xzy(const Vector3 &p_euler); + + Vector3 get_euler_yzx() const; + void set_euler_yzx(const Vector3 &p_euler); + + Vector3 get_euler_yxz() const; + void set_euler_yxz(const Vector3 &p_euler); + + Vector3 get_euler_zxy() const; + void set_euler_zxy(const Vector3 &p_euler); + + Vector3 get_euler_zyx() const; + void set_euler_zyx(const Vector3 &p_euler); + + Quat get_quat() const; + void set_quat(const Quat &p_quat); + + Vector3 get_euler() const { return get_euler_yxz(); } + void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); } + + void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const; + void set_axis_angle(const Vector3 &p_axis, real_t p_phi); + + void scale(const Vector3 &p_scale); + Basis scaled(const Vector3 &p_scale) const; + + void scale_local(const Vector3 &p_scale); + Basis scaled_local(const Vector3 &p_scale) const; + + Vector3 get_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 { + return elements[0][0] * v[0] + elements[1][0] * v[1] + elements[2][0] * v[2]; + } + _FORCE_INLINE_ real_t tdoty(const Vector3 &v) const { + return elements[0][1] * v[0] + elements[1][1] * v[1] + elements[2][1] * v[2]; + } + _FORCE_INLINE_ real_t tdotz(const Vector3 &v) const { + return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2]; + } + + bool is_equal_approx(const Basis &p_basis) const; + // For complicated reasons, the second argument is always discarded. See #45062. + bool is_equal_approx(const Basis &a, const Basis &b) const { return is_equal_approx(a); } + bool is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon = UNIT_EPSILON) const; + + bool operator==(const Basis &p_matrix) const; + bool operator!=(const Basis &p_matrix) const; + + _FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const; + _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const; + _FORCE_INLINE_ void operator*=(const Basis &p_matrix); + _FORCE_INLINE_ Basis operator*(const Basis &p_matrix) const; + _FORCE_INLINE_ void operator+=(const Basis &p_matrix); + _FORCE_INLINE_ Basis operator+(const Basis &p_matrix) const; + _FORCE_INLINE_ void operator-=(const Basis &p_matrix); + _FORCE_INLINE_ Basis operator-(const Basis &p_matrix) const; + _FORCE_INLINE_ void operator*=(real_t p_val); + _FORCE_INLINE_ Basis operator*(real_t p_val) const; + + 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 &p_to, const real_t &p_weight) const; + + operator String() const; + + /* create / set */ + + _FORCE_INLINE_ void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { + elements[0][0] = xx; + elements[0][1] = xy; + elements[0][2] = xz; + elements[1][0] = yx; + elements[1][1] = yy; + elements[1][2] = yz; + elements[2][0] = zx; + elements[2][1] = zy; + elements[2][2] = zz; + } + _FORCE_INLINE_ void set(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) { + set_axis(0, p_x); + set_axis(1, p_y); + set_axis(2, p_z); + } + _FORCE_INLINE_ Vector3 get_column(int i) const { + return Vector3(elements[0][i], elements[1][i], elements[2][i]); + } + + _FORCE_INLINE_ Vector3 get_row(int i) const { + return Vector3(elements[i][0], elements[i][1], elements[i][2]); + } + _FORCE_INLINE_ Vector3 get_main_diagonal() const { + return Vector3(elements[0][0], elements[1][1], elements[2][2]); + } + + _FORCE_INLINE_ void set_row(int i, const Vector3 &p_row) { + elements[i][0] = p_row.x; + elements[i][1] = p_row.y; + elements[i][2] = p_row.z; + } + + _FORCE_INLINE_ void set_zero() { + elements[0].zero(); + elements[1].zero(); + elements[2].zero(); + } + + _FORCE_INLINE_ Basis transpose_xform(const Basis &m) const { + return Basis( + elements[0].x * m[0].x + elements[1].x * m[1].x + elements[2].x * m[2].x, + elements[0].x * m[0].y + elements[1].x * m[1].y + elements[2].x * m[2].y, + elements[0].x * m[0].z + elements[1].x * m[1].z + elements[2].x * m[2].z, + elements[0].y * m[0].x + elements[1].y * m[1].x + elements[2].y * m[2].x, + elements[0].y * m[0].y + elements[1].y * m[1].y + elements[2].y * m[2].y, + elements[0].y * m[0].z + elements[1].y * m[1].z + elements[2].y * m[2].z, + elements[0].z * m[0].x + elements[1].z * m[1].x + elements[2].z * m[2].x, + elements[0].z * m[0].y + elements[1].z * m[1].y + elements[2].z * m[2].y, + elements[0].z * m[0].z + elements[1].z * m[1].z + elements[2].z * m[2].z); + } + Basis(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { + set(xx, xy, xz, yx, yy, yz, zx, zy, zz); + } + + void orthonormalize(); + Basis orthonormalized() const; + + bool is_symmetric() const; + Basis diagonalize(); + + 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; + elements[1] = row1; + elements[2] = row2; + } + + _FORCE_INLINE_ Basis() {} +}; + +_FORCE_INLINE_ void Basis::operator*=(const Basis &p_matrix) { + set( + p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]), + p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]), + p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2])); +} + +_FORCE_INLINE_ Basis Basis::operator*(const Basis &p_matrix) const { + return Basis( + p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]), + p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]), + p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2])); +} + +_FORCE_INLINE_ void Basis::operator+=(const Basis &p_matrix) { + elements[0] += p_matrix.elements[0]; + elements[1] += p_matrix.elements[1]; + elements[2] += p_matrix.elements[2]; +} + +_FORCE_INLINE_ Basis Basis::operator+(const Basis &p_matrix) const { + Basis ret(*this); + ret += p_matrix; + return ret; +} + +_FORCE_INLINE_ void Basis::operator-=(const Basis &p_matrix) { + elements[0] -= p_matrix.elements[0]; + elements[1] -= p_matrix.elements[1]; + elements[2] -= p_matrix.elements[2]; +} + +_FORCE_INLINE_ Basis Basis::operator-(const Basis &p_matrix) const { + Basis ret(*this); + ret -= p_matrix; + return ret; +} + +_FORCE_INLINE_ void Basis::operator*=(real_t p_val) { + elements[0] *= p_val; + elements[1] *= p_val; + elements[2] *= p_val; +} + +_FORCE_INLINE_ Basis Basis::operator*(real_t p_val) const { + Basis ret(*this); + ret *= p_val; + return ret; +} + +Vector3 Basis::xform(const Vector3 &p_vector) const { + return Vector3( + elements[0].dot(p_vector), + elements[1].dot(p_vector), + elements[2].dot(p_vector)); +} + +Vector3 Basis::xform_inv(const Vector3 &p_vector) const { + return Vector3( + (elements[0][0] * p_vector.x) + (elements[1][0] * p_vector.y) + (elements[2][0] * p_vector.z), + (elements[0][1] * p_vector.x) + (elements[1][1] * p_vector.y) + (elements[2][1] * p_vector.z), + (elements[0][2] * p_vector.x) + (elements[1][2] * p_vector.y) + (elements[2][2] * p_vector.z)); +} + +real_t Basis::determinant() const { + return elements[0][0] * (elements[1][1] * elements[2][2] - elements[2][1] * elements[1][2]) - + elements[1][0] * (elements[0][1] * elements[2][2] - elements[2][1] * elements[0][2]) + + elements[2][0] * (elements[0][1] * elements[1][2] - elements[1][1] * elements[0][2]); +} +#endif // BASIS_H diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp new file mode 100644 index 0000000..2d62539 --- /dev/null +++ b/core/math/camera_matrix.cpp @@ -0,0 +1,664 @@ +/*************************************************************************/ +/* camera_matrix.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "camera_matrix.h" + +#include "core/math/math.h" + +void CameraMatrix::set_identity() { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + matrix[i][j] = (i == j) ? 1 : 0; + } + } +} + +void CameraMatrix::set_zero() { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + matrix[i][j] = 0; + } + } +} + +Plane CameraMatrix::xform4(const Plane &p_vec4) const { + Plane ret; + + ret.normal.x = matrix[0][0] * p_vec4.normal.x + matrix[1][0] * p_vec4.normal.y + matrix[2][0] * p_vec4.normal.z + matrix[3][0] * p_vec4.d; + ret.normal.y = matrix[0][1] * p_vec4.normal.x + matrix[1][1] * p_vec4.normal.y + matrix[2][1] * p_vec4.normal.z + matrix[3][1] * p_vec4.d; + ret.normal.z = matrix[0][2] * p_vec4.normal.x + matrix[1][2] * p_vec4.normal.y + matrix[2][2] * p_vec4.normal.z + matrix[3][2] * p_vec4.d; + ret.d = matrix[0][3] * p_vec4.normal.x + matrix[1][3] * p_vec4.normal.y + matrix[2][3] * p_vec4.normal.z + matrix[3][3] * p_vec4.d; + return ret; +} + +void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov) { + if (p_flip_fov) { + p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect); + } + + real_t sine, cotangent, deltaZ; + real_t radians = p_fovy_degrees / 2.0 * Math_PI / 180.0; + + deltaZ = p_z_far - p_z_near; + sine = Math::sin(radians); + + if ((deltaZ == 0) || (sine == 0) || (p_aspect == 0)) { + return; + } + cotangent = Math::cos(radians) / sine; + + set_identity(); + + matrix[0][0] = cotangent / p_aspect; + matrix[1][1] = cotangent; + matrix[2][2] = -(p_z_far + p_z_near) / deltaZ; + matrix[2][3] = -1; + matrix[3][2] = -2 * p_z_near * p_z_far / deltaZ; + matrix[3][3] = 0; +} + +void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist) { + if (p_flip_fov) { + p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect); + } + + real_t left, right, modeltranslation, ymax, xmax, frustumshift; + + ymax = p_z_near * tan(p_fovy_degrees * Math_PI / 360.0f); + xmax = ymax * p_aspect; + frustumshift = (p_intraocular_dist / 2.0) * p_z_near / p_convergence_dist; + + switch (p_eye) { + case 1: { // left eye + left = -xmax + frustumshift; + right = xmax + frustumshift; + modeltranslation = p_intraocular_dist / 2.0; + }; break; + case 2: { // right eye + left = -xmax - frustumshift; + right = xmax - frustumshift; + modeltranslation = -p_intraocular_dist / 2.0; + }; break; + default: { // mono, should give the same result as set_perspective(p_fovy_degrees,p_aspect,p_z_near,p_z_far,p_flip_fov) + left = -xmax; + right = xmax; + modeltranslation = 0.0; + }; break; + }; + + set_frustum(left, right, -ymax, ymax, p_z_near, p_z_far); + + // translate matrix by (modeltranslation, 0.0, 0.0) + CameraMatrix cm; + cm.set_identity(); + cm.matrix[3][0] = modeltranslation; + *this = *this * cm; +} + +void CameraMatrix::set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far) { + // we first calculate our base frustum on our values without taking our lens magnification into account. + real_t f1 = (p_intraocular_dist * 0.5) / p_display_to_lens; + real_t f2 = ((p_display_width - p_intraocular_dist) * 0.5) / p_display_to_lens; + real_t f3 = (p_display_width / 4.0) / p_display_to_lens; + + // now we apply our oversample factor to increase our FOV. how much we oversample is always a balance we strike between performance and how much + // we're willing to sacrifice in FOV. + real_t add = ((f1 + f2) * (p_oversample - 1.0)) / 2.0; + f1 += add; + f2 += add; + f3 *= p_oversample; + + // always apply KEEP_WIDTH aspect ratio + f3 /= p_aspect; + + switch (p_eye) { + case 1: { // left eye + set_frustum(-f2 * p_z_near, f1 * p_z_near, -f3 * p_z_near, f3 * p_z_near, p_z_near, p_z_far); + }; break; + case 2: { // right eye + set_frustum(-f1 * p_z_near, f2 * p_z_near, -f3 * p_z_near, f3 * p_z_near, p_z_near, p_z_far); + }; break; + default: { // mono, does not apply here! + }; break; + }; +}; + +void CameraMatrix::set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar) { + set_identity(); + + matrix[0][0] = 2.0 / (p_right - p_left); + matrix[3][0] = -((p_right + p_left) / (p_right - p_left)); + matrix[1][1] = 2.0 / (p_top - p_bottom); + matrix[3][1] = -((p_top + p_bottom) / (p_top - p_bottom)); + matrix[2][2] = -2.0 / (p_zfar - p_znear); + matrix[3][2] = -((p_zfar + p_znear) / (p_zfar - p_znear)); + matrix[3][3] = 1.0; +} + +void CameraMatrix::set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov) { + if (!p_flip_fov) { + p_size *= p_aspect; + } + + set_orthogonal(-p_size / 2, +p_size / 2, -p_size / p_aspect / 2, +p_size / p_aspect / 2, p_znear, p_zfar); +} + +void CameraMatrix::set_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far) { + ERR_FAIL_COND(p_right <= p_left); + ERR_FAIL_COND(p_top <= p_bottom); + ERR_FAIL_COND(p_far <= p_near); + + real_t *te = &matrix[0][0]; + real_t x = 2 * p_near / (p_right - p_left); + real_t y = 2 * p_near / (p_top - p_bottom); + + real_t a = (p_right + p_left) / (p_right - p_left); + real_t b = (p_top + p_bottom) / (p_top - p_bottom); + real_t c = -(p_far + p_near) / (p_far - p_near); + real_t d = -2 * p_far * p_near / (p_far - p_near); + + te[0] = x; + te[1] = 0; + te[2] = 0; + te[3] = 0; + te[4] = 0; + te[5] = y; + te[6] = 0; + te[7] = 0; + te[8] = a; + te[9] = b; + te[10] = c; + te[11] = -1; + te[12] = 0; + te[13] = 0; + te[14] = d; + te[15] = 0; +} + +void CameraMatrix::set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov) { + if (!p_flip_fov) { + p_size *= p_aspect; + } + + set_frustum(-p_size / 2 + p_offset.x, +p_size / 2 + p_offset.x, -p_size / p_aspect / 2 + p_offset.y, +p_size / p_aspect / 2 + p_offset.y, p_near, p_far); +} + +real_t CameraMatrix::get_z_far() const { + const real_t *matrix = (const real_t *)this->matrix; + Plane new_plane = Plane(matrix[3] - matrix[2], + matrix[7] - matrix[6], + matrix[11] - matrix[10], + matrix[15] - matrix[14]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + + return new_plane.d; +} +real_t CameraMatrix::get_z_near() const { + const real_t *matrix = (const real_t *)this->matrix; + Plane new_plane = Plane(matrix[3] + matrix[2], + matrix[7] + matrix[6], + matrix[11] + matrix[10], + -matrix[15] - matrix[14]); + + new_plane.normalize(); + return new_plane.d; +} + +Vector2 CameraMatrix::get_viewport_half_extents() const { + const real_t *matrix = (const real_t *)this->matrix; + ///////--- Near Plane ---/////// + Plane near_plane = Plane(matrix[3] + matrix[2], + matrix[7] + matrix[6], + matrix[11] + matrix[10], + -matrix[15] - matrix[14]); + near_plane.normalize(); + + ///////--- Right Plane ---/////// + Plane right_plane = Plane(matrix[3] - matrix[0], + matrix[7] - matrix[4], + matrix[11] - matrix[8], + -matrix[15] + matrix[12]); + right_plane.normalize(); + + Plane top_plane = Plane(matrix[3] - matrix[1], + matrix[7] - matrix[5], + matrix[11] - matrix[9], + -matrix[15] + matrix[13]); + top_plane.normalize(); + + Vector3 res; + near_plane.intersect_3(right_plane, top_plane, &res); + + return Vector2(res.x, res.y); +} + +bool CameraMatrix::get_endpoints(const Transform &p_transform, Vector3 *p_8points) const { + Vector planes = get_projection_planes(Transform()); + const Planes intersections[8][3] = { + { PLANE_FAR, PLANE_LEFT, PLANE_TOP }, + { PLANE_FAR, PLANE_LEFT, PLANE_BOTTOM }, + { PLANE_FAR, PLANE_RIGHT, PLANE_TOP }, + { PLANE_FAR, PLANE_RIGHT, PLANE_BOTTOM }, + { PLANE_NEAR, PLANE_LEFT, PLANE_TOP }, + { PLANE_NEAR, PLANE_LEFT, PLANE_BOTTOM }, + { PLANE_NEAR, PLANE_RIGHT, PLANE_TOP }, + { PLANE_NEAR, PLANE_RIGHT, PLANE_BOTTOM }, + }; + + for (int i = 0; i < 8; i++) { + Vector3 point; + bool res = planes[intersections[i][0]].intersect_3(planes[intersections[i][1]], planes[intersections[i][2]], &point); + ERR_FAIL_COND_V(!res, false); + p_8points[i] = p_transform.xform(point); + } + + return true; +} + +Vector CameraMatrix::get_projection_planes(const Transform &p_transform) const { + /** Fast Plane Extraction from combined modelview/projection matrices. + * References: + * https://web.archive.org/web/20011221205252/http://www.markmorley.com/opengl/frustumculling.html + * https://web.archive.org/web/20061020020112/http://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf + */ + + Vector planes; + + const real_t *matrix = (const real_t *)this->matrix; + + Plane new_plane; + + ///////--- Near Plane ---/////// + new_plane = Plane(matrix[3] + matrix[2], + matrix[7] + matrix[6], + matrix[11] + matrix[10], + matrix[15] + matrix[14]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + + planes.push_back(p_transform.xform(new_plane)); + + ///////--- Far Plane ---/////// + new_plane = Plane(matrix[3] - matrix[2], + matrix[7] - matrix[6], + matrix[11] - matrix[10], + matrix[15] - matrix[14]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + + planes.push_back(p_transform.xform(new_plane)); + + ///////--- Left Plane ---/////// + new_plane = Plane(matrix[3] + matrix[0], + matrix[7] + matrix[4], + matrix[11] + matrix[8], + matrix[15] + matrix[12]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + + planes.push_back(p_transform.xform(new_plane)); + + ///////--- Top Plane ---/////// + new_plane = Plane(matrix[3] - matrix[1], + matrix[7] - matrix[5], + matrix[11] - matrix[9], + matrix[15] - matrix[13]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + + planes.push_back(p_transform.xform(new_plane)); + + ///////--- Right Plane ---/////// + new_plane = Plane(matrix[3] - matrix[0], + matrix[7] - matrix[4], + matrix[11] - matrix[8], + matrix[15] - matrix[12]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + + planes.push_back(p_transform.xform(new_plane)); + + ///////--- Bottom Plane ---/////// + new_plane = Plane(matrix[3] + matrix[1], + matrix[7] + matrix[5], + matrix[11] + matrix[9], + matrix[15] + matrix[13]); + + new_plane.normal = -new_plane.normal; + new_plane.normalize(); + + planes.push_back(p_transform.xform(new_plane)); + + return planes; +} + +CameraMatrix CameraMatrix::inverse() const { + CameraMatrix cm = *this; + cm.invert(); + return cm; +} + +void CameraMatrix::invert() { + int i, j, k; + int pvt_i[4], pvt_j[4]; /* Locations of pivot matrix */ + real_t pvt_val; /* Value of current pivot element */ + real_t hold; /* Temporary storage */ + real_t determinat; /* Determinant */ + + determinat = 1.0; + for (k = 0; k < 4; k++) { + /** Locate k'th pivot element **/ + pvt_val = matrix[k][k]; /** Initialize for search **/ + pvt_i[k] = k; + pvt_j[k] = k; + for (i = k; i < 4; i++) { + for (j = k; j < 4; j++) { + if (Math::abs(matrix[i][j]) > Math::abs(pvt_val)) { + pvt_i[k] = i; + pvt_j[k] = j; + pvt_val = matrix[i][j]; + } + } + } + + /** Product of pivots, gives determinant when finished **/ + determinat *= pvt_val; + if (Math::abs(determinat) < 1e-7) { + return; //(false); /** Matrix is singular (zero determinant). **/ + } + + /** "Interchange" rows (with sign change stuff) **/ + i = pvt_i[k]; + if (i != k) { /** If rows are different **/ + for (j = 0; j < 4; j++) { + hold = -matrix[k][j]; + matrix[k][j] = matrix[i][j]; + matrix[i][j] = hold; + } + } + + /** "Interchange" columns **/ + j = pvt_j[k]; + if (j != k) { /** If columns are different **/ + for (i = 0; i < 4; i++) { + hold = -matrix[i][k]; + matrix[i][k] = matrix[i][j]; + matrix[i][j] = hold; + } + } + + /** Divide column by minus pivot value **/ + for (i = 0; i < 4; i++) { + if (i != k) { + matrix[i][k] /= (-pvt_val); + } + } + + /** Reduce the matrix **/ + for (i = 0; i < 4; i++) { + hold = matrix[i][k]; + for (j = 0; j < 4; j++) { + if (i != k && j != k) { + matrix[i][j] += hold * matrix[k][j]; + } + } + } + + /** Divide row by pivot **/ + for (j = 0; j < 4; j++) { + if (j != k) { + matrix[k][j] /= pvt_val; + } + } + + /** Replace pivot by reciprocal (at last we can touch it). **/ + matrix[k][k] = 1.0 / pvt_val; + } + + /* That was most of the work, one final pass of row/column interchange */ + /* to finish */ + for (k = 4 - 2; k >= 0; k--) { /* Don't need to work with 1 by 1 corner*/ + i = pvt_j[k]; /* Rows to swap correspond to pivot COLUMN */ + if (i != k) { /* If rows are different */ + for (j = 0; j < 4; j++) { + hold = matrix[k][j]; + matrix[k][j] = -matrix[i][j]; + matrix[i][j] = hold; + } + } + + j = pvt_i[k]; /* Columns to swap correspond to pivot ROW */ + if (j != k) { /* If columns are different */ + for (i = 0; i < 4; i++) { + hold = matrix[i][k]; + matrix[i][k] = -matrix[i][j]; + matrix[i][j] = hold; + } + } + } +} + +CameraMatrix::CameraMatrix() { + set_identity(); +} + +CameraMatrix CameraMatrix::operator*(const CameraMatrix &p_matrix) const { + CameraMatrix new_matrix; + + for (int j = 0; j < 4; j++) { + for (int i = 0; i < 4; i++) { + real_t ab = 0; + for (int k = 0; k < 4; k++) { + ab += matrix[k][i] * p_matrix.matrix[j][k]; + } + new_matrix.matrix[j][i] = ab; + } + } + + return new_matrix; +} + +void CameraMatrix::set_light_bias() { + real_t *m = &matrix[0][0]; + + m[0] = 0.5; + m[1] = 0.0; + m[2] = 0.0; + m[3] = 0.0; + m[4] = 0.0; + m[5] = 0.5; + m[6] = 0.0; + m[7] = 0.0; + m[8] = 0.0; + m[9] = 0.0; + m[10] = 0.5; + m[11] = 0.0; + m[12] = 0.5; + m[13] = 0.5; + m[14] = 0.5; + m[15] = 1.0; +} + +void CameraMatrix::set_light_atlas_rect(const Rect2 &p_rect) { + real_t *m = &matrix[0][0]; + + m[0] = p_rect.w; + m[1] = 0.0; + m[2] = 0.0; + m[3] = 0.0; + m[4] = 0.0; + m[5] = p_rect.h; + m[6] = 0.0; + m[7] = 0.0; + m[8] = 0.0; + m[9] = 0.0; + m[10] = 1.0; + m[11] = 0.0; + m[12] = p_rect.x; + m[13] = p_rect.y; + m[14] = 0.0; + m[15] = 1.0; +} + +CameraMatrix::operator String() const { + String str; + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + str += String((j > 0) ? ", " : "\n") + String::num(matrix[i][j]); + } + } + + return str; +} + +real_t CameraMatrix::get_aspect() const { + Vector2 vp_he = get_viewport_half_extents(); + return vp_he.x / vp_he.y; +} + +int CameraMatrix::get_pixels_per_meter(int p_for_pixel_width) const { + Vector3 result = xform(Vector3(1, 0, -1)); + + return int((result.x * 0.5 + 0.5) * p_for_pixel_width); +} + +bool CameraMatrix::is_orthogonal() const { + return matrix[3][3] == 1.0; +} + +real_t CameraMatrix::get_fov() const { + const real_t *matrix = (const real_t *)this->matrix; + + Plane right_plane = Plane(matrix[3] - matrix[0], + matrix[7] - matrix[4], + matrix[11] - matrix[8], + -matrix[15] + matrix[12]); + right_plane.normalize(); + + if ((matrix[8] == 0) && (matrix[9] == 0)) { + return Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x))) * 2.0; + } else { + // our frustum is asymmetrical need to calculate the left planes angle separately.. + Plane left_plane = Plane(matrix[3] + matrix[0], + matrix[7] + matrix[4], + matrix[11] + matrix[8], + matrix[15] + matrix[12]); + left_plane.normalize(); + + return Math::rad2deg(Math::acos(Math::abs(left_plane.normal.x))) + Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x))); + } +} + +void CameraMatrix::make_scale(const Vector3 &p_scale) { + set_identity(); + matrix[0][0] = p_scale.x; + matrix[1][1] = p_scale.y; + matrix[2][2] = p_scale.z; +} + +void CameraMatrix::scale_translate_to_fit(const AABB &p_aabb) { + Vector3 min = p_aabb.position; + Vector3 max = p_aabb.position + p_aabb.size; + + matrix[0][0] = 2 / (max.x - min.x); + matrix[1][0] = 0; + matrix[2][0] = 0; + matrix[3][0] = -(max.x + min.x) / (max.x - min.x); + + matrix[0][1] = 0; + matrix[1][1] = 2 / (max.y - min.y); + matrix[2][1] = 0; + matrix[3][1] = -(max.y + min.y) / (max.y - min.y); + + matrix[0][2] = 0; + matrix[1][2] = 0; + matrix[2][2] = 2 / (max.z - min.z); + matrix[3][2] = -(max.z + min.z) / (max.z - min.z); + + matrix[0][3] = 0; + matrix[1][3] = 0; + matrix[2][3] = 0; + matrix[3][3] = 1; +} + +CameraMatrix::operator Transform() const { + Transform tr; + const real_t *m = &matrix[0][0]; + + tr.basis.elements[0][0] = m[0]; + tr.basis.elements[1][0] = m[1]; + tr.basis.elements[2][0] = m[2]; + + tr.basis.elements[0][1] = m[4]; + tr.basis.elements[1][1] = m[5]; + tr.basis.elements[2][1] = m[6]; + + tr.basis.elements[0][2] = m[8]; + tr.basis.elements[1][2] = m[9]; + tr.basis.elements[2][2] = m[10]; + + tr.origin.x = m[12]; + tr.origin.y = m[13]; + tr.origin.z = m[14]; + + return tr; +} + +CameraMatrix::CameraMatrix(const Transform &p_transform) { + const Transform &tr = p_transform; + real_t *m = &matrix[0][0]; + + m[0] = tr.basis.elements[0][0]; + m[1] = tr.basis.elements[1][0]; + m[2] = tr.basis.elements[2][0]; + m[3] = 0.0; + m[4] = tr.basis.elements[0][1]; + m[5] = tr.basis.elements[1][1]; + m[6] = tr.basis.elements[2][1]; + m[7] = 0.0; + m[8] = tr.basis.elements[0][2]; + m[9] = tr.basis.elements[1][2]; + m[10] = tr.basis.elements[2][2]; + m[11] = 0.0; + m[12] = tr.origin.x; + m[13] = tr.origin.y; + m[14] = tr.origin.z; + m[15] = 1.0; +} + +CameraMatrix::~CameraMatrix() { +} diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h new file mode 100644 index 0000000..5a4c328 --- /dev/null +++ b/core/math/camera_matrix.h @@ -0,0 +1,106 @@ +/*************************************************************************/ +/* camera_matrix.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 CAMERA_MATRIX_H +#define CAMERA_MATRIX_H + +#include "core/math/rect2.h" +#include "core/math/transform.h" +#include "core/math/vector2.h" + +struct CameraMatrix { + enum Planes { + PLANE_NEAR, + PLANE_FAR, + PLANE_LEFT, + PLANE_TOP, + PLANE_RIGHT, + PLANE_BOTTOM + }; + + real_t matrix[4][4]; + + void set_identity(); + void set_zero(); + void set_light_bias(); + void set_light_atlas_rect(const Rect2 &p_rect); + void set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov = false); + void set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist); + void set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far); + void set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar); + void set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov = false); + void set_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far); + void set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov = false); + + static real_t get_fovy(real_t p_fovx, real_t p_aspect) { + return Math::rad2deg(Math::atan(p_aspect * Math::tan(Math::deg2rad(p_fovx) * 0.5)) * 2.0); + } + + real_t get_z_far() const; + real_t get_z_near() const; + real_t get_aspect() const; + real_t get_fov() const; + bool is_orthogonal() const; + + Vector get_projection_planes(const Transform &p_transform) const; + + bool get_endpoints(const Transform &p_transform, Vector3 *p_8points) const; + Vector2 get_viewport_half_extents() const; + + void invert(); + CameraMatrix inverse() const; + + CameraMatrix operator*(const CameraMatrix &p_matrix) const; + + Plane xform4(const Plane &p_vec4) const; + _FORCE_INLINE_ Vector3 xform(const Vector3 &p_vec3) const; + + operator String() const; + + void scale_translate_to_fit(const AABB &p_aabb); + void make_scale(const Vector3 &p_scale); + int get_pixels_per_meter(int p_for_pixel_width) const; + operator Transform() const; + + CameraMatrix(); + CameraMatrix(const Transform &p_transform); + ~CameraMatrix(); +}; + +Vector3 CameraMatrix::xform(const Vector3 &p_vec3) const { + Vector3 ret; + ret.x = matrix[0][0] * p_vec3.x + matrix[1][0] * p_vec3.y + matrix[2][0] * p_vec3.z + matrix[3][0]; + ret.y = matrix[0][1] * p_vec3.x + matrix[1][1] * p_vec3.y + matrix[2][1] * p_vec3.z + matrix[3][1]; + ret.z = matrix[0][2] * p_vec3.x + matrix[1][2] * p_vec3.y + matrix[2][2] * p_vec3.z + matrix[3][2]; + real_t w = matrix[0][3] * p_vec3.x + matrix[1][3] * p_vec3.y + matrix[2][3] * p_vec3.z + matrix[3][3]; + return ret / w; +} + +#endif diff --git a/core/math/math_defs.h b/core/math/math_defs.h new file mode 100644 index 0000000..4922254 --- /dev/null +++ b/core/math/math_defs.h @@ -0,0 +1,115 @@ +/*************************************************************************/ +/* math_defs.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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_DEFS_H +#define MATH_DEFS_H + +#define CMP_EPSILON 0.00001 +#define CMP_EPSILON2 (CMP_EPSILON * CMP_EPSILON) + +#define CMP_NORMALIZE_TOLERANCE 0.000001 +#define CMP_POINT_IN_PLANE_EPSILON 0.00001 + +#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 + +//this epsilon is for values related to a unit size (scalar or vector len) +#ifdef PRECISE_MATH_CHECKS +#define UNIT_EPSILON 0.00001 +#else +//tolerate some more floating point error normally +#define UNIT_EPSILON 0.001 +#endif + +#define USEC_TO_SEC(m_usec) ((m_usec) / 1000000.0) + +enum ClockDirection { + CLOCKWISE, + COUNTERCLOCKWISE +}; + +enum Orientation { + + HORIZONTAL, + VERTICAL +}; + +enum HAlign { + + 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/plane.cpp b/core/math/plane.cpp new file mode 100644 index 0000000..6584441 --- /dev/null +++ b/core/math/plane.cpp @@ -0,0 +1,154 @@ +/*************************************************************************/ +/* plane.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "plane.h" + +#include "core/math/math.h" +#include "core/math/math_defs.h" + +void Plane::set_normal(const Vector3 &p_normal) { + normal = p_normal; +} + +void Plane::normalize() { + real_t l = normal.length(); + if (l == 0) { + *this = Plane(0, 0, 0, 0); + return; + } + normal /= l; + d /= l; +} + +Plane Plane::normalized() const { + Plane p = *this; + p.normalize(); + return p; +} + +Vector3 Plane::get_any_point() const { + return get_normal() * d; +} + +Vector3 Plane::get_any_perpendicular_normal() const { + static const Vector3 p1 = Vector3(1, 0, 0); + static const Vector3 p2 = Vector3(0, 1, 0); + Vector3 p; + + if (ABS(normal.dot(p1)) > 0.99) { // if too similar to p1 + p = p2; // use p2 + } else { + p = p1; // use p1 + } + + p -= normal * normal.dot(p); + p.normalize(); + + return p; +} + +/* intersections */ + +bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result) const { + const Plane &p_plane0 = *this; + Vector3 normal0 = p_plane0.normal; + Vector3 normal1 = p_plane1.normal; + Vector3 normal2 = p_plane2.normal; + + real_t denom = normal0.cross(normal1).dot(normal2); + + if (Math::is_zero_approx(denom)) { + return false; + } + + if (r_result) { + *r_result = ((normal1.cross(normal2) * p_plane0.d) + + (normal2.cross(normal0) * p_plane1.d) + + (normal0.cross(normal1) * p_plane2.d)) / + denom; + } + + return true; +} + +bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const { + Vector3 segment = p_dir; + real_t den = normal.dot(segment); + + //printf("den is %i\n",den); + if (Math::is_zero_approx(den)) { + return false; + } + + real_t dist = (normal.dot(p_from) - d) / den; + //printf("dist is %i\n",dist); + + if (dist > CMP_EPSILON) { //this is a ray, before the emitting pos (p_from) doesn't exist + + return false; + } + + dist = -dist; + *p_intersection = p_from + segment * dist; + + return true; +} + +bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const { + Vector3 segment = p_begin - p_end; + real_t den = normal.dot(segment); + + //printf("den is %i\n",den); + if (Math::is_zero_approx(den)) { + return false; + } + + real_t dist = (normal.dot(p_begin) - d) / den; + //printf("dist is %i\n",dist); + + if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { + return false; + } + + dist = -dist; + *p_intersection = p_begin + segment * dist; + + return true; +} + +/* misc */ + +bool Plane::is_equal_approx(const Plane &p_plane) const { + return normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d); +} + +Plane::operator String() const { + return normal.operator String() + ", " + String::num(d); +} diff --git a/core/math/plane.h b/core/math/plane.h new file mode 100644 index 0000000..712becc --- /dev/null +++ b/core/math/plane.h @@ -0,0 +1,133 @@ +/*************************************************************************/ +/* plane.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 PLANE_H +#define PLANE_H + +#include "core/typedefs.h" +#include "core/math/vector3.h" +#include "core/string.h" +#include "core/math/math_defs.h" + +class Plane { +public: + Vector3 normal; + real_t d; + + void set_normal(const Vector3 &p_normal); + _FORCE_INLINE_ Vector3 get_normal() const { return normal; }; ///Point is coplanar, CMP_EPSILON for precision + + void normalize(); + Plane normalized() const; + + /* Plane-Point operations */ + + _FORCE_INLINE_ Vector3 center() const { return normal * d; } + Vector3 get_any_point() const; + Vector3 get_any_perpendicular_normal() const; + + _FORCE_INLINE_ bool is_point_over(const Vector3 &p_point) const; ///< Point is over plane + _FORCE_INLINE_ real_t distance_to(const Vector3 &p_point) const; + _FORCE_INLINE_ bool has_point(const Vector3 &p_point, real_t _epsilon = CMP_EPSILON) const; + + /* intersections */ + + bool intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result = nullptr) const; + bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const; + bool intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const; + + _FORCE_INLINE_ Vector3 project(const Vector3 &p_point) const { + return p_point - normal * distance_to(p_point); + } + + /* misc */ + + Plane operator-() const { return Plane(-normal, -d); } + bool is_equal_approx(const Plane &p_plane) const; + + _FORCE_INLINE_ bool operator==(const Plane &p_plane) const; + _FORCE_INLINE_ bool operator!=(const Plane &p_plane) const; + operator String() const; + + _FORCE_INLINE_ Plane() : + d(0) {} + _FORCE_INLINE_ Plane(real_t p_a, real_t p_b, real_t p_c, real_t p_d) : + normal(p_a, p_b, p_c), + d(p_d) {} + + _FORCE_INLINE_ Plane(const Vector3 &p_normal, real_t p_d); + _FORCE_INLINE_ Plane(const Vector3 &p_point, const Vector3 &p_normal); + _FORCE_INLINE_ Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3, ClockDirection p_dir = CLOCKWISE); +}; + +bool Plane::is_point_over(const Vector3 &p_point) const { + return (normal.dot(p_point) > d); +} + +real_t Plane::distance_to(const Vector3 &p_point) const { + return (normal.dot(p_point) - d); +} + +bool Plane::has_point(const Vector3 &p_point, real_t _epsilon) const { + real_t dist = normal.dot(p_point) - d; + dist = ABS(dist); + return (dist <= _epsilon); +} + +Plane::Plane(const Vector3 &p_normal, real_t p_d) : + normal(p_normal), + d(p_d) { +} + +Plane::Plane(const Vector3 &p_point, const Vector3 &p_normal) : + normal(p_normal), + d(p_normal.dot(p_point)) { +} + +Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3, ClockDirection p_dir) { + if (p_dir == CLOCKWISE) { + normal = (p_point1 - p_point3).cross(p_point1 - p_point2); + } else { + normal = (p_point1 - p_point2).cross(p_point1 - p_point3); + } + + normal.normalize(); + d = normal.dot(p_point1); +} + +bool Plane::operator==(const Plane &p_plane) const { + return normal == p_plane.normal && d == p_plane.d; +} + +bool Plane::operator!=(const Plane &p_plane) const { + return normal != p_plane.normal || d != p_plane.d; +} + +#endif // PLANE_H diff --git a/core/math/quat.cpp b/core/math/quat.cpp new file mode 100644 index 0000000..8a4477b --- /dev/null +++ b/core/math/quat.cpp @@ -0,0 +1,254 @@ +/*************************************************************************/ +/* quat.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "quat.h" + +#include "core/math/basis.h" + +real_t Quat::angle_to(const Quat &p_to) const { + real_t d = dot(p_to); + return Math::acos(CLAMP(d * d * 2 - 1, -1, 1)); +} + +// set_euler_xyz expects a vector containing the Euler angles in the format +// (ax,ay,az), where ax is the angle of rotation around x axis, +// and similar for other axes. +// This implementation uses XYZ convention (Z is the first rotation). +void Quat::set_euler_xyz(const Vector3 &p_euler) { + real_t half_a1 = p_euler.x * 0.5; + real_t half_a2 = p_euler.y * 0.5; + real_t half_a3 = p_euler.z * 0.5; + + // R = X(a1).Y(a2).Z(a3) convention for Euler angles. + // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-2) + // a3 is the angle of the first rotation, following the notation in this reference. + + real_t cos_a1 = Math::cos(half_a1); + real_t sin_a1 = Math::sin(half_a1); + real_t cos_a2 = Math::cos(half_a2); + real_t sin_a2 = Math::sin(half_a2); + real_t cos_a3 = Math::cos(half_a3); + real_t sin_a3 = Math::sin(half_a3); + + set(sin_a1 * cos_a2 * cos_a3 + sin_a2 * sin_a3 * cos_a1, + -sin_a1 * sin_a3 * cos_a2 + sin_a2 * cos_a1 * cos_a3, + sin_a1 * sin_a2 * cos_a3 + sin_a3 * cos_a1 * cos_a2, + -sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3); +} + +// get_euler_xyz returns a vector containing the Euler angles in the format +// (ax,ay,az), where ax is the angle of rotation around x axis, +// and similar for other axes. +// This implementation uses XYZ convention (Z is the first rotation). +Vector3 Quat::get_euler_xyz() const { + Basis m(*this); + return m.get_euler_xyz(); +} + +// set_euler_yxz expects a vector containing the Euler angles in the format +// (ax,ay,az), where ax is the angle of rotation around x axis, +// and similar for other axes. +// This implementation uses YXZ convention (Z is the first rotation). +void Quat::set_euler_yxz(const Vector3 &p_euler) { + real_t half_a1 = p_euler.y * 0.5; + real_t half_a2 = p_euler.x * 0.5; + real_t half_a3 = p_euler.z * 0.5; + + // R = Y(a1).X(a2).Z(a3) convention for Euler angles. + // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6) + // a3 is the angle of the first rotation, following the notation in this reference. + + real_t cos_a1 = Math::cos(half_a1); + real_t sin_a1 = Math::sin(half_a1); + real_t cos_a2 = Math::cos(half_a2); + real_t sin_a2 = Math::sin(half_a2); + real_t cos_a3 = Math::cos(half_a3); + real_t sin_a3 = Math::sin(half_a3); + + 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 * cos_a2 * sin_a3, + sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3); +} + +// get_euler_yxz returns a vector containing the Euler angles in the format +// (ax,ay,az), where ax is the angle of rotation around x axis, +// 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_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized."); +#endif + Basis m(*this); + return m.get_euler_yxz(); +} + +void Quat::operator*=(const Quat &p_q) { + set(w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y, + w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z, + w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x, + w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z); +} + +Quat Quat::operator*(const Quat &p_q) const { + Quat r = *this; + r *= p_q; + return r; +} + +bool Quat::is_equal_approx(const Quat &p_quat) const { + return Math::is_equal_approx(x, p_quat.x) && Math::is_equal_approx(y, p_quat.y) && Math::is_equal_approx(z, p_quat.z) && Math::is_equal_approx(w, p_quat.w); +} + +real_t Quat::length() const { + return Math::sqrt(length_squared()); +} + +void Quat::normalize() { + *this /= length(); +} + +Quat Quat::normalized() const { + return *this / length(); +} + +bool Quat::is_normalized() const { + return Math::is_equal_approx(length_squared(), 1);//, (real_t)UNIT_EPSILON); //use less epsilon +} + +Quat Quat::inverse() const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The quaternion must be normalized."); +#endif + return Quat(-x, -y, -z, w); +} + +Quat Quat::slerp(const Quat &p_to, const real_t &p_weight) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); + ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quat(), "The end quaternion must be normalized."); +#endif + Quat to1; + real_t omega, cosom, sinom, scale0, scale1; + + // calc cosine + cosom = dot(p_to); + + // adjust signs (if necessary) + if (cosom < 0.0) { + cosom = -cosom; + to1.x = -p_to.x; + to1.y = -p_to.y; + to1.z = -p_to.z; + to1.w = -p_to.w; + } else { + to1.x = p_to.x; + to1.y = p_to.y; + to1.z = p_to.z; + to1.w = p_to.w; + } + + // calculate coefficients + + if ((1.0 - cosom) > CMP_EPSILON) { + // standard case (slerp) + omega = Math::acos(cosom); + sinom = Math::sin(omega); + scale0 = Math::sin((1.0 - p_weight) * omega) / sinom; + scale1 = Math::sin(p_weight * omega) / sinom; + } else { + // "from" and "to" quaternions are very close + // ... so we can do a linear interpolation + scale0 = 1.0 - p_weight; + scale1 = p_weight; + } + // calculate final values + return Quat( + scale0 * x + scale1 * to1.x, + scale0 * y + scale1 * to1.y, + scale0 * z + scale1 * to1.z, + scale0 * w + scale1 * to1.w); +} + +Quat Quat::slerpni(const Quat &p_to, const real_t &p_weight) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); + ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quat(), "The end quaternion must be normalized."); +#endif + const Quat &from = *this; + + real_t dot = from.dot(p_to); + + if (Math::abs(dot) > 0.9999) { + return from; + } + + real_t theta = Math::acos(dot), + sinT = 1.0 / Math::sin(theta), + newFactor = Math::sin(p_weight * theta) * sinT, + invFactor = Math::sin((1.0 - p_weight) * theta) * sinT; + + return Quat(invFactor * from.x + newFactor * p_to.x, + invFactor * from.y + newFactor * p_to.y, + invFactor * from.z + newFactor * p_to.z, + invFactor * from.w + newFactor * p_to.w); +} + +Quat Quat::cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); + ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quat(), "The end quaternion must be normalized."); +#endif + //the only way to do slerp :| + real_t t2 = (1.0 - p_weight) * p_weight * 2; + Quat sp = this->slerp(p_b, p_weight); + Quat sq = p_pre_a.slerpni(p_post_b, p_weight); + return sp.slerpni(sq, t2); +} + +Quat::operator String() const { + return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w); +} + +void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) { +#ifdef MATH_CHECKS + ERR_FAIL_COND_MSG(!axis.is_normalized(), "The axis Vector3 must be normalized."); +#endif + real_t d = axis.length(); + if (d == 0) { + set(0, 0, 0, 0); + } else { + real_t sin_angle = Math::sin(angle * 0.5); + real_t cos_angle = Math::cos(angle * 0.5); + real_t s = sin_angle / d; + set(axis.x * s, axis.y * s, axis.z * s, + cos_angle); + } +} diff --git a/core/math/quat.h b/core/math/quat.h new file mode 100644 index 0000000..9b839f4 --- /dev/null +++ b/core/math/quat.h @@ -0,0 +1,232 @@ +/*************************************************************************/ +/* quat.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 QUAT_H +#define QUAT_H + +#include "core/math/math_defs.h" +#include "core/math/math.h" +#include "core/math/vector3.h" +#include "core/string.h" +#include "core/typedefs.h" + +class Quat { +public: + real_t x, y, z, w; + + _FORCE_INLINE_ real_t length_squared() const; + bool is_equal_approx(const Quat &p_quat) const; + real_t length() const; + void normalize(); + Quat normalized() const; + bool is_normalized() const; + Quat inverse() const; + _FORCE_INLINE_ real_t dot(const Quat &p_q) const; + real_t angle_to(const Quat &p_to) const; + + void set_euler_xyz(const Vector3 &p_euler); + Vector3 get_euler_xyz() const; + void set_euler_yxz(const Vector3 &p_euler); + Vector3 get_euler_yxz() const; + + void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }; + Vector3 get_euler() const { return get_euler_yxz(); }; + + Quat slerp(const Quat &p_to, const real_t &p_weight) const; + Quat slerpni(const Quat &p_to, const real_t &p_weight) const; + Quat cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) 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); + 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 &p_q); + Quat operator*(const Quat &p_q) const; + + Quat operator*(const Vector3 &v) const { + return Quat(w * v.x + y * v.z - z * v.y, + w * v.y + z * v.x - x * v.z, + w * v.z + x * v.y - y * v.x, + -x * v.x - y * v.y - z * v.z); + } + + _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized."); +#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 &p_q); + _FORCE_INLINE_ void operator-=(const Quat &p_q); + _FORCE_INLINE_ void operator*=(const real_t &s); + _FORCE_INLINE_ void operator/=(const real_t &s); + _FORCE_INLINE_ Quat operator+(const Quat &q2) const; + _FORCE_INLINE_ Quat operator-(const Quat &q2) const; + _FORCE_INLINE_ Quat operator-() const; + _FORCE_INLINE_ Quat operator*(const real_t &s) const; + _FORCE_INLINE_ Quat operator/(const real_t &s) const; + + _FORCE_INLINE_ bool operator==(const Quat &p_quat) const; + _FORCE_INLINE_ bool operator!=(const Quat &p_quat) const; + + operator String() const; + + inline void set(real_t p_x, real_t p_y, real_t p_z, real_t p_w) { + x = p_x; + y = p_y; + z = p_z; + w = p_w; + } + inline Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) : + x(p_x), + y(p_y), + z(p_z), + w(p_w) { + } + Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); } + + Quat(const Vector3 &euler) { set_euler(euler); } + Quat(const Quat &p_q) : + x(p_q.x), + y(p_q.y), + z(p_q.z), + w(p_q.w) { + } + + Quat operator=(const Quat &p_q) { + x = p_q.x; + y = p_q.y; + z = p_q.z; + w = p_q.w; + return *this; + } + + Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc + { + Vector3 c = v0.cross(v1); + real_t d = v0.dot(v1); + + if (d < -1.0 + CMP_EPSILON) { + x = 0; + y = 1; + z = 0; + w = 0; + } else { + real_t s = Math::sqrt((1.0 + d) * 2.0); + real_t rs = 1.0 / s; + + x = c.x * rs; + y = c.y * rs; + z = c.z * rs; + w = s * 0.5; + } + } + + inline Quat() : + x(0), + y(0), + z(0), + w(1) { + } +}; + +real_t Quat::dot(const Quat &p_q) const { + return x * p_q.x + y * p_q.y + z * p_q.z + w * p_q.w; +} + +real_t Quat::length_squared() const { + return dot(*this); +} + +void Quat::operator+=(const Quat &p_q) { + x += p_q.x; + y += p_q.y; + z += p_q.z; + w += p_q.w; +} + +void Quat::operator-=(const Quat &p_q) { + x -= p_q.x; + y -= p_q.y; + z -= p_q.z; + w -= p_q.w; +} + +void Quat::operator*=(const real_t &s) { + x *= s; + y *= s; + z *= s; + w *= s; +} + +void Quat::operator/=(const real_t &s) { + *this *= 1.0 / s; +} + +Quat Quat::operator+(const Quat &q2) const { + const Quat &q1 = *this; + return Quat(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); +} + +Quat Quat::operator-(const Quat &q2) const { + const Quat &q1 = *this; + return Quat(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w); +} + +Quat Quat::operator-() const { + const Quat &q2 = *this; + return Quat(-q2.x, -q2.y, -q2.z, -q2.w); +} + +Quat Quat::operator*(const real_t &s) const { + return Quat(x * s, y * s, z * s, w * s); +} + +Quat Quat::operator/(const real_t &s) const { + return *this * (1.0 / s); +} + +bool Quat::operator==(const Quat &p_quat) const { + return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w; +} + +bool Quat::operator!=(const Quat &p_quat) const { + return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w; +} + +#endif diff --git a/core/math/transform.cpp b/core/math/transform.cpp new file mode 100644 index 0000000..3928527 --- /dev/null +++ b/core/math/transform.cpp @@ -0,0 +1,202 @@ +/*************************************************************************/ +/* transform.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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.h" + +#include "core/math/math.h" + +void Transform::affine_invert() { + basis.invert(); + origin = basis.xform(-origin); +} + +Transform Transform::affine_inverse() const { + Transform ret = *this; + ret.affine_invert(); + return ret; +} + +void Transform::invert() { + basis.transpose(); + origin = basis.xform(-origin); +} + +Transform Transform::inverse() const { + // FIXME: this function assumes the basis is a rotation matrix, with no scaling. + // Transform::affine_inverse can handle matrices with scaling, so GDScript should eventually use that. + Transform ret = *this; + ret.invert(); + return ret; +} + +void Transform::rotate(const Vector3 &p_axis, real_t p_phi) { + *this = rotated(p_axis, p_phi); +} + +Transform Transform::rotated(const Vector3 &p_axis, real_t p_phi) const { + return Transform(Basis(p_axis, p_phi), Vector3()) * (*this); +} + +void Transform::rotate_basis(const Vector3 &p_axis, real_t p_phi) { + basis.rotate(p_axis, p_phi); +} + +Transform Transform::looking_at(const Vector3 &p_target, const Vector3 &p_up) const { + Transform t = *this; + t.set_look_at(origin, p_target, p_up); + return t; +} + +void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) { +#ifdef MATH_CHECKS + ERR_FAIL_COND(p_eye == p_target); + ERR_FAIL_COND(p_up.length() == 0); +#endif + // Reference: MESA source code + Vector3 v_x, v_y, v_z; + + /* Make rotation matrix */ + + /* Z vector */ + v_z = p_eye - p_target; + + v_z.normalize(); + + v_y = p_up; + + v_x = v_y.cross(v_z); +#ifdef MATH_CHECKS + ERR_FAIL_COND(v_x.length() == 0); +#endif + + /* Recompute Y = Z cross X */ + v_y = v_z.cross(v_x); + + v_x.normalize(); + v_y.normalize(); + + basis.set(v_x, v_y, v_z); + + origin = p_eye; +} + +Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c) const { + /* not sure if very "efficient" but good enough? */ + + Vector3 src_scale = basis.get_scale(); + Quat src_rot = basis.get_rotation_quat(); + Vector3 src_loc = origin; + + Vector3 dst_scale = p_transform.basis.get_scale(); + Quat dst_rot = p_transform.basis.get_rotation_quat(); + Vector3 dst_loc = p_transform.origin; + + Transform interp; + interp.basis.set_quat_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c)); + interp.origin = src_loc.lerp(dst_loc, p_c); + + return interp; +} + +void Transform::scale(const Vector3 &p_scale) { + basis.scale(p_scale); + origin *= p_scale; +} + +Transform Transform::scaled(const Vector3 &p_scale) const { + Transform t = *this; + t.scale(p_scale); + return t; +} + +void Transform::scale_basis(const Vector3 &p_scale) { + basis.scale(p_scale); +} + +void Transform::translate(real_t p_tx, real_t p_ty, real_t p_tz) { + translate(Vector3(p_tx, p_ty, p_tz)); +} +void Transform::translate(const Vector3 &p_translation) { + for (int i = 0; i < 3; i++) { + origin[i] += basis[i].dot(p_translation); + } +} + +Transform Transform::translated(const Vector3 &p_translation) const { + Transform t = *this; + t.translate(p_translation); + return t; +} + +void Transform::orthonormalize() { + basis.orthonormalize(); +} + +Transform Transform::orthonormalized() const { + Transform _copy = *this; + _copy.orthonormalize(); + return _copy; +} + +bool Transform::is_equal_approx(const Transform &p_transform) const { + return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin); +} + +bool Transform::operator==(const Transform &p_transform) const { + return (basis == p_transform.basis && origin == p_transform.origin); +} +bool Transform::operator!=(const Transform &p_transform) const { + return (basis != p_transform.basis || origin != p_transform.origin); +} + +void Transform::operator*=(const Transform &p_transform) { + origin = xform(p_transform.origin); + basis *= p_transform.basis; +} + +Transform Transform::operator*(const Transform &p_transform) const { + Transform t = *this; + t *= p_transform; + return t; +} + +Transform::operator String() const { + return basis.operator String() + " - " + origin.operator String(); +} + +Transform::Transform(const Basis &p_basis, const Vector3 &p_origin) : + basis(p_basis), + origin(p_origin) { +} + +Transform::Transform(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t ox, real_t oy, real_t oz) { + basis = Basis(xx, xy, xz, yx, yy, yz, zx, zy, zz); + origin = Vector3(ox, oy, oz); +} diff --git a/core/math/transform.h b/core/math/transform.h new file mode 100644 index 0000000..244ddf6 --- /dev/null +++ b/core/math/transform.h @@ -0,0 +1,257 @@ +/*************************************************************************/ +/* transform.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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_H +#define TRANSFORM_H + +#include "core/math/aabb.h" +#include "core/math/basis.h" +#include "core/math/plane.h" +#include "core/containers/vector.h" + +class Transform { +public: + Basis basis; + Vector3 origin; + + void invert(); + Transform inverse() const; + + void affine_invert(); + Transform affine_inverse() const; + + Transform rotated(const Vector3 &p_axis, real_t p_phi) const; + + void rotate(const Vector3 &p_axis, real_t p_phi); + void rotate_basis(const Vector3 &p_axis, real_t p_phi); + + void set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up); + Transform looking_at(const Vector3 &p_target, const Vector3 &p_up) const; + + void scale(const Vector3 &p_scale); + Transform scaled(const Vector3 &p_scale) const; + void scale_basis(const Vector3 &p_scale); + void translate(real_t p_tx, real_t p_ty, real_t p_tz); + void translate(const Vector3 &p_translation); + Transform translated(const Vector3 &p_translation) const; + + const Basis &get_basis() const { return basis; } + void set_basis(const Basis &p_basis) { basis = p_basis; } + + const Vector3 &get_origin() const { return origin; } + void set_origin(const Vector3 &p_origin) { origin = p_origin; } + + void orthonormalize(); + Transform orthonormalized() const; + bool is_equal_approx(const Transform &p_transform) const; + + bool operator==(const Transform &p_transform) const; + bool operator!=(const Transform &p_transform) const; + + _FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const; + _FORCE_INLINE_ AABB xform(const AABB &p_aabb) const; + _FORCE_INLINE_ Vector xform(const Vector &p_array) const; + + // NOTE: These are UNSAFE with non-uniform scaling, and will produce incorrect results. + // They use the transpose. + // For safe inverse transforms, xform by the affine_inverse. + _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const; + _FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const; + _FORCE_INLINE_ Vector xform_inv(const Vector &p_array) const; + + // Safe with non-uniform scaling (uses affine_inverse). + _FORCE_INLINE_ Plane xform(const Plane &p_plane) const; + _FORCE_INLINE_ Plane xform_inv(const Plane &p_plane) const; + + // These fast versions use precomputed affine inverse, and should be used in bottleneck areas where + // multiple planes are to be transformed. + _FORCE_INLINE_ Plane xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const; + static _FORCE_INLINE_ Plane xform_inv_fast(const Plane &p_plane, const Transform &p_inverse, const Basis &p_basis_transpose); + + void operator*=(const Transform &p_transform); + Transform operator*(const Transform &p_transform) const; + + Transform interpolate_with(const Transform &p_transform, real_t p_c) const; + + _FORCE_INLINE_ Transform inverse_xform(const Transform &t) const { + Vector3 v = t.origin - origin; + return Transform(basis.transpose_xform(t.basis), + basis.xform(v)); + } + + void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t tx, real_t ty, real_t tz) { + basis.set(xx, xy, xz, yx, yy, yz, zx, zy, zz); + origin.x = tx; + origin.y = ty; + origin.z = tz; + } + + operator String() const; + + Transform(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t ox, real_t oy, real_t oz); + Transform(const Basis &p_basis, const Vector3 &p_origin = Vector3()); + Transform() {} +}; + +_FORCE_INLINE_ Vector3 Transform::xform(const Vector3 &p_vector) const { + return Vector3( + basis[0].dot(p_vector) + origin.x, + basis[1].dot(p_vector) + origin.y, + basis[2].dot(p_vector) + origin.z); +} + +_FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3 &p_vector) const { + Vector3 v = p_vector - origin; + + return Vector3( + (basis.elements[0][0] * v.x) + (basis.elements[1][0] * v.y) + (basis.elements[2][0] * v.z), + (basis.elements[0][1] * v.x) + (basis.elements[1][1] * v.y) + (basis.elements[2][1] * v.z), + (basis.elements[0][2] * v.x) + (basis.elements[1][2] * v.y) + (basis.elements[2][2] * v.z)); +} + +// Neither the plane regular xform or xform_inv are particularly efficient, +// as they do a basis inverse. For xforming a large number +// of planes it is better to pre-calculate the inverse transpose basis once +// and reuse it for each plane, by using the 'fast' version of the functions. +_FORCE_INLINE_ Plane Transform::xform(const Plane &p_plane) const { + Basis b = basis.inverse(); + b.transpose(); + return xform_fast(p_plane, b); +} + +_FORCE_INLINE_ Plane Transform::xform_inv(const Plane &p_plane) const { + Transform inv = affine_inverse(); + Basis basis_transpose = basis.transposed(); + return xform_inv_fast(p_plane, inv, basis_transpose); +} + +_FORCE_INLINE_ AABB Transform::xform(const AABB &p_aabb) const { + /* http://dev.theomader.com/transform-bounding-boxes/ */ + Vector3 min = p_aabb.position; + Vector3 max = p_aabb.position + p_aabb.size; + Vector3 tmin, tmax; + for (int i = 0; i < 3; i++) { + tmin[i] = tmax[i] = origin[i]; + for (int j = 0; j < 3; j++) { + real_t e = basis[i][j] * min[j]; + real_t f = basis[i][j] * max[j]; + if (e < f) { + tmin[i] += e; + tmax[i] += f; + } else { + tmin[i] += f; + tmax[i] += e; + } + } + } + AABB r_aabb; + r_aabb.position = tmin; + r_aabb.size = tmax - tmin; + return r_aabb; +} + +_FORCE_INLINE_ AABB Transform::xform_inv(const AABB &p_aabb) const { + /* define vertices */ + Vector3 vertices[8] = { + Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z), + Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z), + Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z), + Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z), + Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z), + Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z), + Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z), + Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z) + }; + + AABB ret; + + ret.position = xform_inv(vertices[0]); + + for (int i = 1; i < 8; i++) { + ret.expand_to(xform_inv(vertices[i])); + } + + return ret; +} + +Vector Transform::xform(const Vector &p_array) const { + Vector array; + array.resize(p_array.size()); + + for (int i = 0; i < p_array.size(); ++i) { + array[i] = xform(p_array[i]); + } + return array; +} + +Vector Transform::xform_inv(const Vector &p_array) const { + Vector array; + array.resize(p_array.size()); + + for (int i = 0; i < p_array.size(); ++i) { + array[i] = xform_inv(p_array[i]); + } + return array; +} + +_FORCE_INLINE_ Plane Transform::xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const { + // Transform a single point on the plane. + Vector3 point = p_plane.normal * p_plane.d; + point = xform(point); + + // Use inverse transpose for correct normals with non-uniform scaling. + Vector3 normal = p_basis_inverse_transpose.xform(p_plane.normal); + normal.normalize(); + + real_t d = normal.dot(point); + return Plane(normal, d); +} + +_FORCE_INLINE_ Plane Transform::xform_inv_fast(const Plane &p_plane, const Transform &p_inverse, const Basis &p_basis_transpose) { + // Transform a single point on the plane. + Vector3 point = p_plane.normal * p_plane.d; + point = p_inverse.xform(point); + + // Note that instead of precalculating the transpose, an alternative + // would be to use the transpose for the basis transform. + // However that would be less SIMD friendly (requiring a swizzle). + // So the cost is one extra precalced value in the calling code. + // This is probably worth it, as this could be used in bottleneck areas. And + // where it is not a bottleneck, the non-fast method is fine. + + // Use transpose for correct normals with non-uniform scaling. + Vector3 normal = p_basis_transpose.xform(p_plane.normal); + normal.normalize(); + + real_t d = normal.dot(point); + return Plane(normal, d); +} + +#endif // TRANSFORM_H diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp new file mode 100644 index 0000000..608040f --- /dev/null +++ b/core/math/transform_2d.cpp @@ -0,0 +1,274 @@ +/*************************************************************************/ +/* transform_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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" + +/** Generic swap template */ +#ifndef SWAP + +#define SWAP(m_x, m_y) __swap_tmpl((m_x), (m_y)) +template +inline void __swap_tmpl(T &x, T &y) { + T aux = x; + x = y; + y = aux; +} + +#endif //swap + +void Transform2D::invert() { + // FIXME: this function assumes the basis is a rotation matrix, with no scaling. + // Transform2D::affine_inverse can handle matrices with scaling, so GDScript should eventually use that. + SWAP(elements[0][1], elements[1][0]); + elements[2] = basis_xform(-elements[2]); +} + +Transform2D Transform2D::inverse() const { + Transform2D inv = *this; + inv.invert(); + return inv; +} + +void Transform2D::affine_invert() { + real_t det = basis_determinant(); +#ifdef MATH_CHECKS + ERR_FAIL_COND(det == 0); +#endif + real_t idet = 1.0 / det; + + SWAP(elements[0][0], elements[1][1]); + elements[0] *= Vector2(idet, -idet); + elements[1] *= Vector2(-idet, idet); + + elements[2] = basis_xform(-elements[2]); +} + +Transform2D Transform2D::affine_inverse() const { + Transform2D inv = *this; + inv.affine_invert(); + return inv; +} + +void Transform2D::rotate(real_t p_phi) { + *this = Transform2D(p_phi, Vector2()) * (*this); +} + +real_t Transform2D::get_rotation() const { + return Math::atan2(elements[0].y, elements[0].x); +} + +void Transform2D::set_rotation(real_t p_rot) { + Vector2 scale = get_scale(); + real_t cr = Math::cos(p_rot); + real_t sr = Math::sin(p_rot); + elements[0][0] = cr; + elements[0][1] = sr; + elements[1][0] = -sr; + elements[1][1] = cr; + set_scale(scale); +} + +Transform2D::Transform2D(real_t p_rot, const Vector2 &p_pos) { + real_t cr = Math::cos(p_rot); + real_t sr = Math::sin(p_rot); + elements[0][0] = cr; + elements[0][1] = sr; + elements[1][0] = -sr; + elements[1][1] = cr; + elements[2] = p_pos; +} + +Vector2 Transform2D::get_scale() const { + real_t det_sign = SGN(basis_determinant()); + return Vector2(elements[0].length(), det_sign * elements[1].length()); +} + +void Transform2D::set_scale(const Vector2 &p_scale) { + elements[0].normalize(); + elements[1].normalize(); + elements[0] *= p_scale.x; + elements[1] *= p_scale.y; +} + +void Transform2D::scale(const Vector2 &p_scale) { + scale_basis(p_scale); + elements[2] *= p_scale; +} +void Transform2D::scale_basis(const Vector2 &p_scale) { + elements[0][0] *= p_scale.x; + elements[0][1] *= p_scale.y; + elements[1][0] *= p_scale.x; + elements[1][1] *= p_scale.y; +} +void Transform2D::translate(real_t p_tx, real_t p_ty) { + translate(Vector2(p_tx, p_ty)); +} +void Transform2D::translate(const Vector2 &p_translation) { + elements[2] += basis_xform(p_translation); +} + +void Transform2D::orthonormalize() { + // Gram-Schmidt Process + + Vector2 x = elements[0]; + Vector2 y = elements[1]; + + x.normalize(); + y = (y - x * (x.dot(y))); + y.normalize(); + + elements[0] = x; + elements[1] = y; +} + +Transform2D Transform2D::orthonormalized() const { + Transform2D on = *this; + on.orthonormalize(); + return on; +} + +bool Transform2D::is_equal_approx(const Transform2D &p_transform) const { + return elements[0].is_equal_approx(p_transform.elements[0]) && elements[1].is_equal_approx(p_transform.elements[1]) && elements[2].is_equal_approx(p_transform.elements[2]); +} + +bool Transform2D::operator==(const Transform2D &p_transform) const { + for (int i = 0; i < 3; i++) { + if (elements[i] != p_transform.elements[i]) { + return false; + } + } + + return true; +} + +bool Transform2D::operator!=(const Transform2D &p_transform) const { + for (int i = 0; i < 3; i++) { + if (elements[i] != p_transform.elements[i]) { + return true; + } + } + + return false; +} + +void Transform2D::operator*=(const Transform2D &p_transform) { + elements[2] = xform(p_transform.elements[2]); + + real_t x0, x1, y0, y1; + + x0 = tdotx(p_transform.elements[0]); + x1 = tdoty(p_transform.elements[0]); + y0 = tdotx(p_transform.elements[1]); + y1 = tdoty(p_transform.elements[1]); + + elements[0][0] = x0; + elements[0][1] = x1; + elements[1][0] = y0; + elements[1][1] = y1; +} + +Transform2D Transform2D::operator*(const Transform2D &p_transform) const { + Transform2D t = *this; + t *= p_transform; + return t; +} + +Transform2D Transform2D::scaled(const Vector2 &p_scale) const { + Transform2D copy = *this; + copy.scale(p_scale); + return copy; +} + +Transform2D Transform2D::basis_scaled(const Vector2 &p_scale) const { + Transform2D copy = *this; + copy.scale_basis(p_scale); + return copy; +} + +Transform2D Transform2D::untranslated() const { + Transform2D copy = *this; + copy.elements[2] = Vector2(); + return copy; +} + +Transform2D Transform2D::translated(const Vector2 &p_offset) const { + Transform2D copy = *this; + copy.translate(p_offset); + return copy; +} + +Transform2D Transform2D::rotated(real_t p_phi) const { + Transform2D copy = *this; + copy.rotate(p_phi); + return copy; +} + +real_t Transform2D::basis_determinant() const { + return elements[0].x * elements[1].y - elements[0].y * elements[1].x; +} + +Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, real_t p_c) const { + //extract parameters + Vector2 p1 = get_origin(); + Vector2 p2 = p_transform.get_origin(); + + real_t r1 = get_rotation(); + real_t r2 = p_transform.get_rotation(); + + Vector2 s1 = get_scale(); + Vector2 s2 = p_transform.get_scale(); + + //slerp rotation + Vector2 v1(Math::cos(r1), Math::sin(r1)); + Vector2 v2(Math::cos(r2), Math::sin(r2)); + + real_t dot = v1.dot(v2); + + dot = CLAMP(dot, -1.0, 1.0); + + Vector2 v; + + if (dot > 0.9995) { + v = v1.lerp(v2, p_c).normalized(); //linearly interpolate to avoid numerical precision issues + } else { + real_t angle = p_c * Math::acos(dot); + Vector2 v3 = (v2 - v1 * dot).normalized(); + v = v1 * Math::cos(angle) + v3 * Math::sin(angle); + } + + //construct matrix + Transform2D res(Math::atan2(v.y, v.x), p1.lerp(p2, p_c)); + res.scale_basis(s1.lerp(s2, p_c)); + return res; +} + +Transform2D::operator String() const { + return String(String() + elements[0] + ", " + elements[1] + ", " + elements[2]); +} diff --git a/core/math/transform_2d.h b/core/math/transform_2d.h new file mode 100644 index 0000000..121ad93 --- /dev/null +++ b/core/math/transform_2d.h @@ -0,0 +1,224 @@ +/*************************************************************************/ +/* transform_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 "core/math/rect2.h" // also includes vector2, math_funcs, and ustring +#include "core/containers/vector.h" +#include "core/typedefs.h" +#include "core/math/vector2.h" +#include "core/math/math_defs.h" +#include "core/math/math.h" + +struct Transform2D { + // Warning #1: basis of Transform2D is stored differently from Basis. In terms of elements array, the basis matrix looks like "on paper": + // 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 Vector2 &p_scale); + void rotate(real_t p_phi); + + void scale(const Vector2 &p_scale); + void scale_basis(const Vector2 &p_scale); + void translate(real_t p_tx, real_t p_ty); + void translate(const Vector2 &p_translation); + + real_t basis_determinant() const; + + Vector2 get_scale() const; + void set_scale(const Vector2 &p_scale); + + _FORCE_INLINE_ const Vector2 &get_origin() const { return elements[2]; } + _FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { elements[2] = p_origin; } + + Transform2D scaled(const Vector2 &p_scale) const; + Transform2D basis_scaled(const Vector2 &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 is_equal_approx(const Transform2D &p_transform) 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; + _FORCE_INLINE_ Vector xform(const Vector &p_array) const; + _FORCE_INLINE_ Vector xform_inv(const Vector &p_array) 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.w; + Vector2 y = elements[1] * p_rect.h; + Vector2 pos = xform(Vector2(p_rect.x, p_rect.y)); + + Rect2 new_rect; + new_rect.x = pos.x; + new_rect.y = pos.y; + 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 Vector2 &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(Vector2(p_rect.x, p_rect.y)), + xform_inv(Vector2(p_rect.x, p_rect.y + p_rect.h)), + xform_inv(Vector2(p_rect.x + p_rect.w, p_rect.y + p_rect.h)), + xform_inv(Vector2(p_rect.x + p_rect.w, p_rect.y)) + }; + + Rect2 new_rect; + new_rect.x = ends[0].x; + new_rect.y = ends[0].y; + new_rect.expand_to(ends[1]); + new_rect.expand_to(ends[2]); + new_rect.expand_to(ends[3]); + + return new_rect; +} + +Vector Transform2D::xform(const Vector &p_array) const { + Vector array; + array.resize(p_array.size()); + + for (int i = 0; i < p_array.size(); ++i) { + array[i] = xform(p_array[i]); + } + return array; +} + +Vector Transform2D::xform_inv(const Vector &p_array) const { + Vector array; + array.resize(p_array.size()); + + for (int i = 0; i < p_array.size(); ++i) { + array[i] = xform_inv(p_array[i]); + } + return array; +} + +#endif // TRANSFORM_2D_H