GODOT IS OPEN SOURCE

36 files changed, 11844 insertions, 0 deletions

diff --git a/core/math/SCsub b/core/math/SCsub
new file mode 100644
index 0000000000..c6ba1fa537
--- /dev/null
+++ b/core/math/SCsub
@@ -0,0 +1,7 @@
+Import('env')
+
+env.add_source_files(env.core_sources,"*.cpp")
+
+Export('env')
+
+
diff --git a/core/math/aabb.cpp b/core/math/aabb.cpp
new file mode 100644
index 0000000000..0d454cd07d
--- /dev/null
+++ b/core/math/aabb.cpp
@@ -0,0 +1,427 @@
+/*************************************************************************/
+/*  aabb.cpp                                                             */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "aabb.h"
+
+#include "print_string.h"
+
+float AABB::get_area() const {
+
+	return size.x*size.y*size.z;
+
+}
+
+bool AABB::operator==(const AABB& p_rval) const {
+
+	return ((pos==p_rval.pos) && (size==p_rval.size));
+
+}
+bool AABB::operator!=(const AABB& p_rval) const {
+
+	return ((pos!=p_rval.pos) || (size!=p_rval.size));
+
+}
+
+void AABB::merge_with(const AABB& p_aabb) {
+
+	Vector3 beg_1,beg_2;
+	Vector3 end_1,end_2;
+        Vector3 min,max;
+
+	beg_1=pos;
+	beg_2=p_aabb.pos;
+	end_1=Vector3(size.x,size.y,size.z)+beg_1;
+	end_2=Vector3(p_aabb.size.x,p_aabb.size.y,p_aabb.size.z)+beg_2;
+
+	min.x=(beg_1.x<beg_2.x)?beg_1.x:beg_2.x;
+	min.y=(beg_1.y<beg_2.y)?beg_1.y:beg_2.y;
+	min.z=(beg_1.z<beg_2.z)?beg_1.z:beg_2.z;
+
+	max.x=(end_1.x>end_2.x)?end_1.x:end_2.x;
+	max.y=(end_1.y>end_2.y)?end_1.y:end_2.y;
+	max.z=(end_1.z>end_2.z)?end_1.z:end_2.z;
+
+	pos=min;
+	size=max-min;
+}
+
+AABB AABB::intersection(const AABB& p_aabb) const {
+
+	Vector3 src_min=pos;
+	Vector3 src_max=pos+size;
+	Vector3 dst_min=p_aabb.pos;
+	Vector3 dst_max=p_aabb.pos+p_aabb.size;
+
+	Vector3 min,max;
+
+	if (src_min.x > dst_max.x || src_max.x < dst_min.x )
+		return AABB();
+	else {
+
+		min.x= ( src_min.x > dst_min.x ) ? src_min.x :dst_min.x;
+		max.x= ( src_max.x < dst_max.x ) ? src_max.x :dst_max.x;
+
+	}
+
+	if (src_min.y > dst_max.y || src_max.y < dst_min.y )
+		return AABB();
+	else {
+
+		min.y= ( src_min.y > dst_min.y ) ? src_min.y :dst_min.y;
+		max.y= ( src_max.y < dst_max.y ) ? src_max.y :dst_max.y;
+
+	}
+
+	if (src_min.z > dst_max.z || src_max.z < dst_min.z )
+		return AABB();
+	else {
+
+		min.z= ( src_min.z > dst_min.z ) ? src_min.z :dst_min.z;
+		max.z= ( src_max.z < dst_max.z ) ? src_max.z :dst_max.z;
+
+	}
+
+
+	return AABB( min, max-min );
+}
+
+bool AABB::intersects_ray(const Vector3& p_from, const Vector3& p_dir,Vector3* r_clip,Vector3* r_normal) const {
+
+	Vector3 c1, c2;
+	Vector3 end = pos+size;
+	float near=-1e20;
+	float far=1e20;
+	int axis=0;
+
+	for (int i=0;i<3;i++){
+		if (p_dir[i] == 0){
+			if ((p_from[i] < pos[i]) || (p_from[i] > end[i])) {
+				return false;
+			}
+		} else { // ray not parallel to planes in this direction
+			c1[i] = (pos[i] - p_from[i]) / p_dir[i];
+			c2[i] = (end[i] - p_from[i]) / p_dir[i];
+
+			if(c1[i] > c2[i]){
+				SWAP(c1,c2);
+			}
+			if (c1[i] > near){
+				near = c1[i];
+				axis=i;
+			}
+			if (c2[i] < far){
+				far = c2[i];
+			}
+			if( (near > far) || (far < 0) ){
+				return false;
+			}
+		}
+	}
+
+	if (r_clip)
+		*r_clip=c1;
+	if (r_normal) {
+		*r_normal=Vector3();
+		(*r_normal)[axis]=p_dir[axis]?-1:1;
+	}
+
+	return true;
+
+}
+
+
+bool AABB::intersects_segment(const Vector3& p_from, const Vector3& p_to,Vector3* r_clip,Vector3* r_normal) const {
+
+	real_t min=0,max=1;
+	int axis=0;
+	float sign=0;
+
+	for(int i=0;i<3;i++) {
+		real_t seg_from=p_from[i];
+		real_t seg_to=p_to[i];
+		real_t box_begin=pos[i];
+		real_t box_end=box_begin+size[i];
+		real_t cmin,cmax;
+		float csign;
+
+		if (seg_from < seg_to) {
+
+			if (seg_from > box_end || seg_to < box_begin)
+				return false;
+			real_t length=seg_to-seg_from;
+			cmin = (seg_from < box_begin)?((box_begin - seg_from)/length):0;
+			cmax = (seg_to > box_end)?((box_end - seg_from)/length):1;
+			csign=-1.0;
+
+		} else {
+
+			if (seg_to > box_end || seg_from < box_begin)
+				return false;
+			real_t length=seg_to-seg_from;
+			cmin = (seg_from > box_end)?(box_end - seg_from)/length:0;
+			cmax = (seg_to < box_begin)?(box_begin - seg_from)/length:1;
+			csign=1.0;
+		}
+
+		if (cmin > min) {
+			min = cmin;
+			axis=i;
+			sign=csign;
+		}
+		if (cmax < max)
+			max = cmax;
+		if (max < min)
+			return false;
+	}
+
+
+	Vector3 rel=p_to-p_from;
+
+	if (r_normal) {
+		Vector3 normal;
+		normal[axis]=sign;
+		*r_normal=normal;
+	}
+
+	if (r_clip)
+		*r_clip=p_from+rel*min;
+
+	return true;
+
+}
+
+
+bool AABB::intersects_plane(const Plane &p_plane) const {
+
+	Vector3 points[8] = {
+		Vector3( pos.x	, pos.y		, pos.z		),
+		Vector3( pos.x	, pos.y		, pos.z+size.z	),
+		Vector3( pos.x	, pos.y+size.y	, pos.z		),
+		Vector3( pos.x	, pos.y+size.y	, pos.z+size.z	),
+		Vector3( pos.x+size.x	, pos.y		, pos.z		),
+		Vector3( pos.x+size.x	, pos.y		, pos.z+size.z	),
+		Vector3( pos.x+size.x	, pos.y+size.y	, pos.z		),
+		Vector3( pos.x+size.x	, pos.y+size.y	, pos.z+size.z	),
+	};
+
+	bool over=false;
+	bool under=false;
+
+	for (int i=0;i<8;i++) {
+
+		if (p_plane.distance_to(points[i])>0)
+			over=true;
+		else
+			under=true;
+
+	}
+
+	return under && over;
+}
+
+
+
+Vector3 AABB::get_longest_axis() const {
+
+	Vector3 axis(1,0,0);
+	real_t max_size=size.x;
+
+	if (size.y > max_size ) {
+		axis=Vector3(0,1,0);
+		max_size=size.y;
+	}
+
+	if (size.z > max_size ) {
+		axis=Vector3(0,0,1);
+		max_size=size.z;
+	}
+
+	return axis;
+}
+int AABB::get_longest_axis_index() const {
+
+	int axis=0;
+	real_t max_size=size.x;
+
+	if (size.y > max_size ) {
+		axis=1;
+		max_size=size.y;
+	}
+
+	if (size.z > max_size ) {
+		axis=2;
+		max_size=size.z;
+	}
+
+	return axis;
+}
+
+
+Vector3 AABB::get_shortest_axis() const {
+
+	Vector3 axis(1,0,0);
+	real_t max_size=size.x;
+
+	if (size.y < max_size ) {
+		axis=Vector3(0,1,0);
+		max_size=size.y;
+	}
+
+	if (size.z < max_size ) {
+		axis=Vector3(0,0,1);
+		max_size=size.z;
+	}
+
+	return axis;
+}
+int AABB::get_shortest_axis_index() const {
+
+	int axis=0;
+	real_t max_size=size.x;
+
+	if (size.y < max_size ) {
+		axis=1;
+		max_size=size.y;
+	}
+
+	if (size.z < max_size ) {
+		axis=2;
+		max_size=size.z;
+	}
+
+	return axis;
+}
+
+AABB AABB::merge(const AABB& p_with) const {
+
+	AABB aabb=*this;
+	aabb.merge_with(p_with);
+	return aabb;
+}
+AABB AABB::expand(const Vector3& p_vector) const {
+	AABB aabb=*this;
+	aabb.expand_to(p_vector);
+	return aabb;
+
+}
+AABB AABB::grow(real_t p_by) const {
+
+	AABB aabb=*this;
+	aabb.grow_by(p_by);
+	return aabb;
+}
+void AABB::grow_by(real_t p_amount) {
+
+	pos.x-=p_amount;
+	pos.y-=p_amount;
+	pos.z-=p_amount;
+	size.x+=2.0*p_amount;
+	size.y+=2.0*p_amount;
+	size.z+=2.0*p_amount;
+}
+
+void AABB::get_edge(int p_edge,Vector3& r_from,Vector3& r_to) const {
+
+	ERR_FAIL_INDEX(p_edge,12);
+	switch(p_edge) {
+
+		case 0:{
+
+			r_from=Vector3( pos.x+size.x	, pos.y		, pos.z		);
+			r_to=Vector3( pos.x	, pos.y		, pos.z		);
+		} break;
+		case 1:{
+
+			r_from=Vector3( pos.x+size.x	, pos.y		, pos.z+size.z	);
+			r_to=Vector3( pos.x+size.x	, pos.y		, pos.z		);
+		} break;
+		case 2:{
+			r_from=Vector3( pos.x	, pos.y		, pos.z+size.z	);
+			r_to=Vector3( pos.x+size.x	, pos.y		, pos.z+size.z	);
+
+		} break;
+		case 3:{
+
+			r_from=Vector3( pos.x	, pos.y		, pos.z		);
+			r_to=Vector3( pos.x	, pos.y		, pos.z+size.z	);
+
+		} break;
+		case 4:{
+
+			r_from=Vector3( pos.x	, pos.y+size.y		, pos.z		);
+			r_to=Vector3( pos.x+size.x	, pos.y+size.y		, pos.z		);
+		} break;
+		case 5:{
+
+			r_from=Vector3( pos.x+size.x	, pos.y+size.y		, pos.z		);
+			r_to=Vector3( pos.x+size.x	, pos.y+size.y		, pos.z+size.z	);
+		} break;
+		case 6:{
+			r_from=Vector3( pos.x+size.x	, pos.y+size.y		, pos.z+size.z	);
+			r_to=Vector3( pos.x	, pos.y+size.y		, pos.z+size.z	);
+
+		} break;
+		case 7:{
+
+			r_from=Vector3( pos.x	, pos.y+size.y		, pos.z+size.z	);
+			r_to=Vector3( pos.x	, pos.y+size.y		, pos.z		);
+
+		} break;
+		case 8:{
+
+			r_from=Vector3( pos.x	, pos.y		, pos.z+size.z	);
+			r_to=Vector3( pos.x	, pos.y+size.y		, pos.z+size.z	);
+
+		} break;
+		case 9:{
+
+			r_from=Vector3( pos.x	, pos.y		, pos.z		);
+			r_to=Vector3( pos.x	, pos.y+size.y	, pos.z		);
+
+		} break;
+		case 10:{
+
+			r_from=Vector3( pos.x+size.x	, pos.y		, pos.z		);
+			r_to=Vector3( pos.x+size.x	, pos.y+size.y	, pos.z		);
+
+		} break;
+		case 11:{
+
+			r_from=Vector3( pos.x+size.x	, pos.y		, pos.z+size.z		);
+			r_to=Vector3( pos.x+size.x	, pos.y+size.y	, pos.z+size.z		);
+
+		} break;
+
+	}
+
+}
+
+AABB::operator String() const {
+
+	return String()+pos +" - "+ size;
+}
diff --git a/core/math/aabb.h b/core/math/aabb.h
new file mode 100644
index 0000000000..87be03cf16
--- /dev/null
+++ b/core/math/aabb.h
@@ -0,0 +1,319 @@
+/*************************************************************************/
+/*  aabb.h                                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AABB_H
+#define AABB_H
+
+
+
+#include "vector3.h"
+#include "plane.h"
+/**
+ * AABB / AABB (Axis Aligned Bounding Box)
+ * This is implemented by a point (pos) and the box size
+ */
+
+
+
+class AABB {
+public:
+	Vector3 pos;
+	Vector3 size;
+
+	float get_area() const; /// get area
+	_FORCE_INLINE_ bool has_no_area() const {
+	
+		return (size.x<=CMP_EPSILON || size.y<=CMP_EPSILON  || size.z<=CMP_EPSILON);
+	}
+
+	_FORCE_INLINE_ bool has_no_surface() const {
+	
+		return (size.x<=CMP_EPSILON && size.y<=CMP_EPSILON  && size.z<=CMP_EPSILON);
+	}
+	
+	const Vector3& get_pos() const { return pos; }
+	void set_pos(const Vector3& p_pos) { pos=p_pos; }
+	const Vector3& get_size() const { return size; }
+	void set_size(const Vector3& p_size) { size=p_size; }
+
+	
+	bool operator==(const AABB& p_rval) const;
+	bool operator!=(const AABB& p_rval) const;
+
+	_FORCE_INLINE_ bool intersects(const AABB& p_aabb) const; /// Both AABBs overlap
+	_FORCE_INLINE_ bool encloses(const AABB & p_aabb) const; /// p_aabb is completely inside this
+
+	AABB merge(const AABB& p_with) const;
+        void merge_with(const AABB& p_aabb); ///merge with another AABB
+	AABB intersection(const AABB& p_aabb) const; ///get box where two intersect, empty if no intersection occurs
+	bool intersects_segment(const Vector3& p_from, const Vector3& p_to,Vector3* r_clip=NULL,Vector3* r_normal=NULL) const;
+	bool intersects_ray(const Vector3& p_from, const Vector3& p_dir,Vector3* r_clip=NULL,Vector3* r_normal=NULL) const;
+	_FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_plane, int p_plane_count) const;
+	bool intersects_plane(const Plane &p_plane) const;
+
+	_FORCE_INLINE_ bool has_point(const Vector3& p_point) const;
+	_FORCE_INLINE_ Vector3 get_support(const Vector3& p_normal) const;
+
+
+	Vector3 get_longest_axis() const;
+	int get_longest_axis_index() const;
+	_FORCE_INLINE_ real_t get_longest_axis_size() const;
+
+	Vector3 get_shortest_axis() const;
+	int get_shortest_axis_index() const;
+	_FORCE_INLINE_ real_t get_shortest_axis_size() const;
+
+	AABB grow(real_t p_by) const;
+	void grow_by(real_t p_amount);
+
+	void get_edge(int p_edge,Vector3& r_from,Vector3& r_to) const;
+	_FORCE_INLINE_ Vector3 get_endpoint(int p_point) const;
+
+	AABB expand(const Vector3& p_vector) const;
+	_FORCE_INLINE_ void project_range_in_plane(const Plane& p_plane,float &r_min,float& r_max) const;
+	_FORCE_INLINE_ void expand_to(const Vector3& p_vector); /** expand to contain a point if necesary */
+
+	operator String() const;
+
+	_FORCE_INLINE_ AABB() {}
+	inline AABB(const Vector3 &p_pos,const Vector3& p_size) { pos=p_pos; size=p_size; }
+
+
+};
+
+inline bool AABB::intersects(const AABB& p_aabb) const {
+
+        if ( pos.x > (p_aabb.pos.x + p_aabb.size.x) )
+                return false;
+        if ( (pos.x+size.x) < p_aabb.pos.x  )
+                return false;
+        if ( pos.y > (p_aabb.pos.y + p_aabb.size.y) )
+                return false;
+        if ( (pos.y+size.y) < p_aabb.pos.y  )
+                return false;
+        if ( pos.z > (p_aabb.pos.z + p_aabb.size.z) )
+                return false;
+        if ( (pos.z+size.z) < p_aabb.pos.z  )
+                return false;
+
+        return true;
+}
+
+
+inline bool AABB::encloses(const AABB & p_aabb) const {
+
+	Vector3 src_min=pos;
+	Vector3 src_max=pos+size;
+	Vector3 dst_min=p_aabb.pos;
+	Vector3 dst_max=p_aabb.pos+p_aabb.size;
+
+	return  (
+		 (src_min.x <= dst_min.x) &&
+			(src_max.x > dst_max.x) &&
+			(src_min.y <= dst_min.y) &&
+			(src_max.y > dst_max.y) &&
+			(src_min.z <= dst_min.z) &&
+			(src_max.z > dst_max.z) );
+
+}
+
+Vector3 AABB::get_support(const Vector3& p_normal) const {
+
+	Vector3 half_extents = size * 0.5;
+	Vector3 ofs = pos + half_extents;
+
+	return Vector3(
+			(p_normal.x>0) ? -half_extents.x : half_extents.x,
+			(p_normal.y>0) ? -half_extents.y : half_extents.y,
+			(p_normal.z>0) ? -half_extents.z : half_extents.z
+		)+ofs;
+}
+
+
+Vector3 AABB::get_endpoint(int p_point) const {
+
+	switch(p_point) {
+		case 0: return Vector3( pos.x	, pos.y		, pos.z		);
+		case 1: return Vector3( pos.x	, pos.y		, pos.z+size.z	);
+		case 2: return Vector3( pos.x	, pos.y+size.y	, pos.z		);
+		case 3: return Vector3( pos.x	, pos.y+size.y	, pos.z+size.z	);
+		case 4: return Vector3( pos.x+size.x	, pos.y		, pos.z		);
+		case 5: return Vector3( pos.x+size.x	, pos.y		, pos.z+size.z	);
+		case 6: return Vector3( pos.x+size.x	, pos.y+size.y	, pos.z		);
+		case 7: return Vector3( pos.x+size.x	, pos.y+size.y	, pos.z+size.z	);
+	};
+
+	ERR_FAIL_V(Vector3());
+}
+
+bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count) const {
+
+#if 1
+
+	Vector3 half_extents = size * 0.5;
+	Vector3 ofs = pos + half_extents;
+
+	for(int i=0;i<p_plane_count;i++) {
+		const Plane &p=p_planes[i];
+		Vector3 point(
+				(p.normal.x>0) ? -half_extents.x : half_extents.x,
+				(p.normal.y>0) ? -half_extents.y : half_extents.y,
+				(p.normal.z>0) ? -half_extents.z : half_extents.z
+			);
+		point+=ofs;
+		if (p.is_point_over(point))
+			return false;
+	}
+
+	return true;
+#else
+	//cache all points to check against!
+// #warning should be easy to optimize, just use the same as when taking the support and use only that point
+	Vector3 points[8] = {
+		Vector3( pos.x	, pos.y		, pos.z		),
+		Vector3( pos.x	, pos.y		, pos.z+size.z	),
+		Vector3( pos.x	, pos.y+size.y	, pos.z		),
+		Vector3( pos.x	, pos.y+size.y	, pos.z+size.z	),
+		Vector3( pos.x+size.x	, pos.y		, pos.z		),
+		Vector3( pos.x+size.x	, pos.y		, pos.z+size.z	),
+		Vector3( pos.x+size.x	, pos.y+size.y	, pos.z		),
+		Vector3( pos.x+size.x	, pos.y+size.y	, pos.z+size.z	),
+	};
+
+	for (int i=0;i<p_plane_count;i++) { //for each plane
+
+		const Plane & plane=p_planes[i];
+		bool all_points_over=true;
+		//test if it has all points over!
+
+		for (int j=0;j<8;j++) {
+
+
+			if (!plane.is_point_over( points[j] )) {
+
+				all_points_over=false;
+				break;
+			}
+
+		}
+
+		if (all_points_over) {
+
+			return false;
+		}
+	}
+	return true;
+#endif
+}
+
+bool AABB::has_point(const Vector3& p_point) const {
+
+	if (p_point.x<pos.x)
+		return false;
+	if (p_point.y<pos.y)
+		return false;
+	if (p_point.z<pos.z)
+		return false;
+	if (p_point.x>pos.x+size.x)
+		return false;
+	if (p_point.y>pos.y+size.y)
+		return false;
+	if (p_point.z>pos.z+size.z)
+		return false;
+		
+	return true;
+}
+
+
+inline void AABB::expand_to(const Vector3& p_vector) {
+
+	Vector3 begin=pos;
+	Vector3 end=pos+size;
+
+	if (p_vector.x<begin.x)
+		begin.x=p_vector.x;
+	if (p_vector.y<begin.y)
+		begin.y=p_vector.y;
+	if (p_vector.z<begin.z)
+		begin.z=p_vector.z;
+
+	if (p_vector.x>end.x)
+		end.x=p_vector.x;
+	if (p_vector.y>end.y)
+		end.y=p_vector.y;
+	if (p_vector.z>end.z)
+		end.z=p_vector.z;
+
+	pos=begin;
+	size=end-begin;
+}
+
+void AABB::project_range_in_plane(const Plane& p_plane,float &r_min,float& r_max) const {
+
+	Vector3 half_extents( size.x * 0.5, size.y * 0.5, size.z * 0.5 );
+	Vector3 center( pos.x + half_extents.x, pos.y + half_extents.y, pos.z + half_extents.z );
+		
+	float length = p_plane.normal.abs().dot(half_extents);
+	float distance = p_plane.distance_to( center );
+	r_min = distance - length;
+	r_max = distance + length;	
+}
+
+inline real_t AABB::get_longest_axis_size() const {
+
+	real_t max_size=size.x;
+
+	if (size.y > max_size ) {
+		max_size=size.y;
+	}
+
+	if (size.z > max_size ) {
+		max_size=size.z;
+	}
+
+	return max_size;
+}
+
+inline real_t AABB::get_shortest_axis_size() const {
+
+	real_t max_size=size.x;
+
+	if (size.y < max_size ) {
+		max_size=size.y;
+	}
+
+	if (size.z < max_size ) {
+		max_size=size.z;
+	}
+
+	return max_size;
+}
+
+typedef AABB Rect3;
+
+#endif // AABB_H
diff --git a/core/math/bezier_curve.cpp b/core/math/bezier_curve.cpp
new file mode 100644
index 0000000000..2676804945
--- /dev/null
+++ b/core/math/bezier_curve.cpp
@@ -0,0 +1,29 @@
+/*************************************************************************/
+/*  bezier_curve.cpp                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "bezier_curve.h"
diff --git a/core/math/bezier_curve.h b/core/math/bezier_curve.h
new file mode 100644
index 0000000000..6cc4c730db
--- /dev/null
+++ b/core/math/bezier_curve.h
@@ -0,0 +1,35 @@
+/*************************************************************************/
+/*  bezier_curve.h                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BEZIER_CURVE_H
+#define BEZIER_CURVE_H
+
+
+
+
+#endif // BEZIER_CURVE_H
diff --git a/core/math/bsp_tree.cpp b/core/math/bsp_tree.cpp
new file mode 100644
index 0000000000..7f838b1215
--- /dev/null
+++ b/core/math/bsp_tree.cpp
@@ -0,0 +1,628 @@
+/*************************************************************************/
+/*  bsp_tree.cpp                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "bsp_tree.h"
+#include "error_macros.h"
+#include "print_string.h"
+
+
+void BSP_Tree::from_aabb(const AABB& p_aabb) {
+
+	planes.clear();
+
+	for(int i=0;i<3;i++) {
+
+		Vector3 n;
+		n[i]=1;
+		planes.push_back(Plane(n,p_aabb.pos[i]+p_aabb.size[i]));
+		planes.push_back(Plane(-n,-p_aabb.pos[i]));
+	}
+
+	nodes.clear();
+
+	for(int i=0;i<6;i++) {
+
+		Node n;
+		n.plane=i;
+		n.under=(i==0)?UNDER_LEAF:i-1;
+		n.over=OVER_LEAF;
+		nodes.push_back(n);
+	}
+
+	aabb=p_aabb;
+	error_radius=0;
+}
+
+Vector<BSP_Tree::Node> BSP_Tree::get_nodes() const {
+
+	return nodes;
+}
+Vector<Plane> BSP_Tree::get_planes() const {
+
+	return planes;
+}
+	
+AABB BSP_Tree::get_aabb() const {
+
+	return aabb;
+}
+	
+int BSP_Tree::_get_points_inside(int p_node,const Vector3* p_points,int *p_indices, const Vector3& p_center,const Vector3& p_half_extents,int p_indices_count) const {
+
+
+	const Node *node =&nodes[p_node];
+	const Plane &p = planes[node->plane];
+
+	Vector3 min(
+			(p.normal.x>0) ? -p_half_extents.x : p_half_extents.x,
+			(p.normal.y>0) ? -p_half_extents.y : p_half_extents.y,
+			(p.normal.z>0) ? -p_half_extents.z : p_half_extents.z
+		);
+	Vector3 max=-min;
+	max+=p_center;
+	min+=p_center;
+
+	float dist_min = p.distance_to(min);
+	float dist_max = p.distance_to(max);
+
+	if ((dist_min * dist_max) < CMP_EPSILON ) { //intersection, test point by point
+
+
+		int under_count=0;
+
+		//sort points, so the are under first, over last
+		for(int i=0;i<p_indices_count;i++) {
+
+			int index=p_indices[i];
+
+			if (p.is_point_over(p_points[index])) {
+
+				// kind of slow (but cache friendly), should try something else,
+				// but this is a corner case most of the time
+
+				for(int j=index;j<p_indices_count-1;j++)
+					p_indices[j]=p_indices[j+1];
+
+				p_indices[p_indices_count-1]=index;
+
+			} else {
+				under_count++;
+			}
+
+		}
+
+		int total=0;
+
+		if (under_count>0) {
+			if (node->under==UNDER_LEAF) {
+				total+=under_count;
+			} else {
+				total+=_get_points_inside(node->under,p_points,p_indices,p_center,p_half_extents,under_count);
+			}
+		}
+
+		if (under_count!=p_indices_count) {
+			if (node->over==OVER_LEAF) {
+				//total+=0 //if they are over an OVER_LEAF, they are outside the model
+			} else {
+				total+=_get_points_inside(node->over,p_points,&p_indices[under_count],p_center,p_half_extents,p_indices_count-under_count);
+			}
+		}
+
+		return total;
+
+	} else if (dist_min > 0 ) { //all points over plane
+
+		if (node->over==OVER_LEAF) {
+
+			return 0; // all these points are not visible
+		}
+
+
+		return _get_points_inside(node->over,p_points,p_indices,p_center,p_half_extents,p_indices_count);
+	} else if (dist_min <= 0 ) { //all points behind plane
+
+		if (node->under==UNDER_LEAF) {
+
+			return p_indices_count; // all these points are visible
+		}
+		return _get_points_inside(node->under,p_points,p_indices,p_center,p_half_extents,p_indices_count);
+	}
+
+	return 0;
+}
+
+int BSP_Tree::get_points_inside(const Vector3* p_points,int p_point_count) const {
+
+
+	if (nodes.size()==0)
+		return 0;
+
+#if 1
+//this version is easier to debug, and and MUCH faster in real world cases
+
+	int pass_count = 0;
+	const Node *nodesptr=&nodes[0];
+	const Plane *planesptr=&planes[0];
+	int plane_count=planes.size();
+	int node_count=nodes.size();
+
+	if (node_count==0) // no nodes!
+		return 0;
+
+	for(int i=0;i<p_point_count;i++) {
+
+		const Vector3& point = p_points[i];
+		if (!aabb.has_point(point)) {
+			continue;
+		}
+
+		int idx=node_count-1;
+
+		bool pass=false;
+
+		while(true) {
+
+			if (idx==OVER_LEAF) {
+				pass=false;
+				break;
+			} else if (idx==UNDER_LEAF) {
+				pass=true;
+				break;
+			}
+
+			uint16_t plane=nodesptr[ idx ].plane;
+#ifdef DEBUG_ENABLED
+
+			ERR_FAIL_INDEX_V( plane, plane_count, false );
+#endif
+
+			idx = planesptr[ nodesptr[ idx ].plane ].is_point_over(point) ? nodes[ idx ].over : nodes[ idx ].under;
+
+#ifdef DEBUG_ENABLED
+
+			ERR_FAIL_COND_V( idx<MAX_NODES && idx>=node_count, false );
+#endif
+
+		}
+
+		if (pass)
+			pass_count++;
+	}
+
+	return pass_count;
+
+#else
+//this version scales better but it's slower for real world cases
+
+	int *indices = (int*)alloca(p_point_count*sizeof(int));
+	AABB bounds;
+
+	for(int i=0;i<p_point_count;i++) {
+
+		indices[i]=i;
+		if (i==0)
+			bounds.pos=p_points[i];
+		else
+			bounds.expand_to(p_points[i]);
+
+	}
+
+	Vector3 half_extents = bounds.size/2.0;
+	return _get_points_inside(nodes.size()+1,p_points,indices,bounds.pos+half_extents,half_extents,p_point_count);
+#endif
+}
+
+
+
+bool BSP_Tree::point_is_inside(const Vector3& p_point) const {
+
+	if (!aabb.has_point(p_point)) {
+		return false;
+	}
+	
+	int node_count=nodes.size();
+	
+	if (node_count==0) // no nodes!
+		return false;
+	
+	
+	const Node *nodesptr=&nodes[0];
+	const Plane *planesptr=&planes[0];
+	int plane_count=planes.size();
+		
+	int idx=node_count-1;
+	int steps=0;
+	
+	while(true) {
+	
+		if (idx==OVER_LEAF) {
+			return false;
+		}
+		if (idx==UNDER_LEAF) {
+
+			return true;
+		}
+			
+		uint16_t plane=nodesptr[ idx ].plane;
+#ifdef DEBUG_ENABLED
+			
+		ERR_FAIL_INDEX_V( plane, plane_count, false );
+#endif
+		bool over = planesptr[ nodesptr[ idx ].plane ].is_point_over(p_point);
+
+		idx =  over ? nodes[ idx ].over : nodes[ idx ].under;
+		
+#ifdef DEBUG_ENABLED
+			
+		ERR_FAIL_COND_V( idx<MAX_NODES && idx>=node_count, false );
+#endif
+		
+		steps++;
+	}
+
+	return false;
+}
+	
+	
+static int _bsp_find_best_half_plane(const Face3* p_faces,const Vector<int>& p_indices,float p_tolerance) {
+
+	int ic = p_indices.size();
+	const int*indices=p_indices.ptr();
+
+	int best_plane = -1;
+	float best_plane_cost = 1e20;
+
+	// Loop to find the polygon that best divides the set.
+
+	for (int i=0;i<ic;i++) {
+
+		const Face3& f=p_faces[ indices[i] ];
+		Plane p = f.get_plane();
+		
+		int num_over=0,num_under=0,num_spanning=0;
+
+		for(int j=0;j<ic;j++) {
+
+			if (i==j)
+				continue;
+
+			const Face3& g=p_faces[ indices[j] ];
+			int over=0,under=0;
+
+			for(int k=0;k<3;k++) {
+
+				float d = p.distance_to(g.vertex[j]);
+
+				if (Math::abs(d)>p_tolerance) {
+
+					if (d > 0)
+						over++;
+					else
+						under++;
+				}
+
+			}
+
+			if (over && under)
+				num_spanning++;
+			else if (over)
+				num_over++;
+			else
+				num_under++;
+				
+		}
+
+
+
+		//double split_cost = num_spanning / (double) face_count;
+		double relation = Math::abs(num_over-num_under) / (double) ic;
+		
+		// being honest, i never found a way to add split cost to the mix in a meaninguful way
+		// in this engine, also, will likely be ignored anyway
+		
+		double plane_cost = /*split_cost +*/ relation;
+
+		//printf("plane %i, %i over, %i under, %i spanning, cost is %g\n",i,num_over,num_under,num_spanning,plane_cost);
+		if (plane_cost<best_plane_cost) {
+
+			best_plane=i;
+			best_plane_cost=plane_cost;
+		}
+
+	}
+
+	return best_plane;
+
+}
+	
+
+static int _bsp_create_node(const Face3 *p_faces,const Vector<int>& p_indices,Vector<Plane> &p_planes, Vector<BSP_Tree::Node> &p_nodes,float p_tolerance) {
+	
+	ERR_FAIL_COND_V( p_nodes.size() == BSP_Tree::MAX_NODES, -1 );
+
+	// should not reach here
+	ERR_FAIL_COND_V( p_indices.size() == 0, -1 )
+
+	int ic = p_indices.size();
+	const int*indices=p_indices.ptr();
+
+	int divisor_idx = _bsp_find_best_half_plane(p_faces,p_indices,p_tolerance);
+
+	// returned error
+	ERR_FAIL_COND_V( divisor_idx<0 , -1 );
+
+
+	Vector<int> faces_over;
+	Vector<int> faces_under;
+
+	Plane divisor_plane=p_faces[ indices[divisor_idx] ].get_plane();
+
+	for (int i=0;i<ic;i++) {
+
+		if (i==divisor_idx)
+			continue;
+			
+		const Face3& f=p_faces[ indices[i] ];
+
+		//if (f.get_plane().is_almost_like(divisor_plane))
+		//	continue;
+
+		int over_count=0;
+		int under_count=0;
+
+		for(int j=0;j<3;j++) {
+
+			float d = divisor_plane.distance_to(f.vertex[j]);
+			if (Math::abs(d)>p_tolerance) {
+
+				if (d > 0)
+					over_count++;
+				else
+					under_count++;
+			}
+		}
+
+		if (over_count)
+			faces_over.push_back( indices[i] );
+		if (under_count)
+			faces_under.push_back( indices[i] );
+
+	}
+
+
+	
+	uint16_t over_idx=BSP_Tree::OVER_LEAF,under_idx=BSP_Tree::UNDER_LEAF;
+
+	if (faces_over.size()>0) { //have facess above?
+
+		int idx = _bsp_create_node( p_faces, faces_over, p_planes, p_nodes,p_tolerance );
+		if (idx>=0)
+			over_idx=idx;
+	}
+
+	if (faces_under.size()>0) { //have facess above?
+
+		int idx = _bsp_create_node( p_faces,faces_under, p_planes, p_nodes,p_tolerance );
+		if (idx>=0)
+			under_idx=idx;
+	}
+
+	/* Create the node */
+	
+	// find existing divisor plane
+	int divisor_plane_idx=-1;
+	
+	
+	for (int i=0;i<p_planes.size();i++) {
+	
+		if (p_planes[i].is_almost_like( divisor_plane )) {
+			divisor_plane_idx=i;
+			break;
+		}
+	}
+
+	if (divisor_plane_idx==-1) {
+	
+		ERR_FAIL_COND_V( p_planes.size() == BSP_Tree::MAX_PLANES, -1 );
+		divisor_plane_idx=p_planes.size();
+		p_planes.push_back( divisor_plane );
+	}
+	
+	BSP_Tree::Node node;
+	node.plane=divisor_plane_idx;
+	node.under=under_idx;
+	node.over=over_idx;
+	
+	p_nodes.push_back(node);
+	
+	return p_nodes.size()-1;
+}
+	
+
+BSP_Tree::operator Variant() const {
+
+
+	Dictionary d;
+	d["error_radius"]=error_radius;
+
+	Vector<float> plane_values;
+	plane_values.resize(planes.size()*4);
+
+	for(int i=0;i<planes.size();i++) {
+
+		plane_values[i*4+0]=planes[i].normal.x;
+		plane_values[i*4+1]=planes[i].normal.y;
+		plane_values[i*4+2]=planes[i].normal.z;
+		plane_values[i*4+3]=planes[i].d;
+	}
+
+	d["planes"]=plane_values;
+
+	DVector<int> dst_nodes;
+	dst_nodes.resize(nodes.size()*3);
+
+	for(int i=0;i<nodes.size();i++) {
+
+		dst_nodes.set(i*3+0,nodes[i].over);
+		dst_nodes.set(i*3+1,nodes[i].under);
+		dst_nodes.set(i*3+2,nodes[i].plane);
+	}
+
+
+	d["nodes"]=dst_nodes;
+	d["aabb"] = aabb;
+
+	return Variant(d);
+}
+
+BSP_Tree::BSP_Tree() {
+
+}
+
+
+BSP_Tree::BSP_Tree(const Variant& p_variant) {
+
+	Dictionary d=p_variant;
+	ERR_FAIL_COND(!d.has("nodes"));
+	ERR_FAIL_COND(!d.has("planes"));
+	ERR_FAIL_COND(!d.has("aabb"));
+	ERR_FAIL_COND(!d.has("error_radius"));
+
+	DVector<int> src_nodes = d["nodes"];
+	ERR_FAIL_COND(src_nodes.size()%3);
+
+
+	if (d["planes"].get_type()==Variant::REAL_ARRAY) {
+
+		DVector<float> src_planes=d["planes"];
+		int plane_count=src_planes.size();
+		ERR_FAIL_COND(plane_count%4);
+		planes.resize(plane_count/4);
+
+		if (plane_count) {
+			DVector<float>::Read r = src_planes.read();
+			for(int i=0;i<plane_count/4;i++) {
+
+				planes[i].normal.x=r[i*4+0];
+				planes[i].normal.y=r[i*4+1];
+				planes[i].normal.z=r[i*4+2];
+				planes[i].d=r[i*4+3];
+			}
+		}
+
+
+	} else {
+
+		planes = d["planes"];
+	}
+
+
+	error_radius = d["error"];
+	aabb = d["aabb"];
+
+//	int node_count = src_nodes.size();
+	nodes.resize(src_nodes.size()/3);
+
+	DVector<int>::Read r = src_nodes.read();
+
+	for(int i=0;i<nodes.size();i++) {
+
+		nodes[i].over=r[i*3+0];
+		nodes[i].under=r[i*3+1];
+		nodes[i].plane=r[i*3+2];
+	}
+
+}
+
+BSP_Tree::BSP_Tree(const DVector<Face3>& p_faces,float p_error_radius) {
+
+	// compute aabb
+		
+	int face_count=p_faces.size();
+	DVector<Face3>::Read faces_r=p_faces.read();
+	const Face3 *facesptr = faces_r.ptr();
+
+
+	bool first=true;
+
+	Vector<int> indices;
+
+	for (int i=0;i<face_count;i++) {
+			
+		const Face3& f=facesptr[i];
+		
+		if (f.is_degenerate())
+			continue;
+			
+		for (int j=0;j<3;j++) {
+		
+			if (first) {
+			
+				aabb.pos=f.vertex[0];
+				first=false;
+			} else {
+			
+				aabb.expand_to(f.vertex[j]);
+			}
+		}
+
+		indices.push_back(i);
+		
+	}
+
+	ERR_FAIL_COND( aabb.has_no_area() );
+
+	int top = _bsp_create_node(faces_r.ptr(),indices,planes,nodes,aabb.get_longest_axis_size()*0.0001);
+
+	if (top<0) {
+
+		nodes.clear();
+		planes.clear();
+		ERR_FAIL_COND( top < 0 );
+	}
+
+
+
+	
+	error_radius=p_error_radius;
+}
+
+BSP_Tree::BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const AABB& p_aabb,float p_error_radius) {
+
+	nodes=p_nodes;
+	planes=p_planes;
+	aabb=p_aabb;
+	error_radius=p_error_radius;
+
+}
+
+BSP_Tree::~BSP_Tree() {
+
+
+}
diff --git a/core/math/bsp_tree.h b/core/math/bsp_tree.h
new file mode 100644
index 0000000000..03bfd947cb
--- /dev/null
+++ b/core/math/bsp_tree.h
@@ -0,0 +1,141 @@
+/*************************************************************************/
+/*  bsp_tree.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BSP_TREE_H
+#define BSP_TREE_H
+
+#include "plane.h"
+#include "aabb.h"
+#include "face3.h"
+#include "vector.h"
+#include "dvector.h"
+
+#include "variant.h"
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+class BSP_Tree {
+public:
+
+	enum {
+	
+		UNDER_LEAF=0xFFFF,
+		OVER_LEAF=0xFFFE,
+		MAX_NODES=0xFFFE,
+		MAX_PLANES=(1<<16)
+	};
+
+	struct Node {
+		
+		uint16_t plane;
+		uint16_t under;
+		uint16_t over;
+	};
+
+	
+private:	
+	// thanks to the properties of Vector, 
+	// this class can be assigned and passed around between threads
+	// with no cost.
+	
+	Vector<Node> nodes;
+	Vector<Plane> planes;
+	AABB aabb;
+	float error_radius;	
+
+	int _get_points_inside(int p_node,const Vector3* p_points,int *p_indices, const Vector3& p_center,const Vector3& p_half_extents,int p_indices_count) const;
+
+	template<class T>
+	bool _test_convex(const Node* p_nodes, const Plane* p_planes,int p_current, const T& p_convex) const;
+
+public:
+
+	bool is_empty() const { return nodes.size()==0; }
+	Vector<Node> get_nodes() const;
+	Vector<Plane> get_planes() const;
+	AABB get_aabb() const;
+		
+	bool point_is_inside(const Vector3& p_point) const;		
+	int get_points_inside(const Vector3* p_points, int p_point_count) const;
+	template<class T>
+	bool convex_is_inside(const T& p_convex) const;
+
+	operator Variant() const;
+
+	void from_aabb(const AABB& p_aabb);
+
+	BSP_Tree();
+	BSP_Tree(const Variant& p_variant);
+	BSP_Tree(const DVector<Face3>& p_faces,float p_error_radius=0);	
+	BSP_Tree(const Vector<Node> &p_nodes, const Vector<Plane> &p_planes, const AABB& p_aabb,float p_error_radius=0);	
+	~BSP_Tree();
+
+};
+
+template<class T>
+bool BSP_Tree::_test_convex(const Node* p_nodes, const Plane* p_planes,int p_current, const T& p_convex) const {
+
+	if (p_current==UNDER_LEAF)
+		return true;
+	else if (p_current==OVER_LEAF)
+		return false;
+
+	bool collided=false;
+	const Node&n=p_nodes[p_current];
+
+	const Plane& p=p_planes[n.plane];
+
+	float min,max;
+	p_convex.project_range(p.normal,min,max);
+
+	bool go_under = min < p.d;
+	bool go_over = max >= p.d;
+
+	if (go_under && _test_convex(p_nodes,p_planes,n.under,p_convex))
+		collided=true;
+	if (go_over && _test_convex(p_nodes,p_planes,n.over,p_convex))
+		collided=true;
+
+	return collided;
+
+}
+
+template<class T>
+bool BSP_Tree::convex_is_inside(const T& p_convex) const {
+
+	int node_count = nodes.size();
+	if (node_count==0)
+		return false;
+	const Node* nodes=&this->nodes[0];
+	const Plane* planes = &this->planes[0];
+
+	return _test_convex(nodes,planes,node_count-1,p_convex);
+}
+
+
+#endif
diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp
new file mode 100644
index 0000000000..52d77b6ebc
--- /dev/null
+++ b/core/math/camera_matrix.cpp
@@ -0,0 +1,596 @@
+/*************************************************************************/
+/*  camera_matrix.cpp                                                    */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "camera_matrix.h"
+#include "math_funcs.h"
+#include "print_string.h"
+
+void CameraMatrix::set_identity() {
+
+	for (int i=0;i<4;i++) {
+	
+		for (int j=0;j<4;j++) {
+		
+			matrix[i][j]=(i==j)?1:0;
+		}
+	}
+}
+
+
+void CameraMatrix::set_zero() {
+
+	for (int i=0;i<4;i++) {
+
+		for (int j=0;j<4;j++) {
+
+			matrix[i][j]=0;
+		}
+	}
+}
+
+
+Plane CameraMatrix::xform4(const Plane& p_vec4) {
+
+	Plane ret;
+
+	ret.normal.x = matrix[0][0] * p_vec4.normal.x + matrix[1][0] * p_vec4.normal.y + matrix[2][0] * p_vec4.normal.z + matrix[3][0] * p_vec4.d;
+	ret.normal.y = matrix[0][1] * p_vec4.normal.x + matrix[1][1] * p_vec4.normal.y + matrix[2][1] * p_vec4.normal.z + matrix[3][1] * p_vec4.d;
+	ret.normal.z = matrix[0][2] * p_vec4.normal.x + matrix[1][2] * p_vec4.normal.y + matrix[2][2] * p_vec4.normal.z + matrix[3][2] * p_vec4.d;
+	ret.d = matrix[0][3] * p_vec4.normal.x + matrix[1][3] * p_vec4.normal.y + matrix[2][3] * p_vec4.normal.z + matrix[3][3] * p_vec4.d;
+	return ret;
+}
+
+void CameraMatrix::set_perspective(float p_fovy_degrees, float p_aspect, float p_z_near, float p_z_far,bool p_flip_fov) {
+
+	if (p_flip_fov)
+		p_fovy_degrees=get_fovy(p_fovy_degrees,p_aspect);
+
+
+	float sine, cotangent, deltaZ;
+	float radians = p_fovy_degrees / 2.0 * Math_PI / 180.0;
+
+	deltaZ = p_z_far - p_z_near;
+	sine = Math::sin(radians);
+
+		if ((deltaZ == 0) || (sine == 0) || (p_aspect == 0)) {
+		return ;
+	}
+	cotangent = Math::cos(radians) / sine;
+
+	set_identity();
+
+	matrix[0][0] = cotangent / p_aspect;
+	matrix[1][1] = cotangent;
+	matrix[2][2] = -(p_z_far + p_z_near) / deltaZ;
+	matrix[2][3] = -1;
+	matrix[3][2] = -2 * p_z_near * p_z_far / deltaZ;
+	matrix[3][3] = 0;	
+
+}
+
+void CameraMatrix::set_orthogonal(float p_left, float p_right, float p_bottom, float p_top,  float p_znear, float p_zfar) {
+
+
+	set_identity();
+
+	matrix[0][0] = 2.0/(p_right-p_left);
+	matrix[3][0] = -((p_right+p_left)/(p_right-p_left));
+	matrix[1][1] = 2.0/(p_top-p_bottom);
+	matrix[3][1] = -((p_top+p_bottom)/(p_top-p_bottom));
+	matrix[2][2] = -2.0/(p_zfar-p_znear);
+	matrix[3][2] = -((p_zfar+p_znear)/(p_zfar-p_znear));
+	matrix[3][3] = 1.0;
+
+}
+
+void CameraMatrix::set_orthogonal(float p_size, float p_aspect, float p_znear, float p_zfar,bool p_flip_fov) {
+
+	if (p_flip_fov) {
+		p_size*=p_aspect;
+	}
+
+	set_orthogonal(-p_size/2,+p_size/2,-p_size/p_aspect/2,+p_size/p_aspect/2,p_znear,p_zfar);
+}
+
+
+
+void CameraMatrix::set_frustum(float p_left, float p_right, float p_bottom, float p_top, float p_near, float p_far) {
+
+	///@TODO, give a check to this. I'm not sure if it's working.
+	set_identity();
+
+	matrix[0][0]=(2*p_near) / (p_right-p_left);
+	matrix[0][2]=(p_right+p_left) / (p_right-p_left);
+	matrix[1][1]=(2*p_near) / (p_top-p_bottom);
+	matrix[1][2]=(p_top+p_bottom) / (p_top-p_bottom);
+	matrix[2][2]=-(p_far+p_near) / ( p_far-p_near);
+	matrix[2][3]=-(2*p_far*p_near) / (p_far-p_near);
+	matrix[3][2]=-1;
+	matrix[3][3]=0;
+
+}
+
+
+float CameraMatrix::get_z_far() const {
+	
+	const float * matrix = (const float*)this->matrix;			
+	Plane new_plane=Plane(matrix[ 3] - matrix[ 2],
+	                matrix[ 7] - matrix[ 6],
+	                matrix[11] - matrix[10],
+	                matrix[15] - matrix[14]);
+	
+	new_plane.normal=-new_plane.normal;
+	new_plane.normalize();
+	
+	return new_plane.d;
+}
+float CameraMatrix::get_z_near() const {
+	
+	const float * matrix = (const float*)this->matrix;			
+	Plane new_plane=Plane(matrix[ 3] + matrix[ 2],
+	                matrix[ 7] + matrix[ 6],
+	                matrix[11] + matrix[10],
+	               -matrix[15] - matrix[14]);
+	
+	new_plane.normalize();
+	return new_plane.d;
+}
+
+void CameraMatrix::get_viewport_size(float& r_width, float& r_height) const {
+
+	const float * matrix = (const float*)this->matrix;		
+	///////--- Near Plane ---///////
+	Plane near_plane=Plane(matrix[ 3] + matrix[ 2],
+	                matrix[ 7] + matrix[ 6],
+	                matrix[11] + matrix[10],
+			-matrix[15] - matrix[14]).normalized();
+		
+	///////--- Right Plane ---///////
+	Plane right_plane=Plane(matrix[ 3] - matrix[ 0],
+	                matrix[ 7] - matrix[ 4],
+	                matrix[11] - matrix[ 8],
+			- matrix[15] + matrix[12]).normalized();
+	
+	Plane top_plane=Plane(matrix[ 3] - matrix[ 1],
+	                matrix[ 7] - matrix[ 5],
+	                matrix[11] - matrix[ 9],
+			-matrix[15] + matrix[13]).normalized();
+	
+	Vector3 res;
+	near_plane.intersect_3(right_plane,top_plane,&res);
+	
+	r_width=res.x;
+	r_height=res.y;
+}
+
+bool CameraMatrix::get_endpoints(const Transform& p_transform, Vector3 *p_8points) const {
+
+	const float * matrix = (const float*)this->matrix;		
+		
+	///////--- Near Plane ---///////
+	Plane near_plane=Plane(matrix[ 3] + matrix[ 2],
+	                matrix[ 7] + matrix[ 6],
+	                matrix[11] + matrix[10],
+			-matrix[15] - matrix[14]).normalized();
+	
+	///////--- Far Plane ---///////
+	Plane far_plane=Plane(matrix[ 2] - matrix[ 3],
+		      matrix[ 6] - matrix[ 7],
+		      matrix[10] - matrix[11],
+		      matrix[15] - matrix[14]).normalized();
+
+		
+	///////--- Right Plane ---///////
+	Plane right_plane=Plane(matrix[ 0] - matrix[ 3],
+	                matrix[ 4] - matrix[ 7],
+	                matrix[8] - matrix[ 11],
+			- matrix[15] + matrix[12]).normalized();
+	
+	///////--- Top Plane ---///////	
+	Plane top_plane=Plane(matrix[ 1] - matrix[ 3],
+	                matrix[ 5] - matrix[ 7],
+	                matrix[9] - matrix[ 11],
+			-matrix[15] + matrix[13]).normalized();
+		
+	Vector3 near_endpoint;
+	Vector3 far_endpoint;
+	
+	bool res=near_plane.intersect_3(right_plane,top_plane,&near_endpoint);
+	ERR_FAIL_COND_V(!res,false);
+	
+	res=far_plane.intersect_3(right_plane,top_plane,&far_endpoint);
+	ERR_FAIL_COND_V(!res,false);
+	
+	p_8points[0]=p_transform.xform( Vector3( near_endpoint.x, near_endpoint.y, near_endpoint.z ) );
+	p_8points[1]=p_transform.xform( Vector3( near_endpoint.x,-near_endpoint.y, near_endpoint.z ) );
+	p_8points[2]=p_transform.xform( Vector3(-near_endpoint.x, near_endpoint.y, near_endpoint.z ) );
+	p_8points[3]=p_transform.xform( Vector3(-near_endpoint.x,-near_endpoint.y, near_endpoint.z ) );
+	p_8points[4]=p_transform.xform( Vector3( far_endpoint.x, far_endpoint.y, far_endpoint.z ) );
+	p_8points[5]=p_transform.xform( Vector3( far_endpoint.x,-far_endpoint.y, far_endpoint.z ) );
+	p_8points[6]=p_transform.xform( Vector3(-far_endpoint.x, far_endpoint.y, far_endpoint.z ) );
+	p_8points[7]=p_transform.xform( Vector3(-far_endpoint.x,-far_endpoint.y, far_endpoint.z ) );
+	
+	return true;
+}
+
+Vector<Plane> CameraMatrix::get_projection_planes(const Transform& p_transform) const {
+
+	/** Fast Plane Extraction from combined modelview/projection matrices.
+	 * References:
+	 * http://www.markmorley.com/opengl/frustumculling.html
+	 * http://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf
+	 */
+	 
+	Vector<Plane> planes;
+	
+	const float * matrix = (const float*)this->matrix;			
+
+	Plane new_plane;
+
+	///////--- Near Plane ---///////
+	new_plane=Plane(matrix[ 3] + matrix[ 2],
+		      matrix[ 7] + matrix[ 6],
+		      matrix[11] + matrix[10],
+		      matrix[15] + matrix[14]);
+	
+	new_plane.normal=-new_plane.normal;
+	new_plane.normalize();
+
+	planes.push_back( p_transform.xform(new_plane) );
+
+	///////--- Far Plane ---///////
+	new_plane=Plane(matrix[ 3] - matrix[ 2],
+		      matrix[ 7] - matrix[ 6],
+		      matrix[11] - matrix[10],
+		      matrix[15] - matrix[14]);
+
+	new_plane.normal=-new_plane.normal;
+	new_plane.normalize();
+
+	planes.push_back( p_transform.xform(new_plane) );
+
+
+	///////--- Left Plane ---///////
+	new_plane=Plane(matrix[ 3] + matrix[ 0],
+		      matrix[ 7] + matrix[ 4],
+		      matrix[11] + matrix[ 8],
+		      matrix[15] + matrix[12]);
+		      
+	new_plane.normal=-new_plane.normal;
+	new_plane.normalize();
+
+	planes.push_back( p_transform.xform(new_plane) );
+
+
+	///////--- Top Plane ---///////
+	new_plane=Plane(matrix[ 3] - matrix[ 1],
+		      matrix[ 7] - matrix[ 5],
+		      matrix[11] - matrix[ 9],
+		      matrix[15] - matrix[13]);
+		      
+	
+	new_plane.normal=-new_plane.normal;
+	new_plane.normalize();
+
+	planes.push_back( p_transform.xform(new_plane) );
+
+
+	///////--- Right Plane ---///////
+	new_plane=Plane(matrix[ 3] - matrix[ 0],
+		      matrix[ 7] - matrix[ 4],
+		      matrix[11] - matrix[ 8],
+		      matrix[15] - matrix[12]);
+
+	
+	new_plane.normal=-new_plane.normal;
+	new_plane.normalize();
+	
+	planes.push_back( p_transform.xform(new_plane) );
+
+
+	///////--- Bottom Plane ---///////
+	new_plane=Plane(matrix[ 3] + matrix[ 1],
+		      matrix[ 7] + matrix[ 5],
+		      matrix[11] + matrix[ 9],
+		      matrix[15] + matrix[13]);
+
+	
+	new_plane.normal=-new_plane.normal;
+	new_plane.normalize();
+
+	planes.push_back( p_transform.xform(new_plane) );
+	
+	return planes;
+}
+
+
+
+CameraMatrix CameraMatrix::inverse() const {
+
+	CameraMatrix cm = *this;
+	cm.invert();
+	return cm;
+}
+
+void CameraMatrix::invert() {
+	
+	int i,j,k;
+	int pvt_i[4], pvt_j[4];            /* Locations of pivot matrix */
+	float pvt_val;                     /* Value of current pivot element */
+	float hold;                        /* Temporary storage */
+	float determinat;                  /* Determinant */
+
+	determinat = 1.0;
+	for (k=0; k<4; k++)  {
+		/** Locate k'th pivot element **/
+		pvt_val=matrix[k][k];            /** Initialize for search **/
+		pvt_i[k]=k;
+		pvt_j[k]=k;
+		for (i=k; i<4; i++) {
+			for (j=k; j<4; j++) {
+				if (Math::absd(matrix[i][j]) > Math::absd(pvt_val)) {
+					pvt_i[k]=i;
+					pvt_j[k]=j;
+					pvt_val=matrix[i][j];
+				}
+			}
+		}
+
+		/** Product of pivots, gives determinant when finished **/
+		determinat*=pvt_val;
+		if (Math::absd(determinat)<1e-7) {
+			return; //(false);  /** Matrix is singular (zero determinant). **/
+		}
+
+		/** "Interchange" rows (with sign change stuff) **/
+		i=pvt_i[k];
+		if (i!=k) {               /** If rows are different **/
+			for (j=0; j<4; j++) {
+				hold=-matrix[k][j];
+				matrix[k][j]=matrix[i][j];
+				matrix[i][j]=hold;
+			}
+		}
+
+		/** "Interchange" columns **/
+		j=pvt_j[k];
+		if (j!=k) {              /** If columns are different **/
+			for (i=0; i<4; i++) {
+				hold=-matrix[i][k];
+				matrix[i][k]=matrix[i][j];
+				matrix[i][j]=hold;
+			}
+		}
+
+		/** Divide column by minus pivot value **/
+		for (i=0; i<4; i++) {
+			if (i!=k) matrix[i][k]/=( -pvt_val) ;
+		}
+
+		/** Reduce the matrix **/
+		for (i=0; i<4; i++) {
+			hold = matrix[i][k];
+			for (j=0; j<4; j++) {
+				if (i!=k && j!=k) matrix[i][j]+=hold*matrix[k][j];
+			}
+		}
+
+		/** Divide row by pivot **/
+		for (j=0; j<4; j++) {
+			if (j!=k) matrix[k][j]/=pvt_val;
+		}
+
+		/** Replace pivot by reciprocal (at last we can touch it). **/
+		matrix[k][k] = 1.0/pvt_val;
+	}
+
+	/* That was most of the work, one final pass of row/column interchange */
+	/* to finish */
+	for (k=4-2; k>=0; k--) { /* Don't need to work with 1 by 1 corner*/
+		i=pvt_j[k];            /* Rows to swap correspond to pivot COLUMN */
+		if (i!=k) {            /* If rows are different */
+			for(j=0; j<4; j++) {
+				hold = matrix[k][j];
+				matrix[k][j]=-matrix[i][j];
+				matrix[i][j]=hold;
+			}
+		}
+
+		j=pvt_i[k];           /* Columns to swap correspond to pivot ROW */
+		if (j!=k)             /* If columns are different */
+			for (i=0; i<4; i++) {
+			hold=matrix[i][k];
+			matrix[i][k]=-matrix[i][j];
+			matrix[i][j]=hold;
+			}
+	}
+
+	
+}
+
+CameraMatrix::CameraMatrix() {
+
+	set_identity();
+}
+
+CameraMatrix CameraMatrix::operator*(const CameraMatrix& p_matrix) const {
+
+	CameraMatrix new_matrix;
+
+	for( int j = 0; j < 4; j++ ) {
+		for( int i = 0; i < 4; i++ ) {
+			real_t ab = 0;
+			for( int k = 0; k < 4; k++ )
+				ab += matrix[k][i] * p_matrix.matrix[j][k] ;
+			new_matrix.matrix[j][i] = ab;
+		}
+	}
+
+	return new_matrix;
+}
+
+void CameraMatrix::set_light_bias() {
+
+	float *m=&matrix[0][0];
+	
+	m[0]=0.5, 
+	m[1]=0.0, 
+	m[2]=0.0, 
+	m[3]=0.0,
+	m[4]=0.0, 
+	m[5]=0.5, 
+	m[6]=0.0, 
+	m[7]=0.0,
+	m[8]=0.0, 
+	m[9]=0.0, 
+	m[10]=0.5, 
+	m[11]=0.0,
+	m[12]=0.5, 
+	m[13]=0.5, 
+	m[14]=0.5, 
+	m[15]=1.0;	
+		
+}
+
+CameraMatrix::operator String() const {
+
+	String str;
+	for (int i=0;i<4;i++) 
+		for (int j=0;j<4;j++) 
+			str+=String((j>0)?", ":"\n")+rtos(matrix[i][j]);
+
+	return str;
+}
+
+float CameraMatrix::get_aspect() const {
+
+	float w,h;
+	get_viewport_size(w,h);
+	return w/h;
+}
+
+float CameraMatrix::get_fov() const {
+	const float * matrix = (const float*)this->matrix;		
+
+	Plane right_plane=Plane(matrix[ 3] - matrix[ 0],
+			matrix[ 7] - matrix[ 4],
+			matrix[11] - matrix[ 8],
+			- matrix[15] + matrix[12]).normalized();
+
+	return Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x)))*2.0;
+}
+
+
+void CameraMatrix::make_scale(const Vector3 &p_scale) {
+
+	set_identity();
+	matrix[0][0]=p_scale.x;
+	matrix[1][1]=p_scale.y;
+	matrix[2][2]=p_scale.z;
+
+}
+
+void CameraMatrix::scale_translate_to_fit(const AABB& p_aabb) {
+
+	Vector3 min = p_aabb.pos;
+	Vector3 max = p_aabb.pos+p_aabb.size;
+
+
+	matrix[0][0]=2/(max.x-min.x);
+	matrix[1][0]=0;
+	matrix[2][0]=0;
+	matrix[3][0]=-(max.x+min.x)/(max.x-min.x);
+
+	matrix[0][1]=0;
+	matrix[1][1]=2/(max.y-min.y);
+	matrix[2][1]=0;
+	matrix[3][1]=-(max.y+min.y)/(max.y-min.y);
+
+	matrix[0][2]=0;
+	matrix[1][2]=0;
+	matrix[2][2]=2/(max.z-min.z);
+	matrix[3][2]=-(max.z+min.z)/(max.z-min.z);
+
+	matrix[0][3]=0;
+	matrix[1][3]=0;
+	matrix[2][3]=0;
+	matrix[3][3]=1;
+}
+
+CameraMatrix::operator Transform() const {
+
+	Transform tr;
+	const float *m=&matrix[0][0];
+
+	tr.basis.elements[0][0]=m[0];
+	tr.basis.elements[1][0]=m[1];
+	tr.basis.elements[2][0]=m[2];
+
+	tr.basis.elements[0][1]=m[4];
+	tr.basis.elements[1][1]=m[5];
+	tr.basis.elements[2][1]=m[6];
+
+	tr.basis.elements[0][2]=m[8];
+	tr.basis.elements[1][2]=m[9];
+	tr.basis.elements[2][2]=m[10];
+
+	tr.origin.x=m[12];
+	tr.origin.y=m[13];
+	tr.origin.z=m[14];
+
+	return tr;
+}
+
+CameraMatrix::CameraMatrix(const Transform& p_transform) {
+
+	const Transform &tr = p_transform;
+	float *m=&matrix[0][0];
+	
+	m[0]=tr.basis.elements[0][0];
+	m[1]=tr.basis.elements[1][0];
+	m[2]=tr.basis.elements[2][0];
+	m[3]=0.0;
+	m[4]=tr.basis.elements[0][1];
+	m[5]=tr.basis.elements[1][1];
+	m[6]=tr.basis.elements[2][1];
+	m[7]=0.0;
+	m[8]=tr.basis.elements[0][2];
+	m[9]=tr.basis.elements[1][2];
+	m[10]=tr.basis.elements[2][2];
+	m[11]=0.0;
+  	m[12]=tr.origin.x;
+	m[13]=tr.origin.y;
+	m[14]=tr.origin.z;
+	m[15]=1.0;		
+}
+
+CameraMatrix::~CameraMatrix()
+{
+}
+
+
diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h
new file mode 100644
index 0000000000..6ffcb0ed0b
--- /dev/null
+++ b/core/math/camera_matrix.h
@@ -0,0 +1,106 @@
+/*************************************************************************/
+/*  camera_matrix.h                                                      */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef CAMERA_MATRIX_H
+#define CAMERA_MATRIX_H
+
+#include "transform.h"
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+
+
+
+struct CameraMatrix {
+
+	enum Planes {
+		PLANE_NEAR,
+		PLANE_FAR,
+		PLANE_LEFT,
+		PLANE_TOP,
+		PLANE_RIGHT,
+		PLANE_BOTTOM
+	};
+
+ 	float matrix[4][4];
+		
+		
+	void set_identity();
+	void set_zero();
+	void set_light_bias();
+	void set_perspective(float p_fovy_degrees, float p_aspect, float p_z_near, float p_z_far,bool p_flip_fov=false);
+	void set_orthogonal(float p_left, float p_right, float p_bottom, float p_top,  float p_znear, float p_zfar);
+	void set_orthogonal(float p_size, float p_aspect, float p_znear, float p_zfar,bool p_flip_fov=false);
+	void set_frustum(float p_left, float p_right, float p_bottom, float p_top, float p_near, float p_far);
+
+	static float get_fovy(float p_fovx,float p_aspect) {
+
+		return Math::atan(p_aspect * Math::tan(p_fovx * 0.5))*2.0;
+	}
+
+	float get_z_far() const;
+	float get_z_near() const;
+	float get_aspect() const;
+	float get_fov() const;
+	
+	Vector<Plane> get_projection_planes(const Transform& p_transform) const;
+	
+	bool get_endpoints(const Transform& p_transform,Vector3 *p_8points) const;
+	void get_viewport_size(float& r_width, float& r_height) const;
+	
+	void invert();
+	CameraMatrix inverse() const;
+
+	CameraMatrix operator*(const CameraMatrix& p_matrix) const;
+
+	Plane xform4(const Plane& p_vec4);
+	_FORCE_INLINE_ Vector3 xform(const Vector3& p_vec3) const;
+	
+	operator String() const;
+
+	void scale_translate_to_fit(const AABB& p_aabb);
+	void make_scale(const Vector3 &p_scale);
+	operator Transform() const;
+
+	CameraMatrix();	
+	CameraMatrix(const Transform& p_transform);	
+	~CameraMatrix();
+
+};
+
+Vector3 CameraMatrix::xform(const Vector3& p_vec3) const {
+
+	Vector3 ret;
+	ret.x = matrix[0][0] * p_vec3.x + matrix[1][0] * p_vec3.y + matrix[2][0] * p_vec3.z + matrix[3][0];
+	ret.y = matrix[0][1] * p_vec3.x + matrix[1][1] * p_vec3.y + matrix[2][1] * p_vec3.z + matrix[3][1];
+	ret.z = matrix[0][2] * p_vec3.x + matrix[1][2] * p_vec3.y + matrix[2][2] * p_vec3.z + matrix[3][2];
+	float w = matrix[0][3] * p_vec3.x + matrix[1][3] * p_vec3.y + matrix[2][3] * p_vec3.z + matrix[3][3];
+	return ret/w;
+}
+
+#endif
diff --git a/core/math/face3.cpp b/core/math/face3.cpp
new file mode 100644
index 0000000000..9cdf31ed84
--- /dev/null
+++ b/core/math/face3.cpp
@@ -0,0 +1,359 @@
+/*************************************************************************/
+/*  face3.cpp                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "face3.h"
+#include "geometry.h"
+
+int Face3::split_by_plane(const Plane& p_plane,Face3 p_res[3],bool p_is_point_over[3]) const {
+
+	ERR_FAIL_COND_V(is_degenerate(),0);
+
+
+        Vector3 above[4];
+	int above_count=0;
+
+	Vector3 below[4];
+	int below_count=0;
+
+	for (int i=0;i<3;i++) {
+
+		if (p_plane.has_point( vertex[i], CMP_EPSILON )) { // point is in plane
+
+			ERR_FAIL_COND_V(above_count>=4,0);
+			above[above_count++]=vertex[i];
+			ERR_FAIL_COND_V(below_count>=4,0);
+			below[below_count++]=vertex[i];
+
+		} else {
+
+			if (p_plane.is_point_over( vertex[i])) {
+				//Point is over
+				ERR_FAIL_COND_V(above_count>=4,0);
+				above[above_count++]=vertex[i];
+
+			} else {
+				//Point is under
+				ERR_FAIL_COND_V(below_count>=4,0);
+				below[below_count++]=vertex[i];
+			}
+
+			/* Check for Intersection between this and the next vertex*/
+
+			Vector3 inters;
+			if (!p_plane.intersects_segment( vertex[i],vertex[(i+1)%3],&inters))
+				continue;
+
+			/* Intersection goes to both */
+			ERR_FAIL_COND_V(above_count>=4,0);
+			above[above_count++]=inters;
+			ERR_FAIL_COND_V(below_count>=4,0);
+			below[below_count++]=inters;
+		}
+	}
+
+	int polygons_created=0;
+
+	ERR_FAIL_COND_V( above_count>=4 && below_count>=4 , 0 ); //bug in the algo
+
+	if (above_count>=3) {
+
+		p_res[polygons_created]=Face3( above[0], above[1], above[2] );
+		p_is_point_over[polygons_created]=true;
+		polygons_created++;
+
+		if (above_count==4) {
+
+			p_res[polygons_created]=Face3( above[2], above[3], above[0] );
+			p_is_point_over[polygons_created]=true;
+			polygons_created++;
+
+		}
+	}
+
+	if (below_count>=3) {
+
+		p_res[polygons_created]=Face3( below[0], below[1], below[2] );
+		p_is_point_over[polygons_created]=false;
+		polygons_created++;
+
+		if (below_count==4) {
+
+			p_res[polygons_created]=Face3( below[2], below[3], below[0] );
+			p_is_point_over[polygons_created]=false;
+			polygons_created++;
+
+		}
+	}
+
+	return polygons_created;
+}
+
+
+
+bool Face3::intersects_ray(const Vector3& p_from,const Vector3& p_dir,Vector3 * p_intersection) const {
+
+	return Geometry::ray_intersects_triangle(p_from,p_dir,vertex[0],vertex[1],vertex[2],p_intersection);
+
+}
+
+bool Face3::intersects_segment(const Vector3& p_from,const Vector3& p_dir,Vector3 * p_intersection) const {
+
+	return Geometry::segment_intersects_triangle(p_from,p_dir,vertex[0],vertex[1],vertex[2],p_intersection);
+
+}
+
+
+bool Face3::is_degenerate() const {
+
+	Vector3 normal=vec3_cross(vertex[0]-vertex[1], vertex[0]-vertex[2]);
+	return (normal.length_squared() < CMP_EPSILON2);
+}
+
+
+Face3::Side Face3::get_side_of(const Face3& p_face,ClockDirection p_clock_dir) const {
+
+	int over=0,under=0;
+
+	Plane plane=get_plane(p_clock_dir);
+
+	for (int i=0;i<3;i++) {
+
+		const Vector3 &v=p_face.vertex[i];
+
+		if (plane.has_point(v)) //coplanar, dont bother
+			continue;
+
+		if (plane.is_point_over(v))
+			over++;
+		else
+			under++;
+
+	}
+
+	if ( over > 0 && under == 0 )
+		return SIDE_OVER;
+	else if (under > 0 && over ==0 )
+		return SIDE_UNDER;
+	else if (under ==0 && over == 0)
+		return SIDE_COPLANAR;
+	else
+		return SIDE_SPANNING;
+
+}
+
+Vector3 Face3::get_random_point_inside() const {
+
+	float a=Math::random(0,1);
+	float b=Math::random(0,1);
+	if (a>b) {
+		SWAP(a,b);
+	}
+
+	return vertex[0]*a + vertex[1]*(b-a) + vertex[2]*(1.0-b);
+
+}
+
+Plane Face3::get_plane(ClockDirection p_dir) const {
+
+	return Plane( vertex[0], vertex[1], vertex[2] , p_dir );
+
+}
+
+Vector3 Face3::get_median_point() const {
+
+	return (vertex[0] + vertex[1] + vertex[2])/3.0;
+}
+
+
+real_t Face3::get_area() const {
+
+	return vec3_cross(vertex[0]-vertex[1], vertex[0]-vertex[2]).length();
+}
+
+ClockDirection Face3::get_clock_dir() const {
+
+
+	Vector3 normal=vec3_cross(vertex[0]-vertex[1], vertex[0]-vertex[2]);
+	//printf("normal is %g,%g,%g x %g,%g,%g- wtfu is %g\n",tofloat(normal.x),tofloat(normal.y),tofloat(normal.z),tofloat(vertex[0].x),tofloat(vertex[0].y),tofloat(vertex[0].z),tofloat( normal.dot( vertex[0] ) ) );
+	return ( normal.dot( vertex[0] ) >= 0 ) ? CLOCKWISE : COUNTERCLOCKWISE;
+
+}
+
+
+bool Face3::intersects_aabb(const AABB& p_aabb) const {
+
+	/** TEST PLANE **/
+	if (!p_aabb.intersects_plane( get_plane() ))
+		return false;
+
+	/** TEST FACE AXIS */
+
+#define TEST_AXIS(m_ax)\
+	{\
+		float aabb_min=p_aabb.pos.m_ax;\
+		float aabb_max=p_aabb.pos.m_ax+p_aabb.size.m_ax;\
+		float tri_min,tri_max;\
+		for (int i=0;i<3;i++) {\
+			if (i==0 || vertex[i].m_ax > tri_max)\
+				tri_max=vertex[i].m_ax;\
+			if (i==0 || vertex[i].m_ax < tri_min)\
+				tri_min=vertex[i].m_ax;\
+		}\
+\
+		if (tri_max<aabb_min || aabb_max<tri_min)\
+			return false;\
+	}
+
+	TEST_AXIS(x);
+	TEST_AXIS(y);
+	TEST_AXIS(z);
+
+	/** TEST ALL EDGES **/
+
+	Vector3 edge_norms[3]={
+		vertex[0]-vertex[1],
+		vertex[1]-vertex[2],
+		vertex[2]-vertex[0],
+	};
+
+	for (int i=0;i<12;i++) {
+
+		Vector3 from,to;
+		p_aabb.get_edge(i,from,to);
+		Vector3 e1=from-to;
+		for (int j=0;j<3;j++) {
+			Vector3 e2=edge_norms[i];
+
+			Vector3 axis=vec3_cross( e1, e2 );
+
+			if (axis.length_squared()<0.0001)
+				continue; // coplanar
+			axis.normalize();
+
+			float minA,maxA,minB,maxB;
+			p_aabb.project_range_in_plane(Plane(axis,0),minA,maxA);
+			project_range(axis,Transform(),minB,maxB);
+
+			if (maxA<minB || maxB<minA)
+				return false;
+		}
+	}
+	return true;
+
+}
+
+Face3::operator String() const {
+
+	return String()+vertex[0]+", "+vertex[1]+", "+vertex[2];
+}
+
+void Face3::project_range(const Vector3& p_normal,const Transform& p_transform,float& r_min, float& r_max) const {
+
+	for (int i=0;i<3;i++) {
+
+		Vector3 v=p_transform.xform(vertex[i]);
+		float d=p_normal.dot(v);
+
+		if (i==0 || d > r_max)
+			r_max=d;
+
+		if (i==0 || d < r_min)
+			r_min=d;
+	}
+}
+
+
+
+void Face3::get_support(const Vector3& p_normal,const Transform& p_transform,Vector3 *p_vertices,int* p_count,int p_max) const {
+
+#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.98
+#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.05
+
+	if (p_max<=0)
+		return;
+
+	Vector3 n=p_transform.basis.xform_inv(p_normal);
+
+	/** TEST FACE AS SUPPORT **/
+	if (get_plane().normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {
+
+		*p_count=MIN(3,p_max);
+
+		for (int i=0;i<*p_count;i++) {
+
+			p_vertices[i]=p_transform.xform(vertex[i]);
+		}
+
+		return;
+
+	}
+
+	/** FIND SUPPORT VERTEX **/
+
+	int vert_support_idx=-1;
+	float support_max;
+
+	for (int i=0;i<3;i++) {
+
+		float d=n.dot(vertex[i]);
+
+		if (i==0 || d > support_max) {
+			support_max=d;
+			vert_support_idx=i;
+		}
+	}
+
+	/** TEST EDGES AS SUPPORT **/
+
+	for (int i=0;i<3;i++) {
+
+		if (i!=vert_support_idx && i+1!=vert_support_idx)
+			continue;
+
+	// check if edge is valid as a support
+		float dot=(vertex[i]-vertex[(i+1)%3]).normalized().dot(n);
+		dot=ABS(dot);
+		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
+
+			*p_count=MIN(2,p_max);
+
+			for (int j=0;j<*p_count;j++)
+				p_vertices[j]=p_transform.xform(vertex[(j+i)%3]);
+
+			return;
+		}
+	}
+
+
+	*p_count=1;
+	p_vertices[0]=p_transform.xform(vertex[vert_support_idx]);
+
+}
+
+
+
diff --git a/core/math/face3.h b/core/math/face3.h
new file mode 100644
index 0000000000..3a62f7f812
--- /dev/null
+++ b/core/math/face3.h
@@ -0,0 +1,97 @@
+/*************************************************************************/
+/*  face3.h                                                              */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef FACE3_H
+#define FACE3_H
+
+#include "vector3.h"
+#include "plane.h"
+#include "aabb.h"
+#include "transform.h"
+
+class Face3 {
+public:
+
+        enum Side {
+                SIDE_OVER,
+                SIDE_UNDER,
+                SIDE_SPANNING,
+                SIDE_COPLANAR
+        };
+
+
+        Vector3 vertex[3];
+
+        /**
+         *
+         * @param p_plane plane used to split the face
+         * @param p_res array of at least 3 faces, amount used in functio return
+         * @param p_is_point_over array of at least 3 booleans, determining which face is over the plane, amount used in functio return
+         * @param _epsilon constant used for numerical error rounding, to add "thickness" to the plane (so coplanar points can happen)
+         * @return amount of faces generated by the split, either 0 (means no split possible), 2 or 3
+         */
+                  
+        int split_by_plane(const Plane& p_plane,Face3 *p_res,bool *p_is_point_over) const;
+
+	Plane get_plane(ClockDirection p_dir=CLOCKWISE) const;
+	Vector3 get_random_point_inside() const;
+
+
+        Side get_side_of(const Face3& p_face,ClockDirection p_clock_dir=CLOCKWISE) const;
+
+        bool is_degenerate() const;
+	real_t get_area() const;
+
+        Vector3 get_median_point() const;
+
+	bool intersects_ray(const Vector3& p_from,const Vector3& p_dir,Vector3 * p_intersection=0) const;
+	bool intersects_segment(const Vector3& p_from,const Vector3& p_dir,Vector3 * p_intersection=0) const;
+
+        ClockDirection get_clock_dir() const; ///< todo, test if this is returning the proper clockwisity
+
+        void get_support(const Vector3& p_normal,const Transform& p_transform,Vector3 *p_vertices,int* p_count,int p_max) const;
+        void project_range(const Vector3& p_normal,const Transform& p_transform,float& r_min, float& r_max) const;
+        
+        AABB get_aabb() const {
+
+                AABB aabb( vertex[0], Vector3() );
+                aabb.expand_to( vertex[1] );
+                aabb.expand_to( vertex[2] );
+                return aabb;
+        }
+
+        bool intersects_aabb(const AABB& p_aabb) const;
+	operator String() const;
+
+        inline Face3() {}
+        inline Face3(const Vector3 &p_v1,const Vector3 &p_v2,const Vector3 &p_v3) { vertex[0]=p_v1; vertex[1]=p_v2; vertex[2]=p_v3; }
+
+};
+
+
+#endif // FACE3_H
diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp
new file mode 100644
index 0000000000..cb76b9ed0f
--- /dev/null
+++ b/core/math/geometry.cpp
@@ -0,0 +1,1006 @@
+/*************************************************************************/
+/*  geometry.cpp                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "geometry.h"
+#include "print_string.h"
+
+
+
+void Geometry::MeshData::optimize_vertices() {
+
+	Map<int,int> vtx_remap;
+
+	for(int i=0;i<faces.size();i++) {
+
+		for(int j=0;j<faces[i].indices.size();j++) {
+
+			int idx = faces[i].indices[j];
+			if (!vtx_remap.has(idx)) {
+				int ni = vtx_remap.size();
+				vtx_remap[idx]=ni;
+
+
+			}
+
+			faces[i].indices[j]=vtx_remap[idx];
+		}
+	}
+
+	for(int i=0;i<edges.size();i++) {
+
+		int a = edges[i].a;
+		int b = edges[i].b;
+
+		if (!vtx_remap.has(a)) {
+			int ni = vtx_remap.size();
+			vtx_remap[a]=ni;
+		}
+		if (!vtx_remap.has(b)) {
+			int ni = vtx_remap.size();
+			vtx_remap[b]=ni;
+		}
+
+		edges[i].a=vtx_remap[a];
+		edges[i].b=vtx_remap[b];
+	}
+
+	Vector<Vector3> new_vertices;
+	new_vertices.resize(vtx_remap.size());
+
+	for(int i=0;i<vertices.size();i++) {
+
+		if (vtx_remap.has(i))
+			new_vertices[vtx_remap[i]]=vertices[i];
+	}
+	vertices=new_vertices;
+}
+
+Vector< Vector<Vector2> > (*Geometry::_decompose_func)(const Vector<Vector2>& p_polygon)=NULL;
+
+struct _FaceClassify {
+
+	struct _Link {
+
+		int face;
+		int edge;
+		void clear() { face=-1; edge=-1; }
+		_Link() { face=-1; edge=-1; }
+	};
+	bool valid;
+	int group;
+	_Link links[3];
+	Face3 face;
+	_FaceClassify() {
+		group=-1;
+		valid=false;
+	};
+};
+
+static bool _connect_faces(_FaceClassify *p_faces, int len, int p_group) {
+	/* connect faces, error will occur if an edge is shared between more than 2 faces */
+	/* clear connections */
+
+	bool error=false;
+
+	for (int i=0;i<len;i++) {
+
+		for (int j=0;j<3;j++) {
+
+			p_faces[i].links[j].clear();
+		}
+	}
+
+	for (int i=0;i<len;i++) {
+
+		if (p_faces[i].group!=p_group)
+			continue;
+		for (int j=i+1;j<len;j++) {
+
+			if (p_faces[j].group!=p_group)
+				continue;
+
+			for (int k=0;k<3;k++) {
+
+				Vector3 vi1=p_faces[i].face.vertex[k];
+				Vector3 vi2=p_faces[i].face.vertex[(k+1)%3];
+
+				for (int l=0;l<3;l++) {
+
+					Vector3 vj2=p_faces[j].face.vertex[l];
+					Vector3 vj1=p_faces[j].face.vertex[(l+1)%3];
+
+					if (vi1.distance_to(vj1)<0.00001 &&
+					    vi2.distance_to(vj2)<0.00001
+					   ) {
+						if (p_faces[i].links[k].face!=-1) {
+
+							ERR_PRINT("already linked\n");
+							error=true;
+							break;
+						}
+						if (p_faces[j].links[l].face!=-1) {
+
+							ERR_PRINT("already linked\n");
+							error=true;
+							break;
+						}
+
+						p_faces[i].links[k].face=j;
+						p_faces[i].links[k].edge=l;
+						p_faces[j].links[l].face=i;
+						p_faces[j].links[l].edge=k;
+					   }
+				}
+				if (error)
+					break;
+
+			}
+			if (error)
+				break;
+
+		}
+		if (error)
+			break;
+	}
+
+	for (int i=0;i<len;i++) {
+
+		p_faces[i].valid=true;
+		for (int j=0;j<3;j++) {
+
+			if (p_faces[i].links[j].face==-1)
+				p_faces[i].valid=false;
+		}
+		/*printf("face %i is valid: %i, group %i. connected to %i:%i,%i:%i,%i:%i\n",i,p_faces[i].valid,p_faces[i].group,
+			p_faces[i].links[0].face,
+			p_faces[i].links[0].edge,
+			p_faces[i].links[1].face,
+			p_faces[i].links[1].edge,
+			p_faces[i].links[2].face,
+			p_faces[i].links[2].edge);*/
+	}
+	return error;
+}
+
+static bool _group_face(_FaceClassify *p_faces, int len, int p_index,int p_group) {
+
+	if (p_faces[p_index].group>=0)
+		return false;
+
+	p_faces[p_index].group=p_group;
+
+	for (int i=0;i<3;i++) {
+
+		ERR_FAIL_INDEX_V(p_faces[p_index].links[i].face,len,true);
+		_group_face(p_faces,len,p_faces[p_index].links[i].face,p_group);
+	}
+
+	return true;
+}
+
+
+DVector< DVector< Face3 > > Geometry::separate_objects( DVector< Face3 > p_array ) {
+
+	DVector< DVector< Face3 > > objects;
+
+	int len = p_array.size();
+
+	DVector<Face3>::Read r=p_array.read();
+	
+	const Face3* arrayptr = r.ptr();
+
+	DVector< _FaceClassify> fc;
+
+	fc.resize( len );
+
+	DVector< _FaceClassify >::Write fcw=fc.write();
+	
+	_FaceClassify * _fcptr = fcw.ptr();
+
+	for (int i=0;i<len;i++) {
+
+		_fcptr[i].face=arrayptr[i];
+	}
+
+	bool error=_connect_faces(_fcptr,len,-1);
+
+	if (error) {
+
+		ERR_FAIL_COND_V(error, DVector< DVector< Face3 > >() ); // invalid geometry
+	}
+
+	/* group connected faces in separate objects */
+
+	int group=0;
+	for (int i=0;i<len;i++) {
+
+		if (!_fcptr[i].valid)
+			continue;
+		if (_group_face(_fcptr,len,i,group)) {
+			group++;
+		}
+	}
+
+	/* group connected faces in separate objects */
+
+
+	for (int i=0;i<len;i++) {
+
+		_fcptr[i].face=arrayptr[i];
+	}
+
+	if (group>=0) {
+
+		objects.resize(group);
+		DVector< DVector<Face3> >::Write obw=objects.write();
+		DVector< Face3 > *group_faces = obw.ptr();
+
+		for (int i=0;i<len;i++) {
+			if (!_fcptr[i].valid)
+				continue;
+			if (_fcptr[i].group>=0 && _fcptr[i].group<group) {
+
+				group_faces[_fcptr[i].group].push_back( _fcptr[i].face );
+			}
+		}
+	}
+
+
+	return objects;
+
+}
+
+/*** GEOMETRY WRAPPER ***/
+
+enum _CellFlags {
+	
+	_CELL_SOLID=1,
+ 	_CELL_EXTERIOR=2,
+  	_CELL_STEP_MASK=0x1C,
+   	_CELL_STEP_NONE=0<<2,
+	_CELL_STEP_Y_POS=1<<2,
+ 	_CELL_STEP_Y_NEG=2<<2,
+  	_CELL_STEP_X_POS=3<<2,
+   	_CELL_STEP_X_NEG=4<<2,
+    	_CELL_STEP_Z_POS=5<<2,
+     	_CELL_STEP_Z_NEG=6<<2,
+      	_CELL_STEP_DONE=7<<2,
+	_CELL_PREV_MASK=0xE0,
+	_CELL_PREV_NONE=0<<5,
+	_CELL_PREV_Y_POS=1<<5,
+	_CELL_PREV_Y_NEG=2<<5,
+	_CELL_PREV_X_POS=3<<5,
+	_CELL_PREV_X_NEG=4<<5,
+	_CELL_PREV_Z_POS=5<<5,
+	_CELL_PREV_Z_NEG=6<<5,
+	_CELL_PREV_FIRST=7<<5,
+	
+};
+
+static inline void _plot_face(uint8_t*** p_cell_status,int x,int y,int z,int len_x,int len_y,int len_z,const Vector3& voxelsize,const Face3& p_face) {
+
+	AABB aabb( Vector3(x,y,z),Vector3(len_x,len_y,len_z));
+	aabb.pos=aabb.pos*voxelsize;
+	aabb.size=aabb.size*voxelsize;
+
+	if (!p_face.intersects_aabb(aabb))
+		return;
+
+	if (len_x==1 && len_y==1 && len_z==1) {
+
+		p_cell_status[x][y][z]=_CELL_SOLID;
+		return;
+	}
+	
+	
+ 
+	int div_x=len_x>1?2:1;
+	int div_y=len_y>1?2:1;
+	int div_z=len_z>1?2:1;
+
+#define _SPLIT(m_i,m_div,m_v,m_len_v,m_new_v,m_new_len_v)\
+	if (m_div==1) {\
+		m_new_v=m_v;\
+		m_new_len_v=1;	\
+	} else if (m_i==0) {\
+		m_new_v=m_v;\
+		m_new_len_v=m_len_v/2;\
+	} else {\
+		m_new_v=m_v+m_len_v/2;\
+		m_new_len_v=m_len_v-m_len_v/2;		\
+	}
+
+	int new_x;
+	int new_len_x;
+	int new_y;
+	int new_len_y;
+	int new_z;
+	int new_len_z;
+
+	for (int i=0;i<div_x;i++) {
+		
+		
+		_SPLIT(i,div_x,x,len_x,new_x,new_len_x);
+				
+		for (int j=0;j<div_y;j++) {
+
+			_SPLIT(j,div_y,y,len_y,new_y,new_len_y);
+			
+			for (int k=0;k<div_z;k++) {
+
+				_SPLIT(k,div_z,z,len_z,new_z,new_len_z);
+
+				_plot_face(p_cell_status,new_x,new_y,new_z,new_len_x,new_len_y,new_len_z,voxelsize,p_face);
+			}
+		}
+	}
+}
+
+static inline void _mark_outside(uint8_t*** p_cell_status,int x,int y,int z,int len_x,int len_y,int len_z) {
+
+	if (p_cell_status[x][y][z]&3)
+		return; // nothing to do, already used and/or visited
+	
+	p_cell_status[x][y][z]=_CELL_PREV_FIRST;
+	
+	while(true) {
+		
+		uint8_t &c = p_cell_status[x][y][z];
+		
+		//printf("at %i,%i,%i\n",x,y,z);
+		
+		if ( (c&_CELL_STEP_MASK)==_CELL_STEP_NONE) {	
+			/* Haven't been in here, mark as outside */
+			p_cell_status[x][y][z]|=_CELL_EXTERIOR;
+			//printf("not marked as anything, marking exterior\n");
+		}
+		
+		//printf("cell step is %i\n",(c&_CELL_STEP_MASK));
+		
+		if ( (c&_CELL_STEP_MASK)!=_CELL_STEP_DONE) {
+			/* if not done, increase step */
+			c+=1<<2;
+			//printf("incrementing cell step\n");
+		}
+		
+		if ( (c&_CELL_STEP_MASK)==_CELL_STEP_DONE) {
+			/* Go back */
+			//printf("done, going back a cell\n");
+			
+			switch(c&_CELL_PREV_MASK) {
+				case _CELL_PREV_FIRST: {
+					//printf("at end, finished marking\n");				
+					return;
+				} break;
+				case _CELL_PREV_Y_POS: {	
+					y++;
+					ERR_FAIL_COND(y>=len_y);
+				} break;
+				case _CELL_PREV_Y_NEG: {
+					y--;
+					ERR_FAIL_COND(y<0);
+				} break;
+				case _CELL_PREV_X_POS: {
+					x++;
+					ERR_FAIL_COND(x>=len_x);
+				} break;
+				case _CELL_PREV_X_NEG: {
+					x--;
+					ERR_FAIL_COND(x<0);
+				} break;
+				case _CELL_PREV_Z_POS: {
+					z++;
+					ERR_FAIL_COND(z>=len_z);
+				} break;
+				case _CELL_PREV_Z_NEG: {
+					z--;
+					ERR_FAIL_COND(z<0);
+				} break;
+				default: {
+					ERR_FAIL();
+				}
+			}
+			continue;
+		}
+		
+		//printf("attempting new cell!\n");
+		
+		int next_x=x,next_y=y,next_z=z;
+		uint8_t prev=0;
+		
+		switch(c&_CELL_STEP_MASK) {
+			
+			case _CELL_STEP_Y_POS: {
+				
+				next_y++;
+				prev=_CELL_PREV_Y_NEG;
+			} break;
+			case _CELL_STEP_Y_NEG: {
+				next_y--;
+				prev=_CELL_PREV_Y_POS;
+			} break;
+			case _CELL_STEP_X_POS: {
+				next_x++;
+				prev=_CELL_PREV_X_NEG;
+			} break;
+			case _CELL_STEP_X_NEG: {
+				next_x--;
+				prev=_CELL_PREV_X_POS;
+			} break;
+			case _CELL_STEP_Z_POS: {
+				next_z++;
+				prev=_CELL_PREV_Z_NEG;				
+			} break;
+			case _CELL_STEP_Z_NEG: {
+				next_z--;
+				prev=_CELL_PREV_Z_POS;				
+			} break;
+			default: ERR_FAIL(); 
+			
+		}
+		
+		//printf("testing if new cell will be ok...!\n");
+		
+		if (next_x<0 || next_x>=len_x)
+			continue;
+		if (next_y<0 || next_y>=len_y)
+			continue;
+		if (next_z<0 || next_z>=len_z)
+			continue;
+			
+		//printf("testing if new cell is traversable\n");
+			
+		if (p_cell_status[next_x][next_y][next_z]&3)
+			continue;
+		
+		//printf("move to it\n");
+		
+		x=next_x;
+		y=next_y;
+		z=next_z;
+		p_cell_status[x][y][z]|=prev;
+	}
+}
+
+static inline void _build_faces(uint8_t*** p_cell_status,int x,int y,int z,int len_x,int len_y,int len_z,DVector<Face3>& p_faces) {
+	
+	ERR_FAIL_INDEX(x,len_x);
+	ERR_FAIL_INDEX(y,len_y);
+	ERR_FAIL_INDEX(z,len_z);
+	
+	if (p_cell_status[x][y][z]&_CELL_EXTERIOR)
+		return;	
+	
+/*	static const Vector3 vertices[8]={
+		Vector3(0,0,0),
+		Vector3(0,0,1),
+		Vector3(0,1,0),
+		Vector3(0,1,1),
+		Vector3(1,0,0),
+		Vector3(1,0,1),
+		Vector3(1,1,0),
+		Vector3(1,1,1),
+	};
+*/
+#define vert(m_idx) Vector3( (m_idx&4)>>2, (m_idx&2)>>1, m_idx&1 )
+
+	static const uint8_t indices[6][4]={
+		{7,6,4,5},		
+		{7,3,2,6},
+		{7,5,1,3},				
+		{0,2,3,1},			
+		{0,1,5,4},		
+		{0,4,6,2},
+
+	};
+/*	
+
+		{0,1,2,3},			
+		{0,1,4,5},		
+		{0,2,4,6},
+		{4,5,6,7},		
+		{2,3,7,6},
+		{1,3,5,7},				
+
+		{0,2,3,1},			
+		{0,1,5,4},		
+		{0,4,6,2},
+		{7,6,4,5},		
+		{7,3,2,6},
+		{7,5,1,3},				
+*/
+	
+	for (int i=0;i<6;i++) {
+			
+		Vector3 face_points[4];
+		int disp_x=x+((i%3)==0?((i<3)?1:-1):0);
+		int disp_y=y+(((i-1)%3)==0?((i<3)?1:-1):0);
+		int disp_z=z+(((i-2)%3)==0?((i<3)?1:-1):0);
+		
+		bool plot=false;
+		
+		if (disp_x<0 || disp_x>=len_x)
+			plot=true;
+		if (disp_y<0 || disp_y>=len_y)
+			plot=true;
+		if (disp_z<0 || disp_z>=len_z)
+			plot=true;
+		
+		if (!plot && (p_cell_status[disp_x][disp_y][disp_z]&_CELL_EXTERIOR))
+			plot=true;
+			
+		if (!plot)
+			continue;
+		
+		for (int j=0;j<4;j++)
+			face_points[j]=vert( indices[i][j] ) + Vector3(x,y,z);
+		
+		p_faces.push_back( 
+			Face3( 
+				face_points[0],
+				face_points[1],
+				face_points[2]
+			     )
+		);
+    
+		p_faces.push_back( 
+			Face3( 
+				face_points[2],
+				face_points[3],
+				face_points[0]
+			     )
+		);
+		
+	}	
+
+}
+
+DVector< Face3 > Geometry::wrap_geometry( DVector< Face3 > p_array,float *p_error ) {
+
+#define _MIN_SIZE 1.0
+#define _MAX_LENGTH 20
+
+	int face_count=p_array.size();
+	DVector<Face3>::Read facesr=p_array.read();
+	const Face3 *faces = facesr.ptr();
+
+	AABB global_aabb;
+
+	for(int i=0;i<face_count;i++) {
+
+		if (i==0) {
+
+			global_aabb=faces[i].get_aabb();
+		} else {
+
+			global_aabb.merge_with( faces[i].get_aabb() );
+		}
+	}
+	
+	global_aabb.grow_by(0.01); // avoid numerical error
+
+	// determine amount of cells in grid axis
+	int div_x,div_y,div_z;
+
+	if (global_aabb.size.x/_MIN_SIZE<_MAX_LENGTH)
+		div_x=(int)(global_aabb.size.x/_MIN_SIZE)+1;
+	else
+		div_x=_MAX_LENGTH;
+
+	if (global_aabb.size.y/_MIN_SIZE<_MAX_LENGTH)
+		div_y=(int)(global_aabb.size.y/_MIN_SIZE)+1;
+	else
+		div_y=_MAX_LENGTH;
+
+	if (global_aabb.size.z/_MIN_SIZE<_MAX_LENGTH)
+		div_z=(int)(global_aabb.size.z/_MIN_SIZE)+1;
+	else
+		div_z=_MAX_LENGTH;
+
+	Vector3 voxelsize=global_aabb.size;
+	voxelsize.x/=div_x;
+	voxelsize.y/=div_y;
+	voxelsize.z/=div_z;
+
+
+	// create and initialize cells to zero
+	print_line("Wrapper: Initializing Cells");
+
+	uint8_t ***cell_status=memnew_arr(uint8_t**,div_x);
+	for(int i=0;i<div_x;i++) {
+
+		cell_status[i]=memnew_arr(uint8_t*,div_y);
+
+		for(int j=0;j<div_y;j++) {
+
+			cell_status[i][j]=memnew_arr(uint8_t,div_z);
+
+			for(int k=0;k<div_z;k++) {
+
+				cell_status[i][j][k]=0;
+			}
+		}
+	}
+
+	// plot faces into cells
+	print_line("Wrapper (1/6): Plotting Faces");
+	
+	for (int i=0;i<face_count;i++) {
+
+		Face3 f=faces[i];
+		for (int j=0;j<3;j++) {
+
+			f.vertex[j]-=global_aabb.pos;
+		}
+		_plot_face(cell_status,0,0,0,div_x,div_y,div_z,voxelsize,f);
+	}
+
+
+	// determine which cells connect to the outside by traversing the outside and recursively flood-fill marking
+
+	print_line("Wrapper (2/6) Flood Filling");
+
+	for (int i=0;i<div_x;i++) {
+		
+		for (int j=0;j<div_y;j++) {
+		
+			_mark_outside(cell_status,i,j,0,div_x,div_y,div_z);
+			_mark_outside(cell_status,i,j,div_z-1,div_x,div_y,div_z);
+		}
+	}
+	
+	for (int i=0;i<div_z;i++) {
+		
+		for (int j=0;j<div_y;j++) {
+		
+			_mark_outside(cell_status,0,j,i,div_x,div_y,div_z);
+			_mark_outside(cell_status,div_x-1,j,i,div_x,div_y,div_z);
+		}
+	}
+	
+	for (int i=0;i<div_x;i++) {
+		
+		for (int j=0;j<div_z;j++) {
+		
+			_mark_outside(cell_status,i,0,j,div_x,div_y,div_z);
+			_mark_outside(cell_status,i,div_y-1,j,div_x,div_y,div_z);
+		}
+	}
+	
+	// build faces for the inside-outside cell divisors
+	
+	print_line("Wrapper (3/6): Building Faces");
+
+	DVector<Face3> wrapped_faces;
+	
+	for (int i=0;i<div_x;i++) {
+		
+		for (int j=0;j<div_y;j++) {
+		
+			for (int k=0;k<div_z;k++) {
+			
+				_build_faces(cell_status,i,j,k,div_x,div_y,div_z,wrapped_faces);				
+			}
+		}
+	}
+	
+	print_line("Wrapper (4/6): Transforming Back Vertices");
+
+	// transform face vertices to global coords
+	
+	int wrapped_faces_count=wrapped_faces.size();
+	DVector<Face3>::Write wrapped_facesw=wrapped_faces.write();
+	Face3* wrapped_faces_ptr=wrapped_facesw.ptr();
+	
+	for(int i=0;i<wrapped_faces_count;i++) {
+	
+		for(int j=0;j<3;j++) {
+		
+			Vector3& v = wrapped_faces_ptr[i].vertex[j];
+			v=v*voxelsize;
+			v+=global_aabb.pos;
+		}
+	}
+	
+	// clean up grid 
+	print_line("Wrapper (5/6): Grid Cleanup");
+
+	for(int i=0;i<div_x;i++) {
+
+		for(int j=0;j<div_y;j++) {
+
+			memdelete_arr( cell_status[i][j] );
+		}
+		
+		memdelete_arr( cell_status[i] );
+	}
+	
+	memdelete_arr(cell_status);
+	if (p_error)
+		*p_error=voxelsize.length();
+	
+	print_line("Wrapper (6/6): Finished.");
+	return wrapped_faces;
+}
+
+Geometry::MeshData Geometry::build_convex_mesh(const DVector<Plane> &p_planes) {
+
+	MeshData mesh;
+	
+	
+#define SUBPLANE_SIZE 1024.0
+	
+	float subplane_size = 1024.0; // should compute this from the actual plane
+	for (int i=0;i<p_planes.size();i++) {
+	
+		Plane p =p_planes[i];
+		
+		Vector3 ref=Vector3(0.0,1.0,0.0);
+		
+		if (ABS(p.normal.dot(ref))>0.95)
+			ref=Vector3(0.0,0.0,1.0); // change axis
+			
+		Vector3 right = p.normal.cross(ref).normalized();
+		Vector3 up = p.normal.cross( right ).normalized();
+				
+		Vector< Vector3 > vertices;
+		
+		Vector3 center = p.get_any_point();
+		// make a quad clockwise
+		vertices.push_back( center - up * subplane_size + right * subplane_size );
+		vertices.push_back( center - up * subplane_size - right * subplane_size );
+		vertices.push_back( center + up * subplane_size - right * subplane_size );
+		vertices.push_back( center + up * subplane_size + right * subplane_size );
+		
+		for (int j=0;j<p_planes.size();j++) {
+
+			if (j==i)
+				continue;
+			
+
+			Vector< Vector3 > new_vertices;
+			Plane clip=p_planes[j];
+			
+			if (clip.normal.dot(p.normal)>0.95)
+				continue;
+				
+			if (vertices.size()<3)
+				break;
+				
+			for(int k=0;k<vertices.size();k++) {
+			
+				int k_n=(k+1)%vertices.size();
+				
+				Vector3 edge0_A=vertices[k];
+				Vector3 edge1_A=vertices[k_n];
+				
+				real_t dist0 = clip.distance_to(edge0_A);
+				real_t dist1 = clip.distance_to(edge1_A);
+				
+							
+				if ( dist0 <= 0 ) { // behind plane 
+				
+					new_vertices.push_back(vertices[k]);					
+				}
+				
+				
+				// check for different sides and non coplanar
+				if ( (dist0*dist1) < 0) { 
+					
+					// calculate intersection
+					Vector3 rel = edge1_A - edge0_A;
+
+					real_t den=clip.normal.dot( rel );
+					if (Math::abs(den)<CMP_EPSILON)
+						continue; // point too short
+
+					real_t dist=-(clip.normal.dot( edge0_A )-clip.d)/den;
+					Vector3 inters = edge0_A+rel*dist;
+					new_vertices.push_back(inters);
+				}			
+			}
+			
+			vertices=new_vertices;
+		}
+		
+		if (vertices.size()<3)
+			continue;
+		
+		
+		//result is a clockwise face
+		
+		MeshData::Face face;
+				
+		// add face indices
+		for (int j=0;j<vertices.size();j++) {
+		
+			
+			int idx=-1;
+			for (int k=0;k<mesh.vertices.size();k++) {
+		
+				if (mesh.vertices[k].distance_to(vertices[j])<0.001) {
+				
+					idx=k;
+					break;
+				}
+			}
+			
+			if (idx==-1) {
+			
+				idx=mesh.vertices.size();
+				mesh.vertices.push_back(vertices[j]);
+			}
+			
+			face.indices.push_back(idx);
+		}
+		face.plane=p;
+		mesh.faces.push_back(face);
+		
+		//add edge
+		
+		for(int j=0;j<face.indices.size();j++) {
+		
+			int a=face.indices[j];
+			int b=face.indices[(j+1)%face.indices.size()];
+		
+			bool found=false;
+			for(int k=0;k<mesh.edges.size();k++) {
+		
+				if (mesh.edges[k].a==a && mesh.edges[k].b==b) {
+					found=true;
+					break;
+				}
+				if (mesh.edges[k].b==a && mesh.edges[k].a==b) {
+					found=true;
+					break;
+				}				
+			}
+			
+			if (found)
+				continue;
+			MeshData::Edge edge;
+			edge.a=a;
+			edge.b=b;
+			mesh.edges.push_back(edge);
+		}
+		
+		
+	}
+
+	return mesh;
+}
+
+
+DVector<Plane> Geometry::build_box_planes(const Vector3& p_extents) {
+
+	DVector<Plane> planes;
+	
+	planes.push_back( Plane( Vector3(1,0,0), p_extents.x ) );
+	planes.push_back( Plane( Vector3(-1,0,0), p_extents.x ) );
+	planes.push_back( Plane( Vector3(0,1,0), p_extents.y ) );
+	planes.push_back( Plane( Vector3(0,-1,0), p_extents.y ) );
+	planes.push_back( Plane( Vector3(0,0,1), p_extents.z ) );
+	planes.push_back( Plane( Vector3(0,0,-1), p_extents.z ) );
+	
+	return planes;
+}
+
+DVector<Plane> Geometry::build_cylinder_planes(float p_radius, float p_height, int p_sides, Vector3::Axis p_axis) {
+
+	DVector<Plane> planes;
+	
+	for (int i=0;i<p_sides;i++) {
+	
+		Vector3 normal;
+		normal[(p_axis+1)%3]=Math::cos(i*(2.0*Math_PI)/p_sides);
+		normal[(p_axis+2)%3]=Math::sin(i*(2.0*Math_PI)/p_sides);
+		
+		planes.push_back( Plane( normal, p_radius ) );
+	}
+	
+	Vector3 axis;
+	axis[p_axis]=1.0;
+	
+	planes.push_back( Plane( axis, p_height*0.5 ) );
+	planes.push_back( Plane( -axis, p_height*0.5 ) );
+	
+	return planes;
+
+}
+
+DVector<Plane> Geometry::build_sphere_planes(float p_radius, int p_lats,int p_lons, Vector3::Axis p_axis) {
+
+
+	DVector<Plane> planes;
+
+	Vector3 axis;
+	axis[p_axis]=1.0;
+
+	Vector3 axis_neg;
+	axis_neg[(p_axis+1)%3]=1.0;
+	axis_neg[(p_axis+2)%3]=1.0;
+	axis_neg[p_axis]=-1.0;
+
+	for (int i=0;i<p_lons;i++) {
+
+		Vector3 normal;
+		normal[(p_axis+1)%3]=Math::cos(i*(2.0*Math_PI)/p_lons);
+		normal[(p_axis+2)%3]=Math::sin(i*(2.0*Math_PI)/p_lons);
+
+		planes.push_back( Plane( normal, p_radius ) );
+
+		for (int j=1;j<=p_lats;j++) {
+
+			//todo this is stupid, fix
+			Vector3 angle = normal.linear_interpolate(axis,j/(float)p_lats).normalized();
+			Vector3 pos = angle*p_radius;
+			planes.push_back( Plane( pos, angle ) );
+			planes.push_back( Plane( pos * axis_neg, angle * axis_neg) );
+
+		}
+	}
+
+	return planes;
+
+}
+
+DVector<Plane> Geometry::build_capsule_planes(float p_radius, float p_height, int p_sides, int p_lats, Vector3::Axis p_axis) {
+
+	DVector<Plane> planes;
+	
+		Vector3 axis;
+	axis[p_axis]=1.0;
+	
+	Vector3 axis_neg;	
+	axis_neg[(p_axis+1)%3]=1.0;
+	axis_neg[(p_axis+2)%3]=1.0;
+	axis_neg[p_axis]=-1.0;
+	
+	for (int i=0;i<p_sides;i++) {
+	
+		Vector3 normal;
+		normal[(p_axis+1)%3]=Math::cos(i*(2.0*Math_PI)/p_sides);
+		normal[(p_axis+2)%3]=Math::sin(i*(2.0*Math_PI)/p_sides);
+		
+		planes.push_back( Plane( normal, p_radius ) );
+		
+		for (int j=1;j<=p_lats;j++) {
+			
+			Vector3 angle = normal.linear_interpolate(axis,j/(float)p_lats).normalized();
+			Vector3 pos = axis*p_height*0.5 + angle*p_radius;
+			planes.push_back( Plane( pos, angle ) );
+			planes.push_back( Plane( pos * axis_neg, angle * axis_neg) );
+			
+		}
+	}
+
+		
+	return planes;
+
+}
+
diff --git a/core/math/geometry.h b/core/math/geometry.h
new file mode 100644
index 0000000000..826e4697b5
--- /dev/null
+++ b/core/math/geometry.h
@@ -0,0 +1,755 @@
+/*************************************************************************/
+/*  geometry.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef GEOMETRY_H
+#define GEOMETRY_H
+
+#include "vector3.h"
+#include "face3.h"
+#include "dvector.h"
+#include "math_2d.h"
+#include "vector.h"
+#include "print_string.h"
+#include "object.h"
+#include "triangulate.h"
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+
+class Geometry {
+	Geometry();
+public:
+	
+
+
+
+	static float get_closest_points_between_segments( const Vector2& p1,const Vector2& q1, const Vector2& p2,const Vector2& q2, Vector2& c1, Vector2& c2) {
+
+		Vector2 d1 = q1 - p1; // Direction vector of segment S1
+		Vector2 d2 = q2 - p2; // Direction vector of segment S2
+		Vector2 r = p1 - p2;
+		float a = d1.dot(d1); // Squared length of segment S1, always nonnegative
+		float e = d2.dot(d2); // Squared length of segment S2, always nonnegative
+		float f = d2.dot(r);
+		float s,t;
+		// Check if either or both segments degenerate into points
+		if (a <= CMP_EPSILON && e <= CMP_EPSILON) {
+			// Both segments degenerate into points
+			c1 = p1;
+			c2 = p2;
+			return Math::sqrt((c1 - c2).dot(c1 - c2));
+		}
+		if (a <= CMP_EPSILON) {
+			// First segment degenerates into a point
+			s = 0.0f;
+			t = f / e; // s = 0 => t = (b*s + f) / e = f / e
+			t = CLAMP(t, 0.0f, 1.0f);
+		} else {
+			float c = d1.dot(r);
+			if (e <= CMP_EPSILON) {
+				// Second segment degenerates into a point
+				t = 0.0f;
+				s = CLAMP(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
+			} else {
+				// The general nondegenerate case starts here
+				float b = d1.dot(d2);
+				float denom = a*e-b*b; // Always nonnegative
+				// If segments not parallel, compute closest point on L1 to L2 and
+				// clamp to segment S1. Else pick arbitrary s (here 0)
+				if (denom != 0.0f) {
+					s = CLAMP((b*f - c*e) / denom, 0.0f, 1.0f);
+				} else
+					s = 0.0f;
+				// Compute point on L2 closest to S1(s) using
+				// t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
+				t = (b*s + f) / e;
+
+				//If t in [0,1] done. Else clamp t, recompute s for the new value
+				// of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
+				// and clamp s to [0, 1]
+				if (t < 0.0f) {
+					t = 0.0f;
+					s = CLAMP(-c / a, 0.0f, 1.0f);
+				} else if (t > 1.0f) {
+					t = 1.0f;
+					s = CLAMP((b - c) / a, 0.0f, 1.0f);
+				}
+			}
+		}
+		c1 = p1 + d1 * s;
+		c2 = p2 + d2 * t;
+		return Math::sqrt((c1 - c2).dot(c1 - c2));
+	}
+
+
+	static void get_closest_points_between_segments(const Vector3& p1,const Vector3& p2,const Vector3& q1,const Vector3& q2,Vector3& c1, Vector3& c2)
+	{
+		//do the function 'd' as defined by pb. I think is is dot product of some sort
+#define d_of(m,n,o,p) ( (m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z) )
+
+		//caluclate the parpametric position on the 2 curves, mua and mub
+		float mua = ( d_of(p1,q1,q2,q1) * d_of(q2,q1,p2,p1)  - d_of(p1,q1,p2,p1) * d_of(q2,q1,q2,q1) ) / ( d_of(p2,p1,p2,p1) * d_of(q2,q1,q2,q1)  - d_of(q2,q1,p2,p1) * d_of(q2,q1,p2,p1) );
+		float mub = ( d_of(p1,q1,q2,q1) + mua * d_of(q2,q1,p2,p1)  ) / d_of(q2,q1,q2,q1);
+
+		//clip the value between [0..1] constraining the solution to lie on the original curves
+		if (mua < 0) mua = 0;
+		if (mub < 0) mub = 0;
+		if (mua > 1) mua = 1;
+		if (mub > 1) mub = 1;
+		c1 = p1.linear_interpolate(p2,mua);
+		c2 = q1.linear_interpolate(q2,mub);
+	}
+
+	static float get_closest_distance_between_segments( const Vector3& p_from_a,const Vector3& p_to_a, const Vector3& p_from_b,const Vector3& p_to_b) {
+	    Vector3   u = p_to_a - p_from_a;
+	    Vector3   v = p_to_b - p_from_b;
+	    Vector3   w = p_from_a - p_to_a;
+	    real_t    a = u.dot(u);        // always >= 0
+	    real_t    b = u.dot(v);
+	    real_t    c = v.dot(v);        // always >= 0
+	    real_t    d = u.dot(w);
+	    real_t    e = v.dot(w);
+	    real_t    D = a*c - b*b;       // always >= 0
+	    real_t    sc, sN, sD = D;      // sc = sN / sD, default sD = D >= 0
+	    real_t    tc, tN, tD = D;      // tc = tN / tD, default tD = D >= 0
+
+	    // compute the line parameters of the two closest points
+	    if (D < CMP_EPSILON) { // the lines are almost parallel
+		sN = 0.0;        // force using point P0 on segment S1
+		sD = 1.0;        // to prevent possible division by 0.0 later
+		tN = e;
+		tD = c;
+	    }
+	    else {                // get the closest points on the infinite lines
+		sN = (b*e - c*d);
+		tN = (a*e - b*d);
+		if (sN < 0.0) {       // sc < 0 => the s=0 edge is visible
+		    sN = 0.0;
+		    tN = e;
+		    tD = c;
+		}
+		else if (sN > sD) {  // sc > 1 => the s=1 edge is visible
+		    sN = sD;
+		    tN = e + b;
+		    tD = c;
+		}
+	    }
+
+	    if (tN < 0.0) {           // tc < 0 => the t=0 edge is visible
+		tN = 0.0;
+		// recompute sc for this edge
+		if (-d < 0.0)
+		    sN = 0.0;
+		else if (-d > a)
+		    sN = sD;
+		else {
+		    sN = -d;
+		    sD = a;
+		}
+	    }
+	    else if (tN > tD) {      // tc > 1 => the t=1 edge is visible
+		tN = tD;
+		// recompute sc for this edge
+		if ((-d + b) < 0.0)
+		    sN = 0;
+		else if ((-d + b) > a)
+		    sN = sD;
+		else {
+		    sN = (-d + b);
+		    sD = a;
+		}
+	    }
+	    // finally do the division to get sc and tc
+	    sc = (Math::abs(sN) < CMP_EPSILON ? 0.0 : sN / sD);
+	    tc = (Math::abs(tN) < CMP_EPSILON ? 0.0 : tN / tD);
+
+	    // get the difference of the two closest points
+	    Vector3   dP = w + (sc * u) - (tc * v);  // = S1(sc) - S2(tc)
+
+	    return dP.length();   // return the closest distance
+	}
+
+	static inline bool ray_intersects_triangle( const Vector3& p_from, const Vector3& p_dir, const Vector3& p_v0,const Vector3& p_v1,const Vector3& p_v2,Vector3* r_res=0) {
+		Vector3 e1=p_v1-p_v0;
+		Vector3 e2=p_v2-p_v0;
+		Vector3 h = p_dir.cross(e2);
+		real_t a =e1.dot(h);
+		if (a>-CMP_EPSILON && a < CMP_EPSILON) // parallel test
+			return false;
+				
+		real_t f=1.0/a;
+		
+		Vector3 s=p_from-p_v0;
+		real_t u = f * s.dot(h);
+		
+		if ( u< 0.0 || u > 1.0)
+			return false;
+		
+		Vector3 q=s.cross(e1);
+		
+		real_t v = f * p_dir.dot(q);
+		
+		if (v < 0.0 || u + v > 1.0)
+			return false;
+		
+		// at this stage we can compute t to find out where 
+		// the intersection point is on the line
+		real_t t = f * e2.dot(q);
+		
+		if (t > 0.00001) {// ray intersection
+			if (r_res)
+				*r_res=p_from+p_dir*t;
+			return true;
+		} else // this means that there is a line intersection  
+			// but not a ray intersection
+			return false;
+	}	
+	
+	static inline bool segment_intersects_triangle( const Vector3& p_from, const Vector3& p_to, const Vector3& p_v0,const Vector3& p_v1,const Vector3& p_v2,Vector3* r_res=0) {
+	
+		Vector3 rel=p_to-p_from;
+		Vector3 e1=p_v1-p_v0;
+		Vector3 e2=p_v2-p_v0;
+		Vector3 h = rel.cross(e2);
+		real_t a =e1.dot(h);
+		if (a>-CMP_EPSILON && a < CMP_EPSILON) // parallel test
+			return false;
+				
+		real_t f=1.0/a;
+		
+		Vector3 s=p_from-p_v0;
+		real_t u = f * s.dot(h);
+		
+		if ( u< 0.0 || u > 1.0)
+			return false;
+		
+		Vector3 q=s.cross(e1);
+		
+		real_t v = f * rel.dot(q);
+		
+		if (v < 0.0 || u + v > 1.0)
+			return false;
+		
+		// at this stage we can compute t to find out where 
+		// the intersection point is on the line
+		real_t t = f * e2.dot(q);
+		
+		if (t > CMP_EPSILON && t<=1.0) {// ray intersection
+			if (r_res)
+				*r_res=p_from+rel*t;
+			return true;
+		} else // this means that there is a line intersection  
+			// but not a ray intersection
+			return false;
+	}	
+
+	static inline bool segment_intersects_sphere( const Vector3& p_from, const Vector3& p_to, const Vector3& p_sphere_pos,real_t p_sphere_radius,Vector3* r_res=0,Vector3 *r_norm=0) {
+
+
+		Vector3 sphere_pos=p_sphere_pos-p_from;
+		Vector3 rel=(p_to-p_from);
+		float  rel_l=rel.length();
+		if (rel_l<CMP_EPSILON)
+			return false; // both points are the same
+		Vector3 normal=rel/rel_l;
+
+		float sphere_d=normal.dot(sphere_pos);
+
+		//Vector3 ray_closest=normal*sphere_d;
+
+		float ray_distance=sphere_pos.distance_to(normal*sphere_d);
+
+		if (ray_distance>=p_sphere_radius)
+			return false;
+
+		float inters_d2=p_sphere_radius*p_sphere_radius - ray_distance*ray_distance;
+		float inters_d=sphere_d;
+
+		if (inters_d2>=CMP_EPSILON)
+			inters_d-=Math::sqrt(inters_d2);
+
+		// check in segment
+		if (inters_d<0 || inters_d>rel_l)
+			return false;
+
+		Vector3 result=p_from+normal*inters_d;;
+
+		if (r_res)
+			*r_res=result;
+		if (r_norm)
+			*r_norm=(result-p_sphere_pos).normalized();
+
+		return true;
+	}
+
+	static inline bool segment_intersects_cylinder( const Vector3& p_from, const Vector3& p_to, float p_height,float p_radius,Vector3* r_res=0,Vector3 *r_norm=0) {
+
+		Vector3 rel=(p_to-p_from);
+		float  rel_l=rel.length();
+		if (rel_l<CMP_EPSILON)
+			return false; // both points are the same
+
+		// first check if they are parallel
+		Vector3 normal=(rel/rel_l);
+		Vector3 crs = normal.cross(Vector3(0,0,1));
+		float crs_l=crs.length();
+
+		Vector3 z_dir;
+
+		if(crs_l<CMP_EPSILON) {
+			//blahblah parallel
+			z_dir=Vector3(1,0,0); //any x/y vector ok
+		} else {
+			z_dir=crs/crs_l;
+		}
+
+		float dist=z_dir.dot(p_from);
+
+		if (dist>=p_radius)
+			return false; // too far away
+
+		// convert to 2D
+		float w2=p_radius*p_radius-dist*dist;
+		if (w2<CMP_EPSILON)
+			return false; //avoid numerical error
+		Size2 size(Math::sqrt(w2),p_height*0.5);
+
+		Vector3 x_dir=z_dir.cross(Vector3(0,0,1)).normalized();
+
+		Vector2 from2D(x_dir.dot(p_from),p_from.z);
+		Vector2 to2D(x_dir.dot(p_to),p_to.z);
+
+		float min=0,max=1;
+
+		int axis=-1;
+
+		for(int i=0;i<2;i++) {
+
+			real_t seg_from=from2D[i];
+			real_t seg_to=to2D[i];
+			real_t box_begin=-size[i];
+			real_t box_end=size[i];
+			real_t cmin,cmax;
+
+	
+			if (seg_from < seg_to) {
+	
+				if (seg_from > box_end || seg_to < box_begin)
+					return false;
+				real_t length=seg_to-seg_from;
+				cmin = (seg_from < box_begin)?((box_begin - seg_from)/length):0;
+				cmax = (seg_to > box_end)?((box_end - seg_from)/length):1;
+	
+			} else {
+	
+				if (seg_to > box_end || seg_from < box_begin)
+					return false;
+				real_t length=seg_to-seg_from;
+				cmin = (seg_from > box_end)?(box_end - seg_from)/length:0;
+				cmax = (seg_to < box_begin)?(box_begin - seg_from)/length:1;
+			}
+	
+			if (cmin > min) {
+				min = cmin;
+				axis=i;
+			}
+			if (cmax < max)
+				max = cmax;
+			if (max < min)
+				return false;
+		}
+
+
+		// convert to 3D again
+		Vector3 result = p_from + (rel*min);
+		Vector3 res_normal = result;
+
+		if (axis==0) {
+			res_normal.z=0;
+		} else {
+			res_normal.x=0;
+			res_normal.y=0;
+		}
+
+
+		res_normal.normalize();
+
+		if (r_res)
+			*r_res=result;
+		if (r_norm)
+			*r_norm=res_normal;
+
+		return true;
+	}
+
+
+	static bool segment_intersects_convex(const Vector3& p_from, const Vector3& p_to,const Plane* p_planes, int p_plane_count,Vector3 *p_res, Vector3 *p_norm) {
+
+		real_t min=-1e20,max=1e20;
+
+		Vector3 rel=p_to-p_from;
+		real_t rel_l=rel.length();
+
+		if (rel_l<CMP_EPSILON)
+			return false;
+
+		Vector3 dir=rel/rel_l;
+
+		int min_index=-1;
+
+		for (int i=0;i<p_plane_count;i++) {
+
+			const Plane&p=p_planes[i];
+
+			real_t den=p.normal.dot( dir );
+
+			//printf("den is %i\n",den);
+			if (Math::abs(den)<=CMP_EPSILON)
+				continue; // ignore parallel plane
+
+
+			real_t dist=-p.distance_to(p_from ) / den;
+
+			if (den>0) {
+				//backwards facing plane
+				if (dist<max)
+					max=dist;
+			} else {
+
+				//front facing plane
+				if (dist>min) {
+					min=dist;
+					min_index=i;
+				}
+			}
+		}
+
+		if (max<=min || min<0 || min>rel_l || min_index==-1) // exit conditions
+			return false; // no intersection
+
+		if (p_res)
+			*p_res=p_from+dir*min;
+		if (p_norm)
+			*p_norm=p_planes[min_index].normal;
+
+		return true;
+	}
+
+	static Vector3 get_closest_point_to_segment(const Vector3& p_point, const Vector3 *p_segment) {
+
+		Vector3 p=p_point-p_segment[0];
+		Vector3 n=p_segment[1]-p_segment[0];
+		float l =n.length();
+		if (l<1e-10)
+			return p_segment[0]; // both points are the same, just give any
+		n/=l;
+	
+		float d=n.dot(p);
+	
+		if (d<=0.0)
+			return p_segment[0]; // before first point
+		else if (d>=l)
+			return p_segment[1]; // after first point
+		else
+			return p_segment[0]+n*d; // inside
+	}
+
+	static Vector3 get_closest_point_to_segment_uncapped(const Vector3& p_point, const Vector3 *p_segment) {
+
+		Vector3 p=p_point-p_segment[0];
+		Vector3 n=p_segment[1]-p_segment[0];
+		float l =n.length();
+		if (l<1e-10)
+			return p_segment[0]; // both points are the same, just give any
+		n/=l;
+
+		float d=n.dot(p);
+
+		return p_segment[0]+n*d; // inside
+	}
+
+	static Vector2 get_closest_point_to_segment_2d(const Vector2& p_point, const Vector2 *p_segment) {
+
+		Vector2 p=p_point-p_segment[0];
+		Vector2 n=p_segment[1]-p_segment[0];
+		float l =n.length();
+		if (l<1e-10)
+			return p_segment[0]; // both points are the same, just give any
+		n/=l;
+
+		float d=n.dot(p);
+
+		if (d<=0.0)
+			return p_segment[0]; // before first point
+		else if (d>=l)
+			return p_segment[1]; // after first point
+		else
+			return p_segment[0]+n*d; // inside
+	}
+	static Vector2 get_closest_point_to_segment_uncapped_2d(const Vector2& p_point, const Vector2 *p_segment) {
+
+		Vector2 p=p_point-p_segment[0];
+		Vector2 n=p_segment[1]-p_segment[0];
+		float l =n.length();
+		if (l<1e-10)
+			return p_segment[0]; // both points are the same, just give any
+		n/=l;
+
+		float d=n.dot(p);
+
+		return p_segment[0]+n*d; // inside
+	}
+
+	static bool segment_intersects_segment_2d(const Vector2& p_from_a,const Vector2& p_to_a,const Vector2& p_from_b,const Vector2& p_to_b,Vector2* r_result) {
+
+		Vector2 B = p_to_a-p_from_a;
+		Vector2 C = p_from_b-p_from_a;
+		Vector2 D = p_to_b-p_from_a;
+
+		real_t ABlen = B.dot(B);
+		if (ABlen<=0)
+			return false;
+		Vector2 Bn = B/ABlen;
+		C = Vector2( C.x*Bn.x + C.y*Bn.y, C.y*Bn.x - C.x*Bn.y );
+		D = Vector2( D.x*Bn.x + D.y*Bn.y, D.y*Bn.x - D.x*Bn.y );
+
+		if ((C.y<0 && D.y<0) || (C.y>=0 && D.y>=0))
+			return false;
+
+		float ABpos=D.x+(C.x-D.x)*D.y/(D.y-C.y);
+
+		//  Fail if segment C-D crosses line A-B outside of segment A-B.
+		if (ABpos<0 || ABpos>1.0)
+			return false;
+
+		//  (4) Apply the discovered position to line A-B in the original coordinate system.
+		if (r_result)
+			*r_result=p_from_a+B*ABpos;
+
+		return true;
+	}
+
+
+	static inline bool point_in_projected_triangle(const Vector3& p_point,const Vector3& p_v1,const Vector3& p_v2,const Vector3& p_v3) {
+	
+	
+		Vector3 face_n =  (p_v1-p_v3).cross(p_v1-p_v2);		
+		
+		Vector3 n1 =  (p_point-p_v3).cross(p_point-p_v2);		
+		
+		if (face_n.dot(n1)<0)
+			return false;
+			
+		Vector3 n2 =  (p_v1-p_v3).cross(p_v1-p_point);		
+		
+		if (face_n.dot(n2)<0)
+			return false;
+		
+		Vector3 n3 =  (p_v1-p_point).cross(p_v1-p_v2);		
+	
+		if (face_n.dot(n3)<0)
+			return false;
+	
+		return true;
+	
+	}
+
+	static inline bool triangle_sphere_intersection_test(const Vector3 *p_triangle,const Vector3& p_normal,const Vector3& p_sphere_pos, real_t p_sphere_radius,Vector3& r_triangle_contact,Vector3& r_sphere_contact) {
+
+		float d=p_normal.dot(p_sphere_pos)-p_normal.dot(p_triangle[0]);
+
+		if (d > p_sphere_radius || d < -p_sphere_radius) // not touching the plane of the face, return
+			return false;
+
+		Vector3 contact=p_sphere_pos - (p_normal*d);
+
+		/** 2nd) TEST INSIDE TRIANGLE **/
+
+
+		if (Geometry::point_in_projected_triangle(contact,p_triangle[0],p_triangle[1],p_triangle[2])) {
+			r_triangle_contact=contact;
+			r_sphere_contact=p_sphere_pos-p_normal*p_sphere_radius;
+			//printf("solved inside triangle\n");
+			return true;
+		}
+
+		/** 3rd TEST INSIDE EDGE CYLINDERS **/
+
+		const Vector3 verts[4]={p_triangle[0],p_triangle[1],p_triangle[2],p_triangle[0]}; // for() friendly
+
+		for (int i=0;i<3;i++) {
+
+			// check edge cylinder
+
+			Vector3 n1=verts[i]-verts[i+1];
+			Vector3 n2=p_sphere_pos-verts[i+1];
+
+			///@TODO i could discard by range here to make the algorithm quicker? dunno..
+
+			// check point within cylinder radius
+			Vector3 axis =n1.cross(n2).cross(n1);
+			axis.normalize(); // ugh
+
+			float ad=axis.dot(n2);
+
+			if (ABS(ad)>p_sphere_radius) {
+				// no chance with this edge, too far away
+				continue;
+			}
+
+			// check point within edge capsule cylinder
+			/** 4th TEST INSIDE EDGE POINTS **/
+
+			float sphere_at = n1.dot(n2);
+
+			if (sphere_at>=0 && sphere_at<n1.dot(n1)) {
+
+				r_triangle_contact=p_sphere_pos-axis*(axis.dot(n2));
+				r_sphere_contact=p_sphere_pos-axis*p_sphere_radius;
+				// point inside here
+				//printf("solved inside edge\n");
+				return true;
+			}
+
+			float r2=p_sphere_radius*p_sphere_radius;
+
+			if (n2.length_squared()<r2) {
+
+				Vector3 n=(p_sphere_pos-verts[i+1]).normalized();
+
+				//r_triangle_contact=verts[i+1]+n*p_sphere_radius;p_sphere_pos+axis*(p_sphere_radius-axis.dot(n2));
+				r_triangle_contact=verts[i+1];
+				r_sphere_contact=p_sphere_pos-n*p_sphere_radius;
+				//printf("solved inside point segment 1\n");
+				return true;
+			}
+
+			if (n2.distance_squared_to(n1)<r2) {
+				Vector3 n=(p_sphere_pos-verts[i]).normalized();
+
+				//r_triangle_contact=verts[i]+n*p_sphere_radius;p_sphere_pos+axis*(p_sphere_radius-axis.dot(n2));
+				r_triangle_contact=verts[i];
+				r_sphere_contact=p_sphere_pos-n*p_sphere_radius;
+				//printf("solved inside point segment 1\n");
+				return true;
+			}
+
+			break; // It's pointless to continue at this point, so save some cpu cycles
+		}
+
+		return false;
+	}
+
+
+
+	static real_t segment_intersects_circle(const Vector2& p_from, const Vector2& p_to, const Vector2& p_circle_pos, real_t p_circle_radius) {
+
+			Vector2 line_vec = p_to - p_from;
+			Vector2 vec_to_line = p_from - p_circle_pos;
+
+			/* create a quadratic formula of the form ax^2 + bx + c = 0 */
+			real_t a, b, c;
+
+			a = line_vec.dot(line_vec);
+			b = 2 * vec_to_line.dot(line_vec);
+			c = vec_to_line.dot(vec_to_line) - p_circle_radius * p_circle_radius;
+
+			/* solve for t */
+			real_t sqrtterm = b*b - 4*a*c;
+
+			/* if the term we intend to square root is less than 0 then the answer won't be real, so it definitely won't be t in the range 0 to 1 */
+			if(sqrtterm < 0) return -1;
+
+			/* if we can assume that the line segment starts outside the circle (e.g. for continuous time collision detection) then the following can be skipped and we can just return the equivalent of res1 */
+			sqrtterm = Math::sqrt(sqrtterm);
+			real_t res1 = ( -b - sqrtterm ) / (2 * a);
+			//real_t res2 = ( -b + sqrtterm ) / (2 * a);
+
+			return  (res1 >= 0 && res1 <= 1) ? res1 : -1;
+
+	}
+
+
+	static Vector<int> triangulate_polygon(const Vector<Vector2>& p_polygon) {
+
+		Vector<int> triangles;
+		if (!Triangulate::triangulate(p_polygon,triangles))
+			return Vector<int>(); //fail
+		return triangles;
+	}
+
+	static Vector< Vector<Vector2> > (*_decompose_func)(const Vector<Vector2>& p_polygon);
+	static Vector< Vector<Vector2> > decompose_polygon(const Vector<Vector2>& p_polygon) {
+
+		if (_decompose_func)
+			return _decompose_func(p_polygon);
+
+		return Vector< Vector<Vector2> >();
+
+	}
+
+
+	static DVector< DVector< Face3 > > separate_objects( DVector< Face3 > p_array );
+
+	static DVector< Face3 > wrap_geometry( DVector< Face3 > p_array, float *p_error=NULL ); ///< create a "wrap" that encloses the given geometry
+
+
+	struct MeshData {
+		
+		struct Face {
+			Plane plane;
+			Vector<int> indices;
+		};
+		
+		Vector<Face> faces;
+		
+		struct Edge {
+		
+			int a,b;
+		};
+		
+		Vector<Edge> edges;
+		
+		Vector< Vector3 > vertices;
+
+		void optimize_vertices();
+	};
+
+
+	static MeshData build_convex_mesh(const DVector<Plane> &p_planes);
+	static DVector<Plane> build_sphere_planes(float p_radius, int p_lats, int p_lons, Vector3::Axis p_axis=Vector3::AXIS_Z);
+	static DVector<Plane> build_box_planes(const Vector3& p_extents);
+	static DVector<Plane> build_cylinder_planes(float p_radius, float p_height, int p_sides, Vector3::Axis p_axis=Vector3::AXIS_Z);
+	static DVector<Plane> build_capsule_planes(float p_radius, float p_height, int p_sides, int p_lats, Vector3::Axis p_axis=Vector3::AXIS_Z);
+	
+	
+};
+
+
+
+#endif
diff --git a/core/math/math_2d.cpp b/core/math/math_2d.cpp
new file mode 100644
index 0000000000..dbeaea6e75
--- /dev/null
+++ b/core/math/math_2d.cpp
@@ -0,0 +1,597 @@
+/*************************************************************************/
+/*  math_2d.cpp                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "math_2d.h"
+
+
+real_t Vector2::atan2() const {
+
+	return Math::atan2(x,y);
+}
+
+float Vector2::length() const {
+
+	return Math::sqrt( x*x + y*y );
+}
+
+float Vector2::length_squared() const {
+
+	return x*x + y*y;
+}
+
+void Vector2::normalize() {
+	
+	float l = x*x + y*y;
+	if (l!=0) {
+		
+		l=Math::sqrt(l);
+		x/=l;
+		y/=l;
+	}
+}
+
+Vector2 Vector2::normalized() const {
+	
+	Vector2 v=*this;
+	v.normalize();
+	return v;
+}
+
+float Vector2::distance_to(const Vector2& p_vector2) const {
+	
+	return Math::sqrt( (x-p_vector2.x)*(x-p_vector2.x) + (y-p_vector2.y)*(y-p_vector2.y));
+}
+
+float Vector2::distance_squared_to(const Vector2& p_vector2) const {
+
+	return (x-p_vector2.x)*(x-p_vector2.x) + (y-p_vector2.y)*(y-p_vector2.y);
+}
+
+float Vector2::angle_to(const Vector2& p_vector2) const  {
+	
+	return Math::atan2( x-p_vector2.x, y - p_vector2.y );
+}
+
+float Vector2::dot(const Vector2& p_other) const {
+	
+	return x*p_other.x + y*p_other.y;
+}
+
+float Vector2::cross(const Vector2& p_other) const {
+
+	return x*p_other.y - y*p_other.x;
+}
+
+Vector2 Vector2::cross(real_t p_other) const {
+
+	return Vector2(p_other*y,-p_other*x);
+}
+
+
+Vector2 Vector2::operator+(const Vector2& p_v) const {
+	
+	return Vector2(x+p_v.x,y+p_v.y);
+}
+void Vector2::operator+=(const Vector2& p_v) {
+	
+	x+=p_v.x; y+=p_v.y;
+}
+Vector2 Vector2::operator-(const Vector2& p_v) const {
+	
+	return Vector2(x-p_v.x,y-p_v.y);
+}
+void Vector2::operator-=(const Vector2& p_v) {
+	
+	x-=p_v.x; y-=p_v.y;
+}
+
+Vector2 Vector2::operator*(const Vector2 &p_v1) const {
+	
+	return Vector2(x * p_v1.x, y * p_v1.y);
+};
+
+Vector2 Vector2::operator*(const float &rvalue) const {
+	
+	return Vector2(x * rvalue, y * rvalue);
+};
+void Vector2::operator*=(const float &rvalue) {
+	
+	x *= rvalue; y *= rvalue;
+};
+
+Vector2 Vector2::operator/(const Vector2 &p_v1) const {
+	
+	return Vector2(x / p_v1.x, y / p_v1.y);
+};
+
+Vector2 Vector2::operator/(const float &rvalue) const {
+	
+	return Vector2(x / rvalue, y / rvalue);
+};
+
+void Vector2::operator/=(const float &rvalue) {
+	
+	x /= rvalue; y /= rvalue;
+};
+
+Vector2 Vector2::operator-() const {
+	
+	return Vector2(-x,-y);
+}
+
+bool Vector2::operator==(const Vector2& p_vec2) const {
+	
+	return x==p_vec2.x && y==p_vec2.y;
+}
+bool Vector2::operator!=(const Vector2& p_vec2) const {
+	
+	return x!=p_vec2.x || y!=p_vec2.y;
+}
+Vector2 Vector2::floor() const {
+
+	return Vector2( Math::floor(x), Math::floor(y) );
+}
+
+Vector2 Vector2::rotated(float p_by) const {
+
+	Vector2 v;
+	v.set_rotation(atan2()+p_by);
+	v*=length();
+	return v;
+}
+
+Vector2 Vector2::project(const Vector2& p_vec) const {
+
+	Vector2 v1=p_vec;
+	Vector2 v2=*this;
+	return v2 * ( v1.dot(v2) / v2.dot(v2));
+}
+
+Vector2 Vector2::snapped(const Vector2& p_by) const {
+
+	return Vector2(
+		Math::stepify(x,p_by.x),
+		Math::stepify(y,p_by.y)
+	);
+}
+
+Vector2 Vector2::clamped(real_t p_len) const {
+
+	return *this;
+	real_t l = length();
+	Vector2 v = *this;
+	if (l>0 && p_len<l) {
+
+		v/=l;
+		v*=p_len;
+	}
+
+	return v;
+}
+
+Vector2 Vector2::cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const {
+#if 0
+	k[0] = ((*this) (vi[0] + 1, vi[1], vi[2])) - ((*this) (vi[0],
+	vi[1],vi[2])); //fk = a0
+	k[1] = (((*this) (vi[0] + 1, vi[1], vi[2])) - ((*this) ((int) (v(0) -
+	1), vi[1],vi[2])))*0.5; //dk = a1
+	k[2] = (((*this) ((int) (v(0) + 2), vi[1], vi[2])) - ((*this) (vi[0],
+	vi[1],vi[2])))*0.5; //dk+1
+	k[3] = k[0]*3 - k[1]*2 - k[2];//a2
+	k[4] = k[1] + k[2] - k[0]*2;//a3
+
+	//ip = a3(t-tk)³ + a2(t-tk)² + a1(t-tk) + a0
+	//
+	//a3 = dk + dk+1 - Dk
+	//a2 = 3Dk - 2dk - dk+1
+	//a1 = dk
+	//a0 = fk
+	//
+	//dk = (fk+1 - fk-1)*0.5
+	//Dk = (fk+1 - fk)
+
+	float dk =
+#endif
+
+	return Vector2();
+}
+
+Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const {
+
+
+
+	float mu = p_t;
+	float mu2 = mu*mu;
+
+	Vector2 a0 = p_post_b - p_b - p_pre_a + *this;
+	Vector2 a1 = p_pre_a - *this - a0;
+	Vector2 a2 = p_b - p_pre_a;
+	Vector2 a3 = *this;
+
+	return ( a0*mu*mu2 + a1*mu2 + a2*mu + a3 );
+
+	/*
+	float t = p_t;
+	real_t t2 = t*t;
+	real_t t3 = t2*t;
+
+	real_t a =  2.0*t3- 3.0*t2 + 1;
+	real_t b = -2.0*t3+ 3.0*t2;
+	real_t c =    t3- 2.0*t2 + t;
+	real_t d =    t3- t2;
+
+	Vector2 p_a=*this;
+
+	return Vector2(
+		(a * p_a.x) + (b *p_b.x) + (c * p_pre_a.x) + (d * p_post_b.x),
+		(a * p_a.y) + (b *p_b.y) + (c * p_pre_a.y) + (d * p_post_b.y)
+	);
+*/
+
+}
+
+
+bool Rect2::intersects_segment(const Point2& p_from, const Point2& p_to, Point2* r_pos,Point2* r_normal) const {
+
+	real_t min=0,max=1;
+	int axis=0;
+	float sign=0;
+
+	for(int i=0;i<2;i++) {
+		real_t seg_from=p_from[i];
+		real_t seg_to=p_to[i];
+		real_t box_begin=pos[i];
+		real_t box_end=box_begin+size[i];
+		real_t cmin,cmax;
+		float csign;
+
+		if (seg_from < seg_to) {
+
+			if (seg_from > box_end || seg_to < box_begin)
+				return false;
+			real_t length=seg_to-seg_from;
+			cmin = (seg_from < box_begin)?((box_begin - seg_from)/length):0;
+			cmax = (seg_to > box_end)?((box_end - seg_from)/length):1;
+			csign=-1.0;
+
+		} else {
+
+			if (seg_to > box_end || seg_from < box_begin)
+				return false;
+			real_t length=seg_to-seg_from;
+			cmin = (seg_from > box_end)?(box_end - seg_from)/length:0;
+			cmax = (seg_to < box_begin)?(box_begin - seg_from)/length:1;
+			csign=1.0;
+		}
+
+		if (cmin > min) {
+			min = cmin;
+			axis=i;
+			sign=csign;
+		}
+		if (cmax < max)
+			max = cmax;
+		if (max < min)
+			return false;
+	}
+
+
+	Vector2 rel=p_to-p_from;
+
+	if (r_normal) {
+		Vector2 normal;
+		normal[axis]=sign;
+		*r_normal=normal;
+	}
+
+	if (r_pos)
+		*r_pos=p_from+rel*min;
+
+	return true;
+}
+
+/* Point2i */
+
+Point2i Point2i::operator+(const Point2i& p_v) const {
+
+	return Point2i(x+p_v.x,y+p_v.y);
+}
+void Point2i::operator+=(const Point2i& p_v) {
+
+	x+=p_v.x; y+=p_v.y;
+}
+Point2i Point2i::operator-(const Point2i& p_v) const {
+
+	return Point2i(x-p_v.x,y-p_v.y);
+}
+void Point2i::operator-=(const Point2i& p_v) {
+
+	x-=p_v.x; y-=p_v.y;
+}
+
+Point2i Point2i::operator*(const Point2i &p_v1) const {
+
+	return Point2i(x * p_v1.x, y * p_v1.y);
+};
+
+Point2i Point2i::operator*(const int &rvalue) const {
+
+	return Point2i(x * rvalue, y * rvalue);
+};
+void Point2i::operator*=(const int &rvalue) {
+
+	x *= rvalue; y *= rvalue;
+};
+
+Point2i Point2i::operator/(const Point2i &p_v1) const {
+
+	return Point2i(x / p_v1.x, y / p_v1.y);
+};
+
+Point2i Point2i::operator/(const int &rvalue) const {
+
+	return Point2i(x / rvalue, y / rvalue);
+};
+
+void Point2i::operator/=(const int &rvalue) {
+
+	x /= rvalue; y /= rvalue;
+};
+
+Point2i Point2i::operator-() const {
+
+	return Point2i(-x,-y);
+}
+
+bool Point2i::operator==(const Point2i& p_vec2) const {
+
+	return x==p_vec2.x && y==p_vec2.y;
+}
+bool Point2i::operator!=(const Point2i& p_vec2) const {
+
+	return x!=p_vec2.x || y!=p_vec2.y;
+}
+
+void Matrix32::invert() {
+
+	SWAP(elements[0][1],elements[1][0]);
+	elements[2] = basis_xform(-elements[2]);
+}
+
+Matrix32 Matrix32::inverse() const {
+
+	Matrix32 inv=*this;
+	inv.invert();
+	return inv;
+
+}
+
+void Matrix32::affine_invert() {
+
+	float det = elements[0][0]*elements[1][1] - elements[1][0]*elements[0][1];
+	ERR_FAIL_COND(det==0);
+	float idet = 1.0 / det;
+
+	SWAP( elements[0][0],elements[1][1] );
+	elements[0]*=Vector2(idet,-idet);
+	elements[1]*=Vector2(-idet,idet);
+
+	elements[2] = basis_xform(-elements[2]);
+
+}
+
+Matrix32 Matrix32::affine_inverse() const {
+
+	Matrix32 inv=*this;
+	inv.affine_invert();
+	return inv;
+}
+
+void Matrix32::rotate(real_t p_phi) {
+
+	Matrix32 rot(p_phi,Vector2());
+	*this *= rot;
+}
+
+real_t Matrix32::get_rotation() const {
+
+	return Math::atan2(elements[1].x,elements[1].y);
+}
+
+void Matrix32::set_rotation(real_t p_rot) {
+
+	real_t cr = Math::cos(p_rot);
+	real_t sr = Math::sin(p_rot);
+	elements[0][0]=cr;
+	elements[1][1]=cr;
+	elements[0][1]=-sr;
+	elements[1][0]=sr;
+}
+
+Matrix32::Matrix32(real_t p_rot, const Vector2& p_pos) {
+
+	real_t cr = Math::cos(p_rot);
+	real_t sr = Math::sin(p_rot);
+	elements[0][0]=cr;
+	elements[1][1]=cr;
+	elements[0][1]=-sr;
+	elements[1][0]=sr;
+	elements[2]=p_pos;
+}
+
+Vector2 Matrix32::get_scale() const {
+
+	return Vector2( elements[0].length(), elements[1].length() );
+}
+
+void Matrix32::scale(const Vector2& p_scale) {
+
+	elements[0]*=p_scale;
+	elements[1]*=p_scale;
+	elements[2]*=p_scale;
+}
+void Matrix32::scale_basis(const Vector2& p_scale) {
+
+	elements[0]*=p_scale;
+	elements[1]*=p_scale;
+
+}
+void Matrix32::translate( real_t p_tx, real_t p_ty) {
+
+	translate(Vector2(p_tx,p_ty));
+}
+void Matrix32::translate( const Vector2& p_translation ) {
+
+	elements[2]+=basis_xform(p_translation);
+}
+
+
+void Matrix32::orthonormalize() {
+
+	// Gram-Schmidt Process
+
+	Vector2 x=elements[0];
+	Vector2 y=elements[1];
+
+	x.normalize();
+	y = (y-x*(x.dot(y)));
+	y.normalize();
+
+	elements[0]=x;
+	elements[1]=y;
+}
+Matrix32 Matrix32::orthonormalized() const {
+
+	Matrix32 on=*this;
+	on.orthonormalize();
+	return on;
+
+}
+
+bool Matrix32::operator==(const Matrix32& p_transform) const {
+
+	for(int i=0;i<3;i++) {
+		if (elements[i]!=p_transform.elements[i])
+			return false;
+	}
+
+	return true;
+}
+
+bool Matrix32::operator!=(const Matrix32& p_transform) const {
+
+	for(int i=0;i<3;i++) {
+		if (elements[i]!=p_transform.elements[i])
+			return true;
+	}
+
+	return false;
+
+}
+
+void Matrix32::operator*=(const Matrix32& p_transform) {
+
+	elements[2] = xform(p_transform.elements[2]);
+
+	float x0,x1,y0,y1;
+/*
+	x0 = p_transform.tdotx(elements[0]);
+	x1 = p_transform.tdoty(elements[0]);
+	y0 = p_transform.tdotx(elements[1]);
+	y1 = p_transform.tdoty(elements[1]);*/
+
+	x0 = tdotx(p_transform.elements[0]);
+	x1 = tdoty(p_transform.elements[0]);
+	y0 = tdotx(p_transform.elements[1]);
+	y1 = tdoty(p_transform.elements[1]);
+
+	elements[0][0]=x0;
+	elements[0][1]=x1;
+	elements[1][0]=y0;
+	elements[1][1]=y1;
+}
+
+
+Matrix32 Matrix32::operator*(const Matrix32& p_transform) const {
+
+	Matrix32 t = *this;
+	t*=p_transform;
+	return t;
+
+}
+
+Matrix32 Matrix32::scaled(const Vector2& p_scale) const {
+
+	Matrix32 copy=*this;
+	copy.scale(p_scale);
+	return copy;
+
+}
+
+Matrix32 Matrix32::basis_scaled(const Vector2& p_scale) const {
+
+	Matrix32 copy=*this;
+	copy.scale_basis(p_scale);
+	return copy;
+
+}
+
+Matrix32 Matrix32::untranslated() const {
+
+	Matrix32 copy=*this;
+	copy.elements[2]=Vector2();
+	return copy;
+}
+
+Matrix32 Matrix32::translated(const Vector2& p_offset) const {
+
+	Matrix32 copy=*this;
+	copy.translate(p_offset);
+	return copy;
+
+}
+
+Matrix32 Matrix32::rotated(float p_phi) const {
+
+	Matrix32 copy=*this;
+	copy.rotate(p_phi);
+	return copy;
+
+}
+
+
+Matrix32 Matrix32::interpolate_with(const Matrix32& p_transform, float p_c) const {
+
+
+	return Matrix32();
+}
+
+Matrix32::operator String() const {
+
+	return String(String()+elements[0]+", "+elements[1]+", "+elements[2]);
+}
diff --git a/core/math/math_2d.h b/core/math/math_2d.h
new file mode 100644
index 0000000000..212d848bcb
--- /dev/null
+++ b/core/math/math_2d.h
@@ -0,0 +1,670 @@
+/*************************************************************************/
+/*  math_2d.h                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef MATH_2D_H
+#define MATH_2D_H
+
+#include "math_funcs.h"
+#include "ustring.h"
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+enum Margin {
+	
+	MARGIN_LEFT,
+	MARGIN_TOP,
+	MARGIN_RIGHT,
+	MARGIN_BOTTOM	
+};
+
+enum Orientation {
+	
+	HORIZONTAL,
+	VERTICAL
+};
+
+enum HAlign {
+
+	HALIGN_LEFT,
+	HALIGN_CENTER,
+	HALIGN_RIGHT
+};
+
+enum VAlign {
+
+	VALIGN_TOP,
+	VALIGN_CENTER,
+	VALIGN_BOTTOM
+};
+
+struct Vector2 {
+	
+	union {
+		float x;
+		float width;
+	};
+	union {
+		float y;
+		float height;
+	};
+
+
+	_FORCE_INLINE_ float& operator[](int p_idx) {
+		return p_idx?y:x;
+	}
+	_FORCE_INLINE_ const float& operator[](int p_idx) const {
+		return p_idx?y:x;
+	}
+
+	void normalize();
+	Vector2 normalized() const;
+
+	float length() const;
+	float length_squared() const;
+
+	float distance_to(const Vector2& p_vector2) const; 
+	float distance_squared_to(const Vector2& p_vector2) const;
+	float angle_to(const Vector2& p_vector2) const;
+	
+	float dot(const Vector2& p_other) const;
+	float cross(const Vector2& p_other) const;
+	Vector2 cross(real_t p_other) const;
+	Vector2 project(const Vector2& p_vec) const;
+
+	Vector2 plane_project(real_t p_d, const Vector2& p_vec) const;
+
+	Vector2 clamped(real_t p_len) const;
+
+	_FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2& p_a, const Vector2& p_b,float p_t);
+	_FORCE_INLINE_ Vector2 linear_interpolate(const Vector2& p_b,float p_t) const;
+	Vector2 cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const;
+	Vector2 cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const;
+
+
+	Vector2 operator+(const Vector2& p_v) const;
+	void operator+=(const Vector2& p_v);
+	Vector2 operator-(const Vector2& p_v) const;
+	void operator-=(const Vector2& p_v);
+	Vector2 operator*(const Vector2 &p_v1) const;
+	
+	Vector2 operator*(const float &rvalue) const;
+	void operator*=(const float &rvalue);
+	void operator*=(const Vector2 &rvalue) { *this = *this * rvalue; }
+
+	Vector2 operator/(const Vector2 &p_v1) const;
+	
+	Vector2 operator/(const float &rvalue) const;
+	
+	void operator/=(const float &rvalue);
+	
+	Vector2 operator-() const;
+	
+	bool operator==(const Vector2& p_vec2) const;
+	bool operator!=(const Vector2& p_vec2) const;
+
+	bool operator<(const Vector2& p_vec2) const { return (x==p_vec2.x)?(y<p_vec2.y):(x<p_vec2.x); }
+	bool operator<=(const Vector2& p_vec2) const { return (x==p_vec2.x)?(y<=p_vec2.y):(x<=p_vec2.x); }
+
+	real_t atan2() const;
+
+	void set_rotation(float p_radians) {
+
+		x=Math::sin(p_radians);
+		y=Math::cos(p_radians);
+	}
+
+	_FORCE_INLINE_ Vector2 abs() const {
+
+		return Vector2( Math::abs(x), Math::abs(y) );
+	}
+
+	Vector2 rotated(float p_by) const;
+	Vector2 tangent() const {
+
+		return Vector2(y,-x);
+	}
+	
+	Vector2 floor() const;
+	Vector2 snapped(const Vector2& p_by) const;
+	float get_aspect() const { return width/height; }
+
+	
+	operator String() const { return String::num(x)+","+String::num(y); }
+	
+	inline Vector2(float p_x,float p_y) { x=p_x; y=p_y; }
+	inline Vector2() { x=0; y=0; }
+};
+
+_FORCE_INLINE_ Vector2 Vector2::plane_project(real_t p_d, const Vector2& p_vec) const {
+
+	return  p_vec - *this * ( dot(p_vec) -p_d);
+}
+
+
+_FORCE_INLINE_ Vector2 operator*(float p_scalar, const Vector2& p_vec)  {
+
+	return p_vec*p_scalar;
+}
+
+Vector2 Vector2::linear_interpolate(const Vector2& p_b,float p_t) const {
+
+	Vector2 res=*this;
+
+	res.x+= (p_t * (p_b.x-x));
+	res.y+= (p_t * (p_b.y-y));
+
+	return res;
+
+}
+
+Vector2 Vector2::linear_interpolate(const Vector2& p_a, const Vector2& p_b,float p_t)  {
+
+	Vector2 res=p_a;
+
+	res.x+= (p_t * (p_b.x-p_a.x));
+	res.y+= (p_t * (p_b.y-p_a.y));
+
+	return res;
+}
+
+typedef Vector2 Size2;
+typedef Vector2 Point2;
+
+
+struct Rect2 {
+	
+	Point2 pos;
+	Size2 size;
+
+	const Vector2& get_pos() const { return pos; }
+	void set_pos(const Vector2& p_pos) { pos=p_pos; }
+	const Vector2& get_size() const { return size; }
+	void set_size(const Vector2& p_size) { size=p_size; }
+
+	float get_area() const { return size.width*size.height; }
+
+	inline bool intersects(const Rect2& p_rect) const { 
+		if ( pos.x > (p_rect.pos.x + p_rect.size.width) ) 
+			return false;
+		if ( (pos.x+size.width) < p_rect.pos.x  ) 
+			return false;
+		if ( pos.y > (p_rect.pos.y + p_rect.size.height) ) 
+			return false;
+		if ( (pos.y+size.height) < p_rect.pos.y  ) 
+			return false;
+		
+		return true;
+	}
+
+	bool intersects_segment(const Point2& p_from, const Point2& p_to, Point2* r_pos=NULL, Point2* r_normal=NULL) const;
+
+	inline bool encloses(const Rect2& p_rect) const {
+		
+		return 	(p_rect.pos.x>=pos.x) && (p_rect.pos.y>=pos.y) &&
+			((p_rect.pos.x+p_rect.size.x)<(pos.x+size.x)) &&
+			((p_rect.pos.y+p_rect.size.y)<(pos.y+size.y));
+		
+	}
+	
+	inline bool has_no_area() const {
+		
+		return (size.x<=0 || size.y<=0);
+		
+	}
+	inline Rect2 clip(const Rect2& p_rect) const { /// return a clipped rect
+		
+		Rect2 new_rect=p_rect;
+		
+		if (!intersects( new_rect ))
+			return Rect2();
+		
+		new_rect.pos.x = MAX( p_rect.pos.x , pos.x );
+		new_rect.pos.y = MAX( p_rect.pos.y , pos.y );
+		
+		Point2 p_rect_end=p_rect.pos+p_rect.size;
+		Point2 end=pos+size;
+		
+		new_rect.size.x=MIN(p_rect_end.x,end.x) - new_rect.pos.x;
+		new_rect.size.y=MIN(p_rect_end.y,end.y) - new_rect.pos.y;
+		
+		return new_rect;
+	}
+	
+	inline Rect2 merge(const Rect2& p_rect) const { ///< return a merged rect
+		
+		Rect2 new_rect;
+		
+		new_rect.pos.x=MIN( p_rect.pos.x , pos.x );
+		new_rect.pos.y=MIN( p_rect.pos.y , pos.y );
+		
+		
+		new_rect.size.x = MAX( p_rect.pos.x+p_rect.size.x , pos.x+size.x );
+		new_rect.size.y = MAX( p_rect.pos.y+p_rect.size.y , pos.y+size.y );
+		
+		new_rect.size = new_rect.size - new_rect.pos; //make relative again
+		
+		return new_rect;
+	};
+	bool has_point(const Point2& p_point) const {
+		if (p_point.x < pos.x)
+			return false; 
+		if (p_point.y < pos.y)
+			return false; 
+		
+		if (p_point.x >= (pos.x+size.x) )
+			return false; 
+		if (p_point.y >= (pos.y+size.y) )
+			return false; 
+		
+		return true;
+	}
+	
+	bool no_area() const { return (size.width<=0 || size.height<=0 ); }
+	
+	bool operator==(const Rect2& p_rect) const { return pos==p_rect.pos && size==p_rect.size; }
+	bool operator!=(const Rect2& p_rect) const { return pos!=p_rect.pos || size!=p_rect.size; }
+	
+	Rect2 grow(real_t p_by) const {
+		
+		Rect2 g=*this;
+		g.pos.x-=p_by;
+		g.pos.y-=p_by;
+		g.size.width+=p_by*2;
+		g.size.height+=p_by*2;
+		return g;
+	}
+
+	inline Rect2 expand(const Vector2& p_vector) const {
+
+		Rect2 r = *this;
+		r.expand_to(p_vector);
+		return r;
+	}
+
+	inline void expand_to(const Vector2& p_vector) { //in place function for speed
+
+		Vector2 begin=pos;
+		Vector2 end=pos+size;
+
+		if (p_vector.x<begin.x)
+			begin.x=p_vector.x;
+		if (p_vector.y<begin.y)
+			begin.y=p_vector.y;
+
+		if (p_vector.x>end.x)
+			end.x=p_vector.x;
+		if (p_vector.y>end.y)
+			end.y=p_vector.y;
+
+		pos=begin;
+		size=end-begin;
+	}
+
+
+	operator String() const { return String(pos)+","+String(size); }
+	
+	Rect2() {}
+	Rect2( float p_x, float p_y, float p_width, float p_height) { pos=Point2(p_x,p_y); size=Size2( p_width, p_height ); }   
+	Rect2( const Point2& p_pos, const Size2& p_size ) { pos=p_pos; size=p_size; }
+};
+
+
+/* INTEGER STUFF */
+
+struct Point2i {
+
+	union {
+		int x;
+		int width;
+	};
+	union {
+		int y;
+		int height;
+	};
+
+
+	_FORCE_INLINE_ int& operator[](int p_idx) {
+		return p_idx?y:x;
+	}
+	_FORCE_INLINE_ const int& operator[](int p_idx) const {
+		return p_idx?y:x;
+	}
+
+	Point2i operator+(const Point2i& p_v) const;
+	void operator+=(const Point2i& p_v);
+	Point2i operator-(const Point2i& p_v) const;
+	void operator-=(const Point2i& p_v);
+	Point2i operator*(const Point2i &p_v1) const;
+
+	Point2i operator*(const int &rvalue) const;
+	void operator*=(const int &rvalue);
+
+	Point2i operator/(const Point2i &p_v1) const;
+
+	Point2i operator/(const int &rvalue) const;
+
+	void operator/=(const int &rvalue);
+
+	Point2i operator-() const;
+	bool operator<(const Point2i& p_vec2) const { return (x==p_vec2.x)?(y<p_vec2.y):(x<p_vec2.x); }
+	bool operator>(const Point2i& p_vec2) const { return (x==p_vec2.x)?(y>p_vec2.y):(x>p_vec2.x); }
+
+	bool operator==(const Point2i& p_vec2) const;
+	bool operator!=(const Point2i& p_vec2) const;
+
+	float get_aspect() const { return width/(float)height; }
+
+	operator String() const { return String::num(x)+","+String::num(y); }
+
+	operator Vector2() const { return Vector2(x,y); }
+	inline Point2i(const Vector2& p_vec2) { x=(int)p_vec2.x; y=(int)p_vec2.y; }
+	inline Point2i(int p_x,int p_y) { x=p_x; y=p_y; }
+	inline Point2i() { x=0; y=0; }
+};
+
+typedef Point2i Size2i;
+
+struct Rect2i {
+
+	Point2i pos;
+	Size2i size;
+
+	const Point2i& get_pos() const { return pos; }
+	void set_pos(const Point2i& p_pos) { pos=p_pos; }
+	const Point2i& get_size() const { return size; }
+	void set_size(const Point2i& p_size) { size=p_size; }
+
+	int get_area() const { return size.width*size.height; }
+
+	inline bool intersects(const Rect2i& p_rect) const {
+		if ( pos.x > (p_rect.pos.x + p_rect.size.width) )
+			return false;
+		if ( (pos.x+size.width) < p_rect.pos.x  )
+			return false;
+		if ( pos.y > (p_rect.pos.y + p_rect.size.height) )
+			return false;
+		if ( (pos.y+size.height) < p_rect.pos.y  )
+			return false;
+
+		return true;
+	}
+
+	inline bool encloses(const Rect2i& p_rect) const {
+
+		return 	(p_rect.pos.x>=pos.x) && (p_rect.pos.y>=pos.y) &&
+			((p_rect.pos.x+p_rect.size.x)<(pos.x+size.x)) &&
+			((p_rect.pos.y+p_rect.size.y)<(pos.y+size.y));
+
+	}
+
+	inline bool has_no_area() const {
+
+		return (size.x<=0 || size.y<=0);
+
+	}
+	inline Rect2i clip(const Rect2i& p_rect) const { /// return a clipped rect
+
+		Rect2i new_rect=p_rect;
+
+		if (!intersects( new_rect ))
+			return Rect2i();
+
+		new_rect.pos.x = MAX( p_rect.pos.x , pos.x );
+		new_rect.pos.y = MAX( p_rect.pos.y , pos.y );
+
+		Point2 p_rect_end=p_rect.pos+p_rect.size;
+		Point2 end=pos+size;
+
+		new_rect.size.x=(int)(MIN(p_rect_end.x,end.x) - new_rect.pos.x);
+		new_rect.size.y=(int)(MIN(p_rect_end.y,end.y) - new_rect.pos.y);
+
+		return new_rect;
+	}
+
+	inline Rect2i merge(const Rect2i& p_rect) const { ///< return a merged rect
+
+		Rect2i new_rect;
+
+		new_rect.pos.x=MIN( p_rect.pos.x , pos.x );
+		new_rect.pos.y=MIN( p_rect.pos.y , pos.y );
+
+
+		new_rect.size.x = MAX( p_rect.pos.x+p_rect.size.x , pos.x+size.x );
+		new_rect.size.y = MAX( p_rect.pos.y+p_rect.size.y , pos.y+size.y );
+
+		new_rect.size = new_rect.size - new_rect.pos; //make relative again
+
+		return new_rect;
+	};
+	bool has_point(const Point2& p_point) {
+		if (p_point.x < pos.x)
+			return false;
+		if (p_point.y < pos.y)
+			return false;
+
+		if (p_point.x >= (pos.x+size.x) )
+			return false;
+		if (p_point.y >= (pos.y+size.y) )
+			return false;
+
+		return true;
+	}
+
+	bool no_area() { return (size.width<=0 || size.height<=0 ); }
+
+	bool operator==(const Rect2i& p_rect) const { return pos==p_rect.pos && size==p_rect.size; }
+	bool operator!=(const Rect2i& p_rect) const { return pos!=p_rect.pos || size!=p_rect.size; }
+
+	Rect2i grow(int p_by) const {
+
+		Rect2i g=*this;
+		g.pos.x-=p_by;
+		g.pos.y-=p_by;
+		g.size.width+=p_by*2;
+		g.size.height+=p_by*2;
+		return g;
+	}
+
+	inline void expand_to(const Point2i& p_vector) {
+
+		Point2i begin=pos;
+		Point2i end=pos+size;
+
+		if (p_vector.x<begin.x)
+			begin.x=p_vector.x;
+		if (p_vector.y<begin.y)
+			begin.y=p_vector.y;
+
+		if (p_vector.x>end.x)
+			end.x=p_vector.x;
+		if (p_vector.y>end.y)
+			end.y=p_vector.y;
+
+		pos=begin;
+		size=end-begin;
+	}
+
+
+	operator String() const { return String(pos)+","+String(size); }
+
+	operator Rect2() const { return Rect2(pos,size); }
+	Rect2i(const Rect2& p_r2) { pos=p_r2.pos; size=p_r2.size; }
+	Rect2i() {}
+	Rect2i( int p_x, int p_y, int p_width, int p_height) { pos=Point2(p_x,p_y); size=Size2( p_width, p_height ); }
+	Rect2i( const Point2& p_pos, const Size2& p_size ) { pos=p_pos; size=p_size; }
+};
+
+
+
+struct Matrix32 {
+
+
+	Vector2 elements[3];
+
+
+	_FORCE_INLINE_ float tdotx(const Vector2& v) const { return elements[0][0] * v.x + elements[1][0] * v.y; }
+	_FORCE_INLINE_ float tdoty(const Vector2& v) const { return elements[0][1] * v.x + elements[1][1] * v.y; }
+
+	const Vector2& operator[](int p_idx) const { return elements[p_idx]; }
+	Vector2& operator[](int p_idx) { return elements[p_idx]; }
+
+	_FORCE_INLINE_ Vector2 get_axis(int p_axis) const { ERR_FAIL_INDEX_V(p_axis,3,Vector2()); return elements[p_axis]; }
+	_FORCE_INLINE_ void set_axis(int p_axis,const Vector2& p_vec) { ERR_FAIL_INDEX(p_axis,3); elements[p_axis]=p_vec; }
+
+	void invert();
+	Matrix32 inverse() const;
+
+	void affine_invert();
+	Matrix32 affine_inverse() const;
+
+	void set_rotation(real_t p_phi);
+	real_t get_rotation() const;
+	_FORCE_INLINE_ void set_rotation_and_scale(real_t p_phi,const Size2& p_scale);
+	void rotate(real_t p_phi);
+
+	void scale(const Vector2& p_scale);
+	void scale_basis(const Vector2& p_scale);
+	void translate( real_t p_tx, real_t p_ty);
+	void translate( const Vector2& p_translation );
+	Vector2 get_scale() const;
+
+	_FORCE_INLINE_ const Vector2& get_origin() const { return elements[2]; }
+	_FORCE_INLINE_ void set_origin(const Vector2& p_origin) { elements[2]=p_origin; }
+
+	Matrix32 scaled(const Vector2& p_scale) const;
+	Matrix32 basis_scaled(const Vector2& p_scale) const;
+	Matrix32 translated(const Vector2& p_offset) const;
+	Matrix32 rotated(float p_phi) const;
+
+	Matrix32 untranslated() const;
+
+
+	void orthonormalize();
+	Matrix32 orthonormalized() const;
+
+	bool operator==(const Matrix32& p_transform) const;
+	bool operator!=(const Matrix32& p_transform) const;
+
+	void operator*=(const Matrix32& p_transform);
+	Matrix32 operator*(const Matrix32& p_transform) const;
+
+	Matrix32 interpolate_with(const Matrix32& p_transform, float p_c) const;
+
+	_FORCE_INLINE_ Vector2 basis_xform(const Vector2& p_vec) const;
+	_FORCE_INLINE_ Vector2 basis_xform_inv(const Vector2& p_vec) const;
+	_FORCE_INLINE_ Vector2 xform(const Vector2& p_vec) const;
+	_FORCE_INLINE_ Vector2 xform_inv(const Vector2& p_vec) const;
+	_FORCE_INLINE_ Rect2 xform(const Rect2& p_vec) const;
+	_FORCE_INLINE_ Rect2 xform_inv(const Rect2& p_vec) const;
+
+
+	operator String() const;
+
+
+	Matrix32(real_t p_rot, const Vector2& p_pos);
+	Matrix32() { elements[0][0]=1.0; elements[1][1]=1.0; }
+
+};
+
+
+Vector2 Matrix32::basis_xform(const Vector2& v) const {
+
+	return Vector2(
+		tdotx(v),
+		tdoty(v)
+	);
+}
+
+Vector2 Matrix32::basis_xform_inv(const Vector2& v) const{
+
+	return Vector2(
+		elements[0].dot(v),
+		elements[1].dot(v)
+	);
+}
+
+Vector2 Matrix32::xform(const Vector2& v) const {
+
+	return Vector2(
+		tdotx(v),
+		tdoty(v)
+	) + elements[2];
+}
+Vector2 Matrix32::xform_inv(const Vector2& p_vec) const {
+
+	Vector2 v = p_vec - elements[2];
+
+	return Vector2(
+		elements[0].dot(v),
+		elements[1].dot(v)
+	);
+
+}
+Rect2 Matrix32::xform(const Rect2& p_rect) const {
+
+	Vector2 x=elements[0]*p_rect.size.x;
+	Vector2 y=elements[1]*p_rect.size.y;
+	Vector2 pos = xform( p_rect.pos );
+
+	Rect2 new_rect;
+	new_rect.pos=pos;
+	new_rect.expand_to( pos+x );
+	new_rect.expand_to( pos+y );
+	new_rect.expand_to( pos+x+y );
+	return new_rect;
+}
+
+void Matrix32::set_rotation_and_scale(real_t p_rot,const Size2& p_scale) {
+
+	elements[0][0]=Math::cos(p_rot)*p_scale.x;
+	elements[1][1]=Math::cos(p_rot)*p_scale.y;
+	elements[0][1]=-Math::sin(p_rot)*p_scale.x;
+	elements[1][0]=Math::sin(p_rot)*p_scale.y;
+
+}
+
+Rect2 Matrix32::xform_inv(const Rect2& p_rect) const {
+
+	Vector2 ends[4]={
+		xform_inv( p_rect.pos ),
+		xform_inv( Vector2(p_rect.pos.x,p_rect.pos.y+p_rect.size.y ) ),
+		xform_inv( Vector2(p_rect.pos.x+p_rect.size.x,p_rect.pos.y+p_rect.size.y ) ),
+		xform_inv( Vector2(p_rect.pos.x+p_rect.size.x,p_rect.pos.y ) )
+	};
+
+	Rect2 new_rect;
+	new_rect.pos=ends[0];
+	new_rect.expand_to(ends[1]);
+	new_rect.expand_to(ends[2]);
+	new_rect.expand_to(ends[3]);
+
+	return new_rect;
+}
+
+
+#endif
diff --git a/core/math/math_defs.cpp b/core/math/math_defs.cpp
new file mode 100644
index 0000000000..ca43bc7ae3
--- /dev/null
+++ b/core/math/math_defs.cpp
@@ -0,0 +1,30 @@
+/*************************************************************************/
+/*  math_defs.cpp                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "math_defs.h"

+

diff --git a/core/math/math_defs.h b/core/math/math_defs.h
new file mode 100644
index 0000000000..dd0390240a
--- /dev/null
+++ b/core/math/math_defs.h
@@ -0,0 +1,60 @@
+/*************************************************************************/
+/*  math_defs.h                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef MATH_DEFS_H

+#define MATH_DEFS_H

+

+#define CMP_EPSILON 0.00001

+#define CMP_EPSILON2 (CMP_EPSILON*CMP_EPSILON)

+#define CMP_NORMALIZE_TOLERANCE 0.000001

+#define CMP_POINT_IN_PLANE_EPSILON 0.00001

+

+/**

+  * "Real" is a type that will be translated to either floats or fixed depending

+  * on the compilation setting

+  */

+

+enum ClockDirection {

+

+        CLOCKWISE,

+	COUNTERCLOCKWISE

+};

+

+

+#ifdef REAL_T_IS_DOUBLE

+

+typedef double real_t;

+

+#else

+

+typedef float real_t;

+

+#endif

+

+

+#endif // MATH_DEFS_H

diff --git a/core/math/math_funcs.cpp b/core/math/math_funcs.cpp
new file mode 100644
index 0000000000..2ba36e5a49
--- /dev/null
+++ b/core/math/math_funcs.cpp
@@ -0,0 +1,328 @@
+/*************************************************************************/
+/*  math_funcs.cpp                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "math_funcs.h"
+
+#include "core/os/os.h"
+#include <math.h>
+#include "float.h"
+uint32_t Math::default_seed=1;
+
+
+#define PHI 0x9e3779b9
+
+static uint32_t Q[4096], c = 362436;
+
+
+uint32_t Math::rand_from_seed(uint32_t *seed) {
+
+#if 1
+	uint32_t k;
+	uint32_t s = (*seed);
+	if (s == 0)
+		s = 0x12345987;
+	k = s / 127773;
+	s = 16807 * (s - k * 127773) - 2836 * k;
+	if (s < 0)
+		s += 2147483647;
+	(*seed) = s;
+	return (s & Math::RANDOM_MAX);
+#else
+	*seed = *seed * 1103515245 + 12345;
+	return (*seed % ((unsigned int)RANDOM_MAX + 1));
+#endif
+}
+
+void Math::seed(uint32_t x) {
+#if 0
+	int i;
+
+	Q[0] = x;
+	Q[1] = x + PHI;
+	Q[2] = x + PHI + PHI;
+
+	for (i = 3; i < 4096; i++)
+		Q[i] = Q[i - 3] ^ Q[i - 2] ^ PHI ^ i;
+#else
+	default_seed=x;
+#endif
+}
+
+void Math::randomize() {
+
+	seed(OS::get_singleton()->get_ticks_usec()); /* *OS::get_singleton()->get_time().sec); // windows doesn't have get_time(), returns always 0 */
+}
+
+uint32_t Math::rand() {
+
+	return rand_from_seed(&default_seed)&0x7FFFFFFF;
+}
+
+double Math::randf() {
+
+	return (double)rand() / (double)RANDOM_MAX;
+}
+
+double Math::sin(double p_x) {
+
+	return ::sin(p_x);
+
+}
+
+double Math::cos(double p_x) {
+
+	return ::cos(p_x);
+
+}
+
+double Math::tan(double p_x) {
+
+	return ::tan(p_x);
+
+}
+double Math::sinh(double p_x) {
+
+	return ::sinh(p_x);
+}
+
+double Math::cosh(double p_x) {
+
+	return ::cosh(p_x);
+}
+
+double Math::tanh(double p_x) {
+
+	return ::tanh(p_x);
+}
+
+
+double Math::deg2rad(double p_y) {
+
+	return p_y*Math_PI/180.0;
+}
+
+double Math::rad2deg(double p_y) {
+
+	return p_y*180.0/Math_PI;
+}
+
+double Math::round(double p_val) {
+
+	if (p_val>0) {
+		return ::floor(p_val+0.5);
+	} else {
+		p_val=-p_val;
+		return -::floor(p_val+0.5);
+	}
+}
+double Math::asin(double p_x) {
+
+	return ::asin(p_x);
+
+}
+double Math::acos(double p_x) {
+
+	return ::acos(p_x);
+}
+
+double Math::atan(double p_x) {
+
+	return ::atan(p_x);
+}
+
+double Math::dectime(double p_value,double p_amount, double p_step) {
+
+	float sgn = p_value < 0 ? -1.0 : 1.0;
+	float val = absf(p_value);
+	val-=p_amount*p_step;
+	if (val<0.0)
+		val=0.0;
+	return val*sgn;
+}
+
+double Math::atan2(double p_y, double p_x) {
+
+	return ::atan2(p_y,p_x);
+
+}
+double Math::sqrt(double p_x) {
+
+	return ::sqrt(p_x);
+}
+
+double Math::fmod(double p_x,double p_y) {
+
+	return ::fmod(p_x,p_y);
+}
+
+double Math::fposmod(double p_x,double p_y) {
+
+	if (p_x>=0) {
+	
+		return Math::fmod(p_x,p_y);
+		
+	} else {
+	
+		return p_y-Math::fmod(-p_x,p_y); 
+	}
+
+}
+double Math::floor(double p_x) {
+
+	return ::floor(p_x);
+}
+
+double Math::ceil(double p_x) {
+
+	return ::ceil(p_x);
+}
+
+int Math::decimals(double p_step) {
+	
+	int max=4;
+	int i=0;
+	while( (p_step - Math::floor(p_step)) != 0.0 && max) {
+		
+		p_step*=10.0;
+		max--;
+		i++;
+	}
+	
+	return i;
+
+}
+
+double Math::ease(double p_x, double p_c) {
+
+	if (p_c>0) {
+
+		return Math::pow(p_x,p_c);
+	} else  if (p_c<0) {
+		//inout ease
+
+		if (p_x<0.5) {
+			return Math::pow(p_x*2.0,-p_c)*0.5;
+		} else {
+			return (1.0-Math::pow(1.0-(p_x-0.5)*2.0,-p_c))*0.5+0.5;
+		}
+	} else
+		return 0; // no ease (raw)
+
+}
+
+double Math::stepify(double p_value,double p_step) {
+	
+	if (p_step!=0) {
+		
+		p_value=floor( p_value / p_step + 0.5 ) * p_step;		
+	}	
+	return p_value;
+}
+
+bool Math::is_nan(double p_val) {
+
+	return (p_val!=p_val);
+}
+
+bool Math::is_inf(double p_val) {
+
+#ifdef _MSC_VER
+	return !_finite(p_val);
+#else
+	return isinf(p_val);
+#endif
+
+}
+
+uint32_t Math::larger_prime(uint32_t p_val) {
+
+	static const int primes[] = {
+		5,
+		13,
+		23,
+		47,
+		97,
+		193,
+		389,
+		769,
+		1543,
+		3079,
+		6151,
+		12289,
+		24593,
+		49157,
+		98317,
+		196613,
+		393241,
+		786433,
+		1572869,
+		3145739,
+		6291469,
+		12582917,
+		25165843,
+		50331653,
+		100663319,
+		201326611,
+		402653189,
+		805306457,
+		1610612741,
+		0,
+	};
+
+	int idx=0;
+	while (true) {
+
+		ERR_FAIL_COND_V(primes[idx]==0,0);
+		if (primes[idx]>p_val)
+			return primes[idx];
+		idx++;
+	}
+
+	return 0;
+}
+
+double Math::random(double from, double to) {
+
+	unsigned int r = Math::rand();
+	double ret = (double)r/(double)RANDOM_MAX;
+	return (ret)*(to-from) + from;
+}
+
+double Math::pow(double x, double y) {
+
+	return ::pow(x,y);
+}
+
+double Math::log(double x) {
+
+	return ::log(x);
+}
+double Math::exp(double x) {
+
+	return ::exp(x);
+}
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
new file mode 100644
index 0000000000..6a60a7f790
--- /dev/null
+++ b/core/math/math_funcs.h
@@ -0,0 +1,180 @@
+/*************************************************************************/
+/*  math_funcs.h                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef MATH_FUNCS_H
+#define MATH_FUNCS_H
+
+#include "typedefs.h"
+#include "math_defs.h"
+
+#ifndef NO_MATH_H
+#include "math.h"
+#endif
+
+class Math {
+
+
+	static uint32_t default_seed;
+public:
+	Math() {}; // useless to instance
+
+	enum {
+		RANDOM_MAX=2147483647L
+	};
+
+	static double sin(double p_x);
+	static double cos(double p_x);
+	static double tan(double p_x);
+	static double sinh(double p_x);
+	static double cosh(double p_x);
+	static double tanh(double p_x);
+	static double asin(double p_x);
+	static double acos(double p_x);
+	static double atan(double p_x);
+	static double atan2(double p_y, double p_x);
+	static double deg2rad(double p_y);
+	static double rad2deg(double p_y);
+	static double sqrt(double p_x);
+	static double fmod(double p_x,double p_y);
+	static double fposmod(double p_x,double p_y);
+	static uint32_t rand_from_seed(uint32_t *seed);
+	static double floor(double p_x);
+	static double ceil(double p_x);
+	static double ease(double p_x, double p_c);
+	static int decimals(double p_step);
+	static double stepify(double p_value,double p_step);
+	static void seed(uint32_t x=0);
+	static void randomize();
+	static uint32_t larger_prime(uint32_t p_val);
+	static double dectime(double p_value,double p_amount, double p_step);
+
+
+	static inline double linear2db(double p_linear) {
+
+		return Math::log( p_linear ) * 8.6858896380650365530225783783321;
+	}
+
+	static inline double db2linear(double p_linear) {
+
+		return Math::exp( p_linear * 0.11512925464970228420089957273422 );
+	}
+
+	static bool is_nan(double p_val);
+	static bool is_inf(double p_val);
+
+
+
+	static uint32_t rand();
+	static double randf();
+	
+	static double round(double p_val);
+
+	static double random(double from, double to);
+	
+
+	static _FORCE_INLINE_ real_t abs(real_t g) {
+
+#ifdef REAL_T_IS_DOUBLE	
+	
+		return absd(g);		
+#else
+
+		return absf(g);		
+#endif
+	}
+
+	static _FORCE_INLINE_ float absf(float g) {
+		
+		union {
+			float f;
+			uint32_t i;
+		} u;
+
+		u.f=g;
+		u.i&=2147483647u;
+		return u.f;
+	}
+
+	static _FORCE_INLINE_ double absd(double g) {
+
+		union {
+			double d;
+			uint64_t i;
+		} u;
+		u.d=g;
+		u.i&=(uint64_t)9223372036854775807ll;
+		return u.d;
+	}
+
+	//this function should be as fast as possible and rounding mode should not matter
+	static _FORCE_INLINE_ int fast_ftoi(float a) {
+
+		static int b;
+
+#if defined(_MSC_VER)
+		__asm fld a
+		__asm fistp b
+/*#elif defined( __GNUC__ ) && ( defined( __i386__ ) || defined( __x86_64__ ) )
+		// use AT&T inline assembly style, document that
+		// we use memory as output (=m) and input (m)
+		__asm__ __volatile__ (
+		"flds %1        \n\t"
+		"fistpl %0      \n\t"
+		: "=m" (b)
+		: "m" (a));*/
+#else
+		b=lrintf(a); //assuming everything but msvc has lrint
+#endif
+		return	b;
+	}
+
+
+#if defined(__GNUC__)
+
+	static _FORCE_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE
+#else
+
+	static _FORCE_INLINE_ int64_t dtoll(double p_double) { return (int64_t)p_double; } ///@TODO OPTIMIZE
+#endif
+
+	static _FORCE_INLINE_ float lerp(float a, float b, float c) {
+
+		return a+(b-a)*c;
+	}
+
+	static double pow(double x, double y);
+	static double log(double x);
+	static double exp(double x);
+	
+};
+
+
+#define Math_PI 3.14159265358979323846
+#define Math_SQRT12 0.7071067811865475244008443621048490
+
+#endif // MATH_FUNCS_H
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
new file mode 100644
index 0000000000..ff62e7786b
--- /dev/null
+++ b/core/math/matrix3.cpp
@@ -0,0 +1,479 @@
+/*************************************************************************/
+/*  matrix3.cpp                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "matrix3.h"
+#include "math_funcs.h"
+#include "os/copymem.h"
+
+#define cofac(row1,col1, row2, col2)\
+	(elements[row1][col1] * elements[row2][col2] - elements[row1][col2] * elements[row2][col1])
+
+void Matrix3::from_z(const Vector3& p_z) {
+
+	if (Math::abs(p_z.z) > Math_SQRT12 ) {
+
+		// choose p in y-z plane
+		real_t a = p_z[1]*p_z[1] + p_z[2]*p_z[2];
+		real_t k = 1.0/Math::sqrt(a);
+		elements[0]=Vector3(0,-p_z[2]*k,p_z[1]*k);
+		elements[1]=Vector3(a*k,-p_z[0]*elements[0][2],p_z[0]*elements[0][1]);
+	} else {
+
+		// choose p in x-y plane
+		real_t a = p_z.x*p_z.x + p_z.y*p_z.y;
+		real_t k = 1.0/Math::sqrt(a);
+		elements[0]=Vector3(-p_z.y*k,p_z.x*k,0);
+		elements[1]=Vector3(-p_z.z*elements[0].y,p_z.z*elements[0].x,a*k);
+	}
+	elements[2]=p_z;
+}
+
+void Matrix3::invert() {
+
+
+	real_t co[3]={
+		cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)
+	};
+	real_t det =	elements[0][0] * co[0]+
+			elements[0][1] * co[1]+
+			elements[0][2] * co[2];
+	
+	ERR_FAIL_COND( det == 0 );
+	real_t s = 1.0/det;
+	
+	set(  co[0]*s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, 
+	      co[1]*s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+	      co[2]*s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s );
+
+}
+
+void Matrix3::orthonormalize() {
+
+	// Gram-Schmidt Process
+
+	Vector3 x=get_axis(0);
+	Vector3 y=get_axis(1);
+	Vector3 z=get_axis(2);
+
+	x.normalize();
+	y = (y-x*(x.dot(y)));
+	y.normalize();
+	z = (z-x*(x.dot(z))-y*(y.dot(z)));
+	z.normalize();
+
+	set_axis(0,x);
+	set_axis(1,y);
+	set_axis(2,z);
+
+}
+
+Matrix3 Matrix3::orthonormalized() const {
+
+	Matrix3 c = *this;
+	c.orthonormalize();
+	return c;
+}
+
+
+Matrix3 Matrix3::inverse() const {
+
+	Matrix3 inv=*this;
+	inv.invert();
+	return inv;
+}
+
+void Matrix3::transpose() {
+
+	SWAP(elements[0][1],elements[1][0]);
+	SWAP(elements[0][2],elements[2][0]);
+	SWAP(elements[1][2],elements[2][1]);
+}
+
+Matrix3 Matrix3::transposed() const {
+
+	Matrix3 tr=*this;
+	tr.transpose();
+	return tr;
+}
+
+void Matrix3::scale(const Vector3& p_scale) {
+
+	elements[0][0]*=p_scale.x;
+	elements[1][0]*=p_scale.x;
+	elements[2][0]*=p_scale.x;
+	elements[0][1]*=p_scale.y;
+	elements[1][1]*=p_scale.y;
+	elements[2][1]*=p_scale.y;
+	elements[0][2]*=p_scale.z;
+	elements[1][2]*=p_scale.z;
+	elements[2][2]*=p_scale.z;
+}
+
+Matrix3 Matrix3::scaled( const Vector3& p_scale ) const {
+
+	Matrix3 m = *this;
+	m.scale(p_scale);
+	return m;
+}
+
+Vector3 Matrix3::get_scale() const {
+
+	return Vector3(
+		Vector3(elements[0][0],elements[1][0],elements[2][0]).length(),
+		Vector3(elements[0][1],elements[1][1],elements[2][1]).length(),
+		Vector3(elements[0][2],elements[1][2],elements[2][2]).length()
+	);
+		
+}
+void Matrix3::rotate(const Vector3& p_axis, real_t p_phi) {
+
+	*this = *this * Matrix3(p_axis, p_phi);
+}
+
+Matrix3 Matrix3::rotated(const Vector3& p_axis, real_t p_phi) const {
+
+	return *this * Matrix3(p_axis, p_phi);
+
+}
+
+Vector3 Matrix3::get_euler() const {
+
+	// rot =  cy*cz          -cy*sz           sy
+	    //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	    //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+
+	Matrix3 m = *this;
+	m.orthonormalize();
+
+	Vector3 euler;
+
+	euler.y = Math::asin(m[0][2]);
+	if ( euler.y < Math_PI*0.5) {
+		if ( euler.y > -Math_PI*0.5) {
+			euler.x = Math::atan2(-m[1][2],m[2][2]);
+			euler.z = Math::atan2(-m[0][1],m[0][0]);
+
+		} else {
+			real_t r = Math::atan2(m[1][0],m[1][1]);
+			euler.z = 0.0;
+			euler.x = euler.z - r;
+
+		}
+	} else {
+		real_t r = Math::atan2(m[0][1],m[1][1]);
+		euler.z = 0;
+		euler.x = r - euler.z;
+	}
+
+	return euler;
+
+
+}
+
+void Matrix3::set_euler(const Vector3& p_euler) {
+
+	real_t c, s;
+
+	c = Math::cos(p_euler.x);
+	s = Math::sin(p_euler.x);
+	Matrix3 xmat(1.0,0.0,0.0,0.0,c,-s,0.0,s,c);
+
+	c = Math::cos(p_euler.y);
+	s = Math::sin(p_euler.y);
+	Matrix3 ymat(c,0.0,s,0.0,1.0,0.0,-s,0.0,c);
+
+	c = Math::cos(p_euler.z);
+	s = Math::sin(p_euler.z);
+	Matrix3 zmat(c,-s,0.0,s,c,0.0,0.0,0.0,1.0);
+
+	//optimizer will optimize away all this anyway
+	*this = xmat*(ymat*zmat);
+}
+
+bool Matrix3::operator==(const Matrix3& p_matrix) const {
+
+	for (int i=0;i<3;i++) {
+		for (int j=0;j<3;j++) {
+			if (elements[i][j]!=p_matrix.elements[i][j])
+				return false;
+		}
+	}
+	
+	return true;
+}
+bool Matrix3::operator!=(const Matrix3& p_matrix) const {
+
+	return (!(*this==p_matrix));
+}
+
+Matrix3::operator String() const {
+
+	String mtx;
+	for (int i=0;i<3;i++) {
+	
+		for (int j=0;j<3;j++) {
+	
+			if (i!=0 || j!=0)
+				mtx+=", ";
+				
+			mtx+=rtos( elements[i][j] );
+		}
+	}
+	
+	return mtx;
+}
+
+Matrix3::operator Quat() const {
+
+	Matrix3 m=*this;
+	m.orthonormalize();
+
+	real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2];
+	real_t temp[4];
+	
+	if (trace > 0.0) 
+	{
+		real_t s = Math::sqrt(trace + 1.0);
+		temp[3]=(s * 0.5);
+		s = 0.5 / s;
+		
+		temp[0]=((m.elements[2][1] - m.elements[1][2]) * s);
+		temp[1]=((m.elements[0][2] - m.elements[2][0]) * s);
+		temp[2]=((m.elements[1][0] - m.elements[0][1]) * s);
+	} 
+	else 
+	{
+		int i = m.elements[0][0] < m.elements[1][1] ?
+			(m.elements[1][1] < m.elements[2][2] ? 2 : 1) :
+			(m.elements[0][0] < m.elements[2][2] ? 2 : 0);
+		int j = (i + 1) % 3;  
+		int k = (i + 2) % 3;
+		
+		real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0);
+		temp[i] = s * 0.5;
+		s = 0.5 / s;
+		
+		temp[3] = (m.elements[k][j] - m.elements[j][k]) * s;
+		temp[j] = (m.elements[j][i] + m.elements[i][j]) * s;
+		temp[k] = (m.elements[k][i] + m.elements[i][k]) * s;
+	}
+	
+	return Quat(temp[0],temp[1],temp[2],temp[3]);
+
+}
+
+static const Matrix3 _ortho_bases[24]={
+	Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1),
+	Matrix3(0, -1, 0, 1, 0, 0, 0, 0, 1),
+	Matrix3(-1, 0, 0, 0, -1, 0, 0, 0, 1),
+	Matrix3(0, 1, 0, -1, 0, 0, 0, 0, 1),
+	Matrix3(1, 0, 0, 0, 0, -1, 0, 1, 0),
+	Matrix3(0, 0, 1, 1, 0, 0, 0, 1, 0),
+	Matrix3(-1, 0, 0, 0, 0, 1, 0, 1, 0),
+	Matrix3(0, 0, -1, -1, 0, 0, 0, 1, 0),
+	Matrix3(1, 0, 0, 0, -1, 0, 0, 0, -1),
+	Matrix3(0, 1, 0, 1, 0, 0, 0, 0, -1),
+	Matrix3(-1, 0, 0, 0, 1, 0, 0, 0, -1),
+	Matrix3(0, -1, 0, -1, 0, 0, 0, 0, -1),
+	Matrix3(1, 0, 0, 0, 0, 1, 0, -1, 0),
+	Matrix3(0, 0, -1, 1, 0, 0, 0, -1, 0),
+	Matrix3(-1, 0, 0, 0, 0, -1, 0, -1, 0),
+	Matrix3(0, 0, 1, -1, 0, 0, 0, -1, 0),
+	Matrix3(0, 0, 1, 0, 1, 0, -1, 0, 0),
+	Matrix3(0, -1, 0, 0, 0, 1, -1, 0, 0),
+	Matrix3(0, 0, -1, 0, -1, 0, -1, 0, 0),
+	Matrix3(0, 1, 0, 0, 0, -1, -1, 0, 0),
+	Matrix3(0, 0, 1, 0, -1, 0, 1, 0, 0),
+	Matrix3(0, 1, 0, 0, 0, 1, 1, 0, 0),
+	Matrix3(0, 0, -1, 0, 1, 0, 1, 0, 0),
+	Matrix3(0, -1, 0, 0, 0, -1, 1, 0, 0)
+};
+
+int Matrix3::get_orthogonal_index() const {
+
+	//could be sped up if i come up with a way
+	Matrix3 orth=*this;
+	for(int i=0;i<3;i++) {
+		for(int j=0;j<3;j++) {
+
+			float v = orth[i][j];
+			if (v>0.5)
+				v=1.0;
+			else if (v<-0.5)
+				v=-1.0;
+			else
+				v=0;
+
+			orth[i][j]=v;
+		}
+	}
+
+	for(int i=0;i<24;i++) {
+
+		if (_ortho_bases[i]==orth)
+			return i;
+
+
+	}
+
+	return 0;
+}
+
+void Matrix3::set_orthogonal_index(int p_index){
+
+	//there only exist 24 orthogonal bases in r3
+	ERR_FAIL_INDEX(p_index,24);
+
+
+	*this=_ortho_bases[p_index];
+
+}
+
+
+void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const {
+
+
+	double angle,x,y,z; // variables for result
+		double epsilon = 0.01; // margin to allow for rounding errors
+		double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
+
+	if (	(Math::abs(elements[1][0]-elements[0][1])< epsilon)
+		&& (Math::abs(elements[2][0]-elements[0][2])< epsilon)
+		&& (Math::abs(elements[2][1]-elements[1][2])< epsilon)) {
+			// singularity found
+			// first check for identity matrix which must have +1 for all terms
+			//  in leading diagonaland zero in other terms
+		if ((Math::abs(elements[1][0]+elements[0][1]) < epsilon2)
+		  && (Math::abs(elements[2][0]+elements[0][2]) < epsilon2)
+		  && (Math::abs(elements[2][1]+elements[1][2]) < epsilon2)
+		  && (Math::abs(elements[0][0]+elements[1][1]+elements[2][2]-3) < epsilon2)) {
+			// this singularity is identity matrix so angle = 0
+			r_axis=Vector3(0,1,0);
+			r_angle=0;
+			return;
+		}
+		// otherwise this singularity is angle = 180
+		angle = Math_PI;
+		double xx = (elements[0][0]+1)/2;
+		double yy = (elements[1][1]+1)/2;
+		double zz = (elements[2][2]+1)/2;
+		double xy = (elements[1][0]+elements[0][1])/4;
+		double xz = (elements[2][0]+elements[0][2])/4;
+		double yz = (elements[2][1]+elements[1][2])/4;
+		if ((xx > yy) && (xx > zz)) { // elements[0][0] is the largest diagonal term
+			if (xx< epsilon) {
+				x = 0;
+				y = 0.7071;
+				z = 0.7071;
+			} else {
+				x = Math::sqrt(xx);
+				y = xy/x;
+				z = xz/x;
+			}
+		} else if (yy > zz) { // elements[1][1] is the largest diagonal term
+			if (yy< epsilon) {
+				x = 0.7071;
+				y = 0;
+				z = 0.7071;
+			} else {
+				y = Math::sqrt(yy);
+				x = xy/y;
+				z = yz/y;
+			}
+		} else { // elements[2][2] is the largest diagonal term so base result on this
+			if (zz< epsilon) {
+				x = 0.7071;
+				y = 0.7071;
+				z = 0;
+			} else {
+				z = Math::sqrt(zz);
+				x = xz/z;
+				y = yz/z;
+			}
+		}
+		r_axis=Vector3(x,y,z);
+		r_angle=angle;
+		return;
+	}
+	// as we have reached here there are no singularities so we can handle normally
+	double s = Math::sqrt((elements[1][2] - elements[2][1])*(elements[1][2] - elements[2][1])
+		+(elements[2][0] - elements[0][2])*(elements[2][0] - elements[0][2])
+		+(elements[0][1] - elements[1][0])*(elements[0][1] - elements[1][0])); // used to normalise
+	if (Math::abs(s) < 0.001) s=1;
+		// prevent divide by zero, should not happen if matrix is orthogonal and should be
+		// caught by singularity test above, but I've left it in just in case
+	angle = Math::acos(( elements[0][0] + elements[1][1] + elements[2][2] - 1)/2);
+	x = (elements[1][2] - elements[2][1])/s;
+	y = (elements[2][0] - elements[0][2])/s;
+	z = (elements[0][1] - elements[1][0])/s;
+
+	r_axis=Vector3(x,y,z);
+	r_angle=angle;
+}
+
+Matrix3::Matrix3(const Vector3& p_euler) {
+
+	set_euler( p_euler );
+		
+}
+
+Matrix3::Matrix3(const Quat& p_quat) {
+
+	real_t d = p_quat.length_squared();
+	real_t s = 2.0 / d;
+	real_t xs = p_quat.x * s,   ys = p_quat.y * s,   zs = p_quat.z * s;
+	real_t wx = p_quat.w * xs,  wy = p_quat.w * ys,  wz = p_quat.w * zs;
+	real_t xx = p_quat.x * xs,  xy = p_quat.x * ys,  xz = p_quat.x * zs;
+	real_t yy = p_quat.y * ys,  yz = p_quat.y * zs,  zz = p_quat.z * zs;
+	set(	1.0 - (yy + zz), xy - wz, xz + wy, 	
+		xy + wz, 1.0 - (xx + zz), yz - wx, 
+		xz - wy, yz + wx, 1.0 - (xx + yy))	;
+
+}
+
+Matrix3::Matrix3(const Vector3& p_axis, real_t p_phi) {
+
+	Vector3 axis_sq(p_axis.x*p_axis.x,p_axis.y*p_axis.y,p_axis.z*p_axis.z);
+
+	real_t cosine= Math::cos(p_phi);
+	real_t sine= Math::sin(p_phi);
+
+	elements[0][0] = axis_sq.x + cosine * ( 1.0 - axis_sq.x );
+	elements[0][1] = p_axis.x * p_axis.y *  ( 1.0 - cosine ) + p_axis.z * sine;
+	elements[0][2] = p_axis.z * p_axis.x * ( 1.0 - cosine ) - p_axis.y * sine;
+
+	elements[1][0] = p_axis.x * p_axis.y * ( 1.0 - cosine ) - p_axis.z * sine;
+	elements[1][1] = axis_sq.y + cosine  * ( 1.0 - axis_sq.y );
+	elements[1][2] = p_axis.y * p_axis.z * ( 1.0 - cosine ) + p_axis.x * sine;
+
+	elements[2][0] = p_axis.z * p_axis.x * ( 1.0 - cosine ) + p_axis.y * sine;
+	elements[2][1] = p_axis.y * p_axis.z * ( 1.0 - cosine ) - p_axis.x * sine;
+	elements[2][2] = axis_sq.z + cosine  * ( 1.0 - axis_sq.z );
+
+}
+
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
new file mode 100644
index 0000000000..f68703ca23
--- /dev/null
+++ b/core/math/matrix3.h
@@ -0,0 +1,230 @@
+/*************************************************************************/
+/*  matrix3.h                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef MATRIX3_H
+#define MATRIX3_H
+
+#include "vector3.h"
+#include "quat.h"
+
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+class Matrix3 {
+public:
+
+	Vector3 elements[3];
+			
+	_FORCE_INLINE_ const Vector3& operator[](int axis) const {
+	
+		return elements[axis];
+	}
+	_FORCE_INLINE_ Vector3& operator[](int axis) {
+	
+		return elements[axis];
+	}
+
+	void invert(); 
+	void transpose();
+	
+	Matrix3 inverse() const; 
+	Matrix3 transposed() const;
+
+	_FORCE_INLINE_ float determinant() const;
+
+	void from_z(const Vector3& p_z);
+
+	_FORCE_INLINE_ Vector3 get_axis(int p_axis) const {
+		// get actual basis axis (elements is transposed for performance)
+		return Vector3( elements[0][p_axis], elements[1][p_axis], elements[2][p_axis] );
+	}
+	_FORCE_INLINE_ void set_axis(int p_axis, const Vector3& p_value) {
+		// get actual basis axis (elements is transposed for performance)
+		elements[0][p_axis]=p_value.x;
+		elements[1][p_axis]=p_value.y;
+		elements[2][p_axis]=p_value.z;
+	}
+
+	void rotate(const Vector3& p_axis, real_t p_phi);
+	Matrix3 rotated(const Vector3& p_axis, real_t p_phi) const;
+
+	void scale( const Vector3& p_scale );
+	Matrix3 scaled( const Vector3& p_scale ) const;
+	Vector3 get_scale() const;
+
+	Vector3 get_euler() const;
+	void set_euler(const Vector3& p_euler);
+
+	// transposed dot products
+	_FORCE_INLINE_ real_t tdotx(const Vector3& v) const  {
+		return elements[0][0] * v[0] + elements[1][0] * v[1] + elements[2][0] * v[2];
+	}
+	_FORCE_INLINE_ real_t tdoty(const Vector3& v) const {
+		return elements[0][1] * v[0] + elements[1][1] * v[1] + elements[2][1] * v[2];
+	}
+	_FORCE_INLINE_ real_t tdotz(const Vector3& v) const {
+		return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2];
+	}
+	
+	bool operator==(const Matrix3& p_matrix) const;
+	bool operator!=(const Matrix3& p_matrix) const;
+
+	_FORCE_INLINE_ Vector3 xform(const Vector3& p_vector) const;
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3& p_vector) const;
+	_FORCE_INLINE_ void operator*=(const Matrix3& p_matrix);
+	_FORCE_INLINE_ Matrix3 operator*(const Matrix3& p_matrix) const;
+
+	int get_orthogonal_index() const;
+	void set_orthogonal_index(int p_index);
+
+	operator String() const;
+
+	void get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const;
+
+	/* create / set */
+
+
+	_FORCE_INLINE_ void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) {
+	
+		elements[0][0]=xx;
+		elements[0][1]=xy;
+		elements[0][2]=xz;
+		elements[1][0]=yx;
+		elements[1][1]=yy;
+		elements[1][2]=yz;
+		elements[2][0]=zx;
+		elements[2][1]=zy;
+		elements[2][2]=zz;	
+	}
+	_FORCE_INLINE_ Vector3 get_column(int i) const {
+			
+		return Vector3(elements[0][i],elements[1][i],elements[2][i]);
+	}
+		
+	_FORCE_INLINE_ Vector3 get_row(int i) const {
+			
+		return Vector3(elements[i][0],elements[i][1],elements[i][2]);
+	}
+	_FORCE_INLINE_ void set_row(int i, const Vector3& p_row) {
+		elements[i][0]=p_row.x;
+		elements[i][1]=p_row.y;
+		elements[i][2]=p_row.z;
+	}
+
+	_FORCE_INLINE_ void set_zero() {
+		elements[0].zero();
+		elements[1].zero();
+		elements[2].zero();
+	}
+
+	_FORCE_INLINE_ Matrix3 transpose_xform(const Matrix3& m) const
+	{
+		return Matrix3(
+			elements[0].x * m[0].x + elements[1].x * m[1].x + elements[2].x * m[2].x,
+			elements[0].x * m[0].y + elements[1].x * m[1].y + elements[2].x * m[2].y,
+			elements[0].x * m[0].z + elements[1].x * m[1].z + elements[2].x * m[2].z,
+			elements[0].y * m[0].x + elements[1].y * m[1].x + elements[2].y * m[2].x,
+			elements[0].y * m[0].y + elements[1].y * m[1].y + elements[2].y * m[2].y,
+			elements[0].y * m[0].z + elements[1].y * m[1].z + elements[2].y * m[2].z,
+			elements[0].z * m[0].x + elements[1].z * m[1].x + elements[2].z * m[2].x,
+			elements[0].z * m[0].y + elements[1].z * m[1].y + elements[2].z * m[2].y,
+			elements[0].z * m[0].z + elements[1].z * m[1].z + elements[2].z * m[2].z);
+	}
+	Matrix3(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { 
+			
+		set(xx, xy, xz, yx, yy, yz, zx, zy, zz);
+	}
+
+	void orthonormalize();
+	Matrix3 orthonormalized() const;
+
+	operator Quat() const;
+
+	Matrix3(const Quat& p_quat); // euler
+	Matrix3(const Vector3& p_euler); // euler
+	Matrix3(const Vector3& p_axis, real_t p_phi);
+
+	_FORCE_INLINE_ Matrix3() {
+	
+		elements[0][0]=1;
+		elements[0][1]=0;
+		elements[0][2]=0;
+		elements[1][0]=0;
+		elements[1][1]=1;
+		elements[1][2]=0;
+		elements[2][0]=0;
+		elements[2][1]=0;
+		elements[2][2]=1;
+	}
+
+
+};
+
+_FORCE_INLINE_ void Matrix3::operator*=(const Matrix3& p_matrix) {
+
+	set(
+		p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]),
+		p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]),
+		p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2]));
+	
+}
+
+_FORCE_INLINE_ Matrix3 Matrix3::operator*(const Matrix3& p_matrix) const {
+
+	return Matrix3(
+		p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]),
+		p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]),
+		p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2]) );
+
+}
+
+Vector3 Matrix3::xform(const Vector3& p_vector) const {
+
+	return Vector3(
+		elements[0].dot(p_vector),
+		elements[1].dot(p_vector),
+		elements[2].dot(p_vector)
+	);
+}
+
+Vector3 Matrix3::xform_inv(const Vector3& p_vector) const {
+
+	return Vector3(
+		(elements[0][0]*p_vector.x ) + ( elements[1][0]*p_vector.y ) + ( elements[2][0]*p_vector.z ),
+		(elements[0][1]*p_vector.x ) + ( elements[1][1]*p_vector.y ) + ( elements[2][1]*p_vector.z ),
+		(elements[0][2]*p_vector.x ) + ( elements[1][2]*p_vector.y ) + ( elements[2][2]*p_vector.z )
+	);
+}
+
+float Matrix3::determinant() const {
+
+	return elements[0][0]*(elements[1][1]*elements[2][2] - elements[2][1]*elements[1][2]) -
+	       elements[1][0]*(elements[0][1]*elements[2][2] - elements[2][1]*elements[0][2]) +
+	       elements[2][0]*(elements[0][1]*elements[1][2] - elements[1][1]*elements[0][2]);
+}
+#endif
diff --git a/core/math/octree.h b/core/math/octree.h
new file mode 100644
index 0000000000..b64ba381c0
--- /dev/null
+++ b/core/math/octree.h
@@ -0,0 +1,1461 @@
+/*************************************************************************/
+/*  octree.h                                                             */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef OCTREE_H
+#define OCTREE_H
+
+#include "vector3.h"
+#include "aabb.h"
+#include "list.h"
+#include "variant.h"
+#include "map.h"
+#include "print_string.h"
+
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+
+typedef uint32_t OctreeElementID;
+
+#define OCTREE_ELEMENT_INVALID_ID 0
+#define OCTREE_SIZE_LIMIT 1e15
+
+template<class T,bool use_pairs=false,class AL=DefaultAllocator>
+class Octree {
+public:
+
+	typedef void* (*PairCallback)(void*,OctreeElementID, T*,int,OctreeElementID, T*,int);
+	typedef void (*UnpairCallback)(void*,OctreeElementID, T*,int,OctreeElementID, T*,int,void*);
+
+private:	
+	enum {
+	
+		NEG=0,
+		POS=1,		
+	};
+
+	enum {
+		OCTANT_NX_NY_NZ,
+		OCTANT_PX_NY_NZ,
+		OCTANT_NX_PY_NZ,
+		OCTANT_PX_PY_NZ,
+		OCTANT_NX_NY_PZ,
+		OCTANT_PX_NY_PZ,
+		OCTANT_NX_PY_PZ,
+		OCTANT_PX_PY_PZ
+	};
+
+
+	struct PairKey {
+	
+		union {
+			struct {
+				OctreeElementID A;
+				OctreeElementID B;
+			};
+			uint64_t key;
+		};
+			
+		_FORCE_INLINE_ bool operator<(const PairKey& p_pair) const {
+		
+			return key<p_pair.key;
+		}
+		
+		_FORCE_INLINE_ PairKey( OctreeElementID p_A, OctreeElementID p_B) {
+		
+			if (p_A<p_B) {
+			
+				A=p_A;
+				B=p_B;	
+			} else {
+			
+				B=p_A;
+				A=p_B;
+			}
+		}
+		
+		_FORCE_INLINE_ PairKey() {}
+	};	
+		
+	struct Element;
+		
+	struct Octant {
+		
+		// cached for FAST plane check
+		AABB aabb;
+			
+		uint64_t last_pass;
+		Octant *parent;
+		Octant *children[8];
+		
+		int children_count; // cache for amount of childrens (fast check for removal)
+		int parent_index; // cache for parent index (fast check for removal)
+	
+		List<Element*,AL> pairable_elements;
+		List<Element*,AL> elements;
+			
+		Octant() {
+			children_count=0;
+			parent_index=-1;
+			last_pass=0;
+			parent=NULL;
+			for (int i=0;i<8;i++)
+				children[i]=NULL;			
+		}
+		
+		~Octant() {
+			
+			//for (int i=0;i<8;i++)
+			//	memdelete_notnull(children[i]);
+		}
+	};
+	
+
+	struct PairData;
+
+	struct Element {
+		
+		Octree *octree;
+
+		T *userdata;
+		int subindex;
+		bool pairable;
+		uint32_t pairable_mask;
+		uint32_t pairable_type;
+		
+		uint64_t last_pass;
+		OctreeElementID _id;
+		Octant *common_parent;
+		
+		AABB aabb;
+		AABB container_aabb;
+
+		List<PairData*,AL> pair_list;
+		
+		struct OctantOwner {
+		
+			Octant *octant;
+			typename List<Element*,AL>::Element *E;
+		}; // an element can be in max 8 octants 
+		
+		List<OctantOwner,AL> octant_owners;
+		
+		
+		Element() { last_pass=0; _id=0; pairable=false; subindex=0; userdata=0; octree=0; pairable_mask=0; pairable_type=0; common_parent=NULL; }
+	};
+	
+
+	struct PairData {
+
+		int refcount;
+		bool intersect;
+		Element *A,*B;
+		void *ud;
+		typename List<PairData*,AL>::Element *eA,*eB;
+	};
+
+	typedef Map<OctreeElementID, Element, Comparator<OctreeElementID>, AL> ElementMap;
+	typedef Map<PairKey, PairData, Comparator<PairKey>, AL> PairMap;
+	ElementMap element_map;	
+	PairMap pair_map;	
+
+	PairCallback pair_callback;
+	UnpairCallback unpair_callback;
+	void *pair_callback_userdata;
+	void *unpair_callback_userdata;
+	
+	OctreeElementID last_element_id;	
+	uint64_t pass;
+
+	real_t unit_size;
+	Octant *root;
+	int octant_count;
+	int pair_count;
+
+
+
+	_FORCE_INLINE_ void _pair_check(PairData *p_pair) {
+
+		bool intersect=p_pair->A->aabb.intersects( p_pair->B->aabb );
+
+		if (intersect!=p_pair->intersect) {
+
+			if (intersect) {
+
+				if (pair_callback) {
+					p_pair->ud=pair_callback(pair_callback_userdata,p_pair->A->_id, p_pair->A->userdata,p_pair->A->subindex,p_pair->B->_id, p_pair->B->userdata,p_pair->B->subindex);
+
+				}
+				pair_count++;
+			} else {
+
+
+				if (unpair_callback) {
+					unpair_callback(pair_callback_userdata,p_pair->A->_id, p_pair->A->userdata,p_pair->A->subindex,p_pair->B->_id, p_pair->B->userdata,p_pair->B->subindex,p_pair->ud);
+				}
+				pair_count--;
+
+			}
+
+			p_pair->intersect=intersect;
+
+		}
+	}
+
+	_FORCE_INLINE_ void _pair_reference(Element* p_A,Element* p_B) {
+
+		if (p_A==p_B || (p_A->userdata==p_B->userdata && p_A->userdata))
+			return;
+			
+		if ( 	!(p_A->pairable_type&p_B->pairable_mask) &&
+			!(p_B->pairable_type&p_A->pairable_mask) )
+			return; // none can pair with none
+
+		PairKey key(p_A->_id, p_B->_id);
+		typename PairMap::Element *E=pair_map.find(key);
+
+		if (!E) {
+
+			PairData pdata;
+			pdata.refcount=1;
+			pdata.A=p_A;
+			pdata.B=p_B;
+			pdata.intersect=false;
+			E=pair_map.insert(key,pdata);
+			E->get().eA=p_A->pair_list.push_back(&E->get());
+			E->get().eB=p_B->pair_list.push_back(&E->get());
+
+//			if (pair_callback)
+//				pair_callback(pair_callback_userdata,p_A->userdata,p_B->userdata);
+		} else {
+		
+			E->get().refcount++;
+		}
+		
+	}
+	
+	_FORCE_INLINE_ void _pair_unreference(Element* p_A,Element* p_B) {
+	
+		if (p_A==p_B)
+			return;
+	
+		PairKey key(p_A->_id, p_B->_id);
+		typename PairMap::Element *E=pair_map.find(key);
+		if (!E) {
+			return; // no pair
+		}
+
+		E->get().refcount--;
+
+
+		if (E->get().refcount==0) {
+			// bye pair
+
+			if (E->get().intersect) {
+				if (unpair_callback) {
+					unpair_callback(pair_callback_userdata,p_A->_id, p_A->userdata,p_A->subindex,p_B->_id, p_B->userdata,p_B->subindex,E->get().ud);
+				}
+
+				pair_count--;
+			}
+
+			if (p_A==E->get().B) {
+				//may be reaching inverted
+				SWAP(p_A,p_B);
+			}
+
+			p_A->pair_list.erase( E->get().eA );
+			p_B->pair_list.erase( E->get().eB );
+			pair_map.erase(E);
+		}
+	
+	}
+
+	_FORCE_INLINE_ void _element_check_pairs(Element *p_element) {
+
+		typename List<PairData*,AL>::Element *E=p_element->pair_list.front();
+		while(E) {
+
+			_pair_check( E->get() );
+			E=E->next();
+		}
+
+	}
+
+	_FORCE_INLINE_ void _optimize() {
+
+
+		while(root && root->children_count<2 && !root->elements.size() && !(use_pairs && root->pairable_elements.size())) {
+
+
+			Octant *new_root=NULL;
+			if (root->children_count==1) {
+
+				for(int i=0;i<8;i++) {
+
+					if (root->children[i]) {
+						new_root=root->children[i];
+						root->children[i]=NULL;
+						break;
+					}
+				}
+				ERR_FAIL_COND(!new_root);
+				new_root->parent=NULL;
+				new_root->parent_index=-1;
+			}
+
+			memdelete_allocator<Octant,AL>( root );
+			octant_count--;
+			root=new_root;
+
+		}
+	}
+
+
+	void _insert_element(Element *p_element,Octant *p_octant);
+	void _ensure_valid_root(const AABB& p_aabb);
+	bool _remove_element_from_octant(Element *p_element,Octant *p_octant,Octant *p_limit=NULL);
+	void _remove_element(Element *p_element);
+	void _pair_element(Element *p_element,Octant *p_octant);	
+	void _unpair_element(Element *p_element,Octant *p_octant);
+
+
+	struct _CullConvexData {
+
+		const Plane* planes;
+		int plane_count;
+		T** result_array;
+		int *result_idx;
+		int result_max;
+		uint32_t mask;
+	};
+
+	void _cull_convex(Octant *p_octant,_CullConvexData *p_cull);
+	void _cull_AABB(Octant *p_octant,const AABB& p_aabb, T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask);
+	void _cull_segment(Octant *p_octant,const Vector3& p_from, const Vector3& p_to,T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask);
+	void _cull_point(Octant *p_octant,const Vector3& p_point,T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask);
+
+	void _remove_tree(Octant *p_octant) {
+	
+		if (!p_octant)
+			return;
+
+		for(int i=0;i<8;i++) {
+		
+			if (p_octant->children[i])
+				_remove_tree(p_octant->children[i]);
+		}
+		
+		memdelete_allocator<Octant,AL>(p_octant);
+	}
+public:
+
+	OctreeElementID create(T* p_userdata, const AABB& p_aabb=AABB(), int p_subindex=0, bool p_pairable=false,uint32_t p_pairable_type=0,uint32_t pairable_mask=1);
+	void move(OctreeElementID p_id, const AABB& p_aabb);
+	void set_pairable(OctreeElementID p_id,bool p_pairable=false,uint32_t p_pairable_type=0,uint32_t pairable_mask=1);
+	void erase(OctreeElementID p_id);
+
+	bool is_pairable(OctreeElementID p_id) const;
+	T *get(OctreeElementID p_id) const;
+	int get_subindex(OctreeElementID p_id) const;
+
+	int cull_convex(const Vector<Plane>& p_convex,T** p_result_array,int p_result_max,uint32_t p_mask=0xFFFFFFFF);
+	int cull_AABB(const AABB& p_aabb,T** p_result_array,int p_result_max,int *p_subindex_array=NULL,uint32_t p_mask=0xFFFFFFFF);
+	int cull_segment(const Vector3& p_from, const Vector3& p_to,T** p_result_array,int p_result_max,int *p_subindex_array=NULL,uint32_t p_mask=0xFFFFFFFF);
+
+	int cull_point(const Vector3& p_point,T** p_result_array,int p_result_max,int *p_subindex_array=NULL,uint32_t p_mask=0xFFFFFFFF);
+
+	void set_pair_callback( PairCallback p_callback, void *p_userdata );
+	void set_unpair_callback( UnpairCallback p_callback, void *p_userdata );
+
+	int get_octant_count() const { return octant_count; }
+	int get_pair_count() const { return pair_count; }
+	Octree(real_t p_unit_size=1.0);
+	~Octree() { _remove_tree(root); }
+};
+
+
+/* PRIVATE FUNCTIONS */
+
+template<class T,bool use_pairs,class AL>
+T *Octree<T,use_pairs,AL>::get(OctreeElementID p_id) const {
+	const typename ElementMap::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,NULL);
+	return E->get().userdata;
+}
+
+
+template<class T,bool use_pairs,class AL>
+bool Octree<T,use_pairs,AL>::is_pairable(OctreeElementID p_id) const {
+
+	const typename ElementMap::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,false);
+	return E->get().pairable;
+}
+
+template<class T,bool use_pairs,class AL>
+int Octree<T,use_pairs,AL>::get_subindex(OctreeElementID p_id) const {
+
+	const typename ElementMap::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,-1);
+	return E->get().subindex;
+}
+
+#define OCTREE_DIVISOR 4
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_insert_element(Element *p_element,Octant *p_octant) {
+
+	float element_size = p_element->aabb.get_longest_axis_size() * 1.01; // avoid precision issues
+	
+	if (p_octant->aabb.size.x/OCTREE_DIVISOR < element_size) {
+	//if (p_octant->aabb.size.x*0.5 < element_size) {		
+		
+		/* at smallest possible size for the element  */	
+		typename Element::OctantOwner owner;
+		owner.octant=p_octant;
+		
+		if (use_pairs && p_element->pairable) {
+				
+			p_octant->pairable_elements.push_back(p_element);
+			owner.E = p_octant->pairable_elements.back();
+		} else {
+		
+			p_octant->elements.push_back(p_element);
+			owner.E = p_octant->elements.back();
+		}
+		
+		p_element->octant_owners.push_back( owner );
+
+		if (p_element->common_parent==NULL) {
+			p_element->common_parent=p_octant;
+			p_element->container_aabb=p_octant->aabb;
+		} else {
+			p_element->container_aabb.merge_with(p_octant->aabb);
+		}
+
+
+		if (use_pairs && p_octant->children_count>0) {
+		
+			pass++; //elements below this only get ONE reference added
+
+			for (int i=0;i<8;i++) {
+						
+				if (p_octant->children[i]) {
+					_pair_element(p_element,p_octant->children[i]);	
+				}
+			}
+		}
+	} else {
+		/* not big enough, send it to subitems */
+		int splits=0;
+		bool candidate=p_element->common_parent==NULL;
+
+		for (int i=0;i<8;i++) {
+					
+			if (p_octant->children[i]) {
+				/* element exists, go straight to it */
+				if (p_octant->children[i]->aabb.intersects( p_element->aabb ) ) {				
+					_insert_element( p_element, p_octant->children[i] );
+					splits++;
+				}
+			} else {
+				/* check againt AABB where child should be */
+				
+				AABB aabb=p_octant->aabb;
+				aabb.size*=0.5;
+				
+				if (i&1)
+					aabb.pos.x+=aabb.size.x;
+				if (i&2)
+					aabb.pos.y+=aabb.size.y;
+				if (i&4)
+					aabb.pos.z+=aabb.size.z;
+				
+				if (aabb.intersects( p_element->aabb) ) {
+					/* if actually intersects, create the child */
+					
+					Octant *child = memnew_allocator( Octant, AL );
+					p_octant->children[i]=child;
+					child->parent=p_octant;
+					child->parent_index=i;
+
+					child->aabb=aabb;
+
+					p_octant->children_count++;
+
+					_insert_element( p_element, child );
+					octant_count++;
+					splits++;
+
+				}
+			}
+
+		}
+		
+		if (candidate && splits>1) {
+
+			p_element->common_parent=p_octant;
+		}
+		
+	}
+	
+	if (use_pairs) {
+
+		typename List<Element*,AL>::Element *E=p_octant->pairable_elements.front();
+
+		while(E) {
+			_pair_reference( p_element,E->get() );
+			E=E->next();
+		}		
+		
+		if (p_element->pairable) {
+			// and always test non-pairable if element is pairable
+			E=p_octant->elements.front();
+			while(E) {
+				_pair_reference( p_element,E->get() );
+				E=E->next();
+			}
+		}		
+	}
+	
+
+}
+
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_ensure_valid_root(const AABB& p_aabb) {
+
+	if (!root) {
+		// octre is empty
+		
+		AABB base( Vector3(), Vector3(1.0,1.0,1.0) * unit_size);
+		
+		while ( !base.encloses(p_aabb) ) {
+			
+			if ( ABS(base.pos.x+base.size.x) <= ABS(base.pos.x) ) {
+				/* grow towards positive */
+				base.size*=2.0;
+			} else {
+				base.pos-=base.size;				
+				base.size*=2.0;
+			}
+		}
+		
+		root = memnew_allocator( Octant, AL );
+
+		root->parent=NULL;	
+		root->parent_index=-1;	
+		root->aabb=base;
+		
+		octant_count++;
+		
+		
+	} else {		
+	
+		AABB base=root->aabb;
+		
+		while( !base.encloses( p_aabb ) ) {
+			
+			if (base.size.x > OCTREE_SIZE_LIMIT) {
+				ERR_EXPLAIN("Octree upper size limit reeached, does the AABB supplied contain NAN?");
+				ERR_FAIL();
+			}
+
+			Octant * gp = memnew_allocator( Octant, AL );
+			octant_count++;
+			root->parent=gp;
+			
+			if ( ABS(base.pos.x+base.size.x) <= ABS(base.pos.x) ) {
+				/* grow towards positive */
+				base.size*=2.0;
+				gp->aabb=base;
+				gp->children[0]=root;
+				root->parent_index=0;
+			} else {
+				base.pos-=base.size;				
+				base.size*=2.0;
+				gp->aabb=base;
+				gp->children[(1<<0)|(1<<1)|(1<<2)]=root; // add at all-positive
+				root->parent_index=(1<<0)|(1<<1)|(1<<2);
+			}
+						
+			gp->children_count=1;
+			root=gp;			
+		}		
+	}
+}
+
+template<class T,bool use_pairs,class AL>
+bool Octree<T,use_pairs,AL>::_remove_element_from_octant(Element *p_element,Octant *p_octant,Octant *p_limit) {
+
+	bool octant_removed=false;
+	
+	while(true) {
+	
+		// check all exit conditions
+		
+		if (p_octant==p_limit) // reached limit, nothing to erase, exit
+			return octant_removed;
+			
+		bool unpaired=false;
+			
+		if (use_pairs && p_octant->last_pass!=pass) {
+			// check wether we should unpair stuff
+			// always test pairable
+			typename List<Element*,AL>::Element *E=p_octant->pairable_elements.front();
+			while(E) {
+				_pair_unreference( p_element,E->get() );
+				E=E->next();
+			}
+			if (p_element->pairable) {
+				// and always test non-pairable if element is pairable
+				E=p_octant->elements.front();
+				while(E) {
+					_pair_unreference( p_element,E->get() );
+					E=E->next();
+				}
+			}
+			p_octant->last_pass=pass;
+			unpaired=true;
+		}
+	
+		bool removed=false;
+		
+		Octant *parent=p_octant->parent;
+		
+		if (p_octant->children_count==0 && p_octant->elements.empty() && p_octant->pairable_elements.empty()) {
+		
+			// erase octant
+			
+			if (p_octant==root) { // won't have a parent, just erase
+							
+				root=NULL;
+			} else {
+				ERR_FAIL_INDEX_V(p_octant->parent_index,8,octant_removed);
+				
+				parent->children[ p_octant->parent_index ]=NULL;
+				parent->children_count--;
+			}
+
+			memdelete_allocator<Octant,AL>(p_octant);
+			octant_count--;
+			removed=true;
+			octant_removed=true;
+		}
+		
+		if (!removed && !unpaired)
+			return octant_removed; // no reason to keep going up anymore! was already visited and was not removed
+			
+		p_octant=parent;
+		
+	}
+
+	return octant_removed;
+}
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_unpair_element(Element *p_element,Octant *p_octant) {
+
+	
+	// always test pairable				
+	typename List<Element*,AL>::Element *E=p_octant->pairable_elements.front();
+	while(E) {
+		if (E->get()->last_pass!=pass) { // only remove ONE reference
+			_pair_unreference( p_element,E->get() );
+			E->get()->last_pass=pass;
+		}
+		E=E->next();
+	}
+	
+	if (p_element->pairable) {
+		// and always test non-pairable if element is pairable
+		E=p_octant->elements.front();
+		while(E) {
+			if (E->get()->last_pass!=pass) { // only remove ONE reference
+				_pair_unreference( p_element,E->get() );
+				E->get()->last_pass=pass;
+			}
+			E=E->next();
+		}
+	}
+	
+	p_octant->last_pass=pass;
+	
+	if (p_octant->children_count==0)
+		return; // small optimization for leafs
+	
+	for (int i=0;i<8;i++) {
+	
+		if (p_octant->children[i])
+			_unpair_element(p_element,p_octant->children[i]);
+	}
+}
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_pair_element(Element *p_element,Octant *p_octant) {
+
+	// always test pairable
+
+	typename List<Element*,AL>::Element *E=p_octant->pairable_elements.front();
+
+	while(E) {
+
+		if (E->get()->last_pass!=pass) { // only get ONE reference
+			_pair_reference( p_element,E->get() );
+			E->get()->last_pass=pass;
+		}
+		E=E->next();
+	}
+	
+	if (p_element->pairable) {
+		// and always test non-pairable if element is pairable
+		E=p_octant->elements.front();
+		while(E) {
+			if (E->get()->last_pass!=pass) { // only get ONE reference
+				_pair_reference( p_element,E->get() );
+				E->get()->last_pass=pass;
+			}
+			E=E->next();
+		}
+	}
+	p_octant->last_pass=pass;
+	
+	if (p_octant->children_count==0)
+		return; // small optimization for leafs
+		
+	for (int i=0;i<8;i++) {
+	
+		if (p_octant->children[i])
+			_pair_element(p_element,p_octant->children[i]);
+	}
+}
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_remove_element(Element *p_element) {
+
+	pass++; // will do a new pass for this
+	
+	typename List< typename Element::OctantOwner,AL >::Element *I=p_element->octant_owners.front();
+			
+
+	/* FIRST remove going up normally */
+	for(;I;I=I->next()) {
+		
+		Octant *o=I->get().octant;
+
+		if (!use_pairs) // small speedup
+			o->elements.erase( I->get().E );
+
+		_remove_element_from_octant( p_element, o  );
+
+	}
+
+	/* THEN remove going down */
+
+	I=p_element->octant_owners.front();
+
+	if (use_pairs) {
+
+		for(;I;I=I->next()) {
+
+			Octant *o=I->get().octant;
+
+			// erase children pairs, they are erased ONCE even if repeated
+			pass++;
+			for (int i=0;i<8;i++) {
+
+				if (o->children[i])
+					_unpair_element(p_element,o->children[i]);
+			}
+
+			if (p_element->pairable)
+				o->pairable_elements.erase( I->get().E );
+			else
+				o->elements.erase( I->get().E );
+
+		}
+	}
+
+	p_element->octant_owners.clear();
+
+	if(use_pairs) {
+
+		int remaining=p_element->pair_list.size();
+		//p_element->pair_list.clear();
+		ERR_FAIL_COND( remaining );
+	}	
+
+}
+
+template<class T,bool use_pairs,class AL>
+OctreeElementID Octree<T,use_pairs,AL>::create(T* p_userdata, const AABB& p_aabb, int p_subindex,bool p_pairable,uint32_t p_pairable_type,uint32_t p_pairable_mask) {
+
+	// check for AABB validity 
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND_V( p_aabb.pos.x > 1e15 || p_aabb.pos.x < -1e15, 0 );
+	ERR_FAIL_COND_V( p_aabb.pos.y > 1e15 || p_aabb.pos.y < -1e15, 0 );
+	ERR_FAIL_COND_V( p_aabb.pos.z > 1e15 || p_aabb.pos.z < -1e15, 0 );
+	ERR_FAIL_COND_V( p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0, 0 );
+	ERR_FAIL_COND_V( p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0, 0 );
+	ERR_FAIL_COND_V( p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0, 0 );
+	ERR_FAIL_COND_V( Math::is_nan(p_aabb.size.x) , 0 );
+	ERR_FAIL_COND_V( Math::is_nan(p_aabb.size.y) , 0 );
+	ERR_FAIL_COND_V( Math::is_nan(p_aabb.size.z) , 0 );
+
+	
+#endif
+	typename ElementMap::Element *E = element_map.insert(last_element_id++,
+	Element());
+	Element &e = E->get();
+			
+	e.aabb=p_aabb;
+	e.userdata=p_userdata;
+	e.subindex=p_subindex;
+	e.last_pass=0;
+	e.octree=this;
+	e.pairable=p_pairable;
+	e.pairable_type=p_pairable_type;
+	e.pairable_mask=p_pairable_mask;
+	e._id=last_element_id-1;
+	
+	if (!e.aabb.has_no_surface()) {
+		_ensure_valid_root(p_aabb);
+		_insert_element(&e,root);
+		if (use_pairs)
+			_element_check_pairs(&e);
+	}
+
+	return last_element_id-1;
+}
+
+
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::move(OctreeElementID p_id, const AABB& p_aabb) {
+
+#ifdef DEBUG_ENABLED
+	// check for AABB validity
+	ERR_FAIL_COND( p_aabb.pos.x > 1e15 || p_aabb.pos.x < -1e15 );
+	ERR_FAIL_COND( p_aabb.pos.y > 1e15 || p_aabb.pos.y < -1e15 );
+	ERR_FAIL_COND( p_aabb.pos.z > 1e15 || p_aabb.pos.z < -1e15 );
+	ERR_FAIL_COND( p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0 );
+	ERR_FAIL_COND( p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0 );
+	ERR_FAIL_COND( p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0 );
+	ERR_FAIL_COND( Math::is_nan(p_aabb.size.x)  );
+	ERR_FAIL_COND( Math::is_nan(p_aabb.size.y)  );
+	ERR_FAIL_COND( Math::is_nan(p_aabb.size.z)  );
+#endif
+	typename ElementMap::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	Element &e = E->get();
+
+#if 0
+
+	pass++;
+	if (!e.aabb.has_no_surface()) {
+		_remove_element(&e);
+	}
+
+	e.aabb=p_aabb;
+
+	if (!e.aabb.has_no_surface()) {
+		_ensure_valid_root(p_aabb);
+
+		_insert_element(&e,root);
+		if (use_pairs)
+			_element_check_pairs(&e);
+
+	}
+
+	_optimize();
+
+#else
+
+	bool old_has_surf=!e.aabb.has_no_surface();
+	bool new_has_surf=!p_aabb.has_no_surface();
+
+	if (old_has_surf!=new_has_surf) {
+
+
+		if (old_has_surf) {
+			_remove_element(&e); // removing
+			e.common_parent=NULL;
+			e.aabb=AABB();
+			_optimize();
+		} else {
+			_ensure_valid_root(p_aabb); // inserting
+			e.common_parent=NULL;
+			e.aabb=p_aabb;
+			_insert_element(&e,root);
+			if (use_pairs)
+				_element_check_pairs(&e);
+
+		}
+
+		return;
+	}
+
+	if (!old_has_surf) // doing nothing
+		return;
+
+	// it still is enclosed in the same AABB it was assigned to
+	if (e.container_aabb.encloses(p_aabb)) {
+
+		e.aabb=p_aabb;
+		if (use_pairs)
+			_element_check_pairs(&e); // must check pairs anyway
+
+
+		return;
+	}
+
+	AABB combined=e.aabb;
+	combined.merge_with(p_aabb);
+	_ensure_valid_root(combined);
+
+	ERR_FAIL_COND( e.octant_owners.front()==NULL );
+
+	/* FIND COMMON PARENT */
+
+	List<typename Element::OctantOwner,AL> owners = e.octant_owners; // save the octant owners
+	Octant *common_parent=e.common_parent;
+	ERR_FAIL_COND(!common_parent);
+
+
+	//src is now the place towards where insertion is going to happen
+	pass++;
+
+	while(common_parent && !common_parent->aabb.encloses(p_aabb))
+		common_parent=common_parent->parent;
+
+	ERR_FAIL_COND(!common_parent);
+
+	//prepare for reinsert
+	e.octant_owners.clear();
+	e.common_parent=NULL;
+	e.aabb=p_aabb;
+
+	_insert_element(&e,common_parent); // reinsert from this point
+
+	pass++;
+	
+	for(typename List<typename Element::OctantOwner,AL>::Element *E=owners.front();E;) {
+
+		Octant *o=E->get().octant;
+		typename List<typename Element::OctantOwner,AL>::Element *N=E->next();
+
+//		if (!use_pairs)
+//			o->elements.erase( E->get().E );
+
+		if (use_pairs && e.pairable)
+			o->pairable_elements.erase( E->get().E );
+		else
+			o->elements.erase( E->get().E );
+
+		if (_remove_element_from_octant( &e, o, common_parent->parent  )) {
+
+			owners.erase(E);
+		}
+
+		E=N;
+	}
+
+
+	if (use_pairs) {
+		//unpair child elements in anything that survived
+		for(typename List<typename Element::OctantOwner,AL>::Element *E=owners.front();E;E=E->next()) {
+
+			Octant *o=E->get().octant;
+
+			// erase children pairs, unref ONCE
+			pass++;
+			for (int i=0;i<8;i++) {
+
+				if (o->children[i])
+					_unpair_element(&e,o->children[i]);
+			}
+
+		}
+
+		_element_check_pairs(&e);
+	}
+
+
+	_optimize();
+#endif
+
+
+}
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::set_pairable(OctreeElementID p_id,bool p_pairable,uint32_t p_pairable_type,uint32_t p_pairable_mask) {
+
+	typename ElementMap::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	
+	Element &e = E->get();
+
+	if (p_pairable == e.pairable && e.pairable_type==p_pairable_type && e.pairable_mask==p_pairable_mask)
+		return; // no changes, return
+	
+	if (!e.aabb.has_no_surface()) {
+		_remove_element(&e);
+	}
+	
+	e.pairable=p_pairable;
+	e.pairable_type=p_pairable_type;
+	e.pairable_mask=p_pairable_mask;
+	e.common_parent=NULL;
+	
+	if (!e.aabb.has_no_surface()) {
+		_ensure_valid_root(e.aabb);
+		_insert_element(&e,root);
+		if (use_pairs)
+			_element_check_pairs(&e);
+
+	}	
+}
+
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::erase(OctreeElementID p_id) {
+
+	typename ElementMap::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	
+	Element &e = E->get();
+
+	if (!e.aabb.has_no_surface()) {
+	
+		_remove_element(&e);				
+	}
+		
+	element_map.erase(p_id);
+	_optimize();
+}
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_cull_convex(Octant *p_octant,_CullConvexData *p_cull) {
+	
+	if (*p_cull->result_idx==p_cull->result_max)
+		return; //pointless
+	
+	if (!p_octant->elements.empty()) {
+		
+		typename List< Element*,AL >::Element *I;
+		I=p_octant->elements.front();
+		
+		for(;I;I=I->next()) {
+			
+			Element *e=I->get();
+			
+			if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_cull->mask)))
+				continue;
+			e->last_pass=pass;
+			
+			if (e->aabb.intersects_convex_shape(p_cull->planes,p_cull->plane_count)) {
+				
+				if (*p_cull->result_idx<p_cull->result_max) {
+					p_cull->result_array[*p_cull->result_idx] = e->userdata;
+					(*p_cull->result_idx)++;
+				} else {
+					
+					return; // pointless to continue
+				}
+			}
+		}
+	}
+	
+	if (use_pairs && !p_octant->pairable_elements.empty()) {
+		
+		typename List< Element*,AL >::Element *I;
+		I=p_octant->pairable_elements.front();
+		
+		for(;I;I=I->next()) {
+			
+			Element *e=I->get();
+			
+			if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_cull->mask)))
+				continue;
+			e->last_pass=pass;
+			
+			if (e->aabb.intersects_convex_shape(p_cull->planes,p_cull->plane_count)) {
+				
+				if (*p_cull->result_idx<p_cull->result_max) {
+
+					p_cull->result_array[*p_cull->result_idx] = e->userdata;
+					(*p_cull->result_idx)++;
+				} else {
+					
+					return; // pointless to continue
+				}
+			}
+		}
+	}
+		
+	for (int i=0;i<8;i++) {
+		
+		if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes,p_cull->plane_count)) {
+			_cull_convex(p_octant->children[i],p_cull);
+		}
+	}	
+}
+
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_cull_AABB(Octant *p_octant,const AABB& p_aabb, T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask) {
+	
+	if (*p_result_idx==p_result_max)
+		return; //pointless
+	
+	if (!p_octant->elements.empty()) {
+		
+		typename List< Element*,AL >::Element *I;
+		I=p_octant->elements.front();
+		for(;I;I=I->next()) {
+			
+			Element *e=I->get();
+			
+			if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask)))
+				continue;
+			e->last_pass=pass;
+			
+			if (p_aabb.intersects(e->aabb)) {
+				
+				if (*p_result_idx<p_result_max) {
+					
+					p_result_array[*p_result_idx] = e->userdata;
+					if (p_subindex_array)
+						p_subindex_array[*p_result_idx] = e->subindex;
+
+					(*p_result_idx)++;
+				} else {
+					
+					return; // pointless to continue
+				}
+			}
+		}
+	}
+	
+	if (use_pairs && !p_octant->pairable_elements.empty()) {
+		
+		typename List< Element*,AL >::Element *I;
+		I=p_octant->pairable_elements.front();
+		for(;I;I=I->next()) {
+			
+			Element *e=I->get();
+			
+			if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask)))
+				continue;
+			e->last_pass=pass;
+			
+			if (p_aabb.intersects(e->aabb)) {
+				
+				if (*p_result_idx<p_result_max) {
+					
+					p_result_array[*p_result_idx] = e->userdata;
+					if (p_subindex_array)
+						p_subindex_array[*p_result_idx] = e->subindex;
+					(*p_result_idx)++;
+				} else {
+					
+					return; // pointless to continue
+				}
+			}
+		}
+	}	
+
+	for (int i=0;i<8;i++) {
+		
+		if (p_octant->children[i] && p_octant->children[i]->aabb.intersects(p_aabb)) {
+			_cull_AABB(p_octant->children[i],p_aabb, p_result_array,p_result_idx,p_result_max,p_subindex_array,p_mask);
+		}
+	}	
+
+}
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_cull_segment(Octant *p_octant,const Vector3& p_from, const Vector3& p_to,T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask) {
+
+	if (*p_result_idx==p_result_max)
+		return; //pointless
+	
+	if (!p_octant->elements.empty()) {
+		
+		typename List< Element*,AL >::Element *I;
+		I=p_octant->elements.front();
+		for(;I;I=I->next()) {
+			
+			Element *e=I->get();
+			
+			if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask)))
+				continue;
+			e->last_pass=pass;
+									
+			if (e->aabb.intersects_segment(p_from,p_to)) {
+				
+				if (*p_result_idx<p_result_max) {
+					
+					p_result_array[*p_result_idx] = e->userdata;
+					if (p_subindex_array)
+						p_subindex_array[*p_result_idx] = e->subindex;
+					(*p_result_idx)++;
+
+				} else {
+					
+					return; // pointless to continue
+				}
+			}
+		}
+	}
+	
+	if (use_pairs && !p_octant->pairable_elements.empty()) {
+		
+		typename List< Element*,AL >::Element *I;
+		I=p_octant->pairable_elements.front();
+		for(;I;I=I->next()) {
+			
+			Element *e=I->get();
+			
+			if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask)))
+				continue;
+				
+			e->last_pass=pass;
+									
+			if (e->aabb.intersects_segment(p_from,p_to)) {
+				
+				if (*p_result_idx<p_result_max) {
+					
+					p_result_array[*p_result_idx] = e->userdata;
+					if (p_subindex_array)
+						p_subindex_array[*p_result_idx] = e->subindex;
+
+					(*p_result_idx)++;
+
+				} else {
+					
+					return; // pointless to continue
+				}
+			}
+		}
+	}
+	
+
+	for (int i=0;i<8;i++) {
+		
+		if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_segment(p_from,p_to)) {
+			_cull_segment(p_octant->children[i],p_from,p_to, p_result_array,p_result_idx,p_result_max,p_subindex_array,p_mask);
+		}
+	}	
+}
+
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::_cull_point(Octant *p_octant,const Vector3& p_point,T** p_result_array,int *p_result_idx,int p_result_max,int *p_subindex_array,uint32_t p_mask) {
+
+	if (*p_result_idx==p_result_max)
+		return; //pointless
+
+	if (!p_octant->elements.empty()) {
+
+		typename List< Element*,AL >::Element *I;
+		I=p_octant->elements.front();
+		for(;I;I=I->next()) {
+
+			Element *e=I->get();
+
+			if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask)))
+				continue;
+			e->last_pass=pass;
+
+			if (e->aabb.has_point(p_point)) {
+
+				if (*p_result_idx<p_result_max) {
+
+					p_result_array[*p_result_idx] = e->userdata;
+					if (p_subindex_array)
+						p_subindex_array[*p_result_idx] = e->subindex;
+					(*p_result_idx)++;
+
+				} else {
+
+					return; // pointless to continue
+				}
+			}
+		}
+	}
+
+	if (use_pairs && !p_octant->pairable_elements.empty()) {
+
+		typename List< Element*,AL >::Element *I;
+		I=p_octant->pairable_elements.front();
+		for(;I;I=I->next()) {
+
+			Element *e=I->get();
+
+			if (e->last_pass==pass || (use_pairs && !(e->pairable_type&p_mask)))
+				continue;
+
+			e->last_pass=pass;
+
+			if (e->aabb.has_point(p_point)) {
+
+				if (*p_result_idx<p_result_max) {
+
+					p_result_array[*p_result_idx] = e->userdata;
+					if (p_subindex_array)
+						p_subindex_array[*p_result_idx] = e->subindex;
+
+					(*p_result_idx)++;
+
+				} else {
+
+					return; // pointless to continue
+				}
+			}
+		}
+	}
+
+
+	for (int i=0;i<8;i++) {
+
+		//could be optimized..
+		if (p_octant->children[i] && p_octant->children[i]->aabb.has_point(p_point)) {
+			_cull_point(p_octant->children[i],p_point, p_result_array,p_result_idx,p_result_max,p_subindex_array,p_mask);
+		}
+	}
+}
+
+template<class T,bool use_pairs,class AL>
+int Octree<T,use_pairs,AL>::cull_convex(const Vector<Plane>& p_convex,T** p_result_array,int p_result_max,uint32_t p_mask) {
+
+	if (!root)
+		return 0;
+	
+	int result_count=0;
+	pass++;
+	_CullConvexData cdata;
+	cdata.planes=&p_convex[0];
+	cdata.plane_count=p_convex.size();
+	cdata.result_array=p_result_array;
+	cdata.result_max=p_result_max;
+	cdata.result_idx=&result_count;
+	cdata.mask=p_mask;
+	
+	_cull_convex(root,&cdata);
+	
+	return result_count;
+}
+
+
+
+template<class T,bool use_pairs,class AL>
+int Octree<T,use_pairs,AL>::cull_AABB(const AABB& p_aabb,T** p_result_array,int p_result_max,int *p_subindex_array,uint32_t p_mask) {
+
+
+	if (!root)
+		return 0;
+	
+	int result_count=0;
+	pass++;
+	_cull_AABB(root,p_aabb,p_result_array,&result_count,p_result_max,p_subindex_array,p_mask);
+			
+	return result_count;
+}
+
+
+template<class T,bool use_pairs,class AL>
+int Octree<T,use_pairs,AL>::cull_segment(const Vector3& p_from, const Vector3& p_to,T** p_result_array,int p_result_max,int *p_subindex_array,uint32_t p_mask) {
+
+	if (!root)
+		return 0;
+	
+	int result_count=0;
+	pass++;
+	_cull_segment(root,p_from,p_to,p_result_array,&result_count,p_result_max,p_subindex_array,p_mask);
+			
+	return result_count;
+
+}
+
+template<class T,bool use_pairs,class AL>
+int Octree<T,use_pairs,AL>::cull_point(const Vector3& p_point,T** p_result_array,int p_result_max,int *p_subindex_array,uint32_t p_mask) {
+
+	if (!root)
+		return 0;
+
+	int result_count=0;
+	pass++;
+	_cull_point(root,p_point,p_result_array,&result_count,p_result_max,p_subindex_array,p_mask);
+
+	return result_count;
+
+}
+
+
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::set_pair_callback( PairCallback p_callback, void *p_userdata ) {
+
+	pair_callback=p_callback;
+	pair_callback_userdata=p_userdata;
+}
+template<class T,bool use_pairs,class AL>
+void Octree<T,use_pairs,AL>::set_unpair_callback( UnpairCallback p_callback, void *p_userdata ) {
+
+	unpair_callback=p_callback;
+	unpair_callback_userdata=p_userdata;
+
+}
+
+
+template<class T,bool use_pairs,class AL>
+Octree<T,use_pairs,AL>::Octree(real_t p_unit_size) {
+	
+	last_element_id=1;
+	pass=1;
+	unit_size=p_unit_size;
+	root=NULL;
+
+	octant_count=0;
+	pair_count=0;
+
+	pair_callback=NULL;
+	unpair_callback=NULL;
+	pair_callback_userdata=NULL;
+	unpair_callback_userdata=NULL;
+
+
+
+
+}
+
+
+
+
+#endif
diff --git a/core/math/plane.cpp b/core/math/plane.cpp
new file mode 100644
index 0000000000..88c7be5f63
--- /dev/null
+++ b/core/math/plane.cpp
@@ -0,0 +1,166 @@
+/*************************************************************************/
+/*  plane.cpp                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "plane.h"
+
+#include "math_funcs.h"
+
+#define _PLANE_EQ_DOT_EPSILON 0.999
+#define _PLANE_EQ_D_EPSILON 0.0001
+
+void Plane::set_normal(const Vector3& p_normal) {
+
+        normal=p_normal;
+}
+
+void Plane::normalize() {
+
+	real_t l = normal.length();
+	if (l==0) {
+		*this=Plane(0,0,0,0);
+		return;
+	}
+	normal/=l;
+	d/=l;
+}
+
+Plane Plane::normalized() const {
+	
+	Plane p = *this;
+	p.normalize();
+	return p;
+}
+
+Vector3 Plane::get_any_point() const {
+
+	return get_normal()*d;
+}
+
+Vector3 Plane::get_any_perpendicular_normal() const {
+
+	static const Vector3 p1 = Vector3(1,0,0);
+	static const Vector3 p2 = Vector3(0,1,0);
+	Vector3 p;
+	
+	if (ABS(normal.dot(p1)) > 0.99) // if too similar to p1
+		p=p2; // use p2
+	else
+		p=p1; // use p1
+	
+	p-=normal * normal.dot(p);
+	p.normalize();
+
+	return p;
+}
+
+/* intersections */
+
+bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result) const {
+
+	const Plane &p_plane0=*this;
+	Vector3 normal0=p_plane0.normal;
+	Vector3 normal1=p_plane1.normal;
+	Vector3 normal2=p_plane2.normal;
+
+	real_t denom=vec3_cross(normal0,normal1).dot(normal2);
+
+	if (ABS(denom)<=CMP_EPSILON)
+		return false;
+
+        if (r_result) {
+                *r_result = 	( (vec3_cross(normal1, normal2) * p_plane0.d) +
+			(vec3_cross(normal2, normal0) * p_plane1.d) +
+			(vec3_cross(normal0, normal1) * p_plane2.d) )/denom;
+        }
+
+	return true;
+}
+
+
+bool Plane::intersects_ray(Vector3 p_from, Vector3 p_dir, Vector3* p_intersection)  const {
+
+	Vector3 segment=p_dir;
+	real_t den=normal.dot( segment );
+
+	//printf("den is %i\n",den);
+	if (Math::abs(den)<=CMP_EPSILON) {
+
+		return false;
+	}
+
+	real_t dist=(normal.dot( p_from ) - d) / den;
+	//printf("dist is %i\n",dist);
+
+	if (dist>CMP_EPSILON) { //this is a ray, before the emiting pos (p_from) doesnt exist
+
+		return false;
+	}
+
+	dist=-dist;
+	*p_intersection = p_from + segment * dist;
+
+	return true;
+}
+
+bool Plane::intersects_segment(Vector3 p_begin, Vector3 p_end, Vector3* p_intersection)  const {
+
+	Vector3 segment= p_begin - p_end;
+	real_t den=normal.dot( segment );
+
+	//printf("den is %i\n",den);
+	if (Math::abs(den)<=CMP_EPSILON) {
+
+		return false;
+	}
+
+	real_t dist=(normal.dot( p_begin ) - d) / den;
+	//printf("dist is %i\n",dist);
+
+	if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) {
+
+		return false;
+	}
+
+	dist=-dist;
+	*p_intersection = p_begin + segment * dist;
+
+	return true;
+}
+
+/* misc */
+
+bool Plane::is_almost_like(const Plane& p_plane) const {
+
+	return (normal.dot( p_plane.normal ) > _PLANE_EQ_DOT_EPSILON && Math::absd(d-p_plane.d) < _PLANE_EQ_D_EPSILON);
+}
+
+
+Plane::operator String() const {
+
+	return normal.operator String() + ", " + rtos(d);
+}
diff --git a/core/math/plane.h b/core/math/plane.h
new file mode 100644
index 0000000000..608ec26926
--- /dev/null
+++ b/core/math/plane.h
@@ -0,0 +1,146 @@
+/*************************************************************************/
+/*  plane.h                                                              */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef PLANE_H
+#define PLANE_H
+
+
+#include "vector3.h"
+
+class Plane {
+public:
+
+	Vector3 normal;
+	real_t d;
+
+
+	void set_normal(const Vector3& p_normal);
+	_FORCE_INLINE_ Vector3 get_normal() const { return normal; }; ///Point is coplanar, CMP_EPSILON for precision
+
+	void normalize();
+	Plane normalized() const;
+
+        /* Plane-Point operations */
+
+	_FORCE_INLINE_ Vector3 center() const { return normal*d; }
+	Vector3 get_any_point() const;
+	Vector3 get_any_perpendicular_normal() const;
+
+	_FORCE_INLINE_ bool is_point_over(const Vector3 &p_point) const; ///< Point is over plane
+	_FORCE_INLINE_ real_t distance_to(const Vector3 &p_point) const;
+	_FORCE_INLINE_ bool has_point(const Vector3 &p_point,real_t _epsilon=CMP_EPSILON) const;
+
+        /* intersections */
+
+	bool intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result=0) const;
+	bool intersects_ray(Vector3 p_from, Vector3 p_dir, Vector3* p_intersection)  const;
+	bool intersects_segment(Vector3 p_begin, Vector3 p_end, Vector3* p_intersection)  const;
+
+	_FORCE_INLINE_ Vector3 project(const Vector3& p_point) const {
+	
+		return p_point - normal * distance_to(p_point);
+	}	
+
+        /* misc */
+
+	Plane operator-() const { return Plane(-normal,-d); }
+	bool is_almost_like(const Plane& p_plane) const;
+
+	_FORCE_INLINE_ bool operator==(const Plane& p_plane) const;
+	_FORCE_INLINE_ bool operator!=(const Plane& p_plane) const;
+	operator String() const;
+
+	_FORCE_INLINE_ Plane() { d=0; }
+	_FORCE_INLINE_ Plane(real_t p_a, real_t p_b, real_t p_c, real_t p_d) : normal(p_a,p_b,p_c), d(p_d) { };
+
+	_FORCE_INLINE_ Plane(const Vector3 &p_normal, real_t p_d);
+	_FORCE_INLINE_ Plane(const Vector3 &p_point, const Vector3& p_normal);
+	_FORCE_INLINE_ Plane(const Vector3 &p_point1, const Vector3 &p_point2,const Vector3 &p_point3,ClockDirection p_dir = CLOCKWISE);
+
+
+};
+
+
+
+bool Plane::is_point_over(const Vector3 &p_point) const {
+
+	return (normal.dot(p_point) > d);
+}
+
+real_t Plane::distance_to(const Vector3 &p_point) const {
+
+	return (normal.dot(p_point)-d);
+}
+
+bool Plane::has_point(const Vector3 &p_point,real_t _epsilon) const {
+
+	float dist=normal.dot(p_point) - d;
+	dist=ABS(dist);
+	return ( dist <= _epsilon);
+
+}
+
+Plane::Plane(const Vector3 &p_normal, real_t p_d) {
+
+	normal=p_normal;
+	d=p_d;
+}
+
+Plane::Plane(const Vector3 &p_point, const Vector3& p_normal) {
+
+	normal=p_normal;
+	d=p_normal.dot(p_point);
+}
+
+Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3,ClockDirection p_dir) {
+
+	if (p_dir == CLOCKWISE)
+		normal=(p_point1-p_point3).cross(p_point1-p_point2);
+	else
+		normal=(p_point1-p_point2).cross(p_point1-p_point3);
+
+
+	normal.normalize();
+	d = normal.dot(p_point1);
+
+
+}
+
+bool Plane::operator==(const Plane& p_plane) const {
+
+	return normal==p_plane.normal && d == p_plane.d;
+}
+
+bool Plane::operator!=(const Plane& p_plane) const {
+
+	return normal!=p_plane.normal || d != p_plane.d;
+
+}
+
+#endif // PLANE_H
+
diff --git a/core/math/quat.cpp b/core/math/quat.cpp
new file mode 100644
index 0000000000..950a4756ad
--- /dev/null
+++ b/core/math/quat.cpp
@@ -0,0 +1,267 @@
+/*************************************************************************/
+/*  quat.cpp                                                             */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "quat.h"
+#include "print_string.h"
+
+void Quat::set_euler(const Vector3& p_euler) {
+	real_t half_yaw = p_euler.x * 0.5;  
+	real_t half_pitch = p_euler.y * 0.5;  
+	real_t half_roll = p_euler.z * 0.5;  
+	real_t cos_yaw = Math::cos(half_yaw);
+	real_t sin_yaw = Math::sin(half_yaw);
+	real_t cos_pitch = Math::cos(half_pitch);
+	real_t sin_pitch = Math::sin(half_pitch);
+	real_t cos_roll = Math::cos(half_roll);
+	real_t sin_roll = Math::sin(half_roll);
+	set(cos_roll * sin_pitch * cos_yaw+sin_roll * cos_pitch * sin_yaw,
+		cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw,
+		sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw,
+		cos_roll * cos_pitch * cos_yaw+sin_roll * sin_pitch * sin_yaw);
+}
+
+void Quat::operator*=(const Quat& q) {
+
+	set(w * q.x+x * q.w+y * q.z - z * q.y,
+		w * q.y+y * q.w+z * q.x - x * q.z,
+		w * q.z+z * q.w+x * q.y - y * q.x,
+		w * q.w - x * q.x - y * q.y - z * q.z);
+}
+
+Quat Quat::operator*(const Quat& q) const {
+
+	Quat r=*this;
+	r*=q;
+	return r;
+}
+
+
+
+
+real_t Quat::length() const {
+
+	return Math::sqrt(length_squared());
+}
+
+void Quat::normalize() {
+	*this /= length();
+}
+
+
+Quat Quat::normalized() const {
+	return *this / length();
+} 
+
+Quat Quat::inverse() const {
+	return Quat( -x, -y, -z, w );
+}
+
+
+Quat Quat::slerp(const Quat& q, const real_t& t) const {
+
+#if 0
+
+
+	Quat dst=q;
+	Quat src=*this;
+
+	src.normalize();
+	dst.normalize();
+
+	real_t cosine = dst.dot(src);
+
+	if (cosine < 0 && true) {
+		cosine = -cosine;
+		dst = -dst;
+	} else {
+		dst = dst;
+	}
+
+	if (Math::abs(cosine) < 1 - CMP_EPSILON) {
+		// Standard case (slerp)
+		real_t sine = Math::sqrt(1 - cosine*cosine);
+		real_t angle = Math::atan2(sine, cosine);
+		real_t inv_sine = 1.0f / sine;
+		real_t coeff_0 = Math::sin((1.0f - t) * angle) * inv_sine;
+		real_t coeff_1 = Math::sin(t * angle) * inv_sine;
+		Quat ret=  src * coeff_0 + dst * coeff_1;
+
+		return ret;
+	} else {
+		// There are two situations:
+		// 1. "rkP" and "q" are very close (cosine ~= +1), so we can do a linear
+		//    interpolation safely.
+		// 2. "rkP" and "q" are almost invedste of each other (cosine ~= -1), there
+		//    are an infinite number of possibilities interpolation. but we haven't
+		//    have method to fix this case, so just use linear interpolation here.
+		Quat ret =  src * (1.0f - t) + dst *t;
+		// taking the complement requires renormalisation
+		ret.normalize();
+		return ret;
+	}
+#else
+
+	real_t         to1[4];
+	real_t        omega, cosom, sinom, scale0, scale1;
+
+
+	// calc cosine
+	cosom = x * q.x + y * q.y + z * q.z
+			+ w * q.w;
+
+
+	// adjust signs (if necessary)
+	if ( cosom <0.0 ) {
+		cosom = -cosom; to1[0] = - q.x;
+		to1[1] = - q.y;
+		to1[2] = - q.z;
+		to1[3] = - q.w;
+	} else  {
+		to1[0] = q.x;
+		to1[1] = q.y;
+		to1[2] = q.z;
+		to1[3] = q.w;
+	}
+
+
+	// calculate coefficients
+
+	if ( (1.0 - cosom) > CMP_EPSILON ) {
+		// standard case (slerp)
+		omega = Math::acos(cosom);
+		sinom = Math::sin(omega);
+		scale0 = Math::sin((1.0 - t) * omega) / sinom;
+		scale1 = Math::sin(t * omega) / sinom;
+	} else {
+		// "from" and "to" quaternions are very close
+		//  ... so we can do a linear interpolation
+		scale0 = 1.0 - t;
+		scale1 = t;
+	}
+	// calculate final values
+	return Quat(
+		scale0 * x + scale1 * to1[0],
+		scale0 * y + scale1 * to1[1],
+		scale0 * z + scale1 * to1[2],
+		scale0 * w + scale1 * to1[3]
+	);
+#endif
+}
+
+Quat Quat::slerpni(const Quat& q, const real_t& t) const {
+
+	const Quat &from = *this;
+
+	float dot = from.dot(q);
+
+	if (Math::absf(dot) > 0.9999f) return from;
+
+	float	theta		= Math::acos(dot),
+		sinT		= 1.0f / Math::sin(theta),
+		newFactor	= Math::sin(t * theta) * sinT,
+		invFactor	= Math::sin((1.0f - t) * theta) * sinT;
+
+	return Quat( invFactor * from.x + newFactor * q.x,
+			   invFactor * from.y + newFactor * q.y,
+			   invFactor * from.z + newFactor * q.z,
+			   invFactor * from.w + newFactor * q.w );
+
+#if 0
+	real_t         to1[4];
+	real_t        omega, cosom, sinom, scale0, scale1;
+
+
+	// calc cosine
+	cosom = x * q.x + y * q.y + z * q.z
+			+ w * q.w;
+
+
+	// adjust signs (if necessary)
+	if ( cosom <0.0 && false) {
+		cosom = -cosom; to1[0] = - q.x;
+		to1[1] = - q.y;
+		to1[2] = - q.z;
+		to1[3] = - q.w;
+	} else  {
+		to1[0] = q.x;
+		to1[1] = q.y;
+		to1[2] = q.z;
+		to1[3] = q.w;
+	}
+
+
+	// calculate coefficients
+
+	if ( (1.0 - cosom) > CMP_EPSILON ) {
+		// standard case (slerp)
+		omega = Math::acos(cosom);
+		sinom = Math::sin(omega);
+		scale0 = Math::sin((1.0 - t) * omega) / sinom;
+		scale1 = Math::sin(t * omega) / sinom;
+	} else {
+		// "from" and "to" quaternions are very close
+		//  ... so we can do a linear interpolation
+		scale0 = 1.0 - t;
+		scale1 = t;
+	}
+	// calculate final values
+	return Quat(
+		scale0 * x + scale1 * to1[0],
+		scale0 * y + scale1 * to1[1],
+		scale0 * z + scale1 * to1[2],
+		scale0 * w + scale1 * to1[3]
+	);
+#endif
+}
+
+Quat Quat::cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq,const real_t& t) const {
+
+	//the only way to do slerp :|
+	float t2 = (1.0-t)*t*2;
+	Quat sp = this->slerp(q,t);
+	Quat sq = prep.slerpni(postq,t);
+	return sp.slerpni(sq,t2);
+
+}
+
+
+Quat::operator String() const {
+
+	return String::num(x)+","+String::num(y)+","+ String::num(z)+","+ String::num(w); 
+}
+
+Quat::Quat(const Vector3& axis, const real_t& angle) {
+	real_t d = axis.length();
+	if (d==0)
+		set(0,0,0,0);
+	else {
+		real_t s = Math::sin(-angle * 0.5) / d;
+		set(axis.x * s, axis.y * s, axis.z * s, 
+			Math::cos(-angle * 0.5));
+	}
+}
diff --git a/core/math/quat.h b/core/math/quat.h
new file mode 100644
index 0000000000..d326073033
--- /dev/null
+++ b/core/math/quat.h
@@ -0,0 +1,157 @@
+/*************************************************************************/
+/*  quat.h                                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef QUAT_H
+#define QUAT_H
+
+#include "math_defs.h"
+#include "math_funcs.h"
+#include "ustring.h"
+#include "vector3.h"
+
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+class Quat{
+public:
+
+	real_t x,y,z,w;
+
+	_FORCE_INLINE_ real_t length_squared() const;
+	real_t length() const;
+	void normalize();
+	Quat normalized() const;
+	Quat inverse() const;
+	_FORCE_INLINE_ real_t dot(const Quat& q) const;
+	void set_euler(const Vector3& p_euler);
+	Quat slerp(const Quat& q, const real_t& t) const;
+	Quat slerpni(const Quat& q, const real_t& t) const;
+	Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq,const real_t& t) const;
+
+	_FORCE_INLINE_ void get_axis_and_angle(Vector3& r_axis, real_t &r_angle) const {
+		r_angle = 2 * Math::acos(w);
+		r_axis.x = -x / Math::sqrt(1-w*w);
+		r_axis.y = -y / Math::sqrt(1-w*w);
+		r_axis.z = -z / Math::sqrt(1-w*w);
+	}
+
+	void operator*=(const Quat& q);
+	Quat operator*(const Quat& q) const;
+
+
+	_FORCE_INLINE_ void operator+=(const Quat& q);
+	_FORCE_INLINE_ void operator-=(const Quat& q);
+	_FORCE_INLINE_ void operator*=(const real_t& s);
+	_FORCE_INLINE_ void operator/=(const real_t& s);
+	_FORCE_INLINE_ Quat operator+(const Quat& q2) const;
+	_FORCE_INLINE_ Quat operator-(const Quat& q2) const;
+	_FORCE_INLINE_ Quat operator-() const;
+	_FORCE_INLINE_ Quat operator*(const real_t& s) const;
+	_FORCE_INLINE_ Quat operator/(const real_t& s) const;
+	
+
+	_FORCE_INLINE_ bool operator==(const Quat& p_quat) const;
+	_FORCE_INLINE_ bool operator!=(const Quat& p_quat) const;
+	
+	operator String() const;
+	
+	inline void set( real_t p_x, real_t p_y, real_t p_z, real_t p_w) {
+		x=p_x; y=p_y; z=p_z; w=p_w;	
+	}
+	inline Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) {
+		x=p_x; y=p_y; z=p_z; w=p_w;	
+	}
+	Quat(const Vector3& axis, const real_t& angle);
+	inline Quat() {x=y=z=0; w=1; }
+	
+
+};
+
+
+real_t Quat::dot(const Quat& q) const {
+	return x * q.x+y * q.y+z * q.z+w * q.w;
+}
+
+real_t Quat::length_squared() const {
+	return dot(*this);
+}
+
+void Quat::operator+=(const Quat& q) {
+	x += q.x; y += q.y; z += q.z; w += q.w;	
+}
+
+void Quat::operator-=(const Quat& q) {
+	x -= q.x; y -= q.y; z -= q.z; w -= q.w;
+}
+
+void Quat::operator*=(const real_t& s) {
+	x *= s; y *= s; z *= s; w *= s;
+}
+
+
+void Quat::operator/=(const real_t& s) {
+
+	*this *= 1.0 / s;
+}
+
+Quat Quat::operator+(const Quat& q2) const {
+	const Quat& q1 = *this;
+	return Quat( q1.x+q2.x, q1.y+q2.y, q1.z+q2.z, q1.w+q2.w );
+}
+
+Quat Quat::operator-(const Quat& q2) const {
+	const Quat& q1 = *this;
+	return Quat( q1.x-q2.x, q1.y-q2.y, q1.z-q2.z, q1.w-q2.w);
+}
+
+Quat Quat::operator-() const {
+	const Quat& q2 = *this;
+	return Quat( -q2.x, -q2.y,  -q2.z,  -q2.w);
+}
+
+Quat Quat::operator*(const real_t& s) const {
+	return Quat(x * s, y * s, z * s, w * s);
+}
+
+Quat Quat::operator/(const real_t& s) const {
+	return *this * (1.0 / s);
+}
+
+
+bool Quat::operator==(const Quat& p_quat) const {
+
+	return x==p_quat.x && y==p_quat.y && z==p_quat.z && w==p_quat.w;
+}
+
+bool Quat::operator!=(const Quat& p_quat) const {
+
+	return x!=p_quat.x || y!=p_quat.y || z!=p_quat.z || w!=p_quat.w;
+}
+
+
+#endif
diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp
new file mode 100644
index 0000000000..ed30de6915
--- /dev/null
+++ b/core/math/quick_hull.cpp
@@ -0,0 +1,513 @@
+/*************************************************************************/
+/*  quick_hull.cpp                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "quick_hull.h"
+#include "map.h"
+
+uint32_t QuickHull::debug_stop_after=0xFFFFFFFF;
+
+Error QuickHull::build(const Vector<Vector3>& p_points, Geometry::MeshData &r_mesh) {
+
+
+	static const real_t over_tolerance = 0.0001;
+
+	/* CREATE AABB VOLUME */
+
+	AABB aabb;
+	for(int i=0;i<p_points.size();i++) {
+
+		if (i==0) {
+			aabb.pos=p_points[i];
+		} else {
+			aabb.expand_to(p_points[i]);
+		}
+	}
+
+
+	if (aabb.size==Vector3()) {
+		return ERR_CANT_CREATE;
+	}
+
+
+	Vector<bool> valid_points;
+	valid_points.resize(p_points.size());
+	Set<Vector3> valid_cache;
+
+	for(int i=0;i<p_points.size();i++) {
+
+		Vector3 sp = p_points[i].snapped(0.0001);
+		if (valid_cache.has(sp)) {
+			valid_points[i]=false;
+			print_line("INVALIDATED: "+itos(i));
+		}else {
+			valid_points[i]=true;
+			valid_cache.insert(sp);
+		}
+	}
+
+
+
+	/* CREATE INITIAL SIMPLEX */
+
+	int longest_axis = aabb.get_longest_axis_index();
+
+	//first two vertices are the most distant
+	int simplex[4];
+
+	{
+		real_t max,min;
+
+		for(int i=0;i<p_points.size();i++) {
+
+			if (!valid_points[i])
+				continue;
+			float d = p_points[i][longest_axis];
+			if (i==0 || d < min) {
+
+				simplex[0]=i;
+				min=d;
+			}
+
+			if (i==0 || d > max) {
+				simplex[1]=i;
+				max=d;
+			}
+
+		}
+	}
+
+	//third vertex is one most further away from the line
+
+
+	{
+		float maxd;
+		Vector3 rel12 =  p_points[simplex[0]] - p_points[simplex[1]];
+
+		for(int i=0;i<p_points.size();i++) {
+
+			if (!valid_points[i])
+				continue;
+
+			Vector3 n = rel12.cross(p_points[simplex[0]]-p_points[i]).cross(rel12).normalized();
+			real_t d = Math::abs(n.dot(p_points[simplex[0]])-n.dot(p_points[i]));
+
+			if (i==0 || d>maxd) {
+
+				maxd=d;
+				simplex[2]=i;
+			}
+		}
+	}
+
+	//fourth vertex is the one  most further away from the plane
+
+	{
+		float maxd;
+		Plane p(p_points[simplex[0]],p_points[simplex[1]],p_points[simplex[2]]);
+
+		for(int i=0;i<p_points.size();i++) {
+
+			if (!valid_points[i])
+				continue;
+
+			real_t d = Math::abs(p.distance_to(p_points[i]));
+
+			if (i==0 || d>maxd) {
+
+				maxd=d;
+				simplex[3]=i;
+			}
+		}
+	}
+
+
+	//compute center of simplex, this is a point always warranted to be inside
+	Vector3 center;
+
+	for(int i=0;i<4;i++) {
+		center+=p_points[simplex[i]];
+	}
+
+	center/=4.0;
+
+	//add faces
+
+	List<Face> faces;
+
+	for(int i=0;i<4;i++) {
+
+		static const int face_order[4][3]={
+			{0,1,2},
+			{0,1,3},
+			{0,2,3},
+			{1,2,3}
+		};
+
+		Face f;
+		for(int j=0;j<3;j++) {
+			f.vertices[j]=simplex[face_order[i][j]];
+		}
+
+
+		Plane p(p_points[f.vertices[0]],p_points[f.vertices[1]],p_points[f.vertices[2]]);
+
+		if (p.is_point_over(center)) {
+			//flip face to clockwise if facing inwards
+			SWAP( f.vertices[0], f.vertices[1] );
+			p=-p;
+		}
+
+
+		f.plane = p;
+
+		faces.push_back(f);
+
+	}
+
+
+	/* COMPUTE AVAILABLE VERTICES */
+
+	for(int i=0;i<p_points.size();i++) {
+
+		if (i==simplex[0])
+			continue;
+		if (i==simplex[1])
+			continue;
+		if (i==simplex[2])
+			continue;
+		if (i==simplex[3])
+			continue;
+		if (!valid_points[i])
+			continue;
+
+		for(List<Face>::Element *E=faces.front();E;E=E->next()) {
+
+			if (E->get().plane.distance_to(p_points[i]) > over_tolerance ) {
+
+				E->get().points_over.push_back(i);
+				break;
+			}
+		}
+
+
+
+	}
+
+	faces.sort(); // sort them, so the ones with points are in the back
+
+
+	/* BUILD HULL */
+
+
+	//poop face (while still remain)
+	//find further away point
+	//find lit faces
+	//determine horizon edges
+	//build new faces with horizon edges, them assign points side from all lit faces
+	//remove lit faces
+
+
+	uint32_t debug_stop = debug_stop_after;
+
+	while(debug_stop>0 && faces.back()->get().points_over.size()) {
+
+		debug_stop--;
+		Face& f = faces.back()->get();
+
+		//find vertex most outside
+		int next=-1;
+		real_t next_d=0;
+
+		for(int i=0;i<f.points_over.size();i++) {
+
+			real_t d = f.plane.distance_to(p_points[f.points_over[i]]);
+
+			if (d > next_d) {
+				next_d=d;
+				next=i;
+			}
+		}
+
+		ERR_FAIL_COND_V(next==-1,ERR_BUG);
+
+
+
+		Vector3 v = p_points[f.points_over[next]];
+
+		//find lit faces and lit edges
+		List< List<Face>::Element* > lit_faces; //lit face is a death sentence
+
+		Map<Edge,FaceConnect> lit_edges; //create this on the flight, should not be that bad for performance and simplifies code a lot
+
+		for(List<Face>::Element *E=faces.front();E;E=E->next()) {
+
+			if (E->get().plane.distance_to(v) >0 ) {
+
+				lit_faces.push_back(E);
+
+				for(int i=0;i<3;i++) {
+					uint32_t a = E->get().vertices[i];
+					uint32_t b = E->get().vertices[(i+1)%3];
+					Edge e(a,b);
+
+					Map<Edge,FaceConnect>::Element *F=lit_edges.find(e);
+					if (!F) {
+						F=lit_edges.insert(e,FaceConnect());
+					}
+					if (e.vertices[0]==a) {
+						//left
+						F->get().left=E;
+					} else {
+
+						F->get().right=E;
+					}
+				}
+			}
+		}
+
+
+		//create new faces from horizon edges
+		List< List<Face>::Element* > new_faces; //new faces
+
+		for(Map<Edge,FaceConnect>::Element *E=lit_edges.front();E;E=E->next()) {
+
+			FaceConnect& fc = E->get();
+			if (fc.left && fc.right) {
+				continue; //edge is uninteresting, not on horizont
+			}
+
+			//create new face!
+
+			Face face;
+			face.vertices[0]=f.points_over[next];
+			face.vertices[1]=E->key().vertices[0];
+			face.vertices[2]=E->key().vertices[1];
+
+			Plane p(p_points[face.vertices[0]],p_points[face.vertices[1]],p_points[face.vertices[2]]);
+
+			if (p.is_point_over(center)) {
+				//flip face to clockwise if facing inwards
+				SWAP( face.vertices[0], face.vertices[1] );
+				p = -p;
+			}
+
+			face.plane = p;
+			new_faces.push_back( faces.push_back(face) );
+		}
+
+		//distribute points into new faces
+
+		for(List< List<Face>::Element* >::Element *F=lit_faces.front();F;F=F->next()) {
+
+			Face &lf = F->get()->get();
+
+			for(int i=0;i<lf.points_over.size();i++) {
+
+				if (lf.points_over[i]==f.points_over[next]) //do not add current one
+					continue;
+
+				Vector3 p = p_points[lf.points_over[i]];
+				for (List< List<Face>::Element* >::Element *E=new_faces.front();E;E=E->next()) {
+
+					Face &f2 = E->get()->get();
+					if (f2.plane.distance_to(p)>over_tolerance) {
+						f2.points_over.push_back(lf.points_over[i]);
+						break;
+					}
+				}
+
+
+			}
+		}
+
+		//erase lit faces
+
+		while(lit_faces.size()) {
+
+			faces.erase(lit_faces.front()->get());
+			lit_faces.pop_front();
+		}
+
+		//put faces that contain no points on the front
+
+		for (List< List<Face>::Element* >::Element *E=new_faces.front();E;E=E->next()) {
+
+			Face &f2 = E->get()->get();
+			if (f2.points_over.size()==0) {
+				faces.move_to_front(E->get());
+			}
+		}
+
+		//whew, done with iteration, go next
+
+
+
+	}
+
+	/* CREATE MESHDATA */
+
+
+	//make a map of edges again
+	Map<Edge,RetFaceConnect> ret_edges;
+	List<Geometry::MeshData::Face> ret_faces;
+
+
+	for(List<Face>::Element *E=faces.front();E;E=E->next()) {
+
+		Geometry::MeshData::Face f;
+		f.plane = E->get().plane;
+
+
+
+		for(int i=0;i<3;i++) {
+			f.indices.push_back(E->get().vertices[i]);
+		}
+
+		List<Geometry::MeshData::Face>::Element *F = ret_faces.push_back(f);
+
+		for(int i=0;i<3;i++) {
+
+			uint32_t a = E->get().vertices[i];
+			uint32_t b = E->get().vertices[(i+1)%3];
+			Edge e(a,b);
+
+			Map<Edge,RetFaceConnect>::Element *G=ret_edges.find(e);
+			if (!G) {
+				G=ret_edges.insert(e,RetFaceConnect());
+			}
+			if (e.vertices[0]==a) {
+				//left
+				G->get().left=F;
+			} else {
+
+				G->get().right=F;
+			}
+		}
+	}
+
+	//fill faces
+
+	for (List<Geometry::MeshData::Face>::Element *E=ret_faces.front();E;E=E->next()) {
+
+		Geometry::MeshData::Face& f = E->get();
+
+		for(int i=0;i<f.indices.size();i++) {
+
+			uint32_t a = E->get().indices[i];
+			uint32_t b = E->get().indices[(i+1)%f.indices.size()];
+			Edge e(a,b);
+
+			Map<Edge,RetFaceConnect>::Element *F=ret_edges.find(e);
+
+			ERR_CONTINUE(!F);
+
+			List<Geometry::MeshData::Face>::Element *O = F->get().left == E ? F->get().right : F->get().left;
+			ERR_CONTINUE(O==E);
+
+			if (O->get().plane.is_almost_like(f.plane)) {
+				//merge and delete edge and contiguous face, while repointing edges (uuugh!)
+				int ois = O->get().indices.size();
+				int merged=0;
+
+
+				for(int j=0;j<ois;j++) {
+					//search a
+					if (O->get().indices[j]==a) {
+						//append the rest
+						for(int k=0;k<ois;k++) {
+
+							int idx = O->get().indices[(k+j)%ois];
+							int idxn = O->get().indices[(k+j+1)%ois];
+							if (idx==b && idxn==a) {//already have b!
+								break;
+							}
+							if (idx!=a) {
+								f.indices.insert(i+1,idx);
+								i++;
+								merged++;
+							}
+							Edge e2(idx,idxn);
+
+							Map<Edge,RetFaceConnect>::Element *F2=ret_edges.find(e2);
+
+							ERR_CONTINUE(!F2);
+							//change faceconnect, point to this face instead
+							if (F2->get().left == O)
+								F2->get().left=E;
+							else if (F2->get().right == O)
+								F2->get().right=E;
+
+						}
+
+						break;
+					}
+				}
+
+
+				ret_edges.erase(F); //remove the edge
+				ret_faces.erase(O); //remove the face
+
+
+			}
+
+		}
+
+	}
+
+	//fill mesh
+	r_mesh.faces.clear();
+	r_mesh.faces.resize(ret_faces.size());
+//	print_line("FACECOUNT: "+itos(r_mesh.faces.size()));
+
+	int idx=0;
+	for (List<Geometry::MeshData::Face>::Element *E=ret_faces.front();E;E=E->next()) {
+		r_mesh.faces[idx++]=E->get();
+
+
+	}
+	r_mesh.edges.resize(ret_edges.size());
+	idx=0;
+	for(Map<Edge,RetFaceConnect>::Element *E=ret_edges.front();E;E=E->next()) {
+
+		Geometry::MeshData::Edge e;
+		e.a=E->key().vertices[0];
+		e.b=E->key().vertices[1];
+		r_mesh.edges[idx++]=e;
+	}
+
+	r_mesh.vertices=p_points;
+
+	//r_mesh.optimize_vertices();
+/*
+	print_line("FACES: "+itos(r_mesh.faces.size()));
+	print_line("EDGES: "+itos(r_mesh.edges.size()));
+	print_line("VERTICES: "+itos(r_mesh.vertices.size()));
+*/
+
+	return OK;
+}
diff --git a/core/math/quick_hull.h b/core/math/quick_hull.h
new file mode 100644
index 0000000000..d7f056d366
--- /dev/null
+++ b/core/math/quick_hull.h
@@ -0,0 +1,95 @@
+/*************************************************************************/
+/*  quick_hull.h                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef QUICK_HULL_H
+#define QUICK_HULL_H
+
+#include "aabb.h"
+#include "set.h"
+#include "list.h"
+#include "geometry.h"
+
+class QuickHull {
+
+public:
+
+
+	struct Edge {
+
+		union {
+			uint32_t vertices[2];
+			uint64_t id;
+		};
+
+
+		bool operator<(const Edge& p_edge) const {
+			return id < p_edge.id;
+		}
+
+		Edge(int p_vtx_a=0,int p_vtx_b=0) {
+
+			if (p_vtx_a>p_vtx_b) {
+				SWAP(p_vtx_a,p_vtx_b);
+			}
+
+			vertices[0]=p_vtx_a;
+			vertices[1]=p_vtx_b;
+		}
+	};
+
+	struct Face {
+
+		Plane plane;
+		int vertices[3];
+		Vector<int> points_over;
+
+		bool operator<(const Face& p_face) const {
+
+			return points_over.size() < p_face.points_over.size();
+		}
+
+	};
+private:
+
+	struct FaceConnect {
+		List<Face>::Element *left,*right;
+		FaceConnect() { left=NULL; right=NULL; }
+	};
+	struct RetFaceConnect {
+		List<Geometry::MeshData::Face>::Element *left,*right;
+		RetFaceConnect() { left=NULL; right=NULL; }
+	};
+
+public:
+
+	static uint32_t debug_stop_after;
+	static Error build(const Vector<Vector3>& p_points,Geometry::MeshData& r_mesh);
+
+};
+
+#endif // QUICK_HULL_H
diff --git a/core/math/transform.cpp b/core/math/transform.cpp
new file mode 100644
index 0000000000..bb874facbd
--- /dev/null
+++ b/core/math/transform.cpp
@@ -0,0 +1,218 @@
+/*************************************************************************/
+/*  transform.cpp                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "transform.h"
+#include "math_funcs.h"
+#include "os/copymem.h"
+#include "print_string.h"
+	
+
+void Transform::affine_invert() {
+
+	basis.invert();
+	origin = basis.xform(-origin);
+}
+
+Transform Transform::affine_inverse() const {
+
+	Transform ret=*this;
+	ret.affine_invert();
+	return ret;
+
+}
+
+
+void Transform::invert() {
+
+	basis.transpose();
+	origin = basis.xform(-origin);	
+}
+
+Transform Transform::inverse() const {
+
+	Transform ret=*this;
+	ret.invert();
+	return ret;
+}
+
+
+void Transform::rotate(const Vector3& p_axis,real_t p_phi) {
+
+	*this = *this * Transform( Matrix3( p_axis, p_phi ), Vector3()  );
+}
+
+Transform Transform::rotated(const Vector3& p_axis,real_t p_phi) const{
+
+	return *this * Transform( Matrix3( p_axis, p_phi ), Vector3()  );
+}
+
+void Transform::rotate_basis(const Vector3& p_axis,real_t p_phi) {
+
+	basis.rotate(p_axis,p_phi);
+}
+
+Transform Transform::looking_at( const Vector3& p_target, const Vector3& p_up ) const {
+
+	Transform t = *this;
+	t.set_look_at(origin,p_target,p_up);
+	return t;
+}
+
+void Transform::set_look_at( const Vector3& p_eye, const Vector3& p_target, const Vector3& p_up ) {
+
+	// Reference: MESA source code
+	Vector3 v_x, v_y, v_z;
+	
+	/* Make rotation matrix */
+	
+	/* Z vector */
+	v_z = p_eye - p_target;
+	
+	v_z.normalize();
+	
+	v_y = p_up;
+	
+
+	v_x=v_y.cross(v_z);
+	
+	/* Recompute Y = Z cross X */
+	v_y=v_z.cross(v_x);
+	
+	v_x.normalize();
+	v_y.normalize();
+	
+	basis.set_axis(0,v_x);
+	basis.set_axis(1,v_y);
+	basis.set_axis(2,v_z);
+	origin=p_eye;
+	
+}
+
+Transform Transform::interpolate_with(const Transform& p_transform, float p_c) const {
+
+	/* not sure if very "efficient" but good enough? */
+
+	Vector3 src_scale = basis.get_scale();
+	Quat src_rot = basis;
+	Vector3 src_loc = origin;
+
+	Vector3 dst_scale = p_transform.basis.get_scale();
+	Quat dst_rot = p_transform.basis;
+	Vector3 dst_loc = p_transform.origin;
+
+	Transform dst;
+	dst.basis=src_rot.slerp(dst_rot,p_c);
+	dst.basis.scale(src_scale.linear_interpolate(dst_scale,p_c));
+	dst.origin=src_loc.linear_interpolate(dst_loc,p_c);
+
+	return dst;
+}
+
+void Transform::scale(const Vector3& p_scale) {
+
+	basis.scale(p_scale);
+	origin*=p_scale;
+}
+
+Transform Transform::scaled(const Vector3& p_scale) const {
+
+	Transform t = *this;
+	t.scale(p_scale);
+	return t;
+}
+
+void Transform::scale_basis(const Vector3& p_scale) {
+
+	basis.scale(p_scale);
+}
+
+void Transform::translate( real_t p_tx, real_t p_ty, real_t p_tz)  {
+	translate( Vector3(p_tx,p_ty,p_tz) );
+
+}
+void Transform::translate( const Vector3& p_translation ) {
+
+	for( int i = 0; i < 3; i++ ) {
+		origin[i] += basis[i].dot(p_translation);
+	}
+}
+
+Transform Transform::translated( const Vector3& p_translation ) const {
+
+	Transform t=*this;
+	t.translate(p_translation);
+	return t;
+}
+
+void Transform::orthonormalize() {
+
+	basis.orthonormalize();
+}
+
+Transform Transform::orthonormalized() const {
+
+	Transform _copy = *this;
+	_copy.orthonormalize();
+	return _copy;
+}
+
+bool Transform::operator==(const Transform& p_transform) const {
+
+	return (basis==p_transform.basis && origin==p_transform.origin);
+}
+bool Transform::operator!=(const Transform& p_transform) const {
+
+	return (basis!=p_transform.basis || origin!=p_transform.origin);
+}
+
+void Transform::operator*=(const Transform& p_transform) {
+
+	origin=xform(p_transform.origin);
+	basis*=p_transform.basis;	
+}
+
+Transform Transform::operator*(const Transform& p_transform) const {
+
+	Transform t=*this;
+	t*=p_transform;
+	return t;
+}
+
+Transform::operator String() const {
+
+	return basis.operator String() + " - " + origin.operator String();
+}
+
+
+Transform::Transform(const Matrix3& p_basis, const Vector3& p_origin) {
+
+	basis=p_basis;
+	origin=p_origin;
+}
+
+
diff --git a/core/math/transform.h b/core/math/transform.h
new file mode 100644
index 0000000000..b1a0ea1ab8
--- /dev/null
+++ b/core/math/transform.h
@@ -0,0 +1,268 @@
+/*************************************************************************/
+/*  transform.h                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef TRANSFORM_H
+#define TRANSFORM_H
+
+#include "matrix3.h"
+#include "plane.h"
+#include "aabb.h"
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+class Transform {
+public:
+
+	Matrix3 basis;
+	Vector3 origin;
+	
+	void invert(); 
+	Transform inverse() const; 
+
+	void affine_invert();
+	Transform affine_inverse() const;
+
+	Transform rotated(const Vector3& p_axis,real_t p_phi) const;
+
+	void rotate(const Vector3& p_axis,real_t p_phi);
+	void rotate_basis(const Vector3& p_axis,real_t p_phi);
+
+	void set_look_at( const Vector3& p_eye, const Vector3& p_target, const Vector3& p_up );
+	Transform looking_at( const Vector3& p_target, const Vector3& p_up ) const;
+
+	void scale(const Vector3& p_scale);
+	Transform scaled(const Vector3& p_scale) const;
+	void scale_basis(const Vector3& p_scale);
+	void translate( real_t p_tx, real_t p_ty, real_t p_tz );
+	void translate( const Vector3& p_translation );
+	Transform translated( const Vector3& p_translation ) const;
+
+	const Matrix3& get_basis() const { return basis; }
+	void set_basis(const Matrix3& p_basis) { basis=p_basis; }
+
+	const Vector3& get_origin() const { return origin; }
+	void set_origin(const Vector3& p_origin) { origin=p_origin; }
+
+	void orthonormalize();
+	Transform orthonormalized() const;
+
+	bool operator==(const Transform& p_transform) const;
+	bool operator!=(const Transform& p_transform) const;
+
+	_FORCE_INLINE_ Vector3 xform(const Vector3& p_vector) const;
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3& p_vector) const;
+	
+	_FORCE_INLINE_ Plane xform(const Plane& p_plane) const;
+	_FORCE_INLINE_ Plane xform_inv(const Plane& p_plane) const;
+
+	_FORCE_INLINE_ AABB xform(const AABB& p_aabb) const;
+	_FORCE_INLINE_ AABB xform_inv(const AABB& p_aabb) const;
+	
+	void operator*=(const Transform& p_transform);
+	Transform operator*(const Transform& p_transform) const;
+
+	Transform interpolate_with(const Transform& p_transform, float p_c) const;
+	
+	_FORCE_INLINE_ Transform inverse_xform(const Transform& t) const {
+		
+		Vector3 v = t.origin - origin;
+		return Transform(basis.transpose_xform(t.basis),
+			basis.xform(v));
+	}
+	
+	void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz,real_t tx, real_t ty, real_t tz) {
+	
+		basis.elements[0][0]=xx;
+		basis.elements[0][1]=xy;
+		basis.elements[0][2]=xz;
+		basis.elements[1][0]=yx;
+		basis.elements[1][1]=yy;
+		basis.elements[1][2]=yz;
+		basis.elements[2][0]=zx;
+		basis.elements[2][1]=zy;
+		basis.elements[2][2]=zz;	
+		origin.x=tx;
+		origin.y=ty;
+		origin.z=tz;
+	}
+	
+	operator String() const;
+	
+	Transform(const Matrix3& p_basis, const Vector3& p_origin=Vector3());
+	Transform() {}
+
+};
+
+
+_FORCE_INLINE_ Vector3 Transform::xform(const Vector3& p_vector) const {
+
+	return Vector3(
+		basis[0].dot(p_vector)+origin.x,
+		basis[1].dot(p_vector)+origin.y,
+		basis[2].dot(p_vector)+origin.z
+	);
+}
+_FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3& p_vector) const {
+	
+	Vector3 v = p_vector - origin;
+	
+	return Vector3(
+		(basis.elements[0][0]*v.x ) + ( basis.elements[1][0]*v.y ) + ( basis.elements[2][0]*v.z ),
+		(basis.elements[0][1]*v.x ) + ( basis.elements[1][1]*v.y ) + ( basis.elements[2][1]*v.z ),
+		(basis.elements[0][2]*v.x ) + ( basis.elements[1][2]*v.y ) + ( basis.elements[2][2]*v.z )
+	);
+}
+
+_FORCE_INLINE_ Plane Transform::xform(const Plane& p_plane) const {
+
+	
+	Vector3 point=p_plane.normal*p_plane.d;
+	Vector3 point_dir=point+p_plane.normal;
+	point=xform(point);
+	point_dir=xform(point_dir);
+	
+	Vector3 normal=point_dir-point;
+	normal.normalize();
+	real_t d=normal.dot(point);
+	
+	return Plane(normal,d);
+
+}
+_FORCE_INLINE_ Plane Transform::xform_inv(const Plane& p_plane) const {
+
+	Vector3 point=p_plane.normal*p_plane.d;
+	Vector3 point_dir=point+p_plane.normal;
+	xform_inv(point);
+	xform_inv(point_dir);
+	
+	Vector3 normal=point_dir-point;
+	normal.normalize();
+	real_t d=normal.dot(point);
+	
+	return Plane(normal,d);
+
+}
+
+_FORCE_INLINE_ AABB Transform::xform(const AABB& p_aabb) const {
+	/* define vertices */
+#if 1
+	Vector3 x=basis.get_axis(0)*p_aabb.size.x;
+	Vector3 y=basis.get_axis(1)*p_aabb.size.y;
+	Vector3 z=basis.get_axis(2)*p_aabb.size.z;
+	Vector3 pos = xform( p_aabb.pos );
+//could be even further optimized
+	AABB new_aabb;
+	new_aabb.pos=pos;
+	new_aabb.expand_to( pos+x );
+	new_aabb.expand_to( pos+y );
+	new_aabb.expand_to( pos+z );
+	new_aabb.expand_to( pos+x+y );
+	new_aabb.expand_to( pos+x+z );
+	new_aabb.expand_to( pos+y+z );
+	new_aabb.expand_to( pos+x+y+z );
+	return new_aabb;
+#else
+
+
+	Vector3 vertices[8]={
+			Vector3(p_aabb.pos.x+p_aabb.size.x,	p_aabb.pos.y+p_aabb.size.y,	p_aabb.pos.z+p_aabb.size.z),
+			Vector3(p_aabb.pos.x+p_aabb.size.x,	p_aabb.pos.y+p_aabb.size.y,	p_aabb.pos.z),
+			Vector3(p_aabb.pos.x+p_aabb.size.x,	p_aabb.pos.y,		p_aabb.pos.z+p_aabb.size.z),
+			Vector3(p_aabb.pos.x+p_aabb.size.x,	p_aabb.pos.y,		p_aabb.pos.z),
+			Vector3(p_aabb.pos.x,	p_aabb.pos.y+p_aabb.size.y,	p_aabb.pos.z+p_aabb.size.z),
+			Vector3(p_aabb.pos.x,	p_aabb.pos.y+p_aabb.size.y,	p_aabb.pos.z),
+			Vector3(p_aabb.pos.x,	p_aabb.pos.y,		p_aabb.pos.z+p_aabb.size.z),
+			Vector3(p_aabb.pos.x,	p_aabb.pos.y,		p_aabb.pos.z)
+	};
+	
+	
+	AABB ret;
+	
+	ret.pos=xform(vertices[0]);
+	
+	for (int i=1;i<8;i++) {
+	
+		ret.expand_to( xform(vertices[i]) );
+	}
+	
+	return ret;
+#endif
+
+}
+_FORCE_INLINE_ AABB Transform::xform_inv(const AABB& p_aabb) const {
+
+	/* define vertices */
+	Vector3 vertices[8]={
+			Vector3(p_aabb.pos.x+p_aabb.size.x,	p_aabb.pos.y+p_aabb.size.y,	p_aabb.pos.z+p_aabb.size.z),
+			Vector3(p_aabb.pos.x+p_aabb.size.x,	p_aabb.pos.y+p_aabb.size.y,	p_aabb.pos.z),
+			Vector3(p_aabb.pos.x+p_aabb.size.x,	p_aabb.pos.y,		p_aabb.pos.z+p_aabb.size.z),
+			Vector3(p_aabb.pos.x+p_aabb.size.x,	p_aabb.pos.y,		p_aabb.pos.z),
+			Vector3(p_aabb.pos.x,	p_aabb.pos.y+p_aabb.size.y,	p_aabb.pos.z+p_aabb.size.z),
+			Vector3(p_aabb.pos.x,	p_aabb.pos.y+p_aabb.size.y,	p_aabb.pos.z),
+			Vector3(p_aabb.pos.x,	p_aabb.pos.y,		p_aabb.pos.z+p_aabb.size.z),
+			Vector3(p_aabb.pos.x,	p_aabb.pos.y,		p_aabb.pos.z)
+	};
+	
+	
+	AABB ret;
+	
+	ret.pos=xform_inv(vertices[0]);
+	
+	for (int i=1;i<8;i++) {
+	
+		ret.expand_to( xform_inv(vertices[i]) );
+	}
+	
+	return ret;
+
+}
+
+#ifdef OPTIMIZED_TRANSFORM_IMPL_OVERRIDE
+
+#else
+
+struct OptimizedTransform {
+
+	Transform transform;
+
+	_FORCE_INLINE_ void invert() {transform.invert(); }
+	_FORCE_INLINE_ void affine_invert() {transform.affine_invert(); }
+	_FORCE_INLINE_ Vector3 xform(const Vector3& p_vec) const { return transform.xform(p_vec); };
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3& p_vec) const { return transform.xform_inv(p_vec); };
+	_FORCE_INLINE_ OptimizedTransform operator*(const OptimizedTransform& p_ot ) const { return OptimizedTransform( transform * p_ot.transform ) ; }
+	_FORCE_INLINE_ Transform get_transform() const { return transform; }
+	_FORCE_INLINE_ void set_transform(const Transform& p_transform)  { transform=p_transform; }
+
+	OptimizedTransform(const Transform& p_transform) {
+		transform=p_transform;
+	}
+};
+
+#endif
+
+#endif
diff --git a/core/math/triangle_mesh.cpp b/core/math/triangle_mesh.cpp
new file mode 100644
index 0000000000..111ceca185
--- /dev/null
+++ b/core/math/triangle_mesh.cpp
@@ -0,0 +1,568 @@
+/*************************************************************************/
+/*  triangle_mesh.cpp                                                    */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "triangle_mesh.h"

+#include "sort.h"

+

+

+

+int TriangleMesh::_create_bvh(BVH*p_bvh,BVH** p_bb,int p_from,int p_size,int p_depth,int&max_depth,int&max_alloc) {

+

+

+	if (p_depth>max_depth) {

+		max_depth=p_depth;

+	}

+

+	if (p_size==1) {

+

+

+		return p_bb[p_from]-p_bvh;

+	} else if (p_size==0) {

+

+		return -1;

+	}

+

+

+	AABB aabb;

+	aabb=p_bb[p_from]->aabb;

+	for(int i=1;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
new file mode 100644
index 0000000000..ab0cb713b1
--- /dev/null
+++ b/core/math/triangle_mesh.h
@@ -0,0 +1,99 @@
+/*************************************************************************/
+/*  triangle_mesh.h                                                      */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef TRIANGLE_MESH_H

+#define TRIANGLE_MESH_H

+

+#include "reference.h"

+#include "face3.h"

+class TriangleMesh : public Reference {

+

+	OBJ_TYPE( TriangleMesh, Reference);

+

+	struct Triangle {

+

+		Vector3 normal;

+		int indices[3];

+	};

+

+	DVector<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
new file mode 100644
index 0000000000..4a870def4b
--- /dev/null
+++ b/core/math/triangulate.cpp
@@ -0,0 +1,168 @@
+/*************************************************************************/
+/*  triangulate.cpp                                                      */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "triangulate.h"
+
+float Triangulate::get_area(const Vector<Vector2> &contour)
+{
+
+  int n = contour.size();
+  const Vector2 *c=&contour[0];
+
+  float A=0.0f;
+
+  for(int p=n-1,q=0; q<n; p=q++)
+  {
+    A+= c[p].cross(c[q]);
+  }
+  return A*0.5f;
+}
+
+   /*
+     is_inside_triangle decides if a point P is Inside of the triangle
+     defined by A, B, C.
+   */
+
+bool Triangulate::is_inside_triangle(float Ax, float Ay,
+		      float Bx, float By,
+		      float Cx, float Cy,
+		      float Px, float Py)
+
+{
+  float ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
+  float cCROSSap, bCROSScp, aCROSSbp;
+
+  ax = Cx - Bx;  ay = Cy - By;
+  bx = Ax - Cx;  by = Ay - Cy;
+  cx = Bx - Ax;  cy = By - Ay;
+  apx= Px - Ax;  apy= Py - Ay;
+  bpx= Px - Bx;  bpy= Py - By;
+  cpx= Px - Cx;  cpy= Py - Cy;
+
+  aCROSSbp = ax*bpy - ay*bpx;
+  cCROSSap = cx*apy - cy*apx;
+  bCROSScp = bx*cpy - by*cpx;
+
+  return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
+};
+
+bool Triangulate::snip(const Vector<Vector2> &p_contour,int u,int v,int w,int n,int *V)
+{
+  int p;
+  float Ax, Ay, Bx, By, Cx, Cy, Px, Py;
+  const Vector2 *contour=&p_contour[0];
+
+  Ax = contour[V[u]].x;
+  Ay = contour[V[u]].y;
+
+  Bx = contour[V[v]].x;
+  By = contour[V[v]].y;
+
+  Cx = contour[V[w]].x;
+  Cy = contour[V[w]].y;
+
+  if ( CMP_EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false;
+
+  for (p=0;p<n;p++)
+  {
+    if( (p == u) || (p == v) || (p == w) ) continue;
+    Px = contour[V[p]].x;
+    Py = contour[V[p]].y;
+    if (is_inside_triangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py)) return false;
+  }
+
+  return true;
+}
+
+bool Triangulate::triangulate(const Vector<Vector2> &contour,Vector<int> &result)
+{
+  /* allocate and initialize list of Vertices in polygon */
+
+  int n = contour.size();
+  if ( n < 3 ) return false;
+
+
+
+  int *V = (int*)alloca(sizeof(int)*n);
+
+  /* we want a counter-clockwise polygon in V */
+
+  if ( 0.0f < get_area(contour) )
+    for (int v=0; v<n; v++) V[v] = v;
+  else
+    for(int v=0; v<n; v++) V[v] = (n-1)-v;
+
+  int nv = n;
+
+  /*  remove nv-2 Vertices, creating 1 triangle every time */
+  int count = 2*nv;   /* error detection */
+
+  for(int m=0, v=nv-1; nv>2; )
+  {
+    /* if we loop, it is probably a non-simple polygon */
+    if (0 >= (count--))
+    {
+      //** Triangulate: ERROR - probable bad polygon!
+      return false;
+    }
+
+    /* three consecutive vertices in current polygon, <u,v,w> */
+    int u = v  ; if (nv <= u) u = 0;     /* previous */
+    v = u+1; if (nv <= v) v = 0;     /* new v    */
+    int w = v+1; if (nv <= w) w = 0;     /* next     */
+
+    if ( snip(contour,u,v,w,nv,V) )
+    {
+      int a,b,c,s,t;
+
+      /* true names of the vertices */
+      a = V[u]; b = V[v]; c = V[w];
+
+      /* output Triangle */
+      /*
+      result.push_back( contour[a] );
+      result.push_back( contour[b] );
+      result.push_back( contour[c] );
+*/
+
+      result.push_back( a );
+      result.push_back( b );
+      result.push_back( c );
+
+      m++;
+
+      /* remove v from remaining polygon */
+      for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; nv--;
+
+      /* resest error detection counter */
+      count = 2*nv;
+    }
+  }
+
+  return true;
+}
diff --git a/core/math/triangulate.h b/core/math/triangulate.h
new file mode 100644
index 0000000000..b20b37bd44
--- /dev/null
+++ b/core/math/triangulate.h
@@ -0,0 +1,65 @@
+/*************************************************************************/
+/*  triangulate.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef TRIANGULATE_H
+#define TRIANGULATE_H
+
+#include "math_2d.h"
+
+
+/*
+http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml
+*/
+
+class Triangulate
+{
+public:
+
+	// triangulate a contour/polygon, places results in STL vector
+	// as series of triangles.
+	static bool triangulate(const Vector< Vector2 > &contour, Vector<int> &result);
+
+	// compute area of a contour/polygon
+	static float get_area(const Vector< Vector2 > &contour);
+
+	// decide if point Px/Py is inside triangle defined by
+	// (Ax,Ay) (Bx,By) (Cx,Cy)
+	static bool is_inside_triangle(float Ax, float Ay,
+		      float Bx, float By,
+		      float Cx, float Cy,
+		      float Px, float Py);
+
+
+private:
+	static bool snip(const Vector<Vector2> &p_contour,int u,int v,int w,int n,int *V);
+
+};
+
+
+
+#endif
diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp
new file mode 100644
index 0000000000..cf6fd9242e
--- /dev/null
+++ b/core/math/vector3.cpp
@@ -0,0 +1,201 @@
+/*************************************************************************/
+/*  vector3.cpp                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "vector3.h"
+
+
+
+void Vector3::rotate(const Vector3& p_axis,float p_phi) {
+
+	Vector3 axis1 = cross(p_axis);
+	float l = axis1.length();
+	if (l==0)
+		return;
+	axis1/=l;
+	Vector3 axis2 = axis1.cross(p_axis).normalized();
+
+	float _x = axis1.dot(*this);
+	float _y = axis2.dot(*this);
+
+	float ang = Math::atan2(_x,_y);
+
+	ang+=p_phi;
+
+	*this=((axis1 * Math::cos(ang)) + (axis2 * Math::sin(ang))) * length();
+
+}
+
+Vector3 Vector3::rotated(const Vector3& p_axis,float p_phi) const {
+
+	Vector3 r = *this;
+	r.rotate(p_axis,p_phi);
+	return r;
+}
+
+void Vector3::set_axis(int p_axis,real_t p_value) {
+	ERR_FAIL_INDEX(p_axis,3);
+	coord[p_axis]=p_value;
+
+}
+real_t Vector3::get_axis(int p_axis) const {
+
+        ERR_FAIL_INDEX_V(p_axis,3,0);
+        return operator[](p_axis);
+}
+
+int Vector3::min_axis() const {
+
+        return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2);
+}
+int Vector3::max_axis() const {
+
+	return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0);
+}
+
+
+void Vector3::snap(float p_val) {
+
+	x+=p_val/2.0;
+	x-=Math::fmod(x,p_val);
+	y+=p_val/2.0;
+	y-=Math::fmod(y,p_val);
+	z+=p_val/2.0;
+	z-=Math::fmod(z,p_val);
+
+}
+Vector3 Vector3::snapped(float p_val) const {
+
+        Vector3 v=*this;
+        v.snap(p_val);
+        return v;
+}
+
+
+Vector3 Vector3::cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const {
+
+	Vector3 p0=p_pre_a;
+	Vector3 p1=*this;
+	Vector3 p2=p_b;
+	Vector3 p3=p_post_b;
+
+	{
+		//normalize
+
+		float ab = p0.distance_to(p1);
+		float bc = p1.distance_to(p2);
+		float cd = p2.distance_to(p3);
+
+		if (ab>0)
+			p0 = p1+(p0-p1)*(bc/ab);
+		if (cd>0)
+			p3 = p2+(p3-p2)*(bc/cd);
+	}
+
+
+	float t = p_t;
+	float t2 = t * t;
+	float t3 = t2 * t;
+
+	Vector3 out;
+	out = 0.5f * ( ( p1 * 2.0f) +
+	( -p0 + p2 ) * t +
+	( 2.0f * p0 - 5.0f * p1 + 4 * p2 - p3 ) * t2 +
+	( -p0 + 3.0f * p1 - 3.0f * p2 + p3 ) * t3 );
+	return out;
+
+}
+
+Vector3 Vector3::cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const {
+
+	Vector3 p0=p_pre_a;
+	Vector3 p1=*this;
+	Vector3 p2=p_b;
+	Vector3 p3=p_post_b;
+
+	float t = p_t;
+	float t2 = t * t;
+	float t3 = t2 * t;
+
+	Vector3 out;
+	out = 0.5f * ( ( p1 * 2.0f) +
+	( -p0 + p2 ) * t +
+	( 2.0f * p0 - 5.0f * p1 + 4 * p2 - p3 ) * t2 +
+	( -p0 + 3.0f * p1 - 3.0f * p2 + p3 ) * t3 );
+	return out;
+
+}
+
+#if 0
+Vector3 Vector3::cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const {
+
+	Vector3 p0=p_pre_a;
+	Vector3 p1=*this;
+	Vector3 p2=p_b;
+	Vector3 p3=p_post_b;
+
+	if (true) {
+
+		float ab = p0.distance_to(p1);
+		float bc = p1.distance_to(p2);
+		float cd = p2.distance_to(p3);
+
+		//if (ab>bc) {
+		if (ab>0)
+			p0 = p1+(p0-p1)*(bc/ab);
+		//}
+
+		//if (cd>bc) {
+		if (cd>0)
+			p3 = p2+(p3-p2)*(bc/cd);
+		//}
+	}
+
+	float t = p_t;
+	float t2 = t * t;
+	float t3 = t2 * t;
+
+	Vector3 out;
+	out.x = 0.5f * ( ( 2.0f * p1.x ) +
+	( -p0.x + p2.x ) * t +
+	( 2.0f * p0.x - 5.0f * p1.x + 4 * p2.x - p3.x ) * t2 +
+	( -p0.x + 3.0f * p1.x - 3.0f * p2.x + p3.x ) * t3 );
+	out.y = 0.5f * ( ( 2.0f * p1.y ) +
+	( -p0.y + p2.y ) * t +
+	( 2.0f * p0.y - 5.0f * p1.y + 4 * p2.y - p3.y ) * t2 +
+	( -p0.y + 3.0f * p1.y - 3.0f * p2.y + p3.y ) * t3 );
+	out.z = 0.5f * ( ( 2.0f * p1.z ) +
+	( -p0.z + p2.z ) * t +
+	( 2.0f * p0.z - 5.0f * p1.z + 4 * p2.z - p3.z ) * t2 +
+	( -p0.z + 3.0f * p1.z - 3.0f * p2.z + p3.z ) * t3 );
+	return out;
+}
+# endif
+Vector3::operator String() const {
+
+	return (rtos(x)+", "+rtos(y)+", "+rtos(z));
+}
diff --git a/core/math/vector3.h b/core/math/vector3.h
new file mode 100644
index 0000000000..959f7cd0a8
--- /dev/null
+++ b/core/math/vector3.h
@@ -0,0 +1,373 @@
+/*************************************************************************/
+/*  vector3.h                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef VECTOR3_H
+#define VECTOR3_H
+
+#include "typedefs.h"
+#include "math_defs.h"
+#include "math_funcs.h"
+#include "ustring.h"
+
+
+struct Vector3 {
+
+	enum Axis {
+		AXIS_X,
+		AXIS_Y,
+		AXIS_Z,	
+	};
+
+	union {
+	
+#ifdef USE_QUAD_VECTORS
+
+		struct {
+			real_t x;
+			real_t y;
+			real_t z;
+			real_t _unused;
+		};		
+		real_t coord[4];
+#else
+
+		struct {
+			real_t x;
+			real_t y;
+			real_t z;
+		};
+		
+		real_t coord[3];
+#endif
+	};
+
+	_FORCE_INLINE_ const real_t& operator[](int p_axis) const {
+	
+		return coord[p_axis];
+	}
+
+	_FORCE_INLINE_ real_t& operator[](int p_axis) {
+	
+		return coord[p_axis];
+	}
+
+        void set_axis(int p_axis,real_t p_value);
+        real_t get_axis(int p_axis) const;
+
+        int min_axis() const;
+        int max_axis() const;
+
+        _FORCE_INLINE_ real_t length() const;
+        _FORCE_INLINE_ real_t length_squared() const;
+        
+        _FORCE_INLINE_ void normalize();
+        _FORCE_INLINE_ Vector3 normalized() const;
+        _FORCE_INLINE_ Vector3 inverse() const;
+
+	_FORCE_INLINE_ void zero();
+
+        void snap(float p_val);
+        Vector3 snapped(float p_val) const;
+
+	void rotate(const Vector3& p_axis,float p_phi);
+	Vector3 rotated(const Vector3& p_axis,float p_phi) const;
+
+        /* Static Methods between 2 vector3s */
+
+        _FORCE_INLINE_ Vector3 linear_interpolate(const Vector3& p_b,float p_t) const;
+	Vector3 cubic_interpolate(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const;
+	Vector3 cubic_interpolaten(const Vector3& p_b,const Vector3& p_pre_a, const Vector3& p_post_b,float p_t) const;
+
+        _FORCE_INLINE_ Vector3 cross(const Vector3& p_b) const;
+        _FORCE_INLINE_ real_t dot(const Vector3& p_b) const;
+
+        _FORCE_INLINE_ Vector3 abs() const;
+
+        _FORCE_INLINE_ real_t distance_to(const Vector3& p_b) const;
+        _FORCE_INLINE_ real_t distance_squared_to(const Vector3& p_b) const;
+
+        /* Operators */
+
+        _FORCE_INLINE_ Vector3& operator+=(const Vector3& p_v);
+        _FORCE_INLINE_ Vector3 operator+(const Vector3& p_v) const;
+        _FORCE_INLINE_ Vector3& operator-=(const Vector3& p_v);
+        _FORCE_INLINE_ Vector3 operator-(const Vector3& p_v) const;
+        _FORCE_INLINE_ Vector3& operator*=(const Vector3& p_v);
+        _FORCE_INLINE_ Vector3 operator*(const Vector3& p_v) const;
+        _FORCE_INLINE_ Vector3& operator/=(const Vector3& p_v);
+        _FORCE_INLINE_ Vector3 operator/(const Vector3& p_v) const;
+
+
+        _FORCE_INLINE_ Vector3& operator*=(real_t p_scalar);
+        _FORCE_INLINE_ Vector3 operator*(real_t p_scalar) const;
+        _FORCE_INLINE_ Vector3& operator/=(real_t p_scalar);
+        _FORCE_INLINE_ Vector3 operator/(real_t p_scalar) const;
+
+        _FORCE_INLINE_ Vector3 operator-() const;
+
+        _FORCE_INLINE_ bool operator==(const Vector3& p_v) const;
+        _FORCE_INLINE_ bool operator!=(const Vector3& p_v) const;
+        _FORCE_INLINE_ bool operator<(const Vector3& p_v) const;
+	_FORCE_INLINE_ bool operator<=(const Vector3& p_v) const;
+
+	operator String() const;
+
+       _FORCE_INLINE_ Vector3() { x=y=z=0; }
+        _FORCE_INLINE_ Vector3(real_t p_x,real_t p_y,real_t p_z) { x=p_x; y=p_y; z=p_z; }
+
+};
+
+#ifdef VECTOR3_IMPL_OVERRIDE
+
+#include "vector3_inline.h"
+
+#else
+
+Vector3 Vector3::cross(const Vector3& p_b) const {
+
+	Vector3 ret (
+		(y * p_b.z) - (z * p_b.y),
+		(z * p_b.x) - (x * p_b.z),
+		(x * p_b.y) - (y * p_b.x)
+	);
+
+        return ret;
+}
+real_t Vector3::dot(const Vector3& p_b) const {
+
+        return x*p_b.x + y*p_b.y + z*p_b.z;
+}
+
+Vector3 Vector3::abs() const {
+
+	return Vector3( Math::abs(x), Math::abs(y), Math::abs(z) );
+}	
+
+Vector3 Vector3::linear_interpolate(const Vector3& p_b,float p_t) const {
+
+	return Vector3(
+		x+(p_t * (p_b.x-x)),
+		y+(p_t * (p_b.y-y)),
+		z+(p_t * (p_b.z-z))
+	);
+
+}
+
+
+
+real_t Vector3::distance_to(const Vector3& p_b) const {
+	return (p_b-*this).length();
+}
+real_t Vector3::distance_squared_to(const Vector3& p_b) const {
+
+        return (p_b-*this).length_squared();
+}
+
+/* Operators */
+
+Vector3& Vector3::operator+=(const Vector3& p_v) {
+
+        x+=p_v.x;
+        y+=p_v.y;
+        z+=p_v.z;
+        return *this;
+}
+Vector3 Vector3::operator+(const Vector3& p_v) const {
+
+        return Vector3(x+p_v.x, y+p_v.y, z+ p_v.z);
+}
+
+Vector3& Vector3::operator-=(const Vector3& p_v) {
+
+        x-=p_v.x;
+        y-=p_v.y;
+        z-=p_v.z;
+        return *this;
+}
+Vector3 Vector3::operator-(const Vector3& p_v) const {
+
+        return Vector3(x-p_v.x, y-p_v.y, z- p_v.z);
+}
+
+
+
+Vector3& Vector3::operator*=(const Vector3& p_v) {
+
+        x*=p_v.x;
+        y*=p_v.y;
+        z*=p_v.z;
+        return *this;
+}
+Vector3 Vector3::operator*(const Vector3& p_v) const {
+
+        return Vector3(x*p_v.x, y*p_v.y, z* p_v.z);
+}
+
+Vector3& Vector3::operator/=(const Vector3& p_v) {
+
+        x/=p_v.x;
+        y/=p_v.y;
+        z/=p_v.z;
+        return *this;
+}
+Vector3 Vector3::operator/(const Vector3& p_v) const {
+
+        return Vector3(x/p_v.x, y/p_v.y, z/ p_v.z);
+}
+
+Vector3& Vector3::operator*=(real_t p_scalar) {
+        x*=p_scalar;
+        y*=p_scalar;
+        z*=p_scalar;
+        return *this;
+
+}
+
+_FORCE_INLINE_ Vector3 operator*(real_t p_scalar, const Vector3& p_vec) {
+	return p_vec * p_scalar;
+}
+
+Vector3 Vector3::operator*(real_t p_scalar) const {
+
+        return Vector3( x*p_scalar, y*p_scalar, z*p_scalar);
+}
+
+Vector3& Vector3::operator/=(real_t p_scalar) {
+        x/=p_scalar;
+        y/=p_scalar;
+        z/=p_scalar;
+        return *this;
+
+}
+
+Vector3 Vector3::operator/(real_t p_scalar) const {
+
+        return Vector3( x/p_scalar, y/p_scalar, z/p_scalar);
+}
+
+Vector3 Vector3::operator-() const {
+
+        return Vector3( -x, -y, -z );
+}
+
+
+bool Vector3::operator==(const Vector3& p_v) const {
+
+        return (x==p_v.x && y==p_v.y && z==p_v.z);
+}
+
+bool Vector3::operator!=(const Vector3& p_v) const {
+
+        return (x!=p_v.x || y!=p_v.y || z!=p_v.z);
+}
+
+bool Vector3::operator<(const Vector3& p_v) const {
+
+	if (x==p_v.x) {
+		if (y==p_v.y)
+			return z<p_v.z;
+		else
+			return y<p_v.y;
+	} else
+		return x<p_v.x;
+	
+}
+
+bool Vector3::operator<=(const Vector3& p_v) const {
+
+	if (x==p_v.x) {
+		if (y==p_v.y)
+			return z<=p_v.z;
+		else
+			return y<p_v.y;
+	} else
+		return x<p_v.x;
+
+}
+
+_FORCE_INLINE_ Vector3 vec3_cross(const Vector3& p_a, const Vector3& p_b) {
+
+	return p_a.cross(p_b);
+}
+
+_FORCE_INLINE_ real_t vec3_dot(const Vector3& p_a, const Vector3& p_b) {
+
+	return p_a.dot(p_b);
+}
+
+real_t Vector3::length() const {
+
+	real_t x2=x*x;
+	real_t y2=y*y;
+	real_t z2=z*z;
+
+	return Math::sqrt(x2+y2+z2);
+}
+real_t Vector3::length_squared() const {
+
+	real_t x2=x*x;
+	real_t y2=y*y;
+	real_t z2=z*z;
+
+	return x2+y2+z2;
+
+}
+
+void Vector3::normalize() {
+
+        real_t l=length();
+        if (l==0) {
+
+		x=y=z=0;
+        } else {
+
+		x/=l;
+		y/=l;
+		z/=l;
+        }
+}
+Vector3 Vector3::normalized() const {
+
+        Vector3 v=*this;
+        v.normalize();
+        return v;
+}
+
+Vector3 Vector3::inverse() const {
+
+	return Vector3( 1.0/x, 1.0/y, 1.0/z );
+}
+
+void Vector3::zero() {
+
+	x=y=z=0;
+}
+
+#endif
+
+#endif // VECTOR3_H