diff options
Diffstat (limited to 'core/math')
38 files changed, 3204 insertions, 743 deletions
diff --git a/core/math/SCsub b/core/math/SCsub index c6ba1fa537..7b4a6acbc0 100644 --- a/core/math/SCsub +++ b/core/math/SCsub @@ -3,5 +3,3 @@ Import('env') env.add_source_files(env.core_sources,"*.cpp") Export('env') - - diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp index 576e4fa928..435df66aab 100644 --- a/core/math/aabb.cpp +++ b/core/math/aabb.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/aabb.h b/core/math/aabb.h index 089d5d15f7..4781e5c263 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -110,17 +110,17 @@ public: inline bool AABB::intersects(const AABB& p_aabb) const { - if ( pos.x > (p_aabb.pos.x + p_aabb.size.x) ) + if ( pos.x >= (p_aabb.pos.x + p_aabb.size.x) ) return false; - if ( (pos.x+size.x) < p_aabb.pos.x ) + if ( (pos.x+size.x) <= p_aabb.pos.x ) return false; - if ( pos.y > (p_aabb.pos.y + p_aabb.size.y) ) + if ( pos.y >= (p_aabb.pos.y + p_aabb.size.y) ) return false; - if ( (pos.y+size.y) < p_aabb.pos.y ) + if ( (pos.y+size.y) <= p_aabb.pos.y ) return false; - if ( pos.z > (p_aabb.pos.z + p_aabb.size.z) ) + if ( pos.z >= (p_aabb.pos.z + p_aabb.size.z) ) return false; - if ( (pos.z+size.z) < p_aabb.pos.z ) + if ( (pos.z+size.z) <= p_aabb.pos.z ) return false; return true; diff --git a/core/math/bezier_curve.cpp b/core/math/bezier_curve.cpp index 2676804945..c9467a77e4 100644 --- a/core/math/bezier_curve.cpp +++ b/core/math/bezier_curve.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/bezier_curve.h b/core/math/bezier_curve.h index 6cc4c730db..de14d68987 100644 --- a/core/math/bezier_curve.h +++ b/core/math/bezier_curve.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp index 7f838b1215..d71b7551d9 100644 --- a/core/math/bsp_tree.cpp +++ b/core/math/bsp_tree.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/bsp_tree.h b/core/math/bsp_tree.h index 03bfd947cb..b980d9590b 100644 --- a/core/math/bsp_tree.h +++ b/core/math/bsp_tree.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp index 52d77b6ebc..f1afa33a4b 100644 --- a/core/math/camera_matrix.cpp +++ b/core/math/camera_matrix.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -67,9 +67,10 @@ Plane CameraMatrix::xform4(const Plane& p_vec4) { 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); + if (p_flip_fov) { + p_fovy_degrees=get_fovy(p_fovy_degrees,1.0/p_aspect); + } float sine, cotangent, deltaZ; float radians = p_fovy_degrees / 2.0 * Math_PI / 180.0; @@ -110,7 +111,7 @@ void CameraMatrix::set_orthogonal(float p_left, float p_right, float p_bottom, f void CameraMatrix::set_orthogonal(float p_size, float p_aspect, float p_znear, float p_zfar,bool p_flip_fov) { - if (p_flip_fov) { + if (!p_flip_fov) { p_size*=p_aspect; } @@ -120,7 +121,7 @@ void CameraMatrix::set_orthogonal(float p_size, float p_aspect, float p_znear, f void CameraMatrix::set_frustum(float p_left, float p_right, float p_bottom, float p_top, float p_near, float p_far) { - +#if 0 ///@TODO, give a check to this. I'm not sure if it's working. set_identity(); @@ -132,10 +133,27 @@ void CameraMatrix::set_frustum(float p_left, float p_right, float p_bottom, floa matrix[2][3]=-(2*p_far*p_near) / (p_far-p_near); matrix[3][2]=-1; matrix[3][3]=0; +#else + float *te = &matrix[0][0]; + float x = 2 * p_near / ( p_right - p_left ); + float y = 2 * p_near / ( p_top - p_bottom ); + + float a = ( p_right + p_left ) / ( p_right - p_left ); + float b = ( p_top + p_bottom ) / ( p_top - p_bottom ); + float c = - ( p_far + p_near ) / ( p_far - p_near ); + float d = - 2 * p_far * p_near / ( p_far - p_near ); + + te[0] = x; te[4] = 0; te[8] = a; te[12] = 0; + te[1] = 0; te[5] = y; te[9] = b; te[13] = 0; + te[2] = 0; te[6] = 0; te[10] = c; te[14] = d; + te[3] = 0; te[7] = 0; te[11] = - 1; te[15] = 0; + +#endif } + float CameraMatrix::get_z_far() const { const float * matrix = (const float*)this->matrix; diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h index 6ffcb0ed0b..52b84f4870 100644 --- a/core/math/camera_matrix.h +++ b/core/math/camera_matrix.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -60,7 +60,7 @@ struct CameraMatrix { static float get_fovy(float p_fovx,float p_aspect) { - return Math::atan(p_aspect * Math::tan(p_fovx * 0.5))*2.0; + return Math::rad2deg(Math::atan(p_aspect * Math::tan(Math::deg2rad(p_fovx) * 0.5))*2.0); } float get_z_far() const; diff --git a/core/math/face3.cpp b/core/math/face3.cpp index 814f2d675d..354372df74 100644 --- a/core/math/face3.cpp +++ b/core/math/face3.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -356,4 +356,100 @@ void Face3::get_support(const Vector3& p_normal,const Transform& p_transform,Vec } +Vector3 Face3::get_closest_point_to(const Vector3& p_point) const { + + Vector3 edge0 = vertex[1] - vertex[0]; + Vector3 edge1 = vertex[2] - vertex[0]; + Vector3 v0 = vertex[0] - p_point; + + float a = edge0.dot( edge0 ); + float b = edge0.dot( edge1 ); + float c = edge1.dot( edge1 ); + float d = edge0.dot( v0 ); + float e = edge1.dot( v0 ); + + float det = a*c - b*b; + float s = b*e - c*d; + float t = b*d - a*e; + + if ( s + t < det ) + { + if ( s < 0.f ) + { + if ( t < 0.f ) + { + if ( d < 0.f ) + { + s = CLAMP( -d/a, 0.f, 1.f ); + t = 0.f; + } + else + { + s = 0.f; + t = CLAMP( -e/c, 0.f, 1.f ); + } + } + else + { + s = 0.f; + t = CLAMP( -e/c, 0.f, 1.f ); + } + } + else if ( t < 0.f ) + { + s = CLAMP( -d/a, 0.f, 1.f ); + t = 0.f; + } + else + { + float invDet = 1.f / det; + s *= invDet; + t *= invDet; + } + } + else + { + if ( s < 0.f ) + { + float tmp0 = b+d; + float tmp1 = c+e; + if ( tmp1 > tmp0 ) + { + float numer = tmp1 - tmp0; + float denom = a-2*b+c; + s = CLAMP( numer/denom, 0.f, 1.f ); + t = 1-s; + } + else + { + t = CLAMP( -e/c, 0.f, 1.f ); + s = 0.f; + } + } + else if ( t < 0.f ) + { + if ( a+d > b+e ) + { + float numer = c+e-b-d; + float denom = a-2*b+c; + s = CLAMP( numer/denom, 0.f, 1.f ); + t = 1-s; + } + else + { + s = CLAMP( -e/c, 0.f, 1.f ); + t = 0.f; + } + } + else + { + float numer = c+e-b-d; + float denom = a-2*b+c; + s = CLAMP( numer/denom, 0.f, 1.f ); + t = 1.f - s; + } + } + + return vertex[0] + s * edge0 + t * edge1; +} diff --git a/core/math/face3.h b/core/math/face3.h index 630c408de3..eccbc78122 100644 --- a/core/math/face3.h +++ b/core/math/face3.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -68,6 +68,7 @@ public: real_t get_area() const; Vector3 get_median_point() const; + Vector3 get_closest_point_to(const Vector3& p_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; diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp index cb76b9ed0f..14adde74e7 100644 --- a/core/math/geometry.cpp +++ b/core/math/geometry.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -1004,3 +1004,134 @@ DVector<Plane> Geometry::build_capsule_planes(float p_radius, float p_height, in } + +struct _AtlasWorkRect { + + Size2i s; + Point2i p; + int idx; + _FORCE_INLINE_ bool operator<(const _AtlasWorkRect& p_r) const { return s.width > p_r.s.width; }; +}; + +struct _AtlasWorkRectResult { + + Vector<_AtlasWorkRect> result; + int max_w; + int max_h; +}; + +void Geometry::make_atlas(const Vector<Size2i>& p_rects,Vector<Point2i>& r_result, Size2i& r_size) { + + //super simple, almost brute force scanline stacking fitter + //it's pretty basic for now, but it tries to make sure that the aspect ratio of the + //resulting atlas is somehow square. This is necesary because video cards have limits + //on texture size (usually 2048 or 4096), so the more square a texture, the more chances + //it will work in every hardware. + // for example, it will prioritize a 1024x1024 atlas (works everywhere) instead of a + // 256x8192 atlas (won't work anywhere). + + ERR_FAIL_COND(p_rects.size()==0); + + Vector<_AtlasWorkRect> wrects; + wrects.resize(p_rects.size()); + for(int i=0;i<p_rects.size();i++) { + wrects[i].s=p_rects[i]; + wrects[i].idx=i; + } + wrects.sort(); + int widest = wrects[0].s.width; + + Vector<_AtlasWorkRectResult> results; + + for(int i=0;i<=12;i++) { + + int w = 1<<i; + int max_h=0; + int max_w=0; + if ( w < widest ) + continue; + + Vector<int> hmax; + hmax.resize(w); + for(int j=0;j<w;j++) + hmax[j]=0; + + //place them + int ofs=0; + int limit_h=0; + for(int j=0;j<wrects.size();j++) { + + + if (ofs+wrects[j].s.width > w) { + + ofs=0; + } + + int from_y=0; + for(int k=0;k<wrects[j].s.width;k++) { + + if (hmax[ofs+k] > from_y) + from_y=hmax[ofs+k]; + } + + wrects[j].p.x=ofs; + wrects[j].p.y=from_y; + int end_h = from_y+wrects[j].s.height; + int end_w = ofs+wrects[j].s.width; + if (ofs==0) + limit_h=end_h; + + for(int k=0;k<wrects[j].s.width;k++) { + + hmax[ofs+k]=end_h; + } + + if (end_h > max_h) + max_h=end_h; + + if (end_w > max_w) + max_w=end_w; + + if (ofs==0 || end_h>limit_h ) //while h limit not reched, keep stacking + ofs+=wrects[j].s.width; + + } + + _AtlasWorkRectResult result; + result.result=wrects; + result.max_h=max_h; + result.max_w=max_w; + results.push_back(result); + + } + + //find the result with the best aspect ratio + + int best=-1; + float best_aspect=1e20; + + for(int i=0;i<results.size();i++) { + + float h = nearest_power_of_2(results[i].max_h); + float w = nearest_power_of_2(results[i].max_w); + float aspect = h>w ? h/w : w/h; + if (aspect < best_aspect) { + best=i; + best_aspect=aspect; + } + } + + r_result.resize(p_rects.size()); + + for(int i=0;i<p_rects.size();i++) { + + r_result[ results[best].result[i].idx ]=results[best].result[i].p; + } + + r_size=Size2(results[best].max_w,results[best].max_h ); + +} + + + + diff --git a/core/math/geometry.h b/core/math/geometry.h index 5b21c25bec..b438b41d61 100644 --- a/core/math/geometry.h +++ b/core/math/geometry.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -511,6 +511,20 @@ public: else return p_segment[0]+n*d; // inside } + + static bool is_point_in_triangle(const Vector2& s, const Vector2& a, const Vector2& b, const Vector2& c) + { + int as_x = s.x-a.x; + int as_y = s.y-a.y; + + bool s_ab = (b.x-a.x)*as_y-(b.y-a.y)*as_x > 0; + + if(((c.x-a.x)*as_y-(c.y-a.y)*as_x > 0) == s_ab) return false; + + if(((c.x-b.x)*(s.y-b.y)-(c.y-b.y)*(s.x-b.x) > 0) != s_ab) return false; + + return true; + } static Vector2 get_closest_point_to_segment_uncapped_2d(const Vector2& p_point, const Vector2 *p_segment) { Vector2 p=p_point-p_segment[0]; @@ -821,12 +835,67 @@ public: }; + + _FORCE_INLINE_ static int get_uv84_normal_bit(const Vector3& p_vector) { + + int lat = Math::fast_ftoi(Math::floor(Math::acos(p_vector.dot(Vector3(0,1,0)))*4.0/Math_PI+0.5)); + + if (lat==0) { + return 24; + } else if (lat==4) { + return 25; + } + + int lon = Math::fast_ftoi(Math::floor( (Math_PI+Math::atan2(p_vector.x,p_vector.z))*8.0/(Math_PI*2.0) + 0.5))%8; + + return lon+(lat-1)*8; + } + + _FORCE_INLINE_ static int get_uv84_normal_bit_neighbors(int p_idx) { + + if (p_idx==24) { + return 1|2|4|8; + } else if (p_idx==25) { + return (1<<23)|(1<<22)|(1<<21)|(1<<20); + } else { + + int ret = 0; + if ((p_idx%8) == 0) + ret|=(1<<(p_idx+7)); + else + ret|=(1<<(p_idx-1)); + if ((p_idx%8) == 7) + ret|=(1<<(p_idx-7)); + else + ret|=(1<<(p_idx+1)); + + int mask = ret|(1<<p_idx); + if (p_idx<8) + ret|=24; + else + ret|=mask>>8; + + if (p_idx>=16) + ret|=25; + else + ret|=mask<<8; + + return ret; + } + + } + + + + static MeshData build_convex_mesh(const DVector<Plane> &p_planes); static DVector<Plane> build_sphere_planes(float p_radius, int p_lats, int p_lons, Vector3::Axis p_axis=Vector3::AXIS_Z); static DVector<Plane> build_box_planes(const Vector3& p_extents); static DVector<Plane> build_cylinder_planes(float p_radius, float p_height, int p_sides, Vector3::Axis p_axis=Vector3::AXIS_Z); static DVector<Plane> build_capsule_planes(float p_radius, float p_height, int p_sides, int p_lats, Vector3::Axis p_axis=Vector3::AXIS_Z); - + + static void make_atlas(const Vector<Size2i>& p_rects,Vector<Point2i>& r_result, Size2i& r_size); + }; diff --git a/core/math/math_2d.cpp b/core/math/math_2d.cpp index 6c160abaca..ce03f089e5 100644 --- a/core/math/math_2d.cpp +++ b/core/math/math_2d.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -29,7 +29,7 @@ #include "math_2d.h" -real_t Vector2::atan2() const { +real_t Vector2::angle() const { return Math::atan2(x,y); } @@ -77,6 +77,11 @@ float Vector2::angle_to(const Vector2& p_vector2) const { return Math::atan2( tangent().dot(p_vector2), dot(p_vector2) ); } +float Vector2::angle_to_point(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; @@ -160,7 +165,7 @@ Vector2 Vector2::floor() const { Vector2 Vector2::rotated(float p_by) const { Vector2 v; - v.set_rotation(atan2()+p_by); + v.set_rotation(angle()+p_by); v*=length(); return v; } @@ -225,6 +230,23 @@ Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, co + Vector2 p0=p_pre_a; + Vector2 p1=*this; + Vector2 p2=p_b; + Vector2 p3=p_post_b; + + float t = p_t; + float t2 = t * t; + float t3 = t2 * t; + + Vector2 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; + +/* float mu = p_t; float mu2 = mu*mu; @@ -234,7 +256,7 @@ Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, co Vector2 a3 = *this; return ( a0*mu*mu2 + a1*mu2 + a2*mu + a3 ); - +*/ /* float t = p_t; real_t t2 = t*t; @@ -600,9 +622,39 @@ float Matrix32::basis_determinant() const { } Matrix32 Matrix32::interpolate_with(const Matrix32& p_transform, float p_c) const { + + //extract parameters + Vector2 p1 = get_origin(); + Vector2 p2 = p_transform.get_origin(); + + real_t r1 = get_rotation(); + real_t r2 = p_transform.get_rotation(); + + Vector2 s1 = get_scale(); + Vector2 s2 = p_transform.get_scale(); + + //slerp rotation + Vector2 v1(Math::cos(r1), Math::sin(r1)); + Vector2 v2(Math::cos(r2), Math::sin(r2)); + + real_t dot = v1.dot(v2); + + dot = (dot < -1.0) ? -1.0 : ((dot > 1.0) ? 1.0 : dot); //clamp dot to [-1,1] + + Vector2 v; - - return Matrix32(); + if (dot > 0.9995) { + v = Vector2::linear_interpolate(v1, v2, p_c).normalized(); //linearly interpolate to avoid numerical precision issues + } else { + real_t angle = p_c*Math::acos(dot); + Vector2 v3 = (v2 - v1*dot).normalized(); + v = v1*Math::cos(angle) + v3*Math::sin(angle); + } + + //construct matrix + Matrix32 res(Math::atan2(v.y, v.x), Vector2::linear_interpolate(p1, p2, p_c)); + res.scale_basis(Vector2::linear_interpolate(s1, s2, p_c)); + return res; } Matrix32::operator String() const { diff --git a/core/math/math_2d.h b/core/math/math_2d.h index 3cc5bdc843..3d40e24091 100644 --- a/core/math/math_2d.h +++ b/core/math/math_2d.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -90,7 +90,8 @@ struct Vector2 { float distance_to(const Vector2& p_vector2) const; float distance_squared_to(const Vector2& p_vector2) const; float angle_to(const Vector2& p_vector2) const; - + float angle_to_point(const Vector2& p_vector2) const; + float dot(const Vector2& p_other) const; float cross(const Vector2& p_other) const; Vector2 cross(real_t p_other) const; @@ -132,7 +133,7 @@ struct Vector2 { bool operator<(const Vector2& p_vec2) const { return (x==p_vec2.x)?(y<p_vec2.y):(x<p_vec2.x); } bool operator<=(const Vector2& p_vec2) const { return (x==p_vec2.x)?(y<=p_vec2.y):(x<=p_vec2.x); } - real_t atan2() const; + real_t angle() const; void set_rotation(float p_radians) { @@ -158,8 +159,8 @@ struct Vector2 { operator String() const { return String::num(x)+","+String::num(y); } - inline Vector2(float p_x,float p_y) { x=p_x; y=p_y; } - inline Vector2() { x=0; y=0; } + _FORCE_INLINE_ Vector2(float p_x,float p_y) { x=p_x; y=p_y; } + _FORCE_INLINE_ Vector2() { x=0; y=0; } }; _FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2& p_vec) const { @@ -197,6 +198,8 @@ Vector2 Vector2::linear_interpolate(const Vector2& p_a, const Vector2& p_b,float typedef Vector2 Size2; typedef Vector2 Point2; +struct Matrix32; + struct Rect2 { @@ -211,18 +214,43 @@ struct Rect2 { float get_area() const { return size.width*size.height; } inline bool intersects(const Rect2& p_rect) const { - if ( pos.x > (p_rect.pos.x + p_rect.size.width) ) + if ( pos.x >= (p_rect.pos.x + p_rect.size.width) ) return false; - if ( (pos.x+size.width) < p_rect.pos.x ) + if ( (pos.x+size.width) <= p_rect.pos.x ) return false; - if ( pos.y > (p_rect.pos.y + p_rect.size.height) ) + if ( pos.y >= (p_rect.pos.y + p_rect.size.height) ) return false; - if ( (pos.y+size.height) < p_rect.pos.y ) + if ( (pos.y+size.height) <= p_rect.pos.y ) return false; return true; } + inline float distance_to(const Vector2& p_point) const { + + float dist = 1e20; + + if (p_point.x < pos.x) { + dist=MIN(dist,pos.x-p_point.x); + } + if (p_point.y < pos.y) { + dist=MIN(dist,pos.y-p_point.y); + } + if (p_point.x >= (pos.x+size.x) ) { + dist=MIN(p_point.x-(pos.x+size.x),dist); + } + if (p_point.y >= (pos.y+size.y) ) { + dist=MIN(p_point.y-(pos.y+size.y),dist); + } + + if (dist==1e20) + return 0; + else + return dist; + } + + _FORCE_INLINE_ bool intersects_transformed(const Matrix32& p_xform, const Rect2& p_rect) const; + bool intersects_segment(const Point2& p_from, const Point2& p_to, Point2* r_pos=NULL, Point2* r_normal=NULL) const; inline bool encloses(const Rect2& p_rect) const { @@ -596,6 +624,160 @@ struct Matrix32 { }; +bool Rect2::intersects_transformed(const Matrix32& p_xform, const Rect2& p_rect) const { + + //SAT intersection between local and transformed rect2 + + Vector2 xf_points[4]={ + p_xform.xform(p_rect.pos), + p_xform.xform(Vector2(p_rect.pos.x+p_rect.size.x,p_rect.pos.y)), + p_xform.xform(Vector2(p_rect.pos.x,p_rect.pos.y+p_rect.size.y)), + p_xform.xform(Vector2(p_rect.pos.x+p_rect.size.x,p_rect.pos.y+p_rect.size.y)), + }; + + real_t low_limit; + + //base rect2 first (faster) + + if (xf_points[0].y>pos.y) + goto next1; + if (xf_points[1].y>pos.y) + goto next1; + if (xf_points[2].y>pos.y) + goto next1; + if (xf_points[3].y>pos.y) + goto next1; + + return false; + + next1: + + low_limit=pos.y+size.y; + + if (xf_points[0].y<low_limit) + goto next2; + if (xf_points[1].y<low_limit) + goto next2; + if (xf_points[2].y<low_limit) + goto next2; + if (xf_points[3].y<low_limit) + goto next2; + + return false; + + next2: + + if (xf_points[0].x>pos.x) + goto next3; + if (xf_points[1].x>pos.x) + goto next3; + if (xf_points[2].x>pos.x) + goto next3; + if (xf_points[3].x>pos.x) + goto next3; + + return false; + + next3: + + low_limit=pos.x+size.x; + + if (xf_points[0].x<low_limit) + goto next4; + if (xf_points[1].x<low_limit) + goto next4; + if (xf_points[2].x<low_limit) + goto next4; + if (xf_points[3].x<low_limit) + goto next4; + + return false; + + next4: + + Vector2 xf_points2[4]={ + pos, + Vector2(pos.x+size.x,pos.y), + Vector2(pos.x,pos.y+size.y), + Vector2(pos.x+size.x,pos.y+size.y), + }; + + real_t maxa=p_xform.elements[0].dot(xf_points2[0]); + real_t mina=maxa; + + real_t dp = p_xform.elements[0].dot(xf_points2[1]); + maxa=MAX(dp,maxa); + mina=MIN(dp,mina); + + dp = p_xform.elements[0].dot(xf_points2[2]); + maxa=MAX(dp,maxa); + mina=MIN(dp,mina); + + dp = p_xform.elements[0].dot(xf_points2[3]); + maxa=MAX(dp,maxa); + mina=MIN(dp,mina); + + real_t maxb=p_xform.elements[0].dot(xf_points[0]); + real_t minb=maxb; + + dp = p_xform.elements[0].dot(xf_points[1]); + maxb=MAX(dp,maxb); + minb=MIN(dp,minb); + + dp = p_xform.elements[0].dot(xf_points[2]); + maxb=MAX(dp,maxb); + minb=MIN(dp,minb); + + dp = p_xform.elements[0].dot(xf_points[3]); + maxb=MAX(dp,maxb); + minb=MIN(dp,minb); + + + if ( mina > maxb ) + return false; + if ( minb > maxa ) + return false; + + maxa=p_xform.elements[1].dot(xf_points2[0]); + mina=maxa; + + dp = p_xform.elements[1].dot(xf_points2[1]); + maxa=MAX(dp,maxa); + mina=MIN(dp,mina); + + dp = p_xform.elements[1].dot(xf_points2[2]); + maxa=MAX(dp,maxa); + mina=MIN(dp,mina); + + dp = p_xform.elements[1].dot(xf_points2[3]); + maxa=MAX(dp,maxa); + mina=MIN(dp,mina); + + maxb=p_xform.elements[1].dot(xf_points[0]); + minb=maxb; + + dp = p_xform.elements[1].dot(xf_points[1]); + maxb=MAX(dp,maxb); + minb=MIN(dp,minb); + + dp = p_xform.elements[1].dot(xf_points[2]); + maxb=MAX(dp,maxb); + minb=MIN(dp,minb); + + dp = p_xform.elements[1].dot(xf_points[3]); + maxb=MAX(dp,maxb); + minb=MIN(dp,minb); + + + if ( mina > maxb ) + return false; + if ( minb > maxa ) + return false; + + + return true; + +} Vector2 Matrix32::basis_xform(const Vector2& v) const { diff --git a/core/math/math_defs.cpp b/core/math/math_defs.cpp index ca43bc7ae3..51aaa6800e 100644 --- a/core/math/math_defs.cpp +++ b/core/math/math_defs.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -26,5 +26,5 @@ /* 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"
-
+#include "math_defs.h" + diff --git a/core/math/math_defs.h b/core/math/math_defs.h index dd0390240a..7cb6c7f499 100644 --- a/core/math/math_defs.h +++ b/core/math/math_defs.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -26,35 +26,35 @@ /* 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
+#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 index 92236a374f..3c94ac5bc7 100644 --- a/core/math/math_funcs.cpp +++ b/core/math/math_funcs.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -36,8 +36,9 @@ uint32_t Math::default_seed=1; #define PHI 0x9e3779b9 -static uint32_t Q[4096], c = 362436; - +#if 0 +static uint32_t Q[4096]; +#endif uint32_t Math::rand_from_seed(uint32_t *seed) { @@ -48,8 +49,8 @@ uint32_t Math::rand_from_seed(uint32_t *seed) { s = 0x12345987; k = s / 127773; s = 16807 * (s - k * 127773) - 2836 * k; - if (s < 0) - s += 2147483647; +// if (s < 0) +// s += 2147483647; (*seed) = s; return (s & Math::RANDOM_MAX); #else @@ -76,7 +77,7 @@ void Math::seed(uint32_t x) { void Math::randomize() { OS::Time time = OS::get_singleton()->get_time(); - seed(OS::get_singleton()->get_ticks_usec()*time.hour*time.min*time.sec*rand()); /* *OS::get_singleton()->get_time().sec); // windows doesn't have get_time(), returns always 0 */ + seed(OS::get_singleton()->get_ticks_usec()*(time.hour+1)*(time.min+1)*(time.sec+1)*rand()); /* *OS::get_singleton()->get_time().sec); // windows doesn't have get_time(), returns always 0 */ } uint32_t Math::rand() { @@ -269,7 +270,7 @@ bool Math::is_inf(double p_val) { uint32_t Math::larger_prime(uint32_t p_val) { - static const int primes[] = { + static const uint32_t primes[] = { 5, 13, 23, diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index c98a088912..ec089ebc8b 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -79,9 +79,9 @@ public: return Math::log( p_linear ) * 8.6858896380650365530225783783321; } - static inline double db2linear(double p_linear) { + static inline double db2linear(double p_db) { - return Math::exp( p_linear * 0.11512925464970228420089957273422 ); + return Math::exp( p_db * 0.11512925464970228420089957273422 ); } static bool is_nan(double p_val); @@ -136,7 +136,10 @@ public: static int b; -#if defined(_MSC_VER) && _MSC_VER < 1800 +#if (defined(_WIN32_WINNT) && _WIN32_WINNT >= 0x0603) || WINAPI_FAMILY == WINAPI_FAMILY_PHONE_APP // windows 8 phone? + b = (int)((a>0.0f) ? (a + 0.5f):(a -0.5f)); + +#elif defined(_MSC_VER) && _MSC_VER < 1800 __asm fld a __asm fistp b /*#elif defined( __GNUC__ ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) @@ -147,6 +150,7 @@ public: "fistpl %0 \n\t" : "=m" (b) : "m" (a));*/ + #else b=lrintf(a); //assuming everything but msvc 2012 or earlier has lrint #endif diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp index ff62e7786b..10f1461fdc 100644 --- a/core/math/matrix3.cpp +++ b/core/math/matrix3.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/matrix3.h b/core/math/matrix3.h index f68703ca23..98feb2dbbd 100644 --- a/core/math/matrix3.h +++ b/core/math/matrix3.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/octree.h b/core/math/octree.h index b64ba381c0..84de388178 100644 --- a/core/math/octree.h +++ b/core/math/octree.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/plane.cpp b/core/math/plane.cpp index 88c7be5f63..f9395a002a 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/plane.h b/core/math/plane.h index 608ec26926..604b880266 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/quat.cpp b/core/math/quat.cpp index 950a4756ad..e0c4b0793c 100644 --- a/core/math/quat.cpp +++ b/core/math/quat.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/quat.h b/core/math/quat.h index d326073033..f161e35074 100644 --- a/core/math/quat.h +++ b/core/math/quat.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -64,6 +64,22 @@ public: Quat operator*(const Quat& q) const; + + Quat operator*(const Vector3& v) const + { + return Quat( w * v.x + y * v.z - z * v.y, + w * v.y + z * v.x - x * v.z, + w * v.z + x * v.y - y * v.x, + -x * v.x - y * v.y - z * v.z); + } + + _FORCE_INLINE_ Vector3 xform(const Vector3& v) const { + + Quat q = *this * v; + q *= this->inverse(); + return Vector3(q.x,q.y,q.z); + } + _FORCE_INLINE_ void operator+=(const Quat& q); _FORCE_INLINE_ void operator-=(const Quat& q); _FORCE_INLINE_ void operator*=(const real_t& s); @@ -87,6 +103,29 @@ public: x=p_x; y=p_y; z=p_z; w=p_w; } Quat(const Vector3& axis, const real_t& angle); + + Quat(const Vector3& v0, const Vector3& v1) // shortest arc + { + Vector3 c = v0.cross(v1); + real_t d = v0.dot(v1); + + if (d < -1.0 + CMP_EPSILON) { + x=0; + y=1; + z=0; + w=0; + } else { + + real_t s = Math::sqrt((1.0f + d) * 2.0f); + real_t rs = 1.0f / s; + + x=c.x*rs; + y=c.y*rs; + z=c.z*rs; + w=s * 0.5; + } + } + inline Quat() {x=y=z=0; w=1; } diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index ed30de6915..80ae0f04e1 100644 --- a/core/math/quick_hull.cpp +++ b/core/math/quick_hull.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -63,7 +63,7 @@ Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_me Vector3 sp = p_points[i].snapped(0.0001); if (valid_cache.has(sp)) { valid_points[i]=false; - print_line("INVALIDATED: "+itos(i)); + //print_line("INVALIDATED: "+itos(i)); }else { valid_points[i]=true; valid_cache.insert(sp); @@ -428,6 +428,7 @@ Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_me List<Geometry::MeshData::Face>::Element *O = F->get().left == E ? F->get().right : F->get().left; ERR_CONTINUE(O==E); + ERR_CONTINUE(O==NULL); if (O->get().plane.is_almost_like(f.plane)) { //merge and delete edge and contiguous face, while repointing edges (uuugh!) diff --git a/core/math/quick_hull.h b/core/math/quick_hull.h index d7f056d366..cb486a0b6f 100644 --- a/core/math/quick_hull.h +++ b/core/math/quick_hull.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/transform.cpp b/core/math/transform.cpp index bb874facbd..a6f4f626cc 100644 --- a/core/math/transform.cpp +++ b/core/math/transform.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/transform.h b/core/math/transform.h index b1a0ea1ab8..a992843d70 100644 --- a/core/math/transform.h +++ b/core/math/transform.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp index 111ceca185..70cb639fc2 100644 --- a/core/math/triangle_mesh.cpp +++ b/core/math/triangle_mesh.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -26,543 +26,543 @@ /* 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;i<p_size;i++) {
-
- aabb.merge_with(p_bb[p_from+i]->aabb);
- }
-
- int li=aabb.get_longest_axis_index();
-
- switch(li) {
-
- case Vector3::AXIS_X: {
- SortArray<BVH*,BVHCmpX> 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<BVH*,BVHCmpY> 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<BVH*,BVHCmpZ> 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<Vector3>& 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<BVH>::Write bw = bvh.write();
-
- {
-
- //create faces and indices and base bvh
- //except for the Set for repeated triangles, everything
- //goes in-place.
-
- DVector<Vector3>::Read r = p_faces.read();
- DVector<Triangle>::Write w = triangles.write();
- Map<Vector3,int> db;
-
- for(int i=0;i<fc;i++) {
-
- Triangle&f=w[i];
- const Vector3 *v=&r[i*3];
-
- for(int j=0;j<3;j++) {
-
- int vidx=-1;
- Vector3 vs=v[j].snapped(0.0001);
- Map<Vector3,int>::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<Vector3>::Write vw = vertices.write();
- for (Map<Vector3,int>::Element *E=db.front();E;E=E->next()) {
- vw[E->get()]=E->key();
- }
-
- }
-
- DVector<BVH*> bwptrs;
- bwptrs.resize(fc);
- DVector<BVH*>::Write bwp = bwptrs.write();
- for(int i=0;i<fc;i++) {
-
- bwp[i]=&bw[i];
- }
-
- max_depth=0;
- int max_alloc=fc;
- int max=_create_bvh(bw.ptr(),bwp.ptr(),0,fc,1,max_depth,max_alloc);
-
- bw=DVector<BVH>::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<<VISITED_BIT_SHIFT)-1,
- VISITED_BIT_MASK=~NODE_IDX_MASK,
-
-
- };
-
- int n_count=0;
- Vector3 n;
-
- //for(int i=0;i<max_depth;i++)
- // stack[i]=0;
-
- int level=0;
-
- DVector<Triangle>::Read trianglesr = triangles.read();
- DVector<Vector3>::Read verticesr=vertices.read();
- DVector<BVH>::Read bvhr=bvh.read();
-
- const Triangle *triangleptr=trianglesr.ptr();
- const Vector3 *vertexptr=verticesr.ptr();
- int pos=bvh.size()-1;
- const BVH *bvhptr = bvhr.ptr();
-
- stack[0]=pos;
- while(true) {
-
- uint32_t node = stack[level]&NODE_IDX_MASK;
- const BVH &b = bvhptr[ node ];
- bool done=false;
-
- switch(stack[level]>>VISITED_BIT_SHIFT) {
- case TEST_AABB_BIT: {
-
-
- bool valid = b.aabb.intersects(p_aabb);
- if (!valid) {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- if (b.face_index>=0) {
-
- const Triangle &s=triangleptr[ b.face_index ];
- n+=s.normal;
- n_count++;
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
- }
- }
-
- } continue;
- case VISIT_LEFT_BIT: {
-
- stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.left|TEST_AABB_BIT;
- level++;
-
- } continue;
- case VISIT_RIGHT_BIT: {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.right|TEST_AABB_BIT;
- level++;
- } continue;
- case VISIT_DONE_BIT: {
-
- if (level==0) {
- done=true;
- break;
- } else
- level--;
-
- } continue;
- }
-
-
- if (done)
- break;
- }
-
-
- if (n_count>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<<VISITED_BIT_SHIFT)-1,
- VISITED_BIT_MASK=~NODE_IDX_MASK,
-
-
- };
-
- Vector3 n = (p_end-p_begin).normalized();
- real_t d=1e10;
- bool inters=false;
-
- //for(int i=0;i<max_depth;i++)
- // stack[i]=0;
-
- int level=0;
- //AABB ray_aabb;
- //ray_aabb.pos=p_begin;
- //ray_aabb.expand_to(p_end);
-
-
- DVector<Triangle>::Read trianglesr = triangles.read();
- DVector<Vector3>::Read verticesr=vertices.read();
- DVector<BVH>::Read bvhr=bvh.read();
-
- const Triangle *triangleptr=trianglesr.ptr();
- const Vector3 *vertexptr=verticesr.ptr();
- int pos=bvh.size()-1;
- const BVH *bvhptr = bvhr.ptr();
-
- stack[0]=pos;
- while(true) {
-
- uint32_t node = stack[level]&NODE_IDX_MASK;
- const BVH &b = bvhptr[ node ];
- bool done=false;
-
- switch(stack[level]>>VISITED_BIT_SHIFT) {
- case TEST_AABB_BIT: {
-
-
- bool valid = b.aabb.intersects_segment(p_begin,p_end);
-// bool valid = b.aabb.intersects(ray_aabb);
-
- if (!valid) {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- if (b.face_index>=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 (nd<d) {
-
- d=nd;
- r_point=res;
- r_normal=f3.get_plane().get_normal();
- inters=true;
- }
-
- }
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
- }
- }
-
- } continue;
- case VISIT_LEFT_BIT: {
-
- stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.left|TEST_AABB_BIT;
- level++;
-
- } continue;
- case VISIT_RIGHT_BIT: {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.right|TEST_AABB_BIT;
- level++;
- } continue;
- case VISIT_DONE_BIT: {
-
- if (level==0) {
- done=true;
- break;
- } else
- level--;
-
- } continue;
- }
-
-
- if (done)
- break;
- }
-
-
- if (inters) {
-
- if (n.dot(r_normal)>0)
- 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<<VISITED_BIT_SHIFT)-1,
- VISITED_BIT_MASK=~NODE_IDX_MASK,
-
-
- };
-
- Vector3 n = p_dir;
- real_t d=1e20;
- bool inters=false;
-
- //for(int i=0;i<max_depth;i++)
- // stack[i]=0;
-
- int level=0;
-
- DVector<Triangle>::Read trianglesr = triangles.read();
- DVector<Vector3>::Read verticesr=vertices.read();
- DVector<BVH>::Read bvhr=bvh.read();
-
- const Triangle *triangleptr=trianglesr.ptr();
- const Vector3 *vertexptr=verticesr.ptr();
- int pos=bvh.size()-1;
- const BVH *bvhptr = bvhr.ptr();
-
- stack[0]=pos;
- while(true) {
-
- uint32_t node = stack[level]&NODE_IDX_MASK;
- const BVH &b = bvhptr[ node ];
- bool done=false;
-
- switch(stack[level]>>VISITED_BIT_SHIFT) {
- case TEST_AABB_BIT: {
-
-
- bool valid = b.aabb.intersects_ray(p_begin,p_dir);
- if (!valid) {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- if (b.face_index>=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 (nd<d) {
-
- d=nd;
- r_point=res;
- r_normal=f3.get_plane().get_normal();
- inters=true;
- }
-
- }
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
- } else {
-
- stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
- }
- }
-
- } continue;
- case VISIT_LEFT_BIT: {
-
- stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.left|TEST_AABB_BIT;
- level++;
-
- } continue;
- case VISIT_RIGHT_BIT: {
-
- stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
- stack[level+1]=b.right|TEST_AABB_BIT;
- level++;
- } continue;
- case VISIT_DONE_BIT: {
-
- if (level==0) {
- done=true;
- break;
- } else
- level--;
-
- } continue;
- }
-
-
- if (done)
- break;
- }
-
-
- if (inters) {
-
- if (n.dot(r_normal)>0)
- r_normal=-r_normal;
- }
-
- return inters;
-}
-
-bool TriangleMesh::is_valid() const {
-
- return valid;
-}
-
-DVector<Face3> TriangleMesh::get_faces() const {
-
- if (!valid)
- return DVector<Face3>();
-
- DVector<Face3> faces;
- int ts = triangles.size();
- faces.resize(triangles.size());
-
- DVector<Face3>::Write w=faces.write();
- DVector<Triangle>::Read r = triangles.read();
- DVector<Vector3>::Read rv = vertices.read();
-
- for(int i=0;i<ts;i++) {
- for(int j=0;j<3;j++) {
- w[i].vertex[j]=rv[r[i].indices[j]];
- }
- }
-
- w = DVector<Face3>::Write();
- return faces;
-}
-
-TriangleMesh::TriangleMesh() {
-
- valid=false;
- max_depth=0;
-}
+#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;i<p_size;i++) { + + aabb.merge_with(p_bb[p_from+i]->aabb); + } + + int li=aabb.get_longest_axis_index(); + + switch(li) { + + case Vector3::AXIS_X: { + SortArray<BVH*,BVHCmpX> 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<BVH*,BVHCmpY> 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<BVH*,BVHCmpZ> 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<Vector3>& 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<BVH>::Write bw = bvh.write(); + + { + + //create faces and indices and base bvh + //except for the Set for repeated triangles, everything + //goes in-place. + + DVector<Vector3>::Read r = p_faces.read(); + DVector<Triangle>::Write w = triangles.write(); + Map<Vector3,int> db; + + for(int i=0;i<fc;i++) { + + Triangle&f=w[i]; + const Vector3 *v=&r[i*3]; + + for(int j=0;j<3;j++) { + + int vidx=-1; + Vector3 vs=v[j].snapped(0.0001); + Map<Vector3,int>::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<Vector3>::Write vw = vertices.write(); + for (Map<Vector3,int>::Element *E=db.front();E;E=E->next()) { + vw[E->get()]=E->key(); + } + + } + + DVector<BVH*> bwptrs; + bwptrs.resize(fc); + DVector<BVH*>::Write bwp = bwptrs.write(); + for(int i=0;i<fc;i++) { + + bwp[i]=&bw[i]; + } + + max_depth=0; + int max_alloc=fc; + int max=_create_bvh(bw.ptr(),bwp.ptr(),0,fc,1,max_depth,max_alloc); + + bw=DVector<BVH>::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<<VISITED_BIT_SHIFT)-1, + VISITED_BIT_MASK=~NODE_IDX_MASK, + + + }; + + int n_count=0; + Vector3 n; + + //for(int i=0;i<max_depth;i++) + // stack[i]=0; + + int level=0; + + DVector<Triangle>::Read trianglesr = triangles.read(); + DVector<Vector3>::Read verticesr=vertices.read(); + DVector<BVH>::Read bvhr=bvh.read(); + + const Triangle *triangleptr=trianglesr.ptr(); + const Vector3 *vertexptr=verticesr.ptr(); + int pos=bvh.size()-1; + const BVH *bvhptr = bvhr.ptr(); + + stack[0]=pos; + while(true) { + + uint32_t node = stack[level]&NODE_IDX_MASK; + const BVH &b = bvhptr[ node ]; + bool done=false; + + switch(stack[level]>>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = b.aabb.intersects(p_aabb); + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + if (b.face_index>=0) { + + const Triangle &s=triangleptr[ b.face_index ]; + n+=s.normal; + n_count++; + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + } + } + + } continue; + case VISIT_LEFT_BIT: { + + stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.left|TEST_AABB_BIT; + level++; + + } continue; + case VISIT_RIGHT_BIT: { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.right|TEST_AABB_BIT; + level++; + } continue; + case VISIT_DONE_BIT: { + + if (level==0) { + done=true; + break; + } else + level--; + + } continue; + } + + + if (done) + break; + } + + + if (n_count>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<<VISITED_BIT_SHIFT)-1, + VISITED_BIT_MASK=~NODE_IDX_MASK, + + + }; + + Vector3 n = (p_end-p_begin).normalized(); + real_t d=1e10; + bool inters=false; + + //for(int i=0;i<max_depth;i++) + // stack[i]=0; + + int level=0; + //AABB ray_aabb; + //ray_aabb.pos=p_begin; + //ray_aabb.expand_to(p_end); + + + DVector<Triangle>::Read trianglesr = triangles.read(); + DVector<Vector3>::Read verticesr=vertices.read(); + DVector<BVH>::Read bvhr=bvh.read(); + + const Triangle *triangleptr=trianglesr.ptr(); + const Vector3 *vertexptr=verticesr.ptr(); + int pos=bvh.size()-1; + const BVH *bvhptr = bvhr.ptr(); + + stack[0]=pos; + while(true) { + + uint32_t node = stack[level]&NODE_IDX_MASK; + const BVH &b = bvhptr[ node ]; + bool done=false; + + switch(stack[level]>>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = b.aabb.intersects_segment(p_begin,p_end); +// bool valid = b.aabb.intersects(ray_aabb); + + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + if (b.face_index>=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 (nd<d) { + + d=nd; + r_point=res; + r_normal=f3.get_plane().get_normal(); + inters=true; + } + + } + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + } + } + + } continue; + case VISIT_LEFT_BIT: { + + stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.left|TEST_AABB_BIT; + level++; + + } continue; + case VISIT_RIGHT_BIT: { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.right|TEST_AABB_BIT; + level++; + } continue; + case VISIT_DONE_BIT: { + + if (level==0) { + done=true; + break; + } else + level--; + + } continue; + } + + + if (done) + break; + } + + + if (inters) { + + if (n.dot(r_normal)>0) + 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<<VISITED_BIT_SHIFT)-1, + VISITED_BIT_MASK=~NODE_IDX_MASK, + + + }; + + Vector3 n = p_dir; + real_t d=1e20; + bool inters=false; + + //for(int i=0;i<max_depth;i++) + // stack[i]=0; + + int level=0; + + DVector<Triangle>::Read trianglesr = triangles.read(); + DVector<Vector3>::Read verticesr=vertices.read(); + DVector<BVH>::Read bvhr=bvh.read(); + + const Triangle *triangleptr=trianglesr.ptr(); + const Vector3 *vertexptr=verticesr.ptr(); + int pos=bvh.size()-1; + const BVH *bvhptr = bvhr.ptr(); + + stack[0]=pos; + while(true) { + + uint32_t node = stack[level]&NODE_IDX_MASK; + const BVH &b = bvhptr[ node ]; + bool done=false; + + switch(stack[level]>>VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + + + bool valid = b.aabb.intersects_ray(p_begin,p_dir); + if (!valid) { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + if (b.face_index>=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 (nd<d) { + + d=nd; + r_point=res; + r_normal=f3.get_plane().get_normal(); + inters=true; + } + + } + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + + } else { + + stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node; + } + } + + } continue; + case VISIT_LEFT_BIT: { + + stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.left|TEST_AABB_BIT; + level++; + + } continue; + case VISIT_RIGHT_BIT: { + + stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node; + stack[level+1]=b.right|TEST_AABB_BIT; + level++; + } continue; + case VISIT_DONE_BIT: { + + if (level==0) { + done=true; + break; + } else + level--; + + } continue; + } + + + if (done) + break; + } + + + if (inters) { + + if (n.dot(r_normal)>0) + r_normal=-r_normal; + } + + return inters; +} + +bool TriangleMesh::is_valid() const { + + return valid; +} + +DVector<Face3> TriangleMesh::get_faces() const { + + if (!valid) + return DVector<Face3>(); + + DVector<Face3> faces; + int ts = triangles.size(); + faces.resize(triangles.size()); + + DVector<Face3>::Write w=faces.write(); + DVector<Triangle>::Read r = triangles.read(); + DVector<Vector3>::Read rv = vertices.read(); + + for(int i=0;i<ts;i++) { + for(int j=0;j<3;j++) { + w[i].vertex[j]=rv[r[i].indices[j]]; + } + } + + w = DVector<Face3>::Write(); + return faces; +} + +TriangleMesh::TriangleMesh() { + + valid=false; + max_depth=0; +} diff --git a/core/math/triangle_mesh.h b/core/math/triangle_mesh.h index ab0cb713b1..87d8ce8e6c 100644 --- a/core/math/triangle_mesh.h +++ b/core/math/triangle_mesh.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -26,74 +26,74 @@ /* 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<Triangle> triangles;
- DVector<Vector3> 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> 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<Face3> get_faces() const;
-
-
- void create(const DVector<Vector3>& p_faces);
- TriangleMesh();
-};
-
-#endif // TRIANGLE_MESH_H
+#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<Triangle> triangles; + DVector<Vector3> 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> 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<Face3> get_faces() const; + + + void create(const DVector<Vector3>& p_faces); + TriangleMesh(); +}; + +#endif // TRIANGLE_MESH_H diff --git a/core/math/triangulate.cpp b/core/math/triangulate.cpp index 4a870def4b..b13e13c47d 100644 --- a/core/math/triangulate.cpp +++ b/core/math/triangulate.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/triangulate.h b/core/math/triangulate.h index b20b37bd44..927b7efb8d 100644 --- a/core/math/triangulate.h +++ b/core/math/triangulate.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ diff --git a/core/math/triangulator.cpp b/core/math/triangulator.cpp new file mode 100644 index 0000000000..8f82d76823 --- /dev/null +++ b/core/math/triangulator.cpp @@ -0,0 +1,1550 @@ +//Copyright (C) 2011 by Ivan Fratric +// +//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 <stdio.h> +#include <string.h> +#include <math.h> + +#include "triangulator.h" + + +#define TRIANGULATOR_VERTEXTYPE_REGULAR 0 +#define TRIANGULATOR_VERTEXTYPE_START 1 +#define TRIANGULATOR_VERTEXTYPE_END 2 +#define TRIANGULATOR_VERTEXTYPE_SPLIT 3 +#define TRIANGULATOR_VERTEXTYPE_MERGE 4 + +TriangulatorPoly::TriangulatorPoly() { + hole = false; + numpoints = 0; + points = NULL; +} + +TriangulatorPoly::~TriangulatorPoly() { + if(points) delete [] points; +} + +void TriangulatorPoly::Clear() { + if(points) delete [] points; + hole = false; + numpoints = 0; + points = NULL; +} + +void TriangulatorPoly::Init(long numpoints) { + Clear(); + this->numpoints = numpoints; + points = new Vector2[numpoints]; +} + +void TriangulatorPoly::Triangle(Vector2 &p1, Vector2 &p2, Vector2 &p3) { + Init(3); + points[0] = p1; + points[1] = p2; + points[2] = p3; +} + +TriangulatorPoly::TriangulatorPoly(const TriangulatorPoly &src) { + hole = src.hole; + numpoints = src.numpoints; + points = new Vector2[numpoints]; + memcpy(points, src.points, numpoints*sizeof(Vector2)); +} + +TriangulatorPoly& TriangulatorPoly::operator=(const TriangulatorPoly &src) { + Clear(); + hole = src.hole; + numpoints = src.numpoints; + points = new Vector2[numpoints]; + memcpy(points, src.points, numpoints*sizeof(Vector2)); + return *this; +} + +int TriangulatorPoly::GetOrientation() { + long i1,i2; + real_t area = 0; + for(i1=0; i1<numpoints; i1++) { + i2 = i1+1; + if(i2 == numpoints) i2 = 0; + area += points[i1].x * points[i2].y - points[i1].y * points[i2].x; + } + if(area>0) return TRIANGULATOR_CCW; + if(area<0) return TRIANGULATOR_CW; + return 0; +} + +void TriangulatorPoly::SetOrientation(int orientation) { + int polyorientation = GetOrientation(); + if(polyorientation&&(polyorientation!=orientation)) { + Invert(); + } +} + +void TriangulatorPoly::Invert() { + long i; + Vector2 *invpoints; + + invpoints = new Vector2[numpoints]; + for(i=0;i<numpoints;i++) { + invpoints[i] = points[numpoints-i-1]; + } + + delete [] points; + points = invpoints; +} + +Vector2 TriangulatorPartition::Normalize(const Vector2 &p) { + Vector2 r; + real_t n = sqrt(p.x*p.x + p.y*p.y); + if(n!=0) { + r = p/n; + } else { + r.x = 0; + r.y = 0; + } + return r; +} + +real_t TriangulatorPartition::Distance(const Vector2 &p1, const Vector2 &p2) { + real_t dx,dy; + dx = p2.x - p1.x; + dy = p2.y - p1.y; + return(sqrt(dx*dx + dy*dy)); +} + +//checks if two lines intersect +int TriangulatorPartition::Intersects(Vector2 &p11, Vector2 &p12, Vector2 &p21, Vector2 &p22) { + if((p11.x == p21.x)&&(p11.y == p21.y)) return 0; + if((p11.x == p22.x)&&(p11.y == p22.y)) return 0; + if((p12.x == p21.x)&&(p12.y == p21.y)) return 0; + if((p12.x == p22.x)&&(p12.y == p22.y)) return 0; + + Vector2 v1ort,v2ort,v; + real_t dot11,dot12,dot21,dot22; + + v1ort.x = p12.y-p11.y; + v1ort.y = p11.x-p12.x; + + v2ort.x = p22.y-p21.y; + v2ort.y = p21.x-p22.x; + + v = p21-p11; + dot21 = v.x*v1ort.x + v.y*v1ort.y; + v = p22-p11; + dot22 = v.x*v1ort.x + v.y*v1ort.y; + + v = p11-p21; + dot11 = v.x*v2ort.x + v.y*v2ort.y; + v = p12-p21; + dot12 = v.x*v2ort.x + v.y*v2ort.y; + + if(dot11*dot12>0) return 0; + if(dot21*dot22>0) return 0; + + return 1; +} + +//removes holes from inpolys by merging them with non-holes +int TriangulatorPartition::RemoveHoles(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *outpolys) { + List<TriangulatorPoly> polys; + List<TriangulatorPoly>::Element *holeiter,*polyiter,*iter,*iter2; + long i,i2,holepointindex,polypointindex; + Vector2 holepoint,polypoint,bestpolypoint; + Vector2 linep1,linep2; + Vector2 v1,v2; + TriangulatorPoly newpoly; + bool hasholes; + bool pointvisible; + bool pointfound; + + //check for trivial case (no holes) + hasholes = false; + for(iter = inpolys->front(); iter; iter=iter->next()) { + if(iter->get().IsHole()) { + hasholes = true; + break; + } + } + if(!hasholes) { + for(iter = inpolys->front(); iter; iter=iter->next()) { + outpolys->push_back(iter->get()); + } + return 1; + } + + polys = *inpolys; + + while(1) { + //find the hole point with the largest x + hasholes = false; + for(iter = polys.front(); iter; iter=iter->next()) { + if(!iter->get().IsHole()) continue; + + if(!hasholes) { + hasholes = true; + holeiter = iter; + holepointindex = 0; + } + + for(i=0; i < iter->get().GetNumPoints(); i++) { + if(iter->get().GetPoint(i).x > holeiter->get().GetPoint(holepointindex).x) { + holeiter = iter; + holepointindex = i; + } + } + } + if(!hasholes) break; + holepoint = holeiter->get().GetPoint(holepointindex); + + pointfound = false; + for(iter = polys.front(); iter; iter=iter->next()) { + if(iter->get().IsHole()) continue; + for(i=0; i < iter->get().GetNumPoints(); i++) { + if(iter->get().GetPoint(i).x <= holepoint.x) continue; + if(!InCone(iter->get().GetPoint((i+iter->get().GetNumPoints()-1)%(iter->get().GetNumPoints())), + iter->get().GetPoint(i), + iter->get().GetPoint((i+1)%(iter->get().GetNumPoints())), + holepoint)) + continue; + polypoint = iter->get().GetPoint(i); + if(pointfound) { + v1 = Normalize(polypoint-holepoint); + v2 = Normalize(bestpolypoint-holepoint); + if(v2.x > v1.x) continue; + } + pointvisible = true; + for(iter2 = polys.front(); iter2; iter2=iter2->next()) { + if(iter2->get().IsHole()) continue; + for(i2=0; i2 < iter2->get().GetNumPoints(); i2++) { + linep1 = iter2->get().GetPoint(i2); + linep2 = iter2->get().GetPoint((i2+1)%(iter2->get().GetNumPoints())); + if(Intersects(holepoint,polypoint,linep1,linep2)) { + pointvisible = false; + break; + } + } + if(!pointvisible) break; + } + if(pointvisible) { + pointfound = true; + bestpolypoint = polypoint; + polyiter = iter; + polypointindex = i; + } + } + } + + if(!pointfound) return 0; + + newpoly.Init(holeiter->get().GetNumPoints() + polyiter->get().GetNumPoints() + 2); + i2 = 0; + for(i=0;i<=polypointindex;i++) { + newpoly[i2] = polyiter->get().GetPoint(i); + i2++; + } + for(i=0;i<=holeiter->get().GetNumPoints();i++) { + newpoly[i2] = holeiter->get().GetPoint((i+holepointindex)%holeiter->get().GetNumPoints()); + i2++; + } + for(i=polypointindex;i<polyiter->get().GetNumPoints();i++) { + newpoly[i2] = polyiter->get().GetPoint(i); + i2++; + } + + polys.erase(holeiter); + polys.erase(polyiter); + polys.push_back(newpoly); + } + + for(iter = polys.front(); iter; iter=iter->next()) { + outpolys->push_back(iter->get()); + } + + return 1; +} + +bool TriangulatorPartition::IsConvex(Vector2& p1, Vector2& p2, Vector2& p3) { + real_t tmp; + tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y); + if(tmp>0) return 1; + else return 0; +} + +bool TriangulatorPartition::IsReflex(Vector2& p1, Vector2& p2, Vector2& p3) { + real_t tmp; + tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y); + if(tmp<0) return 1; + else return 0; +} + +bool TriangulatorPartition::IsInside(Vector2& p1, Vector2& p2, Vector2& p3, Vector2 &p) { + if(IsConvex(p1,p,p2)) return false; + if(IsConvex(p2,p,p3)) return false; + if(IsConvex(p3,p,p1)) return false; + return true; +} + +bool TriangulatorPartition::InCone(Vector2 &p1, Vector2 &p2, Vector2 &p3, Vector2 &p) { + bool convex; + + convex = IsConvex(p1,p2,p3); + + if(convex) { + if(!IsConvex(p1,p2,p)) return false; + if(!IsConvex(p2,p3,p)) return false; + return true; + } else { + if(IsConvex(p1,p2,p)) return true; + if(IsConvex(p2,p3,p)) return true; + return false; + } +} + +bool TriangulatorPartition::InCone(PartitionVertex *v, Vector2 &p) { + Vector2 p1,p2,p3; + + p1 = v->previous->p; + p2 = v->p; + p3 = v->next->p; + + return InCone(p1,p2,p3,p); +} + +void TriangulatorPartition::UpdateVertexReflexity(PartitionVertex *v) { + PartitionVertex *v1,*v3; + v1 = v->previous; + v3 = v->next; + v->isConvex = !IsReflex(v1->p,v->p,v3->p); +} + +void TriangulatorPartition::UpdateVertex(PartitionVertex *v, PartitionVertex *vertices, long numvertices) { + long i; + PartitionVertex *v1,*v3; + Vector2 vec1,vec3; + + v1 = v->previous; + v3 = v->next; + + v->isConvex = IsConvex(v1->p,v->p,v3->p); + + vec1 = Normalize(v1->p - v->p); + vec3 = Normalize(v3->p - v->p); + v->angle = vec1.x*vec3.x + vec1.y*vec3.y; + + if(v->isConvex) { + v->isEar = true; + for(i=0;i<numvertices;i++) { + if((vertices[i].p.x==v->p.x)&&(vertices[i].p.y==v->p.y)) continue; + if((vertices[i].p.x==v1->p.x)&&(vertices[i].p.y==v1->p.y)) continue; + if((vertices[i].p.x==v3->p.x)&&(vertices[i].p.y==v3->p.y)) continue; + if(IsInside(v1->p,v->p,v3->p,vertices[i].p)) { + v->isEar = false; + break; + } + } + } else { + v->isEar = false; + } +} + +//triangulation by ear removal +int TriangulatorPartition::Triangulate_EC(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles) { + long numvertices; + PartitionVertex *vertices; + PartitionVertex *ear; + TriangulatorPoly triangle; + long i,j; + bool earfound; + + if(poly->GetNumPoints() < 3) return 0; + if(poly->GetNumPoints() == 3) { + triangles->push_back(*poly); + return 1; + } + + numvertices = poly->GetNumPoints(); + + vertices = new PartitionVertex[numvertices]; + for(i=0;i<numvertices;i++) { + vertices[i].isActive = true; + vertices[i].p = poly->GetPoint(i); + if(i==(numvertices-1)) vertices[i].next=&(vertices[0]); + else vertices[i].next=&(vertices[i+1]); + if(i==0) vertices[i].previous = &(vertices[numvertices-1]); + else vertices[i].previous = &(vertices[i-1]); + } + for(i=0;i<numvertices;i++) { + UpdateVertex(&vertices[i],vertices,numvertices); + } + + for(i=0;i<numvertices-3;i++) { + earfound = false; + //find the most extruded ear + for(j=0;j<numvertices;j++) { + if(!vertices[j].isActive) continue; + if(!vertices[j].isEar) continue; + if(!earfound) { + earfound = true; + ear = &(vertices[j]); + } else { + if(vertices[j].angle > ear->angle) { + ear = &(vertices[j]); + } + } + } + if(!earfound) { + delete [] vertices; + return 0; + } + + triangle.Triangle(ear->previous->p,ear->p,ear->next->p); + triangles->push_back(triangle); + + ear->isActive = false; + ear->previous->next = ear->next; + ear->next->previous = ear->previous; + + if(i==numvertices-4) break; + + UpdateVertex(ear->previous,vertices,numvertices); + UpdateVertex(ear->next,vertices,numvertices); + } + for(i=0;i<numvertices;i++) { + if(vertices[i].isActive) { + triangle.Triangle(vertices[i].previous->p,vertices[i].p,vertices[i].next->p); + triangles->push_back(triangle); + break; + } + } + + delete [] vertices; + + return 1; +} + +int TriangulatorPartition::Triangulate_EC(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *triangles) { + List<TriangulatorPoly> outpolys; + List<TriangulatorPoly>::Element*iter; + + if(!RemoveHoles(inpolys,&outpolys)) return 0; + for(iter=outpolys.front();iter;iter=iter->next()) { + if(!Triangulate_EC(&(iter->get()),triangles)) return 0; + } + return 1; +} + +int TriangulatorPartition::ConvexPartition_HM(TriangulatorPoly *poly, List<TriangulatorPoly> *parts) { + List<TriangulatorPoly> triangles; + List<TriangulatorPoly>::Element *iter1,*iter2; + TriangulatorPoly *poly1,*poly2; + TriangulatorPoly newpoly; + Vector2 d1,d2,p1,p2,p3; + long i11,i12,i21,i22,i13,i23,j,k; + bool isdiagonal; + long numreflex; + + //check if the poly is already convex + numreflex = 0; + for(i11=0;i11<poly->GetNumPoints();i11++) { + if(i11==0) i12 = poly->GetNumPoints()-1; + else i12=i11-1; + if(i11==(poly->GetNumPoints()-1)) i13=0; + else i13=i11+1; + if(IsReflex(poly->GetPoint(i12),poly->GetPoint(i11),poly->GetPoint(i13))) { + numreflex = 1; + break; + } + } + if(numreflex == 0) { + parts->push_back(*poly); + return 1; + } + + if(!Triangulate_EC(poly,&triangles)) return 0; + + for(iter1 = triangles.front(); iter1 ; iter1=iter1->next()) { + poly1 = &(iter1->get()); + for(i11=0;i11<poly1->GetNumPoints();i11++) { + d1 = poly1->GetPoint(i11); + i12 = (i11+1)%(poly1->GetNumPoints()); + d2 = poly1->GetPoint(i12); + + isdiagonal = false; + for(iter2 = iter1; iter2 ; iter2=iter2->next()) { + if(iter1 == iter2) continue; + poly2 = &(iter2->get()); + + for(i21=0;i21<poly2->GetNumPoints();i21++) { + if((d2.x != poly2->GetPoint(i21).x)||(d2.y != poly2->GetPoint(i21).y)) continue; + i22 = (i21+1)%(poly2->GetNumPoints()); + if((d1.x != poly2->GetPoint(i22).x)||(d1.y != poly2->GetPoint(i22).y)) continue; + isdiagonal = true; + break; + } + if(isdiagonal) break; + } + + if(!isdiagonal) continue; + + p2 = poly1->GetPoint(i11); + if(i11 == 0) i13 = poly1->GetNumPoints()-1; + else i13 = i11-1; + p1 = poly1->GetPoint(i13); + if(i22 == (poly2->GetNumPoints()-1)) i23 = 0; + else i23 = i22+1; + p3 = poly2->GetPoint(i23); + + if(!IsConvex(p1,p2,p3)) continue; + + p2 = poly1->GetPoint(i12); + if(i12 == (poly1->GetNumPoints()-1)) i13 = 0; + else i13 = i12+1; + p3 = poly1->GetPoint(i13); + if(i21 == 0) i23 = poly2->GetNumPoints()-1; + else i23 = i21-1; + p1 = poly2->GetPoint(i23); + + if(!IsConvex(p1,p2,p3)) continue; + + newpoly.Init(poly1->GetNumPoints()+poly2->GetNumPoints()-2); + k = 0; + for(j=i12;j!=i11;j=(j+1)%(poly1->GetNumPoints())) { + newpoly[k] = poly1->GetPoint(j); + k++; + } + for(j=i22;j!=i21;j=(j+1)%(poly2->GetNumPoints())) { + newpoly[k] = poly2->GetPoint(j); + k++; + } + + triangles.erase(iter2); + iter1->get() = newpoly; + poly1 = &(iter1->get()); + i11 = -1; + + continue; + } + } + + for(iter1 = triangles.front(); iter1 ; iter1=iter1->next()) { + parts->push_back(iter1->get()); + } + + return 1; +} + +int TriangulatorPartition::ConvexPartition_HM(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *parts) { + List<TriangulatorPoly> outpolys; + List<TriangulatorPoly>::Element* iter; + + if(!RemoveHoles(inpolys,&outpolys)) return 0; + for(iter=outpolys.front();iter;iter=iter->next()) { + if(!ConvexPartition_HM(&(iter->get()),parts)) return 0; + } + return 1; +} + +//minimum-weight polygon triangulation by dynamic programming +//O(n^3) time complexity +//O(n^2) space complexity +int TriangulatorPartition::Triangulate_OPT(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles) { + long i,j,k,gap,n; + DPState **dpstates; + Vector2 p1,p2,p3,p4; + long bestvertex; + real_t weight,minweight,d1,d2; + Diagonal diagonal,newdiagonal; + List<Diagonal> diagonals; + TriangulatorPoly triangle; + int ret = 1; + + n = poly->GetNumPoints(); + dpstates = new DPState *[n]; + for(i=1;i<n;i++) { + dpstates[i] = new DPState[i]; + } + + //init states and visibility + for(i=0;i<(n-1);i++) { + p1 = poly->GetPoint(i); + for(j=i+1;j<n;j++) { + dpstates[j][i].visible = true; + dpstates[j][i].weight = 0; + dpstates[j][i].bestvertex = -1; + if(j!=(i+1)) { + p2 = poly->GetPoint(j); + + //visibility check + if(i==0) p3 = poly->GetPoint(n-1); + else p3 = poly->GetPoint(i-1); + if(i==(n-1)) p4 = poly->GetPoint(0); + else p4 = poly->GetPoint(i+1); + if(!InCone(p3,p1,p4,p2)) { + dpstates[j][i].visible = false; + continue; + } + + if(j==0) p3 = poly->GetPoint(n-1); + else p3 = poly->GetPoint(j-1); + if(j==(n-1)) p4 = poly->GetPoint(0); + else p4 = poly->GetPoint(j+1); + if(!InCone(p3,p2,p4,p1)) { + dpstates[j][i].visible = false; + continue; + } + + for(k=0;k<n;k++) { + p3 = poly->GetPoint(k); + if(k==(n-1)) p4 = poly->GetPoint(0); + else p4 = poly->GetPoint(k+1); + if(Intersects(p1,p2,p3,p4)) { + dpstates[j][i].visible = false; + break; + } + } + } + } + } + dpstates[n-1][0].visible = true; + dpstates[n-1][0].weight = 0; + dpstates[n-1][0].bestvertex = -1; + + for(gap = 2; gap<n; gap++) { + for(i=0; i<(n-gap); i++) { + j = i+gap; + if(!dpstates[j][i].visible) continue; + bestvertex = -1; + for(k=(i+1);k<j;k++) { + if(!dpstates[k][i].visible) continue; + if(!dpstates[j][k].visible) continue; + + if(k<=(i+1)) d1=0; + else d1 = Distance(poly->GetPoint(i),poly->GetPoint(k)); + if(j<=(k+1)) d2=0; + else d2 = Distance(poly->GetPoint(k),poly->GetPoint(j)); + + weight = dpstates[k][i].weight + dpstates[j][k].weight + d1 + d2; + + if((bestvertex == -1)||(weight<minweight)) { + bestvertex = k; + minweight = weight; + } + } + if(bestvertex == -1) { + for(i=1;i<n;i++) { + delete [] dpstates[i]; + } + delete [] dpstates; + + return 0; + } + + dpstates[j][i].bestvertex = bestvertex; + dpstates[j][i].weight = minweight; + } + } + + newdiagonal.index1 = 0; + newdiagonal.index2 = n-1; + diagonals.push_back(newdiagonal); + while(!diagonals.empty()) { + diagonal = (diagonals.front()->get()); + diagonals.pop_front(); + bestvertex = dpstates[diagonal.index2][diagonal.index1].bestvertex; + if(bestvertex == -1) { + ret = 0; + break; + } + triangle.Triangle(poly->GetPoint(diagonal.index1),poly->GetPoint(bestvertex),poly->GetPoint(diagonal.index2)); + triangles->push_back(triangle); + if(bestvertex > (diagonal.index1+1)) { + newdiagonal.index1 = diagonal.index1; + newdiagonal.index2 = bestvertex; + diagonals.push_back(newdiagonal); + } + if(diagonal.index2 > (bestvertex+1)) { + newdiagonal.index1 = bestvertex; + newdiagonal.index2 = diagonal.index2; + diagonals.push_back(newdiagonal); + } + } + + for(i=1;i<n;i++) { + delete [] dpstates[i]; + } + delete [] dpstates; + + return ret; +} + +void TriangulatorPartition::UpdateState(long a, long b, long w, long i, long j, DPState2 **dpstates) { + Diagonal newdiagonal; + List<Diagonal> *pairs; + long w2; + + w2 = dpstates[a][b].weight; + if(w>w2) return; + + pairs = &(dpstates[a][b].pairs); + newdiagonal.index1 = i; + newdiagonal.index2 = j; + + if(w<w2) { + pairs->clear(); + pairs->push_front(newdiagonal); + dpstates[a][b].weight = w; + } else { + if((!pairs->empty())&&(i <= pairs->front()->get().index1)) return; + while((!pairs->empty())&&(pairs->front()->get().index2 >= j)) pairs->pop_front(); + pairs->push_front(newdiagonal); + } +} + +void TriangulatorPartition::TypeA(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates) { + List<Diagonal> *pairs; + List<Diagonal>::Element *iter,*lastiter; + long top; + long w; + + if(!dpstates[i][j].visible) return; + top = j; + w = dpstates[i][j].weight; + if(k-j > 1) { + if (!dpstates[j][k].visible) return; + w += dpstates[j][k].weight + 1; + } + if(j-i > 1) { + pairs = &(dpstates[i][j].pairs); + iter = NULL; + lastiter = NULL; + while(iter!=pairs->front()) { + if (!iter) + iter=pairs->back(); + else + iter=iter->prev(); + + if(!IsReflex(vertices[iter->get().index2].p,vertices[j].p,vertices[k].p)) lastiter = iter; + else break; + } + if(lastiter == NULL) w++; + else { + if(IsReflex(vertices[k].p,vertices[i].p,vertices[lastiter->get().index1].p)) w++; + else top = lastiter->get().index1; + } + } + UpdateState(i,k,w,top,j,dpstates); +} + +void TriangulatorPartition::TypeB(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates) { + List<Diagonal> *pairs; + List<Diagonal>::Element* iter,*lastiter; + long top; + long w; + + if(!dpstates[j][k].visible) return; + top = j; + w = dpstates[j][k].weight; + + if (j-i > 1) { + if (!dpstates[i][j].visible) return; + w += dpstates[i][j].weight + 1; + } + if (k-j > 1) { + pairs = &(dpstates[j][k].pairs); + + iter = pairs->front(); + if((!pairs->empty())&&(!IsReflex(vertices[i].p,vertices[j].p,vertices[iter->get().index1].p))) { + lastiter = iter; + while(iter!=NULL) { + if(!IsReflex(vertices[i].p,vertices[j].p,vertices[iter->get().index1].p)) { + lastiter = iter; + iter=iter->next(); + } + else break; + } + if(IsReflex(vertices[lastiter->get().index2].p,vertices[k].p,vertices[i].p)) w++; + else top = lastiter->get().index2; + } else w++; + } + UpdateState(i,k,w,j,top,dpstates); +} + +int TriangulatorPartition::ConvexPartition_OPT(TriangulatorPoly *poly, List<TriangulatorPoly> *parts) { + Vector2 p1,p2,p3,p4; + PartitionVertex *vertices; + DPState2 **dpstates; + long i,j,k,n,gap; + List<Diagonal> diagonals,diagonals2; + Diagonal diagonal,newdiagonal; + List<Diagonal> *pairs,*pairs2; + List<Diagonal>::Element* iter,*iter2; + int ret; + TriangulatorPoly newpoly; + List<long> indices; + List<long>::Element* iiter; + bool ijreal,jkreal; + + n = poly->GetNumPoints(); + vertices = new PartitionVertex[n]; + + dpstates = new DPState2 *[n]; + for(i=0;i<n;i++) { + dpstates[i] = new DPState2[n]; + } + + //init vertex information + for(i=0;i<n;i++) { + vertices[i].p = poly->GetPoint(i); + vertices[i].isActive = true; + if(i==0) vertices[i].previous = &(vertices[n-1]); + else vertices[i].previous = &(vertices[i-1]); + if(i==(poly->GetNumPoints()-1)) vertices[i].next = &(vertices[0]); + else vertices[i].next = &(vertices[i+1]); + } + for(i=1;i<n;i++) { + UpdateVertexReflexity(&(vertices[i])); + } + + //init states and visibility + for(i=0;i<(n-1);i++) { + p1 = poly->GetPoint(i); + for(j=i+1;j<n;j++) { + dpstates[i][j].visible = true; + if(j==i+1) { + dpstates[i][j].weight = 0; + } else { + dpstates[i][j].weight = 2147483647; + } + if(j!=(i+1)) { + p2 = poly->GetPoint(j); + + //visibility check + if(!InCone(&vertices[i],p2)) { + dpstates[i][j].visible = false; + continue; + } + if(!InCone(&vertices[j],p1)) { + dpstates[i][j].visible = false; + continue; + } + + for(k=0;k<n;k++) { + p3 = poly->GetPoint(k); + if(k==(n-1)) p4 = poly->GetPoint(0); + else p4 = poly->GetPoint(k+1); + if(Intersects(p1,p2,p3,p4)) { + dpstates[i][j].visible = false; + break; + } + } + } + } + } + for(i=0;i<(n-2);i++) { + j = i+2; + if(dpstates[i][j].visible) { + dpstates[i][j].weight = 0; + newdiagonal.index1 = i+1; + newdiagonal.index2 = i+1; + dpstates[i][j].pairs.push_back(newdiagonal); + } + } + + dpstates[0][n-1].visible = true; + vertices[0].isConvex = false; //by convention + + for(gap=3; gap<n; gap++) { + for(i=0;i<n-gap;i++) { + if(vertices[i].isConvex) continue; + k = i+gap; + if(dpstates[i][k].visible) { + if(!vertices[k].isConvex) { + for(j=i+1;j<k;j++) TypeA(i,j,k,vertices,dpstates); + } else { + for(j=i+1;j<(k-1);j++) { + if(vertices[j].isConvex) continue; + TypeA(i,j,k,vertices,dpstates); + } + TypeA(i,k-1,k,vertices,dpstates); + } + } + } + for(k=gap;k<n;k++) { + if(vertices[k].isConvex) continue; + i = k-gap; + if((vertices[i].isConvex)&&(dpstates[i][k].visible)) { + TypeB(i,i+1,k,vertices,dpstates); + for(j=i+2;j<k;j++) { + if(vertices[j].isConvex) continue; + TypeB(i,j,k,vertices,dpstates); + } + } + } + } + + + //recover solution + ret = 1; + newdiagonal.index1 = 0; + newdiagonal.index2 = n-1; + diagonals.push_front(newdiagonal); + while(!diagonals.empty()) { + diagonal = (diagonals.front()->get()); + diagonals.pop_front(); + if((diagonal.index2 - diagonal.index1) <=1) continue; + pairs = &(dpstates[diagonal.index1][diagonal.index2].pairs); + if(pairs->empty()) { + ret = 0; + break; + } + if(!vertices[diagonal.index1].isConvex) { + iter = pairs->back(); + + j = iter->get().index2; + newdiagonal.index1 = j; + newdiagonal.index2 = diagonal.index2; + diagonals.push_front(newdiagonal); + if((j - diagonal.index1)>1) { + if(iter->get().index1 != iter->get().index2) { + pairs2 = &(dpstates[diagonal.index1][j].pairs); + while(1) { + if(pairs2->empty()) { + ret = 0; + break; + } + iter2 = pairs2->back(); + + if(iter->get().index1 != iter2->get().index1) pairs2->pop_back(); + else break; + } + if(ret == 0) break; + } + newdiagonal.index1 = diagonal.index1; + newdiagonal.index2 = j; + diagonals.push_front(newdiagonal); + } + } else { + iter = pairs->front(); + j = iter->get().index1; + newdiagonal.index1 = diagonal.index1; + newdiagonal.index2 = j; + diagonals.push_front(newdiagonal); + if((diagonal.index2 - j) > 1) { + if(iter->get().index1 != iter->get().index2) { + pairs2 = &(dpstates[j][diagonal.index2].pairs); + while(1) { + if(pairs2->empty()) { + ret = 0; + break; + } + iter2 = pairs2->front(); + if(iter->get().index2 != iter2->get().index2) pairs2->pop_front(); + else break; + } + if(ret == 0) break; + } + newdiagonal.index1 = j; + newdiagonal.index2 = diagonal.index2; + diagonals.push_front(newdiagonal); + } + } + } + + if(ret == 0) { + for(i=0;i<n;i++) { + delete [] dpstates[i]; + } + delete [] dpstates; + delete [] vertices; + + return ret; + } + + newdiagonal.index1 = 0; + newdiagonal.index2 = n-1; + diagonals.push_front(newdiagonal); + while(!diagonals.empty()) { + diagonal = (diagonals.front())->get(); + diagonals.pop_front(); + if((diagonal.index2 - diagonal.index1) <= 1) continue; + + indices.clear(); + diagonals2.clear(); + indices.push_back(diagonal.index1); + indices.push_back(diagonal.index2); + diagonals2.push_front(diagonal); + + while(!diagonals2.empty()) { + diagonal = (diagonals2.front()->get()); + diagonals2.pop_front(); + if((diagonal.index2 - diagonal.index1) <= 1) continue; + ijreal = true; + jkreal = true; + pairs = &(dpstates[diagonal.index1][diagonal.index2].pairs); + if(!vertices[diagonal.index1].isConvex) { + iter = pairs->back(); + j = iter->get().index2; + if(iter->get().index1 != iter->get().index2) ijreal = false; + } else { + iter = pairs->front(); + j = iter->get().index1; + if(iter->get().index1 != iter->get().index2) jkreal = false; + } + + newdiagonal.index1 = diagonal.index1; + newdiagonal.index2 = j; + if(ijreal) { + diagonals.push_back(newdiagonal); + } else { + diagonals2.push_back(newdiagonal); + } + + newdiagonal.index1 = j; + newdiagonal.index2 = diagonal.index2; + if(jkreal) { + diagonals.push_back(newdiagonal); + } else { + diagonals2.push_back(newdiagonal); + } + + indices.push_back(j); + } + + indices.sort(); + newpoly.Init((long)indices.size()); + k=0; + for(iiter = indices.front();iiter;iiter=iiter->next()) { + newpoly[k] = vertices[iiter->get()].p; + k++; + } + parts->push_back(newpoly); + } + + for(i=0;i<n;i++) { + delete [] dpstates[i]; + } + delete [] dpstates; + delete [] vertices; + + return ret; +} + +//triangulates a set of polygons by first partitioning them into monotone polygons +//O(n*log(n)) time complexity, O(n) space complexity +//the algorithm used here is outlined in the book +//"Computational Geometry: Algorithms and Applications" +//by Mark de Berg, Otfried Cheong, Marc van Kreveld and Mark Overmars +int TriangulatorPartition::MonotonePartition(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *monotonePolys) { + List<TriangulatorPoly>::Element *iter; + MonotoneVertex *vertices; + long i,numvertices,vindex,vindex2,newnumvertices,maxnumvertices; + long polystartindex, polyendindex; + TriangulatorPoly *poly; + MonotoneVertex *v,*v2,*vprev,*vnext; + ScanLineEdge newedge; + bool error = false; + + numvertices = 0; + for(iter = inpolys->front(); iter ; iter=iter->next()) { + numvertices += iter->get().GetNumPoints(); + } + + maxnumvertices = numvertices*3; + vertices = new MonotoneVertex[maxnumvertices]; + newnumvertices = numvertices; + + polystartindex = 0; + for(iter = inpolys->front(); iter ; iter=iter->next()) { + poly = &(iter->get()); + polyendindex = polystartindex + poly->GetNumPoints()-1; + for(i=0;i<poly->GetNumPoints();i++) { + vertices[i+polystartindex].p = poly->GetPoint(i); + if(i==0) vertices[i+polystartindex].previous = polyendindex; + else vertices[i+polystartindex].previous = i+polystartindex-1; + if(i==(poly->GetNumPoints()-1)) vertices[i+polystartindex].next = polystartindex; + else vertices[i+polystartindex].next = i+polystartindex+1; + } + polystartindex = polyendindex+1; + } + + //construct the priority queue + long *priority = new long [numvertices]; + for(i=0;i<numvertices;i++) priority[i] = i; + SortArray<long,VertexSorter> sorter; + sorter.compare.vertices=vertices; + sorter.sort(priority,numvertices); + + //determine vertex types + char *vertextypes = new char[maxnumvertices]; + for(i=0;i<numvertices;i++) { + v = &(vertices[i]); + vprev = &(vertices[v->previous]); + vnext = &(vertices[v->next]); + + if(Below(vprev->p,v->p)&&Below(vnext->p,v->p)) { + if(IsConvex(vnext->p,vprev->p,v->p)) { + vertextypes[i] = TRIANGULATOR_VERTEXTYPE_START; + } else { + vertextypes[i] = TRIANGULATOR_VERTEXTYPE_SPLIT; + } + } else if(Below(v->p,vprev->p)&&Below(v->p,vnext->p)) { + if(IsConvex(vnext->p,vprev->p,v->p)) + { + vertextypes[i] = TRIANGULATOR_VERTEXTYPE_END; + } else { + vertextypes[i] = TRIANGULATOR_VERTEXTYPE_MERGE; + } + } else { + vertextypes[i] = TRIANGULATOR_VERTEXTYPE_REGULAR; + } + } + + //helpers + long *helpers = new long[maxnumvertices]; + + //binary search tree that holds edges intersecting the scanline + //note that while set doesn't actually have to be implemented as a tree + //complexity requirements for operations are the same as for the balanced binary search tree + Set<ScanLineEdge> edgeTree; + //store iterators to the edge tree elements + //this makes deleting existing edges much faster + Set<ScanLineEdge>::Element **edgeTreeIterators,*edgeIter; + edgeTreeIterators = new Set<ScanLineEdge>::Element*[maxnumvertices]; +// Pair<Set<ScanLineEdge>::Element*,bool> edgeTreeRet; + for(i = 0; i<numvertices; i++) edgeTreeIterators[i] = NULL; + + //for each vertex + for(i=0;i<numvertices;i++) { + vindex = priority[i]; + v = &(vertices[vindex]); + vindex2 = vindex; + v2 = v; + + //depending on the vertex type, do the appropriate action + //comments in the following sections are copied from "Computational Geometry: Algorithms and Applications" + switch(vertextypes[vindex]) { + case TRIANGULATOR_VERTEXTYPE_START: + //Insert ei in T and set helper(ei) to vi. + newedge.p1 = v->p; + newedge.p2 = vertices[v->next].p; + newedge.index = vindex; + edgeTreeIterators[vindex] = edgeTree.insert(newedge); + helpers[vindex] = vindex; + break; + + case TRIANGULATOR_VERTEXTYPE_END: + //if helper(ei-1) is a merge vertex + if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) { + //Insert the diagonal connecting vi to helper(ei-1) in D. + AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous], + vertextypes, edgeTreeIterators, &edgeTree, helpers); + } + //Delete ei-1 from T + edgeTree.erase(edgeTreeIterators[v->previous]); + break; + + case TRIANGULATOR_VERTEXTYPE_SPLIT: + //Search in T to find the edge e j directly left of vi. + newedge.p1 = v->p; + newedge.p2 = v->p; + edgeIter = edgeTree.lower_bound(newedge); + if(edgeIter == edgeTree.front()) { + error = true; + break; + } + edgeIter=edgeIter->prev(); + //Insert the diagonal connecting vi to helper(ej) in D. + AddDiagonal(vertices,&newnumvertices,vindex,helpers[edgeIter->get().index], + vertextypes, edgeTreeIterators, &edgeTree, helpers); + vindex2 = newnumvertices-2; + v2 = &(vertices[vindex2]); + //helper(e j)�vi + helpers[edgeIter->get().index] = vindex; + //Insert ei in T and set helper(ei) to vi. + newedge.p1 = v2->p; + newedge.p2 = vertices[v2->next].p; + newedge.index = vindex2; + + edgeTreeIterators[vindex2] = edgeTree.insert(newedge); + helpers[vindex2] = vindex2; + break; + + case TRIANGULATOR_VERTEXTYPE_MERGE: + //if helper(ei-1) is a merge vertex + if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) { + //Insert the diagonal connecting vi to helper(ei-1) in D. + AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous], + vertextypes, edgeTreeIterators, &edgeTree, helpers); + vindex2 = newnumvertices-2; + v2 = &(vertices[vindex2]); + } + //Delete ei-1 from T. + edgeTree.erase(edgeTreeIterators[v->previous]); + //Search in T to find the edge e j directly left of vi. + newedge.p1 = v->p; + newedge.p2 = v->p; + edgeIter = edgeTree.lower_bound(newedge); + if(edgeIter == edgeTree.front()) { + error = true; + break; + } + edgeIter=edgeIter->prev(); + //if helper(ej) is a merge vertex + if(vertextypes[helpers[edgeIter->get().index]]==TRIANGULATOR_VERTEXTYPE_MERGE) { + //Insert the diagonal connecting vi to helper(e j) in D. + AddDiagonal(vertices,&newnumvertices,vindex2,helpers[edgeIter->get().index], + vertextypes, edgeTreeIterators, &edgeTree, helpers); + } + //helper(e j)�vi + helpers[edgeIter->get().index] = vindex2; + break; + + case TRIANGULATOR_VERTEXTYPE_REGULAR: + //if the interior of P lies to the right of vi + if(Below(v->p,vertices[v->previous].p)) { + //if helper(ei-1) is a merge vertex + if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) { + //Insert the diagonal connecting vi to helper(ei-1) in D. + AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous], + vertextypes, edgeTreeIterators, &edgeTree, helpers); + vindex2 = newnumvertices-2; + v2 = &(vertices[vindex2]); + } + //Delete ei-1 from T. + edgeTree.erase(edgeTreeIterators[v->previous]); + //Insert ei in T and set helper(ei) to vi. + newedge.p1 = v2->p; + newedge.p2 = vertices[v2->next].p; + newedge.index = vindex2; + edgeTreeIterators[vindex2] = edgeTree.insert(newedge); + helpers[vindex2] = vindex; + } else { + //Search in T to find the edge ej directly left of vi. + newedge.p1 = v->p; + newedge.p2 = v->p; + edgeIter = edgeTree.lower_bound(newedge); + if(edgeIter == edgeTree.front()) { + error = true; + break; + } + edgeIter=edgeIter->prev(); + //if helper(ej) is a merge vertex + if(vertextypes[helpers[edgeIter->get().index]]==TRIANGULATOR_VERTEXTYPE_MERGE) { + //Insert the diagonal connecting vi to helper(e j) in D. + AddDiagonal(vertices,&newnumvertices,vindex,helpers[edgeIter->get().index], + vertextypes, edgeTreeIterators, &edgeTree, helpers); + } + //helper(e j)�vi + helpers[edgeIter->get().index] = vindex; + } + break; + } + + if(error) break; + } + + char *used = new char[newnumvertices]; + memset(used,0,newnumvertices*sizeof(char)); + + if(!error) { + //return result + long size; + TriangulatorPoly mpoly; + for(i=0;i<newnumvertices;i++) { + if(used[i]) continue; + v = &(vertices[i]); + vnext = &(vertices[v->next]); + size = 1; + while(vnext!=v) { + vnext = &(vertices[vnext->next]); + size++; + } + mpoly.Init(size); + v = &(vertices[i]); + mpoly[0] = v->p; + vnext = &(vertices[v->next]); + size = 1; + used[i] = 1; + used[v->next] = 1; + while(vnext!=v) { + mpoly[size] = vnext->p; + used[vnext->next] = 1; + vnext = &(vertices[vnext->next]); + size++; + } + monotonePolys->push_back(mpoly); + } + } + + //cleanup + delete [] vertices; + delete [] priority; + delete [] vertextypes; + delete [] edgeTreeIterators; + delete [] helpers; + delete [] used; + + if(error) { + return 0; + } else { + return 1; + } +} + +//adds a diagonal to the doubly-connected list of vertices +void TriangulatorPartition::AddDiagonal(MonotoneVertex *vertices, long *numvertices, long index1, long index2, + char *vertextypes, Set<ScanLineEdge>::Element **edgeTreeIterators, + Set<ScanLineEdge> *edgeTree, long *helpers) +{ + long newindex1,newindex2; + + newindex1 = *numvertices; + (*numvertices)++; + newindex2 = *numvertices; + (*numvertices)++; + + vertices[newindex1].p = vertices[index1].p; + vertices[newindex2].p = vertices[index2].p; + + vertices[newindex2].next = vertices[index2].next; + vertices[newindex1].next = vertices[index1].next; + + vertices[vertices[index2].next].previous = newindex2; + vertices[vertices[index1].next].previous = newindex1; + + vertices[index1].next = newindex2; + vertices[newindex2].previous = index1; + + vertices[index2].next = newindex1; + vertices[newindex1].previous = index2; + + //update all relevant structures + vertextypes[newindex1] = vertextypes[index1]; + edgeTreeIterators[newindex1] = edgeTreeIterators[index1]; + helpers[newindex1] = helpers[index1]; + if(edgeTreeIterators[newindex1] != NULL) + edgeTreeIterators[newindex1]->get().index = newindex1; + vertextypes[newindex2] = vertextypes[index2]; + edgeTreeIterators[newindex2] = edgeTreeIterators[index2]; + helpers[newindex2] = helpers[index2]; + if(edgeTreeIterators[newindex2] != NULL) + edgeTreeIterators[newindex2]->get().index = newindex2; +} + +bool TriangulatorPartition::Below(Vector2 &p1, Vector2 &p2) { + if(p1.y < p2.y) return true; + else if(p1.y == p2.y) { + if(p1.x < p2.x) return true; + } + return false; +} + + + + + +//sorts in the falling order of y values, if y is equal, x is used instead +bool TriangulatorPartition::VertexSorter::operator() (long index1, long index2) const { + if(vertices[index1].p.y > vertices[index2].p.y) return true; + else if(vertices[index1].p.y == vertices[index2].p.y) { + if(vertices[index1].p.x > vertices[index2].p.x) return true; + } + return false; +} + +bool TriangulatorPartition::ScanLineEdge::IsConvex(const Vector2& p1, const Vector2& p2, const Vector2& p3) const { + real_t tmp; + tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y); + if(tmp>0) return 1; + else return 0; +} + +bool TriangulatorPartition::ScanLineEdge::operator < (const ScanLineEdge & other) const { + if(other.p1.y == other.p2.y) { + if(p1.y == p2.y) { + if(p1.y < other.p1.y) return true; + else return false; + } + if(IsConvex(p1,p2,other.p1)) return true; + else return false; + } else if(p1.y == p2.y) { + if(IsConvex(other.p1,other.p2,p1)) return false; + else return true; + } else if(p1.y < other.p1.y) { + if(IsConvex(other.p1,other.p2,p1)) return false; + else return true; + } else { + if(IsConvex(p1,p2,other.p1)) return true; + else return false; + } +} + +//triangulates monotone polygon +//O(n) time, O(n) space complexity +int TriangulatorPartition::TriangulateMonotone(TriangulatorPoly *inPoly, List<TriangulatorPoly> *triangles) { + long i,i2,j,topindex,bottomindex,leftindex,rightindex,vindex; + Vector2 *points; + long numpoints; + TriangulatorPoly triangle; + + numpoints = inPoly->GetNumPoints(); + points = inPoly->GetPoints(); + + //trivial calses + if(numpoints < 3) return 0; + if(numpoints == 3) { + triangles->push_back(*inPoly); + } + + topindex = 0; bottomindex=0; + for(i=1;i<numpoints;i++) { + if(Below(points[i],points[bottomindex])) bottomindex = i; + if(Below(points[topindex],points[i])) topindex = i; + } + + //check if the poly is really monotone + i = topindex; + while(i!=bottomindex) { + i2 = i+1; if(i2>=numpoints) i2 = 0; + if(!Below(points[i2],points[i])) return 0; + i = i2; + } + i = bottomindex; + while(i!=topindex) { + i2 = i+1; if(i2>=numpoints) i2 = 0; + if(!Below(points[i],points[i2])) return 0; + i = i2; + } + + char *vertextypes = new char[numpoints]; + long *priority = new long[numpoints]; + + //merge left and right vertex chains + priority[0] = topindex; + vertextypes[topindex] = 0; + leftindex = topindex+1; if(leftindex>=numpoints) leftindex = 0; + rightindex = topindex-1; if(rightindex<0) rightindex = numpoints-1; + for(i=1;i<(numpoints-1);i++) { + if(leftindex==bottomindex) { + priority[i] = rightindex; + rightindex--; if(rightindex<0) rightindex = numpoints-1; + vertextypes[priority[i]] = -1; + } else if(rightindex==bottomindex) { + priority[i] = leftindex; + leftindex++; if(leftindex>=numpoints) leftindex = 0; + vertextypes[priority[i]] = 1; + } else { + if(Below(points[leftindex],points[rightindex])) { + priority[i] = rightindex; + rightindex--; if(rightindex<0) rightindex = numpoints-1; + vertextypes[priority[i]] = -1; + } else { + priority[i] = leftindex; + leftindex++; if(leftindex>=numpoints) leftindex = 0; + vertextypes[priority[i]] = 1; + } + } + } + priority[i] = bottomindex; + vertextypes[bottomindex] = 0; + + long *stack = new long[numpoints]; + long stackptr = 0; + + stack[0] = priority[0]; + stack[1] = priority[1]; + stackptr = 2; + + //for each vertex from top to bottom trim as many triangles as possible + for(i=2;i<(numpoints-1);i++) { + vindex = priority[i]; + if(vertextypes[vindex]!=vertextypes[stack[stackptr-1]]) { + for(j=0;j<(stackptr-1);j++) { + if(vertextypes[vindex]==1) { + triangle.Triangle(points[stack[j+1]],points[stack[j]],points[vindex]); + } else { + triangle.Triangle(points[stack[j]],points[stack[j+1]],points[vindex]); + } + triangles->push_back(triangle); + } + stack[0] = priority[i-1]; + stack[1] = priority[i]; + stackptr = 2; + } else { + stackptr--; + while(stackptr>0) { + if(vertextypes[vindex]==1) { + if(IsConvex(points[vindex],points[stack[stackptr-1]],points[stack[stackptr]])) { + triangle.Triangle(points[vindex],points[stack[stackptr-1]],points[stack[stackptr]]); + triangles->push_back(triangle); + stackptr--; + } else { + break; + } + } else { + if(IsConvex(points[vindex],points[stack[stackptr]],points[stack[stackptr-1]])) { + triangle.Triangle(points[vindex],points[stack[stackptr]],points[stack[stackptr-1]]); + triangles->push_back(triangle); + stackptr--; + } else { + break; + } + } + } + stackptr++; + stack[stackptr] = vindex; + stackptr++; + } + } + vindex = priority[i]; + for(j=0;j<(stackptr-1);j++) { + if(vertextypes[stack[j+1]]==1) { + triangle.Triangle(points[stack[j]],points[stack[j+1]],points[vindex]); + } else { + triangle.Triangle(points[stack[j+1]],points[stack[j]],points[vindex]); + } + triangles->push_back(triangle); + } + + delete [] priority; + delete [] vertextypes; + delete [] stack; + + return 1; +} + +int TriangulatorPartition::Triangulate_MONO(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *triangles) { + List<TriangulatorPoly> monotone; + List<TriangulatorPoly>::Element* iter; + + if(!MonotonePartition(inpolys,&monotone)) return 0; + for(iter = monotone.front(); iter;iter=iter->next()) { + if(!TriangulateMonotone(&(iter->get()),triangles)) return 0; + } + return 1; +} + +int TriangulatorPartition::Triangulate_MONO(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles) { + List<TriangulatorPoly> polys; + polys.push_back(*poly); + + return Triangulate_MONO(&polys, triangles); +} diff --git a/core/math/triangulator.h b/core/math/triangulator.h new file mode 100644 index 0000000000..b6dd7e8236 --- /dev/null +++ b/core/math/triangulator.h @@ -0,0 +1,306 @@ +//Copyright (C) 2011 by Ivan Fratric +// +//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 TRIANGULATOR_H +#define TRIANGULATOR_H + +#include "math_2d.h" +#include "list.h" +#include "set.h" +//2D point structure + + +#define TRIANGULATOR_CCW 1 +#define TRIANGULATOR_CW -1 +//Polygon implemented as an array of points with a 'hole' flag +class TriangulatorPoly { +protected: + + + + Vector2 *points; + long numpoints; + bool hole; + +public: + + //constructors/destructors + TriangulatorPoly(); + ~TriangulatorPoly(); + + TriangulatorPoly(const TriangulatorPoly &src); + TriangulatorPoly& operator=(const TriangulatorPoly &src); + + //getters and setters + long GetNumPoints() { + return numpoints; + } + + bool IsHole() { + return hole; + } + + void SetHole(bool hole) { + this->hole = hole; + } + + Vector2 &GetPoint(long i) { + return points[i]; + } + + Vector2 *GetPoints() { + return points; + } + + Vector2& operator[] (int i) { + return points[i]; + } + + //clears the polygon points + void Clear(); + + //inits the polygon with numpoints vertices + void Init(long numpoints); + + //creates a triangle with points p1,p2,p3 + void Triangle(Vector2 &p1, Vector2 &p2, Vector2 &p3); + + //inverts the orfer of vertices + void Invert(); + + //returns the orientation of the polygon + //possible values: + // Triangulator_CCW : polygon vertices are in counter-clockwise order + // Triangulator_CW : polygon vertices are in clockwise order + // 0 : the polygon has no (measurable) area + int GetOrientation(); + + //sets the polygon orientation + //orientation can be + // Triangulator_CCW : sets vertices in counter-clockwise order + // Triangulator_CW : sets vertices in clockwise order + void SetOrientation(int orientation); +}; + +class TriangulatorPartition { +protected: + struct PartitionVertex { + bool isActive; + bool isConvex; + bool isEar; + + Vector2 p; + real_t angle; + PartitionVertex *previous; + PartitionVertex *next; + }; + + struct MonotoneVertex { + Vector2 p; + long previous; + long next; + }; + + struct VertexSorter{ + mutable MonotoneVertex *vertices; + bool operator() (long index1, long index2) const; + }; + + struct Diagonal { + long index1; + long index2; + }; + + //dynamic programming state for minimum-weight triangulation + struct DPState { + bool visible; + real_t weight; + long bestvertex; + }; + + //dynamic programming state for convex partitioning + struct DPState2 { + bool visible; + long weight; + List<Diagonal> pairs; + }; + + //edge that intersects the scanline + struct ScanLineEdge { + mutable long index; + Vector2 p1; + Vector2 p2; + + //determines if the edge is to the left of another edge + bool operator< (const ScanLineEdge & other) const; + + bool IsConvex(const Vector2& p1, const Vector2& p2, const Vector2& p3) const; + }; + + //standard helper functions + bool IsConvex(Vector2& p1, Vector2& p2, Vector2& p3); + bool IsReflex(Vector2& p1, Vector2& p2, Vector2& p3); + bool IsInside(Vector2& p1, Vector2& p2, Vector2& p3, Vector2 &p); + + bool InCone(Vector2 &p1, Vector2 &p2, Vector2 &p3, Vector2 &p); + bool InCone(PartitionVertex *v, Vector2 &p); + + int Intersects(Vector2 &p11, Vector2 &p12, Vector2 &p21, Vector2 &p22); + + Vector2 Normalize(const Vector2 &p); + real_t Distance(const Vector2 &p1, const Vector2 &p2); + + //helper functions for Triangulate_EC + void UpdateVertexReflexity(PartitionVertex *v); + void UpdateVertex(PartitionVertex *v,PartitionVertex *vertices, long numvertices); + + //helper functions for ConvexPartition_OPT + void UpdateState(long a, long b, long w, long i, long j, DPState2 **dpstates); + void TypeA(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates); + void TypeB(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates); + + //helper functions for MonotonePartition + bool Below(Vector2 &p1, Vector2 &p2); + void AddDiagonal(MonotoneVertex *vertices, long *numvertices, long index1, long index2, + char *vertextypes, Set<ScanLineEdge>::Element **edgeTreeIterators, + Set<ScanLineEdge> *edgeTree, long *helpers); + + //triangulates a monotone polygon, used in Triangulate_MONO + int TriangulateMonotone(TriangulatorPoly *inPoly, List<TriangulatorPoly> *triangles); + +public: + + //simple heuristic procedure for removing holes from a list of polygons + //works by creating a diagonal from the rightmost hole vertex to some visible vertex + //time complexity: O(h*(n^2)), h is the number of holes, n is the number of vertices + //space complexity: O(n) + //params: + // inpolys : a list of polygons that can contain holes + // vertices of all non-hole polys have to be in counter-clockwise order + // vertices of all hole polys have to be in clockwise order + // outpolys : a list of polygons without holes + //returns 1 on success, 0 on failure + int RemoveHoles(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *outpolys); + + //triangulates a polygon by ear clipping + //time complexity O(n^2), n is the number of vertices + //space complexity: O(n) + //params: + // poly : an input polygon to be triangulated + // vertices have to be in counter-clockwise order + // triangles : a list of triangles (result) + //returns 1 on success, 0 on failure + int Triangulate_EC(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles); + + //triangulates a list of polygons that may contain holes by ear clipping algorithm + //first calls RemoveHoles to get rid of the holes, and then Triangulate_EC for each resulting polygon + //time complexity: O(h*(n^2)), h is the number of holes, n is the number of vertices + //space complexity: O(n) + //params: + // inpolys : a list of polygons to be triangulated (can contain holes) + // vertices of all non-hole polys have to be in counter-clockwise order + // vertices of all hole polys have to be in clockwise order + // triangles : a list of triangles (result) + //returns 1 on success, 0 on failure + int Triangulate_EC(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *triangles); + + //creates an optimal polygon triangulation in terms of minimal edge length + //time complexity: O(n^3), n is the number of vertices + //space complexity: O(n^2) + //params: + // poly : an input polygon to be triangulated + // vertices have to be in counter-clockwise order + // triangles : a list of triangles (result) + //returns 1 on success, 0 on failure + int Triangulate_OPT(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles); + + //triangulates a polygons by firstly partitioning it into monotone polygons + //time complexity: O(n*log(n)), n is the number of vertices + //space complexity: O(n) + //params: + // poly : an input polygon to be triangulated + // vertices have to be in counter-clockwise order + // triangles : a list of triangles (result) + //returns 1 on success, 0 on failure + int Triangulate_MONO(TriangulatorPoly *poly, List<TriangulatorPoly> *triangles); + + //triangulates a list of polygons by firstly partitioning them into monotone polygons + //time complexity: O(n*log(n)), n is the number of vertices + //space complexity: O(n) + //params: + // inpolys : a list of polygons to be triangulated (can contain holes) + // vertices of all non-hole polys have to be in counter-clockwise order + // vertices of all hole polys have to be in clockwise order + // triangles : a list of triangles (result) + //returns 1 on success, 0 on failure + int Triangulate_MONO(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *triangles); + + //creates a monotone partition of a list of polygons that can contain holes + //time complexity: O(n*log(n)), n is the number of vertices + //space complexity: O(n) + //params: + // inpolys : a list of polygons to be triangulated (can contain holes) + // vertices of all non-hole polys have to be in counter-clockwise order + // vertices of all hole polys have to be in clockwise order + // monotonePolys : a list of monotone polygons (result) + //returns 1 on success, 0 on failure + int MonotonePartition(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *monotonePolys); + + //partitions a polygon into convex polygons by using Hertel-Mehlhorn algorithm + //the algorithm gives at most four times the number of parts as the optimal algorithm + //however, in practice it works much better than that and often gives optimal partition + //uses triangulation obtained by ear clipping as intermediate result + //time complexity O(n^2), n is the number of vertices + //space complexity: O(n) + //params: + // poly : an input polygon to be partitioned + // vertices have to be in counter-clockwise order + // parts : resulting list of convex polygons + //returns 1 on success, 0 on failure + int ConvexPartition_HM(TriangulatorPoly *poly, List<TriangulatorPoly> *parts); + + //partitions a list of polygons into convex parts by using Hertel-Mehlhorn algorithm + //the algorithm gives at most four times the number of parts as the optimal algorithm + //however, in practice it works much better than that and often gives optimal partition + //uses triangulation obtained by ear clipping as intermediate result + //time complexity O(n^2), n is the number of vertices + //space complexity: O(n) + //params: + // inpolys : an input list of polygons to be partitioned + // vertices of all non-hole polys have to be in counter-clockwise order + // vertices of all hole polys have to be in clockwise order + // parts : resulting list of convex polygons + //returns 1 on success, 0 on failure + int ConvexPartition_HM(List<TriangulatorPoly> *inpolys, List<TriangulatorPoly> *parts); + + //optimal convex partitioning (in terms of number of resulting convex polygons) + //using the Keil-Snoeyink algorithm + //M. Keil, J. Snoeyink, "On the time bound for convex decomposition of simple polygons", 1998 + //time complexity O(n^3), n is the number of vertices + //space complexity: O(n^3) + // poly : an input polygon to be partitioned + // vertices have to be in counter-clockwise order + // parts : resulting list of convex polygons + //returns 1 on success, 0 on failure + int ConvexPartition_OPT(TriangulatorPoly *poly, List<TriangulatorPoly> *parts); +}; + + +#endif diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp index cf6fd9242e..a3877eb9ff 100644 --- a/core/math/vector3.cpp +++ b/core/math/vector3.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -27,27 +27,12 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "vector3.h" - +#include "matrix3.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(); - + *this=Matrix3(p_axis,p_phi).xform(*this); } Vector3 Vector3::rotated(const Vector3& p_axis,float p_phi) const { diff --git a/core/math/vector3.h b/core/math/vector3.h index 959f7cd0a8..8a3cca8f33 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2015 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 */ @@ -40,11 +40,11 @@ struct Vector3 { enum Axis { AXIS_X, AXIS_Y, - AXIS_Z, + AXIS_Z, }; union { - + #ifdef USE_QUAD_VECTORS struct { @@ -52,7 +52,7 @@ struct Vector3 { real_t y; real_t z; real_t _unused; - }; + }; real_t coord[4]; #else @@ -61,18 +61,18 @@ struct Vector3 { 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]; } @@ -84,7 +84,7 @@ struct Vector3 { _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; @@ -107,10 +107,18 @@ struct Vector3 { _FORCE_INLINE_ real_t dot(const Vector3& p_b) const; _FORCE_INLINE_ Vector3 abs() const; + _FORCE_INLINE_ Vector3 floor() const; + _FORCE_INLINE_ Vector3 ceil() const; _FORCE_INLINE_ real_t distance_to(const Vector3& p_b) const; _FORCE_INLINE_ real_t distance_squared_to(const Vector3& p_b) const; + + + _FORCE_INLINE_ Vector3 slide(const Vector3& p_vec) const; + _FORCE_INLINE_ Vector3 reflect(const Vector3& p_vec) const; + + /* Operators */ _FORCE_INLINE_ Vector3& operator+=(const Vector3& p_v); @@ -166,7 +174,17 @@ real_t Vector3::dot(const Vector3& p_b) const { Vector3 Vector3::abs() const { return Vector3( Math::abs(x), Math::abs(y), Math::abs(z) ); -} +} + +Vector3 Vector3::floor() const { + + return Vector3( Math::floor(x), Math::floor(y), Math::floor(z) ); +} + +Vector3 Vector3::ceil() const { + + return Vector3( Math::ceil(x), Math::ceil(y), Math::ceil(z) ); +} Vector3 Vector3::linear_interpolate(const Vector3& p_b,float p_t) const { @@ -295,7 +313,7 @@ bool Vector3::operator<(const Vector3& p_v) const { return y<p_v.y; } else return x<p_v.x; - + } bool Vector3::operator<=(const Vector3& p_v) const { @@ -368,6 +386,16 @@ void Vector3::zero() { x=y=z=0; } +Vector3 Vector3::slide(const Vector3& p_vec) const { + + return p_vec - *this * this->dot(p_vec); +} +Vector3 Vector3::reflect(const Vector3& p_vec) const { + + return p_vec - *this * this->dot(p_vec) * 2.0; + +} + #endif #endif // VECTOR3_H |