From 0b806ee0fc9097fa7bda7ac0109191c9c5e0a1ac Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Sun, 9 Feb 2014 22:10:30 -0300 Subject: GODOT IS OPEN SOURCE --- core/math/SCsub | 7 + core/math/aabb.cpp | 427 +++++++++++++ core/math/aabb.h | 319 ++++++++++ core/math/bezier_curve.cpp | 29 + core/math/bezier_curve.h | 35 ++ core/math/bsp_tree.cpp | 628 +++++++++++++++++++ core/math/bsp_tree.h | 141 +++++ core/math/camera_matrix.cpp | 596 ++++++++++++++++++ core/math/camera_matrix.h | 106 ++++ core/math/face3.cpp | 359 +++++++++++ core/math/face3.h | 97 +++ core/math/geometry.cpp | 1006 +++++++++++++++++++++++++++++ core/math/geometry.h | 755 ++++++++++++++++++++++ core/math/math_2d.cpp | 597 ++++++++++++++++++ core/math/math_2d.h | 670 ++++++++++++++++++++ core/math/math_defs.cpp | 30 + core/math/math_defs.h | 60 ++ core/math/math_funcs.cpp | 328 ++++++++++ core/math/math_funcs.h | 180 ++++++ core/math/matrix3.cpp | 479 ++++++++++++++ core/math/matrix3.h | 230 +++++++ core/math/octree.h | 1461 +++++++++++++++++++++++++++++++++++++++++++ core/math/plane.cpp | 166 +++++ core/math/plane.h | 146 +++++ core/math/quat.cpp | 267 ++++++++ core/math/quat.h | 157 +++++ core/math/quick_hull.cpp | 513 +++++++++++++++ core/math/quick_hull.h | 95 +++ core/math/transform.cpp | 218 +++++++ core/math/transform.h | 268 ++++++++ core/math/triangle_mesh.cpp | 568 +++++++++++++++++ core/math/triangle_mesh.h | 99 +++ core/math/triangulate.cpp | 168 +++++ core/math/triangulate.h | 65 ++ core/math/vector3.cpp | 201 ++++++ core/math/vector3.h | 373 +++++++++++ 36 files changed, 11844 insertions(+) create mode 100644 core/math/SCsub create mode 100644 core/math/aabb.cpp create mode 100644 core/math/aabb.h create mode 100644 core/math/bezier_curve.cpp create mode 100644 core/math/bezier_curve.h create mode 100644 core/math/bsp_tree.cpp create mode 100644 core/math/bsp_tree.h create mode 100644 core/math/camera_matrix.cpp create mode 100644 core/math/camera_matrix.h create mode 100644 core/math/face3.cpp create mode 100644 core/math/face3.h create mode 100644 core/math/geometry.cpp create mode 100644 core/math/geometry.h create mode 100644 core/math/math_2d.cpp create mode 100644 core/math/math_2d.h create mode 100644 core/math/math_defs.cpp create mode 100644 core/math/math_defs.h create mode 100644 core/math/math_funcs.cpp create mode 100644 core/math/math_funcs.h create mode 100644 core/math/matrix3.cpp create mode 100644 core/math/matrix3.h create mode 100644 core/math/octree.h create mode 100644 core/math/plane.cpp create mode 100644 core/math/plane.h create mode 100644 core/math/quat.cpp create mode 100644 core/math/quat.h create mode 100644 core/math/quick_hull.cpp create mode 100644 core/math/quick_hull.h create mode 100644 core/math/transform.cpp create mode 100644 core/math/transform.h create mode 100644 core/math/triangle_mesh.cpp create mode 100644 core/math/triangle_mesh.h create mode 100644 core/math/triangulate.cpp create mode 100644 core/math/triangulate.h create mode 100644 core/math/vector3.cpp create mode 100644 core/math/vector3.h (limited to 'core/math') diff --git a/core/math/SCsub b/core/math/SCsub new file mode 100644 index 000000000..c6ba1fa53 --- /dev/null +++ b/core/math/SCsub @@ -0,0 +1,7 @@ +Import('env') + +env.add_source_files(env.core_sources,"*.cpp") + +Export('env') + + diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp new file mode 100644 index 000000000..0d454cd07 --- /dev/null +++ b/core/math/aabb.cpp @@ -0,0 +1,427 @@ +/*************************************************************************/ +/* aabb.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "aabb.h" + +#include "print_string.h" + +float AABB::get_area() const { + + return size.x*size.y*size.z; + +} + +bool AABB::operator==(const AABB& p_rval) const { + + return ((pos==p_rval.pos) && (size==p_rval.size)); + +} +bool AABB::operator!=(const AABB& p_rval) const { + + return ((pos!=p_rval.pos) || (size!=p_rval.size)); + +} + +void AABB::merge_with(const AABB& p_aabb) { + + Vector3 beg_1,beg_2; + Vector3 end_1,end_2; + Vector3 min,max; + + beg_1=pos; + beg_2=p_aabb.pos; + end_1=Vector3(size.x,size.y,size.z)+beg_1; + end_2=Vector3(p_aabb.size.x,p_aabb.size.y,p_aabb.size.z)+beg_2; + + min.x=(beg_1.xend_2.x)?end_1.x:end_2.x; + max.y=(end_1.y>end_2.y)?end_1.y:end_2.y; + max.z=(end_1.z>end_2.z)?end_1.z:end_2.z; + + pos=min; + size=max-min; +} + +AABB AABB::intersection(const AABB& p_aabb) const { + + Vector3 src_min=pos; + Vector3 src_max=pos+size; + Vector3 dst_min=p_aabb.pos; + Vector3 dst_max=p_aabb.pos+p_aabb.size; + + Vector3 min,max; + + if (src_min.x > dst_max.x || src_max.x < dst_min.x ) + return AABB(); + else { + + min.x= ( src_min.x > dst_min.x ) ? src_min.x :dst_min.x; + max.x= ( src_max.x < dst_max.x ) ? src_max.x :dst_max.x; + + } + + if (src_min.y > dst_max.y || src_max.y < dst_min.y ) + return AABB(); + else { + + min.y= ( src_min.y > dst_min.y ) ? src_min.y :dst_min.y; + max.y= ( src_max.y < dst_max.y ) ? src_max.y :dst_max.y; + + } + + if (src_min.z > dst_max.z || src_max.z < dst_min.z ) + return AABB(); + else { + + min.z= ( src_min.z > dst_min.z ) ? src_min.z :dst_min.z; + max.z= ( src_max.z < dst_max.z ) ? src_max.z :dst_max.z; + + } + + + return AABB( min, max-min ); +} + +bool AABB::intersects_ray(const Vector3& p_from, const Vector3& p_dir,Vector3* r_clip,Vector3* r_normal) const { + + Vector3 c1, c2; + Vector3 end = pos+size; + float near=-1e20; + float far=1e20; + int axis=0; + + for (int i=0;i<3;i++){ + if (p_dir[i] == 0){ + if ((p_from[i] < pos[i]) || (p_from[i] > end[i])) { + return false; + } + } else { // ray not parallel to planes in this direction + c1[i] = (pos[i] - p_from[i]) / p_dir[i]; + c2[i] = (end[i] - p_from[i]) / p_dir[i]; + + if(c1[i] > c2[i]){ + SWAP(c1,c2); + } + if (c1[i] > near){ + near = c1[i]; + axis=i; + } + if (c2[i] < far){ + far = c2[i]; + } + if( (near > far) || (far < 0) ){ + return false; + } + } + } + + if (r_clip) + *r_clip=c1; + if (r_normal) { + *r_normal=Vector3(); + (*r_normal)[axis]=p_dir[axis]?-1:1; + } + + return true; + +} + + +bool AABB::intersects_segment(const Vector3& p_from, const Vector3& p_to,Vector3* r_clip,Vector3* r_normal) const { + + real_t min=0,max=1; + int axis=0; + float sign=0; + + for(int i=0;i<3;i++) { + real_t seg_from=p_from[i]; + real_t seg_to=p_to[i]; + real_t box_begin=pos[i]; + real_t box_end=box_begin+size[i]; + real_t cmin,cmax; + float csign; + + if (seg_from < seg_to) { + + if (seg_from > box_end || seg_to < box_begin) + return false; + real_t length=seg_to-seg_from; + cmin = (seg_from < box_begin)?((box_begin - seg_from)/length):0; + cmax = (seg_to > box_end)?((box_end - seg_from)/length):1; + csign=-1.0; + + } else { + + if (seg_to > box_end || seg_from < box_begin) + return false; + real_t length=seg_to-seg_from; + cmin = (seg_from > box_end)?(box_end - seg_from)/length:0; + cmax = (seg_to < box_begin)?(box_begin - seg_from)/length:1; + csign=1.0; + } + + if (cmin > min) { + min = cmin; + axis=i; + sign=csign; + } + if (cmax < max) + max = cmax; + if (max < min) + return false; + } + + + Vector3 rel=p_to-p_from; + + if (r_normal) { + Vector3 normal; + normal[axis]=sign; + *r_normal=normal; + } + + if (r_clip) + *r_clip=p_from+rel*min; + + return true; + +} + + +bool AABB::intersects_plane(const Plane &p_plane) const { + + Vector3 points[8] = { + Vector3( pos.x , pos.y , pos.z ), + Vector3( pos.x , pos.y , pos.z+size.z ), + Vector3( pos.x , pos.y+size.y , pos.z ), + Vector3( pos.x , pos.y+size.y , pos.z+size.z ), + Vector3( pos.x+size.x , pos.y , pos.z ), + Vector3( pos.x+size.x , pos.y , pos.z+size.z ), + Vector3( pos.x+size.x , pos.y+size.y , pos.z ), + Vector3( pos.x+size.x , pos.y+size.y , pos.z+size.z ), + }; + + bool over=false; + bool under=false; + + for (int i=0;i<8;i++) { + + if (p_plane.distance_to(points[i])>0) + over=true; + else + under=true; + + } + + return under && over; +} + + + +Vector3 AABB::get_longest_axis() const { + + Vector3 axis(1,0,0); + real_t max_size=size.x; + + if (size.y > max_size ) { + axis=Vector3(0,1,0); + max_size=size.y; + } + + if (size.z > max_size ) { + axis=Vector3(0,0,1); + max_size=size.z; + } + + return axis; +} +int AABB::get_longest_axis_index() const { + + int axis=0; + real_t max_size=size.x; + + if (size.y > max_size ) { + axis=1; + max_size=size.y; + } + + if (size.z > max_size ) { + axis=2; + max_size=size.z; + } + + return axis; +} + + +Vector3 AABB::get_shortest_axis() const { + + Vector3 axis(1,0,0); + real_t max_size=size.x; + + if (size.y < max_size ) { + axis=Vector3(0,1,0); + max_size=size.y; + } + + if (size.z < max_size ) { + axis=Vector3(0,0,1); + max_size=size.z; + } + + return axis; +} +int AABB::get_shortest_axis_index() const { + + int axis=0; + real_t max_size=size.x; + + if (size.y < max_size ) { + axis=1; + max_size=size.y; + } + + if (size.z < max_size ) { + axis=2; + max_size=size.z; + } + + return axis; +} + +AABB AABB::merge(const AABB& p_with) const { + + AABB aabb=*this; + aabb.merge_with(p_with); + return aabb; +} +AABB AABB::expand(const Vector3& p_vector) const { + AABB aabb=*this; + aabb.expand_to(p_vector); + return aabb; + +} +AABB AABB::grow(real_t p_by) const { + + AABB aabb=*this; + aabb.grow_by(p_by); + return aabb; +} +void AABB::grow_by(real_t p_amount) { + + pos.x-=p_amount; + pos.y-=p_amount; + pos.z-=p_amount; + size.x+=2.0*p_amount; + size.y+=2.0*p_amount; + size.z+=2.0*p_amount; +} + +void AABB::get_edge(int p_edge,Vector3& r_from,Vector3& r_to) const { + + ERR_FAIL_INDEX(p_edge,12); + switch(p_edge) { + + case 0:{ + + r_from=Vector3( pos.x+size.x , pos.y , pos.z ); + r_to=Vector3( pos.x , pos.y , pos.z ); + } break; + case 1:{ + + r_from=Vector3( pos.x+size.x , pos.y , pos.z+size.z ); + r_to=Vector3( pos.x+size.x , pos.y , pos.z ); + } break; + case 2:{ + r_from=Vector3( pos.x , pos.y , pos.z+size.z ); + r_to=Vector3( pos.x+size.x , pos.y , pos.z+size.z ); + + } break; + case 3:{ + + r_from=Vector3( pos.x , pos.y , pos.z ); + r_to=Vector3( pos.x , pos.y , pos.z+size.z ); + + } break; + case 4:{ + + r_from=Vector3( pos.x , pos.y+size.y , pos.z ); + r_to=Vector3( pos.x+size.x , pos.y+size.y , pos.z ); + } break; + case 5:{ + + r_from=Vector3( pos.x+size.x , pos.y+size.y , pos.z ); + r_to=Vector3( pos.x+size.x , pos.y+size.y , pos.z+size.z ); + } break; + case 6:{ + r_from=Vector3( pos.x+size.x , pos.y+size.y , pos.z+size.z ); + r_to=Vector3( pos.x , pos.y+size.y , pos.z+size.z ); + + } break; + case 7:{ + + r_from=Vector3( pos.x , pos.y+size.y , pos.z+size.z ); + r_to=Vector3( pos.x , pos.y+size.y , pos.z ); + + } break; + case 8:{ + + r_from=Vector3( pos.x , pos.y , pos.z+size.z ); + r_to=Vector3( pos.x , pos.y+size.y , pos.z+size.z ); + + } break; + case 9:{ + + r_from=Vector3( pos.x , pos.y , pos.z ); + r_to=Vector3( pos.x , pos.y+size.y , pos.z ); + + } break; + case 10:{ + + r_from=Vector3( pos.x+size.x , pos.y , pos.z ); + r_to=Vector3( pos.x+size.x , pos.y+size.y , pos.z ); + + } break; + case 11:{ + + r_from=Vector3( pos.x+size.x , pos.y , pos.z+size.z ); + r_to=Vector3( pos.x+size.x , pos.y+size.y , pos.z+size.z ); + + } break; + + } + +} + +AABB::operator String() const { + + return String()+pos +" - "+ size; +} diff --git a/core/math/aabb.h b/core/math/aabb.h new file mode 100644 index 000000000..87be03cf1 --- /dev/null +++ b/core/math/aabb.h @@ -0,0 +1,319 @@ +/*************************************************************************/ +/* aabb.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 AABB_H +#define AABB_H + + + +#include "vector3.h" +#include "plane.h" +/** + * AABB / AABB (Axis Aligned Bounding Box) + * This is implemented by a point (pos) and the box size + */ + + + +class AABB { +public: + Vector3 pos; + Vector3 size; + + float 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); + } + + _FORCE_INLINE_ bool has_no_surface() const { + + return (size.x<=CMP_EPSILON && size.y<=CMP_EPSILON && size.z<=CMP_EPSILON); + } + + const Vector3& get_pos() const { return pos; } + void set_pos(const Vector3& p_pos) { pos=p_pos; } + const Vector3& get_size() const { return size; } + void set_size(const Vector3& p_size) { size=p_size; } + + + bool operator==(const AABB& p_rval) const; + bool operator!=(const AABB& p_rval) const; + + _FORCE_INLINE_ bool intersects(const AABB& p_aabb) const; /// Both AABBs overlap + _FORCE_INLINE_ bool encloses(const AABB & p_aabb) const; /// p_aabb is completely inside this + + AABB merge(const AABB& p_with) const; + void merge_with(const AABB& p_aabb); ///merge with another AABB + AABB intersection(const AABB& p_aabb) const; ///get box where two intersect, empty if no intersection occurs + bool intersects_segment(const Vector3& p_from, const Vector3& p_to,Vector3* r_clip=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 intersects_convex_shape(const Plane *p_plane, int p_plane_count) const; + bool intersects_plane(const Plane &p_plane) const; + + _FORCE_INLINE_ bool has_point(const Vector3& p_point) const; + _FORCE_INLINE_ Vector3 get_support(const Vector3& p_normal) const; + + + Vector3 get_longest_axis() const; + int get_longest_axis_index() const; + _FORCE_INLINE_ real_t get_longest_axis_size() const; + + Vector3 get_shortest_axis() const; + int get_shortest_axis_index() const; + _FORCE_INLINE_ real_t get_shortest_axis_size() const; + + AABB grow(real_t p_by) const; + 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; + _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; } + + +}; + +inline bool AABB::intersects(const AABB& p_aabb) const { + + if ( pos.x > (p_aabb.pos.x + p_aabb.size.x) ) + return false; + if ( (pos.x+size.x) < p_aabb.pos.x ) + return false; + if ( pos.y > (p_aabb.pos.y + p_aabb.size.y) ) + return false; + if ( (pos.y+size.y) < p_aabb.pos.y ) + return false; + if ( pos.z > (p_aabb.pos.z + p_aabb.size.z) ) + return false; + if ( (pos.z+size.z) < p_aabb.pos.z ) + return false; + + return true; +} + + +inline bool AABB::encloses(const AABB & p_aabb) const { + + Vector3 src_min=pos; + Vector3 src_max=pos+size; + Vector3 dst_min=p_aabb.pos; + Vector3 dst_max=p_aabb.pos+p_aabb.size; + + return ( + (src_min.x <= dst_min.x) && + (src_max.x > dst_max.x) && + (src_min.y <= dst_min.y) && + (src_max.y > dst_max.y) && + (src_min.z <= dst_min.z) && + (src_max.z > dst_max.z) ); + +} + +Vector3 AABB::get_support(const Vector3& p_normal) const { + + Vector3 half_extents = size * 0.5; + Vector3 ofs = pos + half_extents; + + return Vector3( + (p_normal.x>0) ? -half_extents.x : half_extents.x, + (p_normal.y>0) ? -half_extents.y : half_extents.y, + (p_normal.z>0) ? -half_extents.z : half_extents.z + )+ofs; +} + + +Vector3 AABB::get_endpoint(int p_point) const { + + switch(p_point) { + case 0: return Vector3( pos.x , pos.y , pos.z ); + case 1: return Vector3( pos.x , pos.y , pos.z+size.z ); + case 2: return Vector3( pos.x , pos.y+size.y , pos.z ); + case 3: return Vector3( pos.x , pos.y+size.y , pos.z+size.z ); + case 4: return Vector3( pos.x+size.x , pos.y , pos.z ); + case 5: return Vector3( pos.x+size.x , pos.y , pos.z+size.z ); + case 6: return Vector3( pos.x+size.x , pos.y+size.y , pos.z ); + case 7: return Vector3( pos.x+size.x , pos.y+size.y , pos.z+size.z ); + }; + + ERR_FAIL_V(Vector3()); +} + +bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count) const { + +#if 1 + + Vector3 half_extents = size * 0.5; + Vector3 ofs = pos + half_extents; + + for(int i=0;i0) ? -half_extents.x : half_extents.x, + (p.normal.y>0) ? -half_extents.y : half_extents.y, + (p.normal.z>0) ? -half_extents.z : half_extents.z + ); + point+=ofs; + if (p.is_point_over(point)) + return false; + } + + return true; +#else + //cache all points to check against! +// #warning should be easy to optimize, just use the same as when taking the support and use only that point + Vector3 points[8] = { + Vector3( pos.x , pos.y , pos.z ), + Vector3( pos.x , pos.y , pos.z+size.z ), + Vector3( pos.x , pos.y+size.y , pos.z ), + Vector3( pos.x , pos.y+size.y , pos.z+size.z ), + Vector3( pos.x+size.x , pos.y , pos.z ), + Vector3( pos.x+size.x , pos.y , pos.z+size.z ), + Vector3( pos.x+size.x , pos.y+size.y , pos.z ), + Vector3( pos.x+size.x , pos.y+size.y , pos.z+size.z ), + }; + + for (int i=0;ipos.x+size.x) + return false; + if (p_point.y>pos.y+size.y) + return false; + if (p_point.z>pos.z+size.z) + return false; + + return true; +} + + +inline void AABB::expand_to(const Vector3& p_vector) { + + Vector3 begin=pos; + Vector3 end=pos+size; + + if (p_vector.xend.x) + end.x=p_vector.x; + if (p_vector.y>end.y) + end.y=p_vector.y; + if (p_vector.z>end.z) + end.z=p_vector.z; + + pos=begin; + size=end-begin; +} + +void AABB::project_range_in_plane(const Plane& p_plane,float &r_min,float& 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 ); + r_min = distance - length; + r_max = distance + length; +} + +inline real_t AABB::get_longest_axis_size() const { + + real_t max_size=size.x; + + if (size.y > max_size ) { + max_size=size.y; + } + + if (size.z > max_size ) { + max_size=size.z; + } + + return max_size; +} + +inline real_t AABB::get_shortest_axis_size() const { + + real_t max_size=size.x; + + if (size.y < max_size ) { + max_size=size.y; + } + + if (size.z < max_size ) { + max_size=size.z; + } + + return max_size; +} + +typedef AABB Rect3; + +#endif // AABB_H diff --git a/core/math/bezier_curve.cpp b/core/math/bezier_curve.cpp new file mode 100644 index 000000000..267680494 --- /dev/null +++ b/core/math/bezier_curve.cpp @@ -0,0 +1,29 @@ +/*************************************************************************/ +/* bezier_curve.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "bezier_curve.h" diff --git a/core/math/bezier_curve.h b/core/math/bezier_curve.h new file mode 100644 index 000000000..6cc4c730d --- /dev/null +++ b/core/math/bezier_curve.h @@ -0,0 +1,35 @@ +/*************************************************************************/ +/* bezier_curve.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 BEZIER_CURVE_H +#define BEZIER_CURVE_H + + + + +#endif // BEZIER_CURVE_H diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp new file mode 100644 index 000000000..7f838b121 --- /dev/null +++ b/core/math/bsp_tree.cpp @@ -0,0 +1,628 @@ +/*************************************************************************/ +/* bsp_tree.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "bsp_tree.h" +#include "error_macros.h" +#include "print_string.h" + + +void BSP_Tree::from_aabb(const AABB& p_aabb) { + + planes.clear(); + + for(int i=0;i<3;i++) { + + Vector3 n; + n[i]=1; + planes.push_back(Plane(n,p_aabb.pos[i]+p_aabb.size[i])); + planes.push_back(Plane(-n,-p_aabb.pos[i])); + } + + nodes.clear(); + + for(int i=0;i<6;i++) { + + Node n; + n.plane=i; + n.under=(i==0)?UNDER_LEAF:i-1; + n.over=OVER_LEAF; + nodes.push_back(n); + } + + aabb=p_aabb; + error_radius=0; +} + +Vector BSP_Tree::get_nodes() const { + + return nodes; +} +Vector BSP_Tree::get_planes() const { + + return planes; +} + +AABB BSP_Tree::get_aabb() const { + + return aabb; +} + +int BSP_Tree::_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 { + + + const Node *node =&nodes[p_node]; + const Plane &p = planes[node->plane]; + + Vector3 min( + (p.normal.x>0) ? -p_half_extents.x : p_half_extents.x, + (p.normal.y>0) ? -p_half_extents.y : p_half_extents.y, + (p.normal.z>0) ? -p_half_extents.z : p_half_extents.z + ); + Vector3 max=-min; + max+=p_center; + min+=p_center; + + float dist_min = p.distance_to(min); + float dist_max = p.distance_to(max); + + if ((dist_min * dist_max) < CMP_EPSILON ) { //intersection, test point by point + + + int under_count=0; + + //sort points, so the are under first, over last + for(int i=0;i0) { + if (node->under==UNDER_LEAF) { + total+=under_count; + } else { + total+=_get_points_inside(node->under,p_points,p_indices,p_center,p_half_extents,under_count); + } + } + + if (under_count!=p_indices_count) { + if (node->over==OVER_LEAF) { + //total+=0 //if they are over an OVER_LEAF, they are outside the model + } else { + total+=_get_points_inside(node->over,p_points,&p_indices[under_count],p_center,p_half_extents,p_indices_count-under_count); + } + } + + return total; + + } else if (dist_min > 0 ) { //all points over plane + + if (node->over==OVER_LEAF) { + + return 0; // all these points are not visible + } + + + return _get_points_inside(node->over,p_points,p_indices,p_center,p_half_extents,p_indices_count); + } else if (dist_min <= 0 ) { //all points behind plane + + if (node->under==UNDER_LEAF) { + + return p_indices_count; // all these points are visible + } + return _get_points_inside(node->under,p_points,p_indices,p_center,p_half_extents,p_indices_count); + } + + return 0; +} + +int BSP_Tree::get_points_inside(const Vector3* p_points,int p_point_count) const { + + + if (nodes.size()==0) + return 0; + +#if 1 +//this version is easier to debug, and and MUCH faster in real world cases + + int pass_count = 0; + const Node *nodesptr=&nodes[0]; + const Plane *planesptr=&planes[0]; + int plane_count=planes.size(); + int node_count=nodes.size(); + + if (node_count==0) // no nodes! + return 0; + + for(int i=0;i=node_count, false ); +#endif + + } + + if (pass) + pass_count++; + } + + return pass_count; + +#else +//this version scales better but it's slower for real world cases + + int *indices = (int*)alloca(p_point_count*sizeof(int)); + AABB bounds; + + for(int i=0;i=node_count, false ); +#endif + + steps++; + } + + return false; +} + + +static int _bsp_find_best_half_plane(const Face3* p_faces,const Vector& p_indices,float p_tolerance) { + + int ic = p_indices.size(); + const int*indices=p_indices.ptr(); + + int best_plane = -1; + float best_plane_cost = 1e20; + + // Loop to find the polygon that best divides the set. + + for (int i=0;ip_tolerance) { + + if (d > 0) + over++; + else + under++; + } + + } + + if (over && under) + num_spanning++; + else if (over) + num_over++; + else + num_under++; + + } + + + + //double split_cost = num_spanning / (double) face_count; + double relation = Math::abs(num_over-num_under) / (double) 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; + + //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& p_indices,Vector &p_planes, Vector &p_nodes,float p_tolerance) { + + ERR_FAIL_COND_V( p_nodes.size() == BSP_Tree::MAX_NODES, -1 ); + + // should not reach here + ERR_FAIL_COND_V( p_indices.size() == 0, -1 ) + + int ic = p_indices.size(); + const int*indices=p_indices.ptr(); + + int divisor_idx = _bsp_find_best_half_plane(p_faces,p_indices,p_tolerance); + + // returned error + ERR_FAIL_COND_V( divisor_idx<0 , -1 ); + + + Vector faces_over; + Vector faces_under; + + Plane divisor_plane=p_faces[ indices[divisor_idx] ].get_plane(); + + for (int i=0;ip_tolerance) { + + if (d > 0) + over_count++; + else + under_count++; + } + } + + if (over_count) + faces_over.push_back( indices[i] ); + if (under_count) + faces_under.push_back( indices[i] ); + + } + + + + uint16_t over_idx=BSP_Tree::OVER_LEAF,under_idx=BSP_Tree::UNDER_LEAF; + + if (faces_over.size()>0) { //have facess above? + + int idx = _bsp_create_node( p_faces, faces_over, p_planes, p_nodes,p_tolerance ); + if (idx>=0) + over_idx=idx; + } + + if (faces_under.size()>0) { //have facess above? + + int idx = _bsp_create_node( p_faces,faces_under, p_planes, p_nodes,p_tolerance ); + if (idx>=0) + under_idx=idx; + } + + /* Create the node */ + + // find existing divisor plane + int divisor_plane_idx=-1; + + + for (int i=0;i plane_values; + plane_values.resize(planes.size()*4); + + for(int i=0;i dst_nodes; + dst_nodes.resize(nodes.size()*3); + + for(int i=0;i src_nodes = d["nodes"]; + ERR_FAIL_COND(src_nodes.size()%3); + + + if (d["planes"].get_type()==Variant::REAL_ARRAY) { + + DVector 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::Read r = src_planes.read(); + for(int i=0;i::Read r = src_nodes.read(); + + for(int i=0;i& p_faces,float p_error_radius) { + + // compute aabb + + int face_count=p_faces.size(); + DVector::Read faces_r=p_faces.read(); + const Face3 *facesptr = faces_r.ptr(); + + + bool first=true; + + Vector indices; + + for (int i=0;i &p_nodes, const Vector &p_planes, const AABB& p_aabb,float p_error_radius) { + + nodes=p_nodes; + planes=p_planes; + aabb=p_aabb; + error_radius=p_error_radius; + +} + +BSP_Tree::~BSP_Tree() { + + +} diff --git a/core/math/bsp_tree.h b/core/math/bsp_tree.h new file mode 100644 index 000000000..03bfd947c --- /dev/null +++ b/core/math/bsp_tree.h @@ -0,0 +1,141 @@ +/*************************************************************************/ +/* bsp_tree.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 BSP_TREE_H +#define BSP_TREE_H + +#include "plane.h" +#include "aabb.h" +#include "face3.h" +#include "vector.h" +#include "dvector.h" + +#include "variant.h" +/** + @author Juan Linietsky +*/ +class BSP_Tree { +public: + + enum { + + UNDER_LEAF=0xFFFF, + OVER_LEAF=0xFFFE, + MAX_NODES=0xFFFE, + MAX_PLANES=(1<<16) + }; + + struct Node { + + uint16_t plane; + uint16_t under; + uint16_t over; + }; + + +private: + // thanks to the properties of Vector, + // this class can be assigned and passed around between threads + // with no cost. + + Vector nodes; + Vector planes; + AABB aabb; + float 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; + + template + bool _test_convex(const Node* p_nodes, const Plane* p_planes,int p_current, const T& p_convex) const; + +public: + + bool is_empty() const { return nodes.size()==0; } + Vector get_nodes() const; + Vector get_planes() const; + AABB 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; + template + bool convex_is_inside(const T& p_convex) const; + + operator Variant() const; + + void from_aabb(const AABB& p_aabb); + + BSP_Tree(); + BSP_Tree(const Variant& p_variant); + BSP_Tree(const DVector& p_faces,float p_error_radius=0); + BSP_Tree(const Vector &p_nodes, const Vector &p_planes, const AABB& p_aabb,float p_error_radius=0); + ~BSP_Tree(); + +}; + +template +bool BSP_Tree::_test_convex(const Node* p_nodes, const Plane* p_planes,int p_current, const T& p_convex) const { + + if (p_current==UNDER_LEAF) + return true; + else if (p_current==OVER_LEAF) + return false; + + bool collided=false; + const Node&n=p_nodes[p_current]; + + const Plane& p=p_planes[n.plane]; + + float min,max; + p_convex.project_range(p.normal,min,max); + + bool go_under = min < p.d; + bool go_over = max >= p.d; + + if (go_under && _test_convex(p_nodes,p_planes,n.under,p_convex)) + collided=true; + if (go_over && _test_convex(p_nodes,p_planes,n.over,p_convex)) + collided=true; + + return collided; + +} + +template +bool BSP_Tree::convex_is_inside(const T& p_convex) const { + + int node_count = nodes.size(); + if (node_count==0) + return false; + const Node* nodes=&this->nodes[0]; + const Plane* planes = &this->planes[0]; + + return _test_convex(nodes,planes,node_count-1,p_convex); +} + + +#endif diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp new file mode 100644 index 000000000..52d77b6eb --- /dev/null +++ b/core/math/camera_matrix.cpp @@ -0,0 +1,596 @@ +/*************************************************************************/ +/* camera_matrix.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "camera_matrix.h" +#include "math_funcs.h" +#include "print_string.h" + +void CameraMatrix::set_identity() { + + for (int i=0;i<4;i++) { + + for (int j=0;j<4;j++) { + + matrix[i][j]=(i==j)?1:0; + } + } +} + + +void CameraMatrix::set_zero() { + + for (int i=0;i<4;i++) { + + for (int j=0;j<4;j++) { + + matrix[i][j]=0; + } + } +} + + +Plane CameraMatrix::xform4(const Plane& p_vec4) { + + Plane ret; + + ret.normal.x = matrix[0][0] * p_vec4.normal.x + matrix[1][0] * p_vec4.normal.y + matrix[2][0] * p_vec4.normal.z + matrix[3][0] * p_vec4.d; + ret.normal.y = matrix[0][1] * p_vec4.normal.x + matrix[1][1] * p_vec4.normal.y + matrix[2][1] * p_vec4.normal.z + matrix[3][1] * p_vec4.d; + ret.normal.z = matrix[0][2] * p_vec4.normal.x + matrix[1][2] * p_vec4.normal.y + matrix[2][2] * p_vec4.normal.z + matrix[3][2] * p_vec4.d; + ret.d = matrix[0][3] * p_vec4.normal.x + matrix[1][3] * p_vec4.normal.y + matrix[2][3] * p_vec4.normal.z + matrix[3][3] * p_vec4.d; + return ret; +} + +void CameraMatrix::set_perspective(float p_fovy_degrees, float p_aspect, float p_z_near, float p_z_far,bool p_flip_fov) { + + if (p_flip_fov) + p_fovy_degrees=get_fovy(p_fovy_degrees,p_aspect); + + + float sine, cotangent, deltaZ; + float radians = p_fovy_degrees / 2.0 * Math_PI / 180.0; + + deltaZ = p_z_far - p_z_near; + sine = Math::sin(radians); + + if ((deltaZ == 0) || (sine == 0) || (p_aspect == 0)) { + return ; + } + cotangent = Math::cos(radians) / sine; + + set_identity(); + + matrix[0][0] = cotangent / p_aspect; + matrix[1][1] = cotangent; + matrix[2][2] = -(p_z_far + p_z_near) / deltaZ; + matrix[2][3] = -1; + matrix[3][2] = -2 * p_z_near * p_z_far / deltaZ; + matrix[3][3] = 0; + +} + +void CameraMatrix::set_orthogonal(float p_left, float p_right, float p_bottom, float p_top, float p_znear, float p_zfar) { + + + set_identity(); + + matrix[0][0] = 2.0/(p_right-p_left); + matrix[3][0] = -((p_right+p_left)/(p_right-p_left)); + matrix[1][1] = 2.0/(p_top-p_bottom); + matrix[3][1] = -((p_top+p_bottom)/(p_top-p_bottom)); + matrix[2][2] = -2.0/(p_zfar-p_znear); + matrix[3][2] = -((p_zfar+p_znear)/(p_zfar-p_znear)); + matrix[3][3] = 1.0; + +} + +void CameraMatrix::set_orthogonal(float p_size, float p_aspect, float p_znear, float p_zfar,bool p_flip_fov) { + + if (p_flip_fov) { + p_size*=p_aspect; + } + + set_orthogonal(-p_size/2,+p_size/2,-p_size/p_aspect/2,+p_size/p_aspect/2,p_znear,p_zfar); +} + + + +void CameraMatrix::set_frustum(float p_left, float p_right, float p_bottom, float p_top, float p_near, float p_far) { + + ///@TODO, give a check to this. I'm not sure if it's working. + set_identity(); + + matrix[0][0]=(2*p_near) / (p_right-p_left); + matrix[0][2]=(p_right+p_left) / (p_right-p_left); + matrix[1][1]=(2*p_near) / (p_top-p_bottom); + matrix[1][2]=(p_top+p_bottom) / (p_top-p_bottom); + matrix[2][2]=-(p_far+p_near) / ( p_far-p_near); + matrix[2][3]=-(2*p_far*p_near) / (p_far-p_near); + matrix[3][2]=-1; + matrix[3][3]=0; + +} + + +float CameraMatrix::get_z_far() const { + + const float * matrix = (const float*)this->matrix; + Plane new_plane=Plane(matrix[ 3] - matrix[ 2], + matrix[ 7] - matrix[ 6], + matrix[11] - matrix[10], + matrix[15] - matrix[14]); + + new_plane.normal=-new_plane.normal; + new_plane.normalize(); + + return new_plane.d; +} +float CameraMatrix::get_z_near() const { + + const float * matrix = (const float*)this->matrix; + Plane new_plane=Plane(matrix[ 3] + matrix[ 2], + matrix[ 7] + matrix[ 6], + matrix[11] + matrix[10], + -matrix[15] - matrix[14]); + + new_plane.normalize(); + return new_plane.d; +} + +void CameraMatrix::get_viewport_size(float& r_width, float& r_height) const { + + const float * matrix = (const float*)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(); + + ///////--- Right Plane ---/////// + Plane right_plane=Plane(matrix[ 3] - matrix[ 0], + matrix[ 7] - matrix[ 4], + matrix[11] - matrix[ 8], + - matrix[15] + matrix[12]).normalized(); + + Plane top_plane=Plane(matrix[ 3] - matrix[ 1], + matrix[ 7] - matrix[ 5], + matrix[11] - matrix[ 9], + -matrix[15] + matrix[13]).normalized(); + + Vector3 res; + near_plane.intersect_3(right_plane,top_plane,&res); + + r_width=res.x; + r_height=res.y; +} + +bool CameraMatrix::get_endpoints(const Transform& p_transform, Vector3 *p_8points) const { + + const float * matrix = (const float*)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(); + + ///////--- Far Plane ---/////// + Plane far_plane=Plane(matrix[ 2] - matrix[ 3], + matrix[ 6] - matrix[ 7], + matrix[10] - matrix[11], + matrix[15] - matrix[14]).normalized(); + + + ///////--- Right Plane ---/////// + Plane right_plane=Plane(matrix[ 0] - matrix[ 3], + matrix[ 4] - matrix[ 7], + matrix[8] - matrix[ 11], + - matrix[15] + matrix[12]).normalized(); + + ///////--- Top Plane ---/////// + Plane top_plane=Plane(matrix[ 1] - matrix[ 3], + matrix[ 5] - matrix[ 7], + matrix[9] - matrix[ 11], + -matrix[15] + matrix[13]).normalized(); + + Vector3 near_endpoint; + Vector3 far_endpoint; + + bool res=near_plane.intersect_3(right_plane,top_plane,&near_endpoint); + ERR_FAIL_COND_V(!res,false); + + res=far_plane.intersect_3(right_plane,top_plane,&far_endpoint); + ERR_FAIL_COND_V(!res,false); + + p_8points[0]=p_transform.xform( Vector3( near_endpoint.x, near_endpoint.y, near_endpoint.z ) ); + p_8points[1]=p_transform.xform( Vector3( near_endpoint.x,-near_endpoint.y, near_endpoint.z ) ); + p_8points[2]=p_transform.xform( Vector3(-near_endpoint.x, near_endpoint.y, near_endpoint.z ) ); + p_8points[3]=p_transform.xform( Vector3(-near_endpoint.x,-near_endpoint.y, near_endpoint.z ) ); + p_8points[4]=p_transform.xform( Vector3( far_endpoint.x, far_endpoint.y, far_endpoint.z ) ); + p_8points[5]=p_transform.xform( Vector3( far_endpoint.x,-far_endpoint.y, far_endpoint.z ) ); + p_8points[6]=p_transform.xform( Vector3(-far_endpoint.x, far_endpoint.y, far_endpoint.z ) ); + p_8points[7]=p_transform.xform( Vector3(-far_endpoint.x,-far_endpoint.y, far_endpoint.z ) ); + + return true; +} + +Vector CameraMatrix::get_projection_planes(const Transform& p_transform) const { + + /** Fast Plane Extraction from combined modelview/projection matrices. + * References: + * http://www.markmorley.com/opengl/frustumculling.html + * http://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf + */ + + Vector planes; + + const float * matrix = (const float*)this->matrix; + + Plane new_plane; + + ///////--- Near Plane ---/////// + new_plane=Plane(matrix[ 3] + matrix[ 2], + matrix[ 7] + matrix[ 6], + matrix[11] + matrix[10], + matrix[15] + matrix[14]); + + new_plane.normal=-new_plane.normal; + new_plane.normalize(); + + planes.push_back( p_transform.xform(new_plane) ); + + ///////--- Far Plane ---/////// + new_plane=Plane(matrix[ 3] - matrix[ 2], + matrix[ 7] - matrix[ 6], + matrix[11] - matrix[10], + matrix[15] - matrix[14]); + + new_plane.normal=-new_plane.normal; + new_plane.normalize(); + + planes.push_back( p_transform.xform(new_plane) ); + + + ///////--- Left Plane ---/////// + new_plane=Plane(matrix[ 3] + matrix[ 0], + matrix[ 7] + matrix[ 4], + matrix[11] + matrix[ 8], + matrix[15] + matrix[12]); + + new_plane.normal=-new_plane.normal; + new_plane.normalize(); + + planes.push_back( p_transform.xform(new_plane) ); + + + ///////--- Top Plane ---/////// + new_plane=Plane(matrix[ 3] - matrix[ 1], + matrix[ 7] - matrix[ 5], + matrix[11] - matrix[ 9], + matrix[15] - matrix[13]); + + + new_plane.normal=-new_plane.normal; + new_plane.normalize(); + + planes.push_back( p_transform.xform(new_plane) ); + + + ///////--- Right Plane ---/////// + new_plane=Plane(matrix[ 3] - matrix[ 0], + matrix[ 7] - matrix[ 4], + matrix[11] - matrix[ 8], + matrix[15] - matrix[12]); + + + new_plane.normal=-new_plane.normal; + new_plane.normalize(); + + planes.push_back( p_transform.xform(new_plane) ); + + + ///////--- Bottom Plane ---/////// + new_plane=Plane(matrix[ 3] + matrix[ 1], + matrix[ 7] + matrix[ 5], + matrix[11] + matrix[ 9], + matrix[15] + matrix[13]); + + + new_plane.normal=-new_plane.normal; + new_plane.normalize(); + + planes.push_back( p_transform.xform(new_plane) ); + + return planes; +} + + + +CameraMatrix CameraMatrix::inverse() const { + + CameraMatrix cm = *this; + cm.invert(); + return cm; +} + +void CameraMatrix::invert() { + + int i,j,k; + int pvt_i[4], pvt_j[4]; /* Locations of pivot matrix */ + float pvt_val; /* Value of current pivot element */ + float hold; /* Temporary storage */ + float determinat; /* Determinant */ + + determinat = 1.0; + for (k=0; k<4; k++) { + /** Locate k'th pivot element **/ + pvt_val=matrix[k][k]; /** Initialize for search **/ + pvt_i[k]=k; + pvt_j[k]=k; + for (i=k; i<4; i++) { + for (j=k; j<4; j++) { + if (Math::absd(matrix[i][j]) > Math::absd(pvt_val)) { + pvt_i[k]=i; + pvt_j[k]=j; + pvt_val=matrix[i][j]; + } + } + } + + /** Product of pivots, gives determinant when finished **/ + determinat*=pvt_val; + if (Math::absd(determinat)<1e-7) { + return; //(false); /** Matrix is singular (zero determinant). **/ + } + + /** "Interchange" rows (with sign change stuff) **/ + i=pvt_i[k]; + if (i!=k) { /** If rows are different **/ + for (j=0; j<4; j++) { + hold=-matrix[k][j]; + matrix[k][j]=matrix[i][j]; + matrix[i][j]=hold; + } + } + + /** "Interchange" columns **/ + j=pvt_j[k]; + if (j!=k) { /** If columns are different **/ + for (i=0; i<4; i++) { + hold=-matrix[i][k]; + matrix[i][k]=matrix[i][j]; + matrix[i][j]=hold; + } + } + + /** Divide column by minus pivot value **/ + for (i=0; i<4; i++) { + if (i!=k) matrix[i][k]/=( -pvt_val) ; + } + + /** Reduce the matrix **/ + for (i=0; i<4; i++) { + hold = matrix[i][k]; + for (j=0; j<4; j++) { + if (i!=k && j!=k) matrix[i][j]+=hold*matrix[k][j]; + } + } + + /** Divide row by pivot **/ + for (j=0; j<4; j++) { + if (j!=k) matrix[k][j]/=pvt_val; + } + + /** Replace pivot by reciprocal (at last we can touch it). **/ + matrix[k][k] = 1.0/pvt_val; + } + + /* That was most of the work, one final pass of row/column interchange */ + /* to finish */ + for (k=4-2; k>=0; k--) { /* Don't need to work with 1 by 1 corner*/ + i=pvt_j[k]; /* Rows to swap correspond to pivot COLUMN */ + if (i!=k) { /* If rows are different */ + for(j=0; j<4; j++) { + hold = matrix[k][j]; + matrix[k][j]=-matrix[i][j]; + matrix[i][j]=hold; + } + } + + j=pvt_i[k]; /* Columns to swap correspond to pivot ROW */ + if (j!=k) /* If columns are different */ + for (i=0; i<4; i++) { + hold=matrix[i][k]; + matrix[i][k]=-matrix[i][j]; + matrix[i][j]=hold; + } + } + + +} + +CameraMatrix::CameraMatrix() { + + set_identity(); +} + +CameraMatrix CameraMatrix::operator*(const CameraMatrix& p_matrix) const { + + CameraMatrix new_matrix; + + for( int j = 0; j < 4; j++ ) { + for( int i = 0; i < 4; i++ ) { + real_t ab = 0; + for( int k = 0; k < 4; k++ ) + ab += matrix[k][i] * p_matrix.matrix[j][k] ; + new_matrix.matrix[j][i] = ab; + } + } + + return new_matrix; +} + +void CameraMatrix::set_light_bias() { + + float *m=&matrix[0][0]; + + m[0]=0.5, + m[1]=0.0, + m[2]=0.0, + m[3]=0.0, + m[4]=0.0, + m[5]=0.5, + m[6]=0.0, + m[7]=0.0, + m[8]=0.0, + m[9]=0.0, + m[10]=0.5, + m[11]=0.0, + m[12]=0.5, + m[13]=0.5, + m[14]=0.5, + m[15]=1.0; + +} + +CameraMatrix::operator String() const { + + String str; + for (int i=0;i<4;i++) + for (int j=0;j<4;j++) + str+=String((j>0)?", ":"\n")+rtos(matrix[i][j]); + + return str; +} + +float CameraMatrix::get_aspect() const { + + float w,h; + get_viewport_size(w,h); + return w/h; +} + +float CameraMatrix::get_fov() const { + const float * matrix = (const float*)this->matrix; + + Plane right_plane=Plane(matrix[ 3] - matrix[ 0], + matrix[ 7] - matrix[ 4], + matrix[11] - matrix[ 8], + - matrix[15] + matrix[12]).normalized(); + + return Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x)))*2.0; +} + + +void CameraMatrix::make_scale(const Vector3 &p_scale) { + + set_identity(); + matrix[0][0]=p_scale.x; + matrix[1][1]=p_scale.y; + matrix[2][2]=p_scale.z; + +} + +void CameraMatrix::scale_translate_to_fit(const AABB& p_aabb) { + + Vector3 min = p_aabb.pos; + Vector3 max = p_aabb.pos+p_aabb.size; + + + matrix[0][0]=2/(max.x-min.x); + matrix[1][0]=0; + matrix[2][0]=0; + matrix[3][0]=-(max.x+min.x)/(max.x-min.x); + + matrix[0][1]=0; + matrix[1][1]=2/(max.y-min.y); + matrix[2][1]=0; + matrix[3][1]=-(max.y+min.y)/(max.y-min.y); + + matrix[0][2]=0; + matrix[1][2]=0; + matrix[2][2]=2/(max.z-min.z); + matrix[3][2]=-(max.z+min.z)/(max.z-min.z); + + matrix[0][3]=0; + matrix[1][3]=0; + matrix[2][3]=0; + matrix[3][3]=1; +} + +CameraMatrix::operator Transform() const { + + Transform tr; + const float *m=&matrix[0][0]; + + tr.basis.elements[0][0]=m[0]; + tr.basis.elements[1][0]=m[1]; + tr.basis.elements[2][0]=m[2]; + + tr.basis.elements[0][1]=m[4]; + tr.basis.elements[1][1]=m[5]; + tr.basis.elements[2][1]=m[6]; + + tr.basis.elements[0][2]=m[8]; + tr.basis.elements[1][2]=m[9]; + tr.basis.elements[2][2]=m[10]; + + tr.origin.x=m[12]; + tr.origin.y=m[13]; + tr.origin.z=m[14]; + + return tr; +} + +CameraMatrix::CameraMatrix(const Transform& p_transform) { + + const Transform &tr = p_transform; + float *m=&matrix[0][0]; + + m[0]=tr.basis.elements[0][0]; + m[1]=tr.basis.elements[1][0]; + m[2]=tr.basis.elements[2][0]; + m[3]=0.0; + m[4]=tr.basis.elements[0][1]; + m[5]=tr.basis.elements[1][1]; + m[6]=tr.basis.elements[2][1]; + m[7]=0.0; + m[8]=tr.basis.elements[0][2]; + m[9]=tr.basis.elements[1][2]; + m[10]=tr.basis.elements[2][2]; + m[11]=0.0; + m[12]=tr.origin.x; + m[13]=tr.origin.y; + m[14]=tr.origin.z; + m[15]=1.0; +} + +CameraMatrix::~CameraMatrix() +{ +} + + diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h new file mode 100644 index 000000000..6ffcb0ed0 --- /dev/null +++ b/core/math/camera_matrix.h @@ -0,0 +1,106 @@ +/*************************************************************************/ +/* camera_matrix.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 CAMERA_MATRIX_H +#define CAMERA_MATRIX_H + +#include "transform.h" +/** + @author Juan Linietsky +*/ + + + +struct CameraMatrix { + + enum Planes { + PLANE_NEAR, + PLANE_FAR, + PLANE_LEFT, + PLANE_TOP, + PLANE_RIGHT, + PLANE_BOTTOM + }; + + float 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); + + static float get_fovy(float p_fovx,float p_aspect) { + + return Math::atan(p_aspect * Math::tan(p_fovx * 0.5))*2.0; + } + + float get_z_far() const; + float get_z_near() const; + float get_aspect() const; + float get_fov() const; + + Vector 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 invert(); + CameraMatrix inverse() const; + + CameraMatrix operator*(const CameraMatrix& p_matrix) const; + + Plane xform4(const Plane& p_vec4); + _FORCE_INLINE_ Vector3 xform(const Vector3& p_vec3) const; + + operator String() const; + + void scale_translate_to_fit(const AABB& p_aabb); + void make_scale(const Vector3 &p_scale); + operator Transform() const; + + CameraMatrix(); + CameraMatrix(const Transform& p_transform); + ~CameraMatrix(); + +}; + +Vector3 CameraMatrix::xform(const Vector3& p_vec3) const { + + Vector3 ret; + ret.x = matrix[0][0] * p_vec3.x + matrix[1][0] * p_vec3.y + matrix[2][0] * p_vec3.z + matrix[3][0]; + ret.y = matrix[0][1] * p_vec3.x + matrix[1][1] * p_vec3.y + matrix[2][1] * p_vec3.z + matrix[3][1]; + ret.z = matrix[0][2] * p_vec3.x + matrix[1][2] * p_vec3.y + matrix[2][2] * p_vec3.z + matrix[3][2]; + float w = matrix[0][3] * p_vec3.x + matrix[1][3] * p_vec3.y + matrix[2][3] * p_vec3.z + matrix[3][3]; + return ret/w; +} + +#endif diff --git a/core/math/face3.cpp b/core/math/face3.cpp new file mode 100644 index 000000000..9cdf31ed8 --- /dev/null +++ b/core/math/face3.cpp @@ -0,0 +1,359 @@ +/*************************************************************************/ +/* face3.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "face3.h" +#include "geometry.h" + +int Face3::split_by_plane(const Plane& p_plane,Face3 p_res[3],bool p_is_point_over[3]) const { + + ERR_FAIL_COND_V(is_degenerate(),0); + + + Vector3 above[4]; + int above_count=0; + + Vector3 below[4]; + int below_count=0; + + for (int i=0;i<3;i++) { + + if (p_plane.has_point( vertex[i], CMP_EPSILON )) { // point is in plane + + ERR_FAIL_COND_V(above_count>=4,0); + above[above_count++]=vertex[i]; + ERR_FAIL_COND_V(below_count>=4,0); + below[below_count++]=vertex[i]; + + } else { + + if (p_plane.is_point_over( vertex[i])) { + //Point is over + ERR_FAIL_COND_V(above_count>=4,0); + above[above_count++]=vertex[i]; + + } else { + //Point is under + ERR_FAIL_COND_V(below_count>=4,0); + below[below_count++]=vertex[i]; + } + + /* Check for Intersection between this and the next vertex*/ + + Vector3 inters; + if (!p_plane.intersects_segment( vertex[i],vertex[(i+1)%3],&inters)) + continue; + + /* Intersection goes to both */ + ERR_FAIL_COND_V(above_count>=4,0); + above[above_count++]=inters; + ERR_FAIL_COND_V(below_count>=4,0); + below[below_count++]=inters; + } + } + + int polygons_created=0; + + ERR_FAIL_COND_V( above_count>=4 && below_count>=4 , 0 ); //bug in the algo + + if (above_count>=3) { + + p_res[polygons_created]=Face3( above[0], above[1], above[2] ); + p_is_point_over[polygons_created]=true; + polygons_created++; + + if (above_count==4) { + + p_res[polygons_created]=Face3( above[2], above[3], above[0] ); + p_is_point_over[polygons_created]=true; + polygons_created++; + + } + } + + if (below_count>=3) { + + p_res[polygons_created]=Face3( below[0], below[1], below[2] ); + p_is_point_over[polygons_created]=false; + polygons_created++; + + if (below_count==4) { + + p_res[polygons_created]=Face3( below[2], below[3], below[0] ); + p_is_point_over[polygons_created]=false; + polygons_created++; + + } + } + + return polygons_created; +} + + + +bool Face3::intersects_ray(const Vector3& p_from,const Vector3& p_dir,Vector3 * p_intersection) const { + + return Geometry::ray_intersects_triangle(p_from,p_dir,vertex[0],vertex[1],vertex[2],p_intersection); + +} + +bool Face3::intersects_segment(const Vector3& p_from,const Vector3& p_dir,Vector3 * p_intersection) const { + + return Geometry::segment_intersects_triangle(p_from,p_dir,vertex[0],vertex[1],vertex[2],p_intersection); + +} + + +bool Face3::is_degenerate() const { + + Vector3 normal=vec3_cross(vertex[0]-vertex[1], vertex[0]-vertex[2]); + return (normal.length_squared() < CMP_EPSILON2); +} + + +Face3::Side Face3::get_side_of(const Face3& p_face,ClockDirection p_clock_dir) const { + + int over=0,under=0; + + Plane plane=get_plane(p_clock_dir); + + for (int i=0;i<3;i++) { + + const Vector3 &v=p_face.vertex[i]; + + if (plane.has_point(v)) //coplanar, dont bother + continue; + + if (plane.is_point_over(v)) + over++; + else + under++; + + } + + if ( over > 0 && under == 0 ) + return SIDE_OVER; + else if (under > 0 && over ==0 ) + return SIDE_UNDER; + else if (under ==0 && over == 0) + return SIDE_COPLANAR; + else + return SIDE_SPANNING; + +} + +Vector3 Face3::get_random_point_inside() const { + + float a=Math::random(0,1); + float b=Math::random(0,1); + if (a>b) { + SWAP(a,b); + } + + return vertex[0]*a + vertex[1]*(b-a) + vertex[2]*(1.0-b); + +} + +Plane Face3::get_plane(ClockDirection p_dir) const { + + return Plane( vertex[0], vertex[1], vertex[2] , p_dir ); + +} + +Vector3 Face3::get_median_point() const { + + return (vertex[0] + vertex[1] + vertex[2])/3.0; +} + + +real_t Face3::get_area() const { + + return vec3_cross(vertex[0]-vertex[1], vertex[0]-vertex[2]).length(); +} + +ClockDirection Face3::get_clock_dir() const { + + + Vector3 normal=vec3_cross(vertex[0]-vertex[1], vertex[0]-vertex[2]); + //printf("normal is %g,%g,%g x %g,%g,%g- wtfu is %g\n",tofloat(normal.x),tofloat(normal.y),tofloat(normal.z),tofloat(vertex[0].x),tofloat(vertex[0].y),tofloat(vertex[0].z),tofloat( normal.dot( vertex[0] ) ) ); + return ( normal.dot( vertex[0] ) >= 0 ) ? CLOCKWISE : COUNTERCLOCKWISE; + +} + + +bool Face3::intersects_aabb(const AABB& p_aabb) const { + + /** TEST PLANE **/ + if (!p_aabb.intersects_plane( get_plane() )) + return false; + + /** TEST FACE AXIS */ + +#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;\ + for (int i=0;i<3;i++) {\ + if (i==0 || vertex[i].m_ax > tri_max)\ + tri_max=vertex[i].m_ax;\ + if (i==0 || vertex[i].m_ax < tri_min)\ + tri_min=vertex[i].m_ax;\ + }\ +\ + if (tri_max r_max) + r_max=d; + + if (i==0 || d < r_min) + r_min=d; + } +} + + + +void Face3::get_support(const Vector3& p_normal,const Transform& p_transform,Vector3 *p_vertices,int* p_count,int p_max) const { + +#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.98 +#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.05 + + if (p_max<=0) + return; + + Vector3 n=p_transform.basis.xform_inv(p_normal); + + /** TEST FACE AS SUPPORT **/ + if (get_plane().normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { + + *p_count=MIN(3,p_max); + + for (int i=0;i<*p_count;i++) { + + p_vertices[i]=p_transform.xform(vertex[i]); + } + + return; + + } + + /** FIND SUPPORT VERTEX **/ + + int vert_support_idx=-1; + float support_max; + + for (int i=0;i<3;i++) { + + float d=n.dot(vertex[i]); + + if (i==0 || d > support_max) { + support_max=d; + vert_support_idx=i; + } + } + + /** TEST EDGES AS SUPPORT **/ + + for (int i=0;i<3;i++) { + + if (i!=vert_support_idx && i+1!=vert_support_idx) + continue; + + // check if edge is valid as a support + float dot=(vertex[i]-vertex[(i+1)%3]).normalized().dot(n); + dot=ABS(dot); + if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { + + *p_count=MIN(2,p_max); + + for (int j=0;j<*p_count;j++) + p_vertices[j]=p_transform.xform(vertex[(j+i)%3]); + + return; + } + } + + + *p_count=1; + p_vertices[0]=p_transform.xform(vertex[vert_support_idx]); + +} + + + diff --git a/core/math/face3.h b/core/math/face3.h new file mode 100644 index 000000000..3a62f7f81 --- /dev/null +++ b/core/math/face3.h @@ -0,0 +1,97 @@ +/*************************************************************************/ +/* face3.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 FACE3_H +#define FACE3_H + +#include "vector3.h" +#include "plane.h" +#include "aabb.h" +#include "transform.h" + +class Face3 { +public: + + enum Side { + SIDE_OVER, + SIDE_UNDER, + SIDE_SPANNING, + SIDE_COPLANAR + }; + + + Vector3 vertex[3]; + + /** + * + * @param p_plane plane used to split the face + * @param p_res array of at least 3 faces, amount used in functio return + * @param p_is_point_over array of at least 3 booleans, determining which face is over the plane, amount used in functio return + * @param _epsilon constant used for numerical error rounding, to add "thickness" to the plane (so coplanar points can happen) + * @return amount of faces generated by the split, either 0 (means no split possible), 2 or 3 + */ + + int split_by_plane(const Plane& p_plane,Face3 *p_res,bool *p_is_point_over) const; + + Plane get_plane(ClockDirection p_dir=CLOCKWISE) const; + Vector3 get_random_point_inside() const; + + + Side get_side_of(const Face3& p_face,ClockDirection p_clock_dir=CLOCKWISE) const; + + bool is_degenerate() const; + real_t get_area() const; + + Vector3 get_median_point() const; + + bool intersects_ray(const Vector3& p_from,const Vector3& p_dir,Vector3 * p_intersection=0) const; + bool intersects_segment(const Vector3& p_from,const Vector3& p_dir,Vector3 * p_intersection=0) const; + + 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; + + AABB get_aabb() const { + + AABB aabb( vertex[0], Vector3() ); + aabb.expand_to( vertex[1] ); + aabb.expand_to( vertex[2] ); + return aabb; + } + + bool intersects_aabb(const AABB& p_aabb) const; + operator String() const; + + inline Face3() {} + inline Face3(const Vector3 &p_v1,const Vector3 &p_v2,const Vector3 &p_v3) { vertex[0]=p_v1; vertex[1]=p_v2; vertex[2]=p_v3; } + +}; + + +#endif // FACE3_H diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp new file mode 100644 index 000000000..cb76b9ed0 --- /dev/null +++ b/core/math/geometry.cpp @@ -0,0 +1,1006 @@ +/*************************************************************************/ +/* geometry.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "geometry.h" +#include "print_string.h" + + + +void Geometry::MeshData::optimize_vertices() { + + Map vtx_remap; + + for(int i=0;i new_vertices; + new_vertices.resize(vtx_remap.size()); + + for(int i=0;i > (*Geometry::_decompose_func)(const Vector& p_polygon)=NULL; + +struct _FaceClassify { + + struct _Link { + + int face; + int edge; + void clear() { face=-1; edge=-1; } + _Link() { face=-1; edge=-1; } + }; + bool valid; + int group; + _Link links[3]; + Face3 face; + _FaceClassify() { + group=-1; + valid=false; + }; +}; + +static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) { + /* connect faces, error will occur if an edge is shared between more than 2 faces */ + /* clear connections */ + + bool error=false; + + for (int i=0;i=0) + return false; + + p_faces[p_index].group=p_group; + + for (int i=0;i<3;i++) { + + ERR_FAIL_INDEX_V(p_faces[p_index].links[i].face,len,true); + _group_face(p_faces,len,p_faces[p_index].links[i].face,p_group); + } + + return true; +} + + +DVector< DVector< Face3 > > Geometry::separate_objects( DVector< Face3 > p_array ) { + + DVector< DVector< Face3 > > objects; + + int len = p_array.size(); + + DVector::Read r=p_array.read(); + + const Face3* arrayptr = r.ptr(); + + DVector< _FaceClassify> fc; + + fc.resize( len ); + + DVector< _FaceClassify >::Write fcw=fc.write(); + + _FaceClassify * _fcptr = fcw.ptr(); + + for (int i=0;i >() ); // invalid geometry + } + + /* group connected faces in separate objects */ + + int group=0; + for (int i=0;i=0) { + + objects.resize(group); + DVector< DVector >::Write obw=objects.write(); + DVector< Face3 > *group_faces = obw.ptr(); + + for (int i=0;i=0 && _fcptr[i].group1?2:1; + int div_y=len_y>1?2:1; + int div_z=len_z>1?2:1; + +#define _SPLIT(m_i,m_div,m_v,m_len_v,m_new_v,m_new_len_v)\ + if (m_div==1) {\ + m_new_v=m_v;\ + m_new_len_v=1; \ + } else if (m_i==0) {\ + m_new_v=m_v;\ + m_new_len_v=m_len_v/2;\ + } else {\ + m_new_v=m_v+m_len_v/2;\ + m_new_len_v=m_len_v-m_len_v/2; \ + } + + int new_x; + int new_len_x; + int new_y; + int new_len_y; + int new_z; + int new_len_z; + + for (int i=0;i=len_y); + } break; + case _CELL_PREV_Y_NEG: { + y--; + ERR_FAIL_COND(y<0); + } break; + case _CELL_PREV_X_POS: { + x++; + ERR_FAIL_COND(x>=len_x); + } break; + case _CELL_PREV_X_NEG: { + x--; + ERR_FAIL_COND(x<0); + } break; + case _CELL_PREV_Z_POS: { + z++; + ERR_FAIL_COND(z>=len_z); + } break; + case _CELL_PREV_Z_NEG: { + z--; + ERR_FAIL_COND(z<0); + } break; + default: { + ERR_FAIL(); + } + } + continue; + } + + //printf("attempting new cell!\n"); + + int next_x=x,next_y=y,next_z=z; + uint8_t prev=0; + + switch(c&_CELL_STEP_MASK) { + + case _CELL_STEP_Y_POS: { + + next_y++; + prev=_CELL_PREV_Y_NEG; + } break; + case _CELL_STEP_Y_NEG: { + next_y--; + prev=_CELL_PREV_Y_POS; + } break; + case _CELL_STEP_X_POS: { + next_x++; + prev=_CELL_PREV_X_NEG; + } break; + case _CELL_STEP_X_NEG: { + next_x--; + prev=_CELL_PREV_X_POS; + } break; + case _CELL_STEP_Z_POS: { + next_z++; + prev=_CELL_PREV_Z_NEG; + } break; + case _CELL_STEP_Z_NEG: { + next_z--; + prev=_CELL_PREV_Z_POS; + } break; + default: ERR_FAIL(); + + } + + //printf("testing if new cell will be ok...!\n"); + + if (next_x<0 || next_x>=len_x) + continue; + if (next_y<0 || next_y>=len_y) + continue; + if (next_z<0 || next_z>=len_z) + continue; + + //printf("testing if new cell is traversable\n"); + + if (p_cell_status[next_x][next_y][next_z]&3) + continue; + + //printf("move to it\n"); + + x=next_x; + y=next_y; + z=next_z; + p_cell_status[x][y][z]|=prev; + } +} + +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& p_faces) { + + ERR_FAIL_INDEX(x,len_x); + ERR_FAIL_INDEX(y,len_y); + ERR_FAIL_INDEX(z,len_z); + + if (p_cell_status[x][y][z]&_CELL_EXTERIOR) + return; + +/* static const Vector3 vertices[8]={ + Vector3(0,0,0), + Vector3(0,0,1), + Vector3(0,1,0), + Vector3(0,1,1), + Vector3(1,0,0), + Vector3(1,0,1), + Vector3(1,1,0), + Vector3(1,1,1), + }; +*/ +#define vert(m_idx) Vector3( (m_idx&4)>>2, (m_idx&2)>>1, m_idx&1 ) + + static const uint8_t indices[6][4]={ + {7,6,4,5}, + {7,3,2,6}, + {7,5,1,3}, + {0,2,3,1}, + {0,1,5,4}, + {0,4,6,2}, + + }; +/* + + {0,1,2,3}, + {0,1,4,5}, + {0,2,4,6}, + {4,5,6,7}, + {2,3,7,6}, + {1,3,5,7}, + + {0,2,3,1}, + {0,1,5,4}, + {0,4,6,2}, + {7,6,4,5}, + {7,3,2,6}, + {7,5,1,3}, +*/ + + for (int i=0;i<6;i++) { + + Vector3 face_points[4]; + int disp_x=x+((i%3)==0?((i<3)?1:-1):0); + int disp_y=y+(((i-1)%3)==0?((i<3)?1:-1):0); + int disp_z=z+(((i-2)%3)==0?((i<3)?1:-1):0); + + bool plot=false; + + if (disp_x<0 || disp_x>=len_x) + plot=true; + if (disp_y<0 || disp_y>=len_y) + plot=true; + if (disp_z<0 || disp_z>=len_z) + plot=true; + + if (!plot && (p_cell_status[disp_x][disp_y][disp_z]&_CELL_EXTERIOR)) + plot=true; + + if (!plot) + continue; + + for (int j=0;j<4;j++) + face_points[j]=vert( indices[i][j] ) + Vector3(x,y,z); + + p_faces.push_back( + Face3( + face_points[0], + face_points[1], + face_points[2] + ) + ); + + p_faces.push_back( + Face3( + face_points[2], + face_points[3], + face_points[0] + ) + ); + + } + +} + +DVector< Face3 > Geometry::wrap_geometry( DVector< Face3 > p_array,float *p_error ) { + +#define _MIN_SIZE 1.0 +#define _MAX_LENGTH 20 + + int face_count=p_array.size(); + DVector::Read facesr=p_array.read(); + const Face3 *faces = facesr.ptr(); + + AABB global_aabb; + + for(int i=0;i wrapped_faces; + + for (int i=0;i::Write wrapped_facesw=wrapped_faces.write(); + Face3* wrapped_faces_ptr=wrapped_facesw.ptr(); + + for(int i=0;i &p_planes) { + + MeshData mesh; + + +#define SUBPLANE_SIZE 1024.0 + + float subplane_size = 1024.0; // should compute this from the actual plane + for (int i=0;i0.95) + ref=Vector3(0.0,0.0,1.0); // change axis + + Vector3 right = p.normal.cross(ref).normalized(); + Vector3 up = p.normal.cross( right ).normalized(); + + Vector< Vector3 > vertices; + + Vector3 center = p.get_any_point(); + // make a quad clockwise + vertices.push_back( center - up * subplane_size + right * subplane_size ); + vertices.push_back( center - up * subplane_size - right * subplane_size ); + vertices.push_back( center + up * subplane_size - right * subplane_size ); + vertices.push_back( center + up * subplane_size + right * subplane_size ); + + for (int j=0;j new_vertices; + Plane clip=p_planes[j]; + + if (clip.normal.dot(p.normal)>0.95) + continue; + + if (vertices.size()<3) + break; + + for(int k=0;k Geometry::build_box_planes(const Vector3& p_extents) { + + DVector planes; + + planes.push_back( Plane( Vector3(1,0,0), p_extents.x ) ); + planes.push_back( Plane( Vector3(-1,0,0), p_extents.x ) ); + planes.push_back( Plane( Vector3(0,1,0), p_extents.y ) ); + planes.push_back( Plane( Vector3(0,-1,0), p_extents.y ) ); + planes.push_back( Plane( Vector3(0,0,1), p_extents.z ) ); + planes.push_back( Plane( Vector3(0,0,-1), p_extents.z ) ); + + return planes; +} + +DVector Geometry::build_cylinder_planes(float p_radius, float p_height, int p_sides, Vector3::Axis p_axis) { + + DVector planes; + + for (int i=0;i Geometry::build_sphere_planes(float p_radius, int p_lats,int p_lons, Vector3::Axis p_axis) { + + + DVector planes; + + Vector3 axis; + axis[p_axis]=1.0; + + Vector3 axis_neg; + axis_neg[(p_axis+1)%3]=1.0; + axis_neg[(p_axis+2)%3]=1.0; + axis_neg[p_axis]=-1.0; + + for (int i=0;i Geometry::build_capsule_planes(float p_radius, float p_height, int p_sides, int p_lats, Vector3::Axis p_axis) { + + DVector planes; + + Vector3 axis; + axis[p_axis]=1.0; + + Vector3 axis_neg; + axis_neg[(p_axis+1)%3]=1.0; + axis_neg[(p_axis+2)%3]=1.0; + axis_neg[p_axis]=-1.0; + + for (int i=0;i +*/ + +class Geometry { + Geometry(); +public: + + + + + static float 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; + // Check if either or both segments degenerate into points + if (a <= CMP_EPSILON && e <= CMP_EPSILON) { + // Both segments degenerate into points + c1 = p1; + c2 = p2; + return Math::sqrt((c1 - c2).dot(c1 - c2)); + } + if (a <= CMP_EPSILON) { + // First segment degenerates into a point + s = 0.0f; + t = f / e; // s = 0 => t = (b*s + f) / e = f / e + t = CLAMP(t, 0.0f, 1.0f); + } else { + float 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 + } else { + // The general nondegenerate case starts here + float b = d1.dot(d2); + float 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); + } else + s = 0.0f; + // 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; + + //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); + } + } + } + c1 = p1 + d1 * s; + c2 = p2 + d2 * t; + return Math::sqrt((c1 - c2).dot(c1 - c2)); + } + + + static void get_closest_points_between_segments(const Vector3& p1,const Vector3& p2,const Vector3& q1,const Vector3& q2,Vector3& c1, Vector3& c2) + { + //do the function 'd' as defined by pb. I think is is dot product of some sort +#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); + + //clip the value between [0..1] constraining the solution to lie on the original curves + if (mua < 0) mua = 0; + if (mub < 0) mub = 0; + if (mua > 1) mua = 1; + if (mub > 1) mub = 1; + c1 = p1.linear_interpolate(p2,mua); + 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) { + Vector3 u = p_to_a - p_from_a; + Vector3 v = p_to_b - p_from_b; + Vector3 w = p_from_a - p_to_a; + real_t a = u.dot(u); // always >= 0 + real_t b = u.dot(v); + real_t c = v.dot(v); // always >= 0 + real_t d = u.dot(w); + real_t e = v.dot(w); + real_t D = a*c - b*b; // always >= 0 + real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 + real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 + + // compute the line parameters of the two closest points + if (D < CMP_EPSILON) { // the lines are almost parallel + sN = 0.0; // force using point P0 on segment S1 + sD = 1.0; // to prevent possible division by 0.0 later + tN = e; + tD = c; + } + else { // get the closest points on the infinite lines + sN = (b*e - c*d); + tN = (a*e - b*d); + if (sN < 0.0) { // sc < 0 => the s=0 edge is visible + sN = 0.0; + tN = e; + tD = c; + } + else if (sN > sD) { // sc > 1 => the s=1 edge is visible + sN = sD; + tN = e + b; + tD = c; + } + } + + if (tN < 0.0) { // tc < 0 => the t=0 edge is visible + tN = 0.0; + // recompute sc for this edge + if (-d < 0.0) + sN = 0.0; + else if (-d > a) + sN = sD; + else { + sN = -d; + sD = a; + } + } + else if (tN > tD) { // tc > 1 => the t=1 edge is visible + tN = tD; + // recompute sc for this edge + if ((-d + b) < 0.0) + sN = 0; + else if ((-d + b) > a) + sN = sD; + else { + sN = (-d + b); + sD = a; + } + } + // finally do the division to get sc and tc + sc = (Math::abs(sN) < CMP_EPSILON ? 0.0 : sN / sD); + tc = (Math::abs(tN) < CMP_EPSILON ? 0.0 : tN / tD); + + // get the difference of the two closest points + Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) + + return dP.length(); // return the closest distance + } + + static inline bool ray_intersects_triangle( const Vector3& p_from, const Vector3& p_dir, const Vector3& p_v0,const Vector3& p_v1,const Vector3& p_v2,Vector3* r_res=0) { + Vector3 e1=p_v1-p_v0; + Vector3 e2=p_v2-p_v0; + Vector3 h = p_dir.cross(e2); + real_t a =e1.dot(h); + if (a>-CMP_EPSILON && a < CMP_EPSILON) // parallel test + return false; + + real_t f=1.0/a; + + Vector3 s=p_from-p_v0; + real_t u = f * s.dot(h); + + if ( u< 0.0 || u > 1.0) + return false; + + Vector3 q=s.cross(e1); + + real_t v = f * p_dir.dot(q); + + if (v < 0.0 || u + v > 1.0) + return false; + + // at this stage we can compute t to find out where + // the intersection point is on the line + real_t t = f * e2.dot(q); + + if (t > 0.00001) {// ray intersection + if (r_res) + *r_res=p_from+p_dir*t; + return true; + } else // this means that there is a line intersection + // but not a ray intersection + return false; + } + + static inline bool segment_intersects_triangle( const Vector3& p_from, const Vector3& p_to, const Vector3& p_v0,const Vector3& p_v1,const Vector3& p_v2,Vector3* r_res=0) { + + Vector3 rel=p_to-p_from; + Vector3 e1=p_v1-p_v0; + Vector3 e2=p_v2-p_v0; + Vector3 h = rel.cross(e2); + real_t a =e1.dot(h); + if (a>-CMP_EPSILON && a < CMP_EPSILON) // parallel test + return false; + + real_t f=1.0/a; + + Vector3 s=p_from-p_v0; + real_t u = f * s.dot(h); + + if ( u< 0.0 || u > 1.0) + return false; + + Vector3 q=s.cross(e1); + + real_t v = f * rel.dot(q); + + if (v < 0.0 || u + v > 1.0) + return false; + + // at this stage we can compute t to find out where + // the intersection point is on the line + real_t t = f * e2.dot(q); + + if (t > CMP_EPSILON && t<=1.0) {// ray intersection + if (r_res) + *r_res=p_from+rel*t; + return true; + } else // this means that there is a line intersection + // but not a ray intersection + return false; + } + + static inline bool segment_intersects_sphere( const Vector3& p_from, const Vector3& p_to, const Vector3& p_sphere_pos,real_t p_sphere_radius,Vector3* r_res=0,Vector3 *r_norm=0) { + + + Vector3 sphere_pos=p_sphere_pos-p_from; + Vector3 rel=(p_to-p_from); + float rel_l=rel.length(); + if (rel_l=p_sphere_radius) + return false; + + float inters_d2=p_sphere_radius*p_sphere_radius - ray_distance*ray_distance; + float inters_d=sphere_d; + + if (inters_d2>=CMP_EPSILON) + inters_d-=Math::sqrt(inters_d2); + + // check in segment + if (inters_d<0 || inters_d>rel_l) + return false; + + Vector3 result=p_from+normal*inters_d;; + + if (r_res) + *r_res=result; + if (r_norm) + *r_norm=(result-p_sphere_pos).normalized(); + + 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) { + + Vector3 rel=(p_to-p_from); + float rel_l=rel.length(); + if (rel_l=p_radius) + return false; // too far away + + // convert to 2D + float w2=p_radius*p_radius-dist*dist; + if (w2 box_end || seg_to < box_begin) + return false; + real_t length=seg_to-seg_from; + cmin = (seg_from < box_begin)?((box_begin - seg_from)/length):0; + cmax = (seg_to > box_end)?((box_end - seg_from)/length):1; + + } else { + + if (seg_to > box_end || seg_from < box_begin) + return false; + real_t length=seg_to-seg_from; + cmin = (seg_from > box_end)?(box_end - seg_from)/length:0; + cmax = (seg_to < box_begin)?(box_begin - seg_from)/length:1; + } + + if (cmin > min) { + min = cmin; + axis=i; + } + if (cmax < max) + max = cmax; + if (max < min) + return false; + } + + + // convert to 3D again + Vector3 result = p_from + (rel*min); + Vector3 res_normal = result; + + if (axis==0) { + res_normal.z=0; + } else { + res_normal.x=0; + res_normal.y=0; + } + + + res_normal.normalize(); + + if (r_res) + *r_res=result; + if (r_norm) + *r_norm=res_normal; + + return true; + } + + + static bool segment_intersects_convex(const Vector3& p_from, const Vector3& p_to,const Plane* p_planes, int p_plane_count,Vector3 *p_res, Vector3 *p_norm) { + + real_t min=-1e20,max=1e20; + + Vector3 rel=p_to-p_from; + real_t rel_l=rel.length(); + + if (rel_l0) { + //backwards facing plane + if (distmin) { + min=dist; + min_index=i; + } + } + } + + if (max<=min || min<0 || min>rel_l || min_index==-1) // exit conditions + return false; // no intersection + + if (p_res) + *p_res=p_from+dir*min; + if (p_norm) + *p_norm=p_planes[min_index].normal; + + return true; + } + + static Vector3 get_closest_point_to_segment(const Vector3& p_point, const Vector3 *p_segment) { + + Vector3 p=p_point-p_segment[0]; + Vector3 n=p_segment[1]-p_segment[0]; + float 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); + + if (d<=0.0) + return p_segment[0]; // before first point + else if (d>=l) + return p_segment[1]; // after first point + else + return p_segment[0]+n*d; // inside + } + + static Vector3 get_closest_point_to_segment_uncapped(const Vector3& p_point, const Vector3 *p_segment) { + + Vector3 p=p_point-p_segment[0]; + Vector3 n=p_segment[1]-p_segment[0]; + float 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); + + return p_segment[0]+n*d; // inside + } + + static Vector2 get_closest_point_to_segment_2d(const Vector2& p_point, const Vector2 *p_segment) { + + Vector2 p=p_point-p_segment[0]; + Vector2 n=p_segment[1]-p_segment[0]; + float 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); + + if (d<=0.0) + return p_segment[0]; // before first point + else if (d>=l) + return p_segment[1]; // after first point + else + return p_segment[0]+n*d; // inside + } + static Vector2 get_closest_point_to_segment_uncapped_2d(const Vector2& p_point, const Vector2 *p_segment) { + + Vector2 p=p_point-p_segment[0]; + Vector2 n=p_segment[1]-p_segment[0]; + float 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); + + return p_segment[0]+n*d; // inside + } + + static bool segment_intersects_segment_2d(const Vector2& p_from_a,const Vector2& p_to_a,const Vector2& p_from_b,const Vector2& p_to_b,Vector2* r_result) { + + Vector2 B = p_to_a-p_from_a; + Vector2 C = p_from_b-p_from_a; + Vector2 D = p_to_b-p_from_a; + + real_t ABlen = B.dot(B); + if (ABlen<=0) + return false; + Vector2 Bn = B/ABlen; + C = Vector2( C.x*Bn.x + C.y*Bn.y, C.y*Bn.x - C.x*Bn.y ); + D = Vector2( D.x*Bn.x + D.y*Bn.y, D.y*Bn.x - D.x*Bn.y ); + + 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); + + // Fail if segment C-D crosses line A-B outside of segment A-B. + if (ABpos<0 || ABpos>1.0) + return false; + + // (4) Apply the discovered position to line A-B in the original coordinate system. + if (r_result) + *r_result=p_from_a+B*ABpos; + + return true; + } + + + static inline bool point_in_projected_triangle(const Vector3& p_point,const Vector3& p_v1,const Vector3& p_v2,const Vector3& p_v3) { + + + Vector3 face_n = (p_v1-p_v3).cross(p_v1-p_v2); + + Vector3 n1 = (p_point-p_v3).cross(p_point-p_v2); + + if (face_n.dot(n1)<0) + return false; + + Vector3 n2 = (p_v1-p_v3).cross(p_v1-p_point); + + if (face_n.dot(n2)<0) + return false; + + Vector3 n3 = (p_v1-p_point).cross(p_v1-p_v2); + + if (face_n.dot(n3)<0) + return false; + + return true; + + } + + 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]); + + if (d > p_sphere_radius || d < -p_sphere_radius) // not touching the plane of the face, return + return false; + + Vector3 contact=p_sphere_pos - (p_normal*d); + + /** 2nd) TEST INSIDE TRIANGLE **/ + + + if (Geometry::point_in_projected_triangle(contact,p_triangle[0],p_triangle[1],p_triangle[2])) { + r_triangle_contact=contact; + r_sphere_contact=p_sphere_pos-p_normal*p_sphere_radius; + //printf("solved inside triangle\n"); + return true; + } + + /** 3rd TEST INSIDE EDGE CYLINDERS **/ + + const Vector3 verts[4]={p_triangle[0],p_triangle[1],p_triangle[2],p_triangle[0]}; // for() friendly + + for (int i=0;i<3;i++) { + + // check edge cylinder + + Vector3 n1=verts[i]-verts[i+1]; + Vector3 n2=p_sphere_pos-verts[i+1]; + + ///@TODO i could discard by range here to make the algorithm quicker? dunno.. + + // check point within cylinder radius + Vector3 axis =n1.cross(n2).cross(n1); + axis.normalize(); // ugh + + float ad=axis.dot(n2); + + if (ABS(ad)>p_sphere_radius) { + // no chance with this edge, too far away + continue; + } + + // check point within edge capsule cylinder + /** 4th TEST INSIDE EDGE POINTS **/ + + float sphere_at = n1.dot(n2); + + if (sphere_at>=0 && sphere_at= 0 && res1 <= 1) ? res1 : -1; + + } + + + static Vector triangulate_polygon(const Vector& p_polygon) { + + Vector triangles; + if (!Triangulate::triangulate(p_polygon,triangles)) + return Vector(); //fail + return triangles; + } + + static Vector< Vector > (*_decompose_func)(const Vector& p_polygon); + static Vector< Vector > decompose_polygon(const Vector& p_polygon) { + + if (_decompose_func) + return _decompose_func(p_polygon); + + return Vector< Vector >(); + + } + + + static DVector< DVector< Face3 > > separate_objects( DVector< Face3 > p_array ); + + static DVector< Face3 > wrap_geometry( DVector< Face3 > p_array, float *p_error=NULL ); ///< create a "wrap" that encloses the given geometry + + + struct MeshData { + + struct Face { + Plane plane; + Vector indices; + }; + + Vector faces; + + struct Edge { + + int a,b; + }; + + Vector edges; + + Vector< Vector3 > vertices; + + void optimize_vertices(); + }; + + + static MeshData build_convex_mesh(const DVector &p_planes); + static DVector build_sphere_planes(float p_radius, int p_lats, int p_lons, Vector3::Axis p_axis=Vector3::AXIS_Z); + static DVector build_box_planes(const Vector3& p_extents); + static DVector build_cylinder_planes(float p_radius, float p_height, int p_sides, Vector3::Axis p_axis=Vector3::AXIS_Z); + static DVector build_capsule_planes(float p_radius, float p_height, int p_sides, int p_lats, Vector3::Axis p_axis=Vector3::AXIS_Z); + + +}; + + + +#endif diff --git a/core/math/math_2d.cpp b/core/math/math_2d.cpp new file mode 100644 index 000000000..dbeaea6e7 --- /dev/null +++ b/core/math/math_2d.cpp @@ -0,0 +1,597 @@ +/*************************************************************************/ +/* math_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "math_2d.h" + + +real_t Vector2::atan2() const { + + return Math::atan2(x,y); +} + +float Vector2::length() const { + + return Math::sqrt( x*x + y*y ); +} + +float Vector2::length_squared() const { + + return x*x + y*y; +} + +void Vector2::normalize() { + + float l = x*x + y*y; + if (l!=0) { + + l=Math::sqrt(l); + x/=l; + y/=l; + } +} + +Vector2 Vector2::normalized() const { + + Vector2 v=*this; + v.normalize(); + return v; +} + +float 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 { + + 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 { + + return Math::atan2( x-p_vector2.x, y - p_vector2.y ); +} + +float Vector2::dot(const Vector2& p_other) const { + + return x*p_other.x + y*p_other.y; +} + +float Vector2::cross(const Vector2& p_other) const { + + return x*p_other.y - y*p_other.x; +} + +Vector2 Vector2::cross(real_t p_other) const { + + return Vector2(p_other*y,-p_other*x); +} + + +Vector2 Vector2::operator+(const Vector2& p_v) const { + + return Vector2(x+p_v.x,y+p_v.y); +} +void Vector2::operator+=(const Vector2& p_v) { + + x+=p_v.x; y+=p_v.y; +} +Vector2 Vector2::operator-(const Vector2& p_v) const { + + return Vector2(x-p_v.x,y-p_v.y); +} +void Vector2::operator-=(const Vector2& p_v) { + + x-=p_v.x; y-=p_v.y; +} + +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 { + + return Vector2(x * rvalue, y * rvalue); +}; +void Vector2::operator*=(const float &rvalue) { + + x *= rvalue; y *= rvalue; +}; + +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 { + + return Vector2(x / rvalue, y / rvalue); +}; + +void Vector2::operator/=(const float &rvalue) { + + x /= rvalue; y /= rvalue; +}; + +Vector2 Vector2::operator-() const { + + return Vector2(-x,-y); +} + +bool Vector2::operator==(const Vector2& p_vec2) const { + + return x==p_vec2.x && y==p_vec2.y; +} +bool Vector2::operator!=(const Vector2& p_vec2) const { + + return x!=p_vec2.x || y!=p_vec2.y; +} +Vector2 Vector2::floor() const { + + return Vector2( Math::floor(x), Math::floor(y) ); +} + +Vector2 Vector2::rotated(float p_by) const { + + Vector2 v; + v.set_rotation(atan2()+p_by); + v*=length(); + return v; +} + +Vector2 Vector2::project(const Vector2& p_vec) const { + + Vector2 v1=p_vec; + Vector2 v2=*this; + return v2 * ( v1.dot(v2) / v2.dot(v2)); +} + +Vector2 Vector2::snapped(const Vector2& p_by) const { + + return Vector2( + Math::stepify(x,p_by.x), + Math::stepify(y,p_by.y) + ); +} + +Vector2 Vector2::clamped(real_t p_len) const { + + return *this; + real_t l = length(); + Vector2 v = *this; + if (l>0 && p_len box_end || seg_to < box_begin) + return false; + real_t length=seg_to-seg_from; + cmin = (seg_from < box_begin)?((box_begin - seg_from)/length):0; + cmax = (seg_to > box_end)?((box_end - seg_from)/length):1; + csign=-1.0; + + } else { + + if (seg_to > box_end || seg_from < box_begin) + return false; + real_t length=seg_to-seg_from; + cmin = (seg_from > box_end)?(box_end - seg_from)/length:0; + cmax = (seg_to < box_begin)?(box_begin - seg_from)/length:1; + csign=1.0; + } + + if (cmin > min) { + min = cmin; + axis=i; + sign=csign; + } + if (cmax < max) + max = cmax; + if (max < min) + return false; + } + + + Vector2 rel=p_to-p_from; + + if (r_normal) { + Vector2 normal; + normal[axis]=sign; + *r_normal=normal; + } + + if (r_pos) + *r_pos=p_from+rel*min; + + return true; +} + +/* Point2i */ + +Point2i Point2i::operator+(const Point2i& p_v) const { + + return Point2i(x+p_v.x,y+p_v.y); +} +void Point2i::operator+=(const Point2i& p_v) { + + x+=p_v.x; y+=p_v.y; +} +Point2i Point2i::operator-(const Point2i& p_v) const { + + return Point2i(x-p_v.x,y-p_v.y); +} +void Point2i::operator-=(const Point2i& p_v) { + + x-=p_v.x; y-=p_v.y; +} + +Point2i Point2i::operator*(const Point2i &p_v1) const { + + return Point2i(x * p_v1.x, y * p_v1.y); +}; + +Point2i Point2i::operator*(const int &rvalue) const { + + return Point2i(x * rvalue, y * rvalue); +}; +void Point2i::operator*=(const int &rvalue) { + + x *= rvalue; y *= rvalue; +}; + +Point2i Point2i::operator/(const Point2i &p_v1) const { + + return Point2i(x / p_v1.x, y / p_v1.y); +}; + +Point2i Point2i::operator/(const int &rvalue) const { + + return Point2i(x / rvalue, y / rvalue); +}; + +void Point2i::operator/=(const int &rvalue) { + + x /= rvalue; y /= rvalue; +}; + +Point2i Point2i::operator-() const { + + return Point2i(-x,-y); +} + +bool Point2i::operator==(const Point2i& p_vec2) const { + + return x==p_vec2.x && y==p_vec2.y; +} +bool Point2i::operator!=(const Point2i& p_vec2) const { + + return x!=p_vec2.x || y!=p_vec2.y; +} + +void Matrix32::invert() { + + SWAP(elements[0][1],elements[1][0]); + elements[2] = basis_xform(-elements[2]); +} + +Matrix32 Matrix32::inverse() const { + + Matrix32 inv=*this; + inv.invert(); + return inv; + +} + +void Matrix32::affine_invert() { + + float det = elements[0][0]*elements[1][1] - elements[1][0]*elements[0][1]; + ERR_FAIL_COND(det==0); + float idet = 1.0 / det; + + SWAP( elements[0][0],elements[1][1] ); + elements[0]*=Vector2(idet,-idet); + elements[1]*=Vector2(-idet,idet); + + elements[2] = basis_xform(-elements[2]); + +} + +Matrix32 Matrix32::affine_inverse() const { + + Matrix32 inv=*this; + inv.affine_invert(); + return inv; +} + +void Matrix32::rotate(real_t p_phi) { + + Matrix32 rot(p_phi,Vector2()); + *this *= rot; +} + +real_t Matrix32::get_rotation() const { + + return Math::atan2(elements[1].x,elements[1].y); +} + +void Matrix32::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[1][1]=cr; + elements[0][1]=-sr; + elements[1][0]=sr; +} + +Matrix32::Matrix32(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[1][1]=cr; + elements[0][1]=-sr; + elements[1][0]=sr; + elements[2]=p_pos; +} + +Vector2 Matrix32::get_scale() const { + + return Vector2( elements[0].length(), elements[1].length() ); +} + +void Matrix32::scale(const Vector2& p_scale) { + + elements[0]*=p_scale; + elements[1]*=p_scale; + elements[2]*=p_scale; +} +void Matrix32::scale_basis(const Vector2& p_scale) { + + elements[0]*=p_scale; + elements[1]*=p_scale; + +} +void Matrix32::translate( real_t p_tx, real_t p_ty) { + + translate(Vector2(p_tx,p_ty)); +} +void Matrix32::translate( const Vector2& p_translation ) { + + elements[2]+=basis_xform(p_translation); +} + + +void Matrix32::orthonormalize() { + + // Gram-Schmidt Process + + Vector2 x=elements[0]; + Vector2 y=elements[1]; + + x.normalize(); + y = (y-x*(x.dot(y))); + y.normalize(); + + elements[0]=x; + elements[1]=y; +} +Matrix32 Matrix32::orthonormalized() const { + + Matrix32 on=*this; + on.orthonormalize(); + return on; + +} + +bool Matrix32::operator==(const Matrix32& p_transform) const { + + for(int i=0;i<3;i++) { + if (elements[i]!=p_transform.elements[i]) + return false; + } + + return true; +} + +bool Matrix32::operator!=(const Matrix32& p_transform) const { + + for(int i=0;i<3;i++) { + if (elements[i]!=p_transform.elements[i]) + return true; + } + + return false; + +} + +void Matrix32::operator*=(const Matrix32& p_transform) { + + elements[2] = xform(p_transform.elements[2]); + + float x0,x1,y0,y1; +/* + x0 = p_transform.tdotx(elements[0]); + x1 = p_transform.tdoty(elements[0]); + y0 = p_transform.tdotx(elements[1]); + y1 = p_transform.tdoty(elements[1]);*/ + + x0 = tdotx(p_transform.elements[0]); + x1 = tdoty(p_transform.elements[0]); + y0 = tdotx(p_transform.elements[1]); + y1 = tdoty(p_transform.elements[1]); + + elements[0][0]=x0; + elements[0][1]=x1; + elements[1][0]=y0; + elements[1][1]=y1; +} + + +Matrix32 Matrix32::operator*(const Matrix32& p_transform) const { + + Matrix32 t = *this; + t*=p_transform; + return t; + +} + +Matrix32 Matrix32::scaled(const Vector2& p_scale) const { + + Matrix32 copy=*this; + copy.scale(p_scale); + return copy; + +} + +Matrix32 Matrix32::basis_scaled(const Vector2& p_scale) const { + + Matrix32 copy=*this; + copy.scale_basis(p_scale); + return copy; + +} + +Matrix32 Matrix32::untranslated() const { + + Matrix32 copy=*this; + copy.elements[2]=Vector2(); + return copy; +} + +Matrix32 Matrix32::translated(const Vector2& p_offset) const { + + Matrix32 copy=*this; + copy.translate(p_offset); + return copy; + +} + +Matrix32 Matrix32::rotated(float p_phi) const { + + Matrix32 copy=*this; + copy.rotate(p_phi); + return copy; + +} + + +Matrix32 Matrix32::interpolate_with(const Matrix32& p_transform, float p_c) const { + + + return Matrix32(); +} + +Matrix32::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 new file mode 100644 index 000000000..212d848bc --- /dev/null +++ b/core/math/math_2d.h @@ -0,0 +1,670 @@ +/*************************************************************************/ +/* math_2d.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 MATH_2D_H +#define MATH_2D_H + +#include "math_funcs.h" +#include "ustring.h" +/** + @author Juan Linietsky +*/ +enum Margin { + + MARGIN_LEFT, + MARGIN_TOP, + MARGIN_RIGHT, + MARGIN_BOTTOM +}; + +enum Orientation { + + HORIZONTAL, + VERTICAL +}; + +enum HAlign { + + HALIGN_LEFT, + HALIGN_CENTER, + HALIGN_RIGHT +}; + +enum VAlign { + + VALIGN_TOP, + VALIGN_CENTER, + VALIGN_BOTTOM +}; + +struct Vector2 { + + union { + float x; + float width; + }; + union { + float y; + float height; + }; + + + _FORCE_INLINE_ float& operator[](int p_idx) { + return p_idx?y:x; + } + _FORCE_INLINE_ const float& operator[](int p_idx) const { + return p_idx?y:x; + } + + void normalize(); + Vector2 normalized() const; + + float length() const; + float 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 dot(const Vector2& p_other) const; + float cross(const Vector2& p_other) const; + Vector2 cross(real_t p_other) const; + Vector2 project(const Vector2& p_vec) const; + + Vector2 plane_project(real_t p_d, const Vector2& p_vec) const; + + Vector2 clamped(real_t p_len) const; + + _FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2& p_a, const Vector2& p_b,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; + + + Vector2 operator+(const Vector2& p_v) const; + void operator+=(const Vector2& p_v); + Vector2 operator-(const Vector2& p_v) const; + void operator-=(const Vector2& p_v); + Vector2 operator*(const Vector2 &p_v1) const; + + Vector2 operator*(const float &rvalue) const; + void operator*=(const float &rvalue); + void operator*=(const Vector2 &rvalue) { *this = *this * rvalue; } + + Vector2 operator/(const Vector2 &p_v1) const; + + Vector2 operator/(const float &rvalue) const; + + void operator/=(const float &rvalue); + + Vector2 operator-() const; + + bool operator==(const Vector2& p_vec2) const; + bool operator!=(const Vector2& p_vec2) const; + + bool operator<(const Vector2& p_vec2) const { return (x==p_vec2.x)?(y (p_rect.pos.x + p_rect.size.width) ) + return false; + if ( (pos.x+size.width) < p_rect.pos.x ) + return false; + if ( pos.y > (p_rect.pos.y + p_rect.size.height) ) + return false; + if ( (pos.y+size.height) < p_rect.pos.y ) + return false; + + return true; + } + + bool intersects_segment(const Point2& p_from, const Point2& p_to, Point2* r_pos=NULL, Point2* r_normal=NULL) const; + + inline bool encloses(const Rect2& p_rect) const { + + return (p_rect.pos.x>=pos.x) && (p_rect.pos.y>=pos.y) && + ((p_rect.pos.x+p_rect.size.x)<(pos.x+size.x)) && + ((p_rect.pos.y+p_rect.size.y)<(pos.y+size.y)); + + } + + inline bool has_no_area() const { + + return (size.x<=0 || size.y<=0); + + } + inline Rect2 clip(const Rect2& p_rect) const { /// return a clipped rect + + Rect2 new_rect=p_rect; + + if (!intersects( new_rect )) + return Rect2(); + + new_rect.pos.x = MAX( p_rect.pos.x , pos.x ); + new_rect.pos.y = MAX( p_rect.pos.y , pos.y ); + + Point2 p_rect_end=p_rect.pos+p_rect.size; + Point2 end=pos+size; + + new_rect.size.x=MIN(p_rect_end.x,end.x) - new_rect.pos.x; + new_rect.size.y=MIN(p_rect_end.y,end.y) - new_rect.pos.y; + + return new_rect; + } + + inline Rect2 merge(const Rect2& p_rect) const { ///< return a merged rect + + Rect2 new_rect; + + new_rect.pos.x=MIN( p_rect.pos.x , pos.x ); + new_rect.pos.y=MIN( p_rect.pos.y , pos.y ); + + + new_rect.size.x = MAX( p_rect.pos.x+p_rect.size.x , pos.x+size.x ); + new_rect.size.y = MAX( p_rect.pos.y+p_rect.size.y , pos.y+size.y ); + + new_rect.size = new_rect.size - new_rect.pos; //make relative again + + return new_rect; + }; + bool has_point(const Point2& p_point) const { + if (p_point.x < pos.x) + return false; + if (p_point.y < pos.y) + return false; + + if (p_point.x >= (pos.x+size.x) ) + return false; + if (p_point.y >= (pos.y+size.y) ) + return false; + + return true; + } + + bool no_area() const { return (size.width<=0 || size.height<=0 ); } + + bool operator==(const Rect2& p_rect) const { return pos==p_rect.pos && size==p_rect.size; } + bool operator!=(const Rect2& p_rect) const { return pos!=p_rect.pos || size!=p_rect.size; } + + Rect2 grow(real_t p_by) const { + + Rect2 g=*this; + g.pos.x-=p_by; + g.pos.y-=p_by; + g.size.width+=p_by*2; + g.size.height+=p_by*2; + return g; + } + + inline Rect2 expand(const Vector2& p_vector) const { + + Rect2 r = *this; + r.expand_to(p_vector); + return r; + } + + inline void expand_to(const Vector2& p_vector) { //in place function for speed + + Vector2 begin=pos; + Vector2 end=pos+size; + + if (p_vector.xend.x) + end.x=p_vector.x; + if (p_vector.y>end.y) + end.y=p_vector.y; + + pos=begin; + size=end-begin; + } + + + 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( const Point2& p_pos, const Size2& p_size ) { pos=p_pos; size=p_size; } +}; + + +/* INTEGER STUFF */ + +struct Point2i { + + union { + int x; + int width; + }; + union { + int y; + int height; + }; + + + _FORCE_INLINE_ int& operator[](int p_idx) { + return p_idx?y:x; + } + _FORCE_INLINE_ const int& operator[](int p_idx) const { + return p_idx?y:x; + } + + Point2i operator+(const Point2i& p_v) const; + void operator+=(const Point2i& p_v); + Point2i operator-(const Point2i& p_v) const; + void operator-=(const Point2i& p_v); + Point2i operator*(const Point2i &p_v1) const; + + Point2i operator*(const int &rvalue) const; + void operator*=(const int &rvalue); + + Point2i operator/(const Point2i &p_v1) const; + + Point2i operator/(const int &rvalue) const; + + void operator/=(const int &rvalue); + + Point2i operator-() const; + bool operator<(const Point2i& p_vec2) const { return (x==p_vec2.x)?(y(const Point2i& p_vec2) const { return (x==p_vec2.x)?(y>p_vec2.y):(x>p_vec2.x); } + + bool operator==(const Point2i& p_vec2) const; + bool operator!=(const Point2i& p_vec2) const; + + float get_aspect() const { return width/(float)height; } + + operator String() const { return String::num(x)+","+String::num(y); } + + operator Vector2() const { return Vector2(x,y); } + inline Point2i(const Vector2& p_vec2) { x=(int)p_vec2.x; y=(int)p_vec2.y; } + inline Point2i(int p_x,int p_y) { x=p_x; y=p_y; } + inline Point2i() { x=0; y=0; } +}; + +typedef Point2i Size2i; + +struct Rect2i { + + Point2i pos; + Size2i size; + + const Point2i& get_pos() const { return pos; } + void set_pos(const Point2i& p_pos) { pos=p_pos; } + const Point2i& get_size() const { return size; } + void set_size(const Point2i& p_size) { size=p_size; } + + int get_area() const { return size.width*size.height; } + + inline bool intersects(const Rect2i& p_rect) const { + if ( pos.x > (p_rect.pos.x + p_rect.size.width) ) + return false; + if ( (pos.x+size.width) < p_rect.pos.x ) + return false; + if ( pos.y > (p_rect.pos.y + p_rect.size.height) ) + return false; + if ( (pos.y+size.height) < p_rect.pos.y ) + return false; + + return true; + } + + inline bool encloses(const Rect2i& p_rect) const { + + return (p_rect.pos.x>=pos.x) && (p_rect.pos.y>=pos.y) && + ((p_rect.pos.x+p_rect.size.x)<(pos.x+size.x)) && + ((p_rect.pos.y+p_rect.size.y)<(pos.y+size.y)); + + } + + inline bool has_no_area() const { + + return (size.x<=0 || size.y<=0); + + } + inline Rect2i clip(const Rect2i& p_rect) const { /// return a clipped rect + + Rect2i new_rect=p_rect; + + if (!intersects( new_rect )) + return Rect2i(); + + new_rect.pos.x = MAX( p_rect.pos.x , pos.x ); + new_rect.pos.y = MAX( p_rect.pos.y , pos.y ); + + Point2 p_rect_end=p_rect.pos+p_rect.size; + Point2 end=pos+size; + + new_rect.size.x=(int)(MIN(p_rect_end.x,end.x) - new_rect.pos.x); + new_rect.size.y=(int)(MIN(p_rect_end.y,end.y) - new_rect.pos.y); + + return new_rect; + } + + inline Rect2i merge(const Rect2i& p_rect) const { ///< return a merged rect + + Rect2i new_rect; + + new_rect.pos.x=MIN( p_rect.pos.x , pos.x ); + new_rect.pos.y=MIN( p_rect.pos.y , pos.y ); + + + new_rect.size.x = MAX( p_rect.pos.x+p_rect.size.x , pos.x+size.x ); + new_rect.size.y = MAX( p_rect.pos.y+p_rect.size.y , pos.y+size.y ); + + new_rect.size = new_rect.size - new_rect.pos; //make relative again + + return new_rect; + }; + bool has_point(const Point2& p_point) { + if (p_point.x < pos.x) + return false; + if (p_point.y < pos.y) + return false; + + if (p_point.x >= (pos.x+size.x) ) + return false; + if (p_point.y >= (pos.y+size.y) ) + return false; + + return true; + } + + bool no_area() { return (size.width<=0 || size.height<=0 ); } + + bool operator==(const Rect2i& p_rect) const { return pos==p_rect.pos && size==p_rect.size; } + bool operator!=(const Rect2i& p_rect) const { return pos!=p_rect.pos || size!=p_rect.size; } + + Rect2i grow(int p_by) const { + + Rect2i g=*this; + g.pos.x-=p_by; + g.pos.y-=p_by; + g.size.width+=p_by*2; + g.size.height+=p_by*2; + return g; + } + + inline void expand_to(const Point2i& p_vector) { + + Point2i begin=pos; + Point2i end=pos+size; + + if (p_vector.xend.x) + end.x=p_vector.x; + if (p_vector.y>end.y) + end.y=p_vector.y; + + pos=begin; + size=end-begin; + } + + + operator String() const { return String(pos)+","+String(size); } + + operator Rect2() const { return Rect2(pos,size); } + Rect2i(const Rect2& p_r2) { pos=p_r2.pos; size=p_r2.size; } + Rect2i() {} + Rect2i( int p_x, int p_y, int p_width, int p_height) { pos=Point2(p_x,p_y); size=Size2( p_width, p_height ); } + Rect2i( const Point2& p_pos, const Size2& p_size ) { pos=p_pos; size=p_size; } +}; + + + +struct Matrix32 { + + + 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; } + + const Vector2& operator[](int p_idx) const { return elements[p_idx]; } + Vector2& operator[](int p_idx) { return elements[p_idx]; } + + _FORCE_INLINE_ Vector2 get_axis(int p_axis) const { ERR_FAIL_INDEX_V(p_axis,3,Vector2()); return elements[p_axis]; } + _FORCE_INLINE_ void set_axis(int p_axis,const Vector2& p_vec) { ERR_FAIL_INDEX(p_axis,3); elements[p_axis]=p_vec; } + + void invert(); + Matrix32 inverse() const; + + void affine_invert(); + Matrix32 affine_inverse() const; + + void set_rotation(real_t p_phi); + real_t get_rotation() const; + _FORCE_INLINE_ void set_rotation_and_scale(real_t p_phi,const Size2& p_scale); + void rotate(real_t p_phi); + + void scale(const Vector2& p_scale); + void scale_basis(const Vector2& p_scale); + void translate( real_t p_tx, real_t p_ty); + void translate( const Vector2& p_translation ); + Vector2 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 Vector2& p_scale) const; + Matrix32 basis_scaled(const Vector2& p_scale) const; + Matrix32 translated(const Vector2& p_offset) const; + Matrix32 rotated(float p_phi) const; + + Matrix32 untranslated() const; + + + void orthonormalize(); + Matrix32 orthonormalized() const; + + bool operator==(const Matrix32& p_transform) const; + bool operator!=(const Matrix32& p_transform) const; + + void operator*=(const Matrix32& p_transform); + Matrix32 operator*(const Matrix32& p_transform) const; + + Matrix32 interpolate_with(const Matrix32& p_transform, float p_c) const; + + _FORCE_INLINE_ Vector2 basis_xform(const Vector2& p_vec) const; + _FORCE_INLINE_ Vector2 basis_xform_inv(const Vector2& p_vec) const; + _FORCE_INLINE_ Vector2 xform(const Vector2& p_vec) const; + _FORCE_INLINE_ Vector2 xform_inv(const Vector2& p_vec) const; + _FORCE_INLINE_ Rect2 xform(const Rect2& p_vec) const; + _FORCE_INLINE_ Rect2 xform_inv(const Rect2& p_vec) const; + + + operator String() const; + + + Matrix32(real_t p_rot, const Vector2& p_pos); + Matrix32() { elements[0][0]=1.0; elements[1][1]=1.0; } + +}; + + +Vector2 Matrix32::basis_xform(const Vector2& v) const { + + return Vector2( + tdotx(v), + tdoty(v) + ); +} + +Vector2 Matrix32::basis_xform_inv(const Vector2& v) const{ + + return Vector2( + elements[0].dot(v), + elements[1].dot(v) + ); +} + +Vector2 Matrix32::xform(const Vector2& v) const { + + return Vector2( + tdotx(v), + tdoty(v) + ) + elements[2]; +} +Vector2 Matrix32::xform_inv(const Vector2& p_vec) const { + + Vector2 v = p_vec - elements[2]; + + return Vector2( + elements[0].dot(v), + elements[1].dot(v) + ); + +} +Rect2 Matrix32::xform(const Rect2& p_rect) const { + + Vector2 x=elements[0]*p_rect.size.x; + Vector2 y=elements[1]*p_rect.size.y; + Vector2 pos = xform( p_rect.pos ); + + Rect2 new_rect; + new_rect.pos=pos; + new_rect.expand_to( pos+x ); + new_rect.expand_to( pos+y ); + new_rect.expand_to( pos+x+y ); + return new_rect; +} + +void Matrix32::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; + +} + +Rect2 Matrix32::xform_inv(const Rect2& p_rect) const { + + Vector2 ends[4]={ + xform_inv( p_rect.pos ), + xform_inv( Vector2(p_rect.pos.x,p_rect.pos.y+p_rect.size.y ) ), + xform_inv( Vector2(p_rect.pos.x+p_rect.size.x,p_rect.pos.y+p_rect.size.y ) ), + xform_inv( Vector2(p_rect.pos.x+p_rect.size.x,p_rect.pos.y ) ) + }; + + Rect2 new_rect; + new_rect.pos=ends[0]; + new_rect.expand_to(ends[1]); + new_rect.expand_to(ends[2]); + new_rect.expand_to(ends[3]); + + return new_rect; +} + + +#endif diff --git a/core/math/math_defs.cpp b/core/math/math_defs.cpp new file mode 100644 index 000000000..ca43bc7ae --- /dev/null +++ b/core/math/math_defs.cpp @@ -0,0 +1,30 @@ +/*************************************************************************/ +/* math_defs.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "math_defs.h" + diff --git a/core/math/math_defs.h b/core/math/math_defs.h new file mode 100644 index 000000000..dd0390240 --- /dev/null +++ b/core/math/math_defs.h @@ -0,0 +1,60 @@ +/*************************************************************************/ +/* math_defs.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 MATH_DEFS_H +#define MATH_DEFS_H + +#define CMP_EPSILON 0.00001 +#define CMP_EPSILON2 (CMP_EPSILON*CMP_EPSILON) +#define CMP_NORMALIZE_TOLERANCE 0.000001 +#define CMP_POINT_IN_PLANE_EPSILON 0.00001 + +/** + * "Real" is a type that will be translated to either floats or fixed depending + * on the compilation setting + */ + +enum ClockDirection { + + CLOCKWISE, + COUNTERCLOCKWISE +}; + + +#ifdef REAL_T_IS_DOUBLE + +typedef double real_t; + +#else + +typedef float real_t; + +#endif + + +#endif // MATH_DEFS_H diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp new file mode 100644 index 000000000..2ba36e5a4 --- /dev/null +++ b/core/math/math_funcs.cpp @@ -0,0 +1,328 @@ +/*************************************************************************/ +/* math_funcs.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "math_funcs.h" + +#include "core/os/os.h" +#include +#include "float.h" +uint32_t Math::default_seed=1; + + +#define PHI 0x9e3779b9 + +static uint32_t Q[4096], c = 362436; + + +uint32_t Math::rand_from_seed(uint32_t *seed) { + +#if 1 + uint32_t k; + uint32_t s = (*seed); + if (s == 0) + s = 0x12345987; + k = s / 127773; + s = 16807 * (s - k * 127773) - 2836 * k; + if (s < 0) + s += 2147483647; + (*seed) = s; + return (s & Math::RANDOM_MAX); +#else + *seed = *seed * 1103515245 + 12345; + return (*seed % ((unsigned int)RANDOM_MAX + 1)); +#endif +} + +void Math::seed(uint32_t x) { +#if 0 + int i; + + Q[0] = x; + Q[1] = x + PHI; + Q[2] = x + PHI + PHI; + + for (i = 3; i < 4096; i++) + Q[i] = Q[i - 3] ^ Q[i - 2] ^ PHI ^ i; +#else + default_seed=x; +#endif +} + +void Math::randomize() { + + seed(OS::get_singleton()->get_ticks_usec()); /* *OS::get_singleton()->get_time().sec); // windows doesn't have get_time(), returns always 0 */ +} + +uint32_t Math::rand() { + + return rand_from_seed(&default_seed)&0x7FFFFFFF; +} + +double Math::randf() { + + return (double)rand() / (double)RANDOM_MAX; +} + +double Math::sin(double p_x) { + + return ::sin(p_x); + +} + +double Math::cos(double p_x) { + + return ::cos(p_x); + +} + +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::decimals(double p_step) { + + int max=4; + int i=0; + while( (p_step - Math::floor(p_step)) != 0.0 && max) { + + p_step*=10.0; + max--; + i++; + } + + return i; + +} + +double Math::ease(double p_x, double p_c) { + + if (p_c>0) { + + return Math::pow(p_x,p_c); + } else if (p_c<0) { + //inout ease + + if (p_x<0.5) { + return Math::pow(p_x*2.0,-p_c)*0.5; + } else { + return (1.0-Math::pow(1.0-(p_x-0.5)*2.0,-p_c))*0.5+0.5; + } + } 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; + } + 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) { + + static const int primes[] = { + 5, + 13, + 23, + 47, + 97, + 193, + 389, + 769, + 1543, + 3079, + 6151, + 12289, + 24593, + 49157, + 98317, + 196613, + 393241, + 786433, + 1572869, + 3145739, + 6291469, + 12582917, + 25165843, + 50331653, + 100663319, + 201326611, + 402653189, + 805306457, + 1610612741, + 0, + }; + + int idx=0; + while (true) { + + ERR_FAIL_COND_V(primes[idx]==0,0); + if (primes[idx]>p_val) + return primes[idx]; + idx++; + } + + return 0; +} + +double Math::random(double from, double to) { + + unsigned int r = Math::rand(); + double ret = (double)r/(double)RANDOM_MAX; + return (ret)*(to-from) + from; +} + +double Math::pow(double x, double y) { + + return ::pow(x,y); +} + +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 new file mode 100644 index 000000000..6a60a7f79 --- /dev/null +++ b/core/math/math_funcs.h @@ -0,0 +1,180 @@ +/*************************************************************************/ +/* math_funcs.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 MATH_FUNCS_H +#define MATH_FUNCS_H + +#include "typedefs.h" +#include "math_defs.h" + +#ifndef NO_MATH_H +#include "math.h" +#endif + +class Math { + + + static uint32_t default_seed; +public: + 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 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 inline double linear2db(double p_linear) { + + return Math::log( p_linear ) * 8.6858896380650365530225783783321; + } + + static inline double db2linear(double p_linear) { + + return Math::exp( p_linear * 0.11512925464970228420089957273422 ); + } + + static bool is_nan(double p_val); + static bool is_inf(double p_val); + + + + static uint32_t rand(); + static double randf(); + + static double round(double p_val); + + static double random(double from, double to); + + + static _FORCE_INLINE_ real_t abs(real_t g) { + +#ifdef REAL_T_IS_DOUBLE + + return absd(g); +#else + + return absf(g); +#endif + } + + static _FORCE_INLINE_ float absf(float g) { + + union { + float f; + uint32_t i; + } u; + + u.f=g; + u.i&=2147483647u; + return u.f; + } + + static _FORCE_INLINE_ double absd(double g) { + + union { + double d; + uint64_t i; + } u; + u.d=g; + u.i&=(uint64_t)9223372036854775807ll; + return u.d; + } + + //this function should be as fast as possible and rounding mode should not matter + static _FORCE_INLINE_ int fast_ftoi(float a) { + + static int b; + +#if defined(_MSC_VER) + __asm fld a + __asm fistp b +/*#elif defined( __GNUC__ ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) + // use AT&T inline assembly style, document that + // we use memory as output (=m) and input (m) + __asm__ __volatile__ ( + "flds %1 \n\t" + "fistpl %0 \n\t" + : "=m" (b) + : "m" (a));*/ +#else + b=lrintf(a); //assuming everything but msvc has lrint +#endif + return b; + } + + +#if defined(__GNUC__) + + static _FORCE_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE +#else + + static _FORCE_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE +#endif + + static _FORCE_INLINE_ float lerp(float a, float b, float c) { + + return a+(b-a)*c; + } + + static double pow(double x, double y); + static double log(double x); + static double exp(double x); + +}; + + +#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 new file mode 100644 index 000000000..ff62e7786 --- /dev/null +++ b/core/math/matrix3.cpp @@ -0,0 +1,479 @@ +/*************************************************************************/ +/* matrix3.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "matrix3.h" +#include "math_funcs.h" +#include "os/copymem.h" + +#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) { + + if (Math::abs(p_z.z) > Math_SQRT12 ) { + + // choose p in y-z plane + real_t a = p_z[1]*p_z[1] + p_z[2]*p_z[2]; + real_t k = 1.0/Math::sqrt(a); + elements[0]=Vector3(0,-p_z[2]*k,p_z[1]*k); + elements[1]=Vector3(a*k,-p_z[0]*elements[0][2],p_z[0]*elements[0][1]); + } else { + + // choose p in x-y plane + real_t a = p_z.x*p_z.x + p_z.y*p_z.y; + real_t k = 1.0/Math::sqrt(a); + elements[0]=Vector3(-p_z.y*k,p_z.x*k,0); + elements[1]=Vector3(-p_z.z*elements[0].y,p_z.z*elements[0].x,a*k); + } + elements[2]=p_z; +} + +void Matrix3::invert() { + + + real_t co[3]={ + cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1) + }; + real_t det = elements[0][0] * co[0]+ + elements[0][1] * co[1]+ + elements[0][2] * co[2]; + + ERR_FAIL_COND( det == 0 ); + real_t s = 1.0/det; + + set( co[0]*s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co[1]*s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co[2]*s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s ); + +} + +void Matrix3::orthonormalize() { + + // Gram-Schmidt Process + + Vector3 x=get_axis(0); + Vector3 y=get_axis(1); + Vector3 z=get_axis(2); + + x.normalize(); + y = (y-x*(x.dot(y))); + y.normalize(); + z = (z-x*(x.dot(z))-y*(y.dot(z))); + z.normalize(); + + set_axis(0,x); + set_axis(1,y); + set_axis(2,z); + +} + +Matrix3 Matrix3::orthonormalized() const { + + Matrix3 c = *this; + c.orthonormalize(); + return c; +} + + +Matrix3 Matrix3::inverse() const { + + Matrix3 inv=*this; + inv.invert(); + return inv; +} + +void Matrix3::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 { + + Matrix3 tr=*this; + tr.transpose(); + return tr; +} + +void Matrix3::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[1][1]*=p_scale.y; + elements[2][1]*=p_scale.y; + elements[0][2]*=p_scale.z; + elements[1][2]*=p_scale.z; + elements[2][2]*=p_scale.z; +} + +Matrix3 Matrix3::scaled( const Vector3& p_scale ) const { + + Matrix3 m = *this; + m.scale(p_scale); + return m; +} + +Vector3 Matrix3::get_scale() const { + + return Vector3( + Vector3(elements[0][0],elements[1][0],elements[2][0]).length(), + Vector3(elements[0][1],elements[1][1],elements[2][1]).length(), + Vector3(elements[0][2],elements[1][2],elements[2][2]).length() + ); + +} +void Matrix3::rotate(const Vector3& p_axis, real_t p_phi) { + + *this = *this * Matrix3(p_axis, p_phi); +} + +Matrix3 Matrix3::rotated(const Vector3& p_axis, real_t p_phi) const { + + return *this * Matrix3(p_axis, p_phi); + +} + +Vector3 Matrix3::get_euler() const { + + // 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(); + + Vector3 euler; + + euler.y = Math::asin(m[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]); + + } else { + real_t r = Math::atan2(m[1][0],m[1][1]); + euler.z = 0.0; + euler.x = euler.z - r; + + } + } else { + real_t r = Math::atan2(m[0][1],m[1][1]); + euler.z = 0; + euler.x = r - euler.z; + } + + return euler; + + +} + +void Matrix3::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); + + 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); + + 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); + + //optimizer will optimize away all this anyway + *this = xmat*(ymat*zmat); +} + +bool Matrix3::operator==(const Matrix3& p_matrix) const { + + for (int i=0;i<3;i++) { + for (int j=0;j<3;j++) { + if (elements[i][j]!=p_matrix.elements[i][j]) + return false; + } + } + + return true; +} +bool Matrix3::operator!=(const Matrix3& p_matrix) const { + + return (!(*this==p_matrix)); +} + +Matrix3::operator String() const { + + String mtx; + for (int i=0;i<3;i++) { + + for (int j=0;j<3;j++) { + + if (i!=0 || j!=0) + mtx+=", "; + + mtx+=rtos( elements[i][j] ); + } + } + + return mtx; +} + +Matrix3::operator Quat() const { + + Matrix3 m=*this; + m.orthonormalize(); + + real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; + real_t temp[4]; + + if (trace > 0.0) + { + real_t s = Math::sqrt(trace + 1.0); + temp[3]=(s * 0.5); + s = 0.5 / s; + + temp[0]=((m.elements[2][1] - m.elements[1][2]) * s); + temp[1]=((m.elements[0][2] - m.elements[2][0]) * s); + temp[2]=((m.elements[1][0] - m.elements[0][1]) * s); + } + else + { + int i = m.elements[0][0] < m.elements[1][1] ? + (m.elements[1][1] < m.elements[2][2] ? 2 : 1) : + (m.elements[0][0] < m.elements[2][2] ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0); + temp[i] = s * 0.5; + s = 0.5 / s; + + temp[3] = (m.elements[k][j] - m.elements[j][k]) * s; + temp[j] = (m.elements[j][i] + m.elements[i][j]) * s; + temp[k] = (m.elements[k][i] + m.elements[i][k]) * s; + } + + return Quat(temp[0],temp[1],temp[2],temp[3]); + +} + +static const 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) +}; + +int Matrix3::get_orthogonal_index() const { + + //could be sped up if i come up with a way + Matrix3 orth=*this; + for(int i=0;i<3;i++) { + for(int j=0;j<3;j++) { + + float v = orth[i][j]; + if (v>0.5) + v=1.0; + else if (v<-0.5) + v=-1.0; + else + v=0; + + orth[i][j]=v; + } + } + + for(int i=0;i<24;i++) { + + if (_ortho_bases[i]==orth) + return i; + + + } + + return 0; +} + +void Matrix3::set_orthogonal_index(int p_index){ + + //there only exist 24 orthogonal bases in r3 + ERR_FAIL_INDEX(p_index,24); + + + *this=_ortho_bases[p_index]; + +} + + +void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const { + + + 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 + + if ( (Math::abs(elements[1][0]-elements[0][1])< epsilon) + && (Math::abs(elements[2][0]-elements[0][2])< epsilon) + && (Math::abs(elements[2][1]-elements[1][2])< epsilon)) { + // singularity found + // first check for identity matrix which must have +1 for all terms + // in leading diagonaland zero in other terms + if ((Math::abs(elements[1][0]+elements[0][1]) < epsilon2) + && (Math::abs(elements[2][0]+elements[0][2]) < epsilon2) + && (Math::abs(elements[2][1]+elements[1][2]) < epsilon2) + && (Math::abs(elements[0][0]+elements[1][1]+elements[2][2]-3) < epsilon2)) { + // this singularity is identity matrix so angle = 0 + r_axis=Vector3(0,1,0); + r_angle=0; + return; + } + // otherwise this singularity is angle = 180 + angle = Math_PI; + 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; + if ((xx > yy) && (xx > zz)) { // elements[0][0] is the largest diagonal term + if (xx< epsilon) { + x = 0; + y = 0.7071; + z = 0.7071; + } else { + x = Math::sqrt(xx); + y = xy/x; + z = xz/x; + } + } else if (yy > zz) { // elements[1][1] is the largest diagonal term + if (yy< epsilon) { + x = 0.7071; + y = 0; + z = 0.7071; + } else { + y = Math::sqrt(yy); + x = xy/y; + z = yz/y; + } + } else { // elements[2][2] is the largest diagonal term so base result on this + if (zz< epsilon) { + x = 0.7071; + y = 0.7071; + z = 0; + } else { + z = Math::sqrt(zz); + x = xz/z; + y = yz/z; + } + } + r_axis=Vector3(x,y,z); + r_angle=angle; + return; + } + // as we have reached here there are no singularities so we can handle normally + double 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 + 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; + + r_axis=Vector3(x,y,z); + r_angle=angle; +} + +Matrix3::Matrix3(const Vector3& p_euler) { + + set_euler( p_euler ); + +} + +Matrix3::Matrix3(const Quat& p_quat) { + + real_t d = p_quat.length_squared(); + real_t s = 2.0 / d; + real_t xs = p_quat.x * s, ys = p_quat.y * s, zs = p_quat.z * s; + real_t wx = p_quat.w * xs, wy = p_quat.w * ys, wz = p_quat.w * zs; + real_t xx = p_quat.x * xs, xy = p_quat.x * ys, xz = p_quat.x * zs; + real_t yy = p_quat.y * ys, yz = p_quat.y * zs, zz = p_quat.z * zs; + set( 1.0 - (yy + zz), xy - wz, xz + wy, + xy + wz, 1.0 - (xx + zz), yz - wx, + xz - wy, yz + wx, 1.0 - (xx + yy)) ; + +} + +Matrix3::Matrix3(const Vector3& p_axis, real_t p_phi) { + + Vector3 axis_sq(p_axis.x*p_axis.x,p_axis.y*p_axis.y,p_axis.z*p_axis.z); + + real_t cosine= Math::cos(p_phi); + real_t sine= Math::sin(p_phi); + + elements[0][0] = axis_sq.x + cosine * ( 1.0 - axis_sq.x ); + elements[0][1] = p_axis.x * p_axis.y * ( 1.0 - cosine ) + p_axis.z * sine; + elements[0][2] = p_axis.z * p_axis.x * ( 1.0 - cosine ) - p_axis.y * sine; + + elements[1][0] = p_axis.x * p_axis.y * ( 1.0 - cosine ) - p_axis.z * sine; + elements[1][1] = axis_sq.y + cosine * ( 1.0 - axis_sq.y ); + elements[1][2] = p_axis.y * p_axis.z * ( 1.0 - cosine ) + p_axis.x * sine; + + elements[2][0] = p_axis.z * p_axis.x * ( 1.0 - cosine ) + p_axis.y * sine; + elements[2][1] = p_axis.y * p_axis.z * ( 1.0 - cosine ) - p_axis.x * sine; + elements[2][2] = axis_sq.z + cosine * ( 1.0 - axis_sq.z ); + +} + diff --git a/core/math/matrix3.h b/core/math/matrix3.h new file mode 100644 index 000000000..f68703ca2 --- /dev/null +++ b/core/math/matrix3.h @@ -0,0 +1,230 @@ +/*************************************************************************/ +/* matrix3.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 MATRIX3_H +#define MATRIX3_H + +#include "vector3.h" +#include "quat.h" + +/** + @author Juan Linietsky +*/ +class Matrix3 { +public: + + Vector3 elements[3]; + + _FORCE_INLINE_ const Vector3& operator[](int axis) const { + + return elements[axis]; + } + _FORCE_INLINE_ Vector3& operator[](int axis) { + + return elements[axis]; + } + + void invert(); + void transpose(); + + Matrix3 inverse() const; + Matrix3 transposed() const; + + _FORCE_INLINE_ float determinant() const; + + void from_z(const Vector3& p_z); + + _FORCE_INLINE_ Vector3 get_axis(int p_axis) const { + // get actual basis axis (elements is transposed for performance) + return Vector3( elements[0][p_axis], elements[1][p_axis], elements[2][p_axis] ); + } + _FORCE_INLINE_ void set_axis(int p_axis, const Vector3& p_value) { + // get actual basis axis (elements is transposed for performance) + elements[0][p_axis]=p_value.x; + elements[1][p_axis]=p_value.y; + elements[2][p_axis]=p_value.z; + } + + void rotate(const Vector3& p_axis, real_t p_phi); + Matrix3 rotated(const Vector3& p_axis, real_t p_phi) const; + + void scale( const Vector3& p_scale ); + Matrix3 scaled( const Vector3& p_scale ) const; + Vector3 get_scale() const; + + Vector3 get_euler() const; + void set_euler(const Vector3& p_euler); + + // transposed dot products + _FORCE_INLINE_ real_t tdotx(const Vector3& v) const { + return elements[0][0] * v[0] + elements[1][0] * v[1] + elements[2][0] * v[2]; + } + _FORCE_INLINE_ real_t tdoty(const Vector3& v) const { + return elements[0][1] * v[0] + elements[1][1] * v[1] + elements[2][1] * v[2]; + } + _FORCE_INLINE_ real_t tdotz(const Vector3& v) const { + return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2]; + } + + bool operator==(const Matrix3& p_matrix) const; + bool operator!=(const Matrix3& 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; + + int get_orthogonal_index() const; + void set_orthogonal_index(int p_index); + + operator String() const; + + void get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const; + + /* create / set */ + + + _FORCE_INLINE_ void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { + + elements[0][0]=xx; + elements[0][1]=xy; + elements[0][2]=xz; + elements[1][0]=yx; + elements[1][1]=yy; + elements[1][2]=yz; + elements[2][0]=zx; + elements[2][1]=zy; + elements[2][2]=zz; + } + _FORCE_INLINE_ Vector3 get_column(int i) const { + + return Vector3(elements[0][i],elements[1][i],elements[2][i]); + } + + _FORCE_INLINE_ Vector3 get_row(int i) const { + + return Vector3(elements[i][0],elements[i][1],elements[i][2]); + } + _FORCE_INLINE_ void set_row(int i, const Vector3& p_row) { + elements[i][0]=p_row.x; + elements[i][1]=p_row.y; + elements[i][2]=p_row.z; + } + + _FORCE_INLINE_ void set_zero() { + elements[0].zero(); + elements[1].zero(); + elements[2].zero(); + } + + _FORCE_INLINE_ Matrix3 transpose_xform(const Matrix3& m) const + { + return Matrix3( + elements[0].x * m[0].x + elements[1].x * m[1].x + elements[2].x * m[2].x, + elements[0].x * m[0].y + elements[1].x * m[1].y + elements[2].x * m[2].y, + elements[0].x * m[0].z + elements[1].x * m[1].z + elements[2].x * m[2].z, + elements[0].y * m[0].x + elements[1].y * m[1].x + elements[2].y * m[2].x, + elements[0].y * m[0].y + elements[1].y * m[1].y + elements[2].y * m[2].y, + elements[0].y * m[0].z + elements[1].y * m[1].z + elements[2].y * m[2].z, + elements[0].z * m[0].x + elements[1].z * m[1].x + elements[2].z * m[2].x, + elements[0].z * m[0].y + elements[1].z * m[1].y + elements[2].z * m[2].y, + elements[0].z * m[0].z + elements[1].z * m[1].z + elements[2].z * m[2].z); + } + 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) { + + set(xx, xy, xz, yx, yy, yz, zx, zy, zz); + } + + void orthonormalize(); + Matrix3 orthonormalized() const; + + operator Quat() const; + + Matrix3(const Quat& p_quat); // euler + Matrix3(const Vector3& p_euler); // euler + Matrix3(const Vector3& p_axis, real_t p_phi); + + _FORCE_INLINE_ Matrix3() { + + elements[0][0]=1; + elements[0][1]=0; + elements[0][2]=0; + elements[1][0]=0; + elements[1][1]=1; + elements[1][2]=0; + elements[2][0]=0; + elements[2][1]=0; + elements[2][2]=1; + } + + +}; + +_FORCE_INLINE_ void Matrix3::operator*=(const Matrix3& p_matrix) { + + set( + p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]), + p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]), + p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2])); + +} + +_FORCE_INLINE_ Matrix3 Matrix3::operator*(const Matrix3& p_matrix) const { + + return Matrix3( + 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 { + + return Vector3( + elements[0].dot(p_vector), + elements[1].dot(p_vector), + elements[2].dot(p_vector) + ); +} + +Vector3 Matrix3::xform_inv(const Vector3& p_vector) const { + + return Vector3( + (elements[0][0]*p_vector.x ) + ( elements[1][0]*p_vector.y ) + ( elements[2][0]*p_vector.z ), + (elements[0][1]*p_vector.x ) + ( elements[1][1]*p_vector.y ) + ( elements[2][1]*p_vector.z ), + (elements[0][2]*p_vector.x ) + ( elements[1][2]*p_vector.y ) + ( elements[2][2]*p_vector.z ) + ); +} + +float Matrix3::determinant() const { + + return elements[0][0]*(elements[1][1]*elements[2][2] - elements[2][1]*elements[1][2]) - + elements[1][0]*(elements[0][1]*elements[2][2] - elements[2][1]*elements[0][2]) + + elements[2][0]*(elements[0][1]*elements[1][2] - elements[1][1]*elements[0][2]); +} +#endif diff --git a/core/math/octree.h b/core/math/octree.h new file mode 100644 index 000000000..b64ba381c --- /dev/null +++ b/core/math/octree.h @@ -0,0 +1,1461 @@ +/*************************************************************************/ +/* octree.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 OCTREE_H +#define OCTREE_H + +#include "vector3.h" +#include "aabb.h" +#include "list.h" +#include "variant.h" +#include "map.h" +#include "print_string.h" + +/** + @author Juan Linietsky +*/ + +typedef uint32_t OctreeElementID; + +#define OCTREE_ELEMENT_INVALID_ID 0 +#define OCTREE_SIZE_LIMIT 1e15 + +template +class Octree { +public: + + typedef void* (*PairCallback)(void*,OctreeElementID, T*,int,OctreeElementID, T*,int); + typedef void (*UnpairCallback)(void*,OctreeElementID, T*,int,OctreeElementID, T*,int,void*); + +private: + enum { + + NEG=0, + POS=1, + }; + + enum { + OCTANT_NX_NY_NZ, + OCTANT_PX_NY_NZ, + OCTANT_NX_PY_NZ, + OCTANT_PX_PY_NZ, + OCTANT_NX_NY_PZ, + OCTANT_PX_NY_PZ, + OCTANT_NX_PY_PZ, + OCTANT_PX_PY_PZ + }; + + + struct PairKey { + + union { + struct { + OctreeElementID A; + OctreeElementID B; + }; + uint64_t key; + }; + + _FORCE_INLINE_ bool operator<(const PairKey& p_pair) const { + + return key pairable_elements; + List elements; + + Octant() { + children_count=0; + parent_index=-1; + last_pass=0; + parent=NULL; + for (int i=0;i<8;i++) + children[i]=NULL; + } + + ~Octant() { + + //for (int i=0;i<8;i++) + // memdelete_notnull(children[i]); + } + }; + + + struct PairData; + + struct Element { + + Octree *octree; + + T *userdata; + int subindex; + bool pairable; + uint32_t pairable_mask; + uint32_t pairable_type; + + uint64_t last_pass; + OctreeElementID _id; + Octant *common_parent; + + AABB aabb; + AABB container_aabb; + + List pair_list; + + struct OctantOwner { + + Octant *octant; + typename List::Element *E; + }; // an element can be in max 8 octants + + List octant_owners; + + + Element() { last_pass=0; _id=0; pairable=false; subindex=0; userdata=0; octree=0; pairable_mask=0; pairable_type=0; common_parent=NULL; } + }; + + + struct PairData { + + int refcount; + bool intersect; + Element *A,*B; + void *ud; + typename List::Element *eA,*eB; + }; + + typedef Map, AL> ElementMap; + typedef Map, AL> PairMap; + ElementMap element_map; + PairMap pair_map; + + PairCallback pair_callback; + UnpairCallback unpair_callback; + void *pair_callback_userdata; + void *unpair_callback_userdata; + + OctreeElementID last_element_id; + uint64_t pass; + + real_t unit_size; + Octant *root; + int octant_count; + int pair_count; + + + + _FORCE_INLINE_ void _pair_check(PairData *p_pair) { + + bool intersect=p_pair->A->aabb.intersects( p_pair->B->aabb ); + + if (intersect!=p_pair->intersect) { + + if (intersect) { + + if (pair_callback) { + p_pair->ud=pair_callback(pair_callback_userdata,p_pair->A->_id, p_pair->A->userdata,p_pair->A->subindex,p_pair->B->_id, p_pair->B->userdata,p_pair->B->subindex); + + } + pair_count++; + } else { + + + if (unpair_callback) { + unpair_callback(pair_callback_userdata,p_pair->A->_id, p_pair->A->userdata,p_pair->A->subindex,p_pair->B->_id, p_pair->B->userdata,p_pair->B->subindex,p_pair->ud); + } + pair_count--; + + } + + p_pair->intersect=intersect; + + } + } + + _FORCE_INLINE_ void _pair_reference(Element* p_A,Element* p_B) { + + if (p_A==p_B || (p_A->userdata==p_B->userdata && p_A->userdata)) + return; + + if ( !(p_A->pairable_type&p_B->pairable_mask) && + !(p_B->pairable_type&p_A->pairable_mask) ) + return; // none can pair with none + + PairKey key(p_A->_id, p_B->_id); + typename PairMap::Element *E=pair_map.find(key); + + if (!E) { + + PairData pdata; + pdata.refcount=1; + pdata.A=p_A; + pdata.B=p_B; + pdata.intersect=false; + E=pair_map.insert(key,pdata); + 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); + } else { + + E->get().refcount++; + } + + } + + _FORCE_INLINE_ void _pair_unreference(Element* p_A,Element* p_B) { + + if (p_A==p_B) + return; + + PairKey key(p_A->_id, p_B->_id); + typename PairMap::Element *E=pair_map.find(key); + if (!E) { + return; // no pair + } + + E->get().refcount--; + + + if (E->get().refcount==0) { + // bye pair + + if (E->get().intersect) { + if (unpair_callback) { + unpair_callback(pair_callback_userdata,p_A->_id, p_A->userdata,p_A->subindex,p_B->_id, p_B->userdata,p_B->subindex,E->get().ud); + } + + pair_count--; + } + + if (p_A==E->get().B) { + //may be reaching inverted + SWAP(p_A,p_B); + } + + p_A->pair_list.erase( E->get().eA ); + p_B->pair_list.erase( E->get().eB ); + pair_map.erase(E); + } + + } + + _FORCE_INLINE_ void _element_check_pairs(Element *p_element) { + + typename List::Element *E=p_element->pair_list.front(); + while(E) { + + _pair_check( E->get() ); + E=E->next(); + } + + } + + _FORCE_INLINE_ void _optimize() { + + + while(root && root->children_count<2 && !root->elements.size() && !(use_pairs && root->pairable_elements.size())) { + + + Octant *new_root=NULL; + if (root->children_count==1) { + + for(int i=0;i<8;i++) { + + if (root->children[i]) { + new_root=root->children[i]; + root->children[i]=NULL; + break; + } + } + ERR_FAIL_COND(!new_root); + new_root->parent=NULL; + new_root->parent_index=-1; + } + + memdelete_allocator( root ); + octant_count--; + root=new_root; + + } + } + + + void _insert_element(Element *p_element,Octant *p_octant); + void _ensure_valid_root(const AABB& 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); + void _unpair_element(Element *p_element,Octant *p_octant); + + + struct _CullConvexData { + + const Plane* planes; + int plane_count; + T** result_array; + int *result_idx; + int result_max; + uint32_t mask; + }; + + 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_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); + + void _remove_tree(Octant *p_octant) { + + if (!p_octant) + return; + + for(int i=0;i<8;i++) { + + if (p_octant->children[i]) + _remove_tree(p_octant->children[i]); + } + + memdelete_allocator(p_octant); + } +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); + 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); + + bool is_pairable(OctreeElementID p_id) const; + T *get(OctreeElementID p_id) const; + int get_subindex(OctreeElementID p_id) const; + + int cull_convex(const Vector& 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_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); + + void set_pair_callback( PairCallback p_callback, void *p_userdata ); + void set_unpair_callback( UnpairCallback p_callback, void *p_userdata ); + + int get_octant_count() const { return octant_count; } + int get_pair_count() const { return pair_count; } + Octree(real_t p_unit_size=1.0); + ~Octree() { _remove_tree(root); } +}; + + +/* PRIVATE FUNCTIONS */ + +template +T *Octree::get(OctreeElementID p_id) const { + const typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E,NULL); + return E->get().userdata; +} + + +template +bool Octree::is_pairable(OctreeElementID p_id) const { + + const typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E,false); + return E->get().pairable; +} + +template +int Octree::get_subindex(OctreeElementID p_id) const { + + const typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E,-1); + return E->get().subindex; +} + +#define OCTREE_DIVISOR 4 + +template +void Octree::_insert_element(Element *p_element,Octant *p_octant) { + + float 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) { + + /* at smallest possible size for the element */ + typename Element::OctantOwner owner; + owner.octant=p_octant; + + if (use_pairs && p_element->pairable) { + + p_octant->pairable_elements.push_back(p_element); + owner.E = p_octant->pairable_elements.back(); + } else { + + p_octant->elements.push_back(p_element); + owner.E = p_octant->elements.back(); + } + + p_element->octant_owners.push_back( owner ); + + if (p_element->common_parent==NULL) { + p_element->common_parent=p_octant; + p_element->container_aabb=p_octant->aabb; + } else { + p_element->container_aabb.merge_with(p_octant->aabb); + } + + + if (use_pairs && p_octant->children_count>0) { + + pass++; //elements below this only get ONE reference added + + for (int i=0;i<8;i++) { + + if (p_octant->children[i]) { + _pair_element(p_element,p_octant->children[i]); + } + } + } + } else { + /* not big enough, send it to subitems */ + int splits=0; + bool candidate=p_element->common_parent==NULL; + + for (int i=0;i<8;i++) { + + if (p_octant->children[i]) { + /* element exists, go straight to it */ + if (p_octant->children[i]->aabb.intersects( p_element->aabb ) ) { + _insert_element( p_element, p_octant->children[i] ); + splits++; + } + } else { + /* check againt AABB where child should be */ + + AABB aabb=p_octant->aabb; + aabb.size*=0.5; + + if (i&1) + aabb.pos.x+=aabb.size.x; + if (i&2) + aabb.pos.y+=aabb.size.y; + if (i&4) + aabb.pos.z+=aabb.size.z; + + if (aabb.intersects( p_element->aabb) ) { + /* if actually intersects, create the child */ + + Octant *child = memnew_allocator( Octant, AL ); + p_octant->children[i]=child; + child->parent=p_octant; + child->parent_index=i; + + child->aabb=aabb; + + p_octant->children_count++; + + _insert_element( p_element, child ); + octant_count++; + splits++; + + } + } + + } + + if (candidate && splits>1) { + + p_element->common_parent=p_octant; + } + + } + + if (use_pairs) { + + typename List::Element *E=p_octant->pairable_elements.front(); + + while(E) { + _pair_reference( p_element,E->get() ); + E=E->next(); + } + + if (p_element->pairable) { + // and always test non-pairable if element is pairable + E=p_octant->elements.front(); + while(E) { + _pair_reference( p_element,E->get() ); + E=E->next(); + } + } + } + + +} + + +template +void Octree::_ensure_valid_root(const AABB& p_aabb) { + + if (!root) { + // octre is empty + + AABB base( Vector3(), Vector3(1.0,1.0,1.0) * unit_size); + + while ( !base.encloses(p_aabb) ) { + + if ( ABS(base.pos.x+base.size.x) <= ABS(base.pos.x) ) { + /* grow towards positive */ + base.size*=2.0; + } else { + base.pos-=base.size; + base.size*=2.0; + } + } + + root = memnew_allocator( Octant, AL ); + + root->parent=NULL; + root->parent_index=-1; + root->aabb=base; + + octant_count++; + + + } else { + + AABB base=root->aabb; + + while( !base.encloses( p_aabb ) ) { + + if (base.size.x > OCTREE_SIZE_LIMIT) { + ERR_EXPLAIN("Octree upper size limit reeached, does the AABB supplied contain NAN?"); + ERR_FAIL(); + } + + Octant * gp = memnew_allocator( Octant, AL ); + octant_count++; + root->parent=gp; + + if ( ABS(base.pos.x+base.size.x) <= ABS(base.pos.x) ) { + /* grow towards positive */ + base.size*=2.0; + gp->aabb=base; + gp->children[0]=root; + root->parent_index=0; + } else { + base.pos-=base.size; + base.size*=2.0; + gp->aabb=base; + gp->children[(1<<0)|(1<<1)|(1<<2)]=root; // add at all-positive + root->parent_index=(1<<0)|(1<<1)|(1<<2); + } + + gp->children_count=1; + root=gp; + } + } +} + +template +bool Octree::_remove_element_from_octant(Element *p_element,Octant *p_octant,Octant *p_limit) { + + bool octant_removed=false; + + while(true) { + + // check all exit conditions + + if (p_octant==p_limit) // reached limit, nothing to erase, exit + return octant_removed; + + bool unpaired=false; + + if (use_pairs && p_octant->last_pass!=pass) { + // check wether we should unpair stuff + // always test pairable + typename List::Element *E=p_octant->pairable_elements.front(); + while(E) { + _pair_unreference( p_element,E->get() ); + E=E->next(); + } + if (p_element->pairable) { + // and always test non-pairable if element is pairable + E=p_octant->elements.front(); + while(E) { + _pair_unreference( p_element,E->get() ); + E=E->next(); + } + } + p_octant->last_pass=pass; + unpaired=true; + } + + bool removed=false; + + Octant *parent=p_octant->parent; + + if (p_octant->children_count==0 && p_octant->elements.empty() && p_octant->pairable_elements.empty()) { + + // erase octant + + if (p_octant==root) { // won't have a parent, just erase + + root=NULL; + } else { + ERR_FAIL_INDEX_V(p_octant->parent_index,8,octant_removed); + + parent->children[ p_octant->parent_index ]=NULL; + parent->children_count--; + } + + memdelete_allocator(p_octant); + octant_count--; + removed=true; + octant_removed=true; + } + + if (!removed && !unpaired) + return octant_removed; // no reason to keep going up anymore! was already visited and was not removed + + p_octant=parent; + + } + + return octant_removed; +} + +template +void Octree::_unpair_element(Element *p_element,Octant *p_octant) { + + + // always test pairable + typename List::Element *E=p_octant->pairable_elements.front(); + while(E) { + if (E->get()->last_pass!=pass) { // only remove ONE reference + _pair_unreference( p_element,E->get() ); + E->get()->last_pass=pass; + } + E=E->next(); + } + + if (p_element->pairable) { + // and always test non-pairable if element is pairable + E=p_octant->elements.front(); + while(E) { + if (E->get()->last_pass!=pass) { // only remove ONE reference + _pair_unreference( p_element,E->get() ); + E->get()->last_pass=pass; + } + E=E->next(); + } + } + + p_octant->last_pass=pass; + + if (p_octant->children_count==0) + return; // small optimization for leafs + + for (int i=0;i<8;i++) { + + if (p_octant->children[i]) + _unpair_element(p_element,p_octant->children[i]); + } +} + +template +void Octree::_pair_element(Element *p_element,Octant *p_octant) { + + // always test pairable + + typename List::Element *E=p_octant->pairable_elements.front(); + + while(E) { + + if (E->get()->last_pass!=pass) { // only get ONE reference + _pair_reference( p_element,E->get() ); + E->get()->last_pass=pass; + } + E=E->next(); + } + + if (p_element->pairable) { + // and always test non-pairable if element is pairable + E=p_octant->elements.front(); + while(E) { + if (E->get()->last_pass!=pass) { // only get ONE reference + _pair_reference( p_element,E->get() ); + E->get()->last_pass=pass; + } + E=E->next(); + } + } + p_octant->last_pass=pass; + + if (p_octant->children_count==0) + return; // small optimization for leafs + + for (int i=0;i<8;i++) { + + if (p_octant->children[i]) + _pair_element(p_element,p_octant->children[i]); + } +} + +template +void Octree::_remove_element(Element *p_element) { + + pass++; // will do a new pass for this + + typename List< typename Element::OctantOwner,AL >::Element *I=p_element->octant_owners.front(); + + + /* FIRST remove going up normally */ + for(;I;I=I->next()) { + + Octant *o=I->get().octant; + + if (!use_pairs) // small speedup + o->elements.erase( I->get().E ); + + _remove_element_from_octant( p_element, o ); + + } + + /* THEN remove going down */ + + I=p_element->octant_owners.front(); + + if (use_pairs) { + + for(;I;I=I->next()) { + + Octant *o=I->get().octant; + + // erase children pairs, they are erased ONCE even if repeated + pass++; + for (int i=0;i<8;i++) { + + if (o->children[i]) + _unpair_element(p_element,o->children[i]); + } + + if (p_element->pairable) + o->pairable_elements.erase( I->get().E ); + else + o->elements.erase( I->get().E ); + + } + } + + p_element->octant_owners.clear(); + + if(use_pairs) { + + int remaining=p_element->pair_list.size(); + //p_element->pair_list.clear(); + ERR_FAIL_COND( remaining ); + } + +} + +template +OctreeElementID Octree::create(T* p_userdata, const AABB& 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 + ERR_FAIL_COND_V( p_aabb.pos.x > 1e15 || p_aabb.pos.x < -1e15, 0 ); + ERR_FAIL_COND_V( p_aabb.pos.y > 1e15 || p_aabb.pos.y < -1e15, 0 ); + ERR_FAIL_COND_V( p_aabb.pos.z > 1e15 || p_aabb.pos.z < -1e15, 0 ); + ERR_FAIL_COND_V( p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0, 0 ); + ERR_FAIL_COND_V( p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0, 0 ); + ERR_FAIL_COND_V( p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0, 0 ); + ERR_FAIL_COND_V( Math::is_nan(p_aabb.size.x) , 0 ); + ERR_FAIL_COND_V( Math::is_nan(p_aabb.size.y) , 0 ); + ERR_FAIL_COND_V( Math::is_nan(p_aabb.size.z) , 0 ); + + +#endif + typename ElementMap::Element *E = element_map.insert(last_element_id++, + Element()); + Element &e = E->get(); + + e.aabb=p_aabb; + e.userdata=p_userdata; + e.subindex=p_subindex; + e.last_pass=0; + e.octree=this; + e.pairable=p_pairable; + e.pairable_type=p_pairable_type; + e.pairable_mask=p_pairable_mask; + e._id=last_element_id-1; + + if (!e.aabb.has_no_surface()) { + _ensure_valid_root(p_aabb); + _insert_element(&e,root); + if (use_pairs) + _element_check_pairs(&e); + } + + return last_element_id-1; +} + + + +template +void Octree::move(OctreeElementID p_id, const AABB& p_aabb) { + +#ifdef DEBUG_ENABLED + // check for AABB validity + ERR_FAIL_COND( p_aabb.pos.x > 1e15 || p_aabb.pos.x < -1e15 ); + ERR_FAIL_COND( p_aabb.pos.y > 1e15 || p_aabb.pos.y < -1e15 ); + ERR_FAIL_COND( p_aabb.pos.z > 1e15 || p_aabb.pos.z < -1e15 ); + ERR_FAIL_COND( p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0 ); + ERR_FAIL_COND( p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0 ); + ERR_FAIL_COND( p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0 ); + ERR_FAIL_COND( Math::is_nan(p_aabb.size.x) ); + ERR_FAIL_COND( Math::is_nan(p_aabb.size.y) ); + ERR_FAIL_COND( Math::is_nan(p_aabb.size.z) ); +#endif + typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND(!E); + Element &e = E->get(); + +#if 0 + + pass++; + if (!e.aabb.has_no_surface()) { + _remove_element(&e); + } + + e.aabb=p_aabb; + + if (!e.aabb.has_no_surface()) { + _ensure_valid_root(p_aabb); + + _insert_element(&e,root); + if (use_pairs) + _element_check_pairs(&e); + + } + + _optimize(); + +#else + + bool old_has_surf=!e.aabb.has_no_surface(); + bool new_has_surf=!p_aabb.has_no_surface(); + + if (old_has_surf!=new_has_surf) { + + + if (old_has_surf) { + _remove_element(&e); // removing + e.common_parent=NULL; + e.aabb=AABB(); + _optimize(); + } else { + _ensure_valid_root(p_aabb); // inserting + e.common_parent=NULL; + e.aabb=p_aabb; + _insert_element(&e,root); + if (use_pairs) + _element_check_pairs(&e); + + } + + return; + } + + if (!old_has_surf) // doing nothing + return; + + // it still is enclosed in the same AABB it was assigned to + if (e.container_aabb.encloses(p_aabb)) { + + e.aabb=p_aabb; + if (use_pairs) + _element_check_pairs(&e); // must check pairs anyway + + + return; + } + + AABB combined=e.aabb; + combined.merge_with(p_aabb); + _ensure_valid_root(combined); + + ERR_FAIL_COND( e.octant_owners.front()==NULL ); + + /* FIND COMMON PARENT */ + + List owners = e.octant_owners; // save the octant owners + Octant *common_parent=e.common_parent; + ERR_FAIL_COND(!common_parent); + + + //src is now the place towards where insertion is going to happen + pass++; + + while(common_parent && !common_parent->aabb.encloses(p_aabb)) + common_parent=common_parent->parent; + + ERR_FAIL_COND(!common_parent); + + //prepare for reinsert + e.octant_owners.clear(); + e.common_parent=NULL; + e.aabb=p_aabb; + + _insert_element(&e,common_parent); // reinsert from this point + + pass++; + + for(typename List::Element *E=owners.front();E;) { + + Octant *o=E->get().octant; + typename List::Element *N=E->next(); + +// if (!use_pairs) +// o->elements.erase( E->get().E ); + + if (use_pairs && e.pairable) + o->pairable_elements.erase( E->get().E ); + else + o->elements.erase( E->get().E ); + + if (_remove_element_from_octant( &e, o, common_parent->parent )) { + + owners.erase(E); + } + + E=N; + } + + + if (use_pairs) { + //unpair child elements in anything that survived + for(typename List::Element *E=owners.front();E;E=E->next()) { + + Octant *o=E->get().octant; + + // erase children pairs, unref ONCE + pass++; + for (int i=0;i<8;i++) { + + if (o->children[i]) + _unpair_element(&e,o->children[i]); + } + + } + + _element_check_pairs(&e); + } + + + _optimize(); +#endif + + +} + +template +void Octree::set_pairable(OctreeElementID p_id,bool p_pairable,uint32_t p_pairable_type,uint32_t p_pairable_mask) { + + typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND(!E); + + Element &e = E->get(); + + if (p_pairable == e.pairable && e.pairable_type==p_pairable_type && e.pairable_mask==p_pairable_mask) + return; // no changes, return + + if (!e.aabb.has_no_surface()) { + _remove_element(&e); + } + + e.pairable=p_pairable; + e.pairable_type=p_pairable_type; + e.pairable_mask=p_pairable_mask; + e.common_parent=NULL; + + if (!e.aabb.has_no_surface()) { + _ensure_valid_root(e.aabb); + _insert_element(&e,root); + if (use_pairs) + _element_check_pairs(&e); + + } +} + + +template +void Octree::erase(OctreeElementID p_id) { + + typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND(!E); + + Element &e = E->get(); + + if (!e.aabb.has_no_surface()) { + + _remove_element(&e); + } + + element_map.erase(p_id); + _optimize(); +} + +template +void Octree::_cull_convex(Octant *p_octant,_CullConvexData *p_cull) { + + if (*p_cull->result_idx==p_cull->result_max) + return; //pointless + + if (!p_octant->elements.empty()) { + + typename List< Element*,AL >::Element *I; + I=p_octant->elements.front(); + + for(;I;I=I->next()) { + + Element *e=I->get(); + + if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_cull->mask))) + continue; + e->last_pass=pass; + + if (e->aabb.intersects_convex_shape(p_cull->planes,p_cull->plane_count)) { + + if (*p_cull->result_idxresult_max) { + p_cull->result_array[*p_cull->result_idx] = e->userdata; + (*p_cull->result_idx)++; + } else { + + return; // pointless to continue + } + } + } + } + + if (use_pairs && !p_octant->pairable_elements.empty()) { + + typename List< Element*,AL >::Element *I; + I=p_octant->pairable_elements.front(); + + for(;I;I=I->next()) { + + Element *e=I->get(); + + if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_cull->mask))) + continue; + e->last_pass=pass; + + if (e->aabb.intersects_convex_shape(p_cull->planes,p_cull->plane_count)) { + + if (*p_cull->result_idxresult_max) { + + p_cull->result_array[*p_cull->result_idx] = e->userdata; + (*p_cull->result_idx)++; + } else { + + return; // pointless to continue + } + } + } + } + + for (int i=0;i<8;i++) { + + if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes,p_cull->plane_count)) { + _cull_convex(p_octant->children[i],p_cull); + } + } +} + + +template +void Octree::_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) { + + if (*p_result_idx==p_result_max) + return; //pointless + + if (!p_octant->elements.empty()) { + + typename List< Element*,AL >::Element *I; + I=p_octant->elements.front(); + for(;I;I=I->next()) { + + Element *e=I->get(); + + if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask))) + continue; + e->last_pass=pass; + + if (p_aabb.intersects(e->aabb)) { + + if (*p_result_idxuserdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + } else { + + return; // pointless to continue + } + } + } + } + + if (use_pairs && !p_octant->pairable_elements.empty()) { + + typename List< Element*,AL >::Element *I; + I=p_octant->pairable_elements.front(); + for(;I;I=I->next()) { + + Element *e=I->get(); + + if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask))) + continue; + e->last_pass=pass; + + if (p_aabb.intersects(e->aabb)) { + + if (*p_result_idxuserdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + } else { + + return; // pointless to continue + } + } + } + } + + for (int i=0;i<8;i++) { + + if (p_octant->children[i] && p_octant->children[i]->aabb.intersects(p_aabb)) { + _cull_AABB(p_octant->children[i],p_aabb, p_result_array,p_result_idx,p_result_max,p_subindex_array,p_mask); + } + } + +} + +template +void Octree::_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) { + + if (*p_result_idx==p_result_max) + return; //pointless + + if (!p_octant->elements.empty()) { + + typename List< Element*,AL >::Element *I; + I=p_octant->elements.front(); + for(;I;I=I->next()) { + + Element *e=I->get(); + + if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask))) + continue; + e->last_pass=pass; + + if (e->aabb.intersects_segment(p_from,p_to)) { + + if (*p_result_idxuserdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } + } + + if (use_pairs && !p_octant->pairable_elements.empty()) { + + typename List< Element*,AL >::Element *I; + I=p_octant->pairable_elements.front(); + for(;I;I=I->next()) { + + Element *e=I->get(); + + if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask))) + continue; + + e->last_pass=pass; + + if (e->aabb.intersects_segment(p_from,p_to)) { + + if (*p_result_idxuserdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } + } + + + for (int i=0;i<8;i++) { + + if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_segment(p_from,p_to)) { + _cull_segment(p_octant->children[i],p_from,p_to, p_result_array,p_result_idx,p_result_max,p_subindex_array,p_mask); + } + } +} + + +template +void Octree::_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) { + + if (*p_result_idx==p_result_max) + return; //pointless + + if (!p_octant->elements.empty()) { + + typename List< Element*,AL >::Element *I; + I=p_octant->elements.front(); + for(;I;I=I->next()) { + + Element *e=I->get(); + + if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask))) + continue; + e->last_pass=pass; + + if (e->aabb.has_point(p_point)) { + + if (*p_result_idxuserdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } + } + + if (use_pairs && !p_octant->pairable_elements.empty()) { + + typename List< Element*,AL >::Element *I; + I=p_octant->pairable_elements.front(); + for(;I;I=I->next()) { + + Element *e=I->get(); + + if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask))) + continue; + + e->last_pass=pass; + + if (e->aabb.has_point(p_point)) { + + if (*p_result_idxuserdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } + } + + + for (int i=0;i<8;i++) { + + //could be optimized.. + if (p_octant->children[i] && p_octant->children[i]->aabb.has_point(p_point)) { + _cull_point(p_octant->children[i],p_point, p_result_array,p_result_idx,p_result_max,p_subindex_array,p_mask); + } + } +} + +template +int Octree::cull_convex(const Vector& p_convex,T** p_result_array,int p_result_max,uint32_t p_mask) { + + if (!root) + return 0; + + int result_count=0; + pass++; + _CullConvexData cdata; + cdata.planes=&p_convex[0]; + cdata.plane_count=p_convex.size(); + cdata.result_array=p_result_array; + cdata.result_max=p_result_max; + cdata.result_idx=&result_count; + cdata.mask=p_mask; + + _cull_convex(root,&cdata); + + return result_count; +} + + + +template +int Octree::cull_AABB(const AABB& p_aabb,T** p_result_array,int p_result_max,int *p_subindex_array,uint32_t p_mask) { + + + if (!root) + return 0; + + int result_count=0; + pass++; + _cull_AABB(root,p_aabb,p_result_array,&result_count,p_result_max,p_subindex_array,p_mask); + + return result_count; +} + + +template +int Octree::cull_segment(const Vector3& p_from, const Vector3& p_to,T** p_result_array,int p_result_max,int *p_subindex_array,uint32_t p_mask) { + + if (!root) + return 0; + + int result_count=0; + pass++; + _cull_segment(root,p_from,p_to,p_result_array,&result_count,p_result_max,p_subindex_array,p_mask); + + return result_count; + +} + +template +int Octree::cull_point(const Vector3& p_point,T** p_result_array,int p_result_max,int *p_subindex_array,uint32_t p_mask) { + + if (!root) + return 0; + + int result_count=0; + pass++; + _cull_point(root,p_point,p_result_array,&result_count,p_result_max,p_subindex_array,p_mask); + + return result_count; + +} + + +template +void Octree::set_pair_callback( PairCallback p_callback, void *p_userdata ) { + + pair_callback=p_callback; + pair_callback_userdata=p_userdata; +} +template +void Octree::set_unpair_callback( UnpairCallback p_callback, void *p_userdata ) { + + unpair_callback=p_callback; + unpair_callback_userdata=p_userdata; + +} + + +template +Octree::Octree(real_t p_unit_size) { + + last_element_id=1; + pass=1; + unit_size=p_unit_size; + root=NULL; + + octant_count=0; + pair_count=0; + + pair_callback=NULL; + unpair_callback=NULL; + pair_callback_userdata=NULL; + unpair_callback_userdata=NULL; + + + + +} + + + + +#endif diff --git a/core/math/plane.cpp b/core/math/plane.cpp new file mode 100644 index 000000000..88c7be5f6 --- /dev/null +++ b/core/math/plane.cpp @@ -0,0 +1,166 @@ +/*************************************************************************/ +/* plane.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "plane.h" + +#include "math_funcs.h" + +#define _PLANE_EQ_DOT_EPSILON 0.999 +#define _PLANE_EQ_D_EPSILON 0.0001 + +void Plane::set_normal(const Vector3& p_normal) { + + normal=p_normal; +} + +void Plane::normalize() { + + real_t l = normal.length(); + if (l==0) { + *this=Plane(0,0,0,0); + return; + } + normal/=l; + d/=l; +} + +Plane Plane::normalized() const { + + Plane p = *this; + p.normalize(); + return p; +} + +Vector3 Plane::get_any_point() const { + + return get_normal()*d; +} + +Vector3 Plane::get_any_perpendicular_normal() const { + + static const Vector3 p1 = Vector3(1,0,0); + static const Vector3 p2 = Vector3(0,1,0); + Vector3 p; + + if (ABS(normal.dot(p1)) > 0.99) // if too similar to p1 + p=p2; // use p2 + else + p=p1; // use p1 + + p-=normal * normal.dot(p); + p.normalize(); + + return p; +} + +/* intersections */ + +bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result) const { + + const Plane &p_plane0=*this; + Vector3 normal0=p_plane0.normal; + Vector3 normal1=p_plane1.normal; + Vector3 normal2=p_plane2.normal; + + real_t denom=vec3_cross(normal0,normal1).dot(normal2); + + if (ABS(denom)<=CMP_EPSILON) + return false; + + if (r_result) { + *r_result = ( (vec3_cross(normal1, normal2) * p_plane0.d) + + (vec3_cross(normal2, normal0) * p_plane1.d) + + (vec3_cross(normal0, normal1) * p_plane2.d) )/denom; + } + + return true; +} + + +bool Plane::intersects_ray(Vector3 p_from, Vector3 p_dir, Vector3* p_intersection) const { + + Vector3 segment=p_dir; + real_t den=normal.dot( segment ); + + //printf("den is %i\n",den); + if (Math::abs(den)<=CMP_EPSILON) { + + return false; + } + + real_t dist=(normal.dot( p_from ) - d) / den; + //printf("dist is %i\n",dist); + + if (dist>CMP_EPSILON) { //this is a ray, before the emiting pos (p_from) doesnt exist + + return false; + } + + dist=-dist; + *p_intersection = p_from + segment * dist; + + return true; +} + +bool Plane::intersects_segment(Vector3 p_begin, Vector3 p_end, Vector3* p_intersection) const { + + Vector3 segment= p_begin - p_end; + real_t den=normal.dot( segment ); + + //printf("den is %i\n",den); + if (Math::abs(den)<=CMP_EPSILON) { + + return false; + } + + real_t dist=(normal.dot( p_begin ) - d) / den; + //printf("dist is %i\n",dist); + + if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) { + + return false; + } + + dist=-dist; + *p_intersection = p_begin + segment * dist; + + return true; +} + +/* misc */ + +bool Plane::is_almost_like(const Plane& p_plane) const { + + return (normal.dot( p_plane.normal ) > _PLANE_EQ_DOT_EPSILON && Math::absd(d-p_plane.d) < _PLANE_EQ_D_EPSILON); +} + + +Plane::operator String() const { + + return normal.operator String() + ", " + rtos(d); +} diff --git a/core/math/plane.h b/core/math/plane.h new file mode 100644 index 000000000..608ec2692 --- /dev/null +++ b/core/math/plane.h @@ -0,0 +1,146 @@ +/*************************************************************************/ +/* plane.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 PLANE_H +#define PLANE_H + + +#include "vector3.h" + +class Plane { +public: + + Vector3 normal; + real_t d; + + + void set_normal(const Vector3& p_normal); + _FORCE_INLINE_ Vector3 get_normal() const { return normal; }; ///Point is coplanar, CMP_EPSILON for precision + + void normalize(); + Plane normalized() const; + + /* Plane-Point operations */ + + _FORCE_INLINE_ Vector3 center() const { return normal*d; } + Vector3 get_any_point() const; + Vector3 get_any_perpendicular_normal() const; + + _FORCE_INLINE_ bool is_point_over(const Vector3 &p_point) const; ///< Point is over plane + _FORCE_INLINE_ real_t distance_to(const Vector3 &p_point) const; + _FORCE_INLINE_ bool has_point(const Vector3 &p_point,real_t _epsilon=CMP_EPSILON) const; + + /* intersections */ + + bool intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result=0) const; + bool intersects_ray(Vector3 p_from, Vector3 p_dir, Vector3* p_intersection) const; + bool intersects_segment(Vector3 p_begin, Vector3 p_end, Vector3* p_intersection) const; + + _FORCE_INLINE_ Vector3 project(const Vector3& p_point) const { + + return p_point - normal * distance_to(p_point); + } + + /* misc */ + + Plane operator-() const { return Plane(-normal,-d); } + bool is_almost_like(const Plane& p_plane) const; + + _FORCE_INLINE_ bool operator==(const Plane& p_plane) const; + _FORCE_INLINE_ bool operator!=(const Plane& p_plane) const; + operator String() const; + + _FORCE_INLINE_ Plane() { d=0; } + _FORCE_INLINE_ Plane(real_t p_a, real_t p_b, real_t p_c, real_t p_d) : normal(p_a,p_b,p_c), d(p_d) { }; + + _FORCE_INLINE_ Plane(const Vector3 &p_normal, real_t p_d); + _FORCE_INLINE_ Plane(const Vector3 &p_point, const Vector3& p_normal); + _FORCE_INLINE_ Plane(const Vector3 &p_point1, const Vector3 &p_point2,const Vector3 &p_point3,ClockDirection p_dir = CLOCKWISE); + + +}; + + + +bool Plane::is_point_over(const Vector3 &p_point) const { + + return (normal.dot(p_point) > d); +} + +real_t Plane::distance_to(const Vector3 &p_point) const { + + return (normal.dot(p_point)-d); +} + +bool Plane::has_point(const Vector3 &p_point,real_t _epsilon) const { + + float dist=normal.dot(p_point) - d; + dist=ABS(dist); + return ( dist <= _epsilon); + +} + +Plane::Plane(const Vector3 &p_normal, real_t p_d) { + + normal=p_normal; + d=p_d; +} + +Plane::Plane(const Vector3 &p_point, const Vector3& p_normal) { + + normal=p_normal; + d=p_normal.dot(p_point); +} + +Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3,ClockDirection p_dir) { + + if (p_dir == CLOCKWISE) + normal=(p_point1-p_point3).cross(p_point1-p_point2); + else + normal=(p_point1-p_point2).cross(p_point1-p_point3); + + + normal.normalize(); + d = normal.dot(p_point1); + + +} + +bool Plane::operator==(const Plane& p_plane) const { + + return normal==p_plane.normal && d == p_plane.d; +} + +bool Plane::operator!=(const Plane& p_plane) const { + + return normal!=p_plane.normal || d != p_plane.d; + +} + +#endif // PLANE_H + diff --git a/core/math/quat.cpp b/core/math/quat.cpp new file mode 100644 index 000000000..950a4756a --- /dev/null +++ b/core/math/quat.cpp @@ -0,0 +1,267 @@ +/*************************************************************************/ +/* quat.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "quat.h" +#include "print_string.h" + +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); +} + +void Quat::operator*=(const Quat& q) { + + set(w * q.x+x * q.w+y * q.z - z * q.y, + w * q.y+y * q.w+z * q.x - x * q.z, + w * q.z+z * q.w+x * q.y - y * q.x, + w * q.w - x * q.x - y * q.y - z * q.z); +} + +Quat Quat::operator*(const Quat& q) const { + + Quat r=*this; + r*=q; + return r; +} + + + + +real_t Quat::length() const { + + return Math::sqrt(length_squared()); +} + +void Quat::normalize() { + *this /= length(); +} + + +Quat Quat::normalized() const { + return *this / length(); +} + +Quat Quat::inverse() const { + return Quat( -x, -y, -z, w ); +} + + +Quat Quat::slerp(const Quat& q, const real_t& t) const { + +#if 0 + + + Quat dst=q; + Quat src=*this; + + src.normalize(); + dst.normalize(); + + real_t cosine = dst.dot(src); + + if (cosine < 0 && true) { + cosine = -cosine; + dst = -dst; + } else { + dst = dst; + } + + if (Math::abs(cosine) < 1 - CMP_EPSILON) { + // 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 coeff_1 = Math::sin(t * angle) * inv_sine; + Quat ret= src * coeff_0 + dst * coeff_1; + + return ret; + } else { + // There are two situations: + // 1. "rkP" and "q" are very close (cosine ~= +1), so we can do a linear + // interpolation safely. + // 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; + // taking the complement requires renormalisation + ret.normalize(); + return ret; + } +#else + + real_t to1[4]; + real_t omega, cosom, sinom, scale0, scale1; + + + // calc cosine + cosom = x * q.x + y * q.y + z * q.z + + w * q.w; + + + // 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; + } else { + to1[0] = q.x; + to1[1] = q.y; + to1[2] = q.z; + to1[3] = q.w; + } + + + // calculate coefficients + + if ( (1.0 - cosom) > CMP_EPSILON ) { + // standard case (slerp) + omega = Math::acos(cosom); + sinom = Math::sin(omega); + scale0 = Math::sin((1.0 - t) * omega) / sinom; + scale1 = Math::sin(t * omega) / sinom; + } else { + // "from" and "to" quaternions are very close + // ... so we can do a linear interpolation + scale0 = 1.0 - t; + scale1 = t; + } + // 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] + ); +#endif +} + +Quat Quat::slerpni(const Quat& q, const real_t& t) const { + + const Quat &from = *this; + + float dot = from.dot(q); + + if (Math::absf(dot) > 0.9999f) return from; + + float theta = Math::acos(dot), + sinT = 1.0f / Math::sin(theta), + newFactor = Math::sin(t * theta) * sinT, + invFactor = Math::sin((1.0f - 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 ); + +#if 0 + real_t to1[4]; + real_t omega, cosom, sinom, scale0, scale1; + + + // calc cosine + cosom = x * q.x + y * q.y + z * q.z + + w * q.w; + + + // adjust signs (if necessary) + if ( cosom <0.0 && false) { + cosom = -cosom; to1[0] = - q.x; + to1[1] = - q.y; + to1[2] = - q.z; + to1[3] = - q.w; + } else { + to1[0] = q.x; + to1[1] = q.y; + to1[2] = q.z; + to1[3] = q.w; + } + + + // calculate coefficients + + if ( (1.0 - cosom) > CMP_EPSILON ) { + // standard case (slerp) + omega = Math::acos(cosom); + sinom = Math::sin(omega); + scale0 = Math::sin((1.0 - t) * omega) / sinom; + scale1 = Math::sin(t * omega) / sinom; + } else { + // "from" and "to" quaternions are very close + // ... so we can do a linear interpolation + scale0 = 1.0 - t; + scale1 = t; + } + // 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] + ); +#endif +} + +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; + Quat sp = this->slerp(q,t); + Quat sq = prep.slerpni(postq,t); + return sp.slerpni(sq,t2); + +} + + +Quat::operator String() const { + + return String::num(x)+","+String::num(y)+","+ String::num(z)+","+ String::num(w); +} + +Quat::Quat(const Vector3& axis, const real_t& angle) { + real_t d = axis.length(); + if (d==0) + set(0,0,0,0); + else { + real_t s = Math::sin(-angle * 0.5) / d; + set(axis.x * s, axis.y * s, axis.z * s, + Math::cos(-angle * 0.5)); + } +} diff --git a/core/math/quat.h b/core/math/quat.h new file mode 100644 index 000000000..d32607303 --- /dev/null +++ b/core/math/quat.h @@ -0,0 +1,157 @@ +/*************************************************************************/ +/* quat.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 QUAT_H +#define QUAT_H + +#include "math_defs.h" +#include "math_funcs.h" +#include "ustring.h" +#include "vector3.h" + +/** + @author Juan Linietsky +*/ +class Quat{ +public: + + real_t x,y,z,w; + + _FORCE_INLINE_ real_t length_squared() const; + real_t length() const; + void normalize(); + Quat normalized() const; + Quat inverse() const; + _FORCE_INLINE_ real_t dot(const Quat& q) const; + void set_euler(const Vector3& p_euler); + 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); + } + + void operator*=(const Quat& q); + Quat operator*(const Quat& q) const; + + + _FORCE_INLINE_ void operator+=(const Quat& q); + _FORCE_INLINE_ void operator-=(const Quat& q); + _FORCE_INLINE_ void operator*=(const real_t& s); + _FORCE_INLINE_ void operator/=(const real_t& s); + _FORCE_INLINE_ Quat operator+(const Quat& q2) const; + _FORCE_INLINE_ Quat operator-(const Quat& q2) const; + _FORCE_INLINE_ Quat operator-() const; + _FORCE_INLINE_ Quat operator*(const real_t& s) const; + _FORCE_INLINE_ Quat operator/(const real_t& s) const; + + + _FORCE_INLINE_ bool operator==(const Quat& p_quat) const; + _FORCE_INLINE_ bool operator!=(const Quat& p_quat) const; + + operator String() const; + + inline void set( real_t p_x, real_t p_y, real_t p_z, real_t p_w) { + x=p_x; y=p_y; z=p_z; w=p_w; + } + inline Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) { + x=p_x; y=p_y; z=p_z; w=p_w; + } + Quat(const Vector3& axis, const real_t& angle); + inline Quat() {x=y=z=0; w=1; } + + +}; + + +real_t Quat::dot(const Quat& q) const { + return x * q.x+y * q.y+z * q.z+w * q.w; +} + +real_t Quat::length_squared() const { + return dot(*this); +} + +void Quat::operator+=(const Quat& q) { + x += q.x; y += q.y; z += q.z; w += q.w; +} + +void Quat::operator-=(const Quat& q) { + x -= q.x; y -= q.y; z -= q.z; w -= q.w; +} + +void Quat::operator*=(const real_t& s) { + x *= s; y *= s; z *= s; w *= s; +} + + +void Quat::operator/=(const real_t& s) { + + *this *= 1.0 / s; +} + +Quat Quat::operator+(const Quat& q2) const { + const Quat& q1 = *this; + return Quat( q1.x+q2.x, q1.y+q2.y, q1.z+q2.z, q1.w+q2.w ); +} + +Quat Quat::operator-(const Quat& q2) const { + const Quat& q1 = *this; + return Quat( q1.x-q2.x, q1.y-q2.y, q1.z-q2.z, q1.w-q2.w); +} + +Quat Quat::operator-() const { + const Quat& q2 = *this; + return Quat( -q2.x, -q2.y, -q2.z, -q2.w); +} + +Quat Quat::operator*(const real_t& s) const { + return Quat(x * s, y * s, z * s, w * s); +} + +Quat Quat::operator/(const real_t& s) const { + return *this * (1.0 / s); +} + + +bool Quat::operator==(const Quat& p_quat) const { + + return x==p_quat.x && y==p_quat.y && z==p_quat.z && w==p_quat.w; +} + +bool Quat::operator!=(const Quat& p_quat) const { + + return x!=p_quat.x || y!=p_quat.y || z!=p_quat.z || w!=p_quat.w; +} + + +#endif diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp new file mode 100644 index 000000000..ed30de691 --- /dev/null +++ b/core/math/quick_hull.cpp @@ -0,0 +1,513 @@ +/*************************************************************************/ +/* quick_hull.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "quick_hull.h" +#include "map.h" + +uint32_t QuickHull::debug_stop_after=0xFFFFFFFF; + +Error QuickHull::build(const Vector& p_points, Geometry::MeshData &r_mesh) { + + + static const real_t over_tolerance = 0.0001; + + /* CREATE AABB VOLUME */ + + AABB aabb; + for(int i=0;i valid_points; + valid_points.resize(p_points.size()); + Set valid_cache; + + for(int i=0;i max) { + simplex[1]=i; + max=d; + } + + } + } + + //third vertex is one most further away from the line + + + { + float maxd; + Vector3 rel12 = p_points[simplex[0]] - p_points[simplex[1]]; + + for(int i=0;imaxd) { + + maxd=d; + simplex[2]=i; + } + } + } + + //fourth vertex is the one most further away from the plane + + { + float maxd; + Plane p(p_points[simplex[0]],p_points[simplex[1]],p_points[simplex[2]]); + + for(int i=0;imaxd) { + + maxd=d; + simplex[3]=i; + } + } + } + + + //compute center of simplex, this is a point always warranted to be inside + Vector3 center; + + for(int i=0;i<4;i++) { + center+=p_points[simplex[i]]; + } + + center/=4.0; + + //add faces + + List faces; + + for(int i=0;i<4;i++) { + + static const int face_order[4][3]={ + {0,1,2}, + {0,1,3}, + {0,2,3}, + {1,2,3} + }; + + Face f; + for(int j=0;j<3;j++) { + f.vertices[j]=simplex[face_order[i][j]]; + } + + + Plane p(p_points[f.vertices[0]],p_points[f.vertices[1]],p_points[f.vertices[2]]); + + if (p.is_point_over(center)) { + //flip face to clockwise if facing inwards + SWAP( f.vertices[0], f.vertices[1] ); + p=-p; + } + + + f.plane = p; + + faces.push_back(f); + + } + + + /* COMPUTE AVAILABLE VERTICES */ + + for(int i=0;i::Element *E=faces.front();E;E=E->next()) { + + if (E->get().plane.distance_to(p_points[i]) > over_tolerance ) { + + E->get().points_over.push_back(i); + break; + } + } + + + + } + + faces.sort(); // sort them, so the ones with points are in the back + + + /* BUILD HULL */ + + + //poop face (while still remain) + //find further away point + //find lit faces + //determine horizon edges + //build new faces with horizon edges, them assign points side from all lit faces + //remove lit faces + + + uint32_t debug_stop = debug_stop_after; + + while(debug_stop>0 && faces.back()->get().points_over.size()) { + + debug_stop--; + Face& f = faces.back()->get(); + + //find vertex most outside + int next=-1; + real_t next_d=0; + + for(int i=0;i next_d) { + next_d=d; + next=i; + } + } + + ERR_FAIL_COND_V(next==-1,ERR_BUG); + + + + Vector3 v = p_points[f.points_over[next]]; + + //find lit faces and lit edges + List< List::Element* > lit_faces; //lit face is a death sentence + + Map lit_edges; //create this on the flight, should not be that bad for performance and simplifies code a lot + + for(List::Element *E=faces.front();E;E=E->next()) { + + if (E->get().plane.distance_to(v) >0 ) { + + lit_faces.push_back(E); + + for(int i=0;i<3;i++) { + uint32_t a = E->get().vertices[i]; + uint32_t b = E->get().vertices[(i+1)%3]; + Edge e(a,b); + + Map::Element *F=lit_edges.find(e); + if (!F) { + F=lit_edges.insert(e,FaceConnect()); + } + if (e.vertices[0]==a) { + //left + F->get().left=E; + } else { + + F->get().right=E; + } + } + } + } + + + //create new faces from horizon edges + List< List::Element* > new_faces; //new faces + + for(Map::Element *E=lit_edges.front();E;E=E->next()) { + + FaceConnect& fc = E->get(); + if (fc.left && fc.right) { + continue; //edge is uninteresting, not on horizont + } + + //create new face! + + Face face; + face.vertices[0]=f.points_over[next]; + face.vertices[1]=E->key().vertices[0]; + face.vertices[2]=E->key().vertices[1]; + + Plane p(p_points[face.vertices[0]],p_points[face.vertices[1]],p_points[face.vertices[2]]); + + if (p.is_point_over(center)) { + //flip face to clockwise if facing inwards + SWAP( face.vertices[0], face.vertices[1] ); + p = -p; + } + + face.plane = p; + new_faces.push_back( faces.push_back(face) ); + } + + //distribute points into new faces + + for(List< List::Element* >::Element *F=lit_faces.front();F;F=F->next()) { + + Face &lf = F->get()->get(); + + for(int i=0;i::Element* >::Element *E=new_faces.front();E;E=E->next()) { + + Face &f2 = E->get()->get(); + if (f2.plane.distance_to(p)>over_tolerance) { + f2.points_over.push_back(lf.points_over[i]); + break; + } + } + + + } + } + + //erase lit faces + + while(lit_faces.size()) { + + faces.erase(lit_faces.front()->get()); + lit_faces.pop_front(); + } + + //put faces that contain no points on the front + + for (List< List::Element* >::Element *E=new_faces.front();E;E=E->next()) { + + Face &f2 = E->get()->get(); + if (f2.points_over.size()==0) { + faces.move_to_front(E->get()); + } + } + + //whew, done with iteration, go next + + + + } + + /* CREATE MESHDATA */ + + + //make a map of edges again + Map ret_edges; + List ret_faces; + + + for(List::Element *E=faces.front();E;E=E->next()) { + + Geometry::MeshData::Face f; + f.plane = E->get().plane; + + + + for(int i=0;i<3;i++) { + f.indices.push_back(E->get().vertices[i]); + } + + List::Element *F = ret_faces.push_back(f); + + for(int i=0;i<3;i++) { + + uint32_t a = E->get().vertices[i]; + uint32_t b = E->get().vertices[(i+1)%3]; + Edge e(a,b); + + Map::Element *G=ret_edges.find(e); + if (!G) { + G=ret_edges.insert(e,RetFaceConnect()); + } + if (e.vertices[0]==a) { + //left + G->get().left=F; + } else { + + G->get().right=F; + } + } + } + + //fill faces + + for (List::Element *E=ret_faces.front();E;E=E->next()) { + + Geometry::MeshData::Face& f = E->get(); + + for(int i=0;iget().indices[i]; + uint32_t b = E->get().indices[(i+1)%f.indices.size()]; + Edge e(a,b); + + Map::Element *F=ret_edges.find(e); + + ERR_CONTINUE(!F); + + List::Element *O = F->get().left == E ? F->get().right : F->get().left; + ERR_CONTINUE(O==E); + + if (O->get().plane.is_almost_like(f.plane)) { + //merge and delete edge and contiguous face, while repointing edges (uuugh!) + int ois = O->get().indices.size(); + int merged=0; + + + for(int j=0;jget().indices[j]==a) { + //append the rest + for(int k=0;kget().indices[(k+j)%ois]; + int idxn = O->get().indices[(k+j+1)%ois]; + if (idx==b && idxn==a) {//already have b! + break; + } + if (idx!=a) { + f.indices.insert(i+1,idx); + i++; + merged++; + } + Edge e2(idx,idxn); + + Map::Element *F2=ret_edges.find(e2); + + ERR_CONTINUE(!F2); + //change faceconnect, point to this face instead + if (F2->get().left == O) + F2->get().left=E; + else if (F2->get().right == O) + F2->get().right=E; + + } + + break; + } + } + + + ret_edges.erase(F); //remove the edge + ret_faces.erase(O); //remove the face + + + } + + } + + } + + //fill mesh + r_mesh.faces.clear(); + r_mesh.faces.resize(ret_faces.size()); +// print_line("FACECOUNT: "+itos(r_mesh.faces.size())); + + int idx=0; + for (List::Element *E=ret_faces.front();E;E=E->next()) { + r_mesh.faces[idx++]=E->get(); + + + } + r_mesh.edges.resize(ret_edges.size()); + idx=0; + for(Map::Element *E=ret_edges.front();E;E=E->next()) { + + Geometry::MeshData::Edge e; + e.a=E->key().vertices[0]; + e.b=E->key().vertices[1]; + r_mesh.edges[idx++]=e; + } + + r_mesh.vertices=p_points; + + //r_mesh.optimize_vertices(); +/* + print_line("FACES: "+itos(r_mesh.faces.size())); + print_line("EDGES: "+itos(r_mesh.edges.size())); + print_line("VERTICES: "+itos(r_mesh.vertices.size())); +*/ + + return OK; +} diff --git a/core/math/quick_hull.h b/core/math/quick_hull.h new file mode 100644 index 000000000..d7f056d36 --- /dev/null +++ b/core/math/quick_hull.h @@ -0,0 +1,95 @@ +/*************************************************************************/ +/* quick_hull.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 QUICK_HULL_H +#define QUICK_HULL_H + +#include "aabb.h" +#include "set.h" +#include "list.h" +#include "geometry.h" + +class QuickHull { + +public: + + + struct Edge { + + union { + uint32_t vertices[2]; + uint64_t id; + }; + + + bool operator<(const Edge& p_edge) const { + return id < p_edge.id; + } + + Edge(int p_vtx_a=0,int p_vtx_b=0) { + + if (p_vtx_a>p_vtx_b) { + SWAP(p_vtx_a,p_vtx_b); + } + + vertices[0]=p_vtx_a; + vertices[1]=p_vtx_b; + } + }; + + struct Face { + + Plane plane; + int vertices[3]; + Vector points_over; + + bool operator<(const Face& p_face) const { + + return points_over.size() < p_face.points_over.size(); + } + + }; +private: + + struct FaceConnect { + List::Element *left,*right; + FaceConnect() { left=NULL; right=NULL; } + }; + struct RetFaceConnect { + List::Element *left,*right; + RetFaceConnect() { left=NULL; right=NULL; } + }; + +public: + + static uint32_t debug_stop_after; + static Error build(const Vector& p_points,Geometry::MeshData& r_mesh); + +}; + +#endif // QUICK_HULL_H diff --git a/core/math/transform.cpp b/core/math/transform.cpp new file mode 100644 index 000000000..bb874facb --- /dev/null +++ b/core/math/transform.cpp @@ -0,0 +1,218 @@ +/*************************************************************************/ +/* transform.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "transform.h" +#include "math_funcs.h" +#include "os/copymem.h" +#include "print_string.h" + + +void Transform::affine_invert() { + + basis.invert(); + origin = basis.xform(-origin); +} + +Transform Transform::affine_inverse() const { + + Transform ret=*this; + ret.affine_invert(); + return ret; + +} + + +void Transform::invert() { + + basis.transpose(); + origin = basis.xform(-origin); +} + +Transform Transform::inverse() const { + + Transform ret=*this; + ret.invert(); + return ret; +} + + +void Transform::rotate(const Vector3& p_axis,real_t p_phi) { + + *this = *this * Transform( Matrix3( p_axis, p_phi ), Vector3() ); +} + +Transform Transform::rotated(const Vector3& p_axis,real_t p_phi) const{ + + return *this * Transform( Matrix3( p_axis, p_phi ), Vector3() ); +} + +void Transform::rotate_basis(const Vector3& p_axis,real_t p_phi) { + + basis.rotate(p_axis,p_phi); +} + +Transform Transform::looking_at( const Vector3& p_target, const Vector3& p_up ) const { + + Transform t = *this; + t.set_look_at(origin,p_target,p_up); + return t; +} + +void Transform::set_look_at( const Vector3& p_eye, const Vector3& p_target, const Vector3& p_up ) { + + // Reference: MESA source code + Vector3 v_x, v_y, v_z; + + /* Make rotation matrix */ + + /* Z vector */ + v_z = p_eye - p_target; + + v_z.normalize(); + + v_y = p_up; + + + v_x=v_y.cross(v_z); + + /* Recompute Y = Z cross X */ + v_y=v_z.cross(v_x); + + v_x.normalize(); + v_y.normalize(); + + basis.set_axis(0,v_x); + basis.set_axis(1,v_y); + basis.set_axis(2,v_z); + origin=p_eye; + +} + +Transform Transform::interpolate_with(const Transform& p_transform, float p_c) const { + + /* not sure if very "efficient" but good enough? */ + + Vector3 src_scale = basis.get_scale(); + Quat src_rot = basis; + Vector3 src_loc = origin; + + Vector3 dst_scale = p_transform.basis.get_scale(); + Quat dst_rot = p_transform.basis; + Vector3 dst_loc = p_transform.origin; + + Transform dst; + dst.basis=src_rot.slerp(dst_rot,p_c); + dst.basis.scale(src_scale.linear_interpolate(dst_scale,p_c)); + dst.origin=src_loc.linear_interpolate(dst_loc,p_c); + + return dst; +} + +void Transform::scale(const Vector3& p_scale) { + + basis.scale(p_scale); + origin*=p_scale; +} + +Transform Transform::scaled(const Vector3& p_scale) const { + + Transform t = *this; + t.scale(p_scale); + return t; +} + +void Transform::scale_basis(const Vector3& p_scale) { + + basis.scale(p_scale); +} + +void Transform::translate( real_t p_tx, real_t p_ty, real_t p_tz) { + translate( Vector3(p_tx,p_ty,p_tz) ); + +} +void Transform::translate( const Vector3& p_translation ) { + + for( int i = 0; i < 3; i++ ) { + origin[i] += basis[i].dot(p_translation); + } +} + +Transform Transform::translated( const Vector3& p_translation ) const { + + Transform t=*this; + t.translate(p_translation); + return t; +} + +void Transform::orthonormalize() { + + basis.orthonormalize(); +} + +Transform Transform::orthonormalized() const { + + Transform _copy = *this; + _copy.orthonormalize(); + return _copy; +} + +bool Transform::operator==(const Transform& p_transform) const { + + return (basis==p_transform.basis && origin==p_transform.origin); +} +bool Transform::operator!=(const Transform& p_transform) const { + + return (basis!=p_transform.basis || origin!=p_transform.origin); +} + +void Transform::operator*=(const Transform& p_transform) { + + origin=xform(p_transform.origin); + basis*=p_transform.basis; +} + +Transform Transform::operator*(const Transform& p_transform) const { + + Transform t=*this; + t*=p_transform; + return t; +} + +Transform::operator String() const { + + return basis.operator String() + " - " + origin.operator String(); +} + + +Transform::Transform(const Matrix3& p_basis, const Vector3& p_origin) { + + basis=p_basis; + origin=p_origin; +} + + diff --git a/core/math/transform.h b/core/math/transform.h new file mode 100644 index 000000000..b1a0ea1ab --- /dev/null +++ b/core/math/transform.h @@ -0,0 +1,268 @@ +/*************************************************************************/ +/* transform.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 TRANSFORM_H +#define TRANSFORM_H + +#include "matrix3.h" +#include "plane.h" +#include "aabb.h" +/** + @author Juan Linietsky +*/ +class Transform { +public: + + Matrix3 basis; + Vector3 origin; + + void invert(); + Transform inverse() const; + + void affine_invert(); + Transform affine_inverse() const; + + Transform rotated(const Vector3& p_axis,real_t p_phi) const; + + void rotate(const Vector3& p_axis,real_t p_phi); + void rotate_basis(const Vector3& p_axis,real_t p_phi); + + void set_look_at( const Vector3& p_eye, const Vector3& p_target, const Vector3& p_up ); + Transform looking_at( const Vector3& p_target, const Vector3& p_up ) const; + + void scale(const Vector3& p_scale); + Transform scaled(const Vector3& p_scale) const; + void scale_basis(const Vector3& p_scale); + void translate( real_t p_tx, real_t p_ty, real_t p_tz ); + void translate( const Vector3& p_translation ); + Transform translated( const Vector3& p_translation ) const; + + const Matrix3& get_basis() const { return basis; } + void set_basis(const Matrix3& p_basis) { basis=p_basis; } + + const Vector3& get_origin() const { return origin; } + void set_origin(const Vector3& p_origin) { origin=p_origin; } + + void orthonormalize(); + Transform orthonormalized() const; + + bool operator==(const Transform& p_transform) const; + bool operator!=(const Transform& p_transform) const; + + _FORCE_INLINE_ Vector3 xform(const Vector3& p_vector) const; + _FORCE_INLINE_ Vector3 xform_inv(const Vector3& p_vector) const; + + _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; + + void operator*=(const Transform& p_transform); + Transform operator*(const Transform& p_transform) const; + + Transform interpolate_with(const Transform& p_transform, float p_c) const; + + _FORCE_INLINE_ Transform inverse_xform(const Transform& t) const { + + Vector3 v = t.origin - origin; + return Transform(basis.transpose_xform(t.basis), + basis.xform(v)); + } + + void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz,real_t tx, real_t ty, real_t tz) { + + basis.elements[0][0]=xx; + basis.elements[0][1]=xy; + basis.elements[0][2]=xz; + basis.elements[1][0]=yx; + basis.elements[1][1]=yy; + basis.elements[1][2]=yz; + basis.elements[2][0]=zx; + basis.elements[2][1]=zy; + basis.elements[2][2]=zz; + origin.x=tx; + origin.y=ty; + origin.z=tz; + } + + operator String() const; + + Transform(const Matrix3& p_basis, const Vector3& p_origin=Vector3()); + Transform() {} + +}; + + +_FORCE_INLINE_ Vector3 Transform::xform(const Vector3& p_vector) const { + + return Vector3( + basis[0].dot(p_vector)+origin.x, + basis[1].dot(p_vector)+origin.y, + basis[2].dot(p_vector)+origin.z + ); +} +_FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3& p_vector) const { + + Vector3 v = p_vector - origin; + + return Vector3( + (basis.elements[0][0]*v.x ) + ( basis.elements[1][0]*v.y ) + ( basis.elements[2][0]*v.z ), + (basis.elements[0][1]*v.x ) + ( basis.elements[1][1]*v.y ) + ( basis.elements[2][1]*v.z ), + (basis.elements[0][2]*v.x ) + ( basis.elements[1][2]*v.y ) + ( basis.elements[2][2]*v.z ) + ); +} + +_FORCE_INLINE_ Plane Transform::xform(const Plane& p_plane) const { + + + Vector3 point=p_plane.normal*p_plane.d; + Vector3 point_dir=point+p_plane.normal; + point=xform(point); + point_dir=xform(point_dir); + + Vector3 normal=point_dir-point; + normal.normalize(); + real_t d=normal.dot(point); + + return Plane(normal,d); + +} +_FORCE_INLINE_ Plane Transform::xform_inv(const Plane& p_plane) const { + + Vector3 point=p_plane.normal*p_plane.d; + Vector3 point_dir=point+p_plane.normal; + xform_inv(point); + xform_inv(point_dir); + + Vector3 normal=point_dir-point; + normal.normalize(); + real_t d=normal.dot(point); + + return Plane(normal,d); + +} + +_FORCE_INLINE_ AABB Transform::xform(const AABB& p_aabb) const { + /* define vertices */ +#if 1 + Vector3 x=basis.get_axis(0)*p_aabb.size.x; + Vector3 y=basis.get_axis(1)*p_aabb.size.y; + Vector3 z=basis.get_axis(2)*p_aabb.size.z; + Vector3 pos = xform( p_aabb.pos ); +//could be even further optimized + AABB new_aabb; + new_aabb.pos=pos; + new_aabb.expand_to( pos+x ); + new_aabb.expand_to( pos+y ); + new_aabb.expand_to( pos+z ); + new_aabb.expand_to( pos+x+y ); + new_aabb.expand_to( pos+x+z ); + new_aabb.expand_to( pos+y+z ); + new_aabb.expand_to( pos+x+y+z ); + return new_aabb; +#else + + + Vector3 vertices[8]={ + Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z+p_aabb.size.z), + Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z), + Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y, p_aabb.pos.z+p_aabb.size.z), + Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y, p_aabb.pos.z), + Vector3(p_aabb.pos.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z+p_aabb.size.z), + Vector3(p_aabb.pos.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z), + Vector3(p_aabb.pos.x, p_aabb.pos.y, p_aabb.pos.z+p_aabb.size.z), + Vector3(p_aabb.pos.x, p_aabb.pos.y, p_aabb.pos.z) + }; + + + AABB ret; + + ret.pos=xform(vertices[0]); + + for (int i=1;i<8;i++) { + + ret.expand_to( xform(vertices[i]) ); + } + + return ret; +#endif + +} +_FORCE_INLINE_ AABB Transform::xform_inv(const AABB& p_aabb) const { + + /* define vertices */ + Vector3 vertices[8]={ + Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z+p_aabb.size.z), + Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z), + Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y, p_aabb.pos.z+p_aabb.size.z), + Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y, p_aabb.pos.z), + Vector3(p_aabb.pos.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z+p_aabb.size.z), + Vector3(p_aabb.pos.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z), + Vector3(p_aabb.pos.x, p_aabb.pos.y, p_aabb.pos.z+p_aabb.size.z), + Vector3(p_aabb.pos.x, p_aabb.pos.y, p_aabb.pos.z) + }; + + + AABB ret; + + ret.pos=xform_inv(vertices[0]); + + for (int i=1;i<8;i++) { + + ret.expand_to( xform_inv(vertices[i]) ); + } + + return ret; + +} + +#ifdef OPTIMIZED_TRANSFORM_IMPL_OVERRIDE + +#else + +struct OptimizedTransform { + + Transform transform; + + _FORCE_INLINE_ void invert() {transform.invert(); } + _FORCE_INLINE_ void affine_invert() {transform.affine_invert(); } + _FORCE_INLINE_ Vector3 xform(const Vector3& p_vec) const { return transform.xform(p_vec); }; + _FORCE_INLINE_ Vector3 xform_inv(const Vector3& p_vec) const { return transform.xform_inv(p_vec); }; + _FORCE_INLINE_ OptimizedTransform operator*(const OptimizedTransform& p_ot ) const { return OptimizedTransform( transform * p_ot.transform ) ; } + _FORCE_INLINE_ Transform get_transform() const { return transform; } + _FORCE_INLINE_ void set_transform(const Transform& p_transform) { transform=p_transform; } + + OptimizedTransform(const Transform& p_transform) { + transform=p_transform; + } +}; + +#endif + +#endif diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp new file mode 100644 index 000000000..111ceca18 --- /dev/null +++ b/core/math/triangle_mesh.cpp @@ -0,0 +1,568 @@ +/*************************************************************************/ +/* triangle_mesh.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "triangle_mesh.h" +#include "sort.h" + + + +int TriangleMesh::_create_bvh(BVH*p_bvh,BVH** p_bb,int p_from,int p_size,int p_depth,int&max_depth,int&max_alloc) { + + + if (p_depth>max_depth) { + max_depth=p_depth; + } + + if (p_size==1) { + + + return p_bb[p_from]-p_bvh; + } else if (p_size==0) { + + return -1; + } + + + AABB aabb; + aabb=p_bb[p_from]->aabb; + for(int i=1;iaabb); + } + + int li=aabb.get_longest_axis_index(); + + switch(li) { + + case Vector3::AXIS_X: { + SortArray sort_x; + sort_x.nth_element(0,p_size,p_size/2,&p_bb[p_from]); + //sort_x.sort(&p_bb[p_from],p_size); + } break; + case Vector3::AXIS_Y: { + SortArray sort_y; + sort_y.nth_element(0,p_size,p_size/2,&p_bb[p_from]); + //sort_y.sort(&p_bb[p_from],p_size); + } break; + case Vector3::AXIS_Z: { + SortArray sort_z; + sort_z.nth_element(0,p_size,p_size/2,&p_bb[p_from]); + //sort_z.sort(&p_bb[p_from],p_size); + + } break; + } + + + int left = _create_bvh(p_bvh,p_bb,p_from,p_size/2,p_depth+1,max_depth,max_alloc); + int right = _create_bvh(p_bvh,p_bb,p_from+p_size/2,p_size-p_size/2,p_depth+1,max_depth,max_alloc); + + int index=max_alloc++; + BVH *_new = &p_bvh[index]; + _new->aabb=aabb; + _new->center=aabb.pos+aabb.size*0.5; + _new->face_index=-1; + _new->left=left; + _new->right=right; + + return index; + +} + + +void TriangleMesh::create(const DVector& p_faces) { + + valid=false; + + int fc=p_faces.size(); + ERR_FAIL_COND(!fc || ((fc%3) != 0)); + fc/=3; + triangles.resize(fc); + + bvh.resize(fc*3); //will never be larger than this (todo make better) + DVector::Write bw = bvh.write(); + + { + + //create faces and indices and base bvh + //except for the Set for repeated triangles, everything + //goes in-place. + + DVector::Read r = p_faces.read(); + DVector::Write w = triangles.write(); + Map db; + + for(int i=0;i::Element *E=db.find(vs); + if (E) { + vidx=E->get(); + } else { + vidx=db.size(); + db[vs]=vidx; + } + + f.indices[j]=vidx; + if (j==0) + bw[i].aabb.pos=vs; + else + bw[i].aabb.expand_to(vs); + } + + f.normal=Face3(r[i*3+0],r[i*3+1],r[i*3+2]).get_plane().get_normal(); + + bw[i].left=-1; + bw[i].right=-1; + bw[i].face_index=i; + bw[i].center=bw[i].aabb.pos+bw[i].aabb.size*0.5; + } + + vertices.resize(db.size()); + DVector::Write vw = vertices.write(); + for (Map::Element *E=db.front();E;E=E->next()) { + vw[E->get()]=E->key(); + } + + } + + DVector bwptrs; + bwptrs.resize(fc); + DVector::Write bwp = bwptrs.write(); + for(int i=0;i::Write(); //clearup + bvh.resize(max_alloc); //resize back + + valid=true; + +} + + +Vector3 TriangleMesh::get_area_normal(const AABB& p_aabb) const { + + uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth); + + enum { + TEST_AABB_BIT=0, + VISIT_LEFT_BIT=1, + VISIT_RIGHT_BIT=2, + VISIT_DONE_BIT=3, + VISITED_BIT_SHIFT=29, + NODE_IDX_MASK=(1<::Read trianglesr = triangles.read(); + DVector::Read verticesr=vertices.read(); + DVector::Read bvhr=bvh.read(); + + const Triangle *triangleptr=trianglesr.ptr(); + const Vector3 *vertexptr=verticesr.ptr(); + int pos=bvh.size()-1; + const BVH *bvhptr = bvhr.ptr(); + + stack[0]=pos; + while(true) { + + uint32_t node = stack[level]&NODE_IDX_MASK; + const BVH &b = bvhptr[ node ]; + bool done=false; + + switch(stack[level]>>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = b.aabb.intersects(p_aabb); + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<=0) { + + const Triangle &s=triangleptr[ b.face_index ]; + n+=s.normal; + n_count++; + + stack[level]=(VISIT_DONE_BIT<0) + n/=n_count; + + return n; + +} + + +bool TriangleMesh::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const { + + + uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth); + + enum { + TEST_AABB_BIT=0, + VISIT_LEFT_BIT=1, + VISIT_RIGHT_BIT=2, + VISIT_DONE_BIT=3, + VISITED_BIT_SHIFT=29, + NODE_IDX_MASK=(1<::Read trianglesr = triangles.read(); + DVector::Read verticesr=vertices.read(); + DVector::Read bvhr=bvh.read(); + + const Triangle *triangleptr=trianglesr.ptr(); + const Vector3 *vertexptr=verticesr.ptr(); + int pos=bvh.size()-1; + const BVH *bvhptr = bvhr.ptr(); + + stack[0]=pos; + while(true) { + + uint32_t node = stack[level]&NODE_IDX_MASK; + const BVH &b = bvhptr[ node ]; + bool done=false; + + switch(stack[level]>>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = b.aabb.intersects_segment(p_begin,p_end); +// bool valid = b.aabb.intersects(ray_aabb); + + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<=0) { + + const Triangle &s=triangleptr[ b.face_index ]; + Face3 f3(vertexptr[ s.indices[0] ],vertexptr[ s.indices[1] ],vertexptr[ s.indices[2] ]); + + + Vector3 res; + + if (f3.intersects_segment(p_begin,p_end,&res)) { + + + float nd = n.dot(res); + if (nd0) + r_normal=-r_normal; + } + + return inters; +} + + +bool TriangleMesh::intersect_ray(const Vector3& p_begin,const Vector3& p_dir,Vector3 &r_point, Vector3 &r_normal) const { + + + uint32_t* stack = (uint32_t*)alloca(sizeof(int)*max_depth); + + enum { + TEST_AABB_BIT=0, + VISIT_LEFT_BIT=1, + VISIT_RIGHT_BIT=2, + VISIT_DONE_BIT=3, + VISITED_BIT_SHIFT=29, + NODE_IDX_MASK=(1<::Read trianglesr = triangles.read(); + DVector::Read verticesr=vertices.read(); + DVector::Read bvhr=bvh.read(); + + const Triangle *triangleptr=trianglesr.ptr(); + const Vector3 *vertexptr=verticesr.ptr(); + int pos=bvh.size()-1; + const BVH *bvhptr = bvhr.ptr(); + + stack[0]=pos; + while(true) { + + uint32_t node = stack[level]&NODE_IDX_MASK; + const BVH &b = bvhptr[ node ]; + bool done=false; + + switch(stack[level]>>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = b.aabb.intersects_ray(p_begin,p_dir); + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<=0) { + + const Triangle &s=triangleptr[ b.face_index ]; + Face3 f3(vertexptr[ s.indices[0] ],vertexptr[ s.indices[1] ],vertexptr[ s.indices[2] ]); + + + Vector3 res; + + if (f3.intersects_ray(p_begin,p_dir,&res)) { + + + float nd = n.dot(res); + if (nd0) + r_normal=-r_normal; + } + + return inters; +} + +bool TriangleMesh::is_valid() const { + + return valid; +} + +DVector TriangleMesh::get_faces() const { + + if (!valid) + return DVector(); + + DVector faces; + int ts = triangles.size(); + faces.resize(triangles.size()); + + DVector::Write w=faces.write(); + DVector::Read r = triangles.read(); + DVector::Read rv = vertices.read(); + + for(int i=0;i::Write(); + return faces; +} + +TriangleMesh::TriangleMesh() { + + valid=false; + max_depth=0; +} diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h new file mode 100644 index 000000000..ab0cb713b --- /dev/null +++ b/core/math/triangle_mesh.h @@ -0,0 +1,99 @@ +/*************************************************************************/ +/* triangle_mesh.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 TRIANGLE_MESH_H +#define TRIANGLE_MESH_H + +#include "reference.h" +#include "face3.h" +class TriangleMesh : public Reference { + + OBJ_TYPE( TriangleMesh, Reference); + + struct Triangle { + + Vector3 normal; + int indices[3]; + }; + + DVector triangles; + DVector vertices; + + struct BVH { + + AABB aabb; + Vector3 center; //used for sorting + int left; + int right; + + int face_index; + }; + + struct BVHCmpX { + + bool operator()(const BVH* p_left, const BVH* p_right) const { + + return p_left->center.x < p_right->center.x; + } + }; + + struct BVHCmpY { + + bool operator()(const BVH* p_left, const BVH* p_right) const { + + return p_left->center.y < p_right->center.y; + } + }; + struct BVHCmpZ { + + bool operator()(const BVH* p_left, const BVH* p_right) const { + + return p_left->center.z < p_right->center.z; + } + }; + + 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; + int max_depth; + bool valid; + +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 get_faces() const; + + + void create(const DVector& p_faces); + TriangleMesh(); +}; + +#endif // TRIANGLE_MESH_H diff --git a/core/math/triangulate.cpp b/core/math/triangulate.cpp new file mode 100644 index 000000000..4a870def4 --- /dev/null +++ b/core/math/triangulate.cpp @@ -0,0 +1,168 @@ +/*************************************************************************/ +/* triangulate.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "triangulate.h" + +float Triangulate::get_area(const Vector &contour) +{ + + int n = contour.size(); + const Vector2 *c=&contour[0]; + + float A=0.0f; + + for(int p=n-1,q=0; q= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f)); +}; + +bool Triangulate::snip(const Vector &p_contour,int u,int v,int w,int n,int *V) +{ + int p; + float Ax, Ay, Bx, By, Cx, Cy, Px, Py; + const Vector2 *contour=&p_contour[0]; + + Ax = contour[V[u]].x; + Ay = contour[V[u]].y; + + Bx = contour[V[v]].x; + By = contour[V[v]].y; + + Cx = contour[V[w]].x; + Cy = contour[V[w]].y; + + if ( CMP_EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false; + + for (p=0;p &contour,Vector &result) +{ + /* allocate and initialize list of Vertices in polygon */ + + int n = contour.size(); + if ( n < 3 ) return false; + + + + int *V = (int*)alloca(sizeof(int)*n); + + /* we want a counter-clockwise polygon in V */ + + if ( 0.0f < get_area(contour) ) + for (int v=0; v2; ) + { + /* if we loop, it is probably a non-simple polygon */ + if (0 >= (count--)) + { + //** Triangulate: ERROR - probable bad polygon! + return false; + } + + /* three consecutive vertices in current polygon, */ + int u = v ; if (nv <= u) u = 0; /* previous */ + v = u+1; if (nv <= v) v = 0; /* new v */ + int w = v+1; if (nv <= w) w = 0; /* next */ + + if ( snip(contour,u,v,w,nv,V) ) + { + int a,b,c,s,t; + + /* true names of the vertices */ + 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 &contour, Vector &result); + + // compute area of a contour/polygon + static float 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); + + +private: + static bool snip(const Vector &p_contour,int u,int v,int w,int n,int *V); + +}; + + + +#endif diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp new file mode 100644 index 000000000..cf6fd9242 --- /dev/null +++ b/core/math/vector3.cpp @@ -0,0 +1,201 @@ +/*************************************************************************/ +/* vector3.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 "vector3.h" + + + +void Vector3::rotate(const Vector3& p_axis,float p_phi) { + + Vector3 axis1 = cross(p_axis); + float l = axis1.length(); + if (l==0) + return; + axis1/=l; + Vector3 axis2 = axis1.cross(p_axis).normalized(); + + float _x = axis1.dot(*this); + float _y = axis2.dot(*this); + + float ang = Math::atan2(_x,_y); + + ang+=p_phi; + + *this=((axis1 * Math::cos(ang)) + (axis2 * Math::sin(ang))) * length(); + +} + +Vector3 Vector3::rotated(const Vector3& p_axis,float p_phi) const { + + Vector3 r = *this; + r.rotate(p_axis,p_phi); + return r; +} + +void Vector3::set_axis(int p_axis,real_t p_value) { + ERR_FAIL_INDEX(p_axis,3); + coord[p_axis]=p_value; + +} +real_t Vector3::get_axis(int p_axis) const { + + ERR_FAIL_INDEX_V(p_axis,3,0); + return operator[](p_axis); +} + +int Vector3::min_axis() const { + + return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2); +} +int Vector3::max_axis() const { + + return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0); +} + + +void Vector3::snap(float p_val) { + + x+=p_val/2.0; + x-=Math::fmod(x,p_val); + y+=p_val/2.0; + y-=Math::fmod(y,p_val); + z+=p_val/2.0; + z-=Math::fmod(z,p_val); + +} +Vector3 Vector3::snapped(float p_val) const { + + Vector3 v=*this; + v.snap(p_val); + return v; +} + + +Vector3 Vector3::cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const { + + Vector3 p0=p_pre_a; + Vector3 p1=*this; + Vector3 p2=p_b; + Vector3 p3=p_post_b; + + { + //normalize + + float ab = p0.distance_to(p1); + float bc = p1.distance_to(p2); + float cd = p2.distance_to(p3); + + if (ab>0) + p0 = p1+(p0-p1)*(bc/ab); + if (cd>0) + p3 = p2+(p3-p2)*(bc/cd); + } + + + float t = p_t; + float t2 = t * t; + float t3 = t2 * t; + + Vector3 out; + out = 0.5f * ( ( p1 * 2.0f) + + ( -p0 + p2 ) * t + + ( 2.0f * p0 - 5.0f * p1 + 4 * p2 - p3 ) * t2 + + ( -p0 + 3.0f * p1 - 3.0f * 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 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; + + Vector3 out; + out = 0.5f * ( ( p1 * 2.0f) + + ( -p0 + p2 ) * t + + ( 2.0f * p0 - 5.0f * p1 + 4 * p2 - p3 ) * t2 + + ( -p0 + 3.0f * p1 - 3.0f * 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 p0=p_pre_a; + Vector3 p1=*this; + Vector3 p2=p_b; + Vector3 p3=p_post_b; + + if (true) { + + float ab = p0.distance_to(p1); + float bc = p1.distance_to(p2); + float cd = p2.distance_to(p3); + + //if (ab>bc) { + if (ab>0) + p0 = p1+(p0-p1)*(bc/ab); + //} + + //if (cd>bc) { + if (cd>0) + p3 = p2+(p3-p2)*(bc/cd); + //} + } + + float t = p_t; + float t2 = t * t; + float t3 = t2 * t; + + Vector3 out; + out.x = 0.5f * ( ( 2.0f * 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 ) + + ( -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 ) + + ( -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 ); + return out; +} +# endif +Vector3::operator String() const { + + return (rtos(x)+", "+rtos(y)+", "+rtos(z)); +} diff --git a/core/math/vector3.h b/core/math/vector3.h new file mode 100644 index 000000000..959f7cd0a --- /dev/null +++ b/core/math/vector3.h @@ -0,0 +1,373 @@ +/*************************************************************************/ +/* vector3.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 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 VECTOR3_H +#define VECTOR3_H + +#include "typedefs.h" +#include "math_defs.h" +#include "math_funcs.h" +#include "ustring.h" + + +struct Vector3 { + + enum Axis { + AXIS_X, + AXIS_Y, + AXIS_Z, + }; + + union { + +#ifdef USE_QUAD_VECTORS + + struct { + real_t x; + real_t y; + real_t z; + real_t _unused; + }; + real_t coord[4]; +#else + + struct { + real_t x; + real_t y; + real_t z; + }; + + real_t coord[3]; +#endif + }; + + _FORCE_INLINE_ const real_t& operator[](int p_axis) const { + + return coord[p_axis]; + } + + _FORCE_INLINE_ real_t& operator[](int p_axis) { + + return coord[p_axis]; + } + + void set_axis(int p_axis,real_t p_value); + real_t get_axis(int p_axis) const; + + int min_axis() const; + int max_axis() const; + + _FORCE_INLINE_ real_t length() const; + _FORCE_INLINE_ real_t length_squared() const; + + _FORCE_INLINE_ void normalize(); + _FORCE_INLINE_ Vector3 normalized() const; + _FORCE_INLINE_ Vector3 inverse() const; + + _FORCE_INLINE_ void zero(); + + void snap(float p_val); + Vector3 snapped(float p_val) const; + + void rotate(const Vector3& p_axis,float p_phi); + Vector3 rotated(const Vector3& p_axis,float 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 cross(const Vector3& p_b) const; + _FORCE_INLINE_ real_t dot(const Vector3& p_b) const; + + _FORCE_INLINE_ Vector3 abs() const; + + _FORCE_INLINE_ real_t distance_to(const Vector3& p_b) const; + _FORCE_INLINE_ real_t distance_squared_to(const Vector3& p_b) const; + + /* Operators */ + + _FORCE_INLINE_ Vector3& operator+=(const Vector3& p_v); + _FORCE_INLINE_ Vector3 operator+(const Vector3& p_v) const; + _FORCE_INLINE_ Vector3& operator-=(const Vector3& p_v); + _FORCE_INLINE_ Vector3 operator-(const Vector3& p_v) const; + _FORCE_INLINE_ Vector3& operator*=(const Vector3& p_v); + _FORCE_INLINE_ Vector3 operator*(const Vector3& p_v) const; + _FORCE_INLINE_ Vector3& operator/=(const Vector3& p_v); + _FORCE_INLINE_ Vector3 operator/(const Vector3& p_v) const; + + + _FORCE_INLINE_ Vector3& operator*=(real_t p_scalar); + _FORCE_INLINE_ Vector3 operator*(real_t p_scalar) const; + _FORCE_INLINE_ Vector3& operator/=(real_t p_scalar); + _FORCE_INLINE_ Vector3 operator/(real_t p_scalar) const; + + _FORCE_INLINE_ Vector3 operator-() const; + + _FORCE_INLINE_ bool operator==(const Vector3& p_v) const; + _FORCE_INLINE_ bool operator!=(const Vector3& p_v) const; + _FORCE_INLINE_ bool operator<(const Vector3& p_v) const; + _FORCE_INLINE_ bool operator<=(const Vector3& p_v) const; + + operator String() const; + + _FORCE_INLINE_ Vector3() { x=y=z=0; } + _FORCE_INLINE_ Vector3(real_t p_x,real_t p_y,real_t p_z) { x=p_x; y=p_y; z=p_z; } + +}; + +#ifdef VECTOR3_IMPL_OVERRIDE + +#include "vector3_inline.h" + +#else + +Vector3 Vector3::cross(const Vector3& p_b) const { + + Vector3 ret ( + (y * p_b.z) - (z * p_b.y), + (z * p_b.x) - (x * p_b.z), + (x * p_b.y) - (y * p_b.x) + ); + + return ret; +} +real_t Vector3::dot(const Vector3& p_b) const { + + return x*p_b.x + y*p_b.y + z*p_b.z; +} + +Vector3 Vector3::abs() const { + + return Vector3( Math::abs(x), Math::abs(y), Math::abs(z) ); +} + +Vector3 Vector3::linear_interpolate(const Vector3& p_b,float p_t) const { + + return Vector3( + x+(p_t * (p_b.x-x)), + y+(p_t * (p_b.y-y)), + z+(p_t * (p_b.z-z)) + ); + +} + + + +real_t Vector3::distance_to(const Vector3& p_b) const { + return (p_b-*this).length(); +} +real_t Vector3::distance_squared_to(const Vector3& p_b) const { + + return (p_b-*this).length_squared(); +} + +/* Operators */ + +Vector3& Vector3::operator+=(const Vector3& p_v) { + + x+=p_v.x; + y+=p_v.y; + z+=p_v.z; + return *this; +} +Vector3 Vector3::operator+(const Vector3& p_v) const { + + return Vector3(x+p_v.x, y+p_v.y, z+ p_v.z); +} + +Vector3& Vector3::operator-=(const Vector3& p_v) { + + x-=p_v.x; + y-=p_v.y; + z-=p_v.z; + return *this; +} +Vector3 Vector3::operator-(const Vector3& p_v) const { + + return Vector3(x-p_v.x, y-p_v.y, z- p_v.z); +} + + + +Vector3& Vector3::operator*=(const Vector3& p_v) { + + x*=p_v.x; + y*=p_v.y; + z*=p_v.z; + return *this; +} +Vector3 Vector3::operator*(const Vector3& p_v) const { + + return Vector3(x*p_v.x, y*p_v.y, z* p_v.z); +} + +Vector3& Vector3::operator/=(const Vector3& p_v) { + + x/=p_v.x; + y/=p_v.y; + z/=p_v.z; + return *this; +} +Vector3 Vector3::operator/(const Vector3& p_v) const { + + return Vector3(x/p_v.x, y/p_v.y, z/ p_v.z); +} + +Vector3& Vector3::operator*=(real_t p_scalar) { + x*=p_scalar; + y*=p_scalar; + z*=p_scalar; + return *this; + +} + +_FORCE_INLINE_ Vector3 operator*(real_t p_scalar, const Vector3& p_vec) { + return p_vec * p_scalar; +} + +Vector3 Vector3::operator*(real_t p_scalar) const { + + return Vector3( x*p_scalar, y*p_scalar, z*p_scalar); +} + +Vector3& Vector3::operator/=(real_t p_scalar) { + x/=p_scalar; + y/=p_scalar; + z/=p_scalar; + return *this; + +} + +Vector3 Vector3::operator/(real_t p_scalar) const { + + return Vector3( x/p_scalar, y/p_scalar, z/p_scalar); +} + +Vector3 Vector3::operator-() const { + + return Vector3( -x, -y, -z ); +} + + +bool Vector3::operator==(const Vector3& p_v) const { + + return (x==p_v.x && y==p_v.y && z==p_v.z); +} + +bool Vector3::operator!=(const Vector3& p_v) const { + + return (x!=p_v.x || y!=p_v.y || z!=p_v.z); +} + +bool Vector3::operator<(const Vector3& p_v) const { + + if (x==p_v.x) { + if (y==p_v.y) + return z