diff options
Diffstat (limited to 'core/math')
35 files changed, 771 insertions, 715 deletions
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 fbe5f8c741..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 */ diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h index 767236ea04..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 */ diff --git a/core/math/face3.cpp b/core/math/face3.cpp index 1adc95e4e9..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 */ diff --git a/core/math/face3.h b/core/math/face3.h index 5a509299a2..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 */ diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp index 2d525dd1ce..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 */ diff --git a/core/math/geometry.h b/core/math/geometry.h index 4ad4db8523..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 */ diff --git a/core/math/math_2d.cpp b/core/math/math_2d.cpp index 3aaa539fbb..88717723ce 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 */ @@ -230,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; @@ -239,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; @@ -605,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 5bc1b5f0be..5e6cefd114 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 */ @@ -214,18 +214,41 @@ 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; 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 ad48ceaac0..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 @@ -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 6d43ed8e64..33175ed2fc 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 */ 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 04901116b8..de4aedaeec 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 */ diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp index a417cdaddf..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 */ 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/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 d2f2408829..d27b611379 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 */ |