Merge branch 'master' of github.com:okamstudio/godot

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       */