diff options
Diffstat (limited to 'core/math')
40 files changed, 1644 insertions, 1145 deletions
diff --git a/core/math/SCsub b/core/math/SCsub index c2731d60e6..4efc902717 100644 --- a/core/math/SCsub +++ b/core/math/SCsub @@ -2,6 +2,6 @@ Import('env') -env.add_source_files(env.core_sources,"*.cpp") +env.add_source_files(env.core_sources, "*.cpp") Export('env') diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 5bbbbdaa5a..a1f471ebe3 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -1,3 +1,31 @@ +/*************************************************************************/ +/* a_star.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "a_star.h" #include "geometry.h" @@ -11,7 +39,7 @@ int AStar::get_available_point_id() const { return points.back()->key()+1; } -void AStar::add_point(int p_id, const Vector3 &p_pos, float p_weight_scale) { +void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { ERR_FAIL_COND(p_id<0); if (!points.has(p_id)) { Point *pt = memnew( Point ); @@ -34,7 +62,7 @@ Vector3 AStar::get_point_pos(int p_id) const{ return points[p_id]->pos; } -float AStar::get_point_weight_scale(int p_id) const{ +real_t AStar::get_point_weight_scale(int p_id) const{ ERR_FAIL_COND_V(!points.has(p_id),0); @@ -117,11 +145,11 @@ void AStar::clear(){ int AStar::get_closest_point(const Vector3& p_point) const{ int closest_id=-1; - float closest_dist=1e20; + real_t closest_dist=1e20; for (const Map<int,Point*>::Element *E=points.front();E;E=E->next()) { - float d = p_point.distance_squared_to(E->get()->pos); + real_t d = p_point.distance_squared_to(E->get()->pos); if (closest_id<0 || d<closest_dist) { closest_dist=d; closest_id=E->key(); @@ -134,7 +162,7 @@ int AStar::get_closest_point(const Vector3& p_point) const{ } Vector3 AStar::get_closest_pos_in_segment(const Vector3& p_point) const { - float closest_dist = 1e20; + real_t closest_dist = 1e20; bool found=false; Vector3 closest_point; @@ -147,7 +175,7 @@ Vector3 AStar::get_closest_pos_in_segment(const Vector3& p_point) const { }; Vector3 p = Geometry::get_closest_point_to_segment(p_point,segment); - float d = p_point.distance_squared_to(p); + real_t d = p_point.distance_squared_to(p); if (!found || d<closest_dist) { closest_point=p; @@ -192,14 +220,14 @@ bool AStar::_solve(Point* begin_point, Point* end_point) { //check open list SelfList<Point> *least_cost_point=NULL; - float least_cost=1e30; + real_t least_cost=1e30; //this could be faster (cache previous results) for (SelfList<Point> *E=open_list.first();E;E=E->next()) { Point *p=E->self(); - float cost=p->distance; + real_t cost=p->distance; cost+=p->pos.distance_to(end_point->pos); cost*=p->weight_scale; @@ -221,7 +249,7 @@ bool AStar::_solve(Point* begin_point, Point* end_point) { Point* e=p->neighbours[i]; - float distance = p->pos.distance_to(e->pos) + p->distance; + real_t distance = p->pos.distance_to(e->pos) + p->distance; distance*=e->weight_scale; @@ -267,10 +295,10 @@ bool AStar::_solve(Point* begin_point, Point* end_point) { } -DVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { +PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { - ERR_FAIL_COND_V(!points.has(p_from_id),DVector<Vector3>()); - ERR_FAIL_COND_V(!points.has(p_to_id),DVector<Vector3>()); + ERR_FAIL_COND_V(!points.has(p_from_id),PoolVector<Vector3>()); + ERR_FAIL_COND_V(!points.has(p_to_id),PoolVector<Vector3>()); pass++; @@ -279,7 +307,7 @@ DVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { Point* b = points[p_to_id]; if (a==b) { - DVector<Vector3> ret; + PoolVector<Vector3> ret; ret.push_back(a->pos); return ret; } @@ -291,7 +319,7 @@ DVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { bool found_route=_solve(begin_point,end_point); if (!found_route) - return DVector<Vector3>(); + return PoolVector<Vector3>(); //midpoints Point *p=end_point; @@ -301,11 +329,11 @@ DVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { p=p->prev_point; } - DVector<Vector3> path; + PoolVector<Vector3> path; path.resize(pc); { - DVector<Vector3>::Write w = path.write(); + PoolVector<Vector3>::Write w = path.write(); Point *p=end_point; int idx=pc-1; @@ -323,10 +351,10 @@ DVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) { } -DVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { +PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { - ERR_FAIL_COND_V(!points.has(p_from_id),DVector<int>()); - ERR_FAIL_COND_V(!points.has(p_to_id),DVector<int>()); + ERR_FAIL_COND_V(!points.has(p_from_id),PoolVector<int>()); + ERR_FAIL_COND_V(!points.has(p_to_id),PoolVector<int>()); pass++; @@ -335,7 +363,7 @@ DVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { Point* b = points[p_to_id]; if (a==b) { - DVector<int> ret; + PoolVector<int> ret; ret.push_back(a->id); return ret; } @@ -347,7 +375,7 @@ DVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { bool found_route=_solve(begin_point,end_point); if (!found_route) - return DVector<int>(); + return PoolVector<int>(); //midpoints Point *p=end_point; @@ -357,11 +385,11 @@ DVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { p=p->prev_point; } - DVector<int> path; + PoolVector<int> path; path.resize(pc); { - DVector<int>::Write w = path.write(); + PoolVector<int>::Write w = path.write(); p=end_point; int idx=pc-1; @@ -379,23 +407,23 @@ DVector<int> AStar::get_id_path(int p_from_id, int p_to_id) { void AStar::_bind_methods() { - ObjectTypeDB::bind_method(_MD("get_available_point_id"),&AStar::get_available_point_id); - ObjectTypeDB::bind_method(_MD("add_point","id","pos","weight_scale"),&AStar::add_point,DEFVAL(1.0)); - ObjectTypeDB::bind_method(_MD("get_point_pos","id"),&AStar::get_point_pos); - ObjectTypeDB::bind_method(_MD("get_point_weight_scale","id"),&AStar::get_point_weight_scale); - ObjectTypeDB::bind_method(_MD("remove_point","id"),&AStar::remove_point); + ClassDB::bind_method(_MD("get_available_point_id"),&AStar::get_available_point_id); + ClassDB::bind_method(_MD("add_point","id","pos","weight_scale"),&AStar::add_point,DEFVAL(1.0)); + ClassDB::bind_method(_MD("get_point_pos","id"),&AStar::get_point_pos); + ClassDB::bind_method(_MD("get_point_weight_scale","id"),&AStar::get_point_weight_scale); + ClassDB::bind_method(_MD("remove_point","id"),&AStar::remove_point); - ObjectTypeDB::bind_method(_MD("connect_points","id","to_id"),&AStar::connect_points); - ObjectTypeDB::bind_method(_MD("disconnect_points","id","to_id"),&AStar::disconnect_points); - ObjectTypeDB::bind_method(_MD("are_points_connected","id","to_id"),&AStar::are_points_connected); + ClassDB::bind_method(_MD("connect_points","id","to_id"),&AStar::connect_points); + ClassDB::bind_method(_MD("disconnect_points","id","to_id"),&AStar::disconnect_points); + ClassDB::bind_method(_MD("are_points_connected","id","to_id"),&AStar::are_points_connected); - ObjectTypeDB::bind_method(_MD("clear"),&AStar::clear); + ClassDB::bind_method(_MD("clear"),&AStar::clear); - ObjectTypeDB::bind_method(_MD("get_closest_point","to_pos"),&AStar::get_closest_point); - ObjectTypeDB::bind_method(_MD("get_closest_pos_in_segment","to_pos"),&AStar::get_closest_pos_in_segment); + ClassDB::bind_method(_MD("get_closest_point","to_pos"),&AStar::get_closest_point); + ClassDB::bind_method(_MD("get_closest_pos_in_segment","to_pos"),&AStar::get_closest_pos_in_segment); - ObjectTypeDB::bind_method(_MD("get_point_path","from_id","to_id"),&AStar::get_point_path); - ObjectTypeDB::bind_method(_MD("get_id_path","from_id","to_id"),&AStar::get_id_path); + ClassDB::bind_method(_MD("get_point_path","from_id","to_id"),&AStar::get_point_path); + ClassDB::bind_method(_MD("get_id_path","from_id","to_id"),&AStar::get_id_path); } diff --git a/core/math/a_star.h b/core/math/a_star.h index a0517b5941..c4c955ed2d 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -1,12 +1,43 @@ +/*************************************************************************/ +/* a_star.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 ASTAR_H #define ASTAR_H #include "reference.h" #include "self_list.h" +/** + @author Juan Linietsky <reduzio@gmail.com> +*/ class AStar: public Reference { - OBJ_TYPE(AStar,Reference) + GDCLASS(AStar,Reference) uint64_t pass; @@ -17,14 +48,14 @@ class AStar: public Reference { int id; Vector3 pos; - float weight_scale; + real_t weight_scale; uint64_t last_pass; Vector<Point*> neighbours; //used for pathfinding Point *prev_point; - float distance; + real_t distance; Point() : list(this) {} }; @@ -67,9 +98,9 @@ public: int get_available_point_id() const; - void add_point(int p_id,const Vector3& p_pos,float p_weight_scale=1); + void add_point(int p_id,const Vector3& p_pos,real_t p_weight_scale=1); Vector3 get_point_pos(int p_id) const; - float get_point_weight_scale(int p_id) const; + real_t get_point_weight_scale(int p_id) const; void remove_point(int p_id); void connect_points(int p_id,int p_with_id); @@ -82,8 +113,8 @@ public: int get_closest_point(const Vector3& p_point) const; Vector3 get_closest_pos_in_segment(const Vector3& p_point) const; - DVector<Vector3> get_point_path(int p_from_id, int p_to_id); - DVector<int> get_id_path(int p_from_id, int p_to_id); + PoolVector<Vector3> get_point_path(int p_from_id, int p_to_id); + PoolVector<int> get_id_path(int p_from_id, int p_to_id); AStar(); ~AStar(); diff --git a/core/math/audio_frame.cpp b/core/math/audio_frame.cpp new file mode 100644 index 0000000000..566ba23992 --- /dev/null +++ b/core/math/audio_frame.cpp @@ -0,0 +1,2 @@ +#include "audio_frame.h" + diff --git a/core/math/audio_frame.h b/core/math/audio_frame.h new file mode 100644 index 0000000000..acd74903bb --- /dev/null +++ b/core/math/audio_frame.h @@ -0,0 +1,61 @@ +#ifndef AUDIOFRAME_H +#define AUDIOFRAME_H + +#include "typedefs.h" + + +static inline float undenormalise(volatile float f) +{ + union { + uint32_t i; + float f; + } v; + + v.f = f; + + // original: return (v.i & 0x7f800000) == 0 ? 0.0f : f; + // version from Tim Blechmann: + return (v.i & 0x7f800000) < 0x08000000 ? 0.0f : f; +} + + +struct AudioFrame { + + //left and right samples + float l,r; + + _ALWAYS_INLINE_ const float& operator[](int idx) const { return idx==0?l:r; } + _ALWAYS_INLINE_ float& operator[](int idx) { return idx==0?l:r; } + + _ALWAYS_INLINE_ AudioFrame operator+(const AudioFrame& p_frame) const { return AudioFrame(l+p_frame.l,r+p_frame.r); } + _ALWAYS_INLINE_ AudioFrame operator-(const AudioFrame& p_frame) const { return AudioFrame(l-p_frame.l,r-p_frame.r); } + _ALWAYS_INLINE_ AudioFrame operator*(const AudioFrame& p_frame) const { return AudioFrame(l*p_frame.l,r*p_frame.r); } + _ALWAYS_INLINE_ AudioFrame operator/(const AudioFrame& p_frame) const { return AudioFrame(l/p_frame.l,r/p_frame.r); } + + _ALWAYS_INLINE_ AudioFrame operator+(float p_sample) const { return AudioFrame(l+p_sample,r+p_sample); } + _ALWAYS_INLINE_ AudioFrame operator-(float p_sample) const { return AudioFrame(l-p_sample,r-p_sample); } + _ALWAYS_INLINE_ AudioFrame operator*(float p_sample) const { return AudioFrame(l*p_sample,r*p_sample); } + _ALWAYS_INLINE_ AudioFrame operator/(float p_sample) const { return AudioFrame(l/p_sample,r/p_sample); } + + _ALWAYS_INLINE_ void operator+=(const AudioFrame& p_frame) { l+=p_frame.l; r+=p_frame.r; } + _ALWAYS_INLINE_ void operator-=(const AudioFrame& p_frame) { l-=p_frame.l; r-=p_frame.r; } + _ALWAYS_INLINE_ void operator*=(const AudioFrame& p_frame) { l*=p_frame.l; r*=p_frame.r; } + _ALWAYS_INLINE_ void operator/=(const AudioFrame& p_frame) { l/=p_frame.l; r/=p_frame.r; } + + _ALWAYS_INLINE_ void operator+=(float p_sample) { l+=p_sample; r+=p_sample; } + _ALWAYS_INLINE_ void operator-=(float p_sample) { l-=p_sample; r-=p_sample; } + _ALWAYS_INLINE_ void operator*=(float p_sample) { l*=p_sample; r*=p_sample; } + _ALWAYS_INLINE_ void operator/=(float p_sample) { l/=p_sample; r/=p_sample; } + + _ALWAYS_INLINE_ void undenormalise() { + l = ::undenormalise(l); + r = ::undenormalise(r); + } + + _ALWAYS_INLINE_ AudioFrame(float p_l, float p_r) {l=p_l; r=p_r;} + _ALWAYS_INLINE_ AudioFrame(const AudioFrame& p_frame) {l=p_frame.l; r=p_frame.r;} + + _ALWAYS_INLINE_ AudioFrame() {} +}; + +#endif diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp index d16495217c..1ca6385032 100644 --- a/core/math/bsp_tree.cpp +++ b/core/math/bsp_tree.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,7 +31,7 @@ #include "print_string.h" -void BSP_Tree::from_aabb(const AABB& p_aabb) { +void BSP_Tree::from_aabb(const Rect3& p_aabb) { planes.clear(); @@ -67,7 +67,7 @@ Vector<Plane> BSP_Tree::get_planes() const { return planes; } -AABB BSP_Tree::get_aabb() const { +Rect3 BSP_Tree::get_aabb() const { return aabb; } @@ -87,8 +87,8 @@ int BSP_Tree::_get_points_inside(int p_node,const Vector3* p_points,int *p_indic max+=p_center; min+=p_center; - float dist_min = p.distance_to(min); - float dist_max = p.distance_to(max); + real_t dist_min = p.distance_to(min); + real_t dist_max = p.distance_to(max); if ((dist_min * dist_max) < CMP_EPSILON ) { //intersection, test point by point @@ -290,13 +290,13 @@ bool BSP_Tree::point_is_inside(const Vector3& p_point) const { } -static int _bsp_find_best_half_plane(const Face3* p_faces,const Vector<int>& p_indices,float p_tolerance) { +static int _bsp_find_best_half_plane(const Face3* p_faces,const Vector<int>& p_indices,real_t p_tolerance) { int ic = p_indices.size(); const int*indices=p_indices.ptr(); int best_plane = -1; - float best_plane_cost = 1e20; + real_t best_plane_cost = 1e20; // Loop to find the polygon that best divides the set. @@ -317,7 +317,7 @@ static int _bsp_find_best_half_plane(const Face3* p_faces,const Vector<int>& p_i for(int k=0;k<3;k++) { - float d = p.distance_to(g.vertex[j]); + real_t d = p.distance_to(g.vertex[j]); if (Math::abs(d)>p_tolerance) { @@ -340,13 +340,13 @@ static int _bsp_find_best_half_plane(const Face3* p_faces,const Vector<int>& p_i - //double split_cost = num_spanning / (double) face_count; - double relation = Math::abs(num_over-num_under) / (double) ic; + //real_t split_cost = num_spanning / (real_t) face_count; + real_t relation = Math::abs(num_over-num_under) / (real_t) ic; // being honest, i never found a way to add split cost to the mix in a meaninguful way // in this engine, also, will likely be ignored anyway - double plane_cost = /*split_cost +*/ relation; + real_t plane_cost = /*split_cost +*/ relation; //printf("plane %i, %i over, %i under, %i spanning, cost is %g\n",i,num_over,num_under,num_spanning,plane_cost); if (plane_cost<best_plane_cost) { @@ -362,7 +362,7 @@ static int _bsp_find_best_half_plane(const Face3* p_faces,const Vector<int>& p_i } -static int _bsp_create_node(const Face3 *p_faces,const Vector<int>& p_indices,Vector<Plane> &p_planes, Vector<BSP_Tree::Node> &p_nodes,float p_tolerance) { +static int _bsp_create_node(const Face3 *p_faces,const Vector<int>& p_indices,Vector<Plane> &p_planes, Vector<BSP_Tree::Node> &p_nodes,real_t p_tolerance) { ERR_FAIL_COND_V( p_nodes.size() == BSP_Tree::MAX_NODES, -1 ); @@ -390,15 +390,17 @@ static int _bsp_create_node(const Face3 *p_faces,const Vector<int>& p_indices,Ve const Face3& f=p_faces[ indices[i] ]; - //if (f.get_plane().is_almost_like(divisor_plane)) - // continue; + /* + if (f.get_plane().is_almost_like(divisor_plane)) + continue; + */ int over_count=0; int under_count=0; for(int j=0;j<3;j++) { - float d = divisor_plane.distance_to(f.vertex[j]); + real_t d = divisor_plane.distance_to(f.vertex[j]); if (Math::abs(d)>p_tolerance) { if (d > 0) @@ -471,7 +473,7 @@ BSP_Tree::operator Variant() const { Dictionary d; d["error_radius"]=error_radius; - Vector<float> plane_values; + Vector<real_t> plane_values; plane_values.resize(planes.size()*4); for(int i=0;i<planes.size();i++) { @@ -484,7 +486,7 @@ BSP_Tree::operator Variant() const { d["planes"]=plane_values; - DVector<int> dst_nodes; + PoolVector<int> dst_nodes; dst_nodes.resize(nodes.size()*3); for(int i=0;i<nodes.size();i++) { @@ -514,19 +516,19 @@ BSP_Tree::BSP_Tree(const Variant& p_variant) { ERR_FAIL_COND(!d.has("aabb")); ERR_FAIL_COND(!d.has("error_radius")); - DVector<int> src_nodes = d["nodes"]; + PoolVector<int> src_nodes = d["nodes"]; ERR_FAIL_COND(src_nodes.size()%3); - if (d["planes"].get_type()==Variant::REAL_ARRAY) { + if (d["planes"].get_type()==Variant::POOL_REAL_ARRAY) { - DVector<float> src_planes=d["planes"]; + PoolVector<real_t> src_planes=d["planes"]; int plane_count=src_planes.size(); ERR_FAIL_COND(plane_count%4); planes.resize(plane_count/4); if (plane_count) { - DVector<float>::Read r = src_planes.read(); + PoolVector<real_t>::Read r = src_planes.read(); for(int i=0;i<plane_count/4;i++) { planes[i].normal.x=r[i*4+0]; @@ -546,10 +548,10 @@ BSP_Tree::BSP_Tree(const Variant& p_variant) { error_radius = d["error"]; aabb = d["aabb"]; -// int node_count = src_nodes.size(); + //int node_count = src_nodes.size(); nodes.resize(src_nodes.size()/3); - DVector<int>::Read r = src_nodes.read(); + PoolVector<int>::Read r = src_nodes.read(); for(int i=0;i<nodes.size();i++) { @@ -560,12 +562,12 @@ BSP_Tree::BSP_Tree(const Variant& p_variant) { } -BSP_Tree::BSP_Tree(const DVector<Face3>& p_faces,float p_error_radius) { +BSP_Tree::BSP_Tree(const PoolVector<Face3>& p_faces,real_t p_error_radius) { // compute aabb int face_count=p_faces.size(); - DVector<Face3>::Read faces_r=p_faces.read(); + PoolVector<Face3>::Read faces_r=p_faces.read(); const Face3 *facesptr = faces_r.ptr(); @@ -613,7 +615,7 @@ BSP_Tree::BSP_Tree(const DVector<Face3>& p_faces,float p_error_radius) { error_radius=p_error_radius; } -BSP_Tree::BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const AABB& p_aabb,float p_error_radius) { +BSP_Tree::BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const Rect3& p_aabb,real_t p_error_radius) { nodes=p_nodes; planes=p_planes; diff --git a/core/math/bsp_tree.h b/core/math/bsp_tree.h index 6c36d80e3e..c0071438db 100644 --- a/core/math/bsp_tree.h +++ b/core/math/bsp_tree.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,7 +30,7 @@ #define BSP_TREE_H #include "plane.h" -#include "aabb.h" +#include "rect3.h" #include "face3.h" #include "vector.h" #include "dvector.h" @@ -65,8 +65,8 @@ private: Vector<Node> nodes; Vector<Plane> planes; - AABB aabb; - float error_radius; + Rect3 aabb; + real_t error_radius; int _get_points_inside(int p_node,const Vector3* p_points,int *p_indices, const Vector3& p_center,const Vector3& p_half_extents,int p_indices_count) const; @@ -78,7 +78,7 @@ public: bool is_empty() const { return nodes.size()==0; } Vector<Node> get_nodes() const; Vector<Plane> get_planes() const; - AABB get_aabb() const; + Rect3 get_aabb() const; bool point_is_inside(const Vector3& p_point) const; int get_points_inside(const Vector3* p_points, int p_point_count) const; @@ -87,12 +87,12 @@ public: operator Variant() const; - void from_aabb(const AABB& p_aabb); + void from_aabb(const Rect3& p_aabb); BSP_Tree(); BSP_Tree(const Variant& p_variant); - BSP_Tree(const DVector<Face3>& p_faces,float p_error_radius=0); - BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const AABB& p_aabb,float p_error_radius=0); + BSP_Tree(const PoolVector<Face3>& p_faces,real_t p_error_radius=0); + BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const Rect3& p_aabb,real_t p_error_radius=0); ~BSP_Tree(); }; @@ -110,7 +110,7 @@ bool BSP_Tree::_test_convex(const Node* p_nodes, const Plane* p_planes,int p_cur const Plane& p=p_planes[n.plane]; - float min,max; + real_t min,max; p_convex.project_range(p.normal,min,max); bool go_under = min < p.d; diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp index f7dd8839b8..3b47a75c65 100644 --- a/core/math/camera_matrix.cpp +++ b/core/math/camera_matrix.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -54,7 +54,7 @@ void CameraMatrix::set_zero() { } -Plane CameraMatrix::xform4(const Plane& p_vec4) { +Plane CameraMatrix::xform4(const Plane& p_vec4) const { Plane ret; @@ -65,15 +65,15 @@ Plane CameraMatrix::xform4(const Plane& p_vec4) { return ret; } -void CameraMatrix::set_perspective(float p_fovy_degrees, float p_aspect, float p_z_near, float p_z_far,bool p_flip_fov) { +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); } - float sine, cotangent, deltaZ; - float radians = p_fovy_degrees / 2.0 * Math_PI / 180.0; + 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); @@ -94,7 +94,7 @@ void CameraMatrix::set_perspective(float p_fovy_degrees, float p_aspect, float p } -void CameraMatrix::set_orthogonal(float p_left, float p_right, float p_bottom, float p_top, float p_znear, float p_zfar) { +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(); @@ -109,7 +109,7 @@ void CameraMatrix::set_orthogonal(float p_left, float p_right, float p_bottom, f } -void CameraMatrix::set_orthogonal(float p_size, float p_aspect, float p_znear, float p_zfar,bool p_flip_fov) { +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; @@ -120,7 +120,7 @@ void CameraMatrix::set_orthogonal(float p_size, float p_aspect, float p_znear, f -void CameraMatrix::set_frustum(float p_left, float p_right, float p_bottom, float p_top, float p_near, float p_far) { +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) { #if 0 ///@TODO, give a check to this. I'm not sure if it's working. set_identity(); @@ -134,19 +134,31 @@ void CameraMatrix::set_frustum(float p_left, float p_right, float p_bottom, floa matrix[3][2]=-1; matrix[3][3]=0; #else - float *te = &matrix[0][0]; - float x = 2 * p_near / ( p_right - p_left ); - float y = 2 * p_near / ( p_top - p_bottom ); - - float a = ( p_right + p_left ) / ( p_right - p_left ); - float b = ( p_top + p_bottom ) / ( p_top - p_bottom ); - float c = - ( p_far + p_near ) / ( p_far - p_near ); - float d = - 2 * p_far * p_near / ( p_far - p_near ); - - te[0] = x; te[4] = 0; te[8] = a; te[12] = 0; - te[1] = 0; te[5] = y; te[9] = b; te[13] = 0; - te[2] = 0; te[6] = 0; te[10] = c; te[14] = d; - te[3] = 0; te[7] = 0; te[11] = - 1; te[15] = 0; + 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; #endif @@ -154,9 +166,9 @@ void CameraMatrix::set_frustum(float p_left, float p_right, float p_bottom, floa -float CameraMatrix::get_z_far() const { +real_t CameraMatrix::get_z_far() const { - const float * matrix = (const float*)this->matrix; + 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], @@ -167,9 +179,9 @@ float CameraMatrix::get_z_far() const { return new_plane.d; } -float CameraMatrix::get_z_near() const { +real_t CameraMatrix::get_z_near() const { - const float * matrix = (const float*)this->matrix; + 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], @@ -179,25 +191,28 @@ float CameraMatrix::get_z_near() const { return new_plane.d; } -void CameraMatrix::get_viewport_size(float& r_width, float& r_height) const { +void CameraMatrix::get_viewport_size(real_t& r_width, real_t& r_height) const { - const float * matrix = (const float*)this->matrix; + 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]).normalized(); + -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]).normalized(); + - 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]).normalized(); + -matrix[15] + matrix[13]); + top_plane.normalize(); Vector3 res; near_plane.intersect_3(right_plane,top_plane,&res); @@ -208,32 +223,35 @@ void CameraMatrix::get_viewport_size(float& r_width, float& r_height) const { bool CameraMatrix::get_endpoints(const Transform& p_transform, Vector3 *p_8points) const { - const float * matrix = (const float*)this->matrix; + 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]).normalized(); + -matrix[15] - matrix[14]); + near_plane.normalize(); ///////--- Far Plane ---/////// Plane far_plane=Plane(matrix[ 2] - matrix[ 3], matrix[ 6] - matrix[ 7], matrix[10] - matrix[11], - matrix[15] - matrix[14]).normalized(); - + matrix[15] - matrix[14]); + far_plane.normalize(); ///////--- Right Plane ---/////// Plane right_plane=Plane(matrix[ 0] - matrix[ 3], matrix[ 4] - matrix[ 7], matrix[8] - matrix[ 11], - - matrix[15] + matrix[12]).normalized(); + - matrix[15] + matrix[12]); + right_plane.normalize(); ///////--- Top Plane ---/////// Plane top_plane=Plane(matrix[ 1] - matrix[ 3], matrix[ 5] - matrix[ 7], matrix[9] - matrix[ 11], - -matrix[15] + matrix[13]).normalized(); + -matrix[15] + matrix[13]); + top_plane.normalize(); Vector3 near_endpoint; Vector3 far_endpoint; @@ -266,7 +284,7 @@ Vector<Plane> CameraMatrix::get_projection_planes(const Transform& p_transform) Vector<Plane> planes; - const float * matrix = (const float*)this->matrix; + const real_t * matrix = (const real_t*)this->matrix; Plane new_plane; @@ -359,9 +377,9 @@ void CameraMatrix::invert() { int i,j,k; int pvt_i[4], pvt_j[4]; /* Locations of pivot matrix */ - float pvt_val; /* Value of current pivot element */ - float hold; /* Temporary storage */ - float determinat; /* Determinant */ + 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++) { @@ -474,7 +492,7 @@ CameraMatrix CameraMatrix::operator*(const CameraMatrix& p_matrix) const { void CameraMatrix::set_light_bias() { - float *m=&matrix[0][0]; + real_t *m=&matrix[0][0]; m[0]=0.5, m[1]=0.0, @@ -495,6 +513,28 @@ void CameraMatrix::set_light_bias() { } +void CameraMatrix::set_light_atlas_rect(const Rect2& p_rect) { + + real_t *m=&matrix[0][0]; + + m[0]=p_rect.size.width, + m[1]=0.0, + m[2]=0.0, + m[3]=0.0, + m[4]=0.0, + m[5]=p_rect.size.height, + m[6]=0.0, + m[7]=0.0, + m[8]=0.0, + m[9]=0.0, + m[10]=1.0, + m[11]=0.0, + m[12]=p_rect.pos.x, + m[13]=p_rect.pos.y, + m[14]=0.0, + m[15]=1.0; +} + CameraMatrix::operator String() const { String str; @@ -505,20 +545,30 @@ CameraMatrix::operator String() const { return str; } -float CameraMatrix::get_aspect() const { +real_t CameraMatrix::get_aspect() const { - float w,h; + real_t w,h; get_viewport_size(w,h); return w/h; } -float CameraMatrix::get_fov() const { - const float * matrix = (const float*)this->matrix; +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); + +} + +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]).normalized(); + - matrix[15] + matrix[12]); + right_plane.normalize(); return Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x)))*2.0; } @@ -533,7 +583,7 @@ void CameraMatrix::make_scale(const Vector3 &p_scale) { } -void CameraMatrix::scale_translate_to_fit(const AABB& p_aabb) { +void CameraMatrix::scale_translate_to_fit(const Rect3& p_aabb) { Vector3 min = p_aabb.pos; Vector3 max = p_aabb.pos+p_aabb.size; @@ -563,7 +613,7 @@ void CameraMatrix::scale_translate_to_fit(const AABB& p_aabb) { CameraMatrix::operator Transform() const { Transform tr; - const float *m=&matrix[0][0]; + const real_t *m=&matrix[0][0]; tr.basis.elements[0][0]=m[0]; tr.basis.elements[1][0]=m[1]; @@ -587,7 +637,7 @@ CameraMatrix::operator Transform() const { CameraMatrix::CameraMatrix(const Transform& p_transform) { const Transform &tr = p_transform; - float *m=&matrix[0][0]; + real_t *m=&matrix[0][0]; m[0]=tr.basis.elements[0][0]; m[1]=tr.basis.elements[1][0]; diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h index d192b1fef1..c96f8259b5 100644 --- a/core/math/camera_matrix.h +++ b/core/math/camera_matrix.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,6 +30,7 @@ #define CAMERA_MATRIX_H #include "transform.h" +#include "math_2d.h" /** @author Juan Linietsky <reduzio@gmail.com> */ @@ -47,44 +48,46 @@ struct CameraMatrix { PLANE_BOTTOM }; - float matrix[4][4]; + real_t matrix[4][4]; void set_identity(); void set_zero(); void set_light_bias(); - void set_perspective(float p_fovy_degrees, float p_aspect, float p_z_near, float p_z_far,bool p_flip_fov=false); - void set_orthogonal(float p_left, float p_right, float p_bottom, float p_top, float p_znear, float p_zfar); - void set_orthogonal(float p_size, float p_aspect, float p_znear, float p_zfar,bool p_flip_fov=false); - void set_frustum(float p_left, float p_right, float p_bottom, float p_top, float p_near, float p_far); + 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_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); - static float get_fovy(float p_fovx,float p_aspect) { + 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); } - float get_z_far() const; - float get_z_near() const; - float get_aspect() const; - float get_fov() const; + real_t get_z_far() const; + real_t get_z_near() const; + real_t get_aspect() const; + real_t get_fov() const; Vector<Plane> get_projection_planes(const Transform& p_transform) const; bool get_endpoints(const Transform& p_transform,Vector3 *p_8points) const; - void get_viewport_size(float& r_width, float& r_height) const; + void get_viewport_size(real_t& r_width, real_t& r_height) const; void invert(); CameraMatrix inverse() const; CameraMatrix operator*(const CameraMatrix& p_matrix) const; - Plane xform4(const Plane& p_vec4); + 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 scale_translate_to_fit(const Rect3& p_aabb); void make_scale(const Vector3 &p_scale); + int get_pixels_per_meter(int p_for_pixel_width) const; operator Transform() const; CameraMatrix(); @@ -99,7 +102,7 @@ Vector3 CameraMatrix::xform(const Vector3& p_vec3) const { 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]; - float w = matrix[0][3] * p_vec3.x + matrix[1][3] * p_vec3.y + matrix[2][3] * p_vec3.z + matrix[3][3]; + 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; } diff --git a/core/math/face3.cpp b/core/math/face3.cpp index e1af91f28e..60fab6748a 100644 --- a/core/math/face3.cpp +++ b/core/math/face3.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -168,8 +168,8 @@ Face3::Side Face3::get_side_of(const Face3& p_face,ClockDirection p_clock_dir) c Vector3 Face3::get_random_point_inside() const { - float a=Math::random(0,1); - float b=Math::random(0,1); + real_t a=Math::random(0,1); + real_t b=Math::random(0,1); if (a>b) { SWAP(a,b); } @@ -205,7 +205,7 @@ ClockDirection Face3::get_clock_dir() const { } -bool Face3::intersects_aabb(const AABB& p_aabb) const { +bool Face3::intersects_aabb(const Rect3& p_aabb) const { /** TEST PLANE **/ if (!p_aabb.intersects_plane( get_plane() )) @@ -215,9 +215,9 @@ bool Face3::intersects_aabb(const AABB& p_aabb) const { #define TEST_AXIS(m_ax)\ {\ - float aabb_min=p_aabb.pos.m_ax;\ - float aabb_max=p_aabb.pos.m_ax+p_aabb.size.m_ax;\ - float tri_min,tri_max;\ + real_t aabb_min=p_aabb.pos.m_ax;\ + real_t aabb_max=p_aabb.pos.m_ax+p_aabb.size.m_ax;\ + real_t tri_min,tri_max;\ for (int i=0;i<3;i++) {\ if (i==0 || vertex[i].m_ax > tri_max)\ tri_max=vertex[i].m_ax;\ @@ -255,7 +255,7 @@ bool Face3::intersects_aabb(const AABB& p_aabb) const { continue; // coplanar axis.normalize(); - float minA,maxA,minB,maxB; + real_t minA,maxA,minB,maxB; p_aabb.project_range_in_plane(Plane(axis,0),minA,maxA); project_range(axis,Transform(),minB,maxB); @@ -272,12 +272,12 @@ Face3::operator String() const { return String()+vertex[0]+", "+vertex[1]+", "+vertex[2]; } -void Face3::project_range(const Vector3& p_normal,const Transform& p_transform,float& r_min, float& r_max) const { +void Face3::project_range(const Vector3& p_normal,const Transform& p_transform,real_t& r_min, real_t& r_max) const { for (int i=0;i<3;i++) { Vector3 v=p_transform.xform(vertex[i]); - float d=p_normal.dot(v); + real_t d=p_normal.dot(v); if (i==0 || d > r_max) r_max=d; @@ -316,11 +316,11 @@ void Face3::get_support(const Vector3& p_normal,const Transform& p_transform,Vec /** FIND SUPPORT VERTEX **/ int vert_support_idx=-1; - float support_max; + real_t support_max; for (int i=0;i<3;i++) { - float d=n.dot(vertex[i]); + real_t d=n.dot(vertex[i]); if (i==0 || d > support_max) { support_max=d; @@ -336,7 +336,7 @@ void Face3::get_support(const Vector3& p_normal,const Transform& p_transform,Vec continue; // check if edge is valid as a support - float dot=(vertex[i]-vertex[(i+1)%3]).normalized().dot(n); + real_t dot=(vertex[i]-vertex[(i+1)%3]).normalized().dot(n); dot=ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { @@ -362,15 +362,15 @@ Vector3 Face3::get_closest_point_to(const Vector3& p_point) const { Vector3 edge1 = vertex[2] - vertex[0]; Vector3 v0 = vertex[0] - p_point; - float a = edge0.dot( edge0 ); - float b = edge0.dot( edge1 ); - float c = edge1.dot( edge1 ); - float d = edge0.dot( v0 ); - float e = edge1.dot( v0 ); + real_t a = edge0.dot( edge0 ); + real_t b = edge0.dot( edge1 ); + real_t c = edge1.dot( edge1 ); + real_t d = edge0.dot( v0 ); + real_t e = edge1.dot( v0 ); - float det = a*c - b*b; - float s = b*e - c*d; - float t = b*d - a*e; + real_t det = a*c - b*b; + real_t s = b*e - c*d; + real_t t = b*d - a*e; if ( s + t < det ) { @@ -402,7 +402,7 @@ Vector3 Face3::get_closest_point_to(const Vector3& p_point) const { } else { - float invDet = 1.f / det; + real_t invDet = 1.f / det; s *= invDet; t *= invDet; } @@ -411,12 +411,12 @@ Vector3 Face3::get_closest_point_to(const Vector3& p_point) const { { if ( s < 0.f ) { - float tmp0 = b+d; - float tmp1 = c+e; + real_t tmp0 = b+d; + real_t tmp1 = c+e; if ( tmp1 > tmp0 ) { - float numer = tmp1 - tmp0; - float denom = a-2*b+c; + real_t numer = tmp1 - tmp0; + real_t denom = a-2*b+c; s = CLAMP( numer/denom, 0.f, 1.f ); t = 1-s; } @@ -430,8 +430,8 @@ Vector3 Face3::get_closest_point_to(const Vector3& p_point) const { { if ( a+d > b+e ) { - float numer = c+e-b-d; - float denom = a-2*b+c; + real_t numer = c+e-b-d; + real_t denom = a-2*b+c; s = CLAMP( numer/denom, 0.f, 1.f ); t = 1-s; } @@ -443,8 +443,8 @@ Vector3 Face3::get_closest_point_to(const Vector3& p_point) const { } else { - float numer = c+e-b-d; - float denom = a-2*b+c; + real_t numer = c+e-b-d; + real_t denom = a-2*b+c; s = CLAMP( numer/denom, 0.f, 1.f ); t = 1.f - s; } diff --git a/core/math/face3.h b/core/math/face3.h index 3a81da74db..a0da588ea5 100644 --- a/core/math/face3.h +++ b/core/math/face3.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,7 +31,7 @@ #include "vector3.h" #include "plane.h" -#include "aabb.h" +#include "rect3.h" #include "transform.h" class Face3 { @@ -76,18 +76,18 @@ public: ClockDirection get_clock_dir() const; ///< todo, test if this is returning the proper clockwisity void get_support(const Vector3& p_normal,const Transform& p_transform,Vector3 *p_vertices,int* p_count,int p_max) const; - void project_range(const Vector3& p_normal,const Transform& p_transform,float& r_min, float& r_max) const; + void project_range(const Vector3& p_normal,const Transform& p_transform,real_t& r_min, real_t& r_max) const; - AABB get_aabb() const { + Rect3 get_aabb() const { - AABB aabb( vertex[0], Vector3() ); + Rect3 aabb( vertex[0], Vector3() ); aabb.expand_to( vertex[1] ); aabb.expand_to( vertex[2] ); return aabb; } - bool intersects_aabb(const AABB& p_aabb) const; - _FORCE_INLINE_ bool intersects_aabb2(const AABB& p_aabb) const; + bool intersects_aabb(const Rect3& p_aabb) const; + _FORCE_INLINE_ bool intersects_aabb2(const Rect3& p_aabb) const; operator String() const; inline Face3() {} @@ -96,7 +96,7 @@ public: }; -bool Face3::intersects_aabb2(const AABB& p_aabb) const { +bool Face3::intersects_aabb2(const Rect3& p_aabb) const { Vector3 perp = (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]); @@ -109,9 +109,9 @@ bool Face3::intersects_aabb2(const AABB& p_aabb) const { (perp.z>0) ? -half_extents.z : half_extents.z ); - float d = perp.dot(vertex[0]); - float dist_a = perp.dot(ofs+sup)-d; - float dist_b = perp.dot(ofs-sup)-d; + real_t d = perp.dot(vertex[0]); + real_t dist_a = perp.dot(ofs+sup)-d; + real_t dist_b = perp.dot(ofs-sup)-d; if (dist_a*dist_b > 0) return false; //does not intersect the plane @@ -119,9 +119,9 @@ bool Face3::intersects_aabb2(const AABB& p_aabb) const { #define TEST_AXIS(m_ax)\ {\ - float aabb_min=p_aabb.pos.m_ax;\ - float aabb_max=p_aabb.pos.m_ax+p_aabb.size.m_ax;\ - float tri_min,tri_max;\ + real_t aabb_min=p_aabb.pos.m_ax;\ + real_t aabb_max=p_aabb.pos.m_ax+p_aabb.size.m_ax;\ + real_t tri_min,tri_max;\ for (int i=0;i<3;i++) {\ if (i==0 || vertex[i].m_ax > tri_max)\ tri_max=vertex[i].m_ax;\ @@ -236,16 +236,16 @@ bool Face3::intersects_aabb2(const AABB& p_aabb) const { (axis.z>0) ? -half_extents.z : half_extents.z ); - float maxB = axis.dot(ofs+sup2); - float minB = axis.dot(ofs-sup2); + real_t maxB = axis.dot(ofs+sup2); + real_t minB = axis.dot(ofs-sup2); if (minB>maxB) { SWAP(maxB,minB); } - float minT=1e20,maxT=-1e20; + real_t minT=1e20,maxT=-1e20; for (int k=0;k<3;k++) { - float d=axis.dot(vertex[k]); + real_t d=axis.dot(vertex[k]); if (d > maxT) maxT=d; diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp index 790903eff5..6570dfe672 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -204,21 +204,21 @@ static bool _group_face(_FaceClassify *p_faces, int len, int p_index,int p_group } -DVector< DVector< Face3 > > Geometry::separate_objects( DVector< Face3 > p_array ) { +PoolVector< PoolVector< Face3 > > Geometry::separate_objects( PoolVector< Face3 > p_array ) { - DVector< DVector< Face3 > > objects; + PoolVector< PoolVector< Face3 > > objects; int len = p_array.size(); - DVector<Face3>::Read r=p_array.read(); + PoolVector<Face3>::Read r=p_array.read(); const Face3* arrayptr = r.ptr(); - DVector< _FaceClassify> fc; + PoolVector< _FaceClassify> fc; fc.resize( len ); - DVector< _FaceClassify >::Write fcw=fc.write(); + PoolVector< _FaceClassify >::Write fcw=fc.write(); _FaceClassify * _fcptr = fcw.ptr(); @@ -231,7 +231,7 @@ DVector< DVector< Face3 > > Geometry::separate_objects( DVector< Face3 > p_array if (error) { - ERR_FAIL_COND_V(error, DVector< DVector< Face3 > >() ); // invalid geometry + ERR_FAIL_COND_V(error, PoolVector< PoolVector< Face3 > >() ); // invalid geometry } /* group connected faces in separate objects */ @@ -257,8 +257,8 @@ DVector< DVector< Face3 > > Geometry::separate_objects( DVector< Face3 > p_array if (group>=0) { objects.resize(group); - DVector< DVector<Face3> >::Write obw=objects.write(); - DVector< Face3 > *group_faces = obw.ptr(); + PoolVector< PoolVector<Face3> >::Write obw=objects.write(); + PoolVector< Face3 > *group_faces = obw.ptr(); for (int i=0;i<len;i++) { if (!_fcptr[i].valid) @@ -304,7 +304,7 @@ enum _CellFlags { static inline void _plot_face(uint8_t*** p_cell_status,int x,int y,int z,int len_x,int len_y,int len_z,const Vector3& voxelsize,const Face3& p_face) { - AABB aabb( Vector3(x,y,z),Vector3(len_x,len_y,len_z)); + Rect3 aabb( Vector3(x,y,z),Vector3(len_x,len_y,len_z)); aabb.pos=aabb.pos*voxelsize; aabb.size=aabb.size*voxelsize; @@ -487,7 +487,7 @@ static inline void _mark_outside(uint8_t*** p_cell_status,int x,int y,int z,int } } -static inline void _build_faces(uint8_t*** p_cell_status,int x,int y,int z,int len_x,int len_y,int len_z,DVector<Face3>& p_faces) { +static inline void _build_faces(uint8_t*** p_cell_status,int x,int y,int z,int len_x,int len_y,int len_z,PoolVector<Face3>& p_faces) { ERR_FAIL_INDEX(x,len_x); ERR_FAIL_INDEX(y,len_y); @@ -580,16 +580,16 @@ static inline void _build_faces(uint8_t*** p_cell_status,int x,int y,int z,int l } -DVector< Face3 > Geometry::wrap_geometry( DVector< Face3 > p_array,float *p_error ) { +PoolVector< Face3 > Geometry::wrap_geometry( PoolVector< Face3 > p_array,real_t *p_error ) { #define _MIN_SIZE 1.0 #define _MAX_LENGTH 20 int face_count=p_array.size(); - DVector<Face3>::Read facesr=p_array.read(); + PoolVector<Face3>::Read facesr=p_array.read(); const Face3 *faces = facesr.ptr(); - AABB global_aabb; + Rect3 global_aabb; for(int i=0;i<face_count;i++) { @@ -696,7 +696,7 @@ DVector< Face3 > Geometry::wrap_geometry( DVector< Face3 > p_array,float *p_erro //print_line("Wrapper (3/6): Building Faces"); - DVector<Face3> wrapped_faces; + PoolVector<Face3> wrapped_faces; for (int i=0;i<div_x;i++) { @@ -714,7 +714,7 @@ DVector< Face3 > Geometry::wrap_geometry( DVector< Face3 > p_array,float *p_erro // transform face vertices to global coords int wrapped_faces_count=wrapped_faces.size(); - DVector<Face3>::Write wrapped_facesw=wrapped_faces.write(); + PoolVector<Face3>::Write wrapped_facesw=wrapped_faces.write(); Face3* wrapped_faces_ptr=wrapped_facesw.ptr(); for(int i=0;i<wrapped_faces_count;i++) { @@ -748,14 +748,14 @@ DVector< Face3 > Geometry::wrap_geometry( DVector< Face3 > p_array,float *p_erro return wrapped_faces; } -Geometry::MeshData Geometry::build_convex_mesh(const DVector<Plane> &p_planes) { +Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes) { MeshData mesh; #define SUBPLANE_SIZE 1024.0 - float subplane_size = 1024.0; // should compute this from the actual plane + real_t subplane_size = 1024.0; // should compute this from the actual plane for (int i=0;i<p_planes.size();i++) { Plane p =p_planes[i]; @@ -896,9 +896,9 @@ Geometry::MeshData Geometry::build_convex_mesh(const DVector<Plane> &p_planes) { } -DVector<Plane> Geometry::build_box_planes(const Vector3& p_extents) { +PoolVector<Plane> Geometry::build_box_planes(const Vector3& p_extents) { - DVector<Plane> planes; + PoolVector<Plane> planes; planes.push_back( Plane( Vector3(1,0,0), p_extents.x ) ); planes.push_back( Plane( Vector3(-1,0,0), p_extents.x ) ); @@ -910,9 +910,9 @@ DVector<Plane> Geometry::build_box_planes(const Vector3& p_extents) { return planes; } -DVector<Plane> Geometry::build_cylinder_planes(float p_radius, float p_height, int p_sides, Vector3::Axis p_axis) { +PoolVector<Plane> Geometry::build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis) { - DVector<Plane> planes; + PoolVector<Plane> planes; for (int i=0;i<p_sides;i++) { @@ -933,10 +933,10 @@ DVector<Plane> Geometry::build_cylinder_planes(float p_radius, float p_height, i } -DVector<Plane> Geometry::build_sphere_planes(float p_radius, int p_lats,int p_lons, Vector3::Axis p_axis) { +PoolVector<Plane> Geometry::build_sphere_planes(real_t p_radius, int p_lats,int p_lons, Vector3::Axis p_axis) { - DVector<Plane> planes; + PoolVector<Plane> planes; Vector3 axis; axis[p_axis]=1.0; @@ -957,7 +957,7 @@ DVector<Plane> Geometry::build_sphere_planes(float p_radius, int p_lats,int p_lo for (int j=1;j<=p_lats;j++) { //todo this is stupid, fix - Vector3 angle = normal.linear_interpolate(axis,j/(float)p_lats).normalized(); + Vector3 angle = normal.linear_interpolate(axis,j/(real_t)p_lats).normalized(); Vector3 pos = angle*p_radius; planes.push_back( Plane( pos, angle ) ); planes.push_back( Plane( pos * axis_neg, angle * axis_neg) ); @@ -969,9 +969,9 @@ DVector<Plane> Geometry::build_sphere_planes(float p_radius, int p_lats,int p_lo } -DVector<Plane> Geometry::build_capsule_planes(float p_radius, float p_height, int p_sides, int p_lats, Vector3::Axis p_axis) { +PoolVector<Plane> Geometry::build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis) { - DVector<Plane> planes; + PoolVector<Plane> planes; Vector3 axis; axis[p_axis]=1.0; @@ -991,7 +991,7 @@ DVector<Plane> Geometry::build_capsule_planes(float p_radius, float p_height, in for (int j=1;j<=p_lats;j++) { - Vector3 angle = normal.linear_interpolate(axis,j/(float)p_lats).normalized(); + Vector3 angle = normal.linear_interpolate(axis,j/(real_t)p_lats).normalized(); Vector3 pos = axis*p_height*0.5 + angle*p_radius; planes.push_back( Plane( pos, angle ) ); planes.push_back( Plane( pos * axis_neg, angle * axis_neg) ); @@ -1108,13 +1108,13 @@ void Geometry::make_atlas(const Vector<Size2i>& p_rects,Vector<Point2i>& r_resul //find the result with the best aspect ratio int best=-1; - float best_aspect=1e20; + real_t best_aspect=1e20; for(int i=0;i<results.size();i++) { - float h = nearest_power_of_2(results[i].max_h); - float w = nearest_power_of_2(results[i].max_w); - float aspect = h>w ? h/w : w/h; + real_t h = nearest_power_of_2(results[i].max_h); + real_t w = nearest_power_of_2(results[i].max_w); + real_t aspect = h>w ? h/w : w/h; if (aspect < best_aspect) { best=i; best_aspect=aspect; diff --git a/core/math/geometry.h b/core/math/geometry.h index b353423851..13cbbdce6f 100644 --- a/core/math/geometry.h +++ b/core/math/geometry.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -48,15 +48,15 @@ public: - static float get_closest_points_between_segments( const Vector2& p1,const Vector2& q1, const Vector2& p2,const Vector2& q2, Vector2& c1, Vector2& c2) { + static real_t get_closest_points_between_segments( const Vector2& p1,const Vector2& q1, const Vector2& p2,const Vector2& q2, Vector2& c1, Vector2& c2) { Vector2 d1 = q1 - p1; // Direction vector of segment S1 Vector2 d2 = q2 - p2; // Direction vector of segment S2 Vector2 r = p1 - p2; - float a = d1.dot(d1); // Squared length of segment S1, always nonnegative - float e = d2.dot(d2); // Squared length of segment S2, always nonnegative - float f = d2.dot(r); - float s,t; + real_t a = d1.dot(d1); // Squared length of segment S1, always nonnegative + real_t e = d2.dot(d2); // Squared length of segment S2, always nonnegative + real_t f = d2.dot(r); + real_t s,t; // Check if either or both segments degenerate into points if (a <= CMP_EPSILON && e <= CMP_EPSILON) { // Both segments degenerate into points @@ -66,25 +66,25 @@ public: } if (a <= CMP_EPSILON) { // First segment degenerates into a point - s = 0.0f; + s = 0.0; t = f / e; // s = 0 => t = (b*s + f) / e = f / e - t = CLAMP(t, 0.0f, 1.0f); + t = CLAMP(t, 0.0, 1.0); } else { - float c = d1.dot(r); + real_t c = d1.dot(r); if (e <= CMP_EPSILON) { // Second segment degenerates into a point - t = 0.0f; - s = CLAMP(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a + t = 0.0; + s = CLAMP(-c / a, 0.0, 1.0); // t = 0 => s = (b*t - c) / a = -c / a } else { // The general nondegenerate case starts here - float b = d1.dot(d2); - float denom = a*e-b*b; // Always nonnegative + real_t b = d1.dot(d2); + real_t denom = a*e-b*b; // Always nonnegative // If segments not parallel, compute closest point on L1 to L2 and // clamp to segment S1. Else pick arbitrary s (here 0) - if (denom != 0.0f) { - s = CLAMP((b*f - c*e) / denom, 0.0f, 1.0f); + if (denom != 0.0) { + s = CLAMP((b*f - c*e) / denom, 0.0, 1.0); } else - s = 0.0f; + s = 0.0; // Compute point on L2 closest to S1(s) using // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e t = (b*s + f) / e; @@ -92,12 +92,12 @@ public: //If t in [0,1] done. Else clamp t, recompute s for the new value // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a // and clamp s to [0, 1] - if (t < 0.0f) { - t = 0.0f; - s = CLAMP(-c / a, 0.0f, 1.0f); - } else if (t > 1.0f) { - t = 1.0f; - s = CLAMP((b - c) / a, 0.0f, 1.0f); + if (t < 0.0) { + t = 0.0; + s = CLAMP(-c / a, 0.0, 1.0); + } else if (t > 1.0) { + t = 1.0; + s = CLAMP((b - c) / a, 0.0, 1.0); } } } @@ -113,8 +113,8 @@ public: #define d_of(m,n,o,p) ( (m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z) ) //caluclate the parpametric position on the 2 curves, mua and mub - float mua = ( d_of(p1,q1,q2,q1) * d_of(q2,q1,p2,p1) - d_of(p1,q1,p2,p1) * d_of(q2,q1,q2,q1) ) / ( d_of(p2,p1,p2,p1) * d_of(q2,q1,q2,q1) - d_of(q2,q1,p2,p1) * d_of(q2,q1,p2,p1) ); - float mub = ( d_of(p1,q1,q2,q1) + mua * d_of(q2,q1,p2,p1) ) / d_of(q2,q1,q2,q1); + real_t mua = ( d_of(p1,q1,q2,q1) * d_of(q2,q1,p2,p1) - d_of(p1,q1,p2,p1) * d_of(q2,q1,q2,q1) ) / ( d_of(p2,p1,p2,p1) * d_of(q2,q1,q2,q1) - d_of(q2,q1,p2,p1) * d_of(q2,q1,p2,p1) ); + real_t mub = ( d_of(p1,q1,q2,q1) + mua * d_of(q2,q1,p2,p1) ) / d_of(q2,q1,q2,q1); //clip the value between [0..1] constraining the solution to lie on the original curves if (mua < 0) mua = 0; @@ -125,7 +125,7 @@ public: c2 = q1.linear_interpolate(q2,mub); } - static float get_closest_distance_between_segments( const Vector3& p_from_a,const Vector3& p_to_a, const Vector3& p_from_b,const Vector3& p_to_b) { + static real_t get_closest_distance_between_segments( const Vector3& p_from_a,const Vector3& p_to_a, const Vector3& p_from_b,const Vector3& p_to_b) { Vector3 u = p_to_a - p_from_a; Vector3 v = p_to_b - p_from_b; Vector3 w = p_from_a - p_to_a; @@ -273,22 +273,22 @@ public: Vector3 sphere_pos=p_sphere_pos-p_from; Vector3 rel=(p_to-p_from); - float rel_l=rel.length(); + real_t rel_l=rel.length(); if (rel_l<CMP_EPSILON) return false; // both points are the same Vector3 normal=rel/rel_l; - float sphere_d=normal.dot(sphere_pos); + real_t sphere_d=normal.dot(sphere_pos); //Vector3 ray_closest=normal*sphere_d; - float ray_distance=sphere_pos.distance_to(normal*sphere_d); + real_t ray_distance=sphere_pos.distance_to(normal*sphere_d); if (ray_distance>=p_sphere_radius) return false; - float inters_d2=p_sphere_radius*p_sphere_radius - ray_distance*ray_distance; - float inters_d=sphere_d; + real_t inters_d2=p_sphere_radius*p_sphere_radius - ray_distance*ray_distance; + real_t inters_d=sphere_d; if (inters_d2>=CMP_EPSILON) inters_d-=Math::sqrt(inters_d2); @@ -297,7 +297,7 @@ public: if (inters_d<0 || inters_d>rel_l) return false; - Vector3 result=p_from+normal*inters_d;; + Vector3 result=p_from+normal*inters_d; if (r_res) *r_res=result; @@ -307,17 +307,17 @@ public: return true; } - static inline bool segment_intersects_cylinder( const Vector3& p_from, const Vector3& p_to, float p_height,float p_radius,Vector3* r_res=0,Vector3 *r_norm=0) { + static inline bool segment_intersects_cylinder( const Vector3& p_from, const Vector3& p_to, real_t p_height,real_t p_radius,Vector3* r_res=0,Vector3 *r_norm=0) { Vector3 rel=(p_to-p_from); - float rel_l=rel.length(); + real_t rel_l=rel.length(); if (rel_l<CMP_EPSILON) return false; // both points are the same // first check if they are parallel Vector3 normal=(rel/rel_l); Vector3 crs = normal.cross(Vector3(0,0,1)); - float crs_l=crs.length(); + real_t crs_l=crs.length(); Vector3 z_dir; @@ -328,13 +328,13 @@ public: z_dir=crs/crs_l; } - float dist=z_dir.dot(p_from); + real_t dist=z_dir.dot(p_from); if (dist>=p_radius) return false; // too far away // convert to 2D - float w2=p_radius*p_radius-dist*dist; + real_t w2=p_radius*p_radius-dist*dist; if (w2<CMP_EPSILON) return false; //avoid numerical error Size2 size(Math::sqrt(w2),p_height*0.5); @@ -344,7 +344,7 @@ public: Vector2 from2D(x_dir.dot(p_from),p_from.z); Vector2 to2D(x_dir.dot(p_to),p_to.z); - float min=0,max=1; + real_t min=0,max=1; int axis=-1; @@ -464,12 +464,12 @@ public: Vector3 p=p_point-p_segment[0]; Vector3 n=p_segment[1]-p_segment[0]; - float l =n.length(); + real_t l =n.length(); if (l<1e-10) return p_segment[0]; // both points are the same, just give any n/=l; - float d=n.dot(p); + real_t d=n.dot(p); if (d<=0.0) return p_segment[0]; // before first point @@ -483,12 +483,12 @@ public: Vector3 p=p_point-p_segment[0]; Vector3 n=p_segment[1]-p_segment[0]; - float l =n.length(); + real_t l =n.length(); if (l<1e-10) return p_segment[0]; // both points are the same, just give any n/=l; - float d=n.dot(p); + real_t d=n.dot(p); return p_segment[0]+n*d; // inside } @@ -497,12 +497,12 @@ public: Vector2 p=p_point-p_segment[0]; Vector2 n=p_segment[1]-p_segment[0]; - float l =n.length(); + real_t l =n.length(); if (l<1e-10) return p_segment[0]; // both points are the same, just give any n/=l; - float d=n.dot(p); + real_t d=n.dot(p); if (d<=0.0) return p_segment[0]; // before first point @@ -529,12 +529,12 @@ public: Vector2 p=p_point-p_segment[0]; Vector2 n=p_segment[1]-p_segment[0]; - float l =n.length(); + real_t l =n.length(); if (l<1e-10) return p_segment[0]; // both points are the same, just give any n/=l; - float d=n.dot(p); + real_t d=n.dot(p); return p_segment[0]+n*d; // inside } @@ -555,7 +555,7 @@ public: if ((C.y<0 && D.y<0) || (C.y>=0 && D.y>=0)) return false; - float ABpos=D.x+(C.x-D.x)*D.y/(D.y-C.y); + real_t ABpos=D.x+(C.x-D.x)*D.y/(D.y-C.y); // Fail if segment C-D crosses line A-B outside of segment A-B. if (ABpos<0 || ABpos>1.0) @@ -595,7 +595,7 @@ public: static inline bool triangle_sphere_intersection_test(const Vector3 *p_triangle,const Vector3& p_normal,const Vector3& p_sphere_pos, real_t p_sphere_radius,Vector3& r_triangle_contact,Vector3& r_sphere_contact) { - float d=p_normal.dot(p_sphere_pos)-p_normal.dot(p_triangle[0]); + real_t d=p_normal.dot(p_sphere_pos)-p_normal.dot(p_triangle[0]); if (d > p_sphere_radius || d < -p_sphere_radius) // not touching the plane of the face, return return false; @@ -629,7 +629,7 @@ public: Vector3 axis =n1.cross(n2).cross(n1); axis.normalize(); // ugh - float ad=axis.dot(n2); + real_t ad=axis.dot(n2); if (ABS(ad)>p_sphere_radius) { // no chance with this edge, too far away @@ -639,7 +639,7 @@ public: // check point within edge capsule cylinder /** 4th TEST INSIDE EDGE POINTS **/ - float sphere_at = n1.dot(n2); + real_t sphere_at = n1.dot(n2); if (sphere_at>=0 && sphere_at<n1.dot(n1)) { @@ -650,7 +650,7 @@ public: return true; } - float r2=p_sphere_radius*p_sphere_radius; + real_t r2=p_sphere_radius*p_sphere_radius; if (n2.length_squared()<r2) { @@ -726,8 +726,8 @@ public: int outside_count = 0; for (int a = 0; a < polygon.size(); a++) { - //float p_plane.d = (*this) * polygon[a]; - float dist = p_plane.distance_to(polygon[a]); + //real_t p_plane.d = (*this) * polygon[a]; + real_t dist = p_plane.distance_to(polygon[a]); if (dist <-CMP_POINT_IN_PLANE_EPSILON) { location_cache[a] = LOC_INSIDE; inside_count++; @@ -750,9 +750,7 @@ public: return Vector<Vector3>(); //empty } -// long count = 0; long previous = polygon.size() - 1; - Vector<Vector3> clipped; for (int index = 0; index < polygon.size(); index++) { @@ -763,8 +761,8 @@ public: const Vector3& v2 = polygon[index]; Vector3 segment= v1 - v2; - double den=p_plane.normal.dot( segment ); - double dist=p_plane.distance_to( v1 ) / den; + real_t den=p_plane.normal.dot( segment ); + real_t dist=p_plane.distance_to( v1 ) / den; dist=-dist; clipped.push_back( v1 + segment * dist ); } @@ -773,8 +771,8 @@ public: if ((loc == LOC_INSIDE) && (location_cache[previous] == LOC_OUTSIDE)) { const Vector3& v2 = polygon[previous]; Vector3 segment= v1 - v2; - double den=p_plane.normal.dot( segment ); - double dist=p_plane.distance_to( v1 ) / den; + real_t den=p_plane.normal.dot( segment ); + real_t dist=p_plane.distance_to( v1 ) / den; dist=-dist; clipped.push_back( v1 + segment * dist ); } @@ -808,9 +806,9 @@ public: } - static DVector< DVector< Face3 > > separate_objects( DVector< Face3 > p_array ); + static PoolVector< PoolVector< Face3 > > separate_objects( PoolVector< Face3 > p_array ); - static DVector< Face3 > wrap_geometry( DVector< Face3 > p_array, float *p_error=NULL ); ///< create a "wrap" that encloses the given geometry + static PoolVector< Face3 > wrap_geometry( PoolVector< Face3 > p_array, real_t *p_error=NULL ); ///< create a "wrap" that encloses the given geometry struct MeshData { @@ -886,9 +884,9 @@ public: } - static double vec2_cross(const Point2 &O, const Point2 &A, const Point2 &B) + static real_t vec2_cross(const Point2 &O, const Point2 &A, const Point2 &B) { - return (double)(A.x - O.x) * (B.y - O.y) - (double)(A.y - O.y) * (B.x - O.x); + return (real_t)(A.x - O.x) * (B.y - O.y) - (real_t)(A.y - O.y) * (B.x - O.x); } // Returns a list of points on the convex hull in counter-clockwise order. @@ -919,11 +917,11 @@ public: return H; } - static MeshData build_convex_mesh(const DVector<Plane> &p_planes); - static DVector<Plane> build_sphere_planes(float p_radius, int p_lats, int p_lons, Vector3::Axis p_axis=Vector3::AXIS_Z); - static DVector<Plane> build_box_planes(const Vector3& p_extents); - static DVector<Plane> build_cylinder_planes(float p_radius, float p_height, int p_sides, Vector3::Axis p_axis=Vector3::AXIS_Z); - static DVector<Plane> build_capsule_planes(float p_radius, float p_height, int p_sides, int p_lats, Vector3::Axis p_axis=Vector3::AXIS_Z); + static MeshData build_convex_mesh(const PoolVector<Plane> &p_planes); + static PoolVector<Plane> build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis=Vector3::AXIS_Z); + static PoolVector<Plane> build_box_planes(const Vector3& p_extents); + static PoolVector<Plane> build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis=Vector3::AXIS_Z); + static PoolVector<Plane> build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis=Vector3::AXIS_Z); static void make_atlas(const Vector<Size2i>& p_rects,Vector<Point2i>& r_result, Size2i& r_size); diff --git a/core/math/math_2d.cpp b/core/math/math_2d.cpp index e616f05914..76eeece688 100644 --- a/core/math/math_2d.cpp +++ b/core/math/math_2d.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,22 +31,22 @@ real_t Vector2::angle() const { - return Math::atan2(x,y); + return Math::atan2(y,x); } -float Vector2::length() const { +real_t Vector2::length() const { return Math::sqrt( x*x + y*y ); } -float Vector2::length_squared() const { +real_t Vector2::length_squared() const { return x*x + y*y; } void Vector2::normalize() { - float l = x*x + y*y; + real_t l = x*x + y*y; if (l!=0) { l=Math::sqrt(l); @@ -62,32 +62,32 @@ Vector2 Vector2::normalized() const { return v; } -float Vector2::distance_to(const Vector2& p_vector2) const { +real_t Vector2::distance_to(const Vector2& p_vector2) const { return Math::sqrt( (x-p_vector2.x)*(x-p_vector2.x) + (y-p_vector2.y)*(y-p_vector2.y)); } -float Vector2::distance_squared_to(const Vector2& p_vector2) const { +real_t Vector2::distance_squared_to(const Vector2& p_vector2) const { return (x-p_vector2.x)*(x-p_vector2.x) + (y-p_vector2.y)*(y-p_vector2.y); } -float Vector2::angle_to(const Vector2& p_vector2) const { +real_t Vector2::angle_to(const Vector2& p_vector2) const { - return Math::atan2( tangent().dot(p_vector2), dot(p_vector2) ); + return Math::atan2( cross(p_vector2), dot(p_vector2) ); } -float Vector2::angle_to_point(const Vector2& p_vector2) const { +real_t Vector2::angle_to_point(const Vector2& p_vector2) const { - return Math::atan2( x-p_vector2.x, y - p_vector2.y ); + return Math::atan2( y - p_vector2.y, x-p_vector2.x ); } -float Vector2::dot(const Vector2& p_other) const { +real_t Vector2::dot(const Vector2& p_other) const { return x*p_other.x + y*p_other.y; } -float Vector2::cross(const Vector2& p_other) const { +real_t Vector2::cross(const Vector2& p_other) const { return x*p_other.y - y*p_other.x; } @@ -120,11 +120,11 @@ Vector2 Vector2::operator*(const Vector2 &p_v1) const { return Vector2(x * p_v1.x, y * p_v1.y); }; -Vector2 Vector2::operator*(const float &rvalue) const { +Vector2 Vector2::operator*(const real_t &rvalue) const { return Vector2(x * rvalue, y * rvalue); }; -void Vector2::operator*=(const float &rvalue) { +void Vector2::operator*=(const real_t &rvalue) { x *= rvalue; y *= rvalue; }; @@ -134,12 +134,12 @@ Vector2 Vector2::operator/(const Vector2 &p_v1) const { return Vector2(x / p_v1.x, y / p_v1.y); }; -Vector2 Vector2::operator/(const float &rvalue) const { +Vector2 Vector2::operator/(const real_t &rvalue) const { return Vector2(x / rvalue, y / rvalue); }; -void Vector2::operator/=(const float &rvalue) { +void Vector2::operator/=(const real_t &rvalue) { x /= rvalue; y /= rvalue; }; @@ -162,7 +162,7 @@ Vector2 Vector2::floor() const { return Vector2( Math::floor(x), Math::floor(y) ); } -Vector2 Vector2::rotated(float p_by) const { +Vector2 Vector2::rotated(real_t p_by) const { Vector2 v; v.set_rotation(angle()+p_by); @@ -198,7 +198,7 @@ Vector2 Vector2::clamped(real_t p_len) const { return v; } -Vector2 Vector2::cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const { +Vector2 Vector2::cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,real_t p_t) const { #if 0 k[0] = ((*this) (vi[0] + 1, vi[1], vi[2])) - ((*this) (vi[0], vi[1],vi[2])); //fk = a0 @@ -219,13 +219,13 @@ Vector2 Vector2::cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_ //dk = (fk+1 - fk-1)*0.5 //Dk = (fk+1 - fk) - float dk = + real_t dk = #endif return Vector2(); } -Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const { +Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,real_t p_t) const { @@ -234,20 +234,20 @@ Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, co Vector2 p2=p_b; Vector2 p3=p_post_b; - float t = p_t; - float t2 = t * t; - float t3 = t2 * t; + real_t t = p_t; + real_t t2 = t * t; + real_t t3 = t2 * t; Vector2 out; - out = 0.5f * ( ( p1 * 2.0f) + + out = 0.5 * ( ( p1 * 2.0) + ( -p0 + p2 ) * t + - ( 2.0f * p0 - 5.0f * p1 + 4 * p2 - p3 ) * t2 + - ( -p0 + 3.0f * p1 - 3.0f * p2 + p3 ) * t3 ); + ( 2.0 * p0 - 5.0 * p1 + 4 * p2 - p3 ) * t2 + + ( -p0 + 3.0 * p1 - 3.0 * p2 + p3 ) * t3 ); return out; /* - float mu = p_t; - float mu2 = mu*mu; + real_t mu = p_t; + real_t mu2 = mu*mu; Vector2 a0 = p_post_b - p_b - p_pre_a + *this; Vector2 a1 = p_pre_a - *this - a0; @@ -257,7 +257,7 @@ Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, co return ( a0*mu*mu2 + a1*mu2 + a2*mu + a3 ); */ /* - float t = p_t; + real_t t = p_t; real_t t2 = t*t; real_t t3 = t2*t; @@ -291,7 +291,7 @@ bool Rect2::intersects_segment(const Point2& p_from, const Point2& p_to, Point2* real_t min=0,max=1; int axis=0; - float sign=0; + real_t sign=0; for(int i=0;i<2;i++) { real_t seg_from=p_from[i]; @@ -299,7 +299,7 @@ bool Rect2::intersects_segment(const Point2& p_from, const Point2& p_to, Point2* real_t box_begin=pos[i]; real_t box_end=box_begin+size[i]; real_t cmin,cmax; - float csign; + real_t csign; if (seg_from < seg_to) { @@ -408,25 +408,26 @@ bool Point2i::operator!=(const Point2i& p_vec2) const { return x!=p_vec2.x || y!=p_vec2.y; } -void Matrix32::invert() { - +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]); } -Matrix32 Matrix32::inverse() const { +Transform2D Transform2D::inverse() const { - Matrix32 inv=*this; + Transform2D inv=*this; inv.invert(); return inv; } -void Matrix32::affine_invert() { +void Transform2D::affine_invert() { - float det = basis_determinant(); + real_t det = basis_determinant(); ERR_FAIL_COND(det==0); - float idet = 1.0 / det; + real_t idet = 1.0 / det; SWAP( elements[0][0],elements[1][1] ); elements[0]*=Vector2(idet,-idet); @@ -436,72 +437,74 @@ void Matrix32::affine_invert() { } -Matrix32 Matrix32::affine_inverse() const { +Transform2D Transform2D::affine_inverse() const { - Matrix32 inv=*this; + Transform2D inv=*this; inv.affine_invert(); return inv; } -void Matrix32::rotate(real_t p_phi) { - - Matrix32 rot(p_phi,Vector2()); - *this *= rot; +void Transform2D::rotate(real_t p_phi) { + *this = Transform2D(p_phi,Vector2()) * (*this); } -real_t Matrix32::get_rotation() const { - - return Math::atan2(elements[1].x,elements[1].y); +real_t Transform2D::get_rotation() const { + real_t det = basis_determinant(); + Transform2D m = orthonormalized(); + if (det < 0) { + m.scale_basis(Size2(-1,-1)); + } + return Math::atan2(m[0].y,m[0].x); } -void Matrix32::set_rotation(real_t p_rot) { +void Transform2D::set_rotation(real_t p_rot) { 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[0][1]=-sr; - elements[1][0]=sr; } -Matrix32::Matrix32(real_t p_rot, const Vector2& p_pos) { +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[0][1]=-sr; - elements[1][0]=sr; elements[2]=p_pos; } -Size2 Matrix32::get_scale() const { - - return Size2( elements[0].length(), elements[1].length() ); +Size2 Transform2D::get_scale() const { + real_t det_sign = basis_determinant() > 0 ? 1 : -1; + return det_sign * Size2( elements[0].length(), elements[1].length() ); } -void Matrix32::scale(const Size2& p_scale) { - - elements[0]*=p_scale; - elements[1]*=p_scale; +void Transform2D::scale(const Size2& p_scale) { + scale_basis(p_scale); elements[2]*=p_scale; } -void Matrix32::scale_basis(const Size2& p_scale) { +void Transform2D::scale_basis(const Size2& p_scale) { - elements[0]*=p_scale; - elements[1]*=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 Matrix32::translate( real_t p_tx, real_t p_ty) { +void Transform2D::translate( real_t p_tx, real_t p_ty) { translate(Vector2(p_tx,p_ty)); } -void Matrix32::translate( const Vector2& p_translation ) { +void Transform2D::translate( const Vector2& p_translation ) { elements[2]+=basis_xform(p_translation); } -void Matrix32::orthonormalize() { +void Transform2D::orthonormalize() { // Gram-Schmidt Process @@ -515,15 +518,15 @@ void Matrix32::orthonormalize() { elements[0]=x; elements[1]=y; } -Matrix32 Matrix32::orthonormalized() const { +Transform2D Transform2D::orthonormalized() const { - Matrix32 on=*this; + Transform2D on=*this; on.orthonormalize(); return on; } -bool Matrix32::operator==(const Matrix32& p_transform) const { +bool Transform2D::operator==(const Transform2D& p_transform) const { for(int i=0;i<3;i++) { if (elements[i]!=p_transform.elements[i]) @@ -533,7 +536,7 @@ bool Matrix32::operator==(const Matrix32& p_transform) const { return true; } -bool Matrix32::operator!=(const Matrix32& p_transform) const { +bool Transform2D::operator!=(const Transform2D& p_transform) const { for(int i=0;i<3;i++) { if (elements[i]!=p_transform.elements[i]) @@ -544,11 +547,11 @@ bool Matrix32::operator!=(const Matrix32& p_transform) const { } -void Matrix32::operator*=(const Matrix32& p_transform) { +void Transform2D::operator*=(const Transform2D& p_transform) { elements[2] = xform(p_transform.elements[2]); - float x0,x1,y0,y1; + real_t x0,x1,y0,y1; x0 = tdotx(p_transform.elements[0]); x1 = tdoty(p_transform.elements[0]); @@ -562,59 +565,59 @@ void Matrix32::operator*=(const Matrix32& p_transform) { } -Matrix32 Matrix32::operator*(const Matrix32& p_transform) const { +Transform2D Transform2D::operator*(const Transform2D& p_transform) const { - Matrix32 t = *this; + Transform2D t = *this; t*=p_transform; return t; } -Matrix32 Matrix32::scaled(const Size2& p_scale) const { +Transform2D Transform2D::scaled(const Size2& p_scale) const { - Matrix32 copy=*this; + Transform2D copy=*this; copy.scale(p_scale); return copy; } -Matrix32 Matrix32::basis_scaled(const Size2& p_scale) const { +Transform2D Transform2D::basis_scaled(const Size2& p_scale) const { - Matrix32 copy=*this; + Transform2D copy=*this; copy.scale_basis(p_scale); return copy; } -Matrix32 Matrix32::untranslated() const { +Transform2D Transform2D::untranslated() const { - Matrix32 copy=*this; + Transform2D copy=*this; copy.elements[2]=Vector2(); return copy; } -Matrix32 Matrix32::translated(const Vector2& p_offset) const { +Transform2D Transform2D::translated(const Vector2& p_offset) const { - Matrix32 copy=*this; + Transform2D copy=*this; copy.translate(p_offset); return copy; } -Matrix32 Matrix32::rotated(float p_phi) const { +Transform2D Transform2D::rotated(real_t p_phi) const { - Matrix32 copy=*this; + Transform2D copy=*this; copy.rotate(p_phi); return copy; } -float Matrix32::basis_determinant() const { +real_t Transform2D::basis_determinant() const { return elements[0].x * elements[1].y - elements[0].y * elements[1].x; } -Matrix32 Matrix32::interpolate_with(const Matrix32& p_transform, float p_c) const { +Transform2D Transform2D::interpolate_with(const Transform2D& p_transform, real_t p_c) const { //extract parameters Vector2 p1 = get_origin(); @@ -645,12 +648,12 @@ Matrix32 Matrix32::interpolate_with(const Matrix32& p_transform, float p_c) cons } //construct matrix - Matrix32 res(Math::atan2(v.y, v.x), Vector2::linear_interpolate(p1, p2, p_c)); + Transform2D res(Math::atan2(v.y, v.x), Vector2::linear_interpolate(p1, p2, p_c)); res.scale_basis(Vector2::linear_interpolate(s1, s2, p_c)); return res; } -Matrix32::operator String() const { +Transform2D::operator String() const { return String(String()+elements[0]+", "+elements[1]+", "+elements[2]); } diff --git a/core/math/math_2d.h b/core/math/math_2d.h index 38c1ac9656..a24c4266ee 100644 --- a/core/math/math_2d.h +++ b/core/math/math_2d.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -65,35 +65,35 @@ enum VAlign { struct Vector2 { union { - float x; - float width; + real_t x; + real_t width; }; union { - float y; - float height; + real_t y; + real_t height; }; - _FORCE_INLINE_ float& operator[](int p_idx) { + _FORCE_INLINE_ real_t& operator[](int p_idx) { return p_idx?y:x; } - _FORCE_INLINE_ const float& operator[](int p_idx) const { + _FORCE_INLINE_ const real_t& operator[](int p_idx) const { return p_idx?y:x; } void normalize(); Vector2 normalized() const; - float length() const; - float length_squared() const; + real_t length() const; + real_t length_squared() const; - float distance_to(const Vector2& p_vector2) const; - float distance_squared_to(const Vector2& p_vector2) const; - float angle_to(const Vector2& p_vector2) const; - float angle_to_point(const Vector2& p_vector2) const; + real_t distance_to(const Vector2& p_vector2) const; + real_t distance_squared_to(const Vector2& p_vector2) const; + real_t angle_to(const Vector2& p_vector2) const; + real_t angle_to_point(const Vector2& p_vector2) const; - float dot(const Vector2& p_other) const; - float cross(const Vector2& p_other) const; + real_t dot(const Vector2& p_other) const; + real_t cross(const Vector2& p_other) const; Vector2 cross(real_t p_other) const; Vector2 project(const Vector2& p_vec) const; @@ -101,10 +101,10 @@ struct Vector2 { Vector2 clamped(real_t p_len) const; - _FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2& p_a, const Vector2& p_b,float p_t); - _FORCE_INLINE_ Vector2 linear_interpolate(const Vector2& p_b,float p_t) const; - Vector2 cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const; - Vector2 cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const; + _FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2& p_a, const Vector2& p_b,real_t p_t); + _FORCE_INLINE_ Vector2 linear_interpolate(const Vector2& p_b,real_t p_t) const; + Vector2 cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,real_t p_t) const; + Vector2 cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,real_t p_t) const; Vector2 slide(const Vector2& p_vec) const; Vector2 reflect(const Vector2& p_vec) const; @@ -115,15 +115,15 @@ struct Vector2 { void operator-=(const Vector2& p_v); Vector2 operator*(const Vector2 &p_v1) const; - Vector2 operator*(const float &rvalue) const; - void operator*=(const float &rvalue); + Vector2 operator*(const real_t &rvalue) const; + void operator*=(const real_t &rvalue); void operator*=(const Vector2 &rvalue) { *this = *this * rvalue; } Vector2 operator/(const Vector2 &p_v1) const; - Vector2 operator/(const float &rvalue) const; + Vector2 operator/(const real_t &rvalue) const; - void operator/=(const float &rvalue); + void operator/=(const real_t &rvalue); Vector2 operator-() const; @@ -135,10 +135,10 @@ struct Vector2 { real_t angle() const; - void set_rotation(float p_radians) { + void set_rotation(real_t p_radians) { - x=Math::sin(p_radians); - y=Math::cos(p_radians); + x=Math::cos(p_radians); + y=Math::sin(p_radians); } _FORCE_INLINE_ Vector2 abs() const { @@ -146,7 +146,7 @@ struct Vector2 { return Vector2( Math::abs(x), Math::abs(y) ); } - Vector2 rotated(float p_by) const; + Vector2 rotated(real_t p_by) const; Vector2 tangent() const { return Vector2(y,-x); @@ -154,12 +154,12 @@ struct Vector2 { Vector2 floor() const; Vector2 snapped(const Vector2& p_by) const; - float get_aspect() const { return width/height; } + real_t aspect() const { return width/height; } operator String() const { return String::num(x)+", "+String::num(y); } - _FORCE_INLINE_ Vector2(float p_x,float p_y) { x=p_x; y=p_y; } + _FORCE_INLINE_ Vector2(real_t p_x,real_t p_y) { x=p_x; y=p_y; } _FORCE_INLINE_ Vector2() { x=0; y=0; } }; @@ -169,12 +169,12 @@ _FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2& p_vec) } -_FORCE_INLINE_ Vector2 operator*(float p_scalar, const Vector2& p_vec) { +_FORCE_INLINE_ Vector2 operator*(real_t p_scalar, const Vector2& p_vec) { return p_vec*p_scalar; } -Vector2 Vector2::linear_interpolate(const Vector2& p_b,float p_t) const { +Vector2 Vector2::linear_interpolate(const Vector2& p_b,real_t p_t) const { Vector2 res=*this; @@ -185,7 +185,7 @@ Vector2 Vector2::linear_interpolate(const Vector2& p_b,float p_t) const { } -Vector2 Vector2::linear_interpolate(const Vector2& p_a, const Vector2& p_b,float p_t) { +Vector2 Vector2::linear_interpolate(const Vector2& p_a, const Vector2& p_b,real_t p_t) { Vector2 res=p_a; @@ -198,7 +198,7 @@ Vector2 Vector2::linear_interpolate(const Vector2& p_a, const Vector2& p_b,float typedef Vector2 Size2; typedef Vector2 Point2; -struct Matrix32; +struct Transform2D; struct Rect2 { @@ -211,7 +211,7 @@ struct Rect2 { const Vector2& get_size() const { return size; } void set_size(const Vector2& p_size) { size=p_size; } - float get_area() const { return size.width*size.height; } + real_t get_area() const { return size.width*size.height; } inline bool intersects(const Rect2& p_rect) const { if ( pos.x >= (p_rect.pos.x + p_rect.size.width) ) @@ -226,9 +226,9 @@ struct Rect2 { return true; } - inline float distance_to(const Vector2& p_point) const { + inline real_t distance_to(const Vector2& p_point) const { - float dist = 1e20; + real_t dist = 1e20; if (p_point.x < pos.x) { dist=MIN(dist,pos.x-p_point.x); @@ -249,7 +249,7 @@ struct Rect2 { return dist; } - _FORCE_INLINE_ bool intersects_transformed(const Matrix32& p_xform, const Rect2& p_rect) const; + _FORCE_INLINE_ bool intersects_transformed(const Transform2D& p_xform, const Rect2& p_rect) const; bool intersects_segment(const Point2& p_from, const Point2& p_to, Point2* r_pos=NULL, Point2* r_normal=NULL) const; @@ -359,7 +359,7 @@ struct Rect2 { operator String() const { return String(pos)+", "+String(size); } Rect2() {} - Rect2( float p_x, float p_y, float p_width, float p_height) { pos=Point2(p_x,p_y); size=Size2( p_width, p_height ); } + Rect2( real_t p_x, real_t p_y, real_t p_width, real_t p_height) { pos=Point2(p_x,p_y); size=Size2( p_width, p_height ); } Rect2( const Point2& p_pos, const Size2& p_size ) { pos=p_pos; size=p_size; } }; @@ -407,7 +407,7 @@ struct Point2i { bool operator==(const Point2i& p_vec2) const; bool operator!=(const Point2i& p_vec2) const; - float get_aspect() const { return width/(float)height; } + real_t get_aspect() const { return width/(real_t)height; } operator String() const { return String::num(x)+", "+String::num(y); } @@ -551,12 +551,22 @@ struct Rect2i { -struct Matrix32 { +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_ float tdotx(const Vector2& v) const { return elements[0][0] * v.x + elements[1][0] * v.y; } - _FORCE_INLINE_ float tdoty(const Vector2& v) const { return elements[0][1] * v.x + elements[1][1] * v.y; } + _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]; } @@ -565,10 +575,10 @@ struct Matrix32 { _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(); - Matrix32 inverse() const; + Transform2D inverse() const; void affine_invert(); - Matrix32 affine_inverse() const; + Transform2D affine_inverse() const; void set_rotation(real_t p_phi); real_t get_rotation() const; @@ -580,30 +590,30 @@ struct Matrix32 { void translate( real_t p_tx, real_t p_ty); void translate( const Vector2& p_translation ); - float basis_determinant() const; + real_t basis_determinant() const; Size2 get_scale() const; _FORCE_INLINE_ const Vector2& get_origin() const { return elements[2]; } _FORCE_INLINE_ void set_origin(const Vector2& p_origin) { elements[2]=p_origin; } - Matrix32 scaled(const Size2& p_scale) const; - Matrix32 basis_scaled(const Size2& p_scale) const; - Matrix32 translated(const Vector2& p_offset) const; - Matrix32 rotated(float p_phi) const; + Transform2D scaled(const Size2& p_scale) const; + Transform2D basis_scaled(const Size2& p_scale) const; + Transform2D translated(const Vector2& p_offset) const; + Transform2D rotated(real_t p_phi) const; - Matrix32 untranslated() const; + Transform2D untranslated() const; void orthonormalize(); - Matrix32 orthonormalized() const; + Transform2D orthonormalized() const; - bool operator==(const Matrix32& p_transform) const; - bool operator!=(const Matrix32& p_transform) const; + bool operator==(const Transform2D& p_transform) const; + bool operator!=(const Transform2D& p_transform) const; - void operator*=(const Matrix32& p_transform); - Matrix32 operator*(const Matrix32& p_transform) const; + void operator*=(const Transform2D& p_transform); + Transform2D operator*(const Transform2D& p_transform) const; - Matrix32 interpolate_with(const Matrix32& p_transform, float p_c) 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; @@ -614,7 +624,7 @@ struct Matrix32 { operator String() const; - Matrix32(real_t xx, real_t xy, real_t yx, real_t yy, real_t ox, real_t oy) { + 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; @@ -624,11 +634,11 @@ struct Matrix32 { elements[2][1] = oy; } - Matrix32(real_t p_rot, const Vector2& p_pos); - Matrix32() { elements[0][0]=1.0; elements[1][1]=1.0; } + Transform2D(real_t p_rot, const Vector2& p_pos); + Transform2D() { elements[0][0]=1.0; elements[1][1]=1.0; } }; -bool Rect2::intersects_transformed(const Matrix32& p_xform, const Rect2& p_rect) const { +bool Rect2::intersects_transformed(const Transform2D& p_xform, const Rect2& p_rect) const { //SAT intersection between local and transformed rect2 @@ -783,7 +793,7 @@ bool Rect2::intersects_transformed(const Matrix32& p_xform, const Rect2& p_rect) } -Vector2 Matrix32::basis_xform(const Vector2& v) const { +Vector2 Transform2D::basis_xform(const Vector2& v) const { return Vector2( tdotx(v), @@ -791,7 +801,7 @@ Vector2 Matrix32::basis_xform(const Vector2& v) const { ); } -Vector2 Matrix32::basis_xform_inv(const Vector2& v) const{ +Vector2 Transform2D::basis_xform_inv(const Vector2& v) const{ return Vector2( elements[0].dot(v), @@ -799,14 +809,14 @@ Vector2 Matrix32::basis_xform_inv(const Vector2& v) const{ ); } -Vector2 Matrix32::xform(const Vector2& v) const { +Vector2 Transform2D::xform(const Vector2& v) const { return Vector2( tdotx(v), tdoty(v) ) + elements[2]; } -Vector2 Matrix32::xform_inv(const Vector2& p_vec) const { +Vector2 Transform2D::xform_inv(const Vector2& p_vec) const { Vector2 v = p_vec - elements[2]; @@ -816,7 +826,7 @@ Vector2 Matrix32::xform_inv(const Vector2& p_vec) const { ); } -Rect2 Matrix32::xform(const Rect2& p_rect) const { +Rect2 Transform2D::xform(const Rect2& p_rect) const { Vector2 x=elements[0]*p_rect.size.x; Vector2 y=elements[1]*p_rect.size.y; @@ -830,16 +840,16 @@ Rect2 Matrix32::xform(const Rect2& p_rect) const { return new_rect; } -void Matrix32::set_rotation_and_scale(real_t p_rot,const Size2& p_scale) { +void Transform2D::set_rotation_and_scale(real_t p_rot,const Size2& p_scale) { elements[0][0]=Math::cos(p_rot)*p_scale.x; elements[1][1]=Math::cos(p_rot)*p_scale.y; - elements[0][1]=-Math::sin(p_rot)*p_scale.x; - elements[1][0]=Math::sin(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 Matrix32::xform_inv(const Rect2& p_rect) const { +Rect2 Transform2D::xform_inv(const Rect2& p_rect) const { Vector2 ends[4]={ xform_inv( p_rect.pos ), diff --git a/core/math/math_defs.h b/core/math/math_defs.h index e6a56c5e45..feaff38a44 100644 --- a/core/math/math_defs.h +++ b/core/math/math_defs.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp index 46c0218707..c730b4fa30 100644 --- a/core/math/math_funcs.cpp +++ b/core/math/math_funcs.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -27,12 +27,9 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "math_funcs.h" - #include "core/os/os.h" -#include <math.h> -#include "float.h" -uint32_t Math::default_seed=1; +pcg32_random_t Math::default_pcg = {1, PCG_DEFAULT_INC_64}; #define PHI 0x9e3779b9 @@ -40,153 +37,30 @@ uint32_t Math::default_seed=1; static uint32_t Q[4096]; #endif -uint32_t Math::rand_from_seed(uint32_t *seed) { - // Xorshift31 PRNG - if ( *seed == 0 ) *seed = Math::RANDOM_MAX; - (*seed) ^= (*seed) << 13; - (*seed) ^= (*seed) >> 17; - (*seed) ^= (*seed) << 5; - return (*seed) & Math::RANDOM_MAX; +// TODO: we should eventually expose pcg.inc too +uint32_t Math::rand_from_seed(uint64_t *seed) { + pcg32_random_t pcg = {*seed, PCG_DEFAULT_INC_64}; + uint32_t r = pcg32_random_r(&pcg); + *seed = pcg.state; + return r; } -void Math::seed(uint32_t x) { - default_seed=x; +void Math::seed(uint64_t x) { + default_pcg.state=x; } void Math::randomize() { OS::Time time = OS::get_singleton()->get_time(); - seed(OS::get_singleton()->get_ticks_usec()*(time.hour+1)*(time.min+1)*(time.sec+1)*rand()); /* *OS::get_singleton()->get_time().sec); // windows doesn't have get_time(), returns always 0 */ + seed(OS::get_singleton()->get_ticks_usec()*(time.hour+1)*(time.min+1)*(time.sec+1)*rand()); // TODO: can be simplified. } uint32_t Math::rand() { - - return rand_from_seed(&default_seed); -} - -double Math::randf() { - - return (double)rand() / (double)Math::RANDOM_MAX; -} - -double Math::sin(double p_x) { - - return ::sin(p_x); - -} - -double Math::cos(double p_x) { - - return ::cos(p_x); - + return pcg32_random_r(&default_pcg); } -double Math::tan(double p_x) { - - return ::tan(p_x); - -} -double Math::sinh(double p_x) { - - return ::sinh(p_x); -} - -double Math::cosh(double p_x) { - - return ::cosh(p_x); -} - -double Math::tanh(double p_x) { - - return ::tanh(p_x); -} - - -double Math::deg2rad(double p_y) { - - return p_y*Math_PI/180.0; -} - -double Math::rad2deg(double p_y) { - - return p_y*180.0/Math_PI; -} - -double Math::round(double p_val) { - - if (p_val>=0) { - return ::floor(p_val+0.5); - } else { - p_val=-p_val; - return -::floor(p_val+0.5); - } -} - -double Math::asin(double p_x) { - - return ::asin(p_x); - -} - -double Math::acos(double p_x) { - - return ::acos(p_x); -} - -double Math::atan(double p_x) { - - return ::atan(p_x); -} - -double Math::dectime(double p_value,double p_amount, double p_step) { - - float sgn = p_value < 0 ? -1.0 : 1.0; - float val = absf(p_value); - val-=p_amount*p_step; - if (val<0.0) - val=0.0; - return val*sgn; -} - -double Math::atan2(double p_y, double p_x) { - - return ::atan2(p_y,p_x); - -} -double Math::sqrt(double p_x) { - - return ::sqrt(p_x); -} - -double Math::fmod(double p_x,double p_y) { - - return ::fmod(p_x,p_y); -} - -double Math::fposmod(double p_x,double p_y) { - - if (p_x>=0) { - - return Math::fmod(p_x,p_y); - - } else { - - return p_y-Math::fmod(-p_x,p_y); - } - -} -double Math::floor(double p_x) { - - return ::floor(p_x); -} - -double Math::ceil(double p_x) { - - return ::ceil(p_x); -} int Math::step_decimals(double p_step) { - static const int maxn=9; static const double sd[maxn]={ 0.9999, // somehow compensate for floating point error @@ -200,7 +74,7 @@ int Math::step_decimals(double p_step) { 0.000000009999 }; - double as=absf(p_step); + double as=Math::abs(p_step); for(int i=0;i<maxn;i++) { if (as>=sd[i]) { return i; @@ -210,8 +84,16 @@ int Math::step_decimals(double p_step) { return maxn; } -double Math::ease(double p_x, double p_c) { +double Math::dectime(double p_value,double p_amount, double p_step) { + double sgn = p_value < 0 ? -1.0 : 1.0; + double val = Math::abs(p_value); + val-=p_amount*p_step; + if (val<0.0) + val=0.0; + return val*sgn; +} +double Math::ease(double p_x, double p_c) { if (p_x<0) p_x=0; else if (p_x>1.0) @@ -232,32 +114,15 @@ double Math::ease(double p_x, double p_c) { } } else return 0; // no ease (raw) - } double Math::stepify(double p_value,double p_step) { - if (p_step!=0) { - - p_value=floor( p_value / p_step + 0.5 ) * p_step; + p_value=Math::floor( p_value / p_step + 0.5 ) * p_step; } return p_value; } -bool Math::is_nan(double p_val) { - - return (p_val!=p_val); -} - -bool Math::is_inf(double p_val) { - -#ifdef _MSC_VER - return !_finite(p_val); -#else - return isinf(p_val); -#endif - -} uint32_t Math::larger_prime(uint32_t p_val) { @@ -307,22 +172,15 @@ uint32_t Math::larger_prime(uint32_t p_val) { } double Math::random(double from, double to) { - unsigned int r = Math::rand(); double ret = (double)r/(double)RANDOM_MAX; return (ret)*(to-from) + from; } -double Math::pow(double x, double y) { - - return ::pow(x,y); +float Math::random(float from, float to) { + unsigned int r = Math::rand(); + float ret = (float)r/(float)RANDOM_MAX; + return (ret)*(to-from) + from; } -double Math::log(double x) { - return ::log(x); -} -double Math::exp(double x) { - - return ::exp(x); -} diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index fc76d96b2e..511af91835 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,84 +31,153 @@ #include "typedefs.h" #include "math_defs.h" +#include "pcg.h" -#ifndef NO_MATH_H -#include "math.h" -#endif +#include <math.h> +#include <float.h> + +#define Math_PI 3.14159265358979323846 +#define Math_SQRT12 0.7071067811865475244008443621048490 +#define Math_LN2 0.693147180559945309417 class Math { + static pcg32_random_t default_pcg; - static uint32_t default_seed; public: - Math() {}; // useless to instance + Math() {} // useless to instance enum { RANDOM_MAX=2147483647L }; - static double sin(double p_x); - static double cos(double p_x); - static double tan(double p_x); - static double sinh(double p_x); - static double cosh(double p_x); - static double tanh(double p_x); - static double asin(double p_x); - static double acos(double p_x); - static double atan(double p_x); - static double atan2(double p_y, double p_x); - static double deg2rad(double p_y); - static double rad2deg(double p_y); - static double sqrt(double p_x); - static double fmod(double p_x,double p_y); - static double fposmod(double p_x,double p_y); - static uint32_t rand_from_seed(uint32_t *seed); - static double floor(double p_x); - static double ceil(double p_x); - static double ease(double p_x, double p_c); - static int step_decimals(double p_step); - static double stepify(double p_value,double p_step); - static void seed(uint32_t x=0); - static void randomize(); - static uint32_t larger_prime(uint32_t p_val); - static double dectime(double p_value,double p_amount, double p_step); + static _ALWAYS_INLINE_ double sin(double p_x) { return ::sin(p_x); } + static _ALWAYS_INLINE_ float sin(float p_x) { return ::sinf(p_x); } - static inline double linear2db(double p_linear) { + static _ALWAYS_INLINE_ double cos(double p_x) { return ::cos(p_x); } + static _ALWAYS_INLINE_ float cos(float p_x) { return ::cosf(p_x); } - return Math::log( p_linear ) * 8.6858896380650365530225783783321; - } + static _ALWAYS_INLINE_ double tan(double p_x) { return ::tan(p_x); } + static _ALWAYS_INLINE_ float tan(float p_x) { return ::tanf(p_x); } + + static _ALWAYS_INLINE_ double sinh(double p_x) { return ::sinh(p_x); } + static _ALWAYS_INLINE_ float sinh(float p_x) { return ::sinhf(p_x); } + + static _ALWAYS_INLINE_ double cosh(double p_x) { return ::cosh(p_x); } + static _ALWAYS_INLINE_ float cosh(float p_x) { return ::coshf(p_x); } + + static _ALWAYS_INLINE_ double tanh(double p_x) { return ::tanh(p_x); } + static _ALWAYS_INLINE_ float tanh(float p_x) { return ::tanhf(p_x); } - static inline double db2linear(double p_db) { + static _ALWAYS_INLINE_ double asin(double p_x) { return ::asin(p_x); } + static _ALWAYS_INLINE_ float asin(float p_x) { return ::asinf(p_x); } - return Math::exp( p_db * 0.11512925464970228420089957273422 ); + static _ALWAYS_INLINE_ double acos(double p_x) { return ::acos(p_x); } + static _ALWAYS_INLINE_ float acos(float p_x) { return ::acosf(p_x); } + + static _ALWAYS_INLINE_ double atan(double p_x) { return ::atan(p_x); } + static _ALWAYS_INLINE_ float atan(float p_x) { return ::atanf(p_x); } + + static _ALWAYS_INLINE_ double atan2(double p_y, double p_x) { return ::atan2(p_y,p_x); } + static _ALWAYS_INLINE_ float atan2(float p_y, float p_x) { return ::atan2f(p_y,p_x); } + + static _ALWAYS_INLINE_ double sqrt(double p_x) { return ::sqrt(p_x); } + static _ALWAYS_INLINE_ float sqrt(float p_x) { return ::sqrtf(p_x); } + + static _ALWAYS_INLINE_ double fmod(double p_x,double p_y) { return ::fmod(p_x,p_y); } + static _ALWAYS_INLINE_ float fmod(float p_x,float p_y) { return ::fmodf(p_x,p_y); } + + static _ALWAYS_INLINE_ double floor(double p_x) { return ::floor(p_x); } + static _ALWAYS_INLINE_ float floor(float p_x) { return ::floorf(p_x); } + + static _ALWAYS_INLINE_ double ceil(double p_x) { return ::ceil(p_x); } + static _ALWAYS_INLINE_ float ceil(float p_x) { return ::ceilf(p_x); } + + static _ALWAYS_INLINE_ double pow(double p_x, double p_y) { return ::pow(p_x,p_y); } + static _ALWAYS_INLINE_ float pow(float p_x, float p_y) { return ::powf(p_x,p_y); } + + static _ALWAYS_INLINE_ double log(double p_x) { return ::log(p_x); } + static _ALWAYS_INLINE_ float log(float p_x) { return ::logf(p_x); } + + static _ALWAYS_INLINE_ double exp(double p_x) { return ::exp(p_x); } + static _ALWAYS_INLINE_ float exp(float p_x) { return ::expf(p_x); } + + static _ALWAYS_INLINE_ bool is_nan(double p_val) { return (p_val!=p_val); } + static _ALWAYS_INLINE_ bool is_nan(float p_val) { return (p_val!=p_val); } + + static _ALWAYS_INLINE_ bool is_inf(double p_val) { + #ifdef _MSC_VER + return !_finite(p_val); + #else + return isinf(p_val); + #endif + } + + static _ALWAYS_INLINE_ bool is_inf(float p_val) { + #ifdef _MSC_VER + return !_finite(p_val); + #else + return isinf(p_val); + #endif } + + static _ALWAYS_INLINE_ double abs(double g) { return absd(g); } + static _ALWAYS_INLINE_ float abs(float g) { return absf(g); } + static _ALWAYS_INLINE_ int abs(int g) { return g > 0 ? g : -g; } - static bool is_nan(double p_val); - static bool is_inf(double p_val); + static _ALWAYS_INLINE_ double fposmod(double p_x,double p_y) { return (p_x>=0) ? Math::fmod(p_x,p_y) : p_y-Math::fmod(-p_x,p_y); } + static _ALWAYS_INLINE_ float fposmod(float p_x,float p_y) { return (p_x>=0) ? Math::fmod(p_x,p_y) : p_y-Math::fmod(-p_x,p_y); } + static _ALWAYS_INLINE_ double deg2rad(double p_y) { return p_y*Math_PI/180.0; } + static _ALWAYS_INLINE_ float deg2rad(float p_y) { return p_y*Math_PI/180.0; } + static _ALWAYS_INLINE_ double rad2deg(double p_y) { return p_y*180.0/Math_PI; } + static _ALWAYS_INLINE_ float rad2deg(float p_y) { return p_y*180.0/Math_PI; } - static uint32_t rand(); - static double randf(); + static _ALWAYS_INLINE_ double lerp(double a, double b, double c) { return a+(b-a)*c; } + static _ALWAYS_INLINE_ float lerp(float a, float b, float c) { return a+(b-a)*c; } - static double round(double p_val); + static _ALWAYS_INLINE_ double linear2db(double p_linear) { return Math::log( p_linear ) * 8.6858896380650365530225783783321; } + static _ALWAYS_INLINE_ float linear2db(float p_linear) { return Math::log( p_linear ) * 8.6858896380650365530225783783321; } - static double random(double from, double to); + static _ALWAYS_INLINE_ double db2linear(double p_db) { return Math::exp( p_db * 0.11512925464970228420089957273422 ); } + static _ALWAYS_INLINE_ float db2linear(float p_db) { return Math::exp( p_db * 0.11512925464970228420089957273422 ); } + static _ALWAYS_INLINE_ double round(double p_val) { return (p_val>=0) ? Math::floor(p_val+0.5) : -Math::floor(-p_val+0.5); } + static _ALWAYS_INLINE_ float round(float p_val) { return (p_val>=0) ? Math::floor(p_val+0.5) : -Math::floor(-p_val+0.5); } - static _FORCE_INLINE_ real_t abs(real_t g) { + // double only, as these functions are mainly used by the editor and not performance-critical, + static double ease(double p_x, double p_c); + static int step_decimals(double p_step); + static double stepify(double p_value,double p_step); + static double dectime(double p_value,double p_amount, double p_step); -#ifdef REAL_T_IS_DOUBLE + static uint32_t larger_prime(uint32_t p_val); - return absd(g); -#else + static void seed(uint64_t x=0); + static void randomize(); + static uint32_t rand_from_seed(uint64_t *seed); + static uint32_t rand(); + static _ALWAYS_INLINE_ double randf() { return (double)rand() / (double)Math::RANDOM_MAX; } + static _ALWAYS_INLINE_ float randd() { return (float)rand() / (float)Math::RANDOM_MAX; } - return absf(g); -#endif + static double random(double from, double to); + static float random(float from, float to); + static real_t random(int from, int to) { return (real_t)random((real_t)from, (real_t)to); } + + + static _ALWAYS_INLINE_ bool isequal_approx(real_t a, real_t b) { + // TODO: Comparing floats for approximate-equality is non-trivial. + // Using epsilon should cover the typical cases in Godot (where a == b is used to compare two reals), such as matrix and vector comparison operators. + // A proper implementation in terms of ULPs should eventually replace the contents of this function. + // See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ for details. + + return abs(a-b) < CMP_EPSILON; } - static _FORCE_INLINE_ float absf(float g) { + + static _ALWAYS_INLINE_ float absf(float g) { union { float f; @@ -120,7 +189,7 @@ public: return u.f; } - static _FORCE_INLINE_ double absd(double g) { + static _ALWAYS_INLINE_ double absd(double g) { union { double d; @@ -132,12 +201,12 @@ public: } //this function should be as fast as possible and rounding mode should not matter - static _FORCE_INLINE_ int fast_ftoi(float a) { + static _ALWAYS_INLINE_ int fast_ftoi(float a) { static int b; #if (defined(_WIN32_WINNT) && _WIN32_WINNT >= 0x0603) || WINAPI_FAMILY == WINAPI_FAMILY_PHONE_APP // windows 8 phone? - b = (int)((a>0.0f) ? (a + 0.5f):(a -0.5f)); + b = (int)((a>0.0) ? (a + 0.5):(a -0.5)); #elif defined(_MSC_VER) && _MSC_VER < 1800 __asm fld a @@ -160,25 +229,118 @@ public: #if defined(__GNUC__) - static _FORCE_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE + static _ALWAYS_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE + static _ALWAYS_INLINE_ int64_t dtoll(float p_float) { return (int64_t)p_float; } ///@TODO OPTIMIZE and rename #else - static _FORCE_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE + static _ALWAYS_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE + static _ALWAYS_INLINE_ int64_t dtoll(float p_float) { return (int64_t)p_float; } ///@TODO OPTIMIZE and rename #endif - static _FORCE_INLINE_ float lerp(float a, float b, float c) { - return a+(b-a)*c; + static _ALWAYS_INLINE_ uint32_t halfbits_to_floatbits(uint16_t h) + { + uint16_t h_exp, h_sig; + uint32_t f_sgn, f_exp, f_sig; + + h_exp = (h&0x7c00u); + f_sgn = ((uint32_t)h&0x8000u) << 16; + switch (h_exp) { + case 0x0000u: /* 0 or subnormal */ + h_sig = (h&0x03ffu); + /* Signed zero */ + if (h_sig == 0) { + return f_sgn; + } + /* Subnormal */ + h_sig <<= 1; + while ((h_sig&0x0400u) == 0) { + h_sig <<= 1; + h_exp++; + } + f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23; + f_sig = ((uint32_t)(h_sig&0x03ffu)) << 13; + return f_sgn + f_exp + f_sig; + case 0x7c00u: /* inf or NaN */ + /* All-ones exponent and a copy of the significand */ + return f_sgn + 0x7f800000u + (((uint32_t)(h&0x03ffu)) << 13); + default: /* normalized */ + /* Just need to adjust the exponent and shift */ + return f_sgn + (((uint32_t)(h&0x7fffu) + 0x1c000u) << 13); + } } - static double pow(double x, double y); - static double log(double x); - static double exp(double x); + static _ALWAYS_INLINE_ float halfptr_to_float(const uint16_t *h) { + + union { + uint32_t u32; + float f32; + } u; + + u.u32=halfbits_to_floatbits(*h); + return u.f32; + } + + static _ALWAYS_INLINE_ uint16_t make_half_float(float f) { + + union { + float fv; + uint32_t ui; + } ci; + ci.fv=f; + + uint32_t x = ci.ui; + uint32_t sign = (unsigned short)(x >> 31); + uint32_t mantissa; + uint32_t exp; + uint16_t hf; + + // get mantissa + mantissa = x & ((1 << 23) - 1); + // get exponent bits + exp = x & (0xFF << 23); + if (exp >= 0x47800000) + { + // check if the original single precision float number is a NaN + if (mantissa && (exp == (0xFF << 23))) + { + // we have a single precision NaN + mantissa = (1 << 23) - 1; + } + else + { + // 16-bit half-float representation stores number as Inf + mantissa = 0; + } + hf = (((uint16_t)sign) << 15) | (uint16_t)((0x1F << 10)) | + (uint16_t)(mantissa >> 13); + } + // check if exponent is <= -15 + else if (exp <= 0x38000000) + { + + /*// store a denorm half-float value or zero + exp = (0x38000000 - exp) >> 23; + mantissa >>= (14 + exp); + + hf = (((uint16_t)sign) << 15) | (uint16_t)(mantissa); + */ + hf=0; //denormals do not work for 3D, convert to zero + } + else + { + hf = (((uint16_t)sign) << 15) | + (uint16_t)((exp - 0x38000000) >> 13) | + (uint16_t)(mantissa >> 13); + } + + return hf; + } + + }; -#define Math_PI 3.14159265358979323846 -#define Math_SQRT12 0.7071067811865475244008443621048490 #endif // MATH_FUNCS_H diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp index 71e6b62212..1fabfbbd4c 100644 --- a/core/math/matrix3.cpp +++ b/core/math/matrix3.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,7 +33,7 @@ #define cofac(row1,col1, row2, col2)\ (elements[row1][col1] * elements[row2][col2] - elements[row1][col2] * elements[row2][col1]) -void Matrix3::from_z(const Vector3& p_z) { +void Basis::from_z(const Vector3& p_z) { if (Math::abs(p_z.z) > Math_SQRT12 ) { @@ -53,7 +53,7 @@ void Matrix3::from_z(const Vector3& p_z) { elements[2]=p_z; } -void Matrix3::invert() { +void Basis::invert() { real_t co[3]={ @@ -72,7 +72,8 @@ void Matrix3::invert() { } -void Matrix3::orthonormalize() { +void Basis::orthonormalize() { + ERR_FAIL_COND(determinant() == 0); // Gram-Schmidt Process @@ -92,100 +93,230 @@ void Matrix3::orthonormalize() { } -Matrix3 Matrix3::orthonormalized() const { +Basis Basis::orthonormalized() const { - Matrix3 c = *this; + Basis c = *this; c.orthonormalize(); return c; } +bool Basis::is_orthogonal() const { + Basis id; + Basis m = (*this)*transposed(); -Matrix3 Matrix3::inverse() const { + return isequal_approx(id,m); +} + +bool Basis::is_rotation() const { + return Math::isequal_approx(determinant(), 1) && is_orthogonal(); +} + + +bool Basis::is_symmetric() const { + + if (Math::abs(elements[0][1] - elements[1][0]) > CMP_EPSILON) + return false; + if (Math::abs(elements[0][2] - elements[2][0]) > CMP_EPSILON) + return false; + if (Math::abs(elements[1][2] - elements[2][1]) > CMP_EPSILON) + return false; + + return true; +} + + +Basis Basis::diagonalize() { + + //NOTE: only implemented for symmetric matrices + //with the Jacobi iterative method method + + ERR_FAIL_COND_V(!is_symmetric(), Basis()); + + 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::abs(elements[j][j] - elements[i][i]) < CMP_EPSILON) { + angle = Math_PI / 4; + } else { + angle = 0.5 * Math::atan(2 * elements[i][j] / (elements[j][j] - elements[i][i])); + } - Matrix3 inv=*this; + // 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 Matrix3::transpose() { +void Basis::transpose() { SWAP(elements[0][1],elements[1][0]); SWAP(elements[0][2],elements[2][0]); SWAP(elements[1][2],elements[2][1]); } -Matrix3 Matrix3::transposed() const { +Basis Basis::transposed() const { - Matrix3 tr=*this; + Basis tr=*this; tr.transpose(); return tr; } -void Matrix3::scale(const Vector3& p_scale) { +// 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[1][0]*=p_scale.x; - elements[2][0]*=p_scale.x; - elements[0][1]*=p_scale.y; + 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[2][1]*=p_scale.y; - elements[0][2]*=p_scale.z; - elements[1][2]*=p_scale.z; + elements[1][2]*=p_scale.y; + elements[2][0]*=p_scale.z; + elements[2][1]*=p_scale.z; elements[2][2]*=p_scale.z; } -Matrix3 Matrix3::scaled( const Vector3& p_scale ) const { +Basis Basis::scaled( const Vector3& p_scale ) const { - Matrix3 m = *this; + Basis m = *this; m.scale(p_scale); return m; } -Vector3 Matrix3::get_scale() const { - - return Vector3( +Vector3 Basis::get_scale() const { + // We are assuming M = R.S, and performing a polar decomposition to extract R and S. + // FIXME: We eventually need a proper polar decomposition. + // As a cheap workaround until then, to ensure that R is a proper rotation matrix with determinant +1 + // (such that it can be represented by a Quat or Euler angles), we absorb the sign flip into the scaling matrix. + // As such, it works in conjuction with get_rotation(). + real_t det_sign = determinant() > 0 ? 1 : -1; + return det_sign*Vector3( Vector3(elements[0][0],elements[1][0],elements[2][0]).length(), Vector3(elements[0][1],elements[1][1],elements[2][1]).length(), Vector3(elements[0][2],elements[1][2],elements[2][2]).length() ); } -void Matrix3::rotate(const Vector3& p_axis, real_t p_phi) { - *this = *this * Matrix3(p_axis, p_phi); +// 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); } -Matrix3 Matrix3::rotated(const Vector3& p_axis, real_t p_phi) const { +void Basis::rotate(const Vector3& p_axis, real_t p_phi) { + *this = rotated(p_axis, p_phi); +} - return *this * Matrix3(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); } -Vector3 Matrix3::get_euler() const { +Vector3 Basis::get_rotation() 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(); +} +// get_euler 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() 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 - - Matrix3 m = *this; - m.orthonormalize(); + // 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; - euler.y = Math::asin(m[0][2]); + ERR_FAIL_COND_V(is_rotation() == false, euler); + + euler.y = Math::asin(elements[0][2]); if ( euler.y < Math_PI*0.5) { if ( euler.y > -Math_PI*0.5) { - euler.x = Math::atan2(-m[1][2],m[2][2]); - euler.z = Math::atan2(-m[0][1],m[0][0]); + euler.x = Math::atan2(-elements[1][2],elements[2][2]); + euler.z = Math::atan2(-elements[0][1],elements[0][0]); } else { - real_t r = Math::atan2(m[1][0],m[1][1]); + real_t r = Math::atan2(elements[1][0],elements[1][1]); euler.z = 0.0; euler.x = euler.z - r; } } else { - real_t r = Math::atan2(m[0][1],m[1][1]); + real_t r = Math::atan2(elements[0][1],elements[1][1]); euler.z = 0; euler.x = r - euler.z; } @@ -195,43 +326,59 @@ Vector3 Matrix3::get_euler() const { } -void Matrix3::set_euler(const Vector3& p_euler) { +// set_euler expects a vector containing the Euler angles in the format +// (c,b,a), where a is the angle of the first rotation, and c is the last. +// The current implementation uses XYZ convention (Z is the first rotation). +void Basis::set_euler(const Vector3& p_euler) { real_t c, s; c = Math::cos(p_euler.x); s = Math::sin(p_euler.x); - Matrix3 xmat(1.0,0.0,0.0,0.0,c,-s,0.0,s,c); + 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); - Matrix3 ymat(c,0.0,s,0.0,1.0,0.0,-s,0.0,c); + 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); - Matrix3 zmat(c,-s,0.0,s,c,0.0,0.0,0.0,1.0); + 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); } -bool Matrix3::operator==(const Matrix3& p_matrix) const { +bool Basis::isequal_approx(const Basis& a, const Basis& b) const { + + for (int i=0;i<3;i++) { + for (int j=0;j<3;j++) { + if (Math::isequal_approx(a.elements[i][j],b.elements[i][j]) == false) + 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]) + if (elements[i][j] != p_matrix.elements[i][j]) return false; } } return true; } -bool Matrix3::operator!=(const Matrix3& p_matrix) const { + +bool Basis::operator!=(const Basis& p_matrix) const { return (!(*this==p_matrix)); } -Matrix3::operator String() const { +Basis::operator String() const { String mtx; for (int i=0;i<3;i++) { @@ -248,12 +395,10 @@ Matrix3::operator String() const { return mtx; } -Matrix3::operator Quat() const { +Basis::operator Quat() const { + ERR_FAIL_COND_V(is_rotation() == false, Quat()); - Matrix3 m=*this; - m.orthonormalize(); - - real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; + real_t trace = elements[0][0] + elements[1][1] + elements[2][2]; real_t temp[4]; if (trace > 0.0) @@ -262,66 +407,66 @@ Matrix3::operator Quat() const { 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); + temp[0]=((elements[2][1] - elements[1][2]) * s); + temp[1]=((elements[0][2] - elements[2][0]) * s); + temp[2]=((elements[1][0] - elements[0][1]) * s); } 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 i = elements[0][0] < elements[1][1] ? + (elements[1][1] < elements[2][2] ? 2 : 1) : + (elements[0][0] < 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); + real_t s = Math::sqrt(elements[i][i] - elements[j][j] - 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; + temp[3] = (elements[k][j] - elements[j][k]) * s; + temp[j] = (elements[j][i] + elements[i][j]) * s; + temp[k] = (elements[k][i] + elements[i][k]) * s; } return Quat(temp[0],temp[1],temp[2],temp[3]); } -static const Matrix3 _ortho_bases[24]={ - Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1), - Matrix3(0, -1, 0, 1, 0, 0, 0, 0, 1), - Matrix3(-1, 0, 0, 0, -1, 0, 0, 0, 1), - Matrix3(0, 1, 0, -1, 0, 0, 0, 0, 1), - Matrix3(1, 0, 0, 0, 0, -1, 0, 1, 0), - Matrix3(0, 0, 1, 1, 0, 0, 0, 1, 0), - Matrix3(-1, 0, 0, 0, 0, 1, 0, 1, 0), - Matrix3(0, 0, -1, -1, 0, 0, 0, 1, 0), - Matrix3(1, 0, 0, 0, -1, 0, 0, 0, -1), - Matrix3(0, 1, 0, 1, 0, 0, 0, 0, -1), - Matrix3(-1, 0, 0, 0, 1, 0, 0, 0, -1), - Matrix3(0, -1, 0, -1, 0, 0, 0, 0, -1), - Matrix3(1, 0, 0, 0, 0, 1, 0, -1, 0), - Matrix3(0, 0, -1, 1, 0, 0, 0, -1, 0), - Matrix3(-1, 0, 0, 0, 0, -1, 0, -1, 0), - Matrix3(0, 0, 1, -1, 0, 0, 0, -1, 0), - Matrix3(0, 0, 1, 0, 1, 0, -1, 0, 0), - Matrix3(0, -1, 0, 0, 0, 1, -1, 0, 0), - Matrix3(0, 0, -1, 0, -1, 0, -1, 0, 0), - Matrix3(0, 1, 0, 0, 0, -1, -1, 0, 0), - Matrix3(0, 0, 1, 0, -1, 0, 1, 0, 0), - Matrix3(0, 1, 0, 0, 0, 1, 1, 0, 0), - Matrix3(0, 0, -1, 0, 1, 0, 1, 0, 0), - Matrix3(0, -1, 0, 0, 0, -1, 1, 0, 0) +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 Matrix3::get_orthogonal_index() const { +int Basis::get_orthogonal_index() const { //could be sped up if i come up with a way - Matrix3 orth=*this; + Basis orth=*this; for(int i=0;i<3;i++) { for(int j=0;j<3;j++) { - float v = orth[i][j]; + real_t v = orth[i][j]; if (v>0.5) v=1.0; else if (v<-0.5) @@ -344,7 +489,7 @@ int Matrix3::get_orthogonal_index() const { return 0; } -void Matrix3::set_orthogonal_index(int p_index){ +void Basis::set_orthogonal_index(int p_index){ //there only exist 24 orthogonal bases in r3 ERR_FAIL_INDEX(p_index,24); @@ -355,12 +500,13 @@ void Matrix3::set_orthogonal_index(int p_index){ } -void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const { +void Basis::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const { + ERR_FAIL_COND(is_rotation() == false); - double angle,x,y,z; // variables for result - double epsilon = 0.01; // margin to allow for rounding errors - double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees + 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) @@ -379,12 +525,12 @@ void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const { } // otherwise this singularity is angle = 180 angle = Math_PI; - double xx = (elements[0][0]+1)/2; - double yy = (elements[1][1]+1)/2; - double zz = (elements[2][2]+1)/2; - double xy = (elements[1][0]+elements[0][1])/4; - double xz = (elements[2][0]+elements[0][2])/4; - double yz = (elements[2][1]+elements[1][2])/4; + 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; @@ -421,28 +567,27 @@ void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const { return; } // as we have reached here there are no singularities so we can handle normally - double s = Math::sqrt((elements[1][2] - elements[2][1])*(elements[1][2] - elements[2][1]) + 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])); // used to normalise - if (Math::abs(s) < 0.001) s=1; - // prevent divide by zero, should not happen if matrix is orthogonal and should be - // caught by singularity test above, but I've left it in just in case + +(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); - x = (elements[1][2] - elements[2][1])/s; - y = (elements[2][0] - elements[0][2])/s; - z = (elements[0][1] - elements[1][0])/s; + 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; } -Matrix3::Matrix3(const Vector3& p_euler) { +Basis::Basis(const Vector3& p_euler) { set_euler( p_euler ); } -Matrix3::Matrix3(const Quat& p_quat) { +Basis::Basis(const Quat& p_quat) { real_t d = p_quat.length_squared(); real_t s = 2.0 / d; @@ -456,7 +601,8 @@ Matrix3::Matrix3(const Quat& p_quat) { } -Matrix3::Matrix3(const Vector3& p_axis, real_t p_phi) { +Basis::Basis(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_and_angle Vector3 axis_sq(p_axis.x*p_axis.x,p_axis.y*p_axis.y,p_axis.z*p_axis.z); @@ -464,15 +610,15 @@ Matrix3::Matrix3(const Vector3& p_axis, real_t p_phi) { real_t sine= Math::sin(p_phi); elements[0][0] = axis_sq.x + cosine * ( 1.0 - axis_sq.x ); - elements[0][1] = p_axis.x * p_axis.y * ( 1.0 - cosine ) + p_axis.z * sine; - elements[0][2] = p_axis.z * p_axis.x * ( 1.0 - cosine ) - p_axis.y * sine; + elements[0][1] = p_axis.x * p_axis.y * ( 1.0 - cosine ) - p_axis.z * sine; + elements[0][2] = p_axis.z * p_axis.x * ( 1.0 - cosine ) + p_axis.y * sine; - elements[1][0] = p_axis.x * p_axis.y * ( 1.0 - cosine ) - p_axis.z * sine; + elements[1][0] = p_axis.x * p_axis.y * ( 1.0 - cosine ) + p_axis.z * sine; elements[1][1] = axis_sq.y + cosine * ( 1.0 - axis_sq.y ); - elements[1][2] = p_axis.y * p_axis.z * ( 1.0 - cosine ) + p_axis.x * sine; + elements[1][2] = p_axis.y * p_axis.z * ( 1.0 - cosine ) - p_axis.x * sine; - elements[2][0] = p_axis.z * p_axis.x * ( 1.0 - cosine ) + p_axis.y * sine; - elements[2][1] = p_axis.y * p_axis.z * ( 1.0 - cosine ) - p_axis.x * sine; + elements[2][0] = p_axis.z * p_axis.x * ( 1.0 - cosine ) - p_axis.y * sine; + elements[2][1] = p_axis.y * p_axis.z * ( 1.0 - cosine ) + p_axis.x * sine; elements[2][2] = axis_sq.z + cosine * ( 1.0 - axis_sq.z ); } diff --git a/core/math/matrix3.h b/core/math/matrix3.h index e514f490f7..abce1ee45d 100644 --- a/core/math/matrix3.h +++ b/core/math/matrix3.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -26,16 +26,18 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + +#include "vector3.h" + #ifndef MATRIX3_H #define MATRIX3_H -#include "vector3.h" #include "quat.h" /** @author Juan Linietsky <reduzio@gmail.com> */ -class Matrix3 { +class Basis { public: Vector3 elements[3]; @@ -52,10 +54,10 @@ public: void invert(); void transpose(); - Matrix3 inverse() const; - Matrix3 transposed() const; + Basis inverse() const; + Basis transposed() const; - _FORCE_INLINE_ float determinant() const; + _FORCE_INLINE_ real_t determinant() const; void from_z(const Vector3& p_z); @@ -71,10 +73,14 @@ public: } void rotate(const Vector3& p_axis, real_t p_phi); - Matrix3 rotated(const Vector3& p_axis, real_t p_phi) const; + Basis rotated(const Vector3& p_axis, real_t p_phi) const; + + void rotate(const Vector3& p_euler); + Basis rotated(const Vector3& p_euler) const; + Vector3 get_rotation() const; void scale( const Vector3& p_scale ); - Matrix3 scaled( const Vector3& p_scale ) const; + Basis scaled( const Vector3& p_scale ) const; Vector3 get_scale() const; Vector3 get_euler() const; @@ -91,17 +97,28 @@ public: return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2]; } - bool operator==(const Matrix3& p_matrix) const; - bool operator!=(const Matrix3& p_matrix) const; + bool isequal_approx(const Basis& a, const Basis& b) 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 Matrix3& p_matrix); - _FORCE_INLINE_ Matrix3 operator*(const Matrix3& 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-=(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); + bool is_orthogonal() const; + bool is_rotation() const; + operator String() const; void get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const; @@ -130,6 +147,10 @@ public: 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; @@ -142,9 +163,9 @@ public: elements[2].zero(); } - _FORCE_INLINE_ Matrix3 transpose_xform(const Matrix3& m) const + _FORCE_INLINE_ Basis transpose_xform(const Basis& m) const { - return Matrix3( + 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, @@ -155,21 +176,31 @@ public: 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); } - Matrix3(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) { + 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(); - Matrix3 orthonormalized() const; + Basis orthonormalized() const; + + bool is_symmetric() const; + Basis diagonalize(); operator Quat() const; - Matrix3(const Quat& p_quat); // euler - Matrix3(const Vector3& p_euler); // euler - Matrix3(const Vector3& p_axis, real_t p_phi); + Basis(const Quat& p_quat); // euler + Basis(const Vector3& p_euler); // euler + Basis(const Vector3& p_axis, real_t p_phi); - _FORCE_INLINE_ Matrix3() { + _FORCE_INLINE_ Basis(const Vector3& row0, const Vector3& row1, const Vector3& row2) + { + elements[0]=row0; + elements[1]=row1; + elements[2]=row2; + } + + _FORCE_INLINE_ Basis() { elements[0][0]=1; elements[0][1]=0; @@ -185,7 +216,7 @@ public: }; -_FORCE_INLINE_ void Matrix3::operator*=(const Matrix3& p_matrix) { +_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]), @@ -194,16 +225,59 @@ _FORCE_INLINE_ void Matrix3::operator*=(const Matrix3& p_matrix) { } -_FORCE_INLINE_ Matrix3 Matrix3::operator*(const Matrix3& p_matrix) const { +_FORCE_INLINE_ Basis Basis::operator*(const Basis& p_matrix) const { - return Matrix3( + 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]) ); } -Vector3 Matrix3::xform(const Vector3& p_vector) const { + +_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), @@ -212,7 +286,7 @@ Vector3 Matrix3::xform(const Vector3& p_vector) const { ); } -Vector3 Matrix3::xform_inv(const Vector3& p_vector) const { +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 ), @@ -221,7 +295,7 @@ Vector3 Matrix3::xform_inv(const Vector3& p_vector) const { ); } -float Matrix3::determinant() const { +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]) + diff --git a/core/math/octree.h b/core/math/octree.h index 6080b21680..e566df6a4f 100644 --- a/core/math/octree.h +++ b/core/math/octree.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,7 +30,7 @@ #define OCTREE_H #include "vector3.h" -#include "aabb.h" +#include "rect3.h" #include "list.h" #include "variant.h" #include "map.h" @@ -107,7 +107,7 @@ private: struct Octant { // cached for FAST plane check - AABB aabb; + Rect3 aabb; uint64_t last_pass; Octant *parent; @@ -130,8 +130,10 @@ private: ~Octant() { - //for (int i=0;i<8;i++) - // memdelete_notnull(children[i]); + /* + for (int i=0;i<8;i++) + memdelete_notnull(children[i]); + */ } }; @@ -152,8 +154,8 @@ private: OctreeElementID _id; Octant *common_parent; - AABB aabb; - AABB container_aabb; + Rect3 aabb; + Rect3 container_aabb; List<PairData*,AL> pair_list; @@ -250,8 +252,10 @@ private: E->get().eA=p_A->pair_list.push_back(&E->get()); E->get().eB=p_B->pair_list.push_back(&E->get()); -// if (pair_callback) -// pair_callback(pair_callback_userdata,p_A->userdata,p_B->userdata); + /* + if (pair_callback) + pair_callback(pair_callback_userdata,p_A->userdata,p_B->userdata); + */ } else { E->get().refcount++; @@ -338,7 +342,7 @@ private: void _insert_element(Element *p_element,Octant *p_octant); - void _ensure_valid_root(const AABB& p_aabb); + void _ensure_valid_root(const Rect3& p_aabb); bool _remove_element_from_octant(Element *p_element,Octant *p_octant,Octant *p_limit=NULL); void _remove_element(Element *p_element); void _pair_element(Element *p_element,Octant *p_octant); @@ -356,7 +360,7 @@ private: }; void _cull_convex(Octant *p_octant,_CullConvexData *p_cull); - void _cull_AABB(Octant *p_octant,const AABB& p_aabb, T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask); + void _cull_AABB(Octant *p_octant,const Rect3& p_aabb, T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask); void _cull_segment(Octant *p_octant,const Vector3& p_from, const Vector3& p_to,T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask); void _cull_point(Octant *p_octant,const Vector3& p_point,T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask); @@ -375,8 +379,8 @@ private: } public: - OctreeElementID create(T* p_userdata, const AABB& p_aabb=AABB(), int p_subindex=0, bool p_pairable=false,uint32_t p_pairable_type=0,uint32_t pairable_mask=1); - void move(OctreeElementID p_id, const AABB& p_aabb); + OctreeElementID create(T* p_userdata, const Rect3& p_aabb=Rect3(), int p_subindex=0, bool p_pairable=false,uint32_t p_pairable_type=0,uint32_t pairable_mask=1); + void move(OctreeElementID p_id, const Rect3& p_aabb); void set_pairable(OctreeElementID p_id,bool p_pairable=false,uint32_t p_pairable_type=0,uint32_t pairable_mask=1); void erase(OctreeElementID p_id); @@ -385,7 +389,7 @@ public: int get_subindex(OctreeElementID p_id) const; int cull_convex(const Vector<Plane>& p_convex,T** p_result_array,int p_result_max,uint32_t p_mask=0xFFFFFFFF); - int cull_AABB(const AABB& p_aabb,T** p_result_array,int p_result_max,int *p_subindex_array=NULL,uint32_t p_mask=0xFFFFFFFF); + int cull_AABB(const Rect3& p_aabb,T** p_result_array,int p_result_max,int *p_subindex_array=NULL,uint32_t p_mask=0xFFFFFFFF); int cull_segment(const Vector3& p_from, const Vector3& p_to,T** p_result_array,int p_result_max,int *p_subindex_array=NULL,uint32_t p_mask=0xFFFFFFFF); int cull_point(const Vector3& p_point,T** p_result_array,int p_result_max,int *p_subindex_array=NULL,uint32_t p_mask=0xFFFFFFFF); @@ -431,7 +435,7 @@ int Octree<T,use_pairs,AL>::get_subindex(OctreeElementID p_id) const { template<class T,bool use_pairs,class AL> void Octree<T,use_pairs,AL>::_insert_element(Element *p_element,Octant *p_octant) { - float element_size = p_element->aabb.get_longest_axis_size() * 1.01; // avoid precision issues + real_t element_size = p_element->aabb.get_longest_axis_size() * 1.01; // avoid precision issues if (p_octant->aabb.size.x/OCTREE_DIVISOR < element_size) { //if (p_octant->aabb.size.x*0.5 < element_size) { @@ -487,7 +491,7 @@ void Octree<T,use_pairs,AL>::_insert_element(Element *p_element,Octant *p_octant } else { /* check againt AABB where child should be */ - AABB aabb=p_octant->aabb; + Rect3 aabb=p_octant->aabb; aabb.size*=0.5; if (i&1) @@ -549,12 +553,12 @@ void Octree<T,use_pairs,AL>::_insert_element(Element *p_element,Octant *p_octant template<class T,bool use_pairs,class AL> -void Octree<T,use_pairs,AL>::_ensure_valid_root(const AABB& p_aabb) { +void Octree<T,use_pairs,AL>::_ensure_valid_root(const Rect3& p_aabb) { if (!root) { // octre is empty - AABB base( Vector3(), Vector3(1.0,1.0,1.0) * unit_size); + Rect3 base( Vector3(), Vector3(1.0,1.0,1.0) * unit_size); while ( !base.encloses(p_aabb) ) { @@ -578,7 +582,7 @@ void Octree<T,use_pairs,AL>::_ensure_valid_root(const AABB& p_aabb) { } else { - AABB base=root->aabb; + Rect3 base=root->aabb; while( !base.encloses( p_aabb ) ) { @@ -814,7 +818,7 @@ void Octree<T,use_pairs,AL>::_remove_element(Element *p_element) { } template<class T,bool use_pairs,class AL> -OctreeElementID Octree<T,use_pairs,AL>::create(T* p_userdata, const AABB& p_aabb, int p_subindex,bool p_pairable,uint32_t p_pairable_type,uint32_t p_pairable_mask) { +OctreeElementID Octree<T,use_pairs,AL>::create(T* p_userdata, const Rect3& p_aabb, int p_subindex,bool p_pairable,uint32_t p_pairable_type,uint32_t p_pairable_mask) { // check for AABB validity #ifdef DEBUG_ENABLED @@ -857,7 +861,7 @@ OctreeElementID Octree<T,use_pairs,AL>::create(T* p_userdata, const AABB& p_aabb template<class T,bool use_pairs,class AL> -void Octree<T,use_pairs,AL>::move(OctreeElementID p_id, const AABB& p_aabb) { +void Octree<T,use_pairs,AL>::move(OctreeElementID p_id, const Rect3& p_aabb) { #ifdef DEBUG_ENABLED // check for AABB validity @@ -906,7 +910,7 @@ void Octree<T,use_pairs,AL>::move(OctreeElementID p_id, const AABB& p_aabb) { if (old_has_surf) { _remove_element(&e); // removing e.common_parent=NULL; - e.aabb=AABB(); + e.aabb=Rect3(); _optimize(); } else { _ensure_valid_root(p_aabb); // inserting @@ -935,7 +939,7 @@ void Octree<T,use_pairs,AL>::move(OctreeElementID p_id, const AABB& p_aabb) { return; } - AABB combined=e.aabb; + Rect3 combined=e.aabb; combined.merge_with(p_aabb); _ensure_valid_root(combined); @@ -970,8 +974,10 @@ void Octree<T,use_pairs,AL>::move(OctreeElementID p_id, const AABB& p_aabb) { Octant *o=E->get().octant; typename List<typename Element::OctantOwner,AL>::Element *N=E->next(); -// if (!use_pairs) -// o->elements.erase( E->get().E ); + /* + if (!use_pairs) + o->elements.erase( E->get().E ); + */ if (use_pairs && e.pairable) o->pairable_elements.erase( E->get().E ); @@ -1129,7 +1135,7 @@ void Octree<T,use_pairs,AL>::_cull_convex(Octant *p_octant,_CullConvexData *p_cu template<class T,bool use_pairs,class AL> -void Octree<T,use_pairs,AL>::_cull_AABB(Octant *p_octant,const AABB& p_aabb, T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask) { +void Octree<T,use_pairs,AL>::_cull_AABB(Octant *p_octant,const Rect3& p_aabb, T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask) { if (*p_result_idx==p_result_max) return; //pointless @@ -1376,7 +1382,7 @@ int Octree<T,use_pairs,AL>::cull_convex(const Vector<Plane>& p_convex,T** p_resu template<class T,bool use_pairs,class AL> -int Octree<T,use_pairs,AL>::cull_AABB(const AABB& p_aabb,T** p_result_array,int p_result_max,int *p_subindex_array,uint32_t p_mask) { +int Octree<T,use_pairs,AL>::cull_AABB(const Rect3& p_aabb,T** p_result_array,int p_result_max,int *p_subindex_array,uint32_t p_mask) { if (!root) diff --git a/core/math/pcg.cpp b/core/math/pcg.cpp new file mode 100644 index 0000000000..eac3b36d36 --- /dev/null +++ b/core/math/pcg.cpp @@ -0,0 +1,15 @@ +// *Really* minimal PCG32 code / (c) 2014 M.E. O'Neill / pcg-random.org +// Licensed under Apache License 2.0 (NO WARRANTY, etc. see website) + +#include "pcg.h" + +uint32_t pcg32_random_r(pcg32_random_t* rng) +{ + uint64_t oldstate = rng->state; + // Advance internal state + rng->state = oldstate * 6364136223846793005ULL + (rng->inc|1); + // Calculate output function (XSH RR), uses old state for max ILP + uint32_t xorshifted = ((oldstate >> 18u) ^ oldstate) >> 27u; + uint32_t rot = oldstate >> 59u; + return (xorshifted >> rot) | (xorshifted << ((-rot) & 31)); +} diff --git a/core/math/pcg.h b/core/math/pcg.h new file mode 100644 index 0000000000..81f4c9770e --- /dev/null +++ b/core/math/pcg.h @@ -0,0 +1,14 @@ +// *Really* minimal PCG32 code / (c) 2014 M.E. O'Neill / pcg-random.org +// Licensed under Apache License 2.0 (NO WARRANTY, etc. see website) + +#ifndef RANDOM_H +#define RANDOM_H + +#include "typedefs.h" + +#define PCG_DEFAULT_INC_64 1442695040888963407ULL + +typedef struct { uint64_t state; uint64_t inc; } pcg32_random_t; +uint32_t pcg32_random_r(pcg32_random_t* rng); + +#endif // RANDOM_H diff --git a/core/math/plane.cpp b/core/math/plane.cpp index b29350fe3c..2a97932049 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/plane.h b/core/math/plane.h index 81a968682e..8235c59135 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -99,7 +99,7 @@ real_t Plane::distance_to(const Vector3 &p_point) const { bool Plane::has_point(const Vector3 &p_point,real_t _epsilon) const { - float dist=normal.dot(p_point) - d; + real_t dist=normal.dot(p_point) - d; dist=ABS(dist); return ( dist <= _epsilon); diff --git a/core/math/quat.cpp b/core/math/quat.cpp index 73124e5e8e..4085f9b84a 100644 --- a/core/math/quat.cpp +++ b/core/math/quat.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -27,22 +27,40 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "quat.h" +#include "matrix3.h" #include "print_string.h" +// set_euler expects a vector containing the Euler angles in the format +// (c,b,a), where a is the angle of the first rotation, and c is the last. +// The current implementation uses XYZ convention (Z is the first rotation). void Quat::set_euler(const Vector3& p_euler) { - real_t half_yaw = p_euler.x * 0.5; - real_t half_pitch = p_euler.y * 0.5; - real_t half_roll = p_euler.z * 0.5; - real_t cos_yaw = Math::cos(half_yaw); - real_t sin_yaw = Math::sin(half_yaw); - real_t cos_pitch = Math::cos(half_pitch); - real_t sin_pitch = Math::sin(half_pitch); - real_t cos_roll = Math::cos(half_roll); - real_t sin_roll = Math::sin(half_roll); - set(cos_roll * sin_pitch * cos_yaw+sin_roll * cos_pitch * sin_yaw, - cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw, - sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw, - cos_roll * cos_pitch * cos_yaw+sin_roll * sin_pitch * sin_yaw); + 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 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. +// The current implementation uses XYZ convention (Z is the first rotation). +Vector3 Quat::get_euler() const { + Basis m(*this); + return m.get_euler(); } void Quat::operator*=(const Quat& q) { @@ -106,8 +124,8 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const { // Standard case (slerp) real_t sine = Math::sqrt(1 - cosine*cosine); real_t angle = Math::atan2(sine, cosine); - real_t inv_sine = 1.0f / sine; - real_t coeff_0 = Math::sin((1.0f - t) * angle) * inv_sine; + real_t inv_sine = 1.0 / sine; + real_t coeff_0 = Math::sin((1.0 - t) * angle) * inv_sine; real_t coeff_1 = Math::sin(t * angle) * inv_sine; Quat ret= src * coeff_0 + dst * coeff_1; @@ -119,33 +137,32 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const { // 2. "rkP" and "q" are almost invedste of each other (cosine ~= -1), there // are an infinite number of possibilities interpolation. but we haven't // have method to fix this case, so just use linear interpolation here. - Quat ret = src * (1.0f - t) + dst *t; + Quat ret = src * (1.0 - t) + dst *t; // taking the complement requires renormalisation ret.normalize(); return ret; } #else - real_t to1[4]; + Quat to1; real_t omega, cosom, sinom, scale0, scale1; // calc cosine - cosom = x * q.x + y * q.y + z * q.z - + w * q.w; - + cosom = dot(q); // adjust signs (if necessary) if ( cosom <0.0 ) { - cosom = -cosom; to1[0] = - q.x; - to1[1] = - q.y; - to1[2] = - q.z; - to1[3] = - q.w; + cosom = -cosom; + to1.x = - q.x; + to1.y = - q.y; + to1.z = - q.z; + to1.w = - q.w; } else { - to1[0] = q.x; - to1[1] = q.y; - to1[2] = q.z; - to1[3] = q.w; + to1.x = q.x; + to1.y = q.y; + to1.z = q.z; + to1.w = q.w; } @@ -165,10 +182,10 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const { } // calculate final values return Quat( - scale0 * x + scale1 * to1[0], - scale0 * y + scale1 * to1[1], - scale0 * z + scale1 * to1[2], - scale0 * w + scale1 * to1[3] + scale0 * x + scale1 * to1.x, + scale0 * y + scale1 * to1.y, + scale0 * z + scale1 * to1.z, + scale0 * w + scale1 * to1.w ); #endif } @@ -177,19 +194,19 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const { const Quat &from = *this; - float dot = from.dot(q); + real_t dot = from.dot(q); - if (Math::absf(dot) > 0.9999f) return from; + if (Math::absf(dot) > 0.9999) return from; - float theta = Math::acos(dot), - sinT = 1.0f / Math::sin(theta), + real_t theta = Math::acos(dot), + sinT = 1.0 / Math::sin(theta), newFactor = Math::sin(t * theta) * sinT, - invFactor = Math::sin((1.0f - t) * theta) * sinT; + invFactor = Math::sin((1.0 - t) * theta) * sinT; - return Quat( invFactor * from.x + newFactor * q.x, - invFactor * from.y + newFactor * q.y, - invFactor * from.z + newFactor * q.z, - invFactor * from.w + newFactor * q.w ); + return Quat(invFactor * from.x + newFactor * q.x, + invFactor * from.y + newFactor * q.y, + invFactor * from.z + newFactor * q.z, + invFactor * from.w + newFactor * q.w); #if 0 real_t to1[4]; @@ -203,7 +220,7 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const { // adjust signs (if necessary) if ( cosom <0.0 && false) { - cosom = -cosom; to1[0] = - q.x; + cosom = -cosom;to1[0] = - q.x; to1[1] = - q.y; to1[2] = - q.z; to1[3] = - q.w; @@ -242,7 +259,7 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const { Quat Quat::cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq,const real_t& t) const { //the only way to do slerp :| - float t2 = (1.0-t)*t*2; + real_t t2 = (1.0-t)*t*2; Quat sp = this->slerp(q,t); Quat sq = prep.slerpni(postq,t); return sp.slerpni(sq,t2); @@ -260,8 +277,10 @@ Quat::Quat(const Vector3& axis, const real_t& angle) { if (d==0) set(0,0,0,0); else { - real_t s = Math::sin(-angle * 0.5) / d; + 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, - Math::cos(-angle * 0.5)); + cos_angle); } } diff --git a/core/math/quat.h b/core/math/quat.h index 0d206bb3b7..d3a50343a3 100644 --- a/core/math/quat.h +++ b/core/math/quat.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -26,13 +26,15 @@ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ + +#include "vector3.h" + #ifndef QUAT_H #define QUAT_H #include "math_defs.h" #include "math_funcs.h" #include "ustring.h" -#include "vector3.h" /** @author Juan Linietsky <reduzio@gmail.com> @@ -49,15 +51,16 @@ public: Quat inverse() const; _FORCE_INLINE_ real_t dot(const Quat& q) const; void set_euler(const Vector3& p_euler); + Vector3 get_euler() const; Quat slerp(const Quat& q, const real_t& t) const; Quat slerpni(const Quat& q, const real_t& t) const; Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq,const real_t& t) const; _FORCE_INLINE_ void get_axis_and_angle(Vector3& r_axis, real_t &r_angle) const { r_angle = 2 * Math::acos(w); - r_axis.x = -x / Math::sqrt(1-w*w); - r_axis.y = -y / Math::sqrt(1-w*w); - r_axis.z = -z / Math::sqrt(1-w*w); + r_axis.x = x / Math::sqrt(1-w*w); + r_axis.y = y / Math::sqrt(1-w*w); + r_axis.z = z / Math::sqrt(1-w*w); } void operator*=(const Quat& q); @@ -116,8 +119,8 @@ public: w=0; } else { - real_t s = Math::sqrt((1.0f + d) * 2.0f); - real_t rs = 1.0f / s; + real_t s = Math::sqrt((1.0 + d) * 2.0); + real_t rs = 1.0 / s; x=c.x*rs; y=c.y*rs; @@ -183,12 +186,10 @@ Quat Quat::operator/(const real_t& s) const { 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; } diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index 956824d3d0..32fc0e01e8 100644 --- a/core/math/quick_hull.cpp +++ b/core/math/quick_hull.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -38,7 +38,7 @@ Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_me /* CREATE AABB VOLUME */ - AABB aabb; + Rect3 aabb; for(int i=0;i<p_points.size();i++) { if (i==0) { @@ -86,7 +86,7 @@ Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_me if (!valid_points[i]) continue; - float d = p_points[i][longest_axis]; + real_t d = p_points[i][longest_axis]; if (i==0 || d < min) { simplex[0]=i; @@ -105,7 +105,7 @@ Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_me { - float maxd; + real_t maxd; Vector3 rel12 = p_points[simplex[0]] - p_points[simplex[1]]; for(int i=0;i<p_points.size();i++) { @@ -127,7 +127,7 @@ Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_me //fourth vertex is the one most further away from the plane { - float maxd; + real_t maxd; Plane p(p_points[simplex[0]],p_points[simplex[1]],p_points[simplex[2]]); for(int i=0;i<p_points.size();i++) { @@ -483,7 +483,7 @@ Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_me //fill mesh r_mesh.faces.clear(); r_mesh.faces.resize(ret_faces.size()); -// print_line("FACECOUNT: "+itos(r_mesh.faces.size())); + //print_line("FACECOUNT: "+itos(r_mesh.faces.size())); int idx=0; for (List<Geometry::MeshData::Face>::Element *E=ret_faces.front();E;E=E->next()) { diff --git a/core/math/quick_hull.h b/core/math/quick_hull.h index 8c009b907d..7bd23d31f2 100644 --- a/core/math/quick_hull.h +++ b/core/math/quick_hull.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -29,7 +29,7 @@ #ifndef QUICK_HULL_H #define QUICK_HULL_H -#include "aabb.h" +#include "rect3.h" #include "set.h" #include "list.h" #include "geometry.h" diff --git a/core/math/aabb.cpp b/core/math/rect3.cpp index 6d8a5a72f0..d3f95b89e8 100644 --- a/core/math/aabb.cpp +++ b/core/math/rect3.cpp @@ -1,11 +1,11 @@ /*************************************************************************/ -/* aabb.cpp */ +/* rect3.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -26,28 +26,28 @@ /* 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 "rect3.h" #include "print_string.h" -float AABB::get_area() const { +real_t Rect3::get_area() const { return size.x*size.y*size.z; } -bool AABB::operator==(const AABB& p_rval) const { +bool Rect3::operator==(const Rect3& p_rval) const { return ((pos==p_rval.pos) && (size==p_rval.size)); } -bool AABB::operator!=(const AABB& p_rval) const { +bool Rect3::operator!=(const Rect3& p_rval) const { return ((pos!=p_rval.pos) || (size!=p_rval.size)); } -void AABB::merge_with(const AABB& p_aabb) { +void Rect3::merge_with(const Rect3& p_aabb) { Vector3 beg_1,beg_2; Vector3 end_1,end_2; @@ -70,7 +70,7 @@ void AABB::merge_with(const AABB& p_aabb) { size=max-min; } -AABB AABB::intersection(const AABB& p_aabb) const { +Rect3 Rect3::intersection(const Rect3& p_aabb) const { Vector3 src_min=pos; Vector3 src_max=pos+size; @@ -80,7 +80,7 @@ AABB AABB::intersection(const AABB& p_aabb) const { Vector3 min,max; if (src_min.x > dst_max.x || src_max.x < dst_min.x ) - return AABB(); + return Rect3(); else { min.x= ( src_min.x > dst_min.x ) ? src_min.x :dst_min.x; @@ -89,7 +89,7 @@ AABB AABB::intersection(const AABB& p_aabb) const { } if (src_min.y > dst_max.y || src_max.y < dst_min.y ) - return AABB(); + return Rect3(); else { min.y= ( src_min.y > dst_min.y ) ? src_min.y :dst_min.y; @@ -98,7 +98,7 @@ AABB AABB::intersection(const AABB& p_aabb) const { } if (src_min.z > dst_max.z || src_max.z < dst_min.z ) - return AABB(); + return Rect3(); else { min.z= ( src_min.z > dst_min.z ) ? src_min.z :dst_min.z; @@ -107,15 +107,15 @@ AABB AABB::intersection(const AABB& p_aabb) const { } - return AABB( min, max-min ); + return Rect3( min, max-min ); } -bool AABB::intersects_ray(const Vector3& p_from, const Vector3& p_dir,Vector3* r_clip,Vector3* r_normal) const { +bool Rect3::intersects_ray(const Vector3& p_from, const Vector3& p_dir,Vector3* r_clip,Vector3* r_normal) const { Vector3 c1, c2; Vector3 end = pos+size; - float near=-1e20; - float far=1e20; + real_t near=-1e20; + real_t far=1e20; int axis=0; for (int i=0;i<3;i++){ @@ -155,11 +155,11 @@ bool AABB::intersects_ray(const Vector3& p_from, const Vector3& p_dir,Vector3* r } -bool AABB::intersects_segment(const Vector3& p_from, const Vector3& p_to,Vector3* r_clip,Vector3* r_normal) const { +bool Rect3::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; - float sign=0; + real_t sign=0; for(int i=0;i<3;i++) { real_t seg_from=p_from[i]; @@ -167,7 +167,7 @@ bool AABB::intersects_segment(const Vector3& p_from, const Vector3& p_to,Vector3 real_t box_begin=pos[i]; real_t box_end=box_begin+size[i]; real_t cmin,cmax; - float csign; + real_t csign; if (seg_from < seg_to) { @@ -216,7 +216,7 @@ bool AABB::intersects_segment(const Vector3& p_from, const Vector3& p_to,Vector3 } -bool AABB::intersects_plane(const Plane &p_plane) const { +bool Rect3::intersects_plane(const Plane &p_plane) const { Vector3 points[8] = { Vector3( pos.x , pos.y , pos.z ), @@ -246,7 +246,7 @@ bool AABB::intersects_plane(const Plane &p_plane) const { -Vector3 AABB::get_longest_axis() const { +Vector3 Rect3::get_longest_axis() const { Vector3 axis(1,0,0); real_t max_size=size.x; @@ -263,7 +263,7 @@ Vector3 AABB::get_longest_axis() const { return axis; } -int AABB::get_longest_axis_index() const { +int Rect3::get_longest_axis_index() const { int axis=0; real_t max_size=size.x; @@ -282,7 +282,7 @@ int AABB::get_longest_axis_index() const { } -Vector3 AABB::get_shortest_axis() const { +Vector3 Rect3::get_shortest_axis() const { Vector3 axis(1,0,0); real_t max_size=size.x; @@ -299,7 +299,7 @@ Vector3 AABB::get_shortest_axis() const { return axis; } -int AABB::get_shortest_axis_index() const { +int Rect3::get_shortest_axis_index() const { int axis=0; real_t max_size=size.x; @@ -317,26 +317,26 @@ int AABB::get_shortest_axis_index() const { return axis; } -AABB AABB::merge(const AABB& p_with) const { +Rect3 Rect3::merge(const Rect3& p_with) const { - AABB aabb=*this; + Rect3 aabb=*this; aabb.merge_with(p_with); return aabb; } -AABB AABB::expand(const Vector3& p_vector) const { - AABB aabb=*this; +Rect3 Rect3::expand(const Vector3& p_vector) const { + Rect3 aabb=*this; aabb.expand_to(p_vector); return aabb; } -AABB AABB::grow(real_t p_by) const { +Rect3 Rect3::grow(real_t p_by) const { - AABB aabb=*this; + Rect3 aabb=*this; aabb.grow_by(p_by); return aabb; } -void AABB::get_edge(int p_edge,Vector3& r_from,Vector3& r_to) const { +void Rect3::get_edge(int p_edge,Vector3& r_from,Vector3& r_to) const { ERR_FAIL_INDEX(p_edge,12); switch(p_edge) { @@ -412,7 +412,7 @@ void AABB::get_edge(int p_edge,Vector3& r_from,Vector3& r_to) const { } -AABB::operator String() const { +Rect3::operator String() const { return String()+pos +" - "+ size; } diff --git a/core/math/aabb.h b/core/math/rect3.h index 57fe1b32f5..902592b02c 100644 --- a/core/math/aabb.h +++ b/core/math/rect3.h @@ -1,11 +1,11 @@ /*************************************************************************/ -/* aabb.h */ +/* rect3.h */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,6 +33,8 @@ #include "vector3.h" #include "plane.h" +#include "math_defs.h" + /** * AABB / AABB (Axis Aligned Bounding Box) * This is implemented by a point (pos) and the box size @@ -40,12 +42,12 @@ -class AABB { +class Rect3 { public: Vector3 pos; Vector3 size; - float get_area() const; /// get area + real_t get_area() const; /// get area _FORCE_INLINE_ bool has_no_area() const { return (size.x<=CMP_EPSILON || size.y<=CMP_EPSILON || size.z<=CMP_EPSILON); @@ -62,19 +64,19 @@ public: void set_size(const Vector3& p_size) { size=p_size; } - bool operator==(const AABB& p_rval) const; - bool operator!=(const AABB& p_rval) const; + bool operator==(const Rect3& p_rval) const; + bool operator!=(const Rect3& p_rval) 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 + _FORCE_INLINE_ bool intersects(const Rect3& p_aabb) const; /// Both AABBs overlap + _FORCE_INLINE_ bool intersects_inclusive(const Rect3& p_aabb) const; /// Both AABBs (or their faces) overlap + _FORCE_INLINE_ bool encloses(const Rect3 & 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 + Rect3 merge(const Rect3& p_with) const; + void merge_with(const Rect3& p_aabb); ///merge with another AABB + Rect3 intersection(const Rect3& 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=NULL,Vector3* r_normal=NULL) const; bool intersects_ray(const Vector3& p_from, const Vector3& p_dir,Vector3* r_clip=NULL,Vector3* r_normal=NULL) const; - _FORCE_INLINE_ bool smits_intersect_ray(const Vector3 &from,const Vector3& p_dir, float t0, float t1) const; + _FORCE_INLINE_ bool smits_intersect_ray(const Vector3 &from,const Vector3& p_dir, real_t t0, real_t t1) const; _FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_plane, int p_plane_count) const; bool intersects_plane(const Plane &p_plane) const; @@ -91,25 +93,25 @@ public: int get_shortest_axis_index() const; _FORCE_INLINE_ real_t get_shortest_axis_size() const; - AABB grow(real_t p_by) const; + Rect3 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,float &r_min,float& r_max) const; + Rect3 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 necesary */ operator String() const; - _FORCE_INLINE_ AABB() {} - inline AABB(const Vector3 &p_pos,const Vector3& p_size) { pos=p_pos; size=p_size; } + _FORCE_INLINE_ Rect3() {} + inline Rect3(const Vector3 &p_pos,const Vector3& p_size) { pos=p_pos; size=p_size; } }; -inline bool AABB::intersects(const AABB& p_aabb) const { +inline bool Rect3::intersects(const Rect3& p_aabb) const { if ( pos.x >= (p_aabb.pos.x + p_aabb.size.x) ) return false; @@ -127,7 +129,7 @@ inline bool AABB::intersects(const AABB& p_aabb) const { return true; } -inline bool AABB::intersects_inclusive(const AABB& p_aabb) const { +inline bool Rect3::intersects_inclusive(const Rect3& p_aabb) const { if ( pos.x > (p_aabb.pos.x + p_aabb.size.x) ) return false; @@ -145,7 +147,7 @@ inline bool AABB::intersects_inclusive(const AABB& p_aabb) const { return true; } -inline bool AABB::encloses(const AABB & p_aabb) const { +inline bool Rect3::encloses(const Rect3 & p_aabb) const { Vector3 src_min=pos; Vector3 src_max=pos+size; @@ -162,7 +164,7 @@ inline bool AABB::encloses(const AABB & p_aabb) const { } -Vector3 AABB::get_support(const Vector3& p_normal) const { +Vector3 Rect3::get_support(const Vector3& p_normal) const { Vector3 half_extents = size * 0.5; Vector3 ofs = pos + half_extents; @@ -175,7 +177,7 @@ Vector3 AABB::get_support(const Vector3& p_normal) const { } -Vector3 AABB::get_endpoint(int p_point) const { +Vector3 Rect3::get_endpoint(int p_point) const { switch(p_point) { case 0: return Vector3( pos.x , pos.y , pos.z ); @@ -191,7 +193,7 @@ Vector3 AABB::get_endpoint(int p_point) const { ERR_FAIL_V(Vector3()); } -bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count) const { +bool Rect3::intersects_convex_shape(const Plane *p_planes, int p_plane_count) const { #if 1 @@ -251,7 +253,7 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count) con #endif } -bool AABB::has_point(const Vector3& p_point) const { +bool Rect3::has_point(const Vector3& p_point) const { if (p_point.x<pos.x) return false; @@ -270,7 +272,7 @@ bool AABB::has_point(const Vector3& p_point) const { } -inline void AABB::expand_to(const Vector3& p_vector) { +inline void Rect3::expand_to(const Vector3& p_vector) { Vector3 begin=pos; Vector3 end=pos+size; @@ -293,18 +295,18 @@ inline void AABB::expand_to(const Vector3& p_vector) { size=end-begin; } -void AABB::project_range_in_plane(const Plane& p_plane,float &r_min,float& r_max) const { +void Rect3::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( pos.x + half_extents.x, pos.y + half_extents.y, pos.z + half_extents.z ); - float length = p_plane.normal.abs().dot(half_extents); - float distance = p_plane.distance_to( center ); + 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 { +inline real_t Rect3::get_longest_axis_size() const { real_t max_size=size.x; @@ -319,7 +321,7 @@ inline real_t AABB::get_longest_axis_size() const { return max_size; } -inline real_t AABB::get_shortest_axis_size() const { +inline real_t Rect3::get_shortest_axis_size() const { real_t max_size=size.x; @@ -334,14 +336,14 @@ inline real_t AABB::get_shortest_axis_size() const { return max_size; } -bool AABB::smits_intersect_ray(const Vector3 &from,const Vector3& dir, float t0, float t1) const { +bool Rect3::smits_intersect_ray(const Vector3 &from,const Vector3& dir, real_t t0, real_t t1) const { - float divx=1.0/dir.x; - float divy=1.0/dir.y; - float divz=1.0/dir.z; + real_t divx=1.0/dir.x; + real_t divy=1.0/dir.y; + real_t divz=1.0/dir.z; Vector3 upbound=pos+size; - float tmin, tmax, tymin, tymax, tzmin, tzmax; + real_t tmin, tmax, tymin, tymax, tzmin, tzmax; if (dir.x >= 0) { tmin = (pos.x - from.x) * divx; tmax = (upbound.x - from.x) * divx; @@ -381,7 +383,7 @@ bool AABB::smits_intersect_ray(const Vector3 &from,const Vector3& dir, float t0, return ( (tmin < t1) && (tmax > t0) ); } -void AABB::grow_by(real_t p_amount) { +void Rect3::grow_by(real_t p_amount) { pos.x-=p_amount; pos.y-=p_amount; @@ -391,6 +393,5 @@ void AABB::grow_by(real_t p_amount) { size.z+=2.0*p_amount; } -typedef AABB Rect3; #endif // AABB_H diff --git a/core/math/transform.cpp b/core/math/transform.cpp index 22eb6c4fdd..6d9324c176 100644 --- a/core/math/transform.cpp +++ b/core/math/transform.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -54,7 +54,8 @@ void Transform::invert() { } 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; @@ -63,12 +64,12 @@ Transform Transform::inverse() const { void Transform::rotate(const Vector3& p_axis,real_t p_phi) { - *this = *this * Transform( Matrix3( p_axis, p_phi ), Vector3() ); + *this = rotated(p_axis, p_phi); } Transform Transform::rotated(const Vector3& p_axis,real_t p_phi) const{ - return *this * Transform( Matrix3( p_axis, p_phi ), Vector3() ); + return Transform(Basis( p_axis, p_phi ), Vector3()) * (*this); } void Transform::rotate_basis(const Vector3& p_axis,real_t p_phi) { @@ -113,7 +114,7 @@ void Transform::set_look_at( const Vector3& p_eye, const Vector3& p_target, cons } -Transform Transform::interpolate_with(const Transform& p_transform, float p_c) const { +Transform Transform::interpolate_with(const Transform& p_transform, real_t p_c) const { /* not sure if very "efficient" but good enough? */ @@ -209,7 +210,7 @@ Transform::operator String() const { } -Transform::Transform(const Matrix3& p_basis, const Vector3& p_origin) { +Transform::Transform(const Basis& p_basis, const Vector3& p_origin) { basis=p_basis; origin=p_origin; diff --git a/core/math/transform.h b/core/math/transform.h index f948a4c919..45d7b2a12c 100644 --- a/core/math/transform.h +++ b/core/math/transform.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,14 +31,14 @@ #include "matrix3.h" #include "plane.h" -#include "aabb.h" +#include "rect3.h" /** @author Juan Linietsky <reduzio@gmail.com> */ class Transform { public: - Matrix3 basis; + Basis basis; Vector3 origin; void invert(); @@ -62,8 +62,8 @@ public: void translate( const Vector3& p_translation ); Transform translated( const Vector3& p_translation ) const; - const Matrix3& get_basis() const { return basis; } - void set_basis(const Matrix3& p_basis) { basis=p_basis; } + 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; } @@ -80,13 +80,13 @@ public: _FORCE_INLINE_ Plane xform(const Plane& p_plane) const; _FORCE_INLINE_ Plane xform_inv(const Plane& p_plane) const; - _FORCE_INLINE_ AABB xform(const AABB& p_aabb) const; - _FORCE_INLINE_ AABB xform_inv(const AABB& p_aabb) const; + _FORCE_INLINE_ Rect3 xform(const Rect3& p_aabb) const; + _FORCE_INLINE_ Rect3 xform_inv(const Rect3& p_aabb) const; void operator*=(const Transform& p_transform); Transform operator*(const Transform& p_transform) const; - Transform interpolate_with(const Transform& p_transform, float p_c) const; + Transform interpolate_with(const Transform& p_transform, real_t p_c) const; _FORCE_INLINE_ Transform inverse_xform(const Transform& t) const { @@ -113,7 +113,7 @@ public: operator String() const; - Transform(const Matrix3& p_basis, const Vector3& p_origin=Vector3()); + Transform(const Basis& p_basis, const Vector3& p_origin=Vector3()); Transform() {} }; @@ -168,7 +168,7 @@ _FORCE_INLINE_ Plane Transform::xform_inv(const Plane& p_plane) const { } -_FORCE_INLINE_ AABB Transform::xform(const AABB& p_aabb) const { +_FORCE_INLINE_ Rect3 Transform::xform(const Rect3& p_aabb) const { /* define vertices */ #if 1 Vector3 x=basis.get_axis(0)*p_aabb.size.x; @@ -176,7 +176,7 @@ _FORCE_INLINE_ AABB Transform::xform(const AABB& p_aabb) const { Vector3 z=basis.get_axis(2)*p_aabb.size.z; Vector3 pos = xform( p_aabb.pos ); //could be even further optimized - AABB new_aabb; + Rect3 new_aabb; new_aabb.pos=pos; new_aabb.expand_to( pos+x ); new_aabb.expand_to( pos+y ); @@ -214,7 +214,7 @@ _FORCE_INLINE_ AABB Transform::xform(const AABB& p_aabb) const { #endif } -_FORCE_INLINE_ AABB Transform::xform_inv(const AABB& p_aabb) const { +_FORCE_INLINE_ Rect3 Transform::xform_inv(const Rect3& p_aabb) const { /* define vertices */ Vector3 vertices[8]={ @@ -229,7 +229,7 @@ _FORCE_INLINE_ AABB Transform::xform_inv(const AABB& p_aabb) const { }; - AABB ret; + Rect3 ret; ret.pos=xform_inv(vertices[0]); diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 7aea32a8a0..247cb90a48 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -48,7 +48,7 @@ int TriangleMesh::_create_bvh(BVH*p_bvh,BVH** p_bb,int p_from,int p_size,int p_d } - AABB aabb; + Rect3 aabb; aabb=p_bb[p_from]->aabb; for(int i=1;i<p_size;i++) { @@ -94,7 +94,7 @@ int TriangleMesh::_create_bvh(BVH*p_bvh,BVH** p_bb,int p_from,int p_size,int p_d } -void TriangleMesh::create(const DVector<Vector3>& p_faces) { +void TriangleMesh::create(const PoolVector<Vector3>& p_faces) { valid=false; @@ -104,7 +104,7 @@ void TriangleMesh::create(const DVector<Vector3>& p_faces) { triangles.resize(fc); bvh.resize(fc*3); //will never be larger than this (todo make better) - DVector<BVH>::Write bw = bvh.write(); + PoolVector<BVH>::Write bw = bvh.write(); { @@ -112,8 +112,8 @@ void TriangleMesh::create(const DVector<Vector3>& p_faces) { //except for the Set for repeated triangles, everything //goes in-place. - DVector<Vector3>::Read r = p_faces.read(); - DVector<Triangle>::Write w = triangles.write(); + PoolVector<Vector3>::Read r = p_faces.read(); + PoolVector<Triangle>::Write w = triangles.write(); Map<Vector3,int> db; for(int i=0;i<fc;i++) { @@ -149,16 +149,16 @@ void TriangleMesh::create(const DVector<Vector3>& p_faces) { } vertices.resize(db.size()); - DVector<Vector3>::Write vw = vertices.write(); + PoolVector<Vector3>::Write vw = vertices.write(); for (Map<Vector3,int>::Element *E=db.front();E;E=E->next()) { vw[E->get()]=E->key(); } } - DVector<BVH*> bwptrs; + PoolVector<BVH*> bwptrs; bwptrs.resize(fc); - DVector<BVH*>::Write bwp = bwptrs.write(); + PoolVector<BVH*>::Write bwp = bwptrs.write(); for(int i=0;i<fc;i++) { bwp[i]=&bw[i]; @@ -168,7 +168,7 @@ void TriangleMesh::create(const DVector<Vector3>& p_faces) { int max_alloc=fc; int max=_create_bvh(bw.ptr(),bwp.ptr(),0,fc,1,max_depth,max_alloc); - bw=DVector<BVH>::Write(); //clearup + bw=PoolVector<BVH>::Write(); //clearup bvh.resize(max_alloc); //resize back valid=true; @@ -176,7 +176,7 @@ void TriangleMesh::create(const DVector<Vector3>& p_faces) { } -Vector3 TriangleMesh::get_area_normal(const AABB& p_aabb) const { +Vector3 TriangleMesh::get_area_normal(const Rect3& p_aabb) const { uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth); @@ -197,9 +197,9 @@ Vector3 TriangleMesh::get_area_normal(const AABB& p_aabb) const { int level=0; - DVector<Triangle>::Read trianglesr = triangles.read(); - DVector<Vector3>::Read verticesr=vertices.read(); - DVector<BVH>::Read bvhr=bvh.read(); + PoolVector<Triangle>::Read trianglesr = triangles.read(); + PoolVector<Vector3>::Read verticesr=vertices.read(); + PoolVector<BVH>::Read bvhr=bvh.read(); const Triangle *triangleptr=trianglesr.ptr(); int pos=bvh.size()-1; @@ -236,21 +236,22 @@ Vector3 TriangleMesh::get_area_normal(const AABB& p_aabb) const { stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; } } - - } continue; + continue; + } case VISIT_LEFT_BIT: { stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; stack[level+1]=b.left|TEST_AABB_BIT; level++; - - } continue; + continue; + } case VISIT_RIGHT_BIT: { stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; stack[level+1]=b.right|TEST_AABB_BIT; level++; - } continue; + continue; + } case VISIT_DONE_BIT: { if (level==0) { @@ -258,8 +259,8 @@ Vector3 TriangleMesh::get_area_normal(const AABB& p_aabb) const { break; } else level--; - - } continue; + continue; + } } @@ -299,9 +300,9 @@ bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end int level=0; - DVector<Triangle>::Read trianglesr = triangles.read(); - DVector<Vector3>::Read verticesr=vertices.read(); - DVector<BVH>::Read bvhr=bvh.read(); + PoolVector<Triangle>::Read trianglesr = triangles.read(); + PoolVector<Vector3>::Read verticesr=vertices.read(); + PoolVector<BVH>::Read bvhr=bvh.read(); const Triangle *triangleptr=trianglesr.ptr(); const Vector3 *vertexptr=verticesr.ptr(); @@ -320,7 +321,7 @@ bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end bool valid = b.aabb.intersects_segment(p_begin,p_end); -// bool valid = b.aabb.intersects(ray_aabb); + //bool valid = b.aabb.intersects(ray_aabb); if (!valid) { @@ -339,7 +340,7 @@ bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end if (f3.intersects_segment(p_begin,p_end,&res)) { - float nd = n.dot(res); + real_t nd = n.dot(res); if (nd<d) { d=nd; @@ -357,21 +358,22 @@ bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; } } - - } continue; + continue; + } case VISIT_LEFT_BIT: { stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; stack[level+1]=b.left|TEST_AABB_BIT; level++; - - } continue; + continue; + } case VISIT_RIGHT_BIT: { stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; stack[level+1]=b.right|TEST_AABB_BIT; level++; - } continue; + continue; + } case VISIT_DONE_BIT: { if (level==0) { @@ -379,8 +381,8 @@ bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end break; } else level--; - - } continue; + continue; + } } @@ -422,9 +424,9 @@ bool TriangleMesh::intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vec int level=0; - DVector<Triangle>::Read trianglesr = triangles.read(); - DVector<Vector3>::Read verticesr=vertices.read(); - DVector<BVH>::Read bvhr=bvh.read(); + PoolVector<Triangle>::Read trianglesr = triangles.read(); + PoolVector<Vector3>::Read verticesr=vertices.read(); + PoolVector<BVH>::Read bvhr=bvh.read(); const Triangle *triangleptr=trianglesr.ptr(); const Vector3 *vertexptr=verticesr.ptr(); @@ -460,7 +462,7 @@ bool TriangleMesh::intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vec if (f3.intersects_ray(p_begin,p_dir,&res)) { - float nd = n.dot(res); + real_t nd = n.dot(res); if (nd<d) { d=nd; @@ -478,21 +480,22 @@ bool TriangleMesh::intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vec stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; } } - - } continue; + continue; + } case VISIT_LEFT_BIT: { stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; stack[level+1]=b.left|TEST_AABB_BIT; level++; - - } continue; + continue; + } case VISIT_RIGHT_BIT: { stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; stack[level+1]=b.right|TEST_AABB_BIT; level++; - } continue; + continue; + } case VISIT_DONE_BIT: { if (level==0) { @@ -500,8 +503,8 @@ bool TriangleMesh::intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vec break; } else level--; - - } continue; + continue; + } } @@ -524,18 +527,18 @@ bool TriangleMesh::is_valid() const { return valid; } -DVector<Face3> TriangleMesh::get_faces() const { +PoolVector<Face3> TriangleMesh::get_faces() const { if (!valid) - return DVector<Face3>(); + return PoolVector<Face3>(); - DVector<Face3> faces; + PoolVector<Face3> faces; int ts = triangles.size(); faces.resize(triangles.size()); - DVector<Face3>::Write w=faces.write(); - DVector<Triangle>::Read r = triangles.read(); - DVector<Vector3>::Read rv = vertices.read(); + PoolVector<Face3>::Write w=faces.write(); + PoolVector<Triangle>::Read r = triangles.read(); + PoolVector<Vector3>::Read rv = vertices.read(); for(int i=0;i<ts;i++) { for(int j=0;j<3;j++) { @@ -543,7 +546,7 @@ DVector<Face3> TriangleMesh::get_faces() const { } } - w = DVector<Face3>::Write(); + w = PoolVector<Face3>::Write(); return faces; } diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index b5e8f79cde..65250c023d 100644 --- a/core/math/triangle_mesh.h +++ b/core/math/triangle_mesh.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -33,7 +33,7 @@ #include "face3.h" class TriangleMesh : public Reference { - OBJ_TYPE( TriangleMesh, Reference); + GDCLASS( TriangleMesh, Reference); struct Triangle { @@ -41,12 +41,12 @@ class TriangleMesh : public Reference { int indices[3]; }; - DVector<Triangle> triangles; - DVector<Vector3> vertices; + PoolVector<Triangle> triangles; + PoolVector<Vector3> vertices; struct BVH { - AABB aabb; + Rect3 aabb; Vector3 center; //used for sorting int left; int right; @@ -79,7 +79,7 @@ class TriangleMesh : public Reference { int _create_bvh(BVH*p_bvh,BVH** p_bb,int p_from,int p_size,int p_depth,int&max_depth,int&max_alloc); - DVector<BVH> bvh; + PoolVector<BVH> bvh; int max_depth; bool valid; @@ -88,11 +88,11 @@ public: bool is_valid() const; bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const; bool intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vector3 &r_point, Vector3 &r_normal) const; - Vector3 get_area_normal(const AABB& p_aabb) const; - DVector<Face3> get_faces() const; + Vector3 get_area_normal(const Rect3& p_aabb) const; + PoolVector<Face3> get_faces() const; - void create(const DVector<Vector3>& p_faces); + void create(const PoolVector<Vector3>& p_faces); TriangleMesh(); }; diff --git a/core/math/triangulate.cpp b/core/math/triangulate.cpp index 1f5d5ed6b3..128b6ca331 100644 --- a/core/math/triangulate.cpp +++ b/core/math/triangulate.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -28,19 +28,19 @@ /*************************************************************************/ #include "triangulate.h" -float Triangulate::get_area(const Vector<Vector2> &contour) +real_t Triangulate::get_area(const Vector<Vector2> &contour) { int n = contour.size(); const Vector2 *c=&contour[0]; - float A=0.0f; + real_t A=0.0; for(int p=n-1,q=0; q<n; p=q++) { A+= c[p].cross(c[q]); } - return A*0.5f; + return A*0.5; } /* @@ -48,14 +48,14 @@ float Triangulate::get_area(const Vector<Vector2> &contour) defined by A, B, C. */ -bool Triangulate::is_inside_triangle(float Ax, float Ay, - float Bx, float By, - float Cx, float Cy, - float Px, float Py) +bool Triangulate::is_inside_triangle(real_t Ax, real_t Ay, + real_t Bx, real_t By, + real_t Cx, real_t Cy, + real_t Px, real_t Py) { - float ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy; - float cCROSSap, bCROSScp, aCROSSbp; + real_t ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy; + real_t cCROSSap, bCROSScp, aCROSSbp; ax = Cx - Bx; ay = Cy - By; bx = Ax - Cx; by = Ay - Cy; @@ -68,13 +68,13 @@ bool Triangulate::is_inside_triangle(float Ax, float Ay, cCROSSap = cx*apy - cy*apx; bCROSScp = bx*cpy - by*cpx; - return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f)); + return ((aCROSSbp >= 0.0) && (bCROSScp >= 0.0) && (cCROSSap >= 0.0)); }; -bool Triangulate::snip(const Vector<Vector2> &p_contour,int u,int v,int w,int n,int *V) +bool Triangulate::snip(const Vector<Vector2> &p_contour,int u,int v,int w,int n,const Vector<int>& V) { int p; - float Ax, Ay, Bx, By, Cx, Cy, Px, Py; + real_t Ax, Ay, Bx, By, Cx, Cy, Px, Py; const Vector2 *contour=&p_contour[0]; Ax = contour[V[u]].x; @@ -107,12 +107,12 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour,Vector<int> &result if ( n < 3 ) return false; - - int *V = (int*)alloca(sizeof(int)*n); + Vector<int> V; + V.resize(n); /* we want a counter-clockwise polygon in V */ - if ( 0.0f < get_area(contour) ) + if ( 0.0 < get_area(contour) ) for (int v=0; v<n; v++) V[v] = v; else for(int v=0; v<n; v++) V[v] = (n-1)-v; @@ -122,7 +122,7 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour,Vector<int> &result /* remove nv-2 Vertices, creating 1 triangle every time */ int count = 2*nv; /* error detection */ - for(int m=0, v=nv-1; nv>2; ) + for(int v=nv-1; nv>2; ) { /* if we loop, it is probably a non-simple polygon */ if (0 >= (count--)) @@ -144,18 +144,10 @@ bool Triangulate::triangulate(const Vector<Vector2> &contour,Vector<int> &result a = V[u]; b = V[v]; c = V[w]; /* output Triangle */ - /* - result.push_back( contour[a] ); - result.push_back( contour[b] ); - result.push_back( contour[c] ); -*/ - result.push_back( a ); result.push_back( b ); result.push_back( c ); - m++; - /* remove v from remaining polygon */ for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; diff --git a/core/math/triangulate.h b/core/math/triangulate.h index 7dcc91b2fd..ce77334519 100644 --- a/core/math/triangulate.h +++ b/core/math/triangulate.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -45,18 +45,18 @@ public: static bool triangulate(const Vector< Vector2 > &contour, Vector<int> &result); // compute area of a contour/polygon - static float get_area(const Vector< Vector2 > &contour); + static real_t get_area(const Vector< Vector2 > &contour); // decide if point Px/Py is inside triangle defined by // (Ax,Ay) (Bx,By) (Cx,Cy) - static bool is_inside_triangle(float Ax, float Ay, - float Bx, float By, - float Cx, float Cy, - float Px, float Py); + static bool is_inside_triangle(real_t Ax, real_t Ay, + real_t Bx, real_t By, + real_t Cx, real_t Cy, + real_t Px, real_t Py); private: - static bool snip(const Vector<Vector2> &p_contour,int u,int v,int w,int n,int *V); + static bool snip(const Vector<Vector2> &p_contour,int u,int v,int w,int n,const Vector<int>& V); }; diff --git a/core/math/triangulator.cpp b/core/math/triangulator.cpp index 8f82d76823..75b2b064c4 100644 --- a/core/math/triangulator.cpp +++ b/core/math/triangulator.cpp @@ -1128,7 +1128,7 @@ int TriangulatorPartition::MonotonePartition(List<TriangulatorPoly> *inpolys, Li //this makes deleting existing edges much faster Set<ScanLineEdge>::Element **edgeTreeIterators,*edgeIter; edgeTreeIterators = new Set<ScanLineEdge>::Element*[maxnumvertices]; -// Pair<Set<ScanLineEdge>::Element*,bool> edgeTreeRet; + //Pair<Set<ScanLineEdge>::Element*,bool> edgeTreeRet; for(i = 0; i<numvertices; i++) edgeTreeIterators[i] = NULL; //for each vertex diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index fae3831dd6..2ab5fa0465 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -30,12 +30,12 @@ #include "matrix3.h" -void Vector3::rotate(const Vector3& p_axis,float p_phi) { +void Vector3::rotate(const Vector3& p_axis,real_t p_phi) { - *this=Matrix3(p_axis,p_phi).xform(*this); + *this=Basis(p_axis,p_phi).xform(*this); } -Vector3 Vector3::rotated(const Vector3& p_axis,float p_phi) const { +Vector3 Vector3::rotated(const Vector3& p_axis,real_t p_phi) const { Vector3 r = *this; r.rotate(p_axis,p_phi); @@ -63,13 +63,13 @@ int Vector3::max_axis() const { } -void Vector3::snap(float p_val) { +void Vector3::snap(real_t p_val) { x=Math::stepify(x,p_val); y=Math::stepify(y,p_val); z=Math::stepify(z,p_val); } -Vector3 Vector3::snapped(float p_val) const { +Vector3 Vector3::snapped(real_t p_val) const { Vector3 v=*this; v.snap(p_val); @@ -77,7 +77,7 @@ Vector3 Vector3::snapped(float p_val) const { } -Vector3 Vector3::cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const { +Vector3 Vector3::cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,real_t p_t) const { Vector3 p0=p_pre_a; Vector3 p1=*this; @@ -87,9 +87,9 @@ Vector3 Vector3::cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, c { //normalize - float ab = p0.distance_to(p1); - float bc = p1.distance_to(p2); - float cd = p2.distance_to(p3); + real_t ab = p0.distance_to(p1); + real_t bc = p1.distance_to(p2); + real_t cd = p2.distance_to(p3); if (ab>0) p0 = p1+(p0-p1)*(bc/ab); @@ -98,41 +98,41 @@ Vector3 Vector3::cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, c } - float t = p_t; - float t2 = t * t; - float t3 = t2 * t; + real_t t = p_t; + real_t t2 = t * t; + real_t t3 = t2 * t; Vector3 out; - out = 0.5f * ( ( p1 * 2.0f) + + out = 0.5 * ( ( p1 * 2.0) + ( -p0 + p2 ) * t + - ( 2.0f * p0 - 5.0f * p1 + 4 * p2 - p3 ) * t2 + - ( -p0 + 3.0f * p1 - 3.0f * p2 + p3 ) * t3 ); + ( 2.0 * p0 - 5.0 * p1 + 4 * p2 - p3 ) * t2 + + ( -p0 + 3.0 * p1 - 3.0 * p2 + p3 ) * t3 ); return out; } -Vector3 Vector3::cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const { +Vector3 Vector3::cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,real_t p_t) const { Vector3 p0=p_pre_a; Vector3 p1=*this; Vector3 p2=p_b; Vector3 p3=p_post_b; - float t = p_t; - float t2 = t * t; - float t3 = t2 * t; + real_t t = p_t; + real_t t2 = t * t; + real_t t3 = t2 * t; Vector3 out; - out = 0.5f * ( ( p1 * 2.0f) + + out = 0.5 * ( ( p1 * 2.0) + ( -p0 + p2 ) * t + - ( 2.0f * p0 - 5.0f * p1 + 4 * p2 - p3 ) * t2 + - ( -p0 + 3.0f * p1 - 3.0f * p2 + p3 ) * t3 ); + ( 2.0 * p0 - 5.0 * p1 + 4 * p2 - p3 ) * t2 + + ( -p0 + 3.0 * p1 - 3.0 * p2 + p3 ) * t3 ); return out; } #if 0 -Vector3 Vector3::cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const { +Vector3 Vector3::cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,real_t p_t) const { Vector3 p0=p_pre_a; Vector3 p1=*this; @@ -141,9 +141,9 @@ Vector3 Vector3::cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, co if (true) { - float ab = p0.distance_to(p1); - float bc = p1.distance_to(p2); - float cd = p2.distance_to(p3); + real_t ab = p0.distance_to(p1); + real_t bc = p1.distance_to(p2); + real_t cd = p2.distance_to(p3); //if (ab>bc) { if (ab>0) @@ -156,23 +156,23 @@ Vector3 Vector3::cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, co //} } - float t = p_t; - float t2 = t * t; - float t3 = t2 * t; + real_t t = p_t; + real_t t2 = t * t; + real_t t3 = t2 * t; Vector3 out; - out.x = 0.5f * ( ( 2.0f * p1.x ) + + out.x = 0.5 * ( ( 2.0 * p1.x ) + ( -p0.x + p2.x ) * t + - ( 2.0f * p0.x - 5.0f * p1.x + 4 * p2.x - p3.x ) * t2 + - ( -p0.x + 3.0f * p1.x - 3.0f * p2.x + p3.x ) * t3 ); - out.y = 0.5f * ( ( 2.0f * p1.y ) + + ( 2.0 * p0.x - 5.0 * p1.x + 4 * p2.x - p3.x ) * t2 + + ( -p0.x + 3.0 * p1.x - 3.0 * p2.x + p3.x ) * t3 ); + out.y = 0.5 * ( ( 2.0 * p1.y ) + ( -p0.y + p2.y ) * t + - ( 2.0f * p0.y - 5.0f * p1.y + 4 * p2.y - p3.y ) * t2 + - ( -p0.y + 3.0f * p1.y - 3.0f * p2.y + p3.y ) * t3 ); - out.z = 0.5f * ( ( 2.0f * p1.z ) + + ( 2.0 * p0.y - 5.0 * p1.y + 4 * p2.y - p3.y ) * t2 + + ( -p0.y + 3.0 * p1.y - 3.0 * p2.y + p3.y ) * t3 ); + out.z = 0.5 * ( ( 2.0 * p1.z ) + ( -p0.z + p2.z ) * t + - ( 2.0f * p0.z - 5.0f * p1.z + 4 * p2.z - p3.z ) * t2 + - ( -p0.z + 3.0f * p1.z - 3.0f * p2.z + p3.z ) * t3 ); + ( 2.0 * p0.z - 5.0 * p1.z + 4 * p2.z - p3.z ) * t2 + + ( -p0.z + 3.0 * p1.z - 3.0 * p2.z + p3.z ) * t3 ); return out; } # endif diff --git a/core/math/vector3.h b/core/math/vector3.h index 06840be5e7..a289f9bf4c 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -34,6 +34,7 @@ #include "math_funcs.h" #include "ustring.h" +class Basis; struct Vector3 { @@ -78,20 +79,22 @@ struct Vector3 { _FORCE_INLINE_ void zero(); - void snap(float p_val); - Vector3 snapped(float p_val) const; + void snap(real_t p_val); + Vector3 snapped(real_t p_val) const; - void rotate(const Vector3& p_axis,float p_phi); - Vector3 rotated(const Vector3& p_axis,float p_phi) const; + void rotate(const Vector3& p_axis,real_t p_phi); + Vector3 rotated(const Vector3& p_axis,real_t p_phi) const; /* Static Methods between 2 vector3s */ - _FORCE_INLINE_ Vector3 linear_interpolate(const Vector3& p_b,float p_t) const; - Vector3 cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const; - Vector3 cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const; + _FORCE_INLINE_ Vector3 linear_interpolate(const Vector3& p_b,real_t p_t) const; + Vector3 cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,real_t p_t) const; + Vector3 cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,real_t p_t) const; _FORCE_INLINE_ Vector3 cross(const Vector3& p_b) const; _FORCE_INLINE_ real_t dot(const Vector3& p_b) const; + _FORCE_INLINE_ Basis outer(const Vector3& p_b) const; + _FORCE_INLINE_ Basis to_diagonal_matrix() const; _FORCE_INLINE_ Vector3 abs() const; _FORCE_INLINE_ Vector3 floor() const; @@ -144,6 +147,8 @@ struct Vector3 { #else +#include "matrix3.h" + Vector3 Vector3::cross(const Vector3& p_b) const { Vector3 ret ( @@ -160,6 +165,21 @@ real_t Vector3::dot(const Vector3& p_b) const { return x*p_b.x + y*p_b.y + z*p_b.z; } +Basis Vector3::outer(const Vector3& p_b) const { + + Vector3 row0(x*p_b.x, x*p_b.y, x*p_b.z); + Vector3 row1(y*p_b.x, y*p_b.y, y*p_b.z); + Vector3 row2(z*p_b.x, z*p_b.y, z*p_b.z); + + return Basis(row0, row1, row2); +} + +Basis Vector3::to_diagonal_matrix() const { + return Basis(x, 0, 0, + 0, y, 0, + 0, 0, z); +} + Vector3 Vector3::abs() const { return Vector3( Math::abs(x), Math::abs(y), Math::abs(z) ); @@ -175,7 +195,7 @@ Vector3 Vector3::ceil() const { return Vector3( Math::ceil(x), Math::ceil(y), Math::ceil(z) ); } -Vector3 Vector3::linear_interpolate(const Vector3& p_b,float p_t) const { +Vector3 Vector3::linear_interpolate(const Vector3& p_b,real_t p_t) const { return Vector3( x+(p_t * (p_b.x-x)), @@ -293,7 +313,6 @@ bool Vector3::operator==(const Vector3& p_v) const { } bool Vector3::operator!=(const Vector3& p_v) const { - return (x!=p_v.x || y!=p_v.y || z!=p_v.z); } |