GODOT IS OPEN SOURCE

72 files changed, 21829 insertions, 0 deletions

diff --git a/servers/physics/SCsub b/servers/physics/SCsub
new file mode 100644
index 000000000..16fe3a59a
--- /dev/null
+++ b/servers/physics/SCsub
@@ -0,0 +1,7 @@
+Import('env')
+
+env.add_source_files(env.servers_sources,"*.cpp")
+
+Export('env')
+
+
diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp
new file mode 100644
index 000000000..4a303d3fd
--- /dev/null
+++ b/servers/physics/area_pair_sw.cpp
@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  area_pair_sw.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 "area_pair_sw.h"
+#include "collision_solver_sw.h"
+
+
+bool AreaPairSW::setup(float p_step) {
+
+	bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),NULL,this);
+
+	if (result!=colliding) {
+
+		if (result) {
+
+			if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+				body->add_area(area);
+			if (area->has_monitor_callback())
+				area->add_body_to_query(body,body_shape,area_shape);
+
+		} else {
+
+			if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+				body->remove_area(area);
+			if (area->has_monitor_callback())
+				area->remove_body_from_query(body,body_shape,area_shape);
+
+		}
+
+		colliding=result;
+
+	}
+
+	return false; //never do any post solving
+}
+
+void AreaPairSW::solve(float p_step) {
+
+
+}
+
+
+AreaPairSW::AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape) {
+
+
+	body=p_body;
+	area=p_area;
+	body_shape=p_body_shape;
+	area_shape=p_area_shape;
+	colliding=false;
+	body->add_constraint(this,0);
+	area->add_constraint(this);
+
+}
+
+AreaPairSW::~AreaPairSW() {
+
+	if (colliding) {
+
+		if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+			body->remove_area(area);
+		if (area->has_monitor_callback())
+			area->remove_body_from_query(body,body_shape,area_shape);
+
+
+	}
+	body->remove_constraint(this);
+	area->remove_constraint(this);
+}
diff --git a/servers/physics/area_pair_sw.h b/servers/physics/area_pair_sw.h
new file mode 100644
index 000000000..e5e2b5cf5
--- /dev/null
+++ b/servers/physics/area_pair_sw.h
@@ -0,0 +1,53 @@
+/*************************************************************************/
+/*  area_pair_sw.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 AREA_PAIR_SW_H
+#define AREA_PAIR_SW_H
+
+#include "constraint_sw.h"
+#include "body_sw.h"
+#include "area_sw.h"
+
+class AreaPairSW : public ConstraintSW {
+
+	BodySW *body;
+	AreaSW *area;
+	int body_shape;
+	int area_shape;
+	bool colliding;
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape);
+	~AreaPairSW();
+};
+
+#endif // AREA_PAIR__SW_H
+
diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp
new file mode 100644
index 000000000..33962d993
--- /dev/null
+++ b/servers/physics/area_sw.cpp
@@ -0,0 +1,192 @@
+/*************************************************************************/
+/*  area_sw.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 "area_sw.h"
+#include "space_sw.h"
+#include "body_sw.h"
+
+AreaSW::BodyKey::BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
+
+void AreaSW::_shapes_changed() {
+
+
+}
+
+void AreaSW::set_transform(const Transform& p_transform) {
+
+	if (!moved_list.in_list() && get_space())
+		get_space()->area_add_to_moved_list(&moved_list);
+
+	_set_transform(p_transform);
+}
+
+void AreaSW::set_space(SpaceSW *p_space) {
+
+	if (get_space()) {
+		if (monitor_query_list.in_list())
+			get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+		if (moved_list.in_list())
+			get_space()->area_remove_from_moved_list(&moved_list);
+
+	}
+
+	monitored_bodies.clear();
+
+	_set_space(p_space);
+}
+
+
+void AreaSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
+
+
+	if (p_id==monitor_callback_id) {
+		monitor_callback_method=p_method;
+		return;
+	}
+
+	_unregister_shapes();
+
+	monitor_callback_id=p_id;
+	monitor_callback_method=p_method;
+
+	monitored_bodies.clear();
+
+	_shape_changed();
+
+}
+
+
+
+void AreaSW::set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) {
+	bool do_override=p_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override==(space_override_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED))
+		return;
+	_unregister_shapes();
+	space_override_mode=p_mode;
+	_shape_changed();
+}
+
+void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value) {
+
+	switch(p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:  point_attenuation=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_DENSITY: density=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_PRIORITY: priority=p_value; ; break;
+	}
+
+
+}
+
+Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const {
+
+
+	switch(p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY: return gravity;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector;
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return  point_attenuation;
+		case PhysicsServer::AREA_PARAM_DENSITY: return density;
+		case PhysicsServer::AREA_PARAM_PRIORITY: return priority;
+	}
+
+	return Variant();
+}
+
+
+void AreaSW::_queue_monitor_update() {
+
+	ERR_FAIL_COND(!get_space());
+
+	if (!monitor_query_list.in_list())
+		get_space()->area_add_to_monitor_query_list(&monitor_query_list);
+
+
+}
+
+void AreaSW::call_queries() {
+
+	if (monitor_callback_id && !monitored_bodies.empty()) {
+
+		Variant res[5];
+		Variant *resptr[5];
+		for(int i=0;i<5;i++)
+			resptr[i]=&res[i];
+
+		Object *obj = ObjectDB::get_instance(monitor_callback_id);
+		if (!obj) {
+			monitored_bodies.clear();
+			monitor_callback_id=0;
+			return;
+		}
+
+
+
+		for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) {
+
+			if (E->get().state==0)
+				continue; //nothing happened
+
+			res[0]=E->get().state>0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED;
+			res[1]=E->key().rid;
+			res[2]=E->key().instance_id;
+			res[3]=E->key().body_shape;
+			res[4]=E->key().area_shape;
+
+			Variant::CallError ce;
+			obj->call(monitor_callback_method,(const Variant**)resptr,5,ce);
+		}
+	}
+
+	monitored_bodies.clear();
+
+	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+
+}
+
+AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this),  moved_list(this) {
+
+	_set_static(true); //areas are never active
+	space_override_mode=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED;
+	gravity=9.80665;
+	gravity_vector=Vector3(0,-1,0);
+	gravity_is_point=false;
+	point_attenuation=1;
+	density=0.1;
+	priority=0;
+
+
+}
+
+AreaSW::~AreaSW() {
+
+
+}
+
diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h
new file mode 100644
index 000000000..3e39dc3bb
--- /dev/null
+++ b/servers/physics/area_sw.h
@@ -0,0 +1,172 @@
+/*************************************************************************/
+/*  area_sw.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 AREA_SW_H
+#define AREA_SW_H
+
+#include "servers/physics_server.h"
+#include "collision_object_sw.h"
+#include "self_list.h"
+//#include "servers/physics/query_sw.h"
+
+class SpaceSW;
+class BodySW;
+class ConstraintSW;
+
+class AreaSW : public CollisionObjectSW{
+
+
+	PhysicsServer::AreaSpaceOverrideMode space_override_mode;
+	float gravity;
+	Vector3 gravity_vector;
+	bool gravity_is_point;
+	float point_attenuation;
+	float density;
+	int priority;
+
+	ObjectID monitor_callback_id;
+	StringName monitor_callback_method;
+
+	SelfList<AreaSW> monitor_query_list;
+	SelfList<AreaSW> moved_list;
+
+	struct BodyKey {
+
+		RID rid;
+		ObjectID instance_id;
+		uint32_t body_shape;
+		uint32_t area_shape;
+
+		_FORCE_INLINE_ bool operator<( const BodyKey& p_key) const {
+
+			if (rid==p_key.rid) {
+
+				if (body_shape==p_key.body_shape) {
+
+					return area_shape < p_key.area_shape;
+				} else
+					return body_shape < p_key.area_shape;
+			} else
+				return rid < p_key.rid;
+
+		}
+
+		_FORCE_INLINE_ BodyKey() {}
+		BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	};
+
+	struct BodyState {
+
+		int state;
+		_FORCE_INLINE_ void inc() { state++; }
+		_FORCE_INLINE_ void dec() { state--; }
+		_FORCE_INLINE_ BodyState() { state=0; }
+	};
+
+	Map<BodyKey,BodyState> monitored_bodies;
+
+	//virtual void shape_changed_notify(ShapeSW *p_shape);
+	//virtual void shape_deleted_notify(ShapeSW *p_shape);
+
+	Set<ConstraintSW*> constraints;
+
+
+	virtual void _shapes_changed();
+	void _queue_monitor_update();
+
+public:
+
+	//_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; }
+	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
+
+	void set_monitor_callback(ObjectID p_id, const StringName& p_method);
+	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; }
+
+	_FORCE_INLINE_ void add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+
+	void set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value);
+	Variant get_param(PhysicsServer::AreaParameter p_param) const;
+
+	void set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode);
+	PhysicsServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; }
+
+	_FORCE_INLINE_ void set_gravity(float p_gravity) { gravity=p_gravity; }
+	_FORCE_INLINE_ float get_gravity() const { return gravity; }
+
+	_FORCE_INLINE_ void set_gravity_vector(const Vector3& p_gravity) { gravity_vector=p_gravity; }
+	_FORCE_INLINE_ Vector3 get_gravity_vector() const { return gravity_vector; }
+
+	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; }
+	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
+
+	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; }
+	_FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; }
+
+	_FORCE_INLINE_ void set_density(float p_density) { density=p_density; }
+	_FORCE_INLINE_ float get_density() const { return density; }
+
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; }
+	_FORCE_INLINE_ int get_priority() const { return priority; }
+
+	_FORCE_INLINE_ void add_constraint( ConstraintSW* p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; }
+
+	void set_transform(const Transform& p_transform);
+
+	void set_space(SpaceSW *p_space);
+
+
+	void call_queries();
+
+	AreaSW();
+	~AreaSW();
+};
+
+void AreaSW::add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+
+	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	monitored_bodies[bk].inc();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
+void AreaSW::remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+
+	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	monitored_bodies[bk].dec();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
+
+
+
+
+
+
+#endif // AREA__SW_H
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
new file mode 100644
index 000000000..06ae34098
--- /dev/null
+++ b/servers/physics/body_pair_sw.cpp
@@ -0,0 +1,442 @@
+/*************************************************************************/
+/*  body_pair_sw.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 "body_pair_sw.h"
+#include "collision_solver_sw.h"
+#include "space_sw.h"
+
+
+/*
+#define NO_ACCUMULATE_IMPULSES
+#define NO_SPLIT_IMPULSES
+
+#define NO_FRICTION
+*/
+
+#define NO_TANGENTIALS
+/* BODY PAIR */
+
+
+//#define ALLOWED_PENETRATION 0.01
+#define RELAXATION_TIMESTEPS 3
+#define MIN_VELOCITY 0.0001
+
+void BodyPairSW::_contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
+
+	BodyPairSW* pair  = (BodyPairSW*)p_userdata;
+	pair->contact_added_callback(p_point_A,p_point_B);
+
+}
+
+void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B) {
+
+	// check if we already have the contact
+
+	//Vector3 local_A = A->get_inv_transform().xform(p_point_A);
+	//Vector3 local_B = B->get_inv_transform().xform(p_point_B);
+
+	Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
+	Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B-offset_B);
+
+
+
+	int new_index = contact_count;
+
+	ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) );
+
+	Contact contact;
+
+	contact.acc_normal_impulse=0;
+	contact.acc_bias_impulse=0;
+	contact.acc_tangent_impulse=Vector3();
+	contact.local_A=local_A;
+	contact.local_B=local_B;
+	contact.normal=(p_point_A-p_point_B).normalized();
+
+
+
+	// attempt to determine if the contact will be reused
+	real_t contact_recycle_radius=space->get_contact_recycle_radius();
+
+	for (int i=0;i<contact_count;i++) {
+
+		Contact& c = contacts[i];
+		if (
+		c.local_A.distance_squared_to( local_A ) < (contact_recycle_radius*contact_recycle_radius) &&
+		c.local_B.distance_squared_to( local_B ) < (contact_recycle_radius*contact_recycle_radius) ) {
+
+				contact.acc_normal_impulse=c.acc_normal_impulse;
+				contact.acc_bias_impulse=c.acc_bias_impulse;
+				contact.acc_tangent_impulse=c.acc_tangent_impulse;
+				new_index=i;
+				break;
+		}
+	}
+
+	// figure out if the contact amount must be reduced to fit the new contact
+
+	if (new_index == MAX_CONTACTS) {
+
+		// remove the contact with the minimum depth
+
+		int least_deep=-1;
+		float min_depth=1e10;
+
+		for (int i=0;i<=contact_count;i++) {
+
+			Contact& c = (i==contact_count)?contact:contacts[i];
+			Vector3 global_A = A->get_transform().basis.xform(c.local_A);
+			Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B;
+
+			Vector3 axis = global_A - global_B;
+			float depth = axis.dot( c.normal );
+
+			if (depth<min_depth) {
+
+				min_depth=depth;
+				least_deep=i;
+			}
+		}
+
+		ERR_FAIL_COND(least_deep==-1);
+
+		if (least_deep < contact_count) { //replace the last deep contact by the new one
+
+			contacts[least_deep]=contact;
+		}
+
+		return;
+	}
+
+	contacts[new_index]=contact;
+
+	if (new_index==contact_count) {
+
+		contact_count++;
+	}
+
+}
+
+void BodyPairSW::validate_contacts() {
+
+	//make sure to erase contacts that are no longer valid
+
+	real_t contact_max_separation=space->get_contact_max_separation();
+	for (int i=0;i<contact_count;i++) {
+
+		Contact& c = contacts[i];
+
+		Vector3 global_A = A->get_transform().basis.xform(c.local_A);
+		Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B;
+		Vector3 axis = global_A - global_B;
+		float depth = axis.dot( c.normal );
+
+		if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
+			// contact no longer needed, remove
+
+
+			if ((i+1) < contact_count) {
+				// swap with the last one
+				SWAP( contacts[i], contacts[ contact_count-1 ] );
+
+			}
+
+			i--;
+			contact_count--;
+		}
+	}
+}
+
+bool BodyPairSW::setup(float p_step) {
+
+
+	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
+
+	validate_contacts();
+
+	Vector3 offset_A = A->get_transform().get_origin();
+	Transform xform_Au = Transform(A->get_transform().basis,Vector3());
+	Transform xform_A = xform_Au * A->get_shape_transform(shape_A);
+
+	Transform xform_Bu = B->get_transform();
+	xform_Bu.origin-=offset_A;
+	Transform xform_B = xform_Bu * B->get_shape_transform(shape_B);
+
+	ShapeSW *shape_A_ptr=A->get_shape(shape_A);
+	ShapeSW *shape_B_ptr=B->get_shape(shape_B);
+
+	bool collided = CollisionSolverSW::solve_static(shape_A_ptr,xform_A,shape_B_ptr,xform_B,_contact_added_callback,this,&sep_axis);
+	this->collided=collided;
+
+	if (!collided)
+		return false;
+
+
+	//cannot collide
+	if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE)) {
+		return false;
+	}
+
+	real_t max_penetration = space->get_contact_max_allowed_penetration();
+
+	float bias = 0.3f;
+
+	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
+
+		if (shape_A_ptr->get_custom_bias()==0)
+			bias=shape_B_ptr->get_custom_bias();
+		else if (shape_B_ptr->get_custom_bias()==0)
+			bias=shape_A_ptr->get_custom_bias();
+		else
+			bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5;
+	}
+
+
+	real_t inv_dt = 1.0/p_step;
+
+	for(int i=0;i<contact_count;i++) {
+
+		Contact &c = contacts[i];
+		c.active=false;
+
+		Vector3 global_A = xform_Au.xform(c.local_A);
+		Vector3 global_B = xform_Bu.xform(c.local_B);
+
+		real_t depth = c.normal.dot(global_A - global_B);
+
+		if (depth<=0) {
+			c.active=false;
+			continue;
+		}
+
+		c.active=true;
+
+		int gather_A = A->can_report_contacts();
+		int gather_B = B->can_report_contacts();
+
+		c.rA = global_A;
+		c.rB = global_B-offset_B;
+
+		// contact query reporting...
+#if 0
+		if (A->get_body_type() == PhysicsServer::BODY_CHARACTER)
+			static_cast<CharacterBodySW*>(A)->report_character_contact( global_A, global_B, B );
+		if (B->get_body_type() == PhysicsServer::BODY_CHARACTER)
+			static_cast<CharacterBodySW*>(B)->report_character_contact( global_B, global_A, A );
+		if (A->has_contact_query())
+			A->report_contact( global_A, global_B, B );
+		if (B->has_contact_query())
+			B->report_contact( global_B, global_A, A );
+#endif
+
+		if (A->can_report_contacts()) {
+			Vector3 crB = A->get_angular_velocity().cross( c.rA ) + A->get_linear_velocity();
+			A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB);
+		}
+
+		if (B->can_report_contacts()) {
+			Vector3 crA = A->get_angular_velocity().cross( c.rB ) + A->get_linear_velocity();
+			B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA);
+		}
+
+		c.active=true;
+
+		// Precompute normal mass, tangent mass, and bias.
+		Vector3 inertia_A = A->get_inv_inertia_tensor().xform( c.rA.cross( c.normal ) );
+		Vector3 inertia_B = B->get_inv_inertia_tensor().xform( c.rB.cross( c.normal ) );
+		real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
+		kNormal += c.normal.dot( inertia_A.cross(c.rA ) ) + c.normal.dot( inertia_B.cross( c.rB ));
+		c.mass_normal = 1.0f / kNormal;
+
+#if 1
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+
+#else
+		if (depth > max_penetration) {
+			c.bias = (depth - max_penetration) * (1.0/(p_step*(1.0/RELAXATION_TIMESTEPS)));
+		} else {
+			float approach = -0.1f * (depth - max_penetration) / (CMP_EPSILON + max_penetration);
+			approach = CLAMP( approach, CMP_EPSILON, 1.0 );
+			c.bias = approach * (depth - max_penetration) * (1.0/p_step);
+		}
+#endif
+		c.depth=depth;
+
+		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
+		A->apply_impulse( c.rA, -j_vec );
+		B->apply_impulse( c.rB, j_vec );
+		c.acc_bias_impulse=0;
+		Vector3 jb_vec = c.normal * c.acc_bias_impulse;
+		A->apply_bias_impulse( c.rA, -jb_vec );
+		B->apply_bias_impulse( c.rB, jb_vec );
+
+	}
+
+	return true;
+}
+
+void BodyPairSW::solve(float p_step) {
+
+	if (!collided)
+		return;
+
+
+	for(int i=0;i<contact_count;i++) {
+
+		Contact &c = contacts[i];
+		if (!c.active)
+			continue;
+
+		c.active=false; //try to deactivate, will activate itself if still needed
+
+		//bias impule
+
+		Vector3 crbA = A->get_biased_angular_velocity().cross( c.rA );
+		Vector3 crbB = B->get_biased_angular_velocity().cross( c.rB );
+		Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
+
+		real_t vbn = dbv.dot(c.normal);
+
+		if (Math::abs(-vbn+c.bias)>MIN_VELOCITY) {
+
+			real_t jbn = (-vbn + c.bias)*c.mass_normal;
+			real_t jbnOld = c.acc_bias_impulse;
+			c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
+
+			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
+
+
+			A->apply_bias_impulse(c.rA,-jb);
+			B->apply_bias_impulse(c.rB, jb);
+
+			c.active=true;
+		}
+
+
+		Vector3 crA = A->get_angular_velocity().cross( c.rA );
+		Vector3 crB = B->get_angular_velocity().cross( c.rB );
+		Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
+
+		//normal impule
+		real_t vn = dv.dot(c.normal);
+
+		if (Math::abs(vn)>MIN_VELOCITY) {
+
+			real_t bounce=0;
+			real_t jn = (-bounce -vn)*c.mass_normal;
+			real_t jnOld = c.acc_normal_impulse;
+			c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
+
+
+			Vector3 j =c.normal * (c.acc_normal_impulse - jnOld);
+
+
+			A->apply_impulse(c.rA,-j);
+			B->apply_impulse(c.rB, j);
+
+			c.active=true;
+		}
+
+		//friction impule
+
+		real_t friction = A->get_friction() * B->get_friction();
+
+		Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross( c.rA );
+		Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross( c.rB );
+
+		Vector3 dtv = lvB - lvA;
+		real_t tn = c.normal.dot(dtv);
+
+		// tangential velocity
+		Vector3 tv = dtv - c.normal * tn;
+		real_t tvl = tv.length();
+
+		if (tvl > MIN_VELOCITY) {
+
+			tv /= tvl;
+
+			Vector3 temp1 = A->get_inv_inertia_tensor().xform( c.rA.cross( tv ) );
+			Vector3 temp2 = B->get_inv_inertia_tensor().xform( c.rB.cross( tv ) );
+
+			real_t t = -tvl /
+				(A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
+
+			Vector3 jt = t * tv;
+
+
+			Vector3 jtOld = c.acc_tangent_impulse;
+			c.acc_tangent_impulse += jt;
+
+			real_t fi_len = c.acc_tangent_impulse.length();
+			real_t jtMax = c.acc_normal_impulse * friction;
+
+			if (fi_len > CMP_EPSILON && fi_len > jtMax) {
+
+				c.acc_tangent_impulse*=jtMax / fi_len;
+			}
+
+			jt = c.acc_tangent_impulse - jtOld;
+
+
+			A->apply_impulse( c.rA, -jt );
+			B->apply_impulse( c.rB, jt );
+
+			c.active=true;
+
+		}
+
+
+	}
+
+}
+
+
+
+
+
+BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B) : ConstraintSW(_arr,2) {
+
+	A=p_A;
+	B=p_B;
+	shape_A=p_shape_A;
+	shape_B=p_shape_B;
+	space=A->get_space();
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+	contact_count=0;
+	collided=false;
+
+}
+
+
+BodyPairSW::~BodyPairSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+
+}
diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h
new file mode 100644
index 000000000..ad66227b3
--- /dev/null
+++ b/servers/physics/body_pair_sw.h
@@ -0,0 +1,97 @@
+/*************************************************************************/
+/*  body_pair_sw.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 BODY_PAIR_SW_H
+#define BODY_PAIR_SW_H
+
+#include "body_sw.h"
+#include "constraint_sw.h"
+
+class BodyPairSW : public ConstraintSW {
+	enum {
+
+		MAX_CONTACTS=4
+	};
+
+	union {
+		struct {
+			BodySW *A;
+			BodySW *B;
+		};
+
+		BodySW *_arr[2];
+	};
+
+	int shape_A;
+	int shape_B;
+
+
+	struct Contact {
+
+		Vector3 position;
+		Vector3 normal;
+		Vector3 local_A, local_B;
+		real_t acc_normal_impulse;	// accumulated normal impulse (Pn)
+		Vector3 acc_tangent_impulse;	// accumulated tangent impulse (Pt)
+		real_t acc_bias_impulse;	// accumulated normal impulse for position bias (Pnb)
+		real_t mass_normal;
+		real_t bias;
+
+		real_t depth;
+		bool active;
+		Vector3 rA,rB;
+	};
+
+	Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
+
+	Vector3 sep_axis;
+	Contact contacts[MAX_CONTACTS];
+	int contact_count;
+	bool collided;
+	int cc;
+
+
+	static void _contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata);
+
+	void contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B);
+
+	void validate_contacts();
+
+	SpaceSW *space;
+
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B);
+	~BodyPairSW();
+
+};
+
+#endif // BODY_PAIR__SW_H
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
new file mode 100644
index 000000000..b926a9377
--- /dev/null
+++ b/servers/physics/body_sw.cpp
@@ -0,0 +1,631 @@
+/*************************************************************************/
+/*  body_sw.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 "body_sw.h"

+#include "space_sw.h"

+#include "area_sw.h"

+

+void BodySW::_update_inertia() {

+

+	if (get_space() && !inertia_update_list.in_list())

+		get_space()->body_add_to_inertia_update_list(&inertia_update_list);

+

+}

+

+

+void BodySW::_update_inertia_tensor() {

+

+	Matrix3 tb = get_transform().basis;

+	tb.scale(_inv_inertia);

+	_inv_inertia_tensor = tb * get_transform().basis.transposed();

+

+}

+

+void BodySW::update_inertias() {

+

+	//update shapes and motions

+

+	switch(mode) {

+

+		case PhysicsServer::BODY_MODE_RIGID: {

+

+			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)

+			float total_area=0;

+

+			for (int i=0;i<get_shape_count();i++) {

+

+				total_area+=get_shape_aabb(i).get_area();

+			}

+

+			Vector3 _inertia;

+

+

+			for (int i=0;i<get_shape_count();i++) {

+

+				const ShapeSW* shape=get_shape(i);

+

+				float area=get_shape_aabb(i).get_area();

+

+				float mass = area * this->mass / total_area;

+

+				_inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin();

+

+			}

+

+			if (_inertia!=Vector3())

+				_inv_inertia=_inertia.inverse();

+			else

+				_inv_inertia=Vector3();

+

+			if (mass)

+				_inv_mass=1.0/mass;

+			else

+				_inv_mass=0;

+

+		} break;

+

+		case PhysicsServer::BODY_MODE_STATIC_ACTIVE:

+		case PhysicsServer::BODY_MODE_STATIC: {

+

+			_inv_inertia=Vector3();

+			_inv_mass=0;

+		} break;

+		case PhysicsServer::BODY_MODE_CHARACTER: {

+

+			_inv_inertia=Vector3();

+			_inv_mass=1.0/mass;

+

+		} break;

+	}

+	_update_inertia_tensor();

+

+	//_update_shapes();

+

+}

+

+

+

+void BodySW::set_active(bool p_active) {

+

+	if (active==p_active)

+		return;

+

+	active=p_active;

+	if (!p_active) {

+		if (get_space())

+			get_space()->body_remove_from_active_list(&active_list);

+	} else {

+		if (mode==PhysicsServer::BODY_MODE_STATIC)

+			return; //static bodies can't become active

+		if (get_space())

+			get_space()->body_add_to_active_list(&active_list);

+

+		//still_time=0;

+	}

+/*

+	if (!space)

+		return;

+

+	for(int i=0;i<get_shape_count();i++) {

+		Shape &s=shapes[i];

+		if (s.bpid>0) {

+			get_space()->get_broadphase()->set_active(s.bpid,active);

+		}

+	}

+*/

+}

+

+

+

+void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) {

+

+	switch(p_param) {

+		case PhysicsServer::BODY_PARAM_BOUNCE: {

+

+			bounce=p_value;

+		} break;

+		case PhysicsServer::BODY_PARAM_FRICTION: {

+

+			friction=p_value;

+		} break;

+		case PhysicsServer::BODY_PARAM_MASS: {

+			ERR_FAIL_COND(p_value<=0);

+			mass=p_value;

+			_update_inertia();

+

+		} break;

+		default:{}

+	}

+}

+

+float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {

+

+	switch(p_param) {

+		case PhysicsServer::BODY_PARAM_BOUNCE: {

+

+			return bounce;

+		} break;

+		case PhysicsServer::BODY_PARAM_FRICTION: {

+

+			return friction;

+		} break;

+		case PhysicsServer::BODY_PARAM_MASS: {

+			return mass;

+		} break;

+		default:{}

+	}

+

+	return 0;

+}

+

+void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {

+

+	mode=p_mode;

+

+	switch(p_mode) {

+		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!

+		case PhysicsServer::BODY_MODE_STATIC:

+		case PhysicsServer::BODY_MODE_STATIC_ACTIVE: {

+

+			_set_inv_transform(get_transform().affine_inverse());

+			_inv_mass=0;

+			_set_static(p_mode==PhysicsServer::BODY_MODE_STATIC);

+			set_active(p_mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE);

+			linear_velocity=Vector3();

+			angular_velocity=Vector3();

+		} break;

+		case PhysicsServer::BODY_MODE_RIGID: {

+

+			_inv_mass=mass>0?(1.0/mass):0;

+			_set_static(false);

+			simulated_motion=false; //jic

+

+		} break;

+		case PhysicsServer::BODY_MODE_CHARACTER: {

+

+			_inv_mass=mass>0?(1.0/mass):0;

+			_set_static(false);

+			simulated_motion=false; //jic

+		} break;

+	}

+

+	_update_inertia();

+	//if (get_space())

+//		_update_queries();

+

+}

+PhysicsServer::BodyMode BodySW::get_mode() const {

+

+	return mode;

+}

+

+void BodySW::_shapes_changed() {

+

+	_update_inertia();

+}

+

+void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) {

+

+	switch(p_state)	{

+		case PhysicsServer::BODY_STATE_TRANSFORM: {

+

+

+			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) {

+				_set_transform(p_variant);

+				_set_inv_transform(get_transform().affine_inverse());

+				wakeup_neighbours();

+			} else {

+				Transform t = p_variant;

+				t.orthonormalize();

+				_set_transform(t);

+				_set_inv_transform(get_transform().inverse());

+

+			}

+

+		} break;

+		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {

+

+			//if (mode==PhysicsServer::BODY_MODE_STATIC)

+			//	break;

+			linear_velocity=p_variant;

+		} break;

+		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {

+			//if (mode!=PhysicsServer::BODY_MODE_RIGID)

+			//	break;

+			angular_velocity=p_variant;

+

+		} break;

+		case PhysicsServer::BODY_STATE_SLEEPING: {

+			//?

+			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)

+				break;

+			bool do_sleep=p_variant;

+			if (do_sleep) {

+				linear_velocity=Vector3();

+				//biased_linear_velocity=Vector3();

+				angular_velocity=Vector3();

+				//biased_angular_velocity=Vector3();

+				set_active(false);

+			} else {

+				if (mode!=PhysicsServer::BODY_MODE_STATIC)

+					set_active(true);

+			}

+		} break;

+		case PhysicsServer::BODY_STATE_CAN_SLEEP: {

+			can_sleep=p_variant;

+			if (mode==PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep)

+				set_active(true);

+

+		} break;

+	}

+

+}

+Variant BodySW::get_state(PhysicsServer::BodyState p_state) const {

+

+	switch(p_state)	{

+		case PhysicsServer::BODY_STATE_TRANSFORM: {

+			return get_transform();

+		} break;

+		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {

+			return linear_velocity;

+		} break;

+		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {

+			return angular_velocity;

+		} break;

+		case PhysicsServer::BODY_STATE_SLEEPING: {

+			return !is_active();

+		} break;

+		case PhysicsServer::BODY_STATE_CAN_SLEEP: {

+			return can_sleep;

+		} break;

+	}

+

+	return Variant();

+}

+

+

+void BodySW::set_space(SpaceSW *p_space){

+

+	if (get_space()) {

+

+		if (inertia_update_list.in_list())

+			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);

+		if (active_list.in_list())

+			get_space()->body_remove_from_active_list(&active_list);

+		if (direct_state_query_list.in_list())

+			get_space()->body_remove_from_state_query_list(&direct_state_query_list);

+

+	}

+

+	_set_space(p_space);

+

+	if (get_space()) {

+

+		_update_inertia();

+		if (active)

+			get_space()->body_add_to_active_list(&active_list);

+//		_update_queries();

+		//if (is_active()) {

+		//	active=false;

+		//	set_active(true);

+		//}

+

+	}

+

+}

+

+void BodySW::_compute_area_gravity(const AreaSW *p_area) {

+

+	if (p_area->is_gravity_point()) {

+

+		gravity = (p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity();

+

+	} else {

+		gravity = p_area->get_gravity_vector() * p_area->get_gravity();

+	}

+}

+

+void BodySW::integrate_forces(real_t p_step) {

+

+

+	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)

+		return;

+

+	AreaSW *current_area = get_space()->get_default_area();

+	ERR_FAIL_COND(!current_area);

+

+	int prio = current_area->get_priority();

+	int ac = areas.size();

+	if (ac) {

+		const AreaCMP *aa = &areas[0];

+		for(int i=0;i<ac;i++) {

+			if (aa[i].area->get_priority() > prio) {

+				current_area=aa[i].area;

+				prio=current_area->get_priority();

+			}

+		}

+	}

+

+	_compute_area_gravity(current_area);

+	density=current_area->get_density();

+

+	if (!omit_force_integration) {

+		//overriden by direct state query

+

+		Vector3 force=gravity*mass;

+		force+=applied_force;

+		Vector3 torque=applied_torque;

+

+		real_t damp = 1.0 - p_step * density;

+

+		if (damp<0) // reached zero in the given time

+			damp=0;

+

+		real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();

+

+		if (angular_damp<0) // reached zero in the given time

+			angular_damp=0;

+

+		linear_velocity*=damp;

+		angular_velocity*=angular_damp;

+

+		linear_velocity+=_inv_mass * force * p_step;

+		angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step;

+	}

+

+	applied_force=Vector3();

+	applied_torque=Vector3();

+

+	//motion=linear_velocity*p_step;

+

+	biased_angular_velocity=Vector3();

+	biased_linear_velocity=Vector3();

+

+	if (continuous_cd) //shapes temporarily extend for raycast

+		_update_shapes_with_motion(linear_velocity*p_step);

+

+	current_area=NULL; // clear the area, so it is set in the next frame

+	contact_count=0;

+

+}

+

+void BodySW::integrate_velocities(real_t p_step) {

+

+	if (mode==PhysicsServer::BODY_MODE_STATIC)

+		return;

+

+	if (mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) {

+		if (fi_callback)

+			get_space()->body_add_to_state_query_list(&direct_state_query_list);

+		return;

+	}

+

+	Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity;

+

+

+

+	float ang_vel = total_angular_velocity.length();

+	Transform transform = get_transform();

+

+

+	if (ang_vel!=0.0) {

+		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;

+		Matrix3 rot( ang_vel_axis, -ang_vel*p_step );

+		transform.basis = rot * transform.basis;

+		transform.orthonormalize();

+	}

+

+	Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity;

+

+

+	transform.origin+=total_linear_velocity * p_step;

+

+	_set_transform(transform);

+	_set_inv_transform(get_transform().inverse());

+

+	_update_inertia_tensor();

+

+	if (fi_callback) {

+

+		get_space()->body_add_to_state_query_list(&direct_state_query_list);

+	}

+

+}

+

+

+void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {

+

+	Transform inv_xform = p_xform.affine_inverse();

+	if (!get_space()) {

+		_set_transform(p_xform);

+		_set_inv_transform(inv_xform);

+

+		return;

+	}

+

+	//compute a FAKE linear velocity - this is easy

+

+	linear_velocity=(p_xform.origin - get_transform().origin)/p_step;

+

+	//compute a FAKE angular velocity, not so easy

+	Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized();

+	Vector3 axis;

+	float angle;

+

+	rot.get_axis_and_angle(axis,angle);

+	axis.normalize();

+	angular_velocity=axis.normalized() * (angle/p_step);

+	linear_velocity = (p_xform.origin - get_transform().origin)/p_step;

+

+	if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))

+		get_space()->body_add_to_state_query_list(&direct_state_query_list);

+	simulated_motion=true;

+	_set_transform(p_xform);

+

+

+}

+

+void BodySW::wakeup_neighbours() {

+

+	for(Map<ConstraintSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {

+

+		const ConstraintSW *c=E->key();

+		BodySW **n = c->get_body_ptr();

+		int bc=c->get_body_count();

+

+		for(int i=0;i<bc;i++) {

+

+			if (i==E->get())

+				continue;

+			BodySW *b = n[i];

+			if (b->mode!=PhysicsServer::BODY_MODE_RIGID)

+				continue;

+

+			if (!b->is_active())

+				b->set_active(true);

+		}

+	}

+}

+

+void BodySW::call_queries() {

+

+

+	if (fi_callback) {

+

+		PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;

+		dbs->body=this;

+

+		Variant v=dbs;

+

+		Object *obj = ObjectDB::get_instance(fi_callback->id);

+		if (!obj) {

+

+			set_force_integration_callback(0,StringName());

+		} else {

+			const Variant *vp[2]={&v,&fi_callback->udata};

+

+			Variant::CallError ce;

+			int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2;

+			obj->call(fi_callback->method,vp,argc,ce);

+		}

+

+

+	}

+

+	if (simulated_motion) {

+

+	//	linear_velocity=Vector3();

+	//	angular_velocity=0;

+		simulated_motion=false;

+	}

+}

+

+

+bool BodySW::sleep_test(real_t p_step)  {

+

+	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)

+		return true; //

+	else if (mode==PhysicsServer::BODY_MODE_CHARACTER)

+		return !active; // characters don't sleep unless asked to sleep

+	else if (!can_sleep)

+		return false;

+

+

+

+

+	if (Math::abs(angular_velocity.length())<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {

+

+		still_time+=p_step;

+

+		return still_time > get_space()->get_body_time_to_sleep();

+	} else {

+

+		still_time=0; //maybe this should be set to 0 on set_active?

+		return false;

+	}

+}

+

+

+void BodySW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {

+

+	if (fi_callback) {

+

+		memdelete(fi_callback);

+		fi_callback=NULL;

+	}

+

+

+	if (p_id!=0) {

+

+		fi_callback=memnew(ForceIntegrationCallback);

+		fi_callback->id=p_id;

+		fi_callback->method=p_method;

+		fi_callback->udata=p_udata;

+	}

+

+}

+

+BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {

+

+

+	mode=PhysicsServer::BODY_MODE_RIGID;

+	active=true;

+

+	mass=1;

+//	_inv_inertia=Transform();

+	_inv_mass=1;

+	bounce=0;

+	friction=1;

+	omit_force_integration=false;

+//	applied_torque=0;

+	island_step=0;

+	island_next=NULL;

+	island_list_next=NULL;

+	_set_static(false);

+	density=0;

+	contact_count=0;

+	simulated_motion=false;

+	still_time=0;

+	continuous_cd=false;

+	can_sleep=false;

+	fi_callback=NULL;

+

+}

+

+BodySW::~BodySW() {

+

+	if (fi_callback)

+		memdelete(fi_callback);

+}

+

+PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL;

+

+PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() {

+

+	return body->get_space()->get_direct_state();

+}

diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
new file mode 100644
index 000000000..9f0bbc00c
--- /dev/null
+++ b/servers/physics/body_sw.h
@@ -0,0 +1,348 @@
+/*************************************************************************/
+/*  body_sw.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 BODY_SW_H

+#define BODY_SW_H

+

+#include "collision_object_sw.h"

+#include "vset.h"

+#include "area_sw.h"

+

+class ConstraintSW;

+

+

+class BodySW : public CollisionObjectSW {

+

+

+	PhysicsServer::BodyMode mode;

+

+	Vector3 linear_velocity;

+	Vector3 angular_velocity;

+

+	Vector3 biased_linear_velocity;

+	Vector3 biased_angular_velocity;

+	real_t mass;

+	real_t bounce;

+	real_t friction;

+

+	real_t _inv_mass;

+	Vector3 _inv_inertia;

+	Matrix3 _inv_inertia_tensor;

+

+	Vector3 gravity;

+	real_t density;

+

+	real_t still_time;

+

+	Vector3 applied_force;

+	Vector3 applied_torque;

+

+	SelfList<BodySW> active_list;

+	SelfList<BodySW> inertia_update_list;

+	SelfList<BodySW> direct_state_query_list;

+

+	VSet<RID> exceptions;

+	bool omit_force_integration;

+	bool active;

+	bool simulated_motion;

+	bool continuous_cd;

+	bool can_sleep;

+	void _update_inertia();

+	virtual void _shapes_changed();

+

+	Map<ConstraintSW*,int> constraint_map;

+

+	struct AreaCMP {

+

+		AreaSW *area;

+		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }

+		_FORCE_INLINE_ AreaCMP() {}

+		_FORCE_INLINE_ AreaCMP(AreaSW *p_area) { area=p_area;}

+	};

+

+

+	VSet<AreaCMP> areas;

+

+	struct Contact {

+

+

+		Vector3 local_pos;

+		Vector3 local_normal;

+		float depth;

+		int local_shape;

+		Vector3 collider_pos;

+		int collider_shape;

+		ObjectID collider_instance_id;

+		RID collider;

+		Vector3 collider_velocity_at_pos;

+	};

+

+	Vector<Contact> contacts; //no contacts by default

+	int contact_count;

+

+	struct ForceIntegrationCallback {

+

+		ObjectID id;

+		StringName method;

+		Variant udata;

+	};

+

+	ForceIntegrationCallback *fi_callback;

+

+

+	uint64_t island_step;

+	BodySW *island_next;

+	BodySW *island_list_next;

+

+	_FORCE_INLINE_ void _compute_area_gravity(const AreaSW *p_area);

+

+	_FORCE_INLINE_ void _update_inertia_tensor();

+

+friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose

+

+public:

+

+

+	void set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata=Variant());

+

+

+	_FORCE_INLINE_ void add_area(AreaSW *p_area) { areas.insert(AreaCMP(p_area)); }

+	_FORCE_INLINE_ void remove_area(AreaSW *p_area) { areas.erase(AreaCMP(p_area)); }

+

+	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }

+	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }

+

+	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }

+	_FORCE_INLINE_ void add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, float p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos);

+

+

+	_FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}

+	_FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}

+	_FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}

+	_FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}

+

+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }

+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }

+

+	_FORCE_INLINE_ BodySW* get_island_next() const { return island_next; }

+	_FORCE_INLINE_ void set_island_next(BodySW* p_next) { island_next=p_next; }

+

+	_FORCE_INLINE_ BodySW* get_island_list_next() const { return island_list_next; }

+	_FORCE_INLINE_ void set_island_list_next(BodySW* p_next) { island_list_next=p_next; }

+

+	_FORCE_INLINE_ void add_constraint(ConstraintSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }

+	_FORCE_INLINE_ void remove_constraint(ConstraintSW* p_constraint) { constraint_map.erase(p_constraint); }

+	const Map<ConstraintSW*,int>& get_constraint_map() const { return constraint_map; }

+

+	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }

+	_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }

+

+	_FORCE_INLINE_ void set_linear_velocity(const Vector3& p_velocity) {linear_velocity=p_velocity; }

+	_FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; }

+

+	_FORCE_INLINE_ void set_angular_velocity(const Vector3& p_velocity) { angular_velocity=p_velocity; }

+	_FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; }

+

+	_FORCE_INLINE_ const Vector3& get_biased_linear_velocity() const { return biased_linear_velocity; }

+	_FORCE_INLINE_ const Vector3& get_biased_angular_velocity() const { return biased_angular_velocity; }

+

+	_FORCE_INLINE_ void apply_impulse(const Vector3& p_pos, const Vector3& p_j) {

+

+		linear_velocity += p_j * _inv_mass;

+		angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) );

+	}

+

+	_FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& p_j) {

+

+		biased_linear_velocity += p_j * _inv_mass;

+		biased_angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) );

+	}

+

+	_FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) {

+

+		angular_velocity += _inv_inertia_tensor.xform(p_j);

+	}

+

+	_FORCE_INLINE_ void add_force(const Vector3& p_force, const Vector3& p_pos) {

+

+		applied_force += p_force;

+		applied_torque += p_pos.cross(p_force);

+	}

+

+	void set_active(bool p_active);

+	_FORCE_INLINE_ bool is_active() const { return active; }

+

+	void set_param(PhysicsServer::BodyParameter p_param, float);

+	float get_param(PhysicsServer::BodyParameter p_param) const;

+

+	void set_mode(PhysicsServer::BodyMode p_mode);

+	PhysicsServer::BodyMode get_mode() const;

+

+	void set_state(PhysicsServer::BodyState p_state, const Variant& p_variant);

+	Variant get_state(PhysicsServer::BodyState p_state) const;

+

+	void set_applied_force(const Vector3& p_force) { applied_force=p_force; }

+	Vector3 get_applied_force() const { return applied_force; }

+

+	void set_applied_torque(const Vector3& p_torque) { applied_torque=p_torque; }

+	Vector3 get_applied_torque() const { return applied_torque; }

+

+	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; }

+	_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }

+

+	void set_space(SpaceSW *p_space);

+

+	void update_inertias();

+

+	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }

+	_FORCE_INLINE_ Vector3 get_inv_inertia() const { return _inv_inertia; }

+	_FORCE_INLINE_ Matrix3 get_inv_inertia_tensor() const { return _inv_inertia_tensor; }

+	_FORCE_INLINE_ real_t get_friction() const { return friction; }

+	_FORCE_INLINE_ Vector3 get_gravity() const { return gravity; }

+	_FORCE_INLINE_ real_t get_density() const { return density; }

+	_FORCE_INLINE_ real_t get_bounce() const { return bounce; }

+

+	void integrate_forces(real_t p_step);

+	void integrate_velocities(real_t p_step);

+

+	void simulate_motion(const Transform& p_xform,real_t p_step);

+	void call_queries();

+	void wakeup_neighbours();

+

+	bool sleep_test(real_t p_step);

+

+	BodySW();

+	~BodySW();

+

+};

+

+

+//add contact inline

+

+void BodySW::add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, float p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos) {

+

+	int c_max=contacts.size();

+

+	if (c_max==0)

+		return;

+

+	Contact *c = &contacts[0];

+

+

+	int idx=-1;

+

+	if (contact_count<c_max) {

+		idx=contact_count++;

+	} else {

+

+		float least_depth=1e20;

+		int least_deep=-1;

+		for(int i=0;i<c_max;i++) {

+

+			if (i==0 || c[i].depth<least_depth) {

+				least_deep=i;

+				least_depth=c[i].depth;

+			}

+		}

+

+		if (least_deep>=0 && least_depth<p_depth) {

+

+			idx=least_deep;

+		}

+		if (idx==-1)

+			return; //none least deepe than this

+	}

+

+	c[idx].local_pos=p_local_pos;

+	c[idx].local_normal=p_local_normal;

+	c[idx].depth=p_depth;

+	c[idx].local_shape=p_local_shape;

+	c[idx].collider_pos=p_collider_pos;

+	c[idx].collider_shape=p_collider_shape;

+	c[idx].collider_instance_id=p_collider_instance_id;

+	c[idx].collider=p_collider;

+	c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;

+

+}

+

+

+class PhysicsDirectBodyStateSW : public PhysicsDirectBodyState {

+

+	OBJ_TYPE( PhysicsDirectBodyStateSW, PhysicsDirectBodyState );

+

+public:

+

+	static PhysicsDirectBodyStateSW *singleton;

+	BodySW *body;

+	real_t step;

+

+	virtual Vector3 get_total_gravity() const {  return body->get_gravity();  } // get gravity vector working on this body space/area

+	virtual float get_total_density() const {  return body->get_density();  } // get density of this body space/area

+

+	virtual float get_inverse_mass() const {  return body->get_inv_mass();  } // get the mass

+	virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia();   } // get density of this body space

+	virtual Matrix3 get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor();   } // get density of this body space

+

+	virtual void set_linear_velocity(const Vector3& p_velocity) {  body->set_linear_velocity(p_velocity);  }

+	virtual Vector3 get_linear_velocity() const {  return body->get_linear_velocity();  }

+

+	virtual void set_angular_velocity(const Vector3& p_velocity) {  body->set_angular_velocity(p_velocity);  }

+	virtual Vector3 get_angular_velocity() const {  return body->get_angular_velocity();  }

+

+	virtual void set_transform(const Transform& p_transform) {  body->set_state(PhysicsServer::BODY_STATE_TRANSFORM,p_transform);  }

+	virtual Transform get_transform() const {  return body->get_transform();  }

+

+	virtual void add_force(const Vector3& p_force, const Vector3& p_pos) {  body->add_force(p_force,p_pos); }

+

+	virtual void set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }

+	virtual bool is_sleeping() const {  return !body->is_active();  }

+

+	virtual int get_contact_count() const {  return body->contact_count;  }

+

+	virtual Vector3 get_contact_local_pos(int p_contact_idx) const {

+		ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());

+		return body->contacts[p_contact_idx].local_pos;

+	}

+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());   return body->contacts[p_contact_idx].local_normal;   }

+	virtual int get_contact_local_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1);   return body->contacts[p_contact_idx].local_shape;   }

+

+	virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID());   return body->contacts[p_contact_idx].collider;   }

+	virtual Vector3 get_contact_collider_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());   return body->contacts[p_contact_idx].collider_pos;  }

+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0);   return body->contacts[p_contact_idx].collider_instance_id;   }

+	virtual int get_contact_collider_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape;  }

+	virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].collider_velocity_at_pos;  }

+

+	virtual PhysicsDirectSpaceState* get_space_state();

+

+

+	virtual real_t get_step() const { return step; }

+	PhysicsDirectBodyStateSW() { singleton=this; body=NULL; }

+};

+

+

+#endif // BODY__SW_H

diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp
new file mode 100644
index 000000000..5db78e669
--- /dev/null
+++ b/servers/physics/broad_phase_basic.cpp
@@ -0,0 +1,216 @@
+/*************************************************************************/
+/*  broad_phase_basic.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 "broad_phase_basic.h"
+#include "list.h"
+#include "print_string.h"
+BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object_, int p_subindex) {
+
+	if (p_object_==NULL) {
+
+		ERR_FAIL_COND_V(p_object_==NULL,0);
+	}
+
+	current++;
+
+	Element e;
+	e.owner=p_object_;
+	e._static=false;
+	e.subindex=p_subindex;
+
+	element_map[current]=e;
+	return current;
+}
+
+void BroadPhaseBasic::move(ID p_id, const AABB& p_aabb) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	E->get().aabb=p_aabb;
+
+}
+void BroadPhaseBasic::set_static(ID p_id, bool p_static) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	E->get()._static=p_static;
+
+}
+void BroadPhaseBasic::remove(ID p_id) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	List<PairKey> to_erase;
+	//unpair must be done immediately on removal to avoid potential invalid pointers
+	for (Map<PairKey,void*>::Element *F=pair_map.front();F;F=F->next()) {
+
+		if (F->key().a==p_id || F->key().b==p_id) {
+
+			if (unpair_callback) {
+				Element *elem_A=&element_map[F->key().a];
+				Element *elem_B=&element_map[F->key().b];
+				unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,F->get(),unpair_userdata);
+			}
+			to_erase.push_back(F->key());
+		}
+	}
+	while(to_erase.size()) {
+
+		pair_map.erase(to_erase.front()->get());
+		to_erase.pop_front();
+	}
+	element_map.erase(E);
+
+}
+
+CollisionObjectSW *BroadPhaseBasic::get_object(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,NULL);
+	return E->get().owner;
+
+}
+bool BroadPhaseBasic::is_static(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,false);
+	return E->get()._static;
+
+}
+int BroadPhaseBasic::get_subindex(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,-1);
+	return E->get().subindex;
+}
+
+int BroadPhaseBasic::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+
+	int rc=0;
+
+	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+
+		const AABB aabb=E->get().aabb;
+		if (aabb.intersects_segment(p_from,p_to)) {
+
+			p_results[rc]=E->get().owner;
+			p_result_indices[rc]=E->get().subindex;
+			rc++;
+			if (rc>=p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+
+}
+int BroadPhaseBasic::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+
+	int rc=0;
+
+	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+
+		const AABB aabb=E->get().aabb;
+		if (aabb.intersects(p_aabb)) {
+
+			p_results[rc]=E->get().owner;
+			p_result_indices[rc]=E->get().subindex;
+			rc++;
+			if (rc>=p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+}
+
+void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
+
+	pair_userdata=p_userdata;
+	pair_callback=p_pair_callback;
+
+}
+void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) {
+
+	unpair_userdata=p_userdata;
+	unpair_callback=p_pair_callback;
+
+}
+
+void BroadPhaseBasic::update() {
+
+	// recompute pairs
+	for(Map<ID,Element>::Element *I=element_map.front();I;I=I->next()) {
+
+		for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) {
+
+			Element *elem_A=&I->get();
+			Element *elem_B=&J->get();
+
+			if (elem_A->owner == elem_B->owner)
+				continue;
+
+
+			bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static );
+
+			PairKey key(I->key(),J->key());
+
+			Map<PairKey,void*>::Element *E=pair_map.find(key);
+
+			if (!pair_ok && E) {
+				if (unpair_callback)
+					unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,E->get(),unpair_userdata);
+				pair_map.erase(key);
+			}
+
+			if (pair_ok && !E) {
+
+				void *data=NULL;
+				if (pair_callback)
+					data=pair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,unpair_userdata);
+				pair_map.insert(key,data);
+			}
+		}
+	}
+
+}
+
+BroadPhaseSW *BroadPhaseBasic::_create() {
+
+	return memnew( BroadPhaseBasic );
+}
+
+BroadPhaseBasic::BroadPhaseBasic() {
+
+	current=1;
+	unpair_callback=NULL;
+	unpair_userdata=NULL;
+	pair_callback=NULL;
+	pair_userdata=NULL;
+
+}
diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h
new file mode 100644
index 000000000..7b7746629
--- /dev/null
+++ b/servers/physics/broad_phase_basic.h
@@ -0,0 +1,101 @@
+/*************************************************************************/
+/*  broad_phase_basic.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 BROAD_PHASE_BASIC_H
+#define BROAD_PHASE_BASIC_H
+
+#include "broad_phase_sw.h"
+#include "map.h"
+
+class BroadPhaseBasic : public BroadPhaseSW {
+
+	struct Element {
+
+		CollisionObjectSW *owner;
+		bool _static;
+		AABB aabb;
+		int subindex;
+	};
+
+
+	Map<ID,Element> element_map;
+
+	ID current;
+
+	struct PairKey {
+
+		union {
+			struct {
+				ID a;
+				ID b;
+			};
+			uint64_t key;
+		};
+
+		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+			return key < p_key.key;
+		}
+
+		PairKey() { key=0; }
+		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
+
+	};
+
+	Map<PairKey,void*> pair_map;
+
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+public:
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0);
+	virtual void move(ID p_id, const AABB& p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObjectSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+
+	virtual void update();
+
+	static BroadPhaseSW *_create();
+	BroadPhaseBasic();
+};
+
+#endif // BROAD_PHASE_BASIC_H
diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp
new file mode 100644
index 000000000..edf4aae2b
--- /dev/null
+++ b/servers/physics/broad_phase_octree.cpp
@@ -0,0 +1,133 @@
+/*************************************************************************/
+/*  broad_phase_octree.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 "broad_phase_octree.h"
+#include "collision_object_sw.h"
+
+ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) {
+
+	ID oid = octree.create(p_object,AABB(),p_subindex,false,1<<p_object->get_type(),0);
+	return oid;
+}
+
+void BroadPhaseOctree::move(ID p_id, const AABB& p_aabb){
+
+	octree.move(p_id,p_aabb);
+}
+
+void BroadPhaseOctree::set_static(ID p_id, bool p_static){
+
+	CollisionObjectSW *it = octree.get(p_id);
+	octree.set_pairable(p_id,p_static?false:true,1<<it->get_type(),p_static?0:0xFFFFF); //pair everything, don't care 1?
+
+}
+void BroadPhaseOctree::remove(ID p_id){
+
+	octree.erase(p_id);
+}
+
+CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const{
+
+	CollisionObjectSW *it = octree.get(p_id);
+	ERR_FAIL_COND_V(!it,NULL);
+	return it;
+}
+bool BroadPhaseOctree::is_static(ID p_id) const{
+
+	return !octree.is_pairable(p_id);
+}
+int BroadPhaseOctree::get_subindex(ID p_id) const{
+
+	return octree.get_subindex(p_id);
+}
+
+int BroadPhaseOctree::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices){
+
+	return octree.cull_segment(p_from,p_to,p_results,p_max_results,p_result_indices);
+}
+
+int BroadPhaseOctree::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+
+	return octree.cull_AABB(p_aabb,p_results,p_max_results,p_result_indices);
+
+}
+
+
+void* BroadPhaseOctree::_pair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B) {
+
+	BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self);
+	if (!bpo->pair_callback)
+		return NULL;
+
+	return bpo->pair_callback(p_object_A,subindex_A,p_object_B,subindex_B,bpo->pair_userdata);
+
+}
+
+void BroadPhaseOctree::_unpair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B,void*pairdata) {
+
+	BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self);
+	if (!bpo->unpair_callback)
+		return;
+
+	bpo->unpair_callback(p_object_A,subindex_A,p_object_B,subindex_B,pairdata,bpo->unpair_userdata);
+
+}
+
+
+void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback,void *p_userdata){
+
+	pair_callback=p_pair_callback;
+	pair_userdata=p_userdata;
+
+}
+void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata){
+
+	unpair_callback=p_unpair_callback;
+	unpair_userdata=p_userdata;
+
+}
+
+void BroadPhaseOctree::update() {
+	// does.. not?
+}
+
+BroadPhaseSW *BroadPhaseOctree::_create() {
+
+	return memnew( BroadPhaseOctree );
+}
+
+BroadPhaseOctree::BroadPhaseOctree() {
+	octree.set_pair_callback(_pair_callback,this);
+	octree.set_unpair_callback(_unpair_callback,this);
+	pair_callback=NULL;
+	pair_userdata=NULL;
+	pair_callback=NULL;
+	unpair_userdata=NULL;
+}
+
+
diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h
new file mode 100644
index 000000000..d36521396
--- /dev/null
+++ b/servers/physics/broad_phase_octree.h
@@ -0,0 +1,73 @@
+/*************************************************************************/
+/*  broad_phase_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 BROAD_PHASE_OCTREE_H
+#define BROAD_PHASE_OCTREE_H
+
+#include "broad_phase_sw.h"
+#include "octree.h"
+
+class BroadPhaseOctree : public BroadPhaseSW {
+
+
+	Octree<CollisionObjectSW,true> octree;
+
+	static void* _pair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int);
+	static void _unpair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int,void*);
+
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+public:
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0);
+	virtual void move(ID p_id, const AABB& p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObjectSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+
+	virtual void update();
+
+	static BroadPhaseSW *_create();
+	BroadPhaseOctree();
+};
+
+#endif // BROAD_PHASE_OCTREE_H
diff --git a/servers/physics/broad_phase_sw.cpp b/servers/physics/broad_phase_sw.cpp
new file mode 100644
index 000000000..1211db141
--- /dev/null
+++ b/servers/physics/broad_phase_sw.cpp
@@ -0,0 +1,35 @@
+/*************************************************************************/
+/*  broad_phase_sw.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 "broad_phase_sw.h"
+
+BroadPhaseSW::CreateFunction BroadPhaseSW::create_func=NULL;
+
+BroadPhaseSW::~BroadPhaseSW()
+{
+}
diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h
new file mode 100644
index 000000000..57301a2af
--- /dev/null
+++ b/servers/physics/broad_phase_sw.h
@@ -0,0 +1,73 @@
+/*************************************************************************/
+/*  broad_phase_sw.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 BROAD_PHASE_SW_H
+#define BROAD_PHASE_SW_H
+
+#include "math_funcs.h"
+#include "aabb.h"
+
+class CollisionObjectSW;
+
+
+class BroadPhaseSW {
+
+public:
+	typedef BroadPhaseSW* (*CreateFunction)();
+
+	static CreateFunction create_func;
+
+	typedef uint32_t ID;
+
+
+	typedef void* (*PairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_userdata);
+	typedef void (*UnpairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_userdata);
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0)=0;
+	virtual void move(ID p_id, const AABB& p_aabb)=0;
+	virtual void set_static(ID p_id, bool p_static)=0;
+	virtual void remove(ID p_id)=0;
+
+	virtual CollisionObjectSW *get_object(ID p_id) const=0;
+	virtual bool is_static(ID p_id) const=0;
+	virtual int get_subindex(ID p_id) const=0;
+
+	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0;
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0;
+
+	virtual void update()=0;
+
+	virtual ~BroadPhaseSW();
+
+};
+
+#endif // BROAD_PHASE__SW_H
diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp
new file mode 100644
index 000000000..156004d15
--- /dev/null
+++ b/servers/physics/collision_object_sw.cpp
@@ -0,0 +1,219 @@
+/*************************************************************************/
+/*  collision_object_sw.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 "collision_object_sw.h"
+#include "space_sw.h"
+
+void CollisionObjectSW::add_shape(ShapeSW *p_shape,const Transform& p_transform) {
+
+	Shape s;
+	s.shape=p_shape;
+	s.xform=p_transform;
+	s.xform_inv=s.xform.affine_inverse();
+	s.bpid=0; //needs update
+	shapes.push_back(s);
+	p_shape->add_owner(this);
+	_update_shapes();
+	_shapes_changed();
+
+}
+
+void CollisionObjectSW::set_shape(int p_index,ShapeSW *p_shape){
+
+	ERR_FAIL_INDEX(p_index,shapes.size());
+	shapes[p_index].shape->remove_owner(this);
+	shapes[p_index].shape=p_shape;
+
+	p_shape->add_owner(this);
+	_update_shapes();
+	_shapes_changed();
+
+}
+void CollisionObjectSW::set_shape_transform(int p_index,const Transform& p_transform){
+
+	ERR_FAIL_INDEX(p_index,shapes.size());
+
+	shapes[p_index].xform=p_transform;
+	shapes[p_index].xform_inv=p_transform.affine_inverse();
+	_update_shapes();
+	_shapes_changed();
+}
+
+void CollisionObjectSW::remove_shape(ShapeSW *p_shape) {
+
+	//remove a shape, all the times it appears
+	for(int i=0;i<shapes.size();i++) {
+
+		if (shapes[i].shape==p_shape) {
+			remove_shape(i);
+			i--;
+		}
+	}
+}
+
+void CollisionObjectSW::remove_shape(int p_index){
+
+	//remove anything from shape to be erased to end, so subindices don't change
+	ERR_FAIL_INDEX(p_index,shapes.size());
+	for(int i=p_index;i<shapes.size();i++) {
+
+		if (shapes[i].bpid==0)
+			continue;
+		//should never get here with a null owner
+		space->get_broadphase()->remove(shapes[i].bpid);
+		shapes[i].bpid=0;
+	}
+	shapes[p_index].shape->remove_owner(this);
+	shapes.remove(p_index);
+
+	_shapes_changed();
+
+}
+
+void CollisionObjectSW::_set_static(bool p_static) {
+	if (_static==p_static)
+		return;
+	_static=p_static;
+
+	if (!space)
+		return;
+	for(int i=0;i<get_shape_count();i++) {
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+	}
+
+}
+
+void CollisionObjectSW::_unregister_shapes() {
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			space->get_broadphase()->remove(s.bpid);
+			s.bpid=0;
+		}
+	}
+
+}
+
+void CollisionObjectSW::_update_shapes() {
+
+	if (!space)
+		return;
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid==0) {
+			s.bpid=space->get_broadphase()->create(this,i);
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+
+		//not quite correct, should compute the next matrix..
+		AABB shape_aabb=s.shape->get_aabb();
+		Transform xform = transform * s.xform;
+		shape_aabb=xform.xform(shape_aabb);
+		s.aabb_cache=shape_aabb;
+		s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 );
+
+
+		space->get_broadphase()->move(s.bpid,s.aabb_cache);
+	}
+
+}
+
+void CollisionObjectSW::_update_shapes_with_motion(const Vector3& p_motion) {
+
+
+	if (!space)
+		return;
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid==0) {
+			s.bpid=space->get_broadphase()->create(this,i);
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+
+		//not quite correct, should compute the next matrix..
+		AABB shape_aabb=s.shape->get_aabb();
+		Transform xform = transform * s.xform;
+		shape_aabb=xform.xform(shape_aabb);
+		shape_aabb=shape_aabb.merge(AABB( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion
+		s.aabb_cache=shape_aabb;
+
+		space->get_broadphase()->move(s.bpid,shape_aabb);
+	}
+
+
+}
+
+void CollisionObjectSW::_set_space(SpaceSW *p_space) {
+
+	if (space) {
+
+		space->remove_object(this);
+
+		for(int i=0;i<shapes.size();i++) {
+
+			Shape &s=shapes[i];
+			if (s.bpid) {
+				space->get_broadphase()->remove(s.bpid);
+				s.bpid=0;
+			}
+		}
+
+	}
+
+	space=p_space;
+
+	if (space) {
+
+		space->add_object(this);
+		_update_shapes();
+	}
+
+}
+
+void CollisionObjectSW::_shape_changed() {
+
+	_update_shapes();
+	_shapes_changed();
+}
+
+CollisionObjectSW::CollisionObjectSW(Type p_type) {
+
+	_static=true;
+	type=p_type;
+	space=NULL;
+	instance_id=0;
+}
diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h
new file mode 100644
index 000000000..6d60f2f07
--- /dev/null
+++ b/servers/physics/collision_object_sw.h
@@ -0,0 +1,119 @@
+/*************************************************************************/
+/*  collision_object_sw.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 COLLISION_OBJECT_SW_H
+#define COLLISION_OBJECT_SW_H
+
+#include "shape_sw.h"
+#include "servers/physics_server.h"
+#include "self_list.h"
+#include "broad_phase_sw.h"
+
+class SpaceSW;
+
+class CollisionObjectSW : public ShapeOwnerSW {
+public:
+	enum Type {
+		TYPE_AREA,
+		TYPE_BODY
+	};
+private:
+
+	Type type;
+	RID self;
+	ObjectID instance_id;
+
+	struct Shape {
+
+		Transform xform;
+		Transform xform_inv;
+		BroadPhaseSW::ID bpid;
+		AABB aabb_cache; //for rayqueries
+		ShapeSW *shape;
+	};
+
+	Vector<Shape> shapes;
+	SpaceSW *space;
+	Transform transform;
+	Transform inv_transform;
+	bool _static;
+
+	void _update_shapes();
+
+protected:
+
+
+	void _update_shapes_with_motion(const Vector3& p_motion);
+	void _unregister_shapes();
+
+	_FORCE_INLINE_ void _set_transform(const Transform& p_transform) { transform=p_transform; _update_shapes(); }
+	_FORCE_INLINE_ void _set_inv_transform(const Transform& p_transform) { inv_transform=p_transform; }
+	void _set_static(bool p_static);
+
+	virtual void _shapes_changed()=0;
+	void _set_space(SpaceSW *space);
+
+	CollisionObjectSW(Type p_type);
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ void set_instance_id(const ObjectID& p_instance_id) { instance_id=p_instance_id; }
+	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
+
+	void _shape_changed();
+
+	_FORCE_INLINE_ Type get_type() const { return type; }
+	void add_shape(ShapeSW *p_shape,const Transform& p_transform=Transform());
+	void set_shape(int p_index,ShapeSW *p_shape);
+	void set_shape_transform(int p_index,const Transform& p_transform);
+	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
+	_FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { return shapes[p_index].shape; }
+	_FORCE_INLINE_ const Transform& get_shape_transform(int p_index) const { return shapes[p_index].xform; }
+	_FORCE_INLINE_ const Transform& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
+	_FORCE_INLINE_ const AABB& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
+
+	_FORCE_INLINE_ Transform get_transform() const { return transform; }
+	_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
+	_FORCE_INLINE_ SpaceSW* get_space() const { return space; }
+
+
+
+	void remove_shape(ShapeSW *p_shape);
+	void remove_shape(int p_index);
+
+	virtual void set_space(SpaceSW *p_space)=0;
+
+	_FORCE_INLINE_ bool is_static() const { return _static;  }
+
+	virtual ~CollisionObjectSW() {}
+
+};
+
+#endif // COLLISION_OBJECT_SW_H
diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp
new file mode 100644
index 000000000..1cd40db77
--- /dev/null
+++ b/servers/physics/collision_solver_sat.cpp
@@ -0,0 +1,1331 @@
+/*************************************************************************/
+/*  collision_solver_sat.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 "collision_solver_sat.h"
+#include "geometry.h"
+
+#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.02
+
+struct _CollectorCallback {
+
+	CollisionSolverSW::CallbackResult callback;
+	void *userdata;
+	bool swap;
+	bool collided;
+	Vector3 normal;
+	Vector3 *prev_axis;
+
+	_FORCE_INLINE_ void call(const Vector3& p_point_A, const Vector3& p_point_B) {
+
+		//if (normal.dot(p_point_A) >= normal.dot(p_point_B))
+		// return;
+		if (swap)
+			callback(p_point_B,p_point_A,userdata);
+		else
+			callback(p_point_A,p_point_B,userdata);
+	}
+
+};
+
+typedef void (*GenerateContactsFunc)(const Vector3 *,int, const Vector3 *,int ,_CollectorCallback *);
+
+
+static void _generate_contacts_point_point(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A != 1 );
+	ERR_FAIL_COND( p_point_count_B != 1 );
+#endif
+
+	p_callback->call(*p_points_A,*p_points_B);
+}
+
+static void _generate_contacts_point_edge(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A != 1 );
+	ERR_FAIL_COND( p_point_count_B != 2 );
+#endif
+
+	Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B );
+	p_callback->call(*p_points_A,closest_B);
+
+}
+
+static void _generate_contacts_point_face(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A != 1 );
+	ERR_FAIL_COND( p_point_count_B < 3 );
+#endif
+
+
+	Vector3 closest_B=Plane(p_points_B[0],p_points_B[1],p_points_B[2]).project( *p_points_A );
+
+	p_callback->call(*p_points_A,closest_B);
+
+}
+
+
+static void _generate_contacts_edge_edge(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A != 2 );
+	ERR_FAIL_COND( p_point_count_B != 2 ); // circle is actually a 4x3 matrix
+#endif
+
+
+
+	Vector3 rel_A=p_points_A[1]-p_points_A[0];
+	Vector3 rel_B=p_points_B[1]-p_points_B[0];
+
+	Vector3 c=rel_A.cross(rel_B).cross(rel_B);
+
+//	if ( Math::abs(rel_A.dot(c) )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {
+	if ( Math::abs(rel_A.dot(c) )<CMP_EPSILON ) {
+
+		// should handle somehow..
+		//ERR_PRINT("TODO FIX");
+		//return;
+
+		Vector3 axis = rel_A.normalized(); //make an axis
+		Vector3 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]);
+		Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]);
+
+		//sort all 4 points in axis
+		float dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) };
+
+		SortArray<float> sa;
+		sa.sort(dvec,4);
+
+		//use the middle ones as contacts
+		p_callback->call(base_A+axis*dvec[1],base_B+axis*dvec[1]);
+		p_callback->call(base_A+axis*dvec[2],base_B+axis*dvec[2]);
+
+		return;
+
+	}
+
+	real_t d = (c.dot( p_points_B[0] ) - p_points_A[0].dot(c))/rel_A.dot(c);
+
+	if (d<0.0)
+		d=0.0;
+	else if (d>1.0)
+		d=1.0;
+
+	Vector3 closest_A=p_points_A[0]+rel_A*d;
+	Vector3 closest_B=Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B);
+	p_callback->call(closest_A,closest_B);
+
+}
+
+static void _generate_contacts_face_face(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A <2 );
+	ERR_FAIL_COND( p_point_count_B <3 );
+#endif
+
+	static const int max_clip=32;
+
+	Vector3 _clipbuf1[max_clip];
+	Vector3 _clipbuf2[max_clip];
+	Vector3 *clipbuf_src=_clipbuf1;
+	Vector3 *clipbuf_dst=_clipbuf2;
+	int clipbuf_len=p_point_count_A;
+
+	// copy A points to clipbuf_src
+	for (int i=0;i<p_point_count_A;i++) {
+
+		clipbuf_src[i]=p_points_A[i];
+	}
+
+	Plane plane_B(p_points_B[0],p_points_B[1],p_points_B[2]);
+
+	// go through all of B points
+	for (int i=0;i<p_point_count_B;i++) {
+
+		int i_n=(i+1)%p_point_count_B;
+
+		Vector3 edge0_B=p_points_B[i];
+		Vector3 edge1_B=p_points_B[i_n];
+
+		Vector3 clip_normal = (edge0_B - edge1_B).cross( plane_B.normal ).normalized();
+		// make a clip plane
+
+
+		Plane clip(edge0_B,clip_normal);
+		// avoid double clip if A is edge
+		int dst_idx=0;
+		bool edge = clipbuf_len==2;
+		for (int j=0;j<clipbuf_len;j++) {
+
+			int j_n=(j+1)%clipbuf_len;
+
+			Vector3 edge0_A=clipbuf_src[j];
+			Vector3 edge1_A=clipbuf_src[j_n];
+
+			real_t dist0 = clip.distance_to(edge0_A);
+			real_t dist1 = clip.distance_to(edge1_A);
+
+
+			if ( dist0 <= 0 ) { // behind plane
+
+				ERR_FAIL_COND( dst_idx >= max_clip );
+				clipbuf_dst[dst_idx++]=clipbuf_src[j];
+
+			}
+
+
+			// check for different sides and non coplanar
+//			if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) {
+			if ( (dist0*dist1) < 0 && !(edge && j)) {
+
+				// calculate intersection
+				Vector3 rel = edge1_A - edge0_A;
+				real_t den=clip.normal.dot( rel );
+				real_t dist=-(clip.normal.dot( edge0_A )-clip.d)/den;
+				Vector3 inters = edge0_A+rel*dist;
+
+				ERR_FAIL_COND( dst_idx >= max_clip );
+				clipbuf_dst[dst_idx]=inters;
+				dst_idx++;
+			}
+		}
+
+		clipbuf_len=dst_idx;
+		SWAP(clipbuf_src,clipbuf_dst);
+	}
+
+
+	// generate contacts
+	//Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]);
+
+	int added=0;
+
+	for (int i=0;i<clipbuf_len;i++) {
+
+		float d = plane_B.distance_to(clipbuf_src[i]);
+		if (d>CMP_EPSILON)
+			continue;
+
+		Vector3 closest_B=clipbuf_src[i] - plane_B.normal*d;
+
+		p_callback->call(clipbuf_src[i],closest_B);
+		added++;
+
+	}
+
+}
+
+
+static void _generate_contacts_from_supports(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A <1 );
+	ERR_FAIL_COND( p_point_count_B <1 );
+#endif
+
+
+	static const GenerateContactsFunc generate_contacts_func_table[3][3]={
+		{
+			_generate_contacts_point_point,
+			_generate_contacts_point_edge,
+			_generate_contacts_point_face,
+		},{
+			0,
+			_generate_contacts_edge_edge,
+			_generate_contacts_face_face,
+		},{
+			0,0,
+			_generate_contacts_face_face,
+		}
+	};
+
+	int pointcount_B;
+	int pointcount_A;
+	const Vector3 *points_A;
+	const Vector3 *points_B;
+
+	if (p_point_count_A > p_point_count_B) {
+		//swap
+		p_callback->swap = !p_callback->swap;
+		p_callback->normal = -p_callback->normal;
+
+		pointcount_B = p_point_count_A;
+		pointcount_A = p_point_count_B;
+		points_A=p_points_B;
+		points_B=p_points_A;
+	} else {
+
+		pointcount_B = p_point_count_B;
+		pointcount_A = p_point_count_A;
+		points_A=p_points_A;
+		points_B=p_points_B;
+	}
+
+	int version_A = (pointcount_A > 3 ?  3 : pointcount_A) -1;
+	int version_B = (pointcount_B > 3 ?  3 : pointcount_B) -1;
+
+	GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
+	ERR_FAIL_COND(!contacts_func);
+	contacts_func(points_A,pointcount_A,points_B,pointcount_B,p_callback);
+
+}
+
+
+
+template<class ShapeA, class ShapeB>
+class SeparatorAxisTest {
+
+	const ShapeA *shape_A;
+	const ShapeB *shape_B;
+	const Transform *transform_A;
+	const Transform *transform_B;
+	real_t best_depth;
+	Vector3 best_axis;
+	_CollectorCallback *callback;
+
+	Vector3 separator_axis;
+
+public:
+
+	_FORCE_INLINE_ bool test_previous_axis() {
+
+		if (callback && callback->prev_axis && *callback->prev_axis!=Vector3())
+			return test_axis(*callback->prev_axis);
+		else
+			return true;
+	}
+
+	_FORCE_INLINE_ bool test_axis(const Vector3& p_axis) {
+
+		Vector3 axis=p_axis;
+
+		if (	Math::abs(axis.x)<CMP_EPSILON &&
+			Math::abs(axis.y)<CMP_EPSILON &&
+			Math::abs(axis.z)<CMP_EPSILON ) {
+			// strange case, try an upwards separator
+			axis=Vector3(0.0,1.0,0.0);
+		}
+
+		real_t min_A,max_A,min_B,max_B;
+
+		shape_A->project_range(axis,*transform_A,min_A,max_A);
+		shape_B->project_range(axis,*transform_B,min_B,max_B);
+
+		min_B -= ( max_A - min_A ) * 0.5;
+		max_B += ( max_A - min_A ) * 0.5;
+
+		real_t dmin = min_B - ( min_A + max_A ) * 0.5;
+		real_t dmax = max_B - ( min_A + max_A ) * 0.5;
+
+		if (dmin > 0.0 || dmax < 0.0) {
+			separator_axis=axis;
+			return false; // doesn't contain 0
+		}
+
+		//use the smallest depth
+
+		dmin = Math::abs(dmin);
+
+		if ( dmax < dmin ) {
+			if ( dmax < best_depth ) {
+				best_depth=dmax;
+				best_axis=axis;
+			}
+		} else {
+			if ( dmin < best_depth ) {
+				best_depth=dmin;
+				best_axis=-axis; // keep it as A axis
+			}
+		}
+
+		return true;
+	}
+
+
+	_FORCE_INLINE_ void generate_contacts() {
+
+		// nothing to do, don't generate
+		if (best_axis==Vector3(0.0,0.0,0.0))
+			return;
+
+		if (!callback->callback) {
+			//just was checking intersection?
+			callback->collided=true;
+			if (callback->prev_axis)
+				*callback->prev_axis=best_axis;
+			return;
+		}
+
+		static const int max_supports=16;
+
+		Vector3 supports_A[max_supports];
+		int support_count_A;
+		shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(),max_supports,supports_A,support_count_A);
+		for(int i=0;i<support_count_A;i++) {
+			supports_A[i] = transform_A->xform(supports_A[i]);
+		}
+
+
+		Vector3 supports_B[max_supports];
+		int support_count_B;
+		shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(),max_supports,supports_B,support_count_B);
+		for(int i=0;i<support_count_B;i++) {
+			supports_B[i] = transform_B->xform(supports_B[i]);
+		}
+/*
+		print_line("best depth: "+rtos(best_depth));
+		for(int i=0;i<support_count_A;i++) {
+
+			print_line("A-"+itos(i)+": "+supports_A[i]);
+		}
+		for(int i=0;i<support_count_B;i++) {
+
+			print_line("B-"+itos(i)+": "+supports_B[i]);
+		}
+*/
+		callback->normal=best_axis;
+		if (callback->prev_axis)
+			*callback->prev_axis=best_axis;
+		_generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback);
+
+		callback->collided=true;
+		//CollisionSolverSW::CallbackResult cbk=NULL;
+		//cbk(Vector3(),Vector3(),NULL);
+
+	}
+
+	_FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A,const Transform& p_transform_A, const ShapeB *p_shape_B,const Transform& p_transform_B,_CollectorCallback *p_callback) {
+		best_depth=1e15;
+		shape_A=p_shape_A;
+		shape_B=p_shape_B;
+		transform_A=&p_transform_A;
+		transform_B=&p_transform_B;
+		callback=p_callback;
+	}
+
+};
+
+/****** SAT TESTS *******/
+/****** SAT TESTS *******/
+/****** SAT TESTS *******/
+/****** SAT TESTS *******/
+
+
+typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback);
+
+
+static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
+	const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW*>(p_b);
+
+	SeparatorAxisTest<SphereShapeSW,SphereShapeSW> separator(sphere_A,p_transform_a,sphere_B,p_transform_b,p_collector);
+
+	// previous axis
+
+	if (!separator.test_previous_axis())
+		return;
+
+	if (!separator.test_axis( (p_transform_a.origin-p_transform_b.origin).normalized() ))
+		return;
+
+	separator.generate_contacts();
+}
+
+static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
+	const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b);
+
+	SeparatorAxisTest<SphereShapeSW,BoxShapeSW> separator(sphere_A,p_transform_a,box_B,p_transform_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	// test faces
+
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis = p_transform_b.basis.get_axis(i).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+
+	}
+
+	// calculate closest point to sphere
+
+	Vector3 cnormal=p_transform_b.xform_inv( p_transform_a.origin );
+
+	Vector3 cpoint=p_transform_b.xform( Vector3(
+
+		(cnormal.x<0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x,
+		(cnormal.y<0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y,
+		(cnormal.z<0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z
+	) );
+
+	// use point to test axis
+	Vector3 point_axis = (p_transform_a.origin - cpoint).normalized();
+
+	if (!separator.test_axis( point_axis  ))
+		return;
+
+	// test edges
+
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis = point_axis.cross( p_transform_b.basis.get_axis(i) ).cross( p_transform_b.basis.get_axis(i) ).normalized();
+
+		if (!separator.test_axis( axis  ))
+			return;
+	}
+
+	separator.generate_contacts();
+
+
+}
+
+
+static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
+	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
+
+	SeparatorAxisTest<SphereShapeSW,CapsuleShapeSW> separator(sphere_A,p_transform_a,capsule_B,p_transform_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	//capsule sphere 1, sphere
+
+	Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5);
+
+	Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis;
+
+	if (!separator.test_axis( (capsule_ball_1 - p_transform_a.origin).normalized() ) )
+		return;
+
+	//capsule sphere 2, sphere
+
+	Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis;
+
+	if (!separator.test_axis( (capsule_ball_1 - p_transform_a.origin).normalized() ) )
+		return;
+
+	//capsule edge, sphere
+
+	Vector3 b2a = p_transform_a.origin - p_transform_b.origin;
+
+	Vector3 axis = b2a.cross( capsule_axis ).cross( capsule_axis ).normalized();
+
+
+	if (!separator.test_axis( axis ))
+		return;
+
+	separator.generate_contacts();
+}
+
+static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
+	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
+
+	SeparatorAxisTest<SphereShapeSW,ConvexPolygonShapeSW> separator(sphere_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector);
+
+
+	if (!separator.test_previous_axis())
+		return;
+
+	const Geometry::MeshData &mesh = convex_polygon_B->get_mesh();
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int face_count = mesh.faces.size();
+	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
+	int edge_count = mesh.edges.size();
+	const Vector3 *vertices = mesh.vertices.ptr();
+	int vertex_count = mesh.vertices.size();
+
+
+	// faces of B
+	for (int i=0;i<face_count;i++) {
+
+		Vector3 axis = p_transform_b.xform( faces[i].plane ).normal;
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+
+	// edges of B
+	for(int i=0;i<edge_count;i++) {
+
+
+		Vector3 v1=p_transform_b.xform( vertices[ edges[i].a ] );
+		Vector3 v2=p_transform_b.xform( vertices[ edges[i].b ] );
+		Vector3 v3=p_transform_a.origin;
+
+
+		Vector3 n1=v2-v1;
+		Vector3 n2=v2-v3;
+
+		Vector3 axis = n1.cross(n2).cross(n1).normalized();;
+
+		if (!separator.test_axis( axis ))
+			return;
+
+	}
+
+	// vertices of B
+	for(int i=0;i<vertex_count;i++) {
+
+
+		Vector3 v1=p_transform_b.xform( vertices[i] );
+		Vector3 v2=p_transform_a.origin;
+
+		Vector3 axis = (v2-v1).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+
+	}
+
+	separator.generate_contacts();
+
+
+}
+
+static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) {
+
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
+	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
+
+
+
+	SeparatorAxisTest<SphereShapeSW,FaceShapeSW> separator(sphere_A,p_transform_a,face_B,p_transform_b,p_collector);
+
+
+	Vector3 vertex[3]={
+		p_transform_b.xform( face_B->vertex[0] ),
+		p_transform_b.xform( face_B->vertex[1] ),
+		p_transform_b.xform( face_B->vertex[2] ),
+	};
+
+	if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() ))
+		return;
+
+	// edges and points of B
+	for(int i=0;i<3;i++) {
+
+
+		Vector3 n1=vertex[i]-p_transform_a.origin;
+
+		if (!separator.test_axis( n1.normalized() )) {
+			return;
+		}
+
+		Vector3 n2=vertex[(i+1)%3]-vertex[i];
+
+		Vector3 axis = n1.cross(n2).cross(n2).normalized();
+
+		if (!separator.test_axis( axis )) {
+			return;
+		}
+
+	}
+
+	separator.generate_contacts();
+}
+
+
+
+
+
+static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+
+	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
+	const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b);
+
+	SeparatorAxisTest<BoxShapeSW,BoxShapeSW> separator(box_A,p_transform_a,box_B,p_transform_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	// test faces of A
+
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+
+	}
+
+	// test faces of B
+
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis = p_transform_b.basis.get_axis(i).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+
+	}
+
+	// test combined edges
+	for (int i=0;i<3;i++) {
+
+		for (int j=0;j<3;j++) {
+
+			Vector3 axis = p_transform_a.basis.get_axis(i).cross( p_transform_b.basis.get_axis(j) );
+
+			if (axis.length_squared()<CMP_EPSILON)
+				continue;
+			axis.normalize();
+
+
+			if (!separator.test_axis( axis  )) {
+				return;
+			}
+		}
+	}
+
+	separator.generate_contacts();
+
+
+}
+
+
+static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
+	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
+
+	SeparatorAxisTest<BoxShapeSW,CapsuleShapeSW> separator(box_A,p_transform_a,capsule_B,p_transform_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	// faces of A
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis = p_transform_a.basis.get_axis(i);
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+
+	Vector3 cyl_axis = p_transform_b.basis.get_axis(2).normalized();
+
+	// edges of A, capsule cylinder
+
+	for (int i=0;i<3;i++) {
+
+		// cylinder
+		Vector3 box_axis = p_transform_a.basis.get_axis(i);
+		Vector3 axis = box_axis.cross( cyl_axis );
+		if (axis.length_squared() < CMP_EPSILON)
+			continue;
+
+		if (!separator.test_axis( axis.normalized() ))
+			return;
+	}
+
+	// points of A, capsule cylinder
+	// this sure could be made faster somehow..
+
+	for (int i=0;i<2;i++) {
+		for (int j=0;j<2;j++) {
+			for (int k=0;k<2;k++) {
+				Vector3 he = box_A->get_half_extents();
+				he.x*=(i*2-1);
+				he.y*=(j*2-1);
+				he.z*=(k*2-1);
+				Vector3 point=p_transform_a.origin;
+				for(int l=0;l<3;l++)
+					point+=p_transform_a.basis.get_axis(l)*he[l];
+
+				//Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized();
+				Vector3 axis = Plane(cyl_axis,0).project(point).normalized();
+
+				if (!separator.test_axis( axis ))
+					return;
+			}
+		}
+	}
+
+	// capsule balls, edges of A
+
+	for (int i=0;i<2;i++) {
+
+
+		Vector3 capsule_axis = p_transform_b.basis.get_axis(2)*(capsule_B->get_height()*0.5);
+
+		Vector3 sphere_pos = p_transform_b.origin + ((i==0)?capsule_axis:-capsule_axis);
+
+
+		Vector3 cnormal=p_transform_a.xform_inv( sphere_pos );
+
+		Vector3 cpoint=p_transform_a.xform( Vector3(
+
+			(cnormal.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
+			(cnormal.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
+			(cnormal.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z
+		) );
+
+		// use point to test axis
+		Vector3 point_axis = (sphere_pos - cpoint).normalized();
+
+		if (!separator.test_axis( point_axis  ))
+			return;
+
+		// test edges of A
+
+		for (int i=0;i<3;i++) {
+
+			Vector3 axis = point_axis.cross( p_transform_a.basis.get_axis(i) ).cross( p_transform_a.basis.get_axis(i) ).normalized();
+
+			if (!separator.test_axis( axis  ))
+				return;
+		}
+	}
+
+
+	separator.generate_contacts();
+}
+
+
+static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+
+
+	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
+	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
+
+	SeparatorAxisTest<BoxShapeSW,ConvexPolygonShapeSW> separator(box_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+
+	const Geometry::MeshData &mesh = convex_polygon_B->get_mesh();
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int face_count = mesh.faces.size();
+	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
+	int edge_count = mesh.edges.size();
+	const Vector3 *vertices = mesh.vertices.ptr();
+	int vertex_count = mesh.vertices.size();
+
+	// faces of A
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+	// faces of B
+	for (int i=0;i<face_count;i++) {
+
+		Vector3 axis = p_transform_b.xform( faces[i].plane ).normal;
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+	// A<->B edges
+	for (int i=0;i<3;i++) {
+
+		Vector3 e1 = p_transform_a.basis.get_axis(i);
+
+		for (int j=0;j<edge_count;j++) {
+
+			Vector3 e2=p_transform_b.basis.xform(vertices[edges[j].a]) -  p_transform_b.basis.xform(vertices[edges[j].b]);
+
+			Vector3 axis=e1.cross( e2 ).normalized();
+
+			if (!separator.test_axis( axis ))
+				return;
+
+		}
+	}
+
+	separator.generate_contacts();
+
+
+}
+
+static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) {
+
+
+	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
+	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
+
+	SeparatorAxisTest<BoxShapeSW,FaceShapeSW> separator(box_A,p_transform_a,face_B,p_transform_b,p_collector);
+
+	Vector3 vertex[3]={
+		p_transform_b.xform( face_B->vertex[0] ),
+		p_transform_b.xform( face_B->vertex[1] ),
+		p_transform_b.xform( face_B->vertex[2] ),
+	};
+
+	if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() ))
+		return;
+
+	// faces of A
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+	// combined edges
+	for(int i=0;i<3;i++) {
+
+		Vector3 e=vertex[i]-vertex[(i+1)%3];
+
+		for (int i=0;i<3;i++) {
+
+			Vector3 axis = p_transform_a.basis.get_axis(i);
+
+			if (!separator.test_axis( e.cross(axis).normalized() ))
+				return;
+		}
+
+	}
+
+	separator.generate_contacts();
+
+}
+
+static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
+	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
+
+	SeparatorAxisTest<CapsuleShapeSW,CapsuleShapeSW> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	// some values
+
+	Vector3 capsule_A_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5);
+	Vector3 capsule_B_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5);
+
+	Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis;
+	Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis;
+	Vector3 capsule_B_ball_1 = p_transform_b.origin + capsule_B_axis;
+	Vector3 capsule_B_ball_2 = p_transform_b.origin - capsule_B_axis;
+
+	//balls-balls
+
+	if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_1 ).normalized() ) )
+		return;
+	if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_2 ).normalized() ) )
+		return;
+
+	if (!separator.test_axis( (capsule_A_ball_2 - capsule_B_ball_1 ).normalized() ) )
+		return;
+	if (!separator.test_axis( (capsule_A_ball_2 - capsule_B_ball_2 ).normalized() ) )
+		return;
+
+
+	// edges-balls
+
+	if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_1 ).cross(capsule_A_axis).cross(capsule_A_axis).normalized() ) )
+		return;
+
+	if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_2 ).cross(capsule_A_axis).cross(capsule_A_axis).normalized() ) )
+		return;
+
+	if (!separator.test_axis( (capsule_B_ball_1 - capsule_A_ball_1 ).cross(capsule_B_axis).cross(capsule_B_axis).normalized() ) )
+		return;
+
+	if (!separator.test_axis( (capsule_B_ball_1 - capsule_A_ball_2 ).cross(capsule_B_axis).cross(capsule_B_axis).normalized() ) )
+		return;
+
+	// edges
+
+	if (!separator.test_axis( capsule_A_axis.cross(capsule_B_axis).normalized() ) )
+		return;
+
+
+	separator.generate_contacts();
+
+}
+
+static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+
+	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
+	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
+
+	SeparatorAxisTest<CapsuleShapeSW,ConvexPolygonShapeSW> separator(capsule_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	const Geometry::MeshData &mesh = convex_polygon_B->get_mesh();
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int face_count = mesh.faces.size();
+	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
+	int edge_count = mesh.edges.size();
+	const Vector3 *vertices = mesh.vertices.ptr();
+	int vertex_count = mesh.vertices.size();
+
+	// faces of B
+	for (int i=0;i<face_count;i++) {
+
+		Vector3 axis = p_transform_b.xform( faces[i].plane ).normal;
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+	// edges of B, capsule cylinder
+
+	for (int i=0;i<edge_count;i++) {
+
+		// cylinder
+		Vector3 edge_axis = p_transform_b.basis.xform( vertices[ edges[i].a] ) - p_transform_b.basis.xform( vertices[ edges[i].b] );
+		Vector3 axis = edge_axis.cross( p_transform_a.basis.get_axis(2) ).normalized();
+
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+	// capsule balls, edges of B
+
+	for (int i=0;i<2;i++) {
+
+		// edges of B, capsule cylinder
+
+		Vector3 capsule_axis = p_transform_a.basis.get_axis(2)*(capsule_A->get_height()*0.5);
+
+		Vector3 sphere_pos = p_transform_a.origin + ((i==0)?capsule_axis:-capsule_axis);
+
+		for (int j=0;j<edge_count;j++) {
+
+
+			Vector3 n1=sphere_pos - p_transform_b.xform( vertices[ edges[j].a] );
+			Vector3 n2=p_transform_b.basis.xform( vertices[ edges[j].a] ) - p_transform_b.basis.xform( vertices[ edges[j].b] );
+
+			Vector3 axis = n1.cross(n2).cross(n2).normalized();
+
+			if (!separator.test_axis( axis ))
+				return;
+		}
+	}
+
+
+	separator.generate_contacts();
+
+}
+
+static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) {
+
+	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
+	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
+
+	SeparatorAxisTest<CapsuleShapeSW,FaceShapeSW> separator(capsule_A,p_transform_a,face_B,p_transform_b,p_collector);
+
+
+
+	Vector3 vertex[3]={
+		p_transform_b.xform( face_B->vertex[0] ),
+		p_transform_b.xform( face_B->vertex[1] ),
+		p_transform_b.xform( face_B->vertex[2] ),
+	};
+
+	if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() ))
+		return;
+
+	// edges of B, capsule cylinder
+
+	Vector3 capsule_axis = p_transform_a.basis.get_axis(2)*(capsule_A->get_height()*0.5);
+
+	for (int i=0;i<3;i++) {
+
+		// edge-cylinder
+		Vector3 edge_axis = vertex[i]-vertex[(i+1)%3];
+		Vector3 axis = edge_axis.cross( capsule_axis ).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+
+		if (!separator.test_axis( (p_transform_a.origin-vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized() ))
+			return;
+
+		for (int j=0;j<2;j++) {
+
+			// point-spheres
+			Vector3 sphere_pos = p_transform_a.origin + ( (j==0) ? capsule_axis : -capsule_axis );
+
+			Vector3 n1=sphere_pos - vertex[i];
+
+			if (!separator.test_axis( n1.normalized() ))
+				return;
+
+			Vector3 n2=edge_axis;
+
+			axis = n1.cross(n2).cross(n2);
+
+			if (!separator.test_axis( axis.normalized() ))
+				return;
+
+
+		}
+
+	}
+
+
+	separator.generate_contacts();
+
+}
+
+
+static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+
+	const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a);
+	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
+
+	SeparatorAxisTest<ConvexPolygonShapeSW,ConvexPolygonShapeSW> separator(convex_polygon_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	const Geometry::MeshData &mesh_A = convex_polygon_A->get_mesh();
+
+	const Geometry::MeshData::Face *faces_A = mesh_A.faces.ptr();
+	int face_count_A = mesh_A.faces.size();
+	const Geometry::MeshData::Edge *edges_A = mesh_A.edges.ptr();
+	int edge_count_A = mesh_A.edges.size();
+	const Vector3 *vertices_A = mesh_A.vertices.ptr();
+	int vertex_count_A = mesh_A.vertices.size();
+
+	const Geometry::MeshData &mesh_B = convex_polygon_B->get_mesh();
+
+	const Geometry::MeshData::Face *faces_B = mesh_B.faces.ptr();
+	int face_count_B = mesh_B.faces.size();
+	const Geometry::MeshData::Edge *edges_B = mesh_B.edges.ptr();
+	int edge_count_B = mesh_B.edges.size();
+	const Vector3 *vertices_B = mesh_B.vertices.ptr();
+	int vertex_count_B = mesh_B.vertices.size();
+
+	// faces of A
+	for (int i=0;i<face_count_A;i++) {
+
+		Vector3 axis = p_transform_a.xform( faces_A[i].plane ).normal;
+//		Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+	// faces of B
+	for (int i=0;i<face_count_B;i++) {
+
+		Vector3 axis = p_transform_b.xform( faces_B[i].plane ).normal;
+//		Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized();
+
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+	// A<->B edges
+	for (int i=0;i<edge_count_A;i++) {
+
+		Vector3 e1=p_transform_a.basis.xform( vertices_A[ edges_A[i].a] ) -p_transform_a.basis.xform( vertices_A[ edges_A[i].b] );
+
+		for (int j=0;j<edge_count_B;j++) {
+
+			Vector3 e2=p_transform_b.basis.xform( vertices_B[ edges_B[j].a] ) -p_transform_b.basis.xform( vertices_B[ edges_B[j].b] );
+
+			Vector3 axis=e1.cross( e2 ).normalized();
+
+			if (!separator.test_axis( axis ))
+				return;
+
+		}
+	}
+
+	separator.generate_contacts();
+
+}
+
+static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) {
+
+
+	const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a);
+	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
+
+	SeparatorAxisTest<ConvexPolygonShapeSW,FaceShapeSW> separator(convex_polygon_A,p_transform_a,face_B,p_transform_b,p_collector);
+
+	const Geometry::MeshData &mesh = convex_polygon_A->get_mesh();
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int face_count = mesh.faces.size();
+	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
+	int edge_count = mesh.edges.size();
+	const Vector3 *vertices = mesh.vertices.ptr();
+	int vertex_count = mesh.vertices.size();
+
+
+
+	Vector3 vertex[3]={
+		p_transform_b.xform( face_B->vertex[0] ),
+		p_transform_b.xform( face_B->vertex[1] ),
+		p_transform_b.xform( face_B->vertex[2] ),
+	};
+
+	if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() ))
+		return;
+
+
+	// faces of A
+	for (int i=0;i<face_count;i++) {
+
+//		Vector3 axis = p_transform_a.xform( faces[i].plane ).normal;
+		Vector3 axis = p_transform_a.basis.xform( faces[i].plane.normal ).normalized();
+
+		if (!separator.test_axis( axis ))
+			return;
+	}
+
+
+	// A<->B edges
+	for (int i=0;i<edge_count;i++) {
+
+		Vector3 e1=p_transform_a.xform( vertices[edges[i].a] ) - p_transform_a.xform( vertices[edges[i].b] );
+
+		for (int j=0;j<3;j++) {
+
+			Vector3 e2=vertex[j]-vertex[(j+1)%3];
+
+			Vector3 axis=e1.cross( e2 ).normalized();
+
+			if (!separator.test_axis( axis ))
+				return;
+		}
+	}
+
+	separator.generate_contacts();
+
+}
+
+
+bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata,bool p_swap,Vector3* r_prev_axis) {
+
+	PhysicsServer::ShapeType type_A=p_shape_A->get_type();
+
+	ERR_FAIL_COND_V(type_A==PhysicsServer::SHAPE_PLANE,false);
+	ERR_FAIL_COND_V(type_A==PhysicsServer::SHAPE_RAY,false);
+	ERR_FAIL_COND_V(p_shape_A->is_concave(),false);
+
+	PhysicsServer::ShapeType type_B=p_shape_B->get_type();
+
+	ERR_FAIL_COND_V(type_B==PhysicsServer::SHAPE_PLANE,false);
+	ERR_FAIL_COND_V(type_B==PhysicsServer::SHAPE_RAY,false);
+	ERR_FAIL_COND_V(p_shape_B->is_concave(),false);
+
+
+	static const CollisionFunc collision_table[5][5]={
+		{_collision_sphere_sphere,
+		 _collision_sphere_box,
+		 _collision_sphere_capsule,
+		 _collision_sphere_convex_polygon,
+		 _collision_sphere_face},
+		{0,
+		 _collision_box_box,
+		 _collision_box_capsule,
+		 _collision_box_convex_polygon,
+		 _collision_box_face},
+		{0,
+		 0,
+		 _collision_capsule_capsule,
+		 _collision_capsule_convex_polygon,
+		 _collision_capsule_face},
+		{0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon,
+		 _collision_convex_polygon_face},
+		{0,
+		 0,
+		 0,
+		 0,
+		 0},
+	};
+
+	_CollectorCallback callback;
+	callback.callback=p_result_callback;
+	callback.swap=p_swap;
+	callback.userdata=p_userdata;
+	callback.collided=false;
+	callback.prev_axis=r_prev_axis;
+
+	const ShapeSW *A=p_shape_A;
+	const ShapeSW *B=p_shape_B;
+	const Transform *transform_A=&p_transform_A;
+	const Transform *transform_B=&p_transform_B;
+
+	if (type_A > type_B) {
+		SWAP(A,B);
+		SWAP(transform_A,transform_B);
+		SWAP(type_A,type_B);
+		callback.swap = !callback.swap;
+	}
+
+
+	CollisionFunc collision_func = collision_table[type_A-2][type_B-2];
+	ERR_FAIL_COND_V(!collision_func,false);
+
+
+	collision_func(A,*transform_A,B,*transform_B,&callback);
+
+	return callback.collided;
+
+}
diff --git a/servers/physics/collision_solver_sat.h b/servers/physics/collision_solver_sat.h
new file mode 100644
index 000000000..5023a1781
--- /dev/null
+++ b/servers/physics/collision_solver_sat.h
@@ -0,0 +1,37 @@
+/*************************************************************************/
+/*  collision_solver_sat.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 COLLISION_SOLVER_SAT_H
+#define COLLISION_SOLVER_SAT_H
+
+#include "collision_solver_sw.h"
+
+
+bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL);
+
+#endif // COLLISION_SOLVER_SAT_H
diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp
new file mode 100644
index 000000000..da28a4934
--- /dev/null
+++ b/servers/physics/collision_solver_sw.cpp
@@ -0,0 +1,241 @@
+/*************************************************************************/
+/*  collision_solver_sw.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 "collision_solver_sw.h"
+#include "collision_solver_sat.h"
+
+#include "gjk_epa.h"
+#include "collision_solver_sat.h"
+
+
+#define collision_solver sat_calculate_penetration
+//#define collision_solver gjk_epa_calculate_penetration
+
+
+bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+	const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A);
+	if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE)
+		return false;
+	Plane p = p_transform_A.xform(plane->get_plane());
+
+	static const int max_supports = 16;
+	Vector3 supports[max_supports];
+	int support_count;
+
+	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count);
+
+	bool found=false;
+
+	for(int i=0;i<support_count;i++) {
+
+		supports[i] = p_transform_B.xform( supports[i] );
+		if (p.distance_to(supports[i])>=0)
+			continue;
+		found=true;
+
+		Vector3 support_A = p.project(supports[i]);
+
+		if (p_result_callback) {
+			if (p_swap_result)
+				p_result_callback(supports[i],support_A,p_userdata);
+			else
+				p_result_callback(support_A,supports[i],p_userdata);
+		}
+
+	}
+
+
+	return found;
+}
+
+bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+
+	const RayShapeSW *ray = static_cast<const RayShapeSW*>(p_shape_A);
+
+	Vector3 from = p_transform_A.origin;
+	Vector3 to = from+p_transform_A.basis.get_axis(2)*ray->get_length();
+	Vector3 support_A=to;
+
+	Transform ai = p_transform_B.affine_inverse();
+
+	from = ai.xform(from);
+	to = ai.xform(to);
+
+	Vector3 p,n;
+	if (!p_shape_B->intersect_segment(from,to,p,n))
+		return false;
+
+	Vector3 support_B=p_transform_B.xform(p);
+
+	if (p_result_callback) {
+		if (p_swap_result)
+			p_result_callback(support_B,support_A,p_userdata);
+		else
+			p_result_callback(support_A,support_B,p_userdata);
+	}
+	return true;
+}
+
+struct _ConcaveCollisionInfo {
+
+	const Transform *transform_A;
+	const ShapeSW *shape_A;
+	const Transform *transform_B;
+	CollisionSolverSW::CallbackResult result_callback;
+	void *userdata;
+	bool swap_result;
+	bool collided;
+	int aabb_tests;
+	int collisions;
+
+};
+
+void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
+
+
+	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata);
+	cinfo.aabb_tests++;
+
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result );
+	if (!collided)
+		return;
+
+	cinfo.collided=true;
+	cinfo.collisions++;
+
+}
+
+bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+
+	const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B);
+
+	_ConcaveCollisionInfo cinfo;
+	cinfo.transform_A=&p_transform_A;
+	cinfo.shape_A=p_shape_A;
+	cinfo.transform_B=&p_transform_B;
+	cinfo.result_callback=p_result_callback;
+	cinfo.userdata=p_userdata;
+	cinfo.swap_result=p_swap_result;
+	cinfo.collided=false;
+	cinfo.collisions=0;
+
+	cinfo.aabb_tests=0;
+
+	Transform rel_transform = p_transform_A;
+	rel_transform.origin-=p_transform_B.origin;
+
+	//quickly compute a local AABB
+
+	AABB local_aabb;
+	for(int i=0;i<3;i++) {
+
+	     Vector3 axis( p_transform_B.basis.get_axis(i) );
+	     float axis_scale = 1.0/axis.length();
+	     axis*=axis_scale;
+
+	     float smin,smax;
+	     p_shape_A->project_range(axis,rel_transform,smin,smax);
+	     smin*=axis_scale;
+	     smax*=axis_scale;
+
+	     local_aabb.pos[i]=smin;
+	     local_aabb.size[i]=smax-smin;
+	}
+
+	concave_B->cull(local_aabb,concave_callback,&cinfo);
+
+
+	return cinfo.collided;
+}
+
+
+bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis) {
+
+
+	PhysicsServer::ShapeType type_A=p_shape_A->get_type();
+	PhysicsServer::ShapeType type_B=p_shape_B->get_type();
+	bool concave_A=p_shape_A->is_concave();
+	bool concave_B=p_shape_B->is_concave();
+
+	bool swap = false;
+
+	if (type_A>type_B) {
+		SWAP(type_A,type_B);
+		SWAP(concave_A,concave_B);
+		swap=true;
+	}
+
+	if (type_A==PhysicsServer::SHAPE_PLANE) {
+
+		if (type_B==PhysicsServer::SHAPE_PLANE)
+			return false;
+		if (type_B==PhysicsServer::SHAPE_RAY) {
+			return false;
+		}
+
+		if (swap) {
+			return solve_static_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+		} else {
+			return solve_static_plane(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+		}
+
+	} else if (type_A==PhysicsServer::SHAPE_RAY) {
+
+		if (type_B==PhysicsServer::SHAPE_RAY)
+			return false;
+
+		if (swap) {
+			return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+		} else {
+			return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+		}
+
+	} else if (concave_B) {
+
+
+		if (concave_A)
+			return false;
+
+		if (!swap)
+			return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+		else
+			return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+
+
+
+	} else {
+
+		return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis);
+	}
+
+
+	return false;
+}
diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h
new file mode 100644
index 000000000..e135ab92e
--- /dev/null
+++ b/servers/physics/collision_solver_sw.h
@@ -0,0 +1,52 @@
+/*************************************************************************/
+/*  collision_solver_sw.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 COLLISION_SOLVER_SW_H
+#define COLLISION_SOLVER_SW_H
+
+#include "shape_sw.h"
+
+class CollisionSolverSW
+{
+public:
+	typedef void (*CallbackResult)(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata);
+private:
+
+	static void concave_callback(void *p_userdata, ShapeSW *p_convex);
+	static bool solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+	static bool solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+	static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+
+public:
+
+
+	static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL);
+
+};
+
+#endif // COLLISION_SOLVER__SW_H
diff --git a/servers/physics/constraint_sw.cpp b/servers/physics/constraint_sw.cpp
new file mode 100644
index 000000000..f1179bdb5
--- /dev/null
+++ b/servers/physics/constraint_sw.cpp
@@ -0,0 +1,30 @@
+/*************************************************************************/
+/*  constraint_sw.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 "constraint_sw.h"
+
diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h
new file mode 100644
index 000000000..1be96422e
--- /dev/null
+++ b/servers/physics/constraint_sw.h
@@ -0,0 +1,72 @@
+/*************************************************************************/
+/*  constraint_sw.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 CONSTRAINT_SW_H
+#define CONSTRAINT_SW_H
+
+#include "body_sw.h"
+
+class ConstraintSW {
+
+	BodySW **_body_ptr;
+	int _body_count;
+	uint64_t island_step;
+	ConstraintSW *island_next;
+	ConstraintSW *island_list_next;
+
+
+	RID self;
+
+protected:
+	ConstraintSW(BodySW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; }
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+
+
+	_FORCE_INLINE_ ConstraintSW* get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(ConstraintSW* p_next) { island_next=p_next; }
+
+	_FORCE_INLINE_ ConstraintSW* get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(ConstraintSW* p_next) { island_list_next=p_next; }
+
+	_FORCE_INLINE_ BodySW **get_body_ptr() const { return _body_ptr; }
+	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
+
+
+	virtual bool setup(float p_step)=0;
+	virtual void solve(float p_step)=0;
+
+	virtual ~ConstraintSW() {}
+};
+
+#endif // CONSTRAINT__SW_H
diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp
new file mode 100644
index 000000000..37edc0d41
--- /dev/null
+++ b/servers/physics/gjk_epa.cpp
@@ -0,0 +1,900 @@
+/*************************************************************************/
+/*  gjk_epa.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 "gjk_epa.h"
+
+/*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/
+
+	// Config
+
+/* GJK	*/ 
+#define GJK_MAX_ITERATIONS	128
+#define GJK_ACCURARY		((real_t)0.0001)
+#define GJK_MIN_DISTANCE	((real_t)0.0001)
+#define GJK_DUPLICATED_EPS	((real_t)0.0001)
+#define GJK_SIMPLEX2_EPS	((real_t)0.0)
+#define GJK_SIMPLEX3_EPS	((real_t)0.0)
+#define GJK_SIMPLEX4_EPS	((real_t)0.0)
+
+/* EPA	*/ 
+#define EPA_MAX_VERTICES	64
+#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
+#define EPA_MAX_ITERATIONS	255
+#define EPA_ACCURACY		((real_t)0.0001)
+#define EPA_FALLBACK		(10*EPA_ACCURACY)
+#define EPA_PLANE_EPS		((real_t)0.00001)
+#define EPA_INSIDE_EPS		((real_t)0.01)
+
+namespace GjkEpa2 {
+
+
+struct sResults	{
+	enum eStatus {
+		Separated,		/* Shapes doesnt penetrate */ 
+		Penetrating,	/* Shapes are penetrating */ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+	} status;
+		
+	Vector3	witnesses[2];
+	Vector3	normal;
+	real_t	distance;
+};
+
+// Shorthands
+typedef unsigned int	U;
+typedef unsigned char	U1;
+
+// MinkowskiDiff
+struct	MinkowskiDiff {
+
+	const ShapeSW* m_shapes[2];
+
+	Transform transform_A;
+	Transform transform_B;
+
+	// i wonder how this could be sped up... if it can
+	_FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const {
+		return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) );
+	}
+	
+	_FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const {
+		return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) );
+	}
+	
+	_FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const {
+		return ( Support0 ( d )-Support1 ( -d ) );
+	}
+	
+	_FORCE_INLINE_ Vector3	Support ( const Vector3& d,U index ) const
+	{
+		if ( index )
+			return ( Support1 ( d ) );
+		else
+			return ( Support0 ( d ) );
+	}
+};
+
+typedef	MinkowskiDiff tShape;
+
+
+// GJK
+struct	GJK
+{
+	/* Types		*/ 
+	struct	sSV
+	{
+		Vector3	d,w;
+	};
+	struct	sSimplex
+	{
+		sSV*		c[4];
+		real_t	p[4];
+		U			rank;
+	};
+	struct	eStatus	{ enum _ {
+		Valid,
+		Inside,
+		Failed		};};
+		/* Fields		*/ 
+		tShape			m_shape;
+		Vector3		m_ray;
+		real_t		m_distance;
+		sSimplex		m_simplices[2];
+		sSV				m_store[4];
+		sSV*			m_free[4];
+		U				m_nfree;
+		U				m_current;
+		sSimplex*		m_simplex;
+		eStatus::_		m_status;
+		/* Methods		*/ 
+		GJK()
+		{
+			Initialize();
+		}
+		void				Initialize()
+		{
+			m_ray		=	Vector3(0,0,0);
+			m_nfree		=	0;
+			m_status	=	eStatus::Failed;
+			m_current	=	0;
+			m_distance	=	0;
+		}
+		eStatus::_			Evaluate(const tShape& shapearg,const Vector3& guess)
+		{
+			U			iterations=0;
+			real_t	sqdist=0;
+			real_t	alpha=0;
+			Vector3	lastw[4];
+			U			clastw=0;
+			/* Initialize solver		*/ 
+			m_free[0]			=	&m_store[0];
+			m_free[1]			=	&m_store[1];
+			m_free[2]			=	&m_store[2];
+			m_free[3]			=	&m_store[3];
+			m_nfree				=	4;
+			m_current			=	0;
+			m_status			=	eStatus::Valid;
+			m_shape				=	shapearg;
+			m_distance			=	0;
+			/* Initialize simplex		*/ 
+			m_simplices[0].rank	=	0;
+			m_ray				=	guess;
+			const real_t	sqrl=	m_ray.length_squared();
+			appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0));
+			m_simplices[0].p[0]	=	1;
+			m_ray				=	m_simplices[0].c[0]->w;	
+			sqdist				=	sqrl;
+			lastw[0]			=
+				lastw[1]			=
+				lastw[2]			=
+				lastw[3]			=	m_ray;
+			/* Loop						*/ 
+			do	{
+				const U		next=1-m_current;
+				sSimplex&	cs=m_simplices[m_current];
+				sSimplex&	ns=m_simplices[next];
+				/* Check zero							*/ 
+				const real_t	rl=m_ray.length();
+				if(rl<GJK_MIN_DISTANCE)
+				{/* Touching or inside				*/ 
+					m_status=eStatus::Inside;
+					break;
+				}
+				/* Append new vertice in -'v' direction	*/ 
+				appendvertice(cs,-m_ray);
+				const Vector3&	w=cs.c[cs.rank-1]->w;
+				bool				found=false;
+				for(U i=0;i<4;++i)
+				{
+					if((w-lastw[i]).length_squared()<GJK_DUPLICATED_EPS)
+					{ found=true;break; }
+				}
+				if(found)
+				{/* Return old simplex				*/ 
+					removevertice(m_simplices[m_current]);
+					break;
+				}
+				else
+				{/* Update lastw					*/ 
+					lastw[clastw=(clastw+1)&3]=w;
+				}
+				/* Check for termination				*/ 
+				const real_t	omega=vec3_dot(m_ray,w)/rl;
+				alpha=MAX(omega,alpha);
+				if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+				{/* Return old simplex				*/ 
+					removevertice(m_simplices[m_current]);
+					break;
+				}		
+				/* Reduce simplex						*/ 
+				real_t	weights[4];
+				U			mask=0;
+				switch(cs.rank)
+				{
+				case	2:	sqdist=projectorigin(	cs.c[0]->w,
+								cs.c[1]->w,
+								weights,mask);break;
+				case	3:	sqdist=projectorigin(	cs.c[0]->w,
+								cs.c[1]->w,
+								cs.c[2]->w,
+								weights,mask);break;
+				case	4:	sqdist=projectorigin(	cs.c[0]->w,
+								cs.c[1]->w,
+								cs.c[2]->w,
+								cs.c[3]->w,
+								weights,mask);break;
+				}
+				if(sqdist>=0)
+				{/* Valid	*/ 
+					ns.rank		=	0;
+					m_ray		=	Vector3(0,0,0);
+					m_current	=	next;
+					for(U i=0,ni=cs.rank;i<ni;++i)
+					{
+						if(mask&(1<<i))
+						{
+							ns.c[ns.rank]		=	cs.c[i];
+							ns.p[ns.rank++]		=	weights[i];
+							m_ray				+=	cs.c[i]->w*weights[i];
+						}
+						else
+						{
+							m_free[m_nfree++]	=	cs.c[i];
+						}
+					}
+					if(mask==15) m_status=eStatus::Inside;
+				}
+				else
+				{/* Return old simplex				*/ 
+					removevertice(m_simplices[m_current]);
+					break;
+				}
+				m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
+			} while(m_status==eStatus::Valid);
+			m_simplex=&m_simplices[m_current];
+			switch(m_status)
+			{
+			case	eStatus::Valid:		m_distance=m_ray.length();break;
+			case	eStatus::Inside:	m_distance=0;break;
+			default: {}
+			}	
+			return(m_status);
+		}
+		bool					EncloseOrigin()
+		{
+			switch(m_simplex->rank)
+			{
+			case	1:
+				{
+					for(U i=0;i<3;++i)
+					{
+						Vector3		axis=Vector3(0,0,0);
+						axis[i]=1;
+						appendvertice(*m_simplex, axis);
+						if(EncloseOrigin())	return(true);
+						removevertice(*m_simplex);
+						appendvertice(*m_simplex,-axis);
+						if(EncloseOrigin())	return(true);
+						removevertice(*m_simplex);
+					}
+				}
+				break;
+			case	2:
+				{
+					const Vector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
+					for(U i=0;i<3;++i)
+					{
+						Vector3		axis=Vector3(0,0,0);
+						axis[i]=1;
+						const Vector3	p=vec3_cross(d,axis);
+						if(p.length_squared()>0)
+						{
+							appendvertice(*m_simplex, p);
+							if(EncloseOrigin())	return(true);
+							removevertice(*m_simplex);
+							appendvertice(*m_simplex,-p);
+							if(EncloseOrigin())	return(true);
+							removevertice(*m_simplex);
+						}
+					}
+				}
+				break;
+			case	3:
+				{
+					const Vector3	n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+						m_simplex->c[2]->w-m_simplex->c[0]->w);
+					if(n.length_squared()>0)
+					{
+						appendvertice(*m_simplex,n);
+						if(EncloseOrigin())	return(true);
+						removevertice(*m_simplex);
+						appendvertice(*m_simplex,-n);
+						if(EncloseOrigin())	return(true);
+						removevertice(*m_simplex);
+					}
+				}
+				break;
+			case	4:
+				{
+					if(Math::abs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
+						m_simplex->c[1]->w-m_simplex->c[3]->w,
+						m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
+						return(true);
+				}
+				break;
+			}
+			return(false);
+		}
+		/* Internals	*/ 
+		void				getsupport(const Vector3& d,sSV& sv) const
+		{
+			sv.d	=	d/d.length();
+			sv.w	=	m_shape.Support(sv.d);
+		}
+		void				removevertice(sSimplex& simplex)
+		{
+			m_free[m_nfree++]=simplex.c[--simplex.rank];
+		}
+		void				appendvertice(sSimplex& simplex,const Vector3& v)
+		{
+			simplex.p[simplex.rank]=0;
+			simplex.c[simplex.rank]=m_free[--m_nfree];
+			getsupport(v,*simplex.c[simplex.rank++]);
+		}
+		static real_t		det(const Vector3& a,const Vector3& b,const Vector3& c)
+		{
+			return(	a.y*b.z*c.x+a.z*b.x*c.y-
+				a.x*b.z*c.y-a.y*b.x*c.z+
+				a.x*b.y*c.z-a.z*b.y*c.x);
+		}
+		static real_t		projectorigin(	const Vector3& a,
+			const Vector3& b,
+			real_t* w,U& m)
+		{
+			const Vector3	d=b-a;
+			const real_t	l=d.length_squared();
+			if(l>GJK_SIMPLEX2_EPS)
+			{
+				const real_t	t(l>0?-vec3_dot(a,d)/l:0);
+				if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length_squared()); }
+				else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length_squared()); }
+				else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); }
+			}
+			return(-1);
+		}
+		static real_t		projectorigin(	const Vector3& a,
+			const Vector3& b,
+			const Vector3& c,
+			real_t* w,U& m)
+		{
+			static const U		imd3[]={1,2,0};
+			const Vector3*	vt[]={&a,&b,&c};
+			const Vector3		dl[]={a-b,b-c,c-a};
+			const Vector3		n=vec3_cross(dl[0],dl[1]);
+			const real_t		l=n.length_squared();
+			if(l>GJK_SIMPLEX3_EPS)
+			{
+				real_t	mindist=-1;
+				real_t	subw[2];
+				U			subm;
+				for(U i=0;i<3;++i)
+				{
+					if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0)
+					{
+						const U			j=imd3[i];
+						const real_t	subd(projectorigin(*vt[i],*vt[j],subw,subm));
+						if((mindist<0)||(subd<mindist))
+						{
+							mindist		=	subd;
+							m			=	static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
+							w[i]		=	subw[0];
+							w[j]		=	subw[1];
+							w[imd3[j]]	=	0;				
+						}
+					}
+				}
+				if(mindist<0)
+				{
+					const real_t	d=vec3_dot(a,n);	
+					const real_t	s=Math::sqrt(l);
+					const Vector3	p=n*(d/l);
+					mindist	=	p.length_squared();
+					m		=	7;
+					w[0]	=	(vec3_cross(dl[1],b-p)).length()/s;
+					w[1]	=	(vec3_cross(dl[2],c-p)).length()/s;
+					w[2]	=	1-(w[0]+w[1]);
+				}
+				return(mindist);
+			}
+			return(-1);
+		}
+		static real_t		projectorigin(	const Vector3& a,
+			const Vector3& b,
+			const Vector3& c,
+			const Vector3& d,
+			real_t* w,U& m)
+		{
+			static const U		imd3[]={1,2,0};
+			const Vector3*	vt[]={&a,&b,&c,&d};
+			const Vector3		dl[]={a-d,b-d,c-d};
+			const real_t		vl=det(dl[0],dl[1],dl[2]);
+			const bool			ng=(vl*vec3_dot(a,vec3_cross(b-c,a-b)))<=0;
+			if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS))
+			{
+				real_t	mindist=-1;
+				real_t	subw[3];
+				U			subm;
+				for(U i=0;i<3;++i)
+				{
+					const U			j=imd3[i];
+					const real_t	s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j]));
+					if(s>0)
+					{
+						const real_t	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
+						if((mindist<0)||(subd<mindist))
+						{
+							mindist		=	subd;
+							m			=	static_cast<U>((subm&1?1<<i:0)+
+								(subm&2?1<<j:0)+
+								(subm&4?8:0));
+							w[i]		=	subw[0];
+							w[j]		=	subw[1];
+							w[imd3[j]]	=	0;
+							w[3]		=	subw[2];
+						}
+					}
+				}
+				if(mindist<0)
+				{
+					mindist	=	0;
+					m		=	15;
+					w[0]	=	det(c,b,d)/vl;
+					w[1]	=	det(a,c,d)/vl;
+					w[2]	=	det(b,a,d)/vl;
+					w[3]	=	1-(w[0]+w[1]+w[2]);
+				}
+				return(mindist);
+			}
+			return(-1);
+		}
+};
+
+	// EPA
+	struct	EPA
+	{
+		/* Types		*/ 
+		typedef	GJK::sSV	sSV;
+		struct	sFace
+		{
+			Vector3	n;
+			real_t	d;
+			real_t	p;
+			sSV*		c[3];
+			sFace*		f[3];
+			sFace*		l[2];
+			U1			e[3];
+			U1			pass;
+		};
+		struct	sList
+		{
+			sFace*		root;
+			U			count;
+			sList() : root(0),count(0)	{}
+		};
+		struct	sHorizon
+		{
+			sFace*		cf;
+			sFace*		ff;
+			U			nf;
+			sHorizon() : cf(0),ff(0),nf(0)	{}
+		};
+		struct	eStatus { enum _ {
+			Valid,
+			Touching,
+			Degenerated,
+			NonConvex,
+			InvalidHull,		
+			OutOfFaces,
+			OutOfVertices,
+			AccuraryReached,
+			FallBack,
+			Failed		};};
+			/* Fields		*/ 
+			eStatus::_		m_status;
+			GJK::sSimplex	m_result;
+			Vector3		m_normal;
+			real_t		m_depth;
+			sSV				m_sv_store[EPA_MAX_VERTICES];
+			sFace			m_fc_store[EPA_MAX_FACES];
+			U				m_nextsv;
+			sList			m_hull;
+			sList			m_stock;
+			/* Methods		*/ 
+			EPA()
+			{
+				Initialize();	
+			}
+
+
+			static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
+			{
+				fa->e[ea]=(U1)eb;fa->f[ea]=fb;
+				fb->e[eb]=(U1)ea;fb->f[eb]=fa;
+			}
+			static inline void		append(sList& list,sFace* face)
+			{
+				face->l[0]	=	0;
+				face->l[1]	=	list.root;
+				if(list.root) list.root->l[0]=face;
+				list.root	=	face;
+				++list.count;
+			}
+			static inline void		remove(sList& list,sFace* face)
+			{
+				if(face->l[1]) face->l[1]->l[0]=face->l[0];
+				if(face->l[0]) face->l[0]->l[1]=face->l[1];
+				if(face==list.root) list.root=face->l[1];
+				--list.count;
+			}
+
+
+			void				Initialize()
+			{
+				m_status	=	eStatus::Failed;
+				m_normal	=	Vector3(0,0,0);
+				m_depth		=	0;
+				m_nextsv	=	0;
+				for(U i=0;i<EPA_MAX_FACES;++i)
+				{
+					append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
+				}
+			}
+			eStatus::_			Evaluate(GJK& gjk,const Vector3& guess)
+			{
+				GJK::sSimplex&	simplex=*gjk.m_simplex;
+				if((simplex.rank>1)&&gjk.EncloseOrigin())
+				{
+
+					/* Clean up				*/ 
+					while(m_hull.root)
+					{
+						sFace*	f = m_hull.root;
+						remove(m_hull,f);
+						append(m_stock,f);
+					}
+					m_status	=	eStatus::Valid;
+					m_nextsv	=	0;
+					/* Orient simplex		*/ 
+					if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
+						simplex.c[1]->w-simplex.c[3]->w,
+						simplex.c[2]->w-simplex.c[3]->w)<0)
+					{
+						SWAP(simplex.c[0],simplex.c[1]);
+						SWAP(simplex.p[0],simplex.p[1]);
+					}
+					/* Build initial hull	*/ 
+					sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
+						newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
+						newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
+						newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
+					if(m_hull.count==4)
+					{
+						sFace*		best=findbest();
+						sFace		outer=*best;
+						U			pass=0;
+						U			iterations=0;
+						bind(tetra[0],0,tetra[1],0);
+						bind(tetra[0],1,tetra[2],0);
+						bind(tetra[0],2,tetra[3],0);
+						bind(tetra[1],1,tetra[3],2);
+						bind(tetra[1],2,tetra[2],1);
+						bind(tetra[2],2,tetra[3],1);
+						m_status=eStatus::Valid;
+						for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+						{
+							if(m_nextsv<EPA_MAX_VERTICES)
+							{	
+								sHorizon		horizon;
+								sSV*			w=&m_sv_store[m_nextsv++];
+								bool			valid=true;					
+								best->pass	=	(U1)(++pass);
+								gjk.getsupport(best->n,*w);
+								const real_t	wdist=vec3_dot(best->n,w->w)-best->d;
+								if(wdist>EPA_ACCURACY)
+								{
+									for(U j=0;(j<3)&&valid;++j)
+									{
+										valid&=expand(	pass,w,
+											best->f[j],best->e[j],
+											horizon);
+									}
+									if(valid&&(horizon.nf>=3))
+									{
+										bind(horizon.cf,1,horizon.ff,2);
+										remove(m_hull,best);
+										append(m_stock,best);
+										best=findbest();
+										if(best->p>=outer.p) outer=*best;
+									} else { m_status=eStatus::InvalidHull;break; }
+								} else { m_status=eStatus::AccuraryReached;break; }
+							} else { m_status=eStatus::OutOfVertices;break; }
+						}
+						const Vector3	projection=outer.n*outer.d;
+						m_normal	=	outer.n;
+						m_depth		=	outer.d;
+						m_result.rank	=	3;
+						m_result.c[0]	=	outer.c[0];
+						m_result.c[1]	=	outer.c[1];
+						m_result.c[2]	=	outer.c[2];
+						m_result.p[0]	=	vec3_cross(	outer.c[1]->w-projection,
+							outer.c[2]->w-projection).length();
+						m_result.p[1]	=	vec3_cross(	outer.c[2]->w-projection,
+							outer.c[0]->w-projection).length();
+						m_result.p[2]	=	vec3_cross(	outer.c[0]->w-projection,
+							outer.c[1]->w-projection).length();
+						const real_t	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
+						m_result.p[0]	/=	sum;
+						m_result.p[1]	/=	sum;
+						m_result.p[2]	/=	sum;
+						return(m_status);
+					}
+				}
+				/* Fallback		*/ 
+				m_status	=	eStatus::FallBack;
+				m_normal	=	-guess;
+				const real_t	nl=m_normal.length();
+				if(nl>0)
+					m_normal	=	m_normal/nl;
+				else
+					m_normal	=	Vector3(1,0,0);
+				m_depth	=	0;
+				m_result.rank=1;
+				m_result.c[0]=simplex.c[0];
+				m_result.p[0]=1;	
+				return(m_status);
+			}
+			sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
+			{
+				if(m_stock.root)
+				{
+					sFace*	face=m_stock.root;
+					remove(m_stock,face);
+					append(m_hull,face);
+					face->pass	=	0;
+					face->c[0]	=	a;
+					face->c[1]	=	b;
+					face->c[2]	=	c;
+					face->n		=	vec3_cross(b->w-a->w,c->w-a->w);
+					const real_t	l=face->n.length();
+					const bool		v=l>EPA_ACCURACY;
+					face->p		=	MIN(MIN(
+						vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)),
+						vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))),
+						vec3_dot(c->w,vec3_cross(face->n,c->w-a->w)))	/
+						(v?l:1);
+					face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
+					if(v)
+					{
+						face->d		=	vec3_dot(a->w,face->n)/l;
+						face->n		/=	l;
+						if(forced||(face->d>=-EPA_PLANE_EPS))
+						{
+							return(face);
+						} else m_status=eStatus::NonConvex;
+					} else m_status=eStatus::Degenerated;
+					remove(m_hull,face);
+					append(m_stock,face);
+					return(0);
+				}
+				m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
+				return(0);
+			}
+			sFace*				findbest()
+			{
+				sFace*		minf=m_hull.root;
+				real_t	mind=minf->d*minf->d;
+				real_t	maxp=minf->p;
+				for(sFace* f=minf->l[1];f;f=f->l[1])
+				{
+					const real_t	sqd=f->d*f->d;
+					if((f->p>=maxp)&&(sqd<mind))
+					{
+						minf=f;
+						mind=sqd;
+						maxp=f->p;
+					}
+				}
+				return(minf);
+			}
+			bool				expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
+			{
+				static const U	i1m3[]={1,2,0};
+				static const U	i2m3[]={2,0,1};
+				if(f->pass!=pass)
+				{
+					const U	e1=i1m3[e];
+					if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+					{
+						sFace*	nf=newface(f->c[e1],f->c[e],w,false);
+						if(nf)
+						{
+							bind(nf,0,f,e);
+							if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
+							horizon.cf=nf;
+							++horizon.nf;
+							return(true);
+						}
+					}
+					else
+					{
+						const U	e2=i2m3[e];
+						f->pass		=	(U1)pass;
+						if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
+							expand(pass,w,f->f[e2],f->e[e2],horizon))
+						{
+							remove(m_hull,f);
+							append(m_stock,f);
+							return(true);
+						}
+					}
+				}
+				return(false);
+			}
+
+	};
+
+	//
+	static void	Initialize(	const ShapeSW* shape0,const Transform& wtrs0,
+		const ShapeSW* shape1,const Transform& wtrs1,
+		sResults& results,
+		tShape& shape,
+		bool withmargins)
+	{
+		/* Results		*/ 
+		results.witnesses[0]	=
+			results.witnesses[1]	=	Vector3(0,0,0);
+		results.status			=	sResults::Separated;
+		/* Shape		*/ 
+		shape.m_shapes[0]		=	shape0;
+		shape.m_shapes[1]		=	shape1;
+		shape.transform_A		=	wtrs0;
+		shape.transform_B		=	wtrs1;
+		
+	}
+
+
+
+//
+// Api
+//
+
+//
+
+//
+bool Distance(	const ShapeSW*	shape0,
+									  const Transform&		wtrs0,
+									  const ShapeSW*	shape1,
+									  const Transform&		wtrs1,
+									  const Vector3&		guess,
+									  sResults&				results)
+{
+	tShape			shape;
+	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
+	GJK				gjk;
+	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
+	if(gjk_status==GJK::eStatus::Valid)
+	{
+		Vector3	w0=Vector3(0,0,0);
+		Vector3	w1=Vector3(0,0,0);
+		for(U i=0;i<gjk.m_simplex->rank;++i)
+		{
+			const real_t	p=gjk.m_simplex->p[i];
+			w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
+			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+		}
+		results.witnesses[0]	=	wtrs0.xform(w0);
+		results.witnesses[1]	=	wtrs0.xform(w1);
+		results.normal			=	w0-w1;
+		results.distance		=	results.normal.length();
+		results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
+		return(true);
+	}
+	else
+	{
+		results.status	=	gjk_status==GJK::eStatus::Inside?
+			sResults::Penetrating	:
+		sResults::GJK_Failed	;
+		return(false);
+	}
+}
+
+//
+bool Penetration(	const ShapeSW*	shape0,
+									 const Transform&		wtrs0,
+									 const ShapeSW*	shape1,
+									 const Transform&		wtrs1,
+									 const Vector3&		guess,
+									 sResults&				results
+									)
+{
+	tShape			shape;
+	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
+	GJK				gjk;	
+	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,-guess);
+	switch(gjk_status)
+	{
+	case	GJK::eStatus::Inside:
+		{
+			EPA				epa;
+			EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
+			if(epa_status!=EPA::eStatus::Failed)
+			{
+				Vector3	w0=Vector3(0,0,0);
+				for(U i=0;i<epa.m_result.rank;++i)
+				{
+					w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
+				}
+				results.status			=	sResults::Penetrating;
+				results.witnesses[0]	=	w0;
+				results.witnesses[1]	=	w0-epa.m_normal*epa.m_depth;
+				results.normal			=	-epa.m_normal;
+				results.distance		=	-epa.m_depth;
+				return(true);
+			} else results.status=sResults::EPA_Failed;
+		}
+		break;
+	case	GJK::eStatus::Failed:
+		results.status=sResults::GJK_Failed;
+		break;
+	default: {}
+	}
+	return(false);
+}
+
+
+/* Symbols cleanup		*/ 
+
+#undef GJK_MAX_ITERATIONS
+#undef GJK_ACCURARY
+#undef GJK_MIN_DISTANCE
+#undef GJK_DUPLICATED_EPS
+#undef GJK_SIMPLEX2_EPS
+#undef GJK_SIMPLEX3_EPS
+#undef GJK_SIMPLEX4_EPS
+
+#undef EPA_MAX_VERTICES
+#undef EPA_MAX_FACES
+#undef EPA_MAX_ITERATIONS
+#undef EPA_ACCURACY
+#undef EPA_FALLBACK
+#undef EPA_PLANE_EPS
+#undef EPA_INSIDE_EPS
+
+
+} // end of namespace
+
+
+
+bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap ) {
+
+	GjkEpa2::sResults res;
+	
+	if (GjkEpa2::Penetration(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) {
+		if (p_result_callback) {
+			if (p_swap)
+				p_result_callback(res.witnesses[1],res.witnesses[0],p_userdata);
+			else
+				p_result_callback(res.witnesses[0],res.witnesses[1],p_userdata);
+		}
+		return true;
+	} 
+	
+	return false;
+}
+
diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h
new file mode 100644
index 000000000..0303478f1
--- /dev/null
+++ b/servers/physics/gjk_epa.h
@@ -0,0 +1,40 @@
+/*************************************************************************/
+/*  gjk_epa.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 GJK_EPA_H
+#define GJK_EPA_H
+
+#include "shape_sw.h"
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+#include "collision_solver_sw.h"
+
+bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false);
+
+#endif
diff --git a/servers/physics/joints_sw.cpp b/servers/physics/joints_sw.cpp
new file mode 100644
index 000000000..f9e22e166
--- /dev/null
+++ b/servers/physics/joints_sw.cpp
@@ -0,0 +1,450 @@
+/*************************************************************************/
+/*  joints_sw.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 "joints_sw.h"
+#include "space_sw.h"
+
+#if 0
+
+//based on chipmunk joint constraints
+
+/* Copyright (c) 2007 Scott Lembcke
+ *
+ * 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.
+ */
+
+static inline real_t k_scalar(Body2DSW *a,Body2DSW *b,const Vector2& rA, const Vector2& rB, const Vector2& n) {
+
+
+	real_t value=0;
+
+
+	{
+		value+=a->get_inv_mass();
+		real_t rcn = rA.cross(n);
+		value+=a->get_inv_inertia() * rcn * rcn;
+	}
+
+	if (b) {
+
+		value+=b->get_inv_mass();
+		real_t rcn = rB.cross(n);
+		value+=b->get_inv_inertia() * rcn * rcn;
+	}
+
+	return value;
+
+}
+
+
+bool PinJoint2DSW::setup(float p_step) {
+
+	Space2DSW *space = A->get_space();
+	ERR_FAIL_COND_V(!space,false;)
+	rA = A->get_transform().xform(anchor_A);
+	rB = B?B->get_transform().xform(anchor_B):anchor_B;
+
+	Vector2 delta = rB - rA;
+
+	rA-= A->get_transform().get_origin();
+	if (B)
+		rB-=B->get_transform().get_origin();
+
+
+	real_t jdist = delta.length();
+	correct=false;
+	if (jdist==0)
+		return false; // do not correct
+
+	correct=true;
+
+	n = delta / jdist;
+
+	// calculate mass normal
+	mass_normal = 1.0f/k_scalar(A, B, rA, rB, n);
+
+	// calculate bias velocity
+	//real_t maxBias = joint->constraint.maxBias;
+	bias = -(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)*(jdist-dist);
+	bias = CLAMP(bias, -get_max_bias(), +get_max_bias());
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// apply accumulated impulse
+	Vector2 j = n * jn_acc;
+	A->apply_impulse(rA,-j);
+	if (B)
+		B->apply_impulse(rB,j);
+
+	return true;
+}
+
+
+static inline Vector2
+relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB){
+	Vector2 sum = a->get_linear_velocity() -rA.tangent() * a->get_angular_velocity();
+	if (b)
+		return (b->get_linear_velocity() -rB.tangent() * b->get_angular_velocity()) - sum;
+	else
+		return -sum;
+}
+
+static inline real_t
+normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n){
+	return relative_velocity(a, b, rA, rB).dot(n);
+}
+
+
+void PinJoint2DSW::solve(float p_step){
+
+	if (!correct)
+		return;
+
+	Vector2 ln = n;
+
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A,B, rA, rB, ln);
+
+	// compute normal impulse
+	real_t jn = (bias - vrn)*mass_normal;
+	real_t jnOld = jn_acc;
+	jn_acc = CLAMP(jnOld + jn,-jn_max,jn_max); //cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
+	jn = jn_acc - jnOld;
+
+	Vector2 j = jn*ln;
+
+	A->apply_impulse(rA,-j);
+	if (B)
+		B->apply_impulse(rB,j);
+
+}
+
+
+PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) {
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
+	anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos;
+
+	jn_acc=0;
+	dist=0;
+
+	p_body_a->add_constraint(this,0);
+	if (p_body_b)
+		p_body_b->add_constraint(this,1);
+
+}
+
+PinJoint2DSW::~PinJoint2DSW() {
+
+	if (A)
+		A->remove_constraint(this);
+	if (B)
+		B->remove_constraint(this);
+
+}
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+
+static inline void
+k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2)
+{
+	// calculate mass matrix
+	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
+	real_t k11, k12, k21, k22;
+	real_t m_sum = a->get_inv_mass() + b->get_inv_mass();
+
+	// start with I*m_sum
+	k11 = m_sum; k12 = 0.0f;
+	k21 = 0.0f;  k22 = m_sum;
+
+	// add the influence from r1
+	real_t a_i_inv = a->get_inv_inertia();
+	real_t r1xsq =  r1.x * r1.x * a_i_inv;
+	real_t r1ysq =  r1.y * r1.y * a_i_inv;
+	real_t r1nxy = -r1.x * r1.y * a_i_inv;
+	k11 += r1ysq; k12 += r1nxy;
+	k21 += r1nxy; k22 += r1xsq;
+
+	// add the influnce from r2
+	real_t b_i_inv = b->get_inv_inertia();
+	real_t r2xsq =  r2.x * r2.x * b_i_inv;
+	real_t r2ysq =  r2.y * r2.y * b_i_inv;
+	real_t r2nxy = -r2.x * r2.y * b_i_inv;
+	k11 += r2ysq; k12 += r2nxy;
+	k21 += r2nxy; k22 += r2xsq;
+
+	// invert
+	real_t determinant = k11*k22 - k12*k21;
+	ERR_FAIL_COND(determinant== 0.0);
+
+	real_t det_inv = 1.0f/determinant;
+	*k1 = Vector2( k22*det_inv, -k12*det_inv);
+	*k2 = Vector2(-k21*det_inv,  k11*det_inv);
+}
+
+static _FORCE_INLINE_ Vector2
+mult_k(const Vector2& vr, const Vector2 &k1, const Vector2 &k2)
+{
+	return Vector2(vr.dot(k1), vr.dot(k2));
+}
+
+bool GrooveJoint2DSW::setup(float p_step) {
+
+
+	// calculate endpoints in worldspace
+	Vector2 ta = A->get_transform().xform(A_groove_1);
+	Vector2 tb = A->get_transform().xform(A_groove_2);
+	Space2DSW *space=A->get_space();
+
+	// calculate axis
+	Vector2 n = -(tb - ta).tangent().normalized();
+	real_t d = ta.dot(n);
+
+	xf_normal = n;
+	rB = B->get_transform().basis_xform(B_anchor);
+
+	// calculate tangential distance along the axis of rB
+	real_t td = (B->get_transform().get_origin() + rB).cross(n);
+	// calculate clamping factor and rB
+	if(td <= ta.cross(n)){
+		clamp = 1.0f;
+		rA = ta - A->get_transform().get_origin();
+	} else if(td >= tb.cross(n)){
+		clamp = -1.0f;
+		rA = tb - A->get_transform().get_origin();
+	} else {
+		clamp = 0.0f;
+		//joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
+		rA =  ((-n.tangent() * -td) + n*d) - A->get_transform().get_origin();
+	}
+
+	// Calculate mass tensor
+	k_tensor(A, B, rA, rB, &k1, &k2);
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// calculate bias velocity
+//	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+//	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
+
+
+	Vector2 delta = (B->get_transform().get_origin() +rB) - (A->get_transform().get_origin() + rA);
+	gbias=(delta*-(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)).clamped(get_max_bias());
+
+	// apply accumulated impulse
+	A->apply_impulse(rA,-jn_acc);
+	B->apply_impulse(rB,jn_acc);
+
+	correct=true;
+	return true;
+}
+
+void GrooveJoint2DSW::solve(float p_step){
+
+
+	// compute impulse
+	Vector2 vr = relative_velocity(A, B, rA,rB);
+
+	Vector2 j = mult_k(gbias-vr, k1, k2);
+	Vector2 jOld = jn_acc;
+	j+=jOld;
+
+	jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : xf_normal.project(j)).clamped(jn_max);
+
+	j = jn_acc - jOld;
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+}
+
+
+GrooveJoint2DSW::GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
+
+	A=p_body_a;
+	B=p_body_b;
+
+	A_groove_1 = A->get_inv_transform().xform(p_a_groove1);
+	A_groove_2 = A->get_inv_transform().xform(p_a_groove2);
+	B_anchor=B->get_inv_transform().xform(p_b_anchor);
+	A_groove_normal = -(A_groove_2 - A_groove_1).normalized().tangent();
+
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+
+}
+
+GrooveJoint2DSW::~GrooveJoint2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+}
+
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+
+bool DampedSpringJoint2DSW::setup(float p_step) {
+
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B->get_transform().basis_xform(anchor_B);
+
+	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA) ;
+	real_t dist = delta.length();
+
+	if (dist)
+		n=delta/dist;
+	else
+		n=Vector2();
+
+	real_t k = k_scalar(A, B, rA, rB, n);
+	n_mass = 1.0f/k;
+
+	target_vrn = 0.0f;
+	v_coef = 1.0f - Math::exp(-damping*(p_step)*k);
+
+	// apply spring force
+	real_t f_spring = (rest_length - dist) * stiffness;
+	Vector2 j = n * f_spring*(p_step);
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+
+
+	return true;
+}
+
+void DampedSpringJoint2DSW::solve(float p_step) {
+
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn;
+
+	// compute velocity loss from drag
+	// not 100% certain this is derived correctly, though it makes sense
+	real_t v_damp = -vrn*v_coef;
+	target_vrn = vrn + v_damp;
+	Vector2 j=n*v_damp*n_mass;
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+
+}
+
+void DampedSpringJoint2DSW::set_param(Physics2DServer::DampedStringParam p_param, real_t p_value) {
+
+	switch(p_param) {
+
+		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
+
+			rest_length=p_value;
+		} break;
+		case Physics2DServer::DAMPED_STRING_DAMPING: {
+
+			damping=p_value;
+		} break;
+		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
+
+			stiffness=p_value;
+		} break;
+	}
+
+}
+
+real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const{
+
+	switch(p_param) {
+
+		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
+
+			return rest_length;
+		} break;
+		case Physics2DServer::DAMPED_STRING_DAMPING: {
+
+			return damping;
+		} break;
+		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
+
+			return stiffness;
+		} break;
+	}
+
+	ERR_FAIL_V(0);
+}
+
+
+DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
+
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = A->get_inv_transform().xform(p_anchor_a);
+	anchor_B = B->get_inv_transform().xform(p_anchor_b);
+
+	rest_length=p_anchor_a.distance_to(p_anchor_b);
+	stiffness=20;
+	damping=1.5;
+
+
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+
+}
+
+DampedSpringJoint2DSW::~DampedSpringJoint2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+
+}
+
+
+#endif
diff --git a/servers/physics/joints_sw.h b/servers/physics/joints_sw.h
new file mode 100644
index 000000000..c10568fb5
--- /dev/null
+++ b/servers/physics/joints_sw.h
@@ -0,0 +1,176 @@
+/*************************************************************************/
+/*  joints_sw.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 JOINTS_SW_H
+#define JOINTS_SW_H
+
+#include "constraint_sw.h"
+#include "body_sw.h"
+
+
+
+class JointSW : public ConstraintSW {
+
+	real_t max_force;
+	real_t bias;
+	real_t max_bias;
+public:
+
+	_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; }
+	_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
+
+	_FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; }
+	_FORCE_INLINE_ real_t get_bias() const { return bias; }
+
+	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; }
+	_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
+
+//	virtual PhysicsServer::JointType get_type() const=0;
+	JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; };
+
+};
+
+#if 0
+class PinJointSW : public JointSW {
+
+	union {
+		struct {
+			BodySW *A;
+			BodySW *B;
+		};
+
+		BodySW *_arr[2];
+	};
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+	real_t dist;
+	real_t jn_acc;
+	real_t jn_max;
+	real_t max_distance;
+	real_t mass_normal;
+	real_t bias;
+
+	Vector2 rA,rB;
+	Vector2 n; //normal
+	bool correct;
+
+
+public:
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	PinJointSW(const Vector2& p_pos,BodySW* p_body_a,BodySW* p_body_b=NULL);
+	~PinJointSW();
+};
+
+
+class GrooveJointSW : public JointSW {
+
+	union {
+		struct {
+			BodySW *A;
+			BodySW *B;
+		};
+
+		BodySW *_arr[2];
+	};
+
+	Vector2 A_groove_1;
+	Vector2 A_groove_2;
+	Vector2 A_groove_normal;
+	Vector2 B_anchor;
+	Vector2 jn_acc;
+	Vector2 gbias;
+	real_t jn_max;
+	real_t clamp;
+	Vector2 xf_normal;
+	Vector2 rA,rB;
+	Vector2 k1,k2;
+
+
+	bool correct;
+
+public:
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_GROOVE; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	GrooveJointSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, BodySW* p_body_a,BodySW* p_body_b);
+	~GrooveJointSW();
+};
+
+
+class DampedSpringJointSW : public JointSW {
+
+	union {
+		struct {
+			BodySW *A;
+			BodySW *B;
+		};
+
+		BodySW *_arr[2];
+	};
+
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+
+	real_t rest_length;
+	real_t damping;
+	real_t stiffness;
+
+	Vector2 rA,rB;
+	Vector2 n;
+	real_t n_mass;
+	real_t target_vrn;
+	real_t v_coef;
+
+public:
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_DAMPED_SPRING; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+	void set_param(PhysicsServer::DampedStringParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer::DampedStringParam p_param) const;
+
+	DampedSpringJointSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, BodySW* p_body_a,BodySW* p_body_b);
+	~DampedSpringJointSW();
+};
+#endif
+
+#endif // JOINTS__SW_H
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
new file mode 100644
index 000000000..072f11aa5
--- /dev/null
+++ b/servers/physics/physics_server_sw.cpp
@@ -0,0 +1,1050 @@
+/*************************************************************************/
+/*  physics_server_sw.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 "physics_server_sw.h"
+#include "broad_phase_basic.h"
+#include "broad_phase_octree.h"
+
+RID PhysicsServerSW::shape_create(ShapeType p_shape) {
+
+	ShapeSW *shape=NULL;
+	switch(p_shape) {
+
+		case SHAPE_PLANE: {
+
+			shape=memnew( PlaneShapeSW );
+		} break;
+		case SHAPE_RAY: {
+
+			shape=memnew( RayShapeSW );
+		} break;
+		case SHAPE_SPHERE: {
+
+			shape=memnew( SphereShapeSW);
+		} break;
+		case SHAPE_BOX: {
+
+			shape=memnew( BoxShapeSW);
+		} break;
+		case SHAPE_CAPSULE: {
+
+			shape=memnew( CapsuleShapeSW );
+		} break;
+		case SHAPE_CONVEX_POLYGON: {
+
+			shape=memnew( ConvexPolygonShapeSW );
+		} break;
+		case SHAPE_CONCAVE_POLYGON: {
+
+			shape=memnew( ConcavePolygonShapeSW );
+		} break;
+		case SHAPE_HEIGHTMAP: {
+
+			shape=memnew( HeightMapShapeSW );
+		} break;
+		case SHAPE_CUSTOM: {
+
+			ERR_FAIL_V(RID());
+
+		} break;
+
+	}
+
+	RID id = shape_owner.make_rid(shape);
+	shape->set_self(id);
+
+	return id;
+};
+
+void PhysicsServerSW::shape_set_data(RID p_shape, const Variant& p_data) {
+
+	ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	shape->set_data(p_data);
+
+
+};
+
+
+void PhysicsServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
+
+	ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	shape->set_custom_bias(p_bias);
+
+}
+
+
+PhysicsServer::ShapeType PhysicsServerSW::shape_get_type(RID p_shape) const {
+
+	const ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,SHAPE_CUSTOM);
+	return shape->get_type();
+
+};
+
+Variant PhysicsServerSW::shape_get_data(RID p_shape) const {
+
+	const ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,Variant());
+	ERR_FAIL_COND_V(!shape->is_configured(),Variant());
+	return shape->get_data();
+
+};
+
+real_t PhysicsServerSW::shape_get_custom_solver_bias(RID p_shape) const {
+
+	const ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,0);
+	return shape->get_custom_bias();
+
+}
+
+
+RID PhysicsServerSW::space_create() {
+
+	SpaceSW *space = memnew( SpaceSW );
+	RID id = space_owner.make_rid(space);
+	space->set_self(id);
+	RID area_id = area_create();
+	AreaSW *area = area_owner.get(area_id);
+	ERR_FAIL_COND_V(!area,RID());
+	space->set_default_area(area);
+	area->set_space(space);
+	area->set_priority(-1);
+
+	return id;
+};
+
+void PhysicsServerSW::space_set_active(RID p_space,bool p_active) {
+
+	SpaceSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+	if (p_active)
+		active_spaces.insert(space);
+	else
+		active_spaces.erase(space);
+}
+
+bool PhysicsServerSW::space_is_active(RID p_space) const {
+
+	const SpaceSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,false);
+
+	return active_spaces.has(space);
+
+}
+
+void PhysicsServerSW::space_set_param(RID p_space,SpaceParameter p_param, real_t p_value) {
+
+	SpaceSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+
+	space->set_param(p_param,p_value);
+
+}
+
+real_t PhysicsServerSW::space_get_param(RID p_space,SpaceParameter p_param) const {
+
+	const SpaceSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,0);
+	return space->get_param(p_param);
+}
+
+PhysicsDirectSpaceState* PhysicsServerSW::space_get_direct_state(RID p_space) {
+
+	SpaceSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,NULL);
+	if (!doing_sync || space->is_locked()) {
+
+		ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification.");
+		ERR_FAIL_V(NULL);
+
+
+	}
+
+	return space->get_direct_state();
+}
+
+RID PhysicsServerSW::area_create() {
+
+	AreaSW *area = memnew( AreaSW );
+	RID rid = area_owner.make_rid(area);
+	area->set_self(rid);
+	return rid;
+};
+
+void PhysicsServerSW::area_set_space(RID p_area, RID p_space) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	SpaceSW *space=NULL;
+	if (p_space.is_valid()) {
+		space = space_owner.get(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	area->set_space(space);
+
+};
+
+RID PhysicsServerSW::area_get_space(RID p_area) const {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,RID());
+
+	SpaceSW *space = area->get_space();
+	if (!space)
+		return RID();
+	return space->get_self();
+};
+
+void PhysicsServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) {
+
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_space_override_mode(p_mode);
+}
+
+PhysicsServer::AreaSpaceOverrideMode PhysicsServerSW::area_get_space_override_mode(RID p_area) const {
+
+	const AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,AREA_SPACE_OVERRIDE_DISABLED);
+
+	return area->get_space_override_mode();
+}
+
+
+void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform& p_transform) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	area->add_shape(shape,p_transform);
+
+}
+
+void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	ERR_FAIL_COND(!shape->is_configured());
+
+	area->set_shape(p_shape_idx,shape);
+
+}
+void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_shape_transform(p_shape_idx,p_transform);
+}
+
+int PhysicsServerSW::area_get_shape_count(RID p_area) const {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,-1);
+
+	return area->get_shape_count();
+
+}
+RID PhysicsServerSW::area_get_shape(RID p_area, int p_shape_idx) const {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,RID());
+
+	ShapeSW *shape = area->get_shape(p_shape_idx);
+	ERR_FAIL_COND_V(!shape,RID());
+
+	return shape->get_self();
+}
+Transform PhysicsServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,Transform());
+
+	return area->get_shape_transform(p_shape_idx);
+}
+
+void PhysicsServerSW::area_remove_shape(RID p_area, int p_shape_idx) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->remove_shape(p_shape_idx);
+}
+
+void PhysicsServerSW::area_clear_shapes(RID p_area) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	while(area->get_shape_count())
+		area->remove_shape(0);
+
+}
+
+void PhysicsServerSW::area_attach_object_instance_ID(RID p_area,ObjectID p_ID) {
+
+	if (space_owner.owns(p_area)) {
+		SpaceSW *space=space_owner.get(p_area);
+		p_area=space->get_default_area()->get_self();
+	}
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_instance_id(p_ID);
+
+}
+ObjectID PhysicsServerSW::area_get_object_instance_ID(RID p_area) const {
+
+	if (space_owner.owns(p_area)) {
+		SpaceSW *space=space_owner.get(p_area);
+		p_area=space->get_default_area()->get_self();
+	}
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,0);
+	return area->get_instance_id();
+
+
+}
+
+
+void PhysicsServerSW::area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value) {
+
+	if (space_owner.owns(p_area)) {
+		SpaceSW *space=space_owner.get(p_area);
+		p_area=space->get_default_area()->get_self();
+	}
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_param(p_param,p_value);
+
+};
+
+
+void PhysicsServerSW::area_set_transform(RID p_area, const Transform& p_transform) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_transform(p_transform);
+
+};
+
+Variant PhysicsServerSW::area_get_param(RID p_area,AreaParameter p_param) const {
+
+	if (space_owner.owns(p_area)) {
+		SpaceSW *space=space_owner.get(p_area);
+		p_area=space->get_default_area()->get_self();
+	}
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,Variant());
+
+	return area->get_param(p_param);
+};
+
+Transform PhysicsServerSW::area_get_transform(RID p_area) const {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,Transform());
+
+	return area->get_transform();
+};
+
+void PhysicsServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method);
+
+
+}
+
+
+/* BODY API */
+
+RID PhysicsServerSW::body_create(BodyMode p_mode,bool p_init_sleeping) {
+
+	BodySW *body = memnew( BodySW );
+	if (p_mode!=BODY_MODE_RIGID)
+		body->set_mode(p_mode);
+	if (p_init_sleeping)
+		body->set_state(BODY_STATE_SLEEPING,p_init_sleeping);
+	RID rid = body_owner.make_rid(body);
+	body->set_self(rid);
+	return rid;
+};
+
+
+void PhysicsServerSW::body_set_space(RID p_body, RID p_space) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	SpaceSW *space=NULL;
+
+	if (p_space.is_valid()) {
+		space = space_owner.get(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	if (body->get_space()==space)
+		return; //pointles
+
+	body->set_space(space);
+
+};
+
+RID PhysicsServerSW::body_get_space(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,RID());
+
+	SpaceSW *space = body->get_space();
+	if (!space)
+		return RID();
+	return space->get_self();
+};
+
+
+void PhysicsServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_mode(p_mode);
+};
+
+PhysicsServer::BodyMode PhysicsServerSW::body_get_mode(RID p_body, BodyMode p_mode) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,BODY_MODE_STATIC);
+
+	return body->get_mode();
+};
+
+void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform& p_transform) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	body->add_shape(shape,p_transform);
+
+}
+
+void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	ShapeSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	ERR_FAIL_COND(!shape->is_configured());
+
+	body->set_shape(p_shape_idx,shape);
+
+}
+void PhysicsServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_shape_transform(p_shape_idx,p_transform);
+}
+
+int PhysicsServerSW::body_get_shape_count(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,-1);
+
+	return body->get_shape_count();
+
+}
+RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,RID());
+
+	ShapeSW *shape = body->get_shape(p_shape_idx);
+	ERR_FAIL_COND_V(!shape,RID());
+
+	return shape->get_self();
+}
+
+void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+}
+
+bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const{
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,false);
+
+	// todo ?
+
+	return false;
+}
+
+
+Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,Transform());
+
+	return body->get_shape_transform(p_shape_idx);
+}
+
+void PhysicsServerSW::body_remove_shape(RID p_body, int p_shape_idx) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->remove_shape(p_shape_idx);
+}
+
+void PhysicsServerSW::body_clear_shapes(RID p_body) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	while(body->get_shape_count())
+		body->remove_shape(0);
+
+}
+
+void PhysicsServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_continuous_collision_detection(p_enable);
+
+}
+
+bool PhysicsServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,false);
+
+	return body->is_continuous_collision_detection_enabled();
+}
+
+void PhysicsServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_instance_id(p_ID);
+
+};
+
+uint32_t PhysicsServerSW::body_get_object_instance_ID(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+
+	return body->get_instance_id();
+};
+
+
+void PhysicsServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+};
+
+uint32_t PhysicsServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+
+	return 0;
+};
+
+void PhysicsServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_param(p_param,p_value);
+};
+
+float PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+
+	return body->get_param(p_param);
+};
+
+
+void PhysicsServerSW::body_static_simulate_motion(RID p_body,const Transform& p_new_transform) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->simulate_motion(p_new_transform,last_step);
+
+};
+
+void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_state(p_state,p_variant);
+};
+
+Variant PhysicsServerSW::body_get_state(RID p_body, BodyState p_state) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,Variant());
+
+	return body->get_state(p_state);
+};
+
+
+void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3& p_force) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_applied_force(p_force);
+};
+
+Vector3 PhysicsServerSW::body_get_applied_force(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,Vector3());
+	return body->get_applied_force();
+};
+
+void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3& p_torque) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_applied_torque(p_torque);
+};
+
+Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,Vector3());
+
+	return body->get_applied_torque();
+};
+
+void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->apply_impulse(p_pos,p_impulse);
+};
+
+void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	Vector3 v = body->get_linear_velocity();
+	Vector3 axis = p_axis_velocity.normalized();
+	v-=axis*axis.dot(v);
+	v+=p_axis_velocity;
+	body->set_linear_velocity(v);
+
+};
+
+void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_exception(p_body_b);
+
+};
+
+void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->remove_exception(p_body);
+
+};
+
+void PhysicsServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	for(int i=0;i<body->get_exceptions().size();i++) {
+		p_exceptions->push_back(body->get_exceptions()[i]);
+	}
+
+};
+
+void PhysicsServerSW::body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+};
+
+float PhysicsServerSW::body_get_contacts_reported_depth_treshold(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+	return 0;
+};
+
+void PhysicsServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_omit_force_integration(p_omit);
+};
+
+bool PhysicsServerSW::body_is_omitting_force_integration(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,false);
+	return body->get_omit_force_integration();
+};
+
+void PhysicsServerSW::body_set_max_contacts_reported(RID p_body, int p_contacts) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_max_contacts_reported(p_contacts);
+}
+
+int PhysicsServerSW::body_get_max_contacts_reported(RID p_body) const {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,-1);
+	return body->get_max_contacts_reported();
+}
+
+void PhysicsServerSW::body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata) {
+
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_force_integration_callback(p_receiver?p_receiver->get_instance_ID():ObjectID(0),p_method,p_udata);
+
+}
+
+
+/* JOINT API */
+
+#if 0
+void PhysicsServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
+
+	JointSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+
+	switch(p_param) {
+		case JOINT_PARAM_BIAS: joint->set_bias(p_value); break;
+		case JOINT_PARAM_MAX_BIAS: joint->set_max_bias(p_value); break;
+		case JOINT_PARAM_MAX_FORCE: joint->set_max_force(p_value); break;
+	}
+
+
+}
+
+real_t PhysicsServerSW::joint_get_param(RID p_joint,JointParam p_param) const {
+
+	const JointSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint,-1);
+
+	switch(p_param) {
+		case JOINT_PARAM_BIAS: return joint->get_bias(); break;
+		case JOINT_PARAM_MAX_BIAS: return joint->get_max_bias(); break;
+		case JOINT_PARAM_MAX_FORCE: return joint->get_max_force(); break;
+	}
+
+	return 0;
+}
+
+
+RID PhysicsServerSW::pin_joint_create(const Vector3& p_pos,RID p_body_a,RID p_body_b) {
+
+	BodySW *A=body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A,RID());
+	BodySW *B=NULL;
+	if (body_owner.owns(p_body_b)) {
+		B=body_owner.get(p_body_b);
+		ERR_FAIL_COND_V(!B,RID());
+	}
+
+	JointSW *joint = memnew( PinJointSW(p_pos,A,B) );
+	RID self = joint_owner.make_rid(joint);
+	joint->set_self(self);
+
+	return self;
+}
+
+RID PhysicsServerSW::groove_joint_create(const Vector3& p_a_groove1,const Vector3& p_a_groove2, const Vector3& p_b_anchor, RID p_body_a,RID p_body_b) {
+
+
+	BodySW *A=body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A,RID());
+
+	BodySW *B=body_owner.get(p_body_b);
+	ERR_FAIL_COND_V(!B,RID());
+
+	JointSW *joint = memnew( GrooveJointSW(p_a_groove1,p_a_groove2,p_b_anchor,A,B) );
+	RID self = joint_owner.make_rid(joint);
+	joint->set_self(self);
+	return self;
+
+
+}
+
+RID PhysicsServerSW::damped_spring_joint_create(const Vector3& p_anchor_a,const Vector3& p_anchor_b,RID p_body_a,RID p_body_b) {
+
+	BodySW *A=body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A,RID());
+
+	BodySW *B=body_owner.get(p_body_b);
+	ERR_FAIL_COND_V(!B,RID());
+
+	JointSW *joint = memnew( DampedSpringJointSW(p_anchor_a,p_anchor_b,A,B) );
+	RID self = joint_owner.make_rid(joint);
+	joint->set_self(self);
+	return self;
+
+}
+
+void PhysicsServerSW::damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value) {
+
+
+	JointSW *j = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!j);
+	ERR_FAIL_COND(j->get_type()!=JOINT_DAMPED_SPRING);
+
+	DampedSpringJointSW *dsj = static_cast<DampedSpringJointSW*>(j);
+	dsj->set_param(p_param,p_value);
+}
+
+real_t PhysicsServerSW::damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const {
+
+	JointSW *j = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!j,0);
+	ERR_FAIL_COND_V(j->get_type()!=JOINT_DAMPED_SPRING,0);
+
+	DampedSpringJointSW *dsj = static_cast<DampedSpringJointSW*>(j);
+	return dsj->get_param(p_param);
+}
+
+PhysicsServer::JointType PhysicsServerSW::joint_get_type(RID p_joint) const {
+
+
+	JointSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint,JOINT_PIN);
+
+	return joint->get_type();
+}
+
+#endif
+
+void PhysicsServerSW::free(RID p_rid) {
+
+	if (shape_owner.owns(p_rid)) {
+
+		ShapeSW *shape = shape_owner.get(p_rid);
+
+		while(shape->get_owners().size()) {
+			ShapeOwnerSW *so=shape->get_owners().front()->key();
+			so->remove_shape(shape);
+		}
+
+		shape_owner.free(p_rid);
+		memdelete(shape);
+	} else if (body_owner.owns(p_rid)) {
+
+		BodySW *body = body_owner.get(p_rid);
+
+//		if (body->get_state_query())
+//			_clear_query(body->get_state_query());
+
+//		if (body->get_direct_state_query())
+//			_clear_query(body->get_direct_state_query());
+
+		body->set_space(NULL);
+
+
+		while( body->get_shape_count() ) {
+
+			body->remove_shape(0);
+		}
+
+		while (body->get_constraint_map().size()) {
+			RID self = body->get_constraint_map().front()->key()->get_self();
+			ERR_FAIL_COND(!self.is_valid());
+			free(self);
+		}
+
+		body_owner.free(p_rid);
+		memdelete(body);
+
+	} else if (area_owner.owns(p_rid)) {
+
+		AreaSW *area = area_owner.get(p_rid);
+
+//		if (area->get_monitor_query())
+//			_clear_query(area->get_monitor_query());
+
+		area->set_space(NULL);
+
+		while( area->get_shape_count() ) {
+
+			area->remove_shape(0);
+		}
+
+		area_owner.free(p_rid);
+		memdelete(area);
+	} else if (space_owner.owns(p_rid)) {
+
+		SpaceSW *space = space_owner.get(p_rid);
+
+		while(space->get_objects().size()) {
+			CollisionObjectSW *co = (CollisionObjectSW *)space->get_objects().front()->get();
+			co->set_space(NULL);
+		}
+
+		active_spaces.erase(space);
+		free(space->get_default_area()->get_self());
+		space_owner.free(p_rid);
+		memdelete(space);
+	} else if (joint_owner.owns(p_rid)) {
+
+		JointSW *joint = joint_owner.get(p_rid);
+
+		joint_owner.free(p_rid);
+		memdelete(joint);
+
+	} else {
+
+		ERR_EXPLAIN("Invalid ID");
+		ERR_FAIL();
+	}
+
+
+};
+
+void PhysicsServerSW::set_active(bool p_active) {
+
+	active=p_active;
+};
+
+void PhysicsServerSW::init() {
+
+	doing_sync=true;
+	last_step=0.001;
+	iterations=8;// 8?
+	stepper = memnew( StepSW );
+	direct_state = memnew( PhysicsDirectBodyStateSW );
+};
+
+
+void PhysicsServerSW::step(float p_step) {
+
+
+	if (!active)
+		return;
+
+
+	doing_sync=false;
+
+	last_step=p_step;
+	PhysicsDirectBodyStateSW::singleton->step=p_step;
+
+	for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+
+		stepper->step((SpaceSW*)E->get(),p_step,iterations);
+	}
+};
+
+void PhysicsServerSW::sync() {
+
+};
+
+void PhysicsServerSW::flush_queries() {
+
+	if (!active)
+		return;
+
+	doing_sync=true;
+	for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+
+		SpaceSW *space=(SpaceSW *)E->get();
+		space->call_queries();
+	}
+
+};
+
+
+
+void PhysicsServerSW::finish() {
+
+	memdelete(stepper);
+	memdelete(direct_state);
+};
+
+
+PhysicsServerSW::PhysicsServerSW() {
+
+	BroadPhaseSW::create_func=BroadPhaseOctree::_create;
+
+	active=true;
+
+};
+
+PhysicsServerSW::~PhysicsServerSW() {
+
+};
+
+
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
new file mode 100644
index 000000000..2a46ba65f
--- /dev/null
+++ b/servers/physics/physics_server_sw.h
@@ -0,0 +1,215 @@
+/*************************************************************************/
+/*  physics_server_sw.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 PHYSICS_SERVER_SW
+#define PHYSICS_SERVER_SW
+
+
+#include "servers/physics_server.h"
+#include "shape_sw.h"
+#include "space_sw.h"
+#include "step_sw.h"
+#include "joints_sw.h"
+
+
+class PhysicsServerSW : public PhysicsServer {
+
+	OBJ_TYPE( PhysicsServerSW, PhysicsServer );
+
+friend class PhysicsDirectSpaceStateSW;
+	bool active;
+	int iterations;
+	bool doing_sync;
+	real_t last_step;
+
+	StepSW *stepper;
+	Set<const SpaceSW*> active_spaces;
+
+	PhysicsDirectBodyStateSW *direct_state;
+
+	mutable RID_Owner<ShapeSW> shape_owner;
+	mutable RID_Owner<SpaceSW> space_owner;
+	mutable RID_Owner<AreaSW> area_owner;
+	mutable RID_Owner<BodySW> body_owner;
+	mutable RID_Owner<JointSW> joint_owner;
+
+//	void _clear_query(QuerySW *p_query);
+public:
+
+	virtual RID shape_create(ShapeType p_shape);
+	virtual void shape_set_data(RID p_shape, const Variant& p_data);
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
+
+	virtual ShapeType shape_get_type(RID p_shape) const;
+	virtual Variant shape_get_data(RID p_shape) const;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
+
+	/* SPACE API */
+
+	virtual RID space_create();
+	virtual void space_set_active(RID p_space,bool p_active);
+	virtual bool space_is_active(RID p_space) const;
+
+	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value);
+	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const;
+
+	// this function only works on fixed process, errors and returns null otherwise
+	virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space);
+
+
+	/* AREA API */
+
+	virtual RID area_create();
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode);
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const;
+
+	virtual void area_set_space(RID p_area, RID p_space);
+	virtual RID area_get_space(RID p_area) const;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform& p_transform=Transform());
+	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape);
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform);
+
+	virtual int area_get_shape_count(RID p_area) const;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
+	virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx);
+	virtual void area_clear_shapes(RID p_area);
+
+	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID);
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const;
+
+	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value);
+	virtual void area_set_transform(RID p_area, const Transform& p_transform);
+
+	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
+	virtual Transform area_get_transform(RID p_area) const;
+
+	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
+
+
+	/* BODY API */
+
+	// create a body of a given type
+	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false);
+
+	virtual void body_set_space(RID p_body, RID p_space);
+	virtual RID body_get_space(RID p_body) const;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode);
+	virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform& p_transform=Transform());
+	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape);
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform);
+
+	virtual int body_get_shape_count(RID p_body) const;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
+	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const;
+
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable);
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx);
+	virtual void body_clear_shapes(RID p_body);
+
+	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID);
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable);
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const;
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
+
+	//advanced simulation
+	virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform);
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant);
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
+
+	virtual void body_set_applied_force(RID p_body, const Vector3& p_force);
+	virtual Vector3 body_get_applied_force(RID p_body) const;
+
+	virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque);
+	virtual Vector3 body_get_applied_torque(RID p_body) const;
+
+	virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse);
+	virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity);
+
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
+
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold);
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const;
+
+	virtual void body_set_omit_force_integration(RID p_body,bool p_omit);
+	virtual bool body_is_omitting_force_integration(RID p_body) const;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
+	virtual int body_get_max_contacts_reported(RID p_body) const;
+
+	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
+
+	/* JOINT API */
+#if 0
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
+	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const;
+
+	virtual RID pin_joint_create(const Vector3& p_pos,RID p_body_a,RID p_body_b=RID());
+	virtual RID groove_joint_create(const Vector3& p_a_groove1,const Vector3& p_a_groove2, const Vector3& p_b_anchor, RID p_body_a,RID p_body_b);
+	virtual RID damped_spring_joint_create(const Vector3& p_anchor_a,const Vector3& p_anchor_b,RID p_body_a,RID p_body_b=RID());
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value);
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const;
+
+	virtual JointType joint_get_type(RID p_joint) const;
+#endif
+	/* MISC */
+
+	virtual void free(RID p_rid);
+
+	virtual void set_active(bool p_active);
+	virtual void init();
+	virtual void step(float p_step);
+	virtual void sync();
+	virtual void flush_queries();
+	virtual void finish();
+
+	PhysicsServerSW();
+	~PhysicsServerSW();
+
+};
+
+#endif
+
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp
new file mode 100644
index 000000000..70e5c00b9
--- /dev/null
+++ b/servers/physics/shape_sw.cpp
@@ -0,0 +1,1664 @@
+/*************************************************************************/
+/*  shape_sw.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 "shape_sw.h"

+#include "geometry.h"

+#include "sort.h"

+#include "quick_hull.h"

+#define _POINT_SNAP 0.001953125

+#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002

+#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998

+

+

+void ShapeSW::configure(const AABB& p_aabb) {

+	aabb=p_aabb;

+	configured=true;

+	for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) {

+		ShapeOwnerSW* co=(ShapeOwnerSW*)E->key();

+		co->_shape_changed();

+	}

+}

+

+

+Vector3 ShapeSW::get_support(const Vector3& p_normal) const {

+

+	Vector3 res;

+	int amnt;

+	get_supports(p_normal,1,&res,amnt);

+	return res;

+}

+

+void ShapeSW::add_owner(ShapeOwnerSW *p_owner) {

+

+	Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);

+	if (E) {

+		E->get()++;

+	} else {

+		owners[p_owner]=1;

+	}

+}

+

+void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){

+

+	Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);

+	ERR_FAIL_COND(!E);

+	E->get()--;

+	if (E->get()==0) {

+		owners.erase(E);

+	}

+

+}

+

+bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{

+

+	return owners.has(p_owner);

+

+}

+

+const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{

+	return owners;

+}

+

+

+ShapeSW::ShapeSW() {

+

+	custom_bias=0;

+	configured=false;

+}

+

+

+ShapeSW::~ShapeSW() {

+

+	ERR_FAIL_COND(owners.size());

+}

+

+

+

+Plane PlaneShapeSW::get_plane() const {

+

+	return plane;

+}

+

+void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

+

+	// gibberish, a plane is infinity

+	r_min=-1e7;

+	r_max=1e7;

+}

+

+Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const {

+

+	return p_normal*1e15;

+}

+

+

+bool PlaneShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

+

+	bool inters=plane.intersects_segment(p_begin,p_end,&r_result);

+	if(inters)

+		r_normal=plane.normal;

+	return inters;

+}

+

+Vector3 PlaneShapeSW::get_moment_of_inertia(float p_mass) const {

+

+	return Vector3(); //wtf

+}

+

+void PlaneShapeSW::_setup(const Plane& p_plane)  {

+

+	plane=p_plane;

+	configure(AABB(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2)));

+}

+

+void PlaneShapeSW::set_data(const Variant& p_data) {

+

+	_setup(p_data);

+

+}

+

+Variant PlaneShapeSW::get_data() const {

+

+	return plane;

+}

+

+PlaneShapeSW::PlaneShapeSW()  {

+

+

+}

+

+//

+

+float RayShapeSW::get_length() const {

+

+	return length;

+}

+

+void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

+

+	// don't think this will be even used

+	r_min=0;

+	r_max=1;

+}

+

+Vector3 RayShapeSW::get_support(const Vector3& p_normal) const {

+

+	if (p_normal.z>0)

+		return Vector3(0,0,length);

+	else

+		return Vector3(0,0,0);

+}

+

+void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

+

+	if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {

+

+		r_amount=2;

+		r_supports[0]=Vector3(0,0,0);

+		r_supports[1]=Vector3(0,0,length);

+	} if (p_normal.z>0) {

+		r_amount=1;

+		*r_supports=Vector3(0,0,length);

+	} else {

+		r_amount=1;

+		*r_supports=Vector3(0,0,0);

+	}

+}

+

+

+bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

+

+	return false; //simply not possible

+}

+

+Vector3 RayShapeSW::get_moment_of_inertia(float p_mass) const {

+

+	return Vector3();

+}

+

+void RayShapeSW::_setup(float p_length)  {

+

+	length=p_length;

+	configure(AABB(Vector3(0,0,0),Vector3(0.1,0.1,length)));

+}

+

+void RayShapeSW::set_data(const Variant& p_data) {

+

+	_setup(p_data);

+

+}

+

+Variant RayShapeSW::get_data() const {

+

+	return length;

+}

+

+RayShapeSW::RayShapeSW()  {

+

+	length=1;

+}

+

+

+

+/********** SPHERE *************/

+

+real_t SphereShapeSW::get_radius() const {

+

+	return radius;

+}

+

+void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

+

+	float d = p_normal.dot( p_transform.origin );

+

+	// figure out scale at point

+	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);

+	float scale = local_normal.length();

+

+	r_min = d - (radius) * scale;

+	r_max = d + (radius) * scale;

+

+}

+

+Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const {

+

+	return p_normal*radius;

+}

+

+void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

+

+	*r_supports=p_normal*radius;

+	r_amount=1;

+}

+

+bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

+

+	return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal);

+}

+

+Vector3 SphereShapeSW::get_moment_of_inertia(float p_mass) const {

+

+	float s = 0.4 * p_mass * radius * radius;

+	return Vector3(s,s,s);

+}

+

+void SphereShapeSW::_setup(real_t p_radius) {

+

+

+	radius=p_radius;

+	configure(AABB( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0)));

+

+}

+

+void SphereShapeSW::set_data(const Variant& p_data) {

+

+	_setup(p_data);

+}

+

+Variant SphereShapeSW::get_data() const {

+

+	return radius;

+}

+

+SphereShapeSW::SphereShapeSW() {

+

+	radius=0;

+}

+

+

+/********** BOX *************/

+

+

+void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

+

+	// no matter the angle, the box is mirrored anyway

+	Vector3 local_normal=p_transform.basis.xform_inv(p_normal);

+

+	float length = local_normal.abs().dot(half_extents);

+	float distance = p_normal.dot( p_transform.origin );

+

+	r_min = distance - length;

+	r_max = distance + length;

+

+

+}

+

+Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const {

+

+

+	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

+	);

+

+	return point;

+}

+

+void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

+

+	static const int next[3]={1,2,0};

+	static const int next2[3]={2,0,1};

+

+	for (int i=0;i<3;i++) {

+

+		Vector3 axis;

+		axis[i]=1.0;

+		float dot = p_normal.dot( axis );

+		if ( Math::abs( dot ) >  _FACE_IS_VALID_SUPPORT_TRESHOLD ) {

+

+			//Vector3 axis_b;

+

+			bool neg = dot<0;

+			r_amount = 4;

+

+			Vector3 point;

+			point[i]=half_extents[i];

+

+			int i_n=next[i];

+			int i_n2=next2[i];

+

+			static const float sign[4][2]={

+

+				{-1.0, 1.0},

+				{ 1.0, 1.0},

+				{ 1.0,-1.0},

+				{-1.0,-1.0},

+			};

+

+			for (int j=0;j<4;j++) {

+

+				point[i_n]=sign[j][0]*half_extents[i_n];

+				point[i_n2]=sign[j][1]*half_extents[i_n2];

+				r_supports[j]=neg?-point:point;

+

+			}

+

+			if (neg) {

+				SWAP( r_supports[1], r_supports[2] );

+				SWAP( r_supports[0], r_supports[3] );

+			}

+

+			return;

+		}

+

+		r_amount=0;

+

+	}

+

+	for (int i=0;i<3;i++) {

+

+		Vector3 axis;

+		axis[i]=1.0;

+

+		if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) {

+

+			r_amount= 2;

+

+			int i_n=next[i];

+			int i_n2=next2[i];

+

+			Vector3 point=half_extents;

+

+			if (p_normal[i_n]<0) {

+				point[i_n]=-point[i_n];

+			}

+			if (p_normal[i_n2]<0) {

+				point[i_n2]=-point[i_n2];

+			}

+

+			r_supports[0] = point;

+			point[i]=-point[i];

+			r_supports[1] = point;

+			return;

+		}

+	}

+	/* USE POINT */

+

+	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

+	);

+

+	r_amount=1;

+	r_supports[0]=point;

+}

+

+bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

+

+	AABB aabb(-half_extents,half_extents*2.0);

+

+	return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal);

+

+}

+

+Vector3 BoxShapeSW::get_moment_of_inertia(float p_mass) const {

+

+	float lx=half_extents.x;

+	float ly=half_extents.y;

+	float lz=half_extents.z;

+

+	return Vector3(	(p_mass/3.0) * (ly*ly + lz*lz), (p_mass/3.0) * (lx*lx + lz*lz), (p_mass/3.0) * (lx*lx + ly*ly) );

+

+}

+

+void BoxShapeSW::_setup(const Vector3& p_half_extents)  {

+

+	half_extents=p_half_extents.abs();

+

+	configure(AABB(-half_extents,half_extents*2));

+

+

+}

+

+void BoxShapeSW::set_data(const Variant& p_data) {

+

+

+	_setup(p_data);

+}

+

+Variant BoxShapeSW::get_data() const {

+

+	return half_extents;

+}

+

+BoxShapeSW::BoxShapeSW()  {

+

+

+}

+

+

+/********** CAPSULE *************/

+

+

+void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

+

+	Vector3 n=p_transform.basis.xform_inv(p_normal).normalized();

+	float h = (n.z > 0) ? height : -height;

+

+	n *= radius;

+	n.z += h * 0.5;

+

+	r_max=p_normal.dot(p_transform.xform(n));

+	r_min=p_normal.dot(p_transform.xform(-n));

+	return;

+

+	n = p_transform.basis.xform(n);

+

+	float distance = p_normal.dot( p_transform.origin );

+	float length = Math::abs(p_normal.dot(n));

+	r_min = distance - length;

+	r_max = distance + length;

+

+	ERR_FAIL_COND( r_max < r_min );

+

+}

+

+Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const {

+

+	Vector3 n=p_normal;

+

+	float h = (n.z > 0) ? height : -height;

+

+	n*=radius;

+	n.z += h*0.5;

+	return n;

+}

+

+void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

+

+

+	Vector3 n=p_normal;

+

+	float d = n.z;

+

+	if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {

+

+		// make it flat

+		n.z=0.0;

+		n.normalize();

+		n*=radius;

+

+		r_amount=2;

+		r_supports[0]=n;

+		r_supports[0].z+=height*0.5;

+		r_supports[1]=n;

+		r_supports[1].z-=height*0.5;

+

+	} else {

+

+		float h = (d > 0) ? height : -height;

+

+		n*=radius;

+		n.z += h*0.5;

+		r_amount=1;

+		*r_supports=n;

+

+	}

+

+}

+

+

+bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

+

+	Vector3 norm=(p_end-p_begin).normalized();

+	float min_d=1e20;

+

+

+	Vector3 res,n;

+	bool collision=false;

+

+	Vector3 auxres,auxn;

+	bool collided;

+

+	// test against cylinder and spheres :-|

+

+	collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn);

+

+	if (collided) {

+		float d=norm.dot(auxres);

+		if (d<min_d) {

+			min_d=d;

+			res=auxres;

+			n=auxn;

+			collision=true;

+		}

+	}

+

+	collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn);

+

+	if (collided) {

+		float d=norm.dot(auxres);

+		if (d<min_d) {

+			min_d=d;

+			res=auxres;

+			n=auxn;

+			collision=true;

+		}

+	}

+

+	collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn);

+

+	if (collided) {

+		float d=norm.dot(auxres);

+

+		if (d<min_d) {

+			min_d=d;

+			res=auxres;

+			n=auxn;

+			collision=true;

+		}

+	}

+

+	if (collision) {

+

+		r_result=res;

+		r_normal=n;

+	}

+	return collision;

+}

+

+Vector3 CapsuleShapeSW::get_moment_of_inertia(float p_mass) const {

+

+	// use crappy AABB approximation

+	Vector3 extents=get_aabb().size*0.5;

+

+	return Vector3(

+		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),

+		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),

+		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)

+	);

+

+}

+

+

+

+

+void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius)  {

+

+	height=p_height;

+	radius=p_radius;

+	configure(AABB(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0)));

+

+}

+

+void CapsuleShapeSW::set_data(const Variant& p_data) {

+

+	Dictionary d = p_data;

+	ERR_FAIL_COND(!d.has("radius"));

+	ERR_FAIL_COND(!d.has("height"));

+	_setup(d["height"],d["radius"]);

+

+}

+

+Variant CapsuleShapeSW::get_data() const {

+

+	Dictionary d;

+	d["radius"]=radius;

+	d["height"]=height;

+	return d;

+

+}

+

+

+CapsuleShapeSW::CapsuleShapeSW()  {

+

+	height=radius=0;

+

+}

+

+/********** CONVEX POLYGON *************/

+

+

+void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

+

+

+	int vertex_count=mesh.vertices.size();

+	if (vertex_count==0)

+		return;

+

+	const Vector3 *vrts=&mesh.vertices[0];

+

+	for (int i=0;i<vertex_count;i++) {

+

+		float d=p_normal.dot( p_transform.xform( vrts[i] ) );

+

+		if (i==0 || d > r_max)

+			r_max=d;

+		if (i==0 || d < r_min)

+			r_min=d;

+	}

+}

+

+Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const {

+

+	Vector3 n=p_normal;

+

+	int vert_support_idx=-1;

+	float support_max;

+

+	int vertex_count=mesh.vertices.size();

+	if (vertex_count==0)

+		return Vector3();

+

+	const Vector3 *vrts=&mesh.vertices[0];

+

+	for (int i=0;i<vertex_count;i++) {

+

+		float d=n.dot(vrts[i]);

+

+		if (i==0 || d > support_max) {

+			support_max=d;

+			vert_support_idx=i;

+		}

+	}

+

+	return  vrts[vert_support_idx];

+

+}

+

+

+

+void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

+

+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();

+	int fc = mesh.faces.size();

+

+	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();

+	int ec = mesh.edges.size();

+

+	const Vector3 *vertices = mesh.vertices.ptr();

+	int vc = mesh.vertices.size();

+

+	//find vertex first

+	real_t max;

+	int vtx;

+

+	for (int i=0;i<vc;i++) {

+

+		float d=p_normal.dot(vertices[i]);

+

+		if (i==0 || d > max) {

+			max=d;

+			vtx=i;

+		}

+	}

+

+

+	for(int i=0;i<fc;i++) {

+

+		if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) {

+

+			int ic = faces[i].indices.size();

+			const int *ind=faces[i].indices.ptr();

+

+			bool valid=false;

+			for(int j=0;j<ic;j++) {

+				if (ind[j]==vtx) {

+					valid=true;

+					break;

+				}

+			}

+

+			if (!valid)

+				continue;

+

+			int m = MIN(p_max,ic);

+			for(int j=0;j<m;j++) {

+

+				r_supports[j]=vertices[ind[j]];

+			}

+			r_amount=m;

+			return;

+		}

+	}

+

+	for(int i=0;i<ec;i++) {

+

+

+		float dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal);

+		dot=ABS(dot);

+		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) {

+

+			r_amount=2;

+			r_supports[0]=vertices[edges[i].a];

+			r_supports[1]=vertices[edges[i].b];

+			return;

+		}

+	}

+

+

+	r_supports[0]=vertices[vtx];

+	r_amount=1;

+}

+

+bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

+

+

+

+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();

+	int fc = mesh.faces.size();

+

+	const Vector3 *vertices = mesh.vertices.ptr();

+	int vc = mesh.vertices.size();

+

+	Vector3 n = p_end-p_begin;

+	float min = 1e20;

+	bool col=false;

+

+	for(int i=0;i<fc;i++) {

+

+		if (faces[i].plane.normal.dot(n) > 0)

+			continue; //opposing face

+

+		int ic = faces[i].indices.size();

+		const int *ind=faces[i].indices.ptr();

+

+		for(int j=1;j<ic-1;j++) {

+

+			Face3 f(vertices[ind[0]],vertices[ind[i]],vertices[ind[i+1]]);

+			Vector3 result;

+			if (f.intersects_segment(p_begin,p_end,&result)) {

+				float d = n.dot(result);

+				if (d<min) {

+					min=d;

+					r_result=result;

+					r_normal=faces[i].plane.normal;

+					col=true;

+				}

+

+				break;

+			}

+

+		}

+	}

+

+	return col;

+

+}

+

+Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(float p_mass) const {

+

+	// use crappy AABB approximation

+	Vector3 extents=get_aabb().size*0.5;

+

+	return  Vector3(

+		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),

+		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),

+		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)

+	);

+

+}

+

+void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices)  {

+

+	Error err = QuickHull::build(p_vertices,mesh);

+	AABB _aabb;

+

+	for(int i=0;i<mesh.vertices.size();i++) {

+

+		if (i==0)

+			_aabb.pos=mesh.vertices[i];

+		else

+			_aabb.expand_to(mesh.vertices[i]);

+	}

+

+	configure(_aabb);

+

+

+}

+

+void ConvexPolygonShapeSW::set_data(const Variant& p_data) {

+

+	_setup(p_data);

+

+}

+

+Variant ConvexPolygonShapeSW::get_data() const {

+

+	return mesh.vertices;

+}

+

+

+ConvexPolygonShapeSW::ConvexPolygonShapeSW()  {

+

+

+}

+

+

+/********** FACE POLYGON *************/

+

+

+void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &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;

+	}

+}

+

+Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const {

+

+

+	Vector3 n=p_normal;

+

+	int vert_support_idx=-1;

+	float support_max;

+

+	for (int i=0;i<3;i++) {

+

+		//float d=n.dot(vertex[i]);

+		float d=p_normal.dot(vertex[i]);

+

+		if (i==0 || d > support_max) {

+			support_max=d;

+			vert_support_idx=i;

+		}

+	}

+

+	return vertex[vert_support_idx];

+}

+

+void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

+

+	Vector3 n=p_normal;

+

+	/** TEST FACE AS SUPPORT **/

+	if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {

+

+		r_amount=3;

+		for (int i=0;i<3;i++) {

+

+			r_supports[i]=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++) {

+

+		int nx=(i+1)%3;

+		//if (i!=vert_support_idx && nx!=vert_support_idx)

+		//	continue;

+

+	// check if edge is valid as a support

+		float dot=(vertex[i]-vertex[nx]).normalized().dot(n);

+		dot=ABS(dot);

+		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {

+

+			r_amount=2;

+			r_supports[0]=vertex[i];

+			r_supports[1]=vertex[nx];

+			return;

+		}

+	}

+

+	r_amount=1;

+	r_supports[0]=vertex[vert_support_idx];

+}

+

+bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

+

+

+	bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result);

+	if (c)

+		r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal;

+

+	return c;

+}

+

+Vector3 FaceShapeSW::get_moment_of_inertia(float p_mass) const {

+

+	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!

+

+}

+

+FaceShapeSW::FaceShapeSW()  {

+

+	configure(AABB());

+

+}

+

+

+

+DVector<Vector3> ConcavePolygonShapeSW::get_faces() const {

+

+

+	DVector<Vector3> rfaces;

+	rfaces.resize(faces.size()*3);

+

+	for(int i=0;i<faces.size();i++) {

+

+		Face f=faces.get(i);

+

+		for(int j=0;j<3;j++) {

+

+			rfaces.set(i*3+j, vertices.get( f.indices[j] ) );

+		}

+	}

+

+	return rfaces;

+}

+

+void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

+

+	int count=vertices.size();

+	DVector<Vector3>::Read r=vertices.read();

+	const Vector3 *vptr=r.ptr();

+

+	for (int i=0;i<count;i++) {

+

+		float d=p_normal.dot( p_transform.xform( vptr[i] ) );

+

+		if (i==0 || d > r_max)

+			r_max=d;

+		if (i==0 || d < r_min)

+			r_min=d;

+

+	}

+}

+

+Vector3 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const {

+

+

+	int count=vertices.size();

+	DVector<Vector3>::Read r=vertices.read();

+	const Vector3 *vptr=r.ptr();

+

+	Vector3 n=p_normal;

+

+	int vert_support_idx=-1;

+	float support_max;

+

+	for (int i=0;i<count;i++) {

+

+		float d=n.dot(vptr[i]);

+

+		if (i==0 || d > support_max) {

+			support_max=d;

+			vert_support_idx=i;

+		}

+	}

+

+

+	return vptr[vert_support_idx];

+

+}

+

+void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const {

+

+	const BVH *bvh=&p_params->bvh[p_idx];

+

+

+	//if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)

+	//	return; //test against whole AABB, which isn't very costly

+

+

+	//printf("addr: %p\n",bvh);

+	if (!bvh->aabb.intersects_segment(p_params->from,p_params->to)) {

+

+		return;

+	}

+

+

+	if (bvh->face_index>=0) {

+

+

+		Vector3 res;

+		Vector3 vertices[3]={

+			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ],

+			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ],

+			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ]

+		};

+

+		if (Geometry::segment_intersects_triangle(

+				p_params->from,

+				p_params->to,

+				vertices[0],

+				vertices[1],

+				vertices[2],

+				&res)) {

+

+

+			float d=p_params->normal.dot(res) - p_params->normal.dot(p_params->from);

+			//TODO, seems segmen/triangle intersection is broken :(

+			if (d>0 && d<p_params->min_d) {

+

+				p_params->min_d=d;

+				p_params->result=res;

+				p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal;

+				p_params->collisions++;

+			}

+

+		}

+

+

+

+	} else {

+

+		if (bvh->left>=0)

+			_cull_segment(bvh->left,p_params);

+		if (bvh->right>=0)

+			_cull_segment(bvh->right,p_params);

+

+

+	}

+}

+

+bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

+

+	// unlock data

+	DVector<Face>::Read fr=faces.read();

+	DVector<Vector3>::Read vr=vertices.read();

+	DVector<BVH>::Read br=bvh.read();

+

+

+	_SegmentCullParams params;

+	params.from=p_begin;

+	params.to=p_end;

+	params.collisions=0;

+	params.normal=(p_end-p_begin).normalized();

+

+	params.faces=fr.ptr();

+	params.vertices=vr.ptr();

+	params.bvh=br.ptr();

+

+	params.min_d=1e20;

+	// cull

+	_cull_segment(0,&params);

+

+	if (params.collisions>0) {

+

+

+		r_result=params.result;

+		r_normal=params.normal;

+		return true;

+	} else {

+

+		return false;

+	}

+}

+

+void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const {

+

+	const BVH* bvh=&p_params->bvh[p_idx];

+

+	if (!p_params->aabb.intersects( bvh->aabb ))

+		return;

+

+	if (bvh->face_index>=0) {

+

+		const Face *f=&p_params->faces[ bvh->face_index ];

+		FaceShapeSW *face=p_params->face;

+		face->normal=f->normal;

+		face->vertex[0]=p_params->vertices[f->indices[0]];

+		face->vertex[1]=p_params->vertices[f->indices[1]];

+		face->vertex[2]=p_params->vertices[f->indices[2]];

+		p_params->callback(p_params->userdata,face);

+

+	} else {

+

+		if (bvh->left>=0) {

+

+			_cull(bvh->left,p_params);

+

+		}

+

+		if (bvh->right>=0) {

+

+			_cull(bvh->right,p_params);

+		}

+

+	}

+}

+

+void ConcavePolygonShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {

+

+	// make matrix local to concave

+

+	AABB local_aabb=p_local_aabb;

+

+	// unlock data

+	DVector<Face>::Read fr=faces.read();

+	DVector<Vector3>::Read vr=vertices.read();

+	DVector<BVH>::Read br=bvh.read();

+

+	FaceShapeSW face; // use this to send in the callback

+

+	_CullParams params;

+	params.aabb=local_aabb;

+	params.face=&face;

+	params.faces=fr.ptr();

+	params.vertices=vr.ptr();

+	params.bvh=br.ptr();

+	params.callback=p_callback;

+	params.userdata=p_userdata;

+

+	// cull

+	_cull(0,&params);

+

+}

+

+Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(float p_mass) const {

+

+	// use crappy AABB approximation

+	Vector3 extents=get_aabb().size*0.5;

+

+	return Vector3(

+		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),

+		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),

+		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)

+	);

+}

+

+

+struct _VolumeSW_BVH_Element {

+

+	AABB aabb;

+	Vector3 center;

+	int face_index;

+};

+

+struct _VolumeSW_BVH_CompareX {

+

+	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {

+

+		return a.center.x<b.center.x;

+	}

+};

+

+

+struct _VolumeSW_BVH_CompareY {

+

+	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {

+

+		return a.center.y<b.center.y;

+	}

+};

+

+struct _VolumeSW_BVH_CompareZ {

+

+	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {

+

+		return a.center.z<b.center.z;

+	}

+};

+

+struct _VolumeSW_BVH {

+

+	AABB aabb;

+	_VolumeSW_BVH *left;

+	_VolumeSW_BVH *right;

+

+	int face_index;

+};

+

+

+_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) {

+

+	_VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH );

+

+	if (p_size==1) {

+		//leaf

+		bvh->aabb=p_elements[0].aabb;

+		bvh->left=NULL;

+		bvh->right=NULL;

+		bvh->face_index=p_elements->face_index;

+		count++;

+		return bvh;

+	} else {

+

+		bvh->face_index=-1;

+	}

+

+	AABB aabb;

+	for(int i=0;i<p_size;i++) {

+

+		if (i==0)

+			aabb=p_elements[i].aabb;

+		else

+			aabb.merge_with(p_elements[i].aabb);

+	}

+	bvh->aabb=aabb;

+	switch(aabb.get_longest_axis_index()) {

+

+		case 0: {

+

+			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareX> sort_x;

+			sort_x.sort(p_elements,p_size);

+

+		} break;

+		case 1: {

+

+			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareY> sort_y;

+			sort_y.sort(p_elements,p_size);

+		} break;

+		case 2: {

+

+			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareZ> sort_z;

+			sort_z.sort(p_elements,p_size);

+		} break;

+	}

+

+	int split=p_size/2;

+	bvh->left=_volume_sw_build_bvh(p_elements,split,count);

+	bvh->right=_volume_sw_build_bvh(&p_elements[split],p_size-split,count);

+

+//	printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);

+	count++;

+	return bvh;

+}

+

+

+void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) {

+

+	int idx=p_idx;

+

+

+	p_bvh_array[idx].aabb=p_bvh_tree->aabb;

+	p_bvh_array[idx].face_index=p_bvh_tree->face_index;

+//	printf("%p - %i: %i(%p)  -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);

+

+

+	if (p_bvh_tree->left) {

+		p_bvh_array[idx].left=++p_idx;

+		_fill_bvh(p_bvh_tree->left,p_bvh_array,p_idx);

+

+	} else {

+

+		p_bvh_array[p_idx].left=-1;

+	}

+

+	if (p_bvh_tree->right) {

+		p_bvh_array[idx].right=++p_idx;

+		_fill_bvh(p_bvh_tree->right,p_bvh_array,p_idx);

+

+	} else {

+

+		p_bvh_array[p_idx].right=-1;

+	}

+

+	memdelete(p_bvh_tree);

+

+}

+

+void ConcavePolygonShapeSW::_setup(DVector<Vector3> p_faces) {

+

+	int src_face_count=p_faces.size();

+	ERR_FAIL_COND(src_face_count%3);

+	src_face_count/=3;

+

+	DVector<Vector3>::Read r = p_faces.read();

+	const Vector3 * facesr= r.ptr();

+

+#if 0

+	Map<Vector3,int> point_map;

+	List<Face> face_list;

+

+

+	for(int i=0;i<src_face_count;i++) {

+

+		Face3 faceaux;

+

+		for(int j=0;j<3;j++) {

+

+			faceaux.vertex[j]=facesr[i*3+j].snapped(_POINT_SNAP);

+			//faceaux.vertex[j]=facesr[i*3+j];//facesr[i*3+j].snapped(_POINT_SNAP);

+		}

+

+		ERR_CONTINUE( faceaux.is_degenerate() );

+

+		Face face;

+

+		for(int j=0;j<3;j++) {

+

+

+			Map<Vector3,int>::Element *E=point_map.find(faceaux.vertex[j]);

+			if (E) {

+

+				face.indices[j]=E->value();

+			} else {

+

+				face.indices[j]=point_map.size();

+				point_map.insert(faceaux.vertex[j],point_map.size());

+

+			}

+		}

+

+		face_list.push_back(face);

+	}

+

+	vertices.resize( point_map.size() );

+

+	DVector<Vector3>::Write vw = vertices.write();

+	Vector3 *verticesw=vw.ptr();

+

+	AABB _aabb;

+

+	for( Map<Vector3,int>::Element *E=point_map.front();E;E=E->next()) {

+

+		if (E==point_map.front()) {

+			_aabb.pos=E->key();

+		} else {

+

+			_aabb.expand_to(E->key());

+		}

+		verticesw[E->value()]=E->key();

+	}

+

+	point_map.clear(); // not needed anymore

+

+	faces.resize(face_list.size());

+	DVector<Face>::Write w = faces.write();

+	Face *facesw=w.ptr();

+

+	int fc=0;

+

+	for( List<Face>::Element *E=face_list.front();E;E=E->next()) {

+

+		facesw[fc++]=E->get();

+	}

+

+	face_list.clear();

+

+

+	DVector<_VolumeSW_BVH_Element> bvh_array;

+	bvh_array.resize( fc );

+

+	DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();

+	_VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();

+

+

+	for(int i=0;i<fc;i++) {

+

+		AABB face_aabb;

+		face_aabb.pos=verticesw[facesw[i].indices[0]];

+		face_aabb.expand_to( verticesw[facesw[i].indices[1]] );

+		face_aabb.expand_to( verticesw[facesw[i].indices[2]] );

+

+		bvh_arrayw[i].face_index=i;

+		bvh_arrayw[i].aabb=face_aabb;

+		bvh_arrayw[i].center=face_aabb.pos+face_aabb.size*0.5;

+

+	}

+

+	w=DVector<Face>::Write();

+	vw=DVector<Vector3>::Write();

+

+

+	int count=0;

+	_VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,fc,count);

+

+	ERR_FAIL_COND(count==0);

+

+	bvhw=DVector<_VolumeSW_BVH_Element>::Write();

+

+	bvh.resize( count+1 );

+

+	DVector<BVH>::Write bvhw2 = bvh.write();

+	BVH*bvh_arrayw2=bvhw2.ptr();

+

+	int idx=0;

+	_fill_bvh(bvh_tree,bvh_arrayw2,idx);

+

+	set_aabb(_aabb);

+

+#else

+	DVector<_VolumeSW_BVH_Element> bvh_array;

+	bvh_array.resize( src_face_count );

+

+	DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();

+	_VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();

+

+	faces.resize(src_face_count);

+	DVector<Face>::Write w = faces.write();

+	Face *facesw=w.ptr();

+

+	vertices.resize( src_face_count*3 );

+

+	DVector<Vector3>::Write vw = vertices.write();

+	Vector3 *verticesw=vw.ptr();

+

+	AABB _aabb;

+

+

+	for(int i=0;i<src_face_count;i++) {

+

+		Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] );

+

+		bvh_arrayw[i].aabb=face.get_aabb();

+		bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5;

+		bvh_arrayw[i].face_index=i;

+		facesw[i].indices[0]=i*3+0;

+		facesw[i].indices[1]=i*3+1;

+		facesw[i].indices[2]=i*3+2;

+		facesw[i].normal=face.get_plane().normal;

+		verticesw[i*3+0]=face.vertex[0];

+		verticesw[i*3+1]=face.vertex[1];

+		verticesw[i*3+2]=face.vertex[2];

+		if (i==0)

+			_aabb=bvh_arrayw[i].aabb;

+		else

+			_aabb.merge_with(bvh_arrayw[i].aabb);

+

+	}

+

+	w=DVector<Face>::Write();

+	vw=DVector<Vector3>::Write();

+

+	int count=0;

+	_VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count);

+

+	bvh.resize( count+1 );

+

+	DVector<BVH>::Write bvhw2 = bvh.write();

+	BVH*bvh_arrayw2=bvhw2.ptr();

+

+	int idx=0;

+	_fill_bvh(bvh_tree,bvh_arrayw2,idx);

+

+	configure(_aabb); // this type of shape has no margin

+

+

+#endif

+}

+

+

+void ConcavePolygonShapeSW::set_data(const Variant& p_data) {

+

+

+	_setup(p_data);

+}

+

+Variant ConcavePolygonShapeSW::get_data() const {

+

+	return get_faces();

+}

+

+ConcavePolygonShapeSW::ConcavePolygonShapeSW() {

+

+

+}

+

+

+

+/* HEIGHT MAP SHAPE */

+

+DVector<float> HeightMapShapeSW::get_heights() const {

+

+	return heights;

+}

+int HeightMapShapeSW::get_width() const {

+

+	return width;

+}

+int HeightMapShapeSW::get_depth() const {

+

+	return depth;

+}

+float HeightMapShapeSW::get_cell_size() const {

+

+	return cell_size;

+}

+

+

+void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

+

+	//not very useful, but not very used either

+	p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max );

+

+}

+

+Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const {

+

+

+	//not very useful, but not very used either

+	return get_aabb().get_support(p_normal);

+

+}

+

+bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const {

+

+

+	return false;

+}

+

+

+void HeightMapShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {

+

+

+

+}

+

+

+Vector3 HeightMapShapeSW::get_moment_of_inertia(float p_mass) const {

+

+

+	// use crappy AABB approximation

+	Vector3 extents=get_aabb().size*0.5;

+

+	return Vector3(

+		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),

+		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),

+		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)

+	);

+}

+

+

+void HeightMapShapeSW::_setup(DVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) {

+

+	heights=p_heights;

+	width=p_width;

+	depth=p_depth;;

+	cell_size=p_cell_size;

+

+	DVector<real_t>::Read r = heights. read();

+

+	AABB aabb;

+

+	for(int i=0;i<depth;i++) {

+

+		for(int j=0;j<width;j++) {

+

+			float h = r[i*width+j];

+

+			Vector3 pos( j*cell_size, h, i*cell_size );

+			if (i==0 || j==0)

+				aabb.pos=pos;

+			else

+				aabb.expand_to(pos);

+

+		}

+	}

+

+

+	configure(aabb);

+}

+

+void HeightMapShapeSW::set_data(const Variant& p_data) {

+

+	ERR_FAIL_COND( p_data.get_type()!=Variant::DICTIONARY );

+	Dictionary d=p_data;

+	ERR_FAIL_COND( !d.has("width") );

+	ERR_FAIL_COND( !d.has("depth") );

+	ERR_FAIL_COND( !d.has("cell_size") );

+	ERR_FAIL_COND( !d.has("heights") );

+

+	int width=d["width"];

+	int depth=d["depth"];

+	float cell_size=d["cell_size"];

+	DVector<float> heights=d["heights"];

+

+	ERR_FAIL_COND( width<= 0);

+	ERR_FAIL_COND( depth<= 0);

+	ERR_FAIL_COND( cell_size<= CMP_EPSILON);

+	ERR_FAIL_COND( heights.size() != (width*depth) );

+	_setup(heights, width, depth, cell_size );

+

+}

+

+Variant HeightMapShapeSW::get_data() const {

+

+	ERR_FAIL_V(Variant());

+

+}

+

+HeightMapShapeSW::HeightMapShapeSW() {

+

+	width=0;

+	depth=0;

+	cell_size=0;

+}

+

+

+

diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h
new file mode 100644
index 000000000..890d6d874
--- /dev/null
+++ b/servers/physics/shape_sw.h
@@ -0,0 +1,430 @@
+/*************************************************************************/
+/*  shape_sw.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 SHAPE_SW_H

+#define SHAPE_SW_H

+

+#include "servers/physics_server.h"

+#include "bsp_tree.h"

+#include "geometry.h"

+/*

+

+SHAPE_LINE, ///< plane:"plane"

+SHAPE_SEGMENT, ///< float:"length"

+SHAPE_CIRCLE, ///< float:"radius"

+SHAPE_RECTANGLE, ///< vec3:"extents"

+SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"

+SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)

+SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error

+

+*/

+

+class ShapeSW;

+

+class ShapeOwnerSW {

+public:

+

+	virtual void _shape_changed()=0;

+	virtual void remove_shape(ShapeSW *p_shape)=0;

+

+	virtual ~ShapeOwnerSW() {}

+};

+

+

+class ShapeSW {

+

+	RID self;

+	AABB aabb;

+	bool configured;

+	real_t custom_bias;

+

+	Map<ShapeOwnerSW*,int> owners;

+protected:

+

+	void configure(const AABB& p_aabb);

+public:

+

+	enum {

+		MAX_SUPPORTS=8

+	};

+

+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }

+	_FORCE_INLINE_ RID get_self() const {return  self; }

+

+	virtual PhysicsServer::ShapeType get_type() const=0;

+

+	_FORCE_INLINE_ AABB get_aabb() const { return aabb; }

+	_FORCE_INLINE_ bool is_configured() const { return configured; }

+

+	virtual bool is_concave() const { return false; }

+

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const=0;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const=0;

+

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0;

+	virtual Vector3 get_moment_of_inertia(float p_mass) const=0;

+

+	virtual void set_data(const Variant& p_data)=0;

+	virtual Variant get_data() const=0;

+

+	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }

+	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }

+

+	void add_owner(ShapeOwnerSW *p_owner);

+	void remove_owner(ShapeOwnerSW *p_owner);

+	bool is_owner(ShapeOwnerSW *p_owner) const;

+	const Map<ShapeOwnerSW*,int>& get_owners() const;

+

+	ShapeSW();

+	virtual ~ShapeSW();

+};

+

+

+class ConcaveShapeSW : public ShapeSW {

+

+public:

+

+	virtual bool is_concave() const { return true; }

+	typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex);

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }

+

+	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0;

+

+	ConcaveShapeSW() {}

+};

+

+class PlaneShapeSW : public ShapeSW {

+

+	Plane plane;

+

+	void _setup(const Plane& p_plane);

+public:

+

+	Plane get_plane() const;

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; }

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }

+

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	virtual Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	PlaneShapeSW();

+};

+

+class RayShapeSW : public ShapeSW {

+

+	float length;

+

+	void _setup(float p_length);

+public:

+

+	float get_length() const;

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	virtual Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	RayShapeSW();

+};

+

+class SphereShapeSW : public ShapeSW {

+

+	real_t radius;

+

+	void _setup(real_t p_radius);

+public:

+

+	real_t get_radius() const;

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }

+

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	virtual Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	SphereShapeSW();

+};

+

+class BoxShapeSW : public ShapeSW {

+

+	Vector3 half_extents;

+	void _setup(const Vector3& p_half_extents);

+public:

+

+	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }

+

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	virtual Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	BoxShapeSW();

+};

+

+class CapsuleShapeSW : public ShapeSW {

+

+	real_t height;

+	real_t radius;

+

+

+	void _setup(real_t p_height,real_t p_radius);

+public:

+

+	_FORCE_INLINE_ real_t get_height() const { return height; }

+	_FORCE_INLINE_ real_t get_radius() const { return radius; }

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }

+

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	virtual Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	CapsuleShapeSW();

+};

+

+struct ConvexPolygonShapeSW : public ShapeSW {

+

+	Geometry::MeshData mesh;

+

+	void _setup(const Vector<Vector3>& p_vertices);

+public:

+

+	const Geometry::MeshData& get_mesh() const { return mesh; }

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }

+

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	virtual Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	ConvexPolygonShapeSW();

+

+};

+

+

+struct _VolumeSW_BVH;

+struct FaceShapeSW;

+

+struct ConcavePolygonShapeSW : public ConcaveShapeSW {

+	// always a trimesh

+

+	struct Face {

+

+		Vector3 normal;

+		int indices[3];

+	};

+

+	DVector<Face> faces;

+	DVector<Vector3> vertices;

+

+	struct BVH {

+

+		AABB aabb;

+		int left;

+		int right;

+

+		int face_index;

+	};

+

+	DVector<BVH> bvh;

+

+	struct _CullParams {

+

+		AABB aabb;

+		Callback callback;

+		void *userdata;

+		const Face *faces;

+		const Vector3 *vertices;

+		const BVH *bvh;

+		FaceShapeSW *face;

+	};

+

+	struct _SegmentCullParams {

+

+		Vector3 from;

+		Vector3 to;

+		const Face *faces;

+		const Vector3 *vertices;

+		const BVH *bvh;

+

+		Vector3 result;

+		Vector3 normal;

+		real_t min_d;

+		int collisions;

+

+	};

+

+	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;

+	void _cull(int p_idx,_CullParams *p_params) const;

+

+	void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx);

+

+

+	void _setup(DVector<Vector3> p_faces);

+public:

+

+	DVector<Vector3> get_faces() const;

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }

+

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;

+

+	virtual Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	ConcavePolygonShapeSW();

+

+};

+

+

+struct HeightMapShapeSW : public ConcaveShapeSW {

+

+	DVector<real_t> heights;

+	int width;

+	int depth;

+	float cell_size;

+

+//	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;

+//	void _cull(int p_idx,_CullParams *p_params) const;

+

+	void _setup(DVector<float> p_heights,int p_width,int p_depth,float p_cell_size);

+public:

+

+	DVector<real_t> get_heights() const;

+	int get_width() const;

+	int get_depth() const;

+	float get_cell_size() const;

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; }

+

+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	virtual Vector3 get_support(const Vector3& p_normal) const;

+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;

+

+	virtual Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	HeightMapShapeSW();

+

+};

+

+//used internally

+struct FaceShapeSW : public ShapeSW {

+

+	Vector3 normal; //cache

+	Vector3 vertex[3];

+

+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }

+

+	const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; }

+

+	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;

+	Vector3 get_support(const Vector3& p_normal) const;

+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;

+	bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;

+

+	Vector3 get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data) {}

+	virtual Variant get_data() const { return Variant(); }

+

+	FaceShapeSW();

+};

+

+

+

+

+

+struct _ShapeTestConvexBSPSW {

+

+	const BSP_Tree *bsp;

+	const ShapeSW *shape;

+	Transform transform;

+

+	_FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const {

+

+		shape->project_range(p_normal,transform,r_min,r_max);

+	}

+

+};

+

+

+

+

+#endif // SHAPESW_H

diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
new file mode 100644
index 000000000..ca3aa7e1f
--- /dev/null
+++ b/servers/physics/space_sw.cpp
@@ -0,0 +1,429 @@
+/*************************************************************************/
+/*  space_sw.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 "space_sw.h"

+#include "collision_solver_sw.h"

+#include "physics_server_sw.h"

+

+

+bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {

+

+

+	ERR_FAIL_COND_V(space->locked,false);

+

+	Vector3 begin,end;

+	Vector3 normal;

+	begin=p_from;

+	end=p_to;

+	normal=(end-begin).normalized();

+

+

+	int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);

+

+

+	//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision

+

+	bool collided=false;

+	Vector3 res_point,res_normal;

+	int res_shape;

+	const CollisionObjectSW *res_obj;

+	real_t min_d=1e10;

+

+

+	for(int i=0;i<amount;i++) {

+

+		if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)

+			continue; //ignore area

+

+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))

+			continue;

+

+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];

+

+		int shape_idx=space->intersection_query_subindex_results[i];

+		Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();

+

+		Vector3 local_from = inv_xform.xform(begin);

+		Vector3 local_to = inv_xform.xform(end);

+

+		const ShapeSW *shape = col_obj->get_shape(shape_idx);

+

+		Vector3 shape_point,shape_normal;

+

+		if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {

+

+			Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);

+			shape_point=xform.xform(shape_point);

+

+			real_t ld = normal.dot(shape_point);

+

+

+			if (ld<min_d) {

+

+				min_d=ld;

+				res_point=shape_point;

+				res_normal=inv_xform.basis.xform_inv(shape_normal).normalized();

+				res_shape=shape_idx;

+				res_obj=col_obj;

+				collided=true;

+			}

+		}

+

+	}

+

+	if (!collided)

+		return false;

+

+

+	r_result.collider_id=res_obj->get_instance_id();

+	if (r_result.collider_id!=0)

+		r_result.collider=ObjectDB::get_instance(r_result.collider_id);

+	r_result.normal=res_normal;

+	r_result.position=res_point;

+	r_result.rid=res_obj->get_self();

+	r_result.shape=res_shape;

+

+	return true;

+

+}

+

+

+int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {

+

+	if (p_result_max<=0)

+		return 0;

+

+	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);

+	ERR_FAIL_COND_V(!shape,0);

+

+	AABB aabb = p_xform.xform(shape->get_aabb());

+

+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);

+

+	bool collided=false;

+	int cc=0;

+

+	//Transform ai = p_xform.affine_inverse();

+

+	for(int i=0;i<amount;i++) {

+

+		if (cc>=p_result_max)

+			break;

+

+		if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)

+			continue; //ignore area

+

+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))

+			continue;

+

+

+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];

+		int shape_idx=space->intersection_query_subindex_results[i];

+

+		if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL))

+			continue;

+

+		r_results[cc].collider_id=col_obj->get_instance_id();

+		if (r_results[cc].collider_id!=0)

+			r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);

+		r_results[cc].rid=col_obj->get_self();

+		r_results[cc].shape=shape_idx;

+

+		cc++;

+

+	}

+

+	return cc;

+

+}

+

+PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {

+

+

+	space=NULL;

+}

+

+

+////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+

+

+

+

+

+

+

+

+

+void* SpaceSW::_broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self) {

+

+	CollisionObjectSW::Type type_A=A->get_type();

+	CollisionObjectSW::Type type_B=B->get_type();

+	if (type_A>type_B) {

+

+		SWAP(A,B);

+		SWAP(p_subindex_A,p_subindex_B);

+		SWAP(type_A,type_B);

+	}

+

+	SpaceSW *self = (SpaceSW*)p_self;

+

+	if (type_A==CollisionObjectSW::TYPE_AREA) {

+

+

+		ERR_FAIL_COND_V(type_B!=CollisionObjectSW::TYPE_BODY,NULL);

+		AreaSW *area=static_cast<AreaSW*>(A);

+		BodySW *body=static_cast<BodySW*>(B);

+

+

+		AreaPairSW *area_pair = memnew(AreaPairSW(body,p_subindex_B,area,p_subindex_A) );

+

+		return area_pair;

+	} else {

+

+

+		BodyPairSW *b = memnew( BodyPairSW((BodySW*)A,p_subindex_A,(BodySW*)B,p_subindex_B) );

+		return b;

+

+	}

+

+	return NULL;

+}

+

+void SpaceSW::_broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self) {

+

+

+

+	SpaceSW *self = (SpaceSW*)p_self;

+	ConstraintSW *c = (ConstraintSW*)p_data;

+	memdelete(c);

+}

+

+

+const SelfList<BodySW>::List& SpaceSW::get_active_body_list() const {

+

+	return active_list;

+}

+void SpaceSW::body_add_to_active_list(SelfList<BodySW>* p_body) {

+

+	active_list.add(p_body);

+}

+void SpaceSW::body_remove_from_active_list(SelfList<BodySW>* p_body) {

+

+	active_list.remove(p_body);

+

+}

+

+void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW>* p_body) {

+

+

+	inertia_update_list.add(p_body);

+}

+

+void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW>* p_body) {

+

+	inertia_update_list.remove(p_body);

+}

+

+BroadPhaseSW *SpaceSW::get_broadphase() {

+

+	return broadphase;

+}

+

+void SpaceSW::add_object(CollisionObjectSW *p_object) {

+

+	ERR_FAIL_COND( objects.has(p_object) );

+	objects.insert(p_object);

+}

+

+void SpaceSW::remove_object(CollisionObjectSW *p_object) {

+

+	ERR_FAIL_COND( !objects.has(p_object) );

+	objects.erase(p_object);

+}

+

+const Set<CollisionObjectSW*> &SpaceSW::get_objects() const {

+

+	return objects;

+}

+

+void SpaceSW::body_add_to_state_query_list(SelfList<BodySW>* p_body) {

+

+	state_query_list.add(p_body);

+}

+void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW>* p_body) {

+

+	state_query_list.remove(p_body);

+}

+

+void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW>* p_area) {

+

+	monitor_query_list.add(p_area);

+}

+void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area) {

+

+	monitor_query_list.remove(p_area);

+}

+

+void SpaceSW::area_add_to_moved_list(SelfList<AreaSW>* p_area) {

+

+	area_moved_list.add(p_area);

+}

+

+void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW>* p_area) {

+

+	area_moved_list.remove(p_area);

+}

+

+const SelfList<AreaSW>::List& SpaceSW::get_moved_area_list() const {

+

+	return area_moved_list;

+}

+

+

+

+

+void SpaceSW::call_queries() {

+

+	while(state_query_list.first()) {

+

+		BodySW * b = state_query_list.first()->self();

+		b->call_queries();

+		state_query_list.remove(state_query_list.first());

+	}

+

+	while(monitor_query_list.first()) {

+

+		AreaSW * a = monitor_query_list.first()->self();

+		a->call_queries();

+		monitor_query_list.remove(monitor_query_list.first());

+	}

+

+}

+

+void SpaceSW::setup() {

+

+

+	while(inertia_update_list.first()) {

+		inertia_update_list.first()->self()->update_inertias();

+		inertia_update_list.remove(inertia_update_list.first());

+	}

+

+

+}

+

+void SpaceSW::update() {

+

+	broadphase->update();

+

+}

+

+

+void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {

+

+	switch(p_param) {

+

+		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;

+		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;

+		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;

+		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break;

+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break;

+		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;

+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break;

+		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;

+	}

+}

+

+real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const {

+

+	switch(p_param) {

+

+		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;

+		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;

+		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration;

+		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold;

+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold;

+		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;

+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;

+		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;

+	}

+	return 0;

+}

+

+void SpaceSW::lock() {

+

+	locked=true;

+}

+

+void SpaceSW::unlock() {

+

+	locked=false;

+}

+

+bool SpaceSW::is_locked() const {

+

+	return locked;

+}

+

+PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() {

+

+	return direct_access;

+}

+

+SpaceSW::SpaceSW() {

+

+

+	locked=false;

+	contact_recycle_radius=0.01;

+	contact_max_separation=0.05;

+	contact_max_allowed_penetration= 0.01;

+

+	constraint_bias = 0.01;

+	body_linear_velocity_sleep_treshold=0.01;

+	body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI);

+	body_time_to_sleep=0.5;

+	body_angular_velocity_damp_ratio=10;

+

+

+	broadphase = BroadPhaseSW::create_func();

+	broadphase->set_pair_callback(_broadphase_pair,this);

+	broadphase->set_unpair_callback(_broadphase_unpair,this);

+	area=NULL;

+

+	direct_access = memnew( PhysicsDirectSpaceStateSW );

+	direct_access->space=this;

+}

+

+SpaceSW::~SpaceSW() {

+

+	memdelete(broadphase);

+	memdelete( direct_access );

+}

+

+

+

diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h
new file mode 100644
index 000000000..202c7ccbd
--- /dev/null
+++ b/servers/physics/space_sw.h
@@ -0,0 +1,157 @@
+/*************************************************************************/
+/*  space_sw.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 SPACE_SW_H

+#define SPACE_SW_H

+

+#include "typedefs.h"

+#include "hash_map.h"

+#include "body_sw.h"

+#include "area_sw.h"

+#include "body_pair_sw.h"

+#include "area_pair_sw.h"

+#include "broad_phase_sw.h"

+#include "collision_object_sw.h"

+

+

+class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState {

+

+	OBJ_TYPE( PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState );

+public:

+

+	SpaceSW *space;

+

+	bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);

+	int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);

+

+	PhysicsDirectSpaceStateSW();

+};

+

+

+

+class SpaceSW {

+

+

+	PhysicsDirectSpaceStateSW *direct_access;

+	RID self;

+

+	BroadPhaseSW *broadphase;

+	SelfList<BodySW>::List active_list;

+	SelfList<BodySW>::List inertia_update_list;

+	SelfList<BodySW>::List state_query_list;

+	SelfList<AreaSW>::List monitor_query_list;

+	SelfList<AreaSW>::List area_moved_list;

+

+	static void* _broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self);

+	static void _broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self);

+

+	Set<CollisionObjectSW*> objects;

+

+	AreaSW *area;

+

+	real_t contact_recycle_radius;

+	real_t contact_max_separation;

+	real_t contact_max_allowed_penetration;

+	real_t constraint_bias;

+

+	enum {

+

+		INTERSECTION_QUERY_MAX=2048

+	};

+

+	CollisionObjectSW *intersection_query_results[INTERSECTION_QUERY_MAX];

+	int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];

+

+	float body_linear_velocity_sleep_treshold;

+	float body_angular_velocity_sleep_treshold;

+	float body_time_to_sleep;

+	float body_angular_velocity_damp_ratio;

+

+	bool locked;

+

+friend class PhysicsDirectSpaceStateSW;

+

+public:

+

+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }

+	_FORCE_INLINE_ RID get_self() const { return self; }

+

+	void set_default_area(AreaSW *p_area) { area=p_area; }

+	AreaSW *get_default_area() const { return area; }

+

+	const SelfList<BodySW>::List& get_active_body_list() const;

+	void body_add_to_active_list(SelfList<BodySW>* p_body);

+	void body_remove_from_active_list(SelfList<BodySW>* p_body);

+	void body_add_to_inertia_update_list(SelfList<BodySW>* p_body);

+	void body_remove_from_inertia_update_list(SelfList<BodySW>* p_body);

+

+	void body_add_to_state_query_list(SelfList<BodySW>* p_body);

+	void body_remove_from_state_query_list(SelfList<BodySW>* p_body);

+

+	void area_add_to_monitor_query_list(SelfList<AreaSW>* p_area);

+	void area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area);

+	void area_add_to_moved_list(SelfList<AreaSW>* p_area);

+	void area_remove_from_moved_list(SelfList<AreaSW>* p_area);

+	const SelfList<AreaSW>::List& get_moved_area_list() const;

+

+	BroadPhaseSW *get_broadphase();

+

+	void add_object(CollisionObjectSW *p_object);

+	void remove_object(CollisionObjectSW *p_object);

+	const Set<CollisionObjectSW*> &get_objects() const;

+

+	_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }

+	_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }

+	_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }

+	_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }

+	_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; }

+	_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }

+	_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }

+	_FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }

+

+

+	void update();

+	void setup();

+	void call_queries();

+

+

+	bool is_locked() const;

+	void lock();

+	void unlock();

+

+	void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);

+	real_t get_param(PhysicsServer::SpaceParameter p_param) const;

+

+	PhysicsDirectSpaceStateSW *get_direct_state();

+

+	SpaceSW();

+	~SpaceSW();

+};

+

+

+#endif // SPACE__SW_H

diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp
new file mode 100644
index 000000000..b7815d225
--- /dev/null
+++ b/servers/physics/step_sw.cpp
@@ -0,0 +1,237 @@
+/*************************************************************************/
+/*  step_sw.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 "step_sw.h"
+
+
+void StepSW::_populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island) {
+
+	p_body->set_island_step(_step);
+	p_body->set_island_next(*p_island);
+	*p_island=p_body;
+
+	for(Map<ConstraintSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) {
+
+		ConstraintSW *c=(ConstraintSW*)E->key();
+		if (c->get_island_step()==_step)
+			continue; //already processed
+		c->set_island_step(_step);
+		c->set_island_next(*p_constraint_island);
+		*p_constraint_island=c;
+
+
+		for(int i=0;i<c->get_body_count();i++) {
+			if (i==E->get())
+				continue;
+			BodySW *b = c->get_body_ptr()[i];
+			if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+				continue; //no go
+			_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
+		}
+	}
+}
+
+void StepSW::_setup_island(ConstraintSW *p_island,float p_delta) {
+
+	ConstraintSW *ci=p_island;
+	while(ci) {
+		bool process = ci->setup(p_delta);
+		//todo remove from island if process fails
+		ci=ci->get_island_next();
+	}
+}
+
+void StepSW::_solve_island(ConstraintSW *p_island,int p_iterations,float p_delta){
+
+	for(int i=0;i<p_iterations;i++) {
+
+		ConstraintSW *ci=p_island;
+		while(ci) {
+			ci->solve(p_delta);
+			ci=ci->get_island_next();
+		}
+	}
+}
+
+void StepSW::_check_suspend(BodySW *p_island,float p_delta) {
+
+
+	bool can_sleep=true;
+
+	BodySW *b = p_island;
+	while(b) {
+
+		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+			continue; //ignore for static
+
+		if (!b->sleep_test(p_delta))
+			can_sleep=false;
+
+		b=b->get_island_next();
+	}
+
+	//put all to sleep or wake up everyoen
+
+	b = p_island;
+	while(b) {
+
+		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+			continue; //ignore for static
+
+		bool active = b->is_active();
+
+		if (active==can_sleep)
+			b->set_active(!can_sleep);
+
+		b=b->get_island_next();
+	}
+}
+
+void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) {
+
+	p_space->lock(); // can't access space during this
+
+	p_space->setup(); //update inertias, etc
+
+	const SelfList<BodySW>::List * body_list = &p_space->get_active_body_list();
+
+	/* INTEGRATE FORCES */
+	int active_count=0;
+
+	const SelfList<BodySW>*b = body_list->first();
+	while(b) {
+
+		b->self()->integrate_forces(p_delta);
+		b=b->next();
+		active_count++;
+	}
+
+
+
+	/* GENERATE CONSTRAINT ISLANDS */
+
+	BodySW *island_list=NULL;
+	ConstraintSW *constraint_island_list=NULL;
+	b = body_list->first();
+
+	int island_count=0;
+
+	while(b) {
+		BodySW *body = b->self();
+
+
+		if (body->get_island_step()!=_step) {
+
+			BodySW *island=NULL;
+			ConstraintSW *constraint_island=NULL;
+			_populate_island(body,&island,&constraint_island);
+
+			island->set_island_list_next(island_list);
+			island_list=island;
+
+			if (constraint_island) {
+				constraint_island->set_island_list_next(constraint_island_list);
+				constraint_island_list=constraint_island;
+				island_count++;
+			}
+
+		}
+		b=b->next();
+	}
+
+	const SelfList<AreaSW>::List &aml = p_space->get_moved_area_list();
+
+	while(aml.first()) {
+		for(const Set<ConstraintSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) {
+
+			ConstraintSW*c=E->get();
+			if (c->get_island_step()==_step)
+				continue;
+			c->set_island_step(_step);
+			c->set_island_next(NULL);
+			c->set_island_list_next(constraint_island_list);
+			constraint_island_list=c;
+		}
+		p_space->area_remove_from_moved_list((SelfList<AreaSW>*)aml.first()); //faster to remove here
+	}
+
+//	print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
+	/* SETUP CONSTRAINT ISLANDS */
+
+	{
+		ConstraintSW *ci=constraint_island_list;
+		while(ci) {
+
+			_setup_island(ci,p_delta);
+			ci=ci->get_island_list_next();
+		}
+	}
+
+	/* SOLVE CONSTRAINT ISLANDS */
+
+	{
+		ConstraintSW *ci=constraint_island_list;
+		while(ci) {
+			//iterating each island separatedly improves cache efficiency
+			_solve_island(ci,p_iterations,p_delta);
+			ci=ci->get_island_list_next();
+		}
+	}
+
+	/* INTEGRATE VELOCITIES */
+
+	b = body_list->first();
+	while(b) {
+
+		b->self()->integrate_velocities(p_delta);
+		b=b->next();
+	}
+
+	/* SLEEP / WAKE UP ISLANDS */
+
+	{
+		BodySW *bi=island_list;
+		while(bi) {
+
+			_check_suspend(bi,p_delta);
+			bi=bi->get_island_list_next();
+		}
+	}
+
+	p_space->update();
+	p_space->unlock();
+	_step++;
+
+
+
+}
+
+StepSW::StepSW() {
+
+	_step=1;
+}
diff --git a/servers/physics/step_sw.h b/servers/physics/step_sw.h
new file mode 100644
index 000000000..9aaef9792
--- /dev/null
+++ b/servers/physics/step_sw.h
@@ -0,0 +1,48 @@
+/*************************************************************************/
+/*  step_sw.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 STEP_SW_H
+#define STEP_SW_H
+
+#include "space_sw.h"
+
+class StepSW {
+
+	uint64_t _step;
+
+	void _populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island);
+	void _setup_island(ConstraintSW *p_island,float p_delta);
+	void _solve_island(ConstraintSW *p_island,int p_iterations,float p_delta);
+	void _check_suspend(BodySW *p_island,float p_delta);
+public:
+
+	void step(SpaceSW* p_space,float p_delta,int p_iterations);
+	StepSW();
+};
+
+#endif // STEP__SW_H
diff --git a/servers/physics_2d/SCsub b/servers/physics_2d/SCsub
new file mode 100644
index 000000000..a2c2b51a6
--- /dev/null
+++ b/servers/physics_2d/SCsub
@@ -0,0 +1,4 @@
+Import('env')
+
+env.add_source_files(env.servers_sources,"*.cpp")
+
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp
new file mode 100644
index 000000000..6c391a7aa
--- /dev/null
+++ b/servers/physics_2d/area_2d_sw.cpp
@@ -0,0 +1,193 @@
+/*************************************************************************/
+/*  area_2d_sw.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 "area_2d_sw.h"
+#include "space_2d_sw.h"
+#include "body_2d_sw.h"
+
+Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
+
+void Area2DSW::_shapes_changed() {
+
+
+}
+
+void Area2DSW::set_transform(const Matrix32& p_transform) {
+
+	if (!moved_list.in_list() && get_space())
+		get_space()->area_add_to_moved_list(&moved_list);
+
+	_set_transform(p_transform);
+}
+
+void Area2DSW::set_space(Space2DSW *p_space) {
+
+	if (get_space()) {
+		if (monitor_query_list.in_list())
+			get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+		if (moved_list.in_list())
+			get_space()->area_remove_from_moved_list(&moved_list);
+
+	}
+
+	monitored_bodies.clear();
+
+	_set_space(p_space);
+}
+
+
+void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
+
+
+	if (p_id==monitor_callback_id) {
+		monitor_callback_method=p_method;
+		return;
+	}
+
+	_unregister_shapes();
+
+	monitor_callback_id=p_id;
+	monitor_callback_method=p_method;
+
+	monitored_bodies.clear();
+
+	_shape_changed();
+
+}
+
+
+
+void Area2DSW::set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode) {
+	bool do_override=p_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override==space_override_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+		return;
+	_unregister_shapes();
+	space_override_mode=p_mode;
+	_shape_changed();
+}
+
+void Area2DSW::set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value) {
+
+	switch(p_param) {
+		case Physics2DServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:  point_attenuation=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_DENSITY: density=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_PRIORITY: priority=p_value; ; break;
+	}
+
+
+}
+
+Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const {
+
+
+	switch(p_param) {
+		case Physics2DServer::AREA_PARAM_GRAVITY: return gravity;
+		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector;
+		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point;
+		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return  point_attenuation;
+		case Physics2DServer::AREA_PARAM_DENSITY: return density;
+		case Physics2DServer::AREA_PARAM_PRIORITY: return priority;
+	}
+
+	return Variant();
+}
+
+
+void Area2DSW::_queue_monitor_update() {
+
+	ERR_FAIL_COND(!get_space());
+
+	if (!monitor_query_list.in_list())
+		get_space()->area_add_to_monitor_query_list(&monitor_query_list);
+
+
+}
+
+void Area2DSW::call_queries() {
+
+	if (monitor_callback_id && !monitored_bodies.empty()) {
+
+		Variant res[5];
+		Variant *resptr[5];
+		for(int i=0;i<5;i++)
+			resptr[i]=&res[i];
+
+		Object *obj = ObjectDB::get_instance(monitor_callback_id);
+		if (!obj) {
+			monitored_bodies.clear();
+			monitor_callback_id=0;
+			return;
+		}
+
+
+
+		for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) {
+
+			if (E->get().state==0)
+				continue; //nothing happened
+
+			res[0]=E->get().state>0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
+			res[1]=E->key().rid;
+			res[2]=E->key().instance_id;
+			res[3]=E->key().body_shape;
+			res[4]=E->key().area_shape;
+
+			Variant::CallError ce;
+			obj->call(monitor_callback_method,(const Variant**)resptr,5,ce);
+		}
+	}
+
+	monitored_bodies.clear();
+
+	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+
+}
+
+Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this)  {
+
+	_set_static(true); //areas are never active
+	space_override_mode=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
+	gravity=9.80665;
+	gravity_vector=Vector2(0,-1);
+	gravity_is_point=false;
+	point_attenuation=1;
+	density=0.1;
+	priority=0;
+	monitor_callback_id=0;
+
+
+}
+
+Area2DSW::~Area2DSW() {
+
+
+}
+
diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h
new file mode 100644
index 000000000..51e6ccd16
--- /dev/null
+++ b/servers/physics_2d/area_2d_sw.h
@@ -0,0 +1,172 @@
+/*************************************************************************/
+/*  area_2d_sw.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 AREA_2D_SW_H
+#define AREA_2D_SW_H
+
+#include "servers/physics_2d_server.h"
+#include "collision_object_2d_sw.h"
+#include "self_list.h"
+//#include "servers/physics/query_sw.h"
+
+class Space2DSW;
+class Body2DSW;
+class Constraint2DSW;
+
+
+class Area2DSW : public CollisionObject2DSW{
+
+
+	Physics2DServer::AreaSpaceOverrideMode space_override_mode;
+	float gravity;
+	Vector2 gravity_vector;
+	bool gravity_is_point;
+	float point_attenuation;
+	float density;
+	int priority;
+
+	ObjectID monitor_callback_id;
+	StringName monitor_callback_method;
+
+	SelfList<Area2DSW> monitor_query_list;
+	SelfList<Area2DSW> moved_list;
+
+	struct BodyKey {
+
+		RID rid;
+		ObjectID instance_id;
+		uint32_t body_shape;
+		uint32_t area_shape;
+
+		_FORCE_INLINE_ bool operator<( const BodyKey& p_key) const {
+
+			if (rid==p_key.rid) {
+
+				if (body_shape==p_key.body_shape) {
+
+					return area_shape < p_key.area_shape;
+				} else
+					return body_shape < p_key.area_shape;
+			} else
+				return rid < p_key.rid;
+
+		}
+
+		_FORCE_INLINE_ BodyKey() {}
+		BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	};
+
+	struct BodyState {
+
+		int state;
+		_FORCE_INLINE_ void inc() { state++; }
+		_FORCE_INLINE_ void dec() { state--; }
+		_FORCE_INLINE_ BodyState() { state=0; }
+	};
+
+	Map<BodyKey,BodyState> monitored_bodies;
+
+	//virtual void shape_changed_notify(Shape2DSW *p_shape);
+	//virtual void shape_deleted_notify(Shape2DSW *p_shape);
+	Set<Constraint2DSW*> constraints;
+
+
+	virtual void _shapes_changed();
+	void _queue_monitor_update();
+
+public:
+
+	//_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; }
+	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
+
+	void set_monitor_callback(ObjectID p_id, const StringName& p_method);
+	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; }
+
+	_FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+
+	void set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value);
+	Variant get_param(Physics2DServer::AreaParameter p_param) const;
+
+	void set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode);
+	Physics2DServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; }
+
+	_FORCE_INLINE_ void set_gravity(float p_gravity) { gravity=p_gravity; }
+	_FORCE_INLINE_ float get_gravity() const { return gravity; }
+
+	_FORCE_INLINE_ void set_gravity_vector(const Vector2& p_gravity) { gravity_vector=p_gravity; }
+	_FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; }
+
+	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; }
+	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
+
+	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; }
+	_FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; }
+
+	_FORCE_INLINE_ void set_density(float p_density) { density=p_density; }
+	_FORCE_INLINE_ float get_density() const { return density; }
+
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; }
+	_FORCE_INLINE_ int get_priority() const { return priority; }
+
+	_FORCE_INLINE_ void add_constraint( Constraint2DSW* p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint( Constraint2DSW* p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const Set<Constraint2DSW*>& get_constraints() const { return constraints; }
+
+	void set_transform(const Matrix32& p_transform);
+
+	void set_space(Space2DSW *p_space);
+
+
+	void call_queries();
+
+	Area2DSW();
+	~Area2DSW();
+};
+
+void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+
+	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	monitored_bodies[bk].inc();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
+void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+
+	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	monitored_bodies[bk].dec();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
+
+
+
+
+
+
+#endif // AREA_2D_SW_H
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
new file mode 100644
index 000000000..da9a02922
--- /dev/null
+++ b/servers/physics_2d/area_pair_2d_sw.cpp
@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  area_pair_2d_sw.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 "area_pair_2d_sw.h"
+#include "collision_solver_2d_sw.h"
+
+
+bool AreaPair2DSW::setup(float p_step) {
+
+	bool result = CollisionSolver2DSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),body->get_shape_inv_transform(body_shape) * body->get_inv_transform(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),area->get_shape_inv_transform(area_shape) * area->get_inv_transform(),NULL,this);
+
+	if (result!=colliding) {
+
+		if (result) {
+
+			if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+				body->add_area(area);
+			if (area->has_monitor_callback())
+				area->add_body_to_query(body,body_shape,area_shape);
+
+		} else {
+
+			if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+				body->remove_area(area);
+			if (area->has_monitor_callback())
+				area->remove_body_from_query(body,body_shape,area_shape);
+
+		}
+
+		colliding=result;
+
+	}
+
+	return false; //never do any post solving
+}
+
+void AreaPair2DSW::solve(float p_step) {
+
+
+}
+
+
+AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape) {
+
+
+	body=p_body;
+	area=p_area;
+	body_shape=p_body_shape;
+	area_shape=p_area_shape;
+	colliding=false;
+	body->add_constraint(this,0);
+	area->add_constraint(this);
+
+}
+
+AreaPair2DSW::~AreaPair2DSW() {
+
+	if (colliding) {
+
+		if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+			body->remove_area(area);
+		if (area->has_monitor_callback())
+			area->remove_body_from_query(body,body_shape,area_shape);
+
+
+	}
+	body->remove_constraint(this);
+	area->remove_constraint(this);
+}
diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h
new file mode 100644
index 000000000..5dad8c7a1
--- /dev/null
+++ b/servers/physics_2d/area_pair_2d_sw.h
@@ -0,0 +1,53 @@
+/*************************************************************************/
+/*  area_pair_2d_sw.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 AREA_PAIR_2D_SW_H
+#define AREA_PAIR_2D_SW_H
+
+#include "constraint_2d_sw.h"
+#include "body_2d_sw.h"
+#include "area_2d_sw.h"
+
+class AreaPair2DSW : public Constraint2DSW {
+
+	Body2DSW *body;
+	Area2DSW *area;
+	int body_shape;
+	int area_shape;
+	bool colliding;
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape);
+	~AreaPair2DSW();
+};
+
+#endif // AREA_PAIR_2D_SW_H
+
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
new file mode 100644
index 000000000..f0e96ae5a
--- /dev/null
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -0,0 +1,609 @@
+/*************************************************************************/
+/*  body_2d_sw.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 "body_2d_sw.h"

+#include "space_2d_sw.h"

+#include "area_2d_sw.h"

+

+void Body2DSW::_update_inertia() {

+

+	if (get_space() && !inertia_update_list.in_list())

+		get_space()->body_add_to_inertia_update_list(&inertia_update_list);

+

+}

+

+

+

+void Body2DSW::update_inertias() {

+

+	//update shapes and motions

+

+	switch(mode) {

+

+		case Physics2DServer::BODY_MODE_RIGID: {

+

+			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)

+			float total_area=0;

+

+			for (int i=0;i<get_shape_count();i++) {

+

+				total_area+=get_shape_aabb(i).get_area();

+			}

+

+			real_t _inertia=0;

+

+			for (int i=0;i<get_shape_count();i++) {

+

+				const Shape2DSW* shape=get_shape(i);

+

+				float area=get_shape_aabb(i).get_area();

+

+				float mass = area * this->mass / total_area;

+

+				_inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin().length_squared();

+

+			}

+

+			if (_inertia!=0)

+				_inv_inertia=1.0/_inertia;

+			else

+				_inv_inertia=0.0; //wathever

+

+			if (mass)

+				_inv_mass=1.0/mass;

+			else

+				_inv_mass=0;

+

+		} break;

+		case Physics2DServer::BODY_MODE_STATIC_ACTIVE:

+		case Physics2DServer::BODY_MODE_STATIC: {

+

+			_inv_inertia=0;

+			_inv_mass=0;

+		} break;

+		case Physics2DServer::BODY_MODE_CHARACTER: {

+

+			_inv_inertia=0;

+			_inv_mass=1.0/mass;

+

+		} break;

+	}

+	//_update_inertia_tensor();

+

+	//_update_shapes();

+

+}

+

+

+

+void Body2DSW::set_active(bool p_active) {

+

+	if (active==p_active)

+		return;

+

+	active=p_active;

+	if (!p_active) {

+		if (get_space())

+			get_space()->body_remove_from_active_list(&active_list);

+	} else {

+		if (mode==Physics2DServer::BODY_MODE_STATIC)

+			return; //static bodies can't become active

+		if (get_space())

+			get_space()->body_add_to_active_list(&active_list);

+

+		//still_time=0;

+	}

+/*

+	if (!space)

+		return;

+

+	for(int i=0;i<get_shape_count();i++) {

+		Shape &s=shapes[i];

+		if (s.bpid>0) {

+			get_space()->get_broadphase()->set_active(s.bpid,active);

+		}

+	}

+*/

+}

+

+

+

+void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value) {

+

+	switch(p_param) {

+		case Physics2DServer::BODY_PARAM_BOUNCE: {

+

+			bounce=p_value;

+		} break;

+		case Physics2DServer::BODY_PARAM_FRICTION: {

+

+			friction=p_value;

+		} break;

+		case Physics2DServer::BODY_PARAM_MASS: {

+			ERR_FAIL_COND(p_value<=0);

+			mass=p_value;

+			_update_inertia();

+

+		} break;

+		default:{}

+	}

+}

+

+float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {

+

+	switch(p_param) {

+		case Physics2DServer::BODY_PARAM_BOUNCE: {

+

+			return bounce;

+		} break;

+		case Physics2DServer::BODY_PARAM_FRICTION: {

+

+			return friction;

+		} break;

+		case Physics2DServer::BODY_PARAM_MASS: {

+			return mass;

+		} break;

+		default:{}

+	}

+

+	return 0;

+}

+

+void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {

+

+	mode=p_mode;

+

+	switch(p_mode) {

+	//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!

+		case Physics2DServer::BODY_MODE_STATIC:

+		case Physics2DServer::BODY_MODE_STATIC_ACTIVE: {

+

+			_set_inv_transform(get_transform().affine_inverse());

+			_inv_mass=0;

+			_set_static(p_mode==Physics2DServer::BODY_MODE_STATIC);

+			set_active(p_mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE);

+			linear_velocity=Vector2();

+			angular_velocity=0;

+		} break;

+		case Physics2DServer::BODY_MODE_RIGID: {

+

+			_inv_mass=mass>0?(1.0/mass):0;

+			_set_static(false);

+			simulated_motion=false; //jic

+

+		} break;

+		case Physics2DServer::BODY_MODE_CHARACTER: {

+

+			_inv_mass=mass>0?(1.0/mass):0;

+			_set_static(false);

+			simulated_motion=false; //jic

+		} break;

+	}

+

+	_update_inertia();

+	//if (get_space())

+//		_update_queries();

+

+}

+Physics2DServer::BodyMode Body2DSW::get_mode() const {

+

+	return mode;

+}

+

+void Body2DSW::_shapes_changed() {

+

+	_update_inertia();

+	wakeup_neighbours();

+}

+

+void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) {

+

+	switch(p_state)	{

+		case Physics2DServer::BODY_STATE_TRANSFORM: {

+

+

+			if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) {

+				_set_transform(p_variant);

+				_set_inv_transform(get_transform().affine_inverse());

+				wakeup_neighbours();

+			} else {

+				Matrix32 t = p_variant;

+				t.orthonormalize();

+				_set_transform(t);

+				_set_inv_transform(get_transform().inverse());

+

+			}

+

+		} break;

+		case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {

+

+			//if (mode==Physics2DServer::BODY_MODE_STATIC)

+			//	break;

+			linear_velocity=p_variant;

+

+		} break;

+		case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {

+			//if (mode!=Physics2DServer::BODY_MODE_RIGID)

+			//	break;

+			angular_velocity=p_variant;

+

+		} break;

+		case Physics2DServer::BODY_STATE_SLEEPING: {

+			//?

+			if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)

+				break;

+			bool do_sleep=p_variant;

+			if (do_sleep) {

+				linear_velocity=Vector2();

+				//biased_linear_velocity=Vector3();

+				angular_velocity=0;

+				//biased_angular_velocity=Vector3();

+				set_active(false);

+			} else {

+				if (mode!=Physics2DServer::BODY_MODE_STATIC)

+					set_active(true);

+			}

+		} break;

+		case Physics2DServer::BODY_STATE_CAN_SLEEP: {

+			can_sleep=p_variant;

+			if (mode==Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep)

+				set_active(true);

+

+		} break;

+	}

+

+}

+Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const {

+

+	switch(p_state)	{

+		case Physics2DServer::BODY_STATE_TRANSFORM: {

+			return get_transform();

+		} break;

+		case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {

+			return linear_velocity;

+		} break;

+		case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {

+			return angular_velocity;

+		} break;

+		case Physics2DServer::BODY_STATE_SLEEPING: {

+			return !is_active();

+		} break;

+		case Physics2DServer::BODY_STATE_CAN_SLEEP: {

+			return can_sleep;

+		} break;

+	}

+

+	return Variant();

+}

+

+

+void Body2DSW::set_space(Space2DSW *p_space){

+

+	if (get_space()) {

+

+		wakeup_neighbours();

+

+		if (inertia_update_list.in_list())

+			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);

+		if (active_list.in_list())

+			get_space()->body_remove_from_active_list(&active_list);

+		if (direct_state_query_list.in_list())

+			get_space()->body_remove_from_state_query_list(&direct_state_query_list);

+

+	}

+

+	_set_space(p_space);

+

+	if (get_space()) {

+

+		_update_inertia();

+		if (active)

+			get_space()->body_add_to_active_list(&active_list);

+//		_update_queries();

+		//if (is_active()) {

+		//	active=false;

+		//	set_active(true);

+		//}

+

+	}

+

+}

+

+void Body2DSW::_compute_area_gravity(const Area2DSW *p_area) {

+

+	if (p_area->is_gravity_point()) {

+

+		gravity = (p_area->get_transform().get_origin()+p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity();

+

+	} else {

+		gravity = p_area->get_gravity_vector() * p_area->get_gravity();

+	}

+}

+

+void Body2DSW::integrate_forces(real_t p_step) {

+

+	if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)

+		return;

+

+	Area2DSW *current_area = get_space()->get_default_area();

+	ERR_FAIL_COND(!current_area);

+

+	int prio = current_area->get_priority();

+	int ac = areas.size();

+	if (ac) {

+		const AreaCMP *aa = &areas[0];

+		for(int i=0;i<ac;i++) {

+			if (aa[i].area->get_priority() > prio) {

+				current_area=aa[i].area;

+				prio=current_area->get_priority();

+			}

+		}

+	}

+

+	_compute_area_gravity(current_area);

+	density=current_area->get_density();

+

+	if (!omit_force_integration) {

+		//overriden by direct state query

+

+		Vector2 force=gravity*mass;

+		force+=applied_force;

+		real_t torque=applied_torque;

+

+		real_t damp = 1.0 - p_step * density;

+

+		if (damp<0) // reached zero in the given time

+			damp=0;

+

+		real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();

+

+		if (angular_damp<0) // reached zero in the given time

+			angular_damp=0;

+

+		linear_velocity*=damp;

+		angular_velocity*=angular_damp;

+

+		linear_velocity+=_inv_mass * force * p_step;

+		angular_velocity+=_inv_inertia * torque * p_step;

+	}

+

+

+	//motion=linear_velocity*p_step;

+

+	biased_angular_velocity=0;

+	biased_linear_velocity=Vector2();

+

+	if (continuous_cd) //shapes temporarily extend for raycast

+		_update_shapes_with_motion(linear_velocity*p_step);

+

+	current_area=NULL; // clear the area, so it is set in the next frame

+	contact_count=0;

+

+}

+

+void Body2DSW::integrate_velocities(real_t p_step) {

+

+	if (mode==Physics2DServer::BODY_MODE_STATIC)

+		return;

+	if (mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) {

+		if (fi_callback)

+			get_space()->body_add_to_state_query_list(&direct_state_query_list);

+		return;

+	}

+

+	real_t total_angular_velocity = angular_velocity+biased_angular_velocity;

+	Vector2 total_linear_velocity=linear_velocity+biased_linear_velocity;

+

+	real_t angle = get_transform().get_rotation() - total_angular_velocity * p_step;

+	Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;

+

+

+	_set_transform(Matrix32(angle,pos));

+	_set_inv_transform(get_transform().inverse());

+

+

+	if (fi_callback) {

+

+		get_space()->body_add_to_state_query_list(&direct_state_query_list);

+	}

+

+	//_update_inertia_tensor();

+}

+

+

+void Body2DSW::simulate_motion(const Matrix32& p_xform,real_t p_step) {

+

+	Matrix32 inv_xform = p_xform.affine_inverse();

+	if (!get_space()) {

+		_set_transform(p_xform);

+		_set_inv_transform(inv_xform);

+

+		return;

+	}

+

+

+

+	linear_velocity = (p_xform.elements[2] - get_transform().elements[2])/p_step;

+

+	real_t rot = inv_xform.basis_xform(get_transform().elements[1]).atan2();

+	angular_velocity = rot / p_step;

+

+	if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))

+		get_space()->body_add_to_state_query_list(&direct_state_query_list);

+	simulated_motion=true;

+	_set_transform(p_xform);

+

+

+}

+

+void Body2DSW::wakeup_neighbours() {

+

+

+

+	for(Map<Constraint2DSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {

+

+		const Constraint2DSW *c=E->key();

+		Body2DSW **n = c->get_body_ptr();

+		int bc=c->get_body_count();

+

+		for(int i=0;i<bc;i++) {

+

+			if (i==E->get())

+				continue;

+			Body2DSW *b = n[i];

+			if (b->mode!=Physics2DServer::BODY_MODE_RIGID)

+				continue;

+

+			if (!b->is_active())

+				b->set_active(true);

+		}

+	}

+}

+

+void Body2DSW::call_queries() {

+

+

+	if (fi_callback) {

+

+		Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton;

+		dbs->body=this;

+

+		Variant v=dbs;

+		const Variant *vp[2]={&v,&fi_callback->callback_udata};

+

+		Object *obj = ObjectDB::get_instance(fi_callback->id);

+		if (!obj) {

+

+			set_force_integration_callback(NULL,StringName());

+		} else {

+			Variant::CallError ce;			

+			if (fi_callback->callback_udata.get_type()) {

+

+				obj->call(fi_callback->method,vp,2,ce);

+

+			} else {

+				obj->call(fi_callback->method,vp,1,ce);

+			}

+		}

+

+

+	}

+

+	if (simulated_motion) {

+

+	//	linear_velocity=Vector2();

+	//	angular_velocity=0;

+		simulated_motion=false;

+	}

+}

+

+

+bool Body2DSW::sleep_test(real_t p_step)  {

+

+	if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)

+		return true; //

+	else if (mode==Physics2DServer::BODY_MODE_CHARACTER)

+		return !active; // characters don't sleep unless asked to sleep

+	else if (!can_sleep)

+		return false;

+

+

+

+

+	if (Math::abs(angular_velocity)<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {

+

+		still_time+=p_step;

+

+		return still_time > get_space()->get_body_time_to_sleep();

+	} else {

+

+		still_time=0; //maybe this should be set to 0 on set_active?

+		return false;

+	}

+}

+

+

+void Body2DSW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {

+

+	if (fi_callback) {

+

+		memdelete(fi_callback);

+		fi_callback=NULL;

+	}

+

+

+	if (p_id!=0) {

+

+		fi_callback=memnew(ForceIntegrationCallback);

+		fi_callback->id=p_id;

+		fi_callback->method=p_method;

+		fi_callback->callback_udata=p_udata;

+	}

+

+}

+

+Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {

+

+

+	mode=Physics2DServer::BODY_MODE_RIGID;

+	active=true;

+	angular_velocity=0;

+	biased_angular_velocity=0;

+	mass=1;

+	_inv_inertia=0;

+	_inv_mass=1;

+	bounce=0;

+	friction=1;

+	omit_force_integration=false;

+	applied_torque=0;

+	island_step=0;

+	island_next=NULL;

+	island_list_next=NULL;

+	_set_static(false);

+	density=0;

+	contact_count=0;

+	simulated_motion=false;

+	still_time=0;

+	continuous_cd=false;

+	can_sleep=false;

+	fi_callback=NULL;

+

+}

+

+Body2DSW::~Body2DSW() {

+

+	if (fi_callback)

+		memdelete(fi_callback);

+}

+

+Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton=NULL;

+

+Physics2DDirectSpaceState* Physics2DDirectBodyStateSW::get_space_state() {

+

+	return body->get_space()->get_direct_state();

+}

diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
new file mode 100644
index 000000000..55b84ce7a
--- /dev/null
+++ b/servers/physics_2d/body_2d_sw.h
@@ -0,0 +1,334 @@
+/*************************************************************************/
+/*  body_2d_sw.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 BODY_2D_SW_H

+#define BODY_2D_SW_H

+

+#include "collision_object_2d_sw.h"

+#include "vset.h"

+#include "area_2d_sw.h"

+

+class Constraint2DSW;

+

+

+class Body2DSW : public CollisionObject2DSW {

+

+

+	Physics2DServer::BodyMode mode;

+

+	Vector2 biased_linear_velocity;

+	real_t biased_angular_velocity;

+

+	Vector2 linear_velocity;

+	real_t angular_velocity;

+

+	real_t mass;

+	real_t bounce;

+	real_t friction;

+

+	real_t _inv_mass;

+	real_t _inv_inertia;

+

+	Vector2 gravity;

+	real_t density;

+

+	real_t still_time;

+

+	Vector2 applied_force;

+	real_t applied_torque;

+

+	SelfList<Body2DSW> active_list;

+	SelfList<Body2DSW> inertia_update_list;

+	SelfList<Body2DSW> direct_state_query_list;

+

+	VSet<RID> exceptions;

+	bool omit_force_integration;

+	bool active;

+	bool simulated_motion;

+	bool continuous_cd;

+	bool can_sleep;

+	void _update_inertia();

+	virtual void _shapes_changed();

+

+

+	Map<Constraint2DSW*,int> constraint_map;

+

+	struct AreaCMP {

+

+		Area2DSW *area;

+		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }

+		_FORCE_INLINE_ AreaCMP() {}

+		_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;}

+	};

+

+

+	VSet<AreaCMP> areas;

+

+	struct Contact {

+

+

+		Vector2 local_pos;

+		Vector2 local_normal;

+		float depth;

+		int local_shape;

+		Vector2 collider_pos;

+		int collider_shape;

+		ObjectID collider_instance_id;

+		RID collider;

+		Vector2 collider_velocity_at_pos;

+	};

+

+	Vector<Contact> contacts; //no contacts by default

+	int contact_count;

+

+	struct ForceIntegrationCallback {

+

+		ObjectID id;

+		StringName method;

+		Variant callback_udata;

+	};

+

+	ForceIntegrationCallback *fi_callback;

+

+

+	uint64_t island_step;

+	Body2DSW *island_next;

+	Body2DSW *island_list_next;

+

+	_FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area);

+

+friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose

+

+public:

+

+

+	void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant());

+

+

+	_FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); }

+	_FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); }

+

+	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }

+	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }

+

+	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }

+	_FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos);

+

+

+	_FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}

+	_FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}

+	_FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}

+	_FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}

+

+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }

+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }

+

+	_FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; }

+	_FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; }

+

+	_FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; }

+	_FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; }

+

+	_FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }

+	_FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); }

+	const Map<Constraint2DSW*,int>& get_constraint_map() const { return constraint_map; }

+

+	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }

+	_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }

+

+	_FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; }

+	_FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }

+

+	_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; }

+	_FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }

+

+	_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; }

+	_FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }

+

+	_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; }

+	_FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }

+

+	_FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) {

+

+		linear_velocity += p_j * _inv_mass;

+		angular_velocity += _inv_inertia * p_pos.cross(p_j);

+	}

+

+	_FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) {

+

+		biased_linear_velocity += p_j * _inv_mass;

+		biased_angular_velocity += _inv_inertia * p_pos.cross(p_j);

+	}

+

+	void set_active(bool p_active);

+	_FORCE_INLINE_ bool is_active() const { return active; }

+

+	void set_param(Physics2DServer::BodyParameter p_param, float);

+	float get_param(Physics2DServer::BodyParameter p_param) const;

+

+	void set_mode(Physics2DServer::BodyMode p_mode);

+	Physics2DServer::BodyMode get_mode() const;

+

+	void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant);

+	Variant get_state(Physics2DServer::BodyState p_state) const;

+

+	void set_applied_force(const Vector2& p_force) { applied_force=p_force; }

+	Vector2 get_applied_force() const { return applied_force; }

+

+	void set_applied_torque(real_t p_torque) { applied_torque=p_torque; }

+	real_t get_applied_torque() const { return applied_torque; }

+

+	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; }

+	_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }

+

+	void set_space(Space2DSW *p_space);

+

+	void update_inertias();

+

+	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }

+	_FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }

+	_FORCE_INLINE_ real_t get_friction() const { return friction; }

+	_FORCE_INLINE_ Vector2 get_gravity() const { return gravity; }

+	_FORCE_INLINE_ real_t get_density() const { return density; }

+

+	void integrate_forces(real_t p_step);

+	void integrate_velocities(real_t p_step);

+

+	void simulate_motion(const Matrix32& p_xform,real_t p_step);

+	void call_queries();

+	void wakeup_neighbours();

+

+	bool sleep_test(real_t p_step);

+

+	Body2DSW();

+	~Body2DSW();

+

+};

+

+

+//add contact inline

+

+void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) {

+

+	int c_max=contacts.size();

+

+	if (c_max==0)

+		return;

+

+	Contact *c = &contacts[0];

+

+

+	int idx=-1;

+

+	if (contact_count<c_max) {

+		idx=contact_count++;

+	} else {

+

+		float least_depth=1e20;

+		int least_deep=-1;

+		for(int i=0;i<c_max;i++) {

+

+			if (i==0 || c[i].depth<least_depth) {

+				least_deep=i;

+				least_depth=c[i].depth;

+			}

+		}

+

+		if (least_deep>=0 && least_depth<p_depth) {

+

+			idx=least_deep;

+		}

+		if (idx==-1)

+			return; //none least deepe than this

+	}

+

+	c[idx].local_pos=p_local_pos;

+	c[idx].local_normal=p_local_normal;

+	c[idx].depth=p_depth;

+	c[idx].local_shape=p_local_shape;

+	c[idx].collider_pos=p_collider_pos;

+	c[idx].collider_shape=p_collider_shape;

+	c[idx].collider_instance_id=p_collider_instance_id;

+	c[idx].collider=p_collider;

+	c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;

+

+}

+

+

+class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {

+

+	OBJ_TYPE( Physics2DDirectBodyStateSW, Physics2DDirectBodyState );

+

+public:

+

+	static Physics2DDirectBodyStateSW *singleton;

+	Body2DSW *body;

+	real_t step;

+

+	virtual Vector2 get_total_gravity() const {  return body->get_gravity();  } // get gravity vector working on this body space/area

+	virtual float get_total_density() const {  return body->get_density();  } // get density of this body space/area

+

+	virtual float get_inverse_mass() const {  return body->get_inv_mass();  } // get the mass

+	virtual real_t get_inverse_inertia() const { return body->get_inv_inertia();   } // get density of this body space

+

+	virtual void set_linear_velocity(const Vector2& p_velocity) {  body->set_linear_velocity(p_velocity);  }

+	virtual Vector2 get_linear_velocity() const {  return body->get_linear_velocity();  }

+

+	virtual void set_angular_velocity(real_t p_velocity) {  body->set_angular_velocity(p_velocity);  }

+	virtual real_t get_angular_velocity() const {  return body->get_angular_velocity();  }

+

+	virtual void set_transform(const Matrix32& p_transform) {  body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform);  }

+	virtual Matrix32 get_transform() const {  return body->get_transform();  }

+

+	virtual void set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }

+	virtual bool is_sleeping() const {  return !body->is_active();  }

+

+	virtual int get_contact_count() const {  return body->contact_count;  }

+

+	virtual Vector2 get_contact_local_pos(int p_contact_idx) const {

+		ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());

+		return body->contacts[p_contact_idx].local_pos;

+	}

+	virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());   return body->contacts[p_contact_idx].local_normal;   }

+	virtual int get_contact_local_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1);   return body->contacts[p_contact_idx].local_shape;   }

+

+	virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID());   return body->contacts[p_contact_idx].collider;   }

+	virtual Vector2 get_contact_collider_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());   return body->contacts[p_contact_idx].collider_pos;  }

+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0);   return body->contacts[p_contact_idx].collider_instance_id;   }

+	virtual int get_contact_collider_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape;  }

+	virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos;  }

+

+	virtual Physics2DDirectSpaceState* get_space_state();

+

+

+	virtual real_t get_step() const { return step; }

+	Physics2DDirectBodyStateSW() { singleton=this; body=NULL; }

+};

+

+

+#endif // BODY_2D_SW_H

diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
new file mode 100644
index 000000000..6d8215840
--- /dev/null
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -0,0 +1,435 @@
+/*************************************************************************/
+/*  body_pair_2d_sw.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 "body_pair_2d_sw.h"
+#include "collision_solver_2d_sw.h"
+#include "space_2d_sw.h"
+
+
+#define POSITION_CORRECTION
+#define ACCUMULATE_IMPULSES
+
+void BodyPair2DSW::_add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self) {
+
+	BodyPair2DSW *self = (BodyPair2DSW *)p_self;
+
+	self->_contact_added_callback(p_point_A,p_point_B);
+
+}
+
+void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B) {
+
+	// check if we already have the contact
+
+
+	Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
+	Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B-offset_B);
+
+	int new_index = contact_count;
+
+	ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) );
+
+	Contact contact;
+
+	contact.acc_normal_impulse=0;
+	contact.acc_bias_impulse=0;
+	contact.acc_tangent_impulse=0;
+	contact.local_A=local_A;
+	contact.local_B=local_B;
+	contact.normal=(p_point_A-p_point_B).normalized();
+
+	// attempt to determine if the contact will be reused
+
+	real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius();
+
+	for (int i=0;i<contact_count;i++) {
+
+		Contact& c = contacts[i];
+		if (
+		c.local_A.distance_squared_to( local_A ) < (recycle_radius_2) &&
+		c.local_B.distance_squared_to( local_B ) < (recycle_radius_2) ) {
+
+			contact.acc_normal_impulse=c.acc_normal_impulse;
+			contact.acc_tangent_impulse=c.acc_tangent_impulse;
+			contact.acc_bias_impulse=c.acc_bias_impulse;
+			new_index=i;
+			break;
+		}
+	}
+
+	// figure out if the contact amount must be reduced to fit the new contact
+
+	if (new_index == MAX_CONTACTS) {
+
+		// remove the contact with the minimum depth
+
+		int least_deep=-1;
+		real_t min_depth=1e10;
+
+
+		for (int i=0;i<=contact_count;i++) {
+
+			Contact& c = (i==contact_count)?contact:contacts[i];
+			Vector2 global_A = A->get_transform().basis_xform(c.local_A);
+			Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
+
+			Vector2 axis = global_A - global_B;
+			float depth = axis.dot( c.normal );
+
+
+			if (depth<min_depth) {
+
+				min_depth=depth;
+				least_deep=i;
+			}
+		}
+
+		ERR_FAIL_COND(least_deep==-1);
+
+		if (least_deep < contact_count) { //replace the last deep contact by the new one
+
+			contacts[least_deep]=contact;
+		}
+
+		return;
+	}
+
+	contacts[new_index]=contact;
+
+	if (new_index==contact_count) {
+
+		contact_count++;
+	}
+
+}
+
+void BodyPair2DSW::_validate_contacts() {
+
+	//make sure to erase contacts that are no longer valid
+
+	real_t max_separation = space->get_contact_max_separation();
+	real_t max_separation2 = max_separation*max_separation;
+
+	for (int i=0;i<contact_count;i++) {
+
+		Contact& c = contacts[i];
+
+		Vector2 global_A = A->get_transform().basis_xform(c.local_A);
+		Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
+		Vector2 axis = global_A - global_B;
+		float depth = axis.dot( c.normal );
+
+		if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
+			// contact no longer needed, remove
+
+
+			if ((i+1) < contact_count) {
+				// swap with the last one
+				SWAP( contacts[i], contacts[ contact_count-1 ] );
+
+			}
+
+			i--;
+			contact_count--;
+		}
+	}
+}
+
+
+bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result) {
+
+	Vector2 motion = p_A->get_linear_velocity()*p_step;
+	real_t mlen = motion.length();
+	if (mlen<CMP_EPSILON)
+		return false;
+
+	Vector2 mnormal = motion / mlen;
+
+	real_t min,max;
+	p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max);
+	if (mlen < (max-min)*0.3)  //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
+		return false;
+
+	//cast a segment from support in motion normal, in the same direction of motion by motion length
+	int a;
+	Vector2 s[2];
+	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a);
+	Vector2 from = p_xform_A.xform(s[0]);
+	Vector2 to = from + motion;
+
+	Vector2 local_from = p_xform_inv_B.xform(from);
+	Vector2 local_to = p_xform_inv_B.xform(to);
+
+	Vector2 rpos,rnorm;
+	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm))
+		return false;
+
+	//ray hit something
+
+	Vector2 contact_A = to;
+	Vector2 contact_B = p_xform_B.xform(rpos);
+
+	//create a contact
+
+	if (p_swap_result)
+		_contact_added_callback(contact_B,contact_A);
+	else
+		_contact_added_callback(contact_A,contact_B);
+
+
+	return true;
+}
+
+bool BodyPair2DSW::setup(float p_step) {
+
+
+	//use local A coordinates to avoid numerical issues on collision detection
+	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
+
+	_validate_contacts();
+
+	//cannot collide
+	if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE)) {
+		collided=false;
+
+		return false;
+	}
+	Vector2 offset_A = A->get_transform().get_origin();
+	Matrix32 xform_Au = A->get_transform().untranslated();
+	Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A);
+	Matrix32 xform_inv_A = xform_A.affine_inverse();
+
+	Matrix32 xform_Bu = B->get_transform();
+	xform_Bu.elements[2]-=A->get_transform().get_origin();
+	Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B);
+	Matrix32 xform_inv_B = xform_B.affine_inverse();
+
+	Shape2DSW *shape_A_ptr=A->get_shape(shape_A);
+	Shape2DSW *shape_B_ptr=B->get_shape(shape_B);
+
+	collided = CollisionSolver2DSW::solve_static(shape_A_ptr,xform_A,xform_inv_A,shape_B_ptr,xform_B,xform_inv_B,_add_contact,this,&sep_axis);
+	if (!collided) {
+
+		//test ccd (currently just a raycast)
+		if (A->is_continuous_collision_detection_enabled() && A->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
+			if (_test_ccd(p_step,A,shape_A,xform_A,xform_inv_A,B,shape_B,xform_B,xform_inv_B))
+				collided=true;
+		}
+
+		if (B->is_continuous_collision_detection_enabled() && B->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
+			if (_test_ccd(p_step,B,shape_B,xform_B,xform_inv_B,A,shape_A,xform_A,xform_inv_A,true))
+				collided=true;
+		}
+
+		if (!collided)
+			return false;
+
+	}
+
+
+
+	real_t max_penetration = space->get_contact_max_allowed_penetration();
+
+	float bias = 0.3f;
+	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
+
+		if (shape_A_ptr->get_custom_bias()==0)
+			bias=shape_B_ptr->get_custom_bias();
+		else if (shape_B_ptr->get_custom_bias()==0)
+			bias=shape_A_ptr->get_custom_bias();
+		else
+			bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5;
+	}
+
+
+	cc=0;
+
+
+	real_t inv_dt = 1.0/p_step;
+	for (int i = 0; i < contact_count; i++) {
+
+		Contact& c = contacts[i];
+
+		Vector2 global_A = xform_Au.xform(c.local_A);
+		Vector2 global_B = xform_Bu.xform(c.local_B);
+
+		real_t depth = c.normal.dot(global_A - global_B);
+
+		if (depth<=0) {
+			c.active=false;
+			continue;
+		}
+
+		c.active=true;
+
+		int gather_A = A->can_report_contacts();
+		int gather_B = B->can_report_contacts();
+
+		c.rA = global_A;
+		c.rB = global_B-offset_B;
+
+		if (gather_A | gather_B) {
+
+			//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+
+			global_A+=offset_A;
+			global_B+=offset_A;
+
+			if (gather_A) {
+				Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+				A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB+B->get_linear_velocity());
+			}
+			if (gather_B) {
+
+				Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
+				B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA+A->get_linear_velocity());
+			}
+		}
+
+
+		// Precompute normal mass, tangent mass, and bias.
+		real_t rnA = c.rA.dot(c.normal);
+		real_t rnB = c.rB.dot(c.normal);
+		real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
+		kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB);
+		c.mass_normal = 1.0f / kNormal;
+
+		Vector2 tangent = c.normal.tangent();
+		real_t rtA = c.rA.dot(tangent);
+		real_t rtB = c.rB.dot(tangent);
+		real_t kTangent = A->get_inv_mass() + B->get_inv_mass();
+		kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB);
+		c.mass_tangent = 1.0f /  kTangent;
+
+
+
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+		c.depth=depth;
+		//c.acc_bias_impulse=0;
+
+
+#ifdef ACCUMULATE_IMPULSES
+		{
+			// Apply normal + friction impulse
+			Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
+
+
+			A->apply_impulse(c.rA,-P);
+			B->apply_impulse(c.rB, P);
+		}
+
+#endif
+	}
+
+	return true;
+}
+
+void BodyPair2DSW::solve(float p_step) {
+
+	if (!collided)
+		return;
+
+	for (int i = 0; i < contact_count; ++i) {
+
+		Contact& c = contacts[i];
+		cc++;
+
+		if (!c.active)
+			continue;
+
+
+		// Relative velocity at contact
+
+		Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
+		Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+		Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
+
+		Vector2 crbA( -A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x );
+		Vector2 crbB( -B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x );
+		Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
+
+		real_t vn = dv.dot(c.normal);
+		real_t vbn = dbv.dot(c.normal);
+		Vector2 tangent = c.normal.tangent();
+		real_t vt = dv.dot(tangent);
+
+
+		real_t jbn = (c.bias - vbn)*c.mass_normal;
+		real_t jbnOld = c.acc_bias_impulse;
+		c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
+
+		Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
+
+		A->apply_bias_impulse(c.rA,-jb);
+		B->apply_bias_impulse(c.rB, jb);
+
+		real_t bounce=0;
+		real_t jn = -(bounce + vn)*c.mass_normal;
+		real_t jnOld = c.acc_normal_impulse;
+		c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
+
+
+		real_t friction = A->get_friction() * B->get_friction();
+
+		real_t jtMax = friction*c.acc_normal_impulse;
+		real_t jt = -vt*c.mass_tangent;
+		real_t jtOld = c.acc_tangent_impulse;
+		c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax);
+
+		Vector2 j =c.normal * (c.acc_normal_impulse - jnOld) + tangent * ( c.acc_tangent_impulse - jtOld );
+
+
+		A->apply_impulse(c.rA,-j);
+		B->apply_impulse(c.rB, j);
+
+
+	}
+}
+
+
+BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B) : Constraint2DSW(_arr,2) {
+
+	A=p_A;
+	B=p_B;
+	shape_A=p_shape_A;
+	shape_B=p_shape_B;
+	space=A->get_space();
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+	contact_count=0;
+	collided=false;
+
+}
+
+
+BodyPair2DSW::~BodyPair2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+
+}
diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h
new file mode 100644
index 000000000..26278f87c
--- /dev/null
+++ b/servers/physics_2d/body_pair_2d_sw.h
@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  body_pair_2d_sw.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 BODY_PAIR_2D_SW_H
+#define BODY_PAIR_2D_SW_H
+
+#include "body_2d_sw.h"
+#include "constraint_2d_sw.h"
+
+class BodyPair2DSW : public Constraint2DSW {
+
+	enum {
+		MAX_CONTACTS=2
+	};
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+	int shape_A;
+	int shape_B;
+
+	Space2DSW *space;
+
+	struct Contact {
+
+		Vector2 position;
+		Vector2 normal;
+		Vector2 local_A, local_B;
+		real_t acc_normal_impulse;	// accumulated normal impulse (Pn)
+		real_t acc_tangent_impulse;	// accumulated tangent impulse (Pt)
+		real_t acc_bias_impulse;	// accumulated normal impulse for position bias (Pnb)
+		real_t mass_normal, mass_tangent;
+		real_t bias;
+
+		real_t depth;
+		bool active;
+		Vector2 rA,rB;
+	};
+
+	Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection
+
+	Vector2 sep_axis;
+	Contact contacts[MAX_CONTACTS];
+	int contact_count;
+	bool collided;
+	int cc;
+
+
+	bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result=false);
+	void _validate_contacts();
+	static void _add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self);
+	_FORCE_INLINE_ void _contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B);
+
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B);
+	~BodyPair2DSW();
+
+};
+
+#endif // BODY_PAIR_2D_SW_H
diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp
new file mode 100644
index 000000000..9641a986e
--- /dev/null
+++ b/servers/physics_2d/broad_phase_2d_basic.cpp
@@ -0,0 +1,192 @@
+/*************************************************************************/
+/*  broad_phase_2d_basic.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 "broad_phase_2d_basic.h"
+
+ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) {
+
+
+	current++;
+
+	Element e;
+	e.owner=p_object_;
+	e._static=false;
+	e.subindex=p_subindex;
+
+	element_map[current]=e;
+	return current;
+}
+
+void BroadPhase2DBasic::move(ID p_id, const Rect2& p_aabb) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	E->get().aabb=p_aabb;
+
+}
+void BroadPhase2DBasic::set_static(ID p_id, bool p_static) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	E->get()._static=p_static;
+
+}
+void BroadPhase2DBasic::remove(ID p_id) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	element_map.erase(E);
+
+}
+
+CollisionObject2DSW *BroadPhase2DBasic::get_object(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,NULL);
+	return E->get().owner;
+
+}
+bool BroadPhase2DBasic::is_static(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,false);
+	return E->get()._static;
+
+}
+int BroadPhase2DBasic::get_subindex(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,-1);
+	return E->get().subindex;
+}
+
+int BroadPhase2DBasic::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+
+	int rc=0;
+
+	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+
+		const Rect2 aabb=E->get().aabb;
+		if (aabb.intersects_segment(p_from,p_to)) {
+
+			p_results[rc]=E->get().owner;
+			p_result_indices[rc]=E->get().subindex;
+			rc++;
+			if (rc>=p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+
+}
+int BroadPhase2DBasic::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+
+	int rc=0;
+
+	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+
+		const Rect2 aabb=E->get().aabb;
+		if (aabb.intersects(p_aabb)) {
+
+			p_results[rc]=E->get().owner;
+			p_result_indices[rc]=E->get().subindex;
+			rc++;
+			if (rc>=p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+}
+
+void BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
+
+	pair_userdata=p_userdata;
+	pair_callback=p_pair_callback;
+
+}
+void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) {
+
+	unpair_userdata=p_userdata;
+	unpair_callback=p_pair_callback;
+
+}
+
+void BroadPhase2DBasic::update() {
+
+	// recompute pairs
+	for(Map<ID,Element>::Element *I=element_map.front();I;I=I->next()) {
+
+		for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) {
+
+			Element *elem_A=&I->get();
+			Element *elem_B=&J->get();
+
+			if (elem_A->owner == elem_B->owner)
+				continue;
+
+
+			bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static );
+
+			PairKey key(I->key(),J->key());
+
+			Map<PairKey,void*>::Element *E=pair_map.find(key);
+
+			if (!pair_ok && E) {
+				if (unpair_callback)
+					unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,E->get(),unpair_userdata);
+				pair_map.erase(key);
+			}
+
+			if (pair_ok && !E) {
+
+				void *data=NULL;
+				if (pair_callback)
+					data=pair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,unpair_userdata);
+				pair_map.insert(key,data);
+			}
+		}
+	}
+
+}
+
+BroadPhase2DSW *BroadPhase2DBasic::_create() {
+
+	return memnew( BroadPhase2DBasic );
+}
+
+BroadPhase2DBasic::BroadPhase2DBasic() {
+
+	current=1;
+	unpair_callback=NULL;
+	unpair_userdata=NULL;
+	pair_callback=NULL;
+	pair_userdata=NULL;
+
+}
diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h
new file mode 100644
index 000000000..ce1575225
--- /dev/null
+++ b/servers/physics_2d/broad_phase_2d_basic.h
@@ -0,0 +1,100 @@
+/*************************************************************************/
+/*  broad_phase_2d_basic.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 BROAD_PHASE_2D_BASIC_H
+#define BROAD_PHASE_2D_BASIC_H
+
+#include "space_2d_sw.h"
+#include "map.h"
+class BroadPhase2DBasic : public BroadPhase2DSW {
+
+	struct Element {
+
+		CollisionObject2DSW *owner;
+		bool _static;
+		Rect2 aabb;
+		int subindex;
+	};
+
+
+	Map<ID,Element> element_map;
+
+	ID current;
+
+	struct PairKey {
+
+		union {
+			struct {
+				ID a;
+				ID b;
+			};
+			uint64_t key;
+		};
+
+		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+			return key < p_key.key;
+		}
+
+		PairKey() { key=0; }
+		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
+
+	};
+
+	Map<PairKey,void*> pair_map;
+
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+public:
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0);
+	virtual void move(ID p_id, const Rect2& p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObject2DSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+
+	virtual void update();
+
+	static BroadPhase2DSW *_create();
+	BroadPhase2DBasic();
+};
+
+#endif // BROAD_PHASE_2D_BASIC_H
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
new file mode 100644
index 000000000..10da376df
--- /dev/null
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -0,0 +1,665 @@
+/*************************************************************************/
+/*  broad_phase_2d_hash_grid.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 "broad_phase_2d_hash_grid.h"
+#include "globals.h"
+
+void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element* p_with) {
+
+	Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
+
+	ERR_FAIL_COND(p_elem->_static && p_with->_static);
+
+	if (!E) {
+
+		PairData *pd = memnew( PairData );
+		p_elem->paired[p_with]=pd;
+		p_with->paired[p_elem]=pd;
+	} else {
+		E->get()->rc++;
+	}
+
+}
+
+void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element* p_with) {
+
+	Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
+
+	ERR_FAIL_COND(!E); //this should really be paired..
+
+	E->get()->rc--;
+
+	if (E->get()->rc==0) {
+
+		if (E->get()->colliding) {
+			//uncollide
+			if (unpair_callback) {
+				unpair_callback(p_elem->owner,p_elem->subindex,p_with->owner,p_with->subindex,E->get()->ud,unpair_userdata);
+			}
+
+
+		}
+
+		memdelete(E->get());
+		p_elem->paired.erase(E);
+		p_with->paired.erase(p_elem);
+	}
+
+
+}
+
+void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
+
+	for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) {
+
+		bool pairing = p_elem->aabb.intersects( E->key()->aabb );
+
+		if (pairing!=E->get()->colliding) {
+
+			if (pairing) {
+
+				if (pair_callback) {
+					E->get()->ud=pair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,pair_userdata);
+				}
+			} else {
+
+				if (unpair_callback) {
+					unpair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,E->get()->ud,unpair_userdata);
+				}
+
+			}
+
+			E->get()->colliding=pairing;
+		}
+	}
+}
+
+void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
+
+
+	Point2i from = (p_rect.pos/cell_size).floor();
+	Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
+
+	for(int i=from.x;i<=to.x;i++) {
+
+
+		for(int j=from.y;j<=to.y;j++) {
+
+			PosKey pk;
+			pk.x=i;
+			pk.y=j;
+
+			uint32_t idx = pk.hash() % hash_table_size;
+			PosBin *pb = hash_table[idx];
+
+			while (pb) {
+
+				if (pb->key == pk) {
+					break;
+				}
+
+				pb=pb->next;
+			}
+
+
+			bool entered=false;
+
+			if (!pb) {
+				//does not exist, create!
+				pb = memnew( PosBin );
+				pb->key=pk;
+				pb->next=hash_table[idx];
+				hash_table[idx]=pb;
+			}
+
+
+
+			if (p_static) {
+				if (pb->static_object_set[p_elem].inc()==1) {
+					entered=true;
+				}
+			} else {
+				if (pb->object_set[p_elem].inc()==1) {
+
+					entered=true;
+				}
+			}
+
+			if (entered) {
+
+				for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
+
+					if (E->key()->owner==p_elem->owner)
+						continue;
+					_pair_attempt(p_elem,E->key());
+				}
+
+				if (!p_static) {
+
+					for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
+
+						if (E->key()->owner==p_elem->owner)
+							continue;
+						_pair_attempt(p_elem,E->key());
+					}
+				}
+			}
+
+		}
+
+	}
+
+
+}
+
+
+void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
+
+
+	Point2i from = (p_rect.pos/cell_size).floor();
+	Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
+
+	for(int i=from.x;i<=to.x;i++) {
+
+		for(int j=from.y;j<=to.y;j++) {
+
+			PosKey pk;
+			pk.x=i;
+			pk.y=j;
+
+			uint32_t idx = pk.hash() % hash_table_size;
+			PosBin *pb = hash_table[idx];
+
+			while (pb) {
+
+				if (pb->key == pk) {
+					break;
+				}
+
+				pb=pb->next;
+			}
+
+			ERR_CONTINUE(!pb); //should exist!!
+
+			bool exited=false;
+
+
+			if (p_static) {
+				if (pb->static_object_set[p_elem].dec()==0) {
+
+					pb->static_object_set.erase(p_elem);
+					exited=true;
+
+				}
+			} else {
+				if (pb->object_set[p_elem].dec()==0) {
+
+					pb->object_set.erase(p_elem);
+					exited=true;
+
+				}
+			}
+
+			if (exited) {
+
+				for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
+
+					if (E->key()->owner==p_elem->owner)
+						continue;
+					_unpair_attempt(p_elem,E->key());
+
+				}
+
+				if (!p_static) {
+
+					for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
+
+						if (E->key()->owner==p_elem->owner)
+							continue;
+						_unpair_attempt(p_elem,E->key());
+					}
+				}
+			}
+
+			if (pb->object_set.empty() && pb->static_object_set.empty()) {
+
+				if (hash_table[idx]==pb) {
+					hash_table[idx]=pb->next;
+				} else {
+
+					PosBin *px = hash_table[idx];
+
+					while (px) {
+
+						if (px->next==pb) {
+							px->next=pb->next;
+							break;
+						}
+
+						px=px->next;
+					}
+
+					ERR_CONTINUE(!px);
+				}
+
+				memdelete(pb);
+
+			}
+		}
+
+	}
+
+}
+
+
+BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) {
+
+	current++;
+
+	Element e;
+	e.owner=p_object;
+	e._static=false;
+	e.subindex=p_subindex;
+	e.self=current;
+	e.pass=0;
+
+	element_map[current]=e;
+	return current;
+
+}
+
+void BroadPhase2DHashGrid::move(ID p_id, const Rect2& p_aabb) {
+
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+
+	Element &e=E->get();
+
+	if (p_aabb==e.aabb)
+		return;
+
+
+	if (p_aabb!=Rect2()) {
+
+		_enter_grid(&e,p_aabb,e._static);
+	}
+
+	if (e.aabb!=Rect2()) {
+
+		_exit_grid(&e,e.aabb,e._static);
+	}
+
+	e.aabb=p_aabb;
+
+	_check_motion(&e);
+
+	e.aabb=p_aabb;
+
+}
+void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+
+	Element &e=E->get();
+
+	if (e._static==p_static)
+		return;
+
+	if (e.aabb!=Rect2())
+		_exit_grid(&e,e.aabb,e._static);
+
+	e._static=p_static;
+
+	if (e.aabb!=Rect2()) {
+		_enter_grid(&e,e.aabb,e._static);
+		_check_motion(&e);
+	}
+
+}
+void BroadPhase2DHashGrid::remove(ID p_id) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+
+	Element &e=E->get();
+
+	if (e.aabb!=Rect2())
+		_exit_grid(&e,e.aabb,e._static);
+
+	element_map.erase(p_id);
+
+}
+
+CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,NULL);
+	return E->get().owner;
+
+}
+bool BroadPhase2DHashGrid::is_static(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,false);
+	return E->get()._static;
+
+}
+int BroadPhase2DHashGrid::get_subindex(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,-1);
+	return E->get().subindex;
+}
+
+void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) {
+
+
+	PosKey pk;
+	pk.x=p_cell.x;
+	pk.y=p_cell.y;
+
+	uint32_t idx = pk.hash() % hash_table_size;
+	PosBin *pb = hash_table[idx];
+
+	while (pb) {
+
+		if (pb->key == pk) {
+			break;
+		}
+
+		pb=pb->next;
+	}
+
+	if (!pb)
+		return;
+
+
+
+	for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
+
+
+		if (index>=p_max_results)
+			break;
+		if (E->key()->pass==pass)
+			continue;
+
+		E->key()->pass=pass;
+		p_results[index]=E->key()->owner;
+		p_result_indices[index]=E->key()->subindex;
+		index++;
+
+	}
+
+	for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
+
+
+		if (index>=p_max_results)
+			break;
+		if (E->key()->pass==pass)
+			continue;
+
+		E->key()->pass=pass;
+		p_results[index]=E->key()->owner;
+		p_result_indices[index]=E->key()->subindex;
+		index++;
+
+	}
+}
+
+int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+
+	pass++;
+
+	Vector2 dir = (p_to-p_from);
+	if (dir==Vector2())
+		return 0;
+	//avoid divisions by zero
+	dir.normalize();
+	if (dir.x==0.0)
+		dir.x=0.000001;
+	if (dir.y==0.0)
+		dir.y=0.000001;
+	Vector2 delta = dir.abs();
+
+	delta.x=cell_size/delta.x;
+	delta.y=cell_size/delta.y;
+
+	Point2i pos = p_from.floor() / cell_size;
+	Point2i end = p_to.floor() / cell_size;
+	Point2i step = Vector2( SGN(dir.x), SGN(dir.y));
+
+	Vector2 max;
+
+	if (dir.x<0)
+		max.x= (Math::floor(pos.x)*cell_size - p_from.x) / dir.x;
+	else
+		max.x= (Math::floor(pos.x + 1)*cell_size - p_from.x) / dir.x;
+
+	if (dir.y<0)
+		max.y= (Math::floor(pos.y)*cell_size - p_from.y) / dir.y;
+	else
+		max.y= (Math::floor(pos.y + 1)*cell_size - p_from.y) / dir.y;
+
+	int cullcount=0;
+	_cull(pos,p_results,p_max_results,p_result_indices,cullcount);
+
+	bool reached_x=false;
+	bool reached_y=false;
+
+	while(true) {
+
+		if (max.x < max.y) {
+
+			max.x+=delta.x;
+			pos.x+=step.x;
+		} else {
+
+			max.y+=delta.y;
+			pos.y+=step.y;
+
+		}
+
+		if (step.x>0) {
+			if (pos.x>=end.x)
+				reached_x=true;
+		} else if (pos.x<=end.x) {
+
+			reached_x=true;
+		}
+
+		if (step.y>0) {
+			if (pos.y>=end.y)
+				reached_y=true;
+		} else if (pos.y<=end.y) {
+
+			reached_y=true;
+		}
+
+		_cull(pos,p_results,p_max_results,p_result_indices,cullcount);
+
+		if (reached_x && reached_y)
+			break;
+
+	}
+
+	return cullcount;
+}
+
+
+int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+
+
+	return 0;
+}
+
+void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
+
+	pair_callback=p_pair_callback;
+	pair_userdata=p_userdata;
+
+}
+void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata) {
+
+	unpair_callback=p_unpair_callback;
+	unpair_userdata=p_userdata;
+
+}
+
+void BroadPhase2DHashGrid::update() {
+
+
+}
+
+BroadPhase2DSW *BroadPhase2DHashGrid::_create() {
+
+	return memnew( BroadPhase2DHashGrid );
+}
+
+
+BroadPhase2DHashGrid::BroadPhase2DHashGrid() {
+
+	hash_table_size = GLOBAL_DEF("physics_2d/bp_hash_table_size",4096);
+	hash_table_size = Math::larger_prime(hash_table_size);
+	hash_table = memnew_arr( PosBin*, hash_table_size);
+
+	cell_size = GLOBAL_DEF("physics_2d/cell_size",128);
+
+	for(int i=0;i<hash_table_size;i++)
+		hash_table[i]=NULL;
+	pass=1;
+
+	current=0;
+}
+
+BroadPhase2DHashGrid::~BroadPhase2DHashGrid() {
+
+	for(int i=0;i<hash_table_size;i++) {
+		while(hash_table[i]) {
+			PosBin *pb=hash_table[i];
+			hash_table[i]=pb->next;
+			memdelete(pb);
+		}
+	}
+
+	memdelete_arr( hash_table );
+
+
+}
+
+
+
+/* 3D version of voxel traversal:
+
+public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth)
+{
+    // Implementation is based on:
+    // "A Fast Voxel Traversal Algorithm for Ray Tracing"
+    // John Amanatides, Andrew Woo
+    // http://www.cse.yorku.ca/~amana/research/grid.pdf
+    // http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf
+
+    // NOTES:
+    // * This code assumes that the ray's position and direction are in 'cell coordinates', which means
+    //   that one unit equals one cell in all directions.
+    // * When the ray doesn't start within the voxel grid, calculate the first position at which the
+    //   ray could enter the grid. If it never enters the grid, there is nothing more to do here.
+    // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
+    // * The Point3D structure is a simple structure having three integer fields (X, Y and Z).
+
+    // The cell in which the ray starts.
+    Point3D start = GetCellAt(ray.Position);        // Rounds the position's X, Y and Z down to the nearest integer values.
+    int x = start.X;
+    int y = start.Y;
+    int z = start.Z;
+
+    // Determine which way we go.
+    int stepX = Math.Sign(ray.Direction.X);
+    int stepY = Math.Sign(ray.Direction.Y);
+    int stepZ = Math.Sign(ray.Direction.Z);
+
+    // Calculate cell boundaries. When the step (i.e. direction sign) is positive,
+    // the next boundary is AFTER our current position, meaning that we have to add 1.
+    // Otherwise, it is BEFORE our current position, in which case we add nothing.
+    Point3D cellBoundary = new Point3D(
+	x + (stepX > 0 ? 1 : 0),
+	y + (stepY > 0 ? 1 : 0),
+	z + (stepZ > 0 ? 1 : 0));
+
+    // NOTE: For the following calculations, the result will be Single.PositiveInfinity
+    // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand
+    // value of the division also equals zero, the result is Single.NaN, which is not OK.
+
+    // Determine how far we can travel along the ray before we hit a voxel boundary.
+    Vector3 tMax = new Vector3(
+	(cellBoundary.X - ray.Position.X) / ray.Direction.X,    // Boundary is a plane on the YZ axis.
+	(cellBoundary.Y - ray.Position.Y) / ray.Direction.Y,    // Boundary is a plane on the XZ axis.
+	(cellBoundary.Z - ray.Position.Z) / ray.Direction.Z);    // Boundary is a plane on the XY axis.
+    if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity;
+    if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity;
+    if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity;
+
+    // Determine how far we must travel along the ray before we have crossed a gridcell.
+    Vector3 tDelta = new Vector3(
+	stepX / ray.Direction.X,                    // Crossing the width of a cell.
+	stepY / ray.Direction.Y,                    // Crossing the height of a cell.
+	stepZ / ray.Direction.Z);                    // Crossing the depth of a cell.
+    if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity;
+    if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity;
+    if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity;
+
+    // For each step, determine which distance to the next voxel boundary is lowest (i.e.
+    // which voxel boundary is nearest) and walk that way.
+    for (int i = 0; i < maxDepth; i++)
+    {
+	// Return it.
+	yield return new Point3D(x, y, z);
+
+	// Do the next step.
+	if (tMax.X < tMax.Y && tMax.X < tMax.Z)
+	{
+	    // tMax.X is the lowest, an YZ cell boundary plane is nearest.
+	    x += stepX;
+	    tMax.X += tDelta.X;
+	}
+	else if (tMax.Y < tMax.Z)
+	{
+	    // tMax.Y is the lowest, an XZ cell boundary plane is nearest.
+	    y += stepY;
+	    tMax.Y += tDelta.Y;
+	}
+	else
+	{
+	    // tMax.Z is the lowest, an XY cell boundary plane is nearest.
+	    z += stepZ;
+	    tMax.Z += tDelta.Z;
+	}
+    }
+
+    */
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h
new file mode 100644
index 000000000..713d96048
--- /dev/null
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.h
@@ -0,0 +1,192 @@
+/*************************************************************************/
+/*  broad_phase_2d_hash_grid.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 BROAD_PHASE_2D_HASH_GRID_H
+#define BROAD_PHASE_2D_HASH_GRID_H
+
+#include "broad_phase_2d_sw.h"
+#include "map.h"
+
+class BroadPhase2DHashGrid : public BroadPhase2DSW {
+
+
+	struct PairData {
+
+		bool colliding;
+		int rc;
+		void *ud;
+		PairData() { colliding=false; rc=1; ud=NULL; }
+	};
+
+	struct Element {
+
+		ID self;
+		CollisionObject2DSW *owner;
+		bool _static;
+		Rect2 aabb;
+		int subindex;
+		uint64_t pass;
+		Map<Element*,PairData*> paired;
+
+	};
+
+
+	Map<ID,Element> element_map;
+
+	ID current;
+
+	uint64_t pass;
+
+
+	struct PairKey {
+
+		union {
+			struct {
+				ID a;
+				ID b;
+			};
+			uint64_t key;
+		};
+
+		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+			return key < p_key.key;
+		}
+
+		PairKey() { key=0; }
+		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
+
+	};
+
+
+	Map<PairKey,PairData> pair_map;
+
+	int cell_size;
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+	void _enter_grid(Element* p_elem, const Rect2& p_rect,bool p_static);
+	void _exit_grid(Element* p_elem, const Rect2& p_rect,bool p_static);
+	_FORCE_INLINE_ void _cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index);
+
+
+	struct PosKey {
+
+		union {
+			struct {
+				int32_t x;
+				int32_t y;
+			};
+			uint64_t key;
+		};
+
+
+		_FORCE_INLINE_ uint32_t hash() const {
+			uint64_t k=key;
+			k = (~k) + (k << 18); // k = (k << 18) - k - 1;
+			k = k ^ (k >> 31);
+			k = k * 21; // k = (k + (k << 2)) + (k << 4);
+			k = k ^ (k >> 11);
+			k = k + (k << 6);
+			k = k ^ (k >> 22);
+			return k;
+		}
+
+		bool operator==(const PosKey& p_key) const { return key==p_key.key; }
+		_FORCE_INLINE_ bool operator<(const PosKey& p_key) const {
+			return key < p_key.key;
+		}
+
+	};
+
+	struct RC {
+
+		int ref;
+
+		_FORCE_INLINE_ int inc() {
+			ref++;
+			return ref;
+		}
+		_FORCE_INLINE_ int dec() {
+			ref--;
+			return ref;
+		}
+
+		_FORCE_INLINE_ RC() {
+			ref=0;
+		}
+	};
+
+	struct PosBin {
+
+		PosKey key;
+		Map<Element*,RC> object_set;
+		Map<Element*,RC> static_object_set;
+		PosBin *next;
+	};
+
+
+	uint32_t hash_table_size;
+	PosBin **hash_table;
+
+	void _pair_attempt(Element *p_elem, Element* p_with);
+	void _unpair_attempt(Element *p_elem, Element* p_with);
+	void _check_motion(Element *p_elem);
+
+
+public:
+
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0);
+	virtual void move(ID p_id, const Rect2& p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObject2DSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+
+	virtual void update();
+
+
+	static BroadPhase2DSW *_create();
+
+	BroadPhase2DHashGrid();
+	~BroadPhase2DHashGrid();
+
+
+};
+
+#endif // BROAD_PHASE_2D_HASH_GRID_H
diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp
new file mode 100644
index 000000000..7ded6ed01
--- /dev/null
+++ b/servers/physics_2d/broad_phase_2d_sw.cpp
@@ -0,0 +1,35 @@
+/*************************************************************************/
+/*  broad_phase_2d_sw.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 "broad_phase_2d_sw.h"
+
+BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func=NULL;
+
+BroadPhase2DSW::~BroadPhase2DSW()
+{
+}
diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h
new file mode 100644
index 000000000..510f7db11
--- /dev/null
+++ b/servers/physics_2d/broad_phase_2d_sw.h
@@ -0,0 +1,73 @@
+/*************************************************************************/
+/*  broad_phase_2d_sw.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 BROAD_PHASE_2D_SW_H
+#define BROAD_PHASE_2D_SW_H
+
+#include "math_funcs.h"
+#include "math_2d.h"
+
+class CollisionObject2DSW;
+
+
+class BroadPhase2DSW {
+
+public:
+	typedef BroadPhase2DSW* (*CreateFunction)();
+
+	static CreateFunction create_func;
+
+	typedef uint32_t ID;
+
+
+	typedef void* (*PairCallback)(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_userdata);
+	typedef void (*UnpairCallback)(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_userdata);
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0)=0;
+	virtual void move(ID p_id, const Rect2& p_aabb)=0;
+	virtual void set_static(ID p_id, bool p_static)=0;
+	virtual void remove(ID p_id)=0;
+
+	virtual CollisionObject2DSW *get_object(ID p_id) const=0;
+	virtual bool is_static(ID p_id) const=0;
+	virtual int get_subindex(ID p_id) const=0;
+
+	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0;
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0;
+
+	virtual void update()=0;
+
+	virtual ~BroadPhase2DSW();
+
+};
+
+#endif // BROAD_PHASE_2D_SW_H
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
new file mode 100644
index 000000000..6e5a703aa
--- /dev/null
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -0,0 +1,220 @@
+/*************************************************************************/
+/*  collision_object_2d_sw.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 "collision_object_2d_sw.h"
+#include "space_2d_sw.h"
+
+void CollisionObject2DSW::add_shape(Shape2DSW *p_shape,const Matrix32& p_transform) {
+
+	Shape s;
+	s.shape=p_shape;
+	s.xform=p_transform;
+	s.xform_inv=s.xform.affine_inverse();
+	s.bpid=0; //needs update
+	s.trigger=false;
+	shapes.push_back(s);
+	p_shape->add_owner(this);
+	_update_shapes();
+	_shapes_changed();
+
+}
+
+void CollisionObject2DSW::set_shape(int p_index,Shape2DSW *p_shape){
+
+	ERR_FAIL_INDEX(p_index,shapes.size());
+	shapes[p_index].shape->remove_owner(this);
+	shapes[p_index].shape=p_shape;
+
+	p_shape->add_owner(this);
+	_update_shapes();
+	_shapes_changed();
+
+}
+void CollisionObject2DSW::set_shape_transform(int p_index,const Matrix32& p_transform){
+
+	ERR_FAIL_INDEX(p_index,shapes.size());
+
+	shapes[p_index].xform=p_transform;
+	shapes[p_index].xform_inv=p_transform.affine_inverse();
+	_update_shapes();
+	_shapes_changed();
+}
+
+void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) {
+
+	//remove a shape, all the times it appears
+	for(int i=0;i<shapes.size();i++) {
+
+		if (shapes[i].shape==p_shape) {
+			remove_shape(i);
+			i--;
+		}
+	}
+}
+
+void CollisionObject2DSW::remove_shape(int p_index){
+
+	//remove anything from shape to be erased to end, so subindices don't change
+	ERR_FAIL_INDEX(p_index,shapes.size());
+	for(int i=p_index;i<shapes.size();i++) {
+
+		if (shapes[i].bpid==0)
+			continue;
+		//should never get here with a null owner
+		space->get_broadphase()->remove(shapes[i].bpid);
+		shapes[i].bpid=0;
+	}
+	shapes[p_index].shape->remove_owner(this);
+	shapes.remove(p_index);
+
+	_shapes_changed();
+
+}
+
+void CollisionObject2DSW::_set_static(bool p_static) {
+	if (_static==p_static)
+		return;
+	_static=p_static;
+
+	if (!space)
+		return;
+	for(int i=0;i<get_shape_count();i++) {
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+	}
+
+}
+
+void CollisionObject2DSW::_unregister_shapes() {
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			space->get_broadphase()->remove(s.bpid);
+			s.bpid=0;
+		}
+	}
+
+}
+
+void CollisionObject2DSW::_update_shapes() {
+
+	if (!space)
+		return;
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid==0) {
+			s.bpid=space->get_broadphase()->create(this,i);
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+
+		//not quite correct, should compute the next matrix..
+		Rect2 shape_aabb=s.shape->get_aabb();
+		Matrix32 xform = transform * s.xform;
+		shape_aabb=xform.xform(shape_aabb);
+		s.aabb_cache=shape_aabb;
+		s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 );
+
+
+		space->get_broadphase()->move(s.bpid,s.aabb_cache);
+	}
+
+}
+
+void CollisionObject2DSW::_update_shapes_with_motion(const Vector2& p_motion) {
+
+
+	if (!space)
+		return;
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid==0) {
+			s.bpid=space->get_broadphase()->create(this,i);
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+
+		//not quite correct, should compute the next matrix..
+		Rect2 shape_aabb=s.shape->get_aabb();
+		Matrix32 xform = transform * s.xform;
+		shape_aabb=xform.xform(shape_aabb);
+		shape_aabb=shape_aabb.merge(Rect2( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion
+		s.aabb_cache=shape_aabb;
+
+		space->get_broadphase()->move(s.bpid,shape_aabb);
+	}
+
+
+}
+
+void CollisionObject2DSW::_set_space(Space2DSW *p_space) {
+
+	if (space) {
+
+		space->remove_object(this);
+
+		for(int i=0;i<shapes.size();i++) {
+
+			Shape &s=shapes[i];
+			if (s.bpid) {
+				space->get_broadphase()->remove(s.bpid);
+				s.bpid=0;
+			}
+		}
+
+	}
+
+	space=p_space;
+
+	if (space) {
+
+		space->add_object(this);
+		_update_shapes();
+	}
+
+}
+
+void CollisionObject2DSW::_shape_changed() {
+
+	_update_shapes();
+	_shapes_changed();
+}
+
+CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
+
+	_static=true;
+	type=p_type;
+	space=NULL;
+	instance_id=0;
+}
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
new file mode 100644
index 000000000..b2d2c2545
--- /dev/null
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -0,0 +1,123 @@
+/*************************************************************************/
+/*  collision_object_2d_sw.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 COLLISION_OBJECT_2D_SW_H
+#define COLLISION_OBJECT_2D_SW_H
+
+#include "shape_2d_sw.h"
+#include "servers/physics_2d_server.h"
+#include "self_list.h"
+#include "broad_phase_2d_sw.h"
+
+class Space2DSW;
+
+class CollisionObject2DSW : public ShapeOwner2DSW {
+public:
+	enum Type {
+		TYPE_AREA,
+		TYPE_BODY
+	};
+private:
+
+	Type type;
+	RID self;
+	ObjectID instance_id;
+
+	struct Shape {
+
+		Matrix32 xform;
+		Matrix32 xform_inv;
+		BroadPhase2DSW::ID bpid;
+		Rect2 aabb_cache; //for rayqueries
+		Shape2DSW *shape;
+		bool trigger;
+		Shape() { trigger=false; }
+	};
+
+	Vector<Shape> shapes;
+	Space2DSW *space;
+	Matrix32 transform;
+	Matrix32 inv_transform;
+	bool _static;
+
+	void _update_shapes();
+
+protected:
+
+
+	void _update_shapes_with_motion(const Vector2& p_motion);
+	void _unregister_shapes();
+
+	_FORCE_INLINE_ void _set_transform(const Matrix32& p_transform) { transform=p_transform; _update_shapes(); }
+	_FORCE_INLINE_ void _set_inv_transform(const Matrix32& p_transform) { inv_transform=p_transform; }
+	void _set_static(bool p_static);
+
+	virtual void _shapes_changed()=0;
+	void _set_space(Space2DSW *space);
+
+	CollisionObject2DSW(Type p_type);
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ void set_instance_id(const ObjectID& p_instance_id) { instance_id=p_instance_id; }
+	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
+
+	void _shape_changed();
+
+	_FORCE_INLINE_ Type get_type() const { return type; }
+	void add_shape(Shape2DSW *p_shape,const Matrix32& p_transform=Matrix32());
+	void set_shape(int p_index,Shape2DSW *p_shape);
+	void set_shape_transform(int p_index,const Matrix32& p_transform);
+	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
+	_FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const { return shapes[p_index].shape; }
+	_FORCE_INLINE_ const Matrix32& get_shape_transform(int p_index) const { return shapes[p_index].xform; }
+	_FORCE_INLINE_ const Matrix32& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
+	_FORCE_INLINE_ const Rect2& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
+
+	_FORCE_INLINE_ Matrix32 get_transform() const { return transform; }
+	_FORCE_INLINE_ Matrix32 get_inv_transform() const { return inv_transform; }
+	_FORCE_INLINE_ Space2DSW* get_space() const { return space; }
+
+	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx,bool p_enable) { shapes[p_idx].trigger=p_enable; }
+	_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
+
+
+	void remove_shape(Shape2DSW *p_shape);
+	void remove_shape(int p_index);
+
+	virtual void set_space(Space2DSW *p_space)=0;
+
+	_FORCE_INLINE_ bool is_static() const { return _static;  }
+
+	virtual ~CollisionObject2DSW() {}
+
+};
+
+#endif // COLLISION_OBJECT_2D_SW_H
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
new file mode 100644
index 000000000..4b87ff02a
--- /dev/null
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -0,0 +1,1034 @@
+/*************************************************************************/
+/*  collision_solver_2d_sat.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 "collision_solver_2d_sat.h"
+#include "geometry.h"
+
+struct _CollectorCallback2D {
+
+	CollisionSolver2DSW::CallbackResult callback;
+	void *userdata;
+	bool swap;
+	bool collided;
+	Vector2 normal;
+	Vector2 *sep_axis;
+
+	_FORCE_INLINE_ void call(const Vector2& p_point_A, const Vector2& p_point_B) {
+
+		//if (normal.dot(p_point_A) >= normal.dot(p_point_B))
+		// return;
+		if (swap)
+			callback(p_point_B,p_point_A,userdata);
+		else
+			callback(p_point_A,p_point_B,userdata);
+	}
+
+};
+
+typedef void (*GenerateContactsFunc)(const Vector2 *,int, const Vector2 *,int ,_CollectorCallback2D *);
+
+
+_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) {
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A != 1 );
+	ERR_FAIL_COND( p_point_count_B != 1 );
+#endif
+
+	p_collector->call(*p_points_A,*p_points_B);
+}
+
+_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) {
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A != 1 );
+	ERR_FAIL_COND( p_point_count_B != 2 );
+#endif
+
+	Vector2 closest_B = Geometry::get_closest_point_to_segment_uncapped_2d(*p_points_A, p_points_B );
+	p_collector->call(*p_points_A,closest_B);
+
+}
+
+
+struct _generate_contacts_Pair {
+	int idx;
+	float d;
+	_FORCE_INLINE_ bool operator <(const _generate_contacts_Pair& l) const { return d< l.d; }
+};
+
+_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) {
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A != 2 );
+	ERR_FAIL_COND( p_point_count_B != 2 ); // circle is actually a 4x3 matrix
+#endif
+
+
+	Vector2 rel_A=p_points_A[1]-p_points_A[0];
+	Vector2 rel_B=p_points_B[1]-p_points_B[0];
+
+	Vector2 t = p_collector->normal.tangent();
+
+	real_t dA[2]={t.dot(p_points_A[0]),t.dot(p_points_A[1])};
+	Vector2 pA[2]={p_points_A[0],p_points_A[1]};
+
+	if (dA[0]>dA[1]) {
+		SWAP(dA[0],dA[1]);
+		SWAP(pA[0],pA[1]);
+	}
+
+	float dB[2]={t.dot(p_points_B[0]),t.dot(p_points_B[1])};
+	Vector2 pB[2]={p_points_B[0],p_points_B[1]};
+	if (dB[0]>dB[1]) {
+		SWAP(dB[0],dB[1]);
+		SWAP(pB[0],pB[1]);
+	}
+
+
+	if (dA[0]<dB[0]) {
+
+		Vector2 n = (p_points_A[1]-p_points_A[0]).normalized().tangent();
+		real_t d = n.dot(p_points_A[1]);
+
+		if (dA[1]>dB[1]) {
+			//A contains B
+			for(int i=0;i<2;i++) {
+
+				Vector2 b = p_points_B[i];
+				Vector2 a = n.plane_project(d,b);
+				if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-CMP_EPSILON)
+					continue;
+				p_collector->call(a,b);
+
+			}
+		} else {
+
+			// B0,A1 containment
+
+			Vector2 n_B = (p_points_B[1]-p_points_B[0]).normalized().tangent();
+			real_t d_B = n_B.dot(p_points_B[1]);
+
+			// first, B on A
+
+			{
+				Vector2 b = p_points_B[0];
+				Vector2 a = n.plane_project(d,b);
+				if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON)
+					p_collector->call(a,b);
+			}
+
+			// second, A on B
+
+			{
+				Vector2 a = p_points_A[1];
+				Vector2 b = n_B.plane_project(d_B,a);
+				if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON)
+					p_collector->call(a,b);
+			}
+
+
+
+		}
+
+
+	} else {
+
+		Vector2 n = (p_points_B[1]-p_points_B[0]).normalized().tangent();
+		real_t d = n.dot(p_points_B[1]);
+
+		if (dB[1]>dA[1]) {
+			//B contains A
+			for(int i=0;i<2;i++) {
+
+				Vector2 a = p_points_A[i];
+				Vector2 b = n.plane_project(d,a);
+				if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-CMP_EPSILON)
+					continue;
+				p_collector->call(a,b);
+			}
+		} else {
+
+			// A0,B1 containment
+			Vector2 n_A = (p_points_A[1]-p_points_A[0]).normalized().tangent();
+			real_t d_A = n_A.dot(p_points_A[1]);
+
+			// first A on B
+
+			{
+				Vector2 a = p_points_A[0];
+				Vector2 b = n.plane_project(d,a);
+				if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON)
+					p_collector->call(a,b);
+
+			}
+
+			//second, B on A
+
+			{
+
+				Vector2 b = p_points_B[1];
+				Vector2 a = n_A.plane_project(d_A,b);
+				if (p_collector->normal.dot(a) < p_collector->normal.dot(b)-CMP_EPSILON)
+					p_collector->call(a,b);
+			}
+
+		}
+	}
+
+
+#if 0
+
+	Vector2 axis = rel_A.normalized();
+	Vector2 axis_B = rel_B.normalized();
+	if (axis.dot(axis_B)<0)
+		axis_B=-axis_B;
+	axis=(axis+axis_B)*0.5;
+
+	Vector2 normal_A = axis.tangent();
+	real_t dA = normal_A.dot(p_points_A[0]);
+	Vector2 normal_B = rel_B.tangent().normalized();
+	real_t dB = normal_A.dot(p_points_B[0]);
+
+	Vector2 A[4]={ normal_A.plane_project(dA,p_points_B[0]), normal_A.plane_project(dA,p_points_B[1]), p_points_A[0], p_points_A[1] };
+	Vector2 B[4]={ p_points_B[0], p_points_B[1], normal_B.plane_project(dB,p_points_A[0]), normal_B.plane_project(dB,p_points_A[1]) };
+
+	_generate_contacts_Pair dvec[4];
+	for(int i=0;i<4;i++) {
+		dvec[i].d=axis.dot(p_points_A[0]-A[i]);
+		dvec[i].idx=i;
+	}
+
+	SortArray<_generate_contacts_Pair> sa;
+	sa.sort(dvec,4);
+
+	for(int i=1;i<=2;i++) {
+
+		Vector2 a = A[i];
+		Vector2 b = B[i];
+		if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-CMP_EPSILON)
+			continue;
+		p_collector->call(a,b);
+	}
+
+#elif 0
+	Vector2 axis = rel_A.normalized(); //make an axis
+	Vector2 axis_B = rel_B.normalized();
+	if (axis.dot(axis_B)<0)
+		axis_B=-axis_B;
+	axis=(axis+axis_B)*0.5;
+	Vector2 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]);
+	Vector2 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]);
+
+	//sort all 4 points in axis
+	float dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) };
+
+	//todo , find max/min and then use 2 central points
+	SortArray<float> sa;
+	sa.sort(dvec,4);
+
+	//use the middle ones as contacts
+	for (int i=1;i<=2;i++) {
+
+		Vector2 a = base_A+axis*dvec[i];
+		Vector2 b = base_B+axis*dvec[i];
+		if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-0.01) {
+			print_line("fail a: "+a);
+			print_line("fail b: "+b);
+			continue;
+		}
+		print_line("res a: "+a);
+		print_line("res b: "+b);
+		p_collector->call(a,b);
+	}
+#endif
+}
+
+static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) {
+
+
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND( p_point_count_A <1 );
+	ERR_FAIL_COND( p_point_count_B <1 );
+#endif
+
+
+	static const GenerateContactsFunc generate_contacts_func_table[2][2]={
+		{
+			_generate_contacts_point_point,
+			_generate_contacts_point_edge,
+		},{
+			0,
+			_generate_contacts_edge_edge,
+		}
+	};
+
+	int pointcount_B;
+	int pointcount_A;
+	const Vector2 *points_A;
+	const Vector2 *points_B;
+
+	if (p_point_count_A > p_point_count_B) {
+		//swap
+		p_collector->swap = !p_collector->swap;
+		p_collector->normal = -p_collector->normal;
+
+		pointcount_B = p_point_count_A;
+		pointcount_A = p_point_count_B;
+		points_A=p_points_B;
+		points_B=p_points_A;
+	} else {
+
+		pointcount_B = p_point_count_B;
+		pointcount_A = p_point_count_A;
+		points_A=p_points_A;
+		points_B=p_points_B;
+	}
+
+	int version_A = (pointcount_A > 3 ?  3 : pointcount_A) -1;
+	int version_B = (pointcount_B > 3 ?  3 : pointcount_B) -1;
+
+	GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
+	ERR_FAIL_COND(!contacts_func);
+	contacts_func(points_A,pointcount_A,points_B,pointcount_B,p_collector);
+
+}
+
+
+
+template<class ShapeA, class ShapeB>
+class SeparatorAxisTest2D {
+
+	const ShapeA *shape_A;
+	const ShapeB *shape_B;
+	const Matrix32 *transform_A;
+	const Matrix32 *transform_B;
+	const Matrix32 *transform_inv_A;
+	const Matrix32 *transform_inv_B;
+	real_t best_depth;
+	Vector2 best_axis;
+	int best_axis_count;
+	int best_axis_index;
+	_CollectorCallback2D *callback;
+
+public:
+
+	_FORCE_INLINE_ bool test_previous_axis() {
+
+		if (callback && callback->sep_axis && *callback->sep_axis!=Vector2()) {
+			return test_axis(*callback->sep_axis);
+		} else {
+#ifdef DEBUG_ENABLED
+			best_axis_count++;
+#endif
+
+		}
+		return true;
+	}
+
+	_FORCE_INLINE_ bool test_axis(const Vector2& p_axis) {
+
+		Vector2 axis=p_axis;
+
+
+		if (	Math::abs(axis.x)<CMP_EPSILON &&
+			Math::abs(axis.y)<CMP_EPSILON) {
+			// strange case, try an upwards separator
+			axis=Vector2(0.0,1.0);
+		}
+
+		real_t min_A,max_A,min_B,max_B;
+
+		shape_A->project_range(axis,*transform_A,min_A,max_A);
+		shape_B->project_range(axis,*transform_B,min_B,max_B);
+
+		min_B -= ( max_A - min_A ) * 0.5;
+		max_B += ( max_A - min_A ) * 0.5;
+
+		real_t dmin = min_B - ( min_A + max_A ) * 0.5;
+		real_t dmax = max_B - ( min_A + max_A ) * 0.5;
+
+		if (dmin > 0.0 || dmax < 0.0) {
+			if (callback && callback->sep_axis)
+				*callback->sep_axis=axis;
+#ifdef DEBUG_ENABLED
+			best_axis_count++;
+#endif
+
+			return false; // doesn't contain 0
+		}
+
+		//use the smallest depth
+
+		dmin = Math::abs(dmin);
+
+		if ( dmax < dmin ) {
+			if ( dmax < best_depth ) {
+				best_depth=dmax;
+				best_axis=axis;
+#ifdef DEBUG_ENABLED
+				best_axis_index=best_axis_count;
+#endif
+
+			}
+		} else {
+			if ( dmin < best_depth ) {
+				best_depth=dmin;
+				best_axis=-axis; // keep it as A axis
+#ifdef DEBUG_ENABLED
+				best_axis_index=best_axis_count;
+#endif
+			}
+		}
+
+	//	print_line("test axis: "+p_axis+" depth: "+rtos(best_depth));
+#ifdef DEBUG_ENABLED
+		best_axis_count++;
+#endif
+
+		return true;
+	}
+
+
+	_FORCE_INLINE_ void generate_contacts() {
+
+		// nothing to do, don't generate
+		if (best_axis==Vector2(0.0,0.0))
+			return;
+
+		callback->collided=true;
+
+		if (!callback->callback)
+			return; //only collide, no callback
+		static const int max_supports=2;
+
+
+		Vector2 supports_A[max_supports];
+		int support_count_A;
+		shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A);
+		for(int i=0;i<support_count_A;i++) {
+			supports_A[i] = transform_A->xform(supports_A[i]);
+		}
+
+
+
+		Vector2 supports_B[max_supports];
+		int support_count_B;
+		shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B);
+		for(int i=0;i<support_count_B;i++) {
+			supports_B[i] = transform_B->xform(supports_B[i]);
+		}
+/*
+
+
+		print_line("**************************");
+		printf("CBK: %p\n",callback->userdata);
+		print_line("type A: "+itos(shape_A->get_type()));
+		print_line("type B: "+itos(shape_B->get_type()));
+		print_line("xform A: "+*transform_A);
+		print_line("xform B: "+*transform_B);
+		print_line("normal: "+best_axis);
+		print_line("depth: "+rtos(best_depth));
+		print_line("index: "+itos(best_axis_index));
+
+		for(int i=0;i<support_count_A;i++) {
+
+			print_line("A-"+itos(i)+": "+supports_A[i]);
+		}
+
+		for(int i=0;i<support_count_B;i++) {
+
+			print_line("B-"+itos(i)+": "+supports_B[i]);
+		}
+//*/
+
+
+
+
+		callback->normal=best_axis;
+		_generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback);
+
+		if (callback && callback->sep_axis && *callback->sep_axis!=Vector2())
+			*callback->sep_axis=Vector2(); //invalidate previous axis (no test)
+		//CollisionSolver2DSW::CallbackResult cbk=NULL;
+		//cbk(Vector2(),Vector2(),NULL);
+
+	}
+
+	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+		best_depth=1e15;
+		shape_A=p_shape_A;
+		shape_B=p_shape_B;
+		transform_A=&p_transform_a;
+		transform_B=&p_transform_b;
+		transform_inv_A=&p_transform_inv_a;
+		transform_inv_B=&p_transform_inv_b;
+		callback=p_collector;
+#ifdef DEBUG_ENABLED
+		best_axis_count=0;
+		best_axis_index=-1;
+#endif
+	}
+
+};
+
+/****** SAT TESTS *******/
+/****** SAT TESTS *******/
+/****** SAT TESTS *******/
+/****** SAT TESTS *******/
+
+
+typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Matrix32&,const Shape2DSW*,const Matrix32&,const Matrix32&,_CollectorCallback2D *p_collector);
+
+static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
+	const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b);
+
+	SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,segment_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+		return;
+	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_B->get_normal()).normalized()))
+		return;
+
+	separator.generate_contacts();
+
+}
+
+static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
+	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	if (!separator.test_axis(
+				(p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent()
+				))
+		return;
+
+//	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+//		return;
+	if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-p_transform_b.get_origin()).normalized()))
+		return;
+	if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-p_transform_b.get_origin()).normalized()))
+		return;
+
+
+	separator.generate_contacts();
+
+
+}
+
+static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
+	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
+
+	SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+		return;
+
+	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
+		return;
+
+	if (!separator.test_axis(p_transform_b.elements[1].normalized()))
+		return;
+
+	separator.generate_contacts();
+
+}
+
+static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
+	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
+
+	SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+		return;
+
+	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
+		return;
+
+	if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
+		return;
+	if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
+		return;
+	if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
+		return;
+	if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
+		return;
+
+	separator.generate_contacts();
+}
+
+static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+
+	SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+		return;
+
+	for(int i=0;i<convex_B->get_point_count();i++) {
+
+		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+			return;
+	}
+
+	separator.generate_contacts();
+
+}
+
+
+/////////
+
+static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
+	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	if (!separator.test_axis((p_transform_a.get_origin()-p_transform_b.get_origin()).normalized()))
+		return;
+
+	separator.generate_contacts();
+
+}
+
+static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
+	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	const Vector2 &sphere=p_transform_a.elements[2];
+	const Vector2 *axis=&p_transform_b.elements[0];
+	const Vector2& half_extents = rectangle_B->get_half_extents();
+
+	if (!separator.test_axis(axis[0].normalized()))
+		return;
+
+	if (!separator.test_axis(axis[1].normalized()))
+		return;
+
+	Vector2 local_v = p_transform_inv_b.xform(p_transform_a.get_origin());
+
+	Vector2 he(
+		(local_v.x<0) ? -half_extents.x : half_extents.x,
+		(local_v.y<0) ? -half_extents.y : half_extents.y
+	);
+
+
+	if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized()))
+		return;
+
+	separator.generate_contacts();
+}
+
+static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
+	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	//capsule axis
+	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
+		return;
+
+	//capsule endpoints
+	if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
+		return;
+	if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
+		return;
+
+
+	separator.generate_contacts();
+
+
+}
+
+static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	//poly faces and poly points vs circle
+	for(int i=0;i<convex_B->get_point_count();i++) {
+
+		if (!separator.test_axis( (p_transform_b.xform(convex_B->get_point(i))-p_transform_a.get_origin()).normalized() ))
+			return;
+
+		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+			return;
+	}
+
+	separator.generate_contacts();
+}
+
+
+/////////
+
+static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
+	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	//box faces A
+	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
+		return;
+
+	if (!separator.test_axis(p_transform_a.elements[1].normalized()))
+		return;
+
+	//box faces B
+	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
+		return;
+
+	if (!separator.test_axis(p_transform_b.elements[1].normalized()))
+		return;
+
+	separator.generate_contacts();
+}
+
+static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
+	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	//box faces
+	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
+		return;
+
+	if (!separator.test_axis(p_transform_a.elements[1].normalized()))
+		return;
+
+	//capsule axis
+	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
+		return;
+
+
+	//box endpoints to capsule circles
+
+	for(int i=0;i<2;i++) {
+
+		Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
+
+		const Vector2& half_extents = rectangle_A->get_half_extents();
+		Vector2 local_v = p_transform_inv_a.xform(capsule_endpoint);
+
+		Vector2 he(
+			(local_v.x<0) ? -half_extents.x : half_extents.x,
+			(local_v.y<0) ? -half_extents.y : half_extents.y
+		);
+
+
+		if (!separator.test_axis(p_transform_a.xform(he).normalized()))
+			return;
+
+	}
+
+
+	separator.generate_contacts();
+}
+
+static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+
+	SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	//box faces
+	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
+		return;
+
+	if (!separator.test_axis(p_transform_a.elements[1].normalized()))
+		return;
+
+	//convex faces
+	for(int i=0;i<convex_B->get_point_count();i++) {
+
+		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+			return;
+	}
+
+	separator.generate_contacts();
+
+}
+
+
+/////////
+
+static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
+	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	//capsule axis
+
+	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
+		return;
+
+	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
+		return;
+
+	//capsule endpoints
+
+	for(int i=0;i<2;i++) {
+
+		Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(i==0?0.5:-0.5);
+
+		for(int j=0;j<2;j++) {
+
+			Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5);
+
+			if (!separator.test_axis( (capsule_endpoint_A-capsule_endpoint_B).normalized() ))
+				return;
+
+		}
+	}
+
+	separator.generate_contacts();
+
+}
+
+static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+
+
+	SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	//capsule axis
+
+	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
+		return;
+
+
+	//poly vs capsule
+	for(int i=0;i<convex_B->get_point_count();i++) {
+
+		Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i));
+
+		for(int j=0;j<2;j++) {
+
+			Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5);
+
+			if (!separator.test_axis( (cpoint - capsule_endpoint_A).normalized() ))
+				return;
+
+		}
+
+		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+			return;
+	}
+
+	separator.generate_contacts();
+}
+
+
+/////////
+
+
+static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+
+	const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+
+	SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW> separator(convex_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+
+	if (!separator.test_previous_axis())
+		return;
+
+	for(int i=0;i<convex_A->get_point_count();i++) {
+
+		if (!separator.test_axis( p_transform_inv_a.basis_xform_inv(convex_A->get_segment_normal(i)).normalized() ))
+			return;
+	}
+
+	for(int i=0;i<convex_B->get_point_count();i++) {
+
+		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+			return;
+	}
+
+	separator.generate_contacts();
+
+}
+
+
+////////
+
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) {
+
+	Physics2DServer::ShapeType type_A=p_shape_A->get_type();
+
+	ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_LINE,false);
+	//ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_RAY,false);
+	ERR_FAIL_COND_V(p_shape_A->is_concave(),false);
+
+	Physics2DServer::ShapeType type_B=p_shape_B->get_type();
+
+	ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_LINE,false);
+	//ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_RAY,false);
+	ERR_FAIL_COND_V(p_shape_B->is_concave(),false);
+
+
+	static const CollisionFunc collision_table[5][5]={
+		{_collision_segment_segment,
+		 _collision_segment_circle,
+		 _collision_segment_rectangle,
+		 _collision_segment_capsule,
+		 _collision_segment_convex_polygon},
+		{0,
+		 _collision_circle_circle,
+		 _collision_circle_rectangle,
+		 _collision_circle_capsule,
+		 _collision_circle_convex_polygon},
+		{0,
+		 0,
+		 _collision_rectangle_rectangle,
+		 _collision_rectangle_capsule,
+		 _collision_rectangle_convex_polygon},
+		{0,
+		 0,
+		 0,
+		 _collision_capsule_capsule,
+		 _collision_capsule_convex_polygon},
+		{0,
+		 0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon}
+
+	};
+
+	_CollectorCallback2D callback;
+	callback.callback=p_result_callback;
+	callback.swap=p_swap;
+	callback.userdata=p_userdata;
+	callback.collided=false;
+	callback.sep_axis=sep_axis;
+
+	const Shape2DSW *A=p_shape_A;
+	const Shape2DSW *B=p_shape_B;
+	const Matrix32 *transform_A=&p_transform_A;
+	const Matrix32 *transform_B=&p_transform_B;
+	const Matrix32 *transform_inv_A=&p_transform_inv_A;
+	const Matrix32 *transform_inv_B=&p_transform_inv_B;
+
+	if (type_A > type_B) {
+		SWAP(A,B);
+		SWAP(transform_A,transform_B);
+		SWAP(transform_inv_A,transform_inv_B);
+		SWAP(type_A,type_B);
+		callback.swap = !callback.swap;
+	}
+
+
+	CollisionFunc collision_func = collision_table[type_A-2][type_B-2];
+	ERR_FAIL_COND_V(!collision_func,false);
+
+
+	collision_func(A,*transform_A,*transform_inv_A,B,*transform_B,*transform_inv_B,&callback);
+
+	return callback.collided;
+
+
+}
diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h
new file mode 100644
index 000000000..dc6767d65
--- /dev/null
+++ b/servers/physics_2d/collision_solver_2d_sat.h
@@ -0,0 +1,37 @@
+/*************************************************************************/
+/*  collision_solver_2d_sat.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 COLLISION_SOLVER_2D_SAT_H
+#define COLLISION_SOLVER_2D_SAT_H
+
+#include "collision_solver_2d_sw.h"
+
+
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL);
+
+#endif // COLLISION_SOLVER_2D_SAT_H
diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp
new file mode 100644
index 000000000..cee5582c6
--- /dev/null
+++ b/servers/physics_2d/collision_solver_2d_sw.cpp
@@ -0,0 +1,309 @@
+/*************************************************************************/
+/*  collision_solver_2d_sw.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 "collision_solver_2d_sw.h"
+#include "collision_solver_2d_sat.h"
+
+
+#define collision_solver sat_2d_calculate_penetration
+//#define collision_solver gjk_epa_calculate_penetration
+
+
+bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+
+	const LineShape2DSW *line = static_cast<const LineShape2DSW*>(p_shape_A);
+	if (p_shape_B->get_type()==Physics2DServer::SHAPE_LINE)
+		return false;
+
+
+	Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized();
+	Vector2 p = p_transform_A.xform(line->get_normal()*line->get_d());
+	real_t d = n.dot(p);
+
+	Vector2 supports[2];
+	int support_count;
+
+	p_shape_B->get_supports(p_transform_inv_B.basis_xform(-n).normalized(),supports,support_count);
+
+	bool found=false;
+
+
+	for(int i=0;i<support_count;i++) {
+
+		supports[i] = p_transform_B.xform( supports[i] );
+		real_t pd = n.dot(supports[i]);
+		if (pd>=d)
+			continue;
+		found=true;
+
+		Vector2 support_A = supports[i] - n*(pd-d);
+
+		if (p_result_callback) {
+			if (p_swap_result)
+				p_result_callback(supports[i],support_A,p_userdata);
+			else
+				p_result_callback(support_A,supports[i],p_userdata);
+		}
+
+	}
+
+
+	return found;
+}
+
+bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
+
+
+
+	const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A);
+	if (p_shape_B->get_type()==Physics2DServer::SHAPE_RAY)
+		return false;
+
+	Vector2 from = p_transform_A.get_origin();
+	Vector2 to = from+p_transform_A[1]*ray->get_length();
+	Vector2 support_A=to;
+
+	from = p_transform_inv_B.xform(from);
+	to = p_transform_inv_B.xform(to);
+
+	Vector2 p,n;
+	if (!p_shape_B->intersect_segment(from,to,p,n)) {
+
+		if (sep_axis)
+			*sep_axis=p_transform_A[1].normalized();
+		return false;
+	}
+
+
+	Vector2 support_B=p_transform_B.xform(p);
+
+	if (p_result_callback) {
+		if (p_swap_result)
+			p_result_callback(support_B,support_A,p_userdata);
+		else
+			p_result_callback(support_A,support_B,p_userdata);
+	}
+	return true;
+
+}
+
+/*
+bool CollisionSolver2DSW::solve_ray(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_inverse_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+
+	const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A);
+
+	Vector2 from = p_transform_A.origin;
+	Vector2 to = from+p_transform_A.basis.get_axis(2)*ray->get_length();
+	Vector2 support_A=to;
+
+	from = p_inverse_B.xform(from);
+	to = p_inverse_B.xform(to);
+
+	Vector2 p,n;
+	if (!p_shape_B->intersect_segment(from,to,&p,&n))
+		return false;
+
+	Vector2 support_B=p_transform_B.xform(p);
+
+	if (p_result_callback) {
+		if (p_swap_result)
+			p_result_callback(support_B,support_A,p_userdata);
+		else
+			p_result_callback(support_A,support_B,p_userdata);
+	}
+	return true;
+}
+*/
+
+struct _ConcaveCollisionInfo2D {
+
+	const Matrix32 *transform_A;
+	const Matrix32 *transform_inv_A;
+	const Shape2DSW *shape_A;
+	const Matrix32 *transform_B;
+	const Matrix32 *transform_inv_B;
+	CollisionSolver2DSW::CallbackResult result_callback;
+	void *userdata;
+	bool swap_result;
+	bool collided;
+	int aabb_tests;
+	int collisions;
+	Vector2 *sep_axis;
+
+};
+
+void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) {
+
+
+
+	_ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D*)(p_userdata);
+	cinfo.aabb_tests++;
+	if (!cinfo.result_callback && cinfo.collided)
+		return; //already collided and no contacts requested, don't test anymore
+
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, *cinfo.transform_inv_A, p_convex,*cinfo.transform_B,*cinfo.transform_inv_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis );
+	if (!collided)
+		return;
+
+
+	cinfo.collided=true;
+	cinfo.collisions++;
+
+}
+
+bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
+
+
+	const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B);
+
+	_ConcaveCollisionInfo2D cinfo;
+	cinfo.transform_A=&p_transform_A;
+	cinfo.transform_inv_A=&p_transform_inv_A;
+	cinfo.shape_A=p_shape_A;
+	cinfo.transform_B=&p_transform_B;
+	cinfo.transform_inv_B=&p_transform_inv_B;
+	cinfo.result_callback=p_result_callback;
+	cinfo.userdata=p_userdata;
+	cinfo.swap_result=p_swap_result;
+	cinfo.collided=false;
+	cinfo.collisions=0;
+	cinfo.sep_axis=sep_axis;
+
+	cinfo.aabb_tests=0;
+
+	Matrix32 rel_transform = p_transform_A;
+	rel_transform.elements[2]-=p_transform_B.elements[2];
+
+	//quickly compute a local Rect2
+
+	Rect2 local_aabb;
+	for(int i=0;i<2;i++) {
+
+	     Vector2 axis( p_transform_B.elements[i] );
+	     float axis_scale = 1.0/axis.length();
+	     axis*=axis_scale;
+
+	     float smin,smax;
+	     p_shape_A->project_rangev(axis,rel_transform,smin,smax);
+	     smin*=axis_scale;
+	     smax*=axis_scale;
+
+	     local_aabb.pos[i]=smin;
+	     local_aabb.size[i]=smax-smin;
+	}
+
+	concave_B->cull(local_aabb,concave_callback,&cinfo);
+
+
+//	print_line("Rect2 TESTS: "+itos(cinfo.aabb_tests));	
+	return cinfo.collided;
+}
+
+
+bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) {
+
+
+
+	Physics2DServer::ShapeType type_A=p_shape_A->get_type();
+	Physics2DServer::ShapeType type_B=p_shape_B->get_type();
+	bool concave_A=p_shape_A->is_concave();
+	bool concave_B=p_shape_B->is_concave();
+
+	bool swap = false;
+
+	if (type_A>type_B) {
+		SWAP(type_A,type_B);
+		SWAP(concave_A,concave_B);
+		swap=true;
+	}
+
+	if (type_A==Physics2DServer::SHAPE_LINE) {
+
+		if (type_B==Physics2DServer::SHAPE_LINE || type_B==Physics2DServer::SHAPE_RAY) {
+			return false;
+		//if (type_B==Physics2DServer::SHAPE_RAY) {
+		//	return false;
+		}
+
+		if (swap) {
+			return solve_static_line(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true);
+		} else {
+			return solve_static_line(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false);
+		}
+
+	/*} else if (type_A==Physics2DServer::SHAPE_RAY) {
+
+		if (type_B==Physics2DServer::SHAPE_RAY)
+			return false;
+
+		if (swap) {
+			return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_inverse_A,p_result_callback,p_userdata,true);
+		} else {
+			return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_inverse_B,p_result_callback,p_userdata,false);
+		}
+*/
+	} else if (type_A==Physics2DServer::SHAPE_RAY) {
+
+		if (type_B==Physics2DServer::SHAPE_RAY) {
+
+			return false; //no ray-ray
+		}
+
+
+		if (swap) {
+			return solve_raycast(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis);
+		} else {
+			return solve_raycast(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis);
+		}
+
+
+	} else if (concave_B) {
+
+
+		if (concave_A)
+			return false;
+
+		if (!swap)
+			return solve_concave(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis);
+		else
+			return solve_concave(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis);
+
+
+
+	} else {
+
+
+		return collision_solver(p_shape_A, p_transform_A, p_transform_inv_A, p_shape_B, p_transform_B, p_transform_inv_B, p_result_callback,p_userdata,false,sep_axis);
+	}
+
+
+	return false;
+}
+
diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h
new file mode 100644
index 000000000..fc5500bfb
--- /dev/null
+++ b/servers/physics_2d/collision_solver_2d_sw.h
@@ -0,0 +1,52 @@
+/*************************************************************************/
+/*  collision_solver_2d_sw.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 COLLISION_SOLVER_2D_SW_H
+#define COLLISION_SOLVER_2D_SW_H
+
+#include "shape_2d_sw.h"
+
+class CollisionSolver2DSW {
+public:
+	typedef void (*CallbackResult)(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
+private:
+	static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+	static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
+	static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
+	static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
+
+
+
+public:
+
+	static bool solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_inverse_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_inverse_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL);
+
+
+};
+
+#endif // COLLISION_SOLVER_2D_SW_H
diff --git a/servers/physics_2d/constraint_2d_sw.cpp b/servers/physics_2d/constraint_2d_sw.cpp
new file mode 100644
index 000000000..e97b5d794
--- /dev/null
+++ b/servers/physics_2d/constraint_2d_sw.cpp
@@ -0,0 +1,30 @@
+/*************************************************************************/
+/*  constraint_2d_sw.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 "constraint_2d_sw.h"
+
diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h
new file mode 100644
index 000000000..7abe49f5b
--- /dev/null
+++ b/servers/physics_2d/constraint_2d_sw.h
@@ -0,0 +1,72 @@
+/*************************************************************************/
+/*  constraint_2d_sw.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 CONSTRAINT_2D_SW_H
+#define CONSTRAINT_2D_SW_H
+
+#include "body_2d_sw.h"
+
+class Constraint2DSW {
+
+	Body2DSW **_body_ptr;
+	int _body_count;
+	uint64_t island_step;
+	Constraint2DSW *island_next;
+	Constraint2DSW *island_list_next;
+
+
+	RID self;
+
+protected:
+	Constraint2DSW(Body2DSW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; }
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+
+
+	_FORCE_INLINE_ Constraint2DSW* get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(Constraint2DSW* p_next) { island_next=p_next; }
+
+	_FORCE_INLINE_ Constraint2DSW* get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(Constraint2DSW* p_next) { island_list_next=p_next; }
+
+	_FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; }
+	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
+
+
+	virtual bool setup(float p_step)=0;
+	virtual void solve(float p_step)=0;
+
+	virtual ~Constraint2DSW() {}
+};
+
+#endif // CONSTRAINT_2D_SW_H
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
new file mode 100644
index 000000000..fea58b6e8
--- /dev/null
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -0,0 +1,574 @@
+/*************************************************************************/
+/*  joints_2d_sw.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 "joints_2d_sw.h"
+#include "space_2d_sw.h"
+
+//based on chipmunk joint constraints
+
+/* Copyright (c) 2007 Scott Lembcke
+ *
+ * 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.
+ */
+
+static inline real_t k_scalar(Body2DSW *a,Body2DSW *b,const Vector2& rA, const Vector2& rB, const Vector2& n) {
+
+
+	real_t value=0;
+
+
+	{
+		value+=a->get_inv_mass();
+		real_t rcn = rA.cross(n);
+		value+=a->get_inv_inertia() * rcn * rcn;
+	}
+
+	if (b) {
+
+		value+=b->get_inv_mass();
+		real_t rcn = rB.cross(n);
+		value+=b->get_inv_inertia() * rcn * rcn;
+	}
+
+	return value;
+
+}
+
+static inline Vector2
+relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB){
+	Vector2 sum = a->get_linear_velocity() -rA.tangent() * a->get_angular_velocity();
+	if (b)
+		return (b->get_linear_velocity() -rB.tangent() * b->get_angular_velocity()) - sum;
+	else
+		return -sum;
+}
+
+static inline real_t
+normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n){
+	return relative_velocity(a, b, rA, rB).dot(n);
+}
+
+#if 0
+
+bool PinJoint2DSW::setup(float p_step) {
+
+	Space2DSW *space = A->get_space();
+	ERR_FAIL_COND_V(!space,false;)
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B?B->get_transform().basis_xform(anchor_B):anchor_B;
+
+	Vector2 gA = A->get_transform().get_origin();
+	Vector2 gB = B?B->get_transform().get_origin():Vector2();
+
+	Vector2 delta = gB - gA;
+	delta = (delta+rB) -rA;
+
+	real_t jdist = delta.length();
+	correct=false;
+	if (jdist==0)
+		return false; // do not correct
+
+	correct=true;
+
+	n = delta / jdist;
+
+	// calculate mass normal
+	mass_normal = 1.0f/k_scalar(A, B, rA, rB, n);
+
+	// calculate bias velocity
+	//real_t maxBias = joint->constraint.maxBias;
+	bias = -(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)*(jdist-dist);
+	bias = CLAMP(bias, -get_max_bias(), +get_max_bias());
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// apply accumulated impulse
+	Vector2 j = n * jn_acc;
+	A->apply_impulse(rA,-j);
+	if (B)
+		B->apply_impulse(rB,j);
+
+	print_line("setup");
+	return true;
+}
+
+
+
+void PinJoint2DSW::solve(float p_step){
+
+	if (!correct)
+		return;
+
+	Vector2 ln = n;
+
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A,B, rA, rB, ln);
+
+	// compute normal impulse
+	real_t jn = (bias - vrn)*mass_normal;
+	real_t jnOld = jn_acc;
+	jn_acc = CLAMP(jnOld + jn,-jn_max,jn_max); //cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
+	jn = jn_acc - jnOld;
+	print_line("jn_acc: "+rtos(jn_acc));
+	Vector2 j = jn*ln;
+
+	A->apply_impulse(rA,-j);
+	if (B)
+		B->apply_impulse(rB,j);
+
+}
+
+
+PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) {
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
+	anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos;
+
+	jn_acc=0;
+	dist=0;
+
+	p_body_a->add_constraint(this,0);
+	if (p_body_b)
+		p_body_b->add_constraint(this,1);
+
+}
+
+PinJoint2DSW::~PinJoint2DSW() {
+
+	if (A)
+		A->remove_constraint(this);
+	if (B)
+		B->remove_constraint(this);
+
+}
+
+#else
+
+
+bool PinJoint2DSW::setup(float p_step) {
+
+	Space2DSW *space = A->get_space();
+	ERR_FAIL_COND_V(!space,false;)
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B?B->get_transform().basis_xform(anchor_B):anchor_B;
+#if 0
+	Vector2 gA = rA+A->get_transform().get_origin();
+	Vector2 gB = B?rB+B->get_transform().get_origin():rB;
+
+	VectorB delta = gB - gA;
+
+	real_t jdist = delta.length();
+	correct=false;
+	if (jdist==0)
+		return false; // do not correct
+#endif
+
+	// deltaV = deltaV0 + K * impulse
+	// invM = [(1/m1 + 1/m2) * eye(2) - skew(rA) * invI1 * skew(rA) - skew(rB) * invI2 * skew(rB)]
+	//      = [1/m1+1/m2     0    ] + invI1 * [rA.y*rA.y -rA.x*rA.y] + invI2 * [rA.y*rA.y -rA.x*rA.y]
+	//        [    0     1/m1+1/m2]           [-rA.x*rA.y rA.x*rA.x]           [-rA.x*rA.y rA.x*rA.x]
+
+	real_t B_inv_mass = B?B->get_inv_mass():0.0;
+
+
+	Matrix32 K1;
+	K1[0].x = A->get_inv_mass() + B_inv_mass;	K1[1].x = 0.0f;
+	K1[0].y = 0.0f;					K1[1].y = A->get_inv_mass() + B_inv_mass;
+
+	Matrix32 K2;
+	K2[0].x =  A->get_inv_inertia() * rA.y * rA.y;		K2[1].x = -A->get_inv_inertia() * rA.x * rA.y;
+	K2[0].y = -A->get_inv_inertia() * rA.x * rA.y;		K2[1].y =  A->get_inv_inertia() * rA.x * rA.x;
+
+	Matrix32 K;
+	K[0]= K1[0] + K2[0];
+	K[1]= K1[1] + K2[1];
+
+	if (B) {
+
+		Matrix32 K3;
+		K3[0].x =  B->get_inv_inertia() * rB.y * rB.y;		K3[1].x = -B->get_inv_inertia() * rB.x * rB.y;
+		K3[0].y = -B->get_inv_inertia() * rB.x * rB.y;		K3[1].y =  B->get_inv_inertia() * rB.x * rB.x;
+
+		K[0]+=K3[0];
+		K[1]+=K3[1];
+	}
+
+	K[0].x += softness;
+	K[1].y += softness;
+
+	M = K.affine_inverse();
+
+	Vector2 gA = rA+A->get_transform().get_origin();
+	Vector2 gB = B?rB+B->get_transform().get_origin():rB;
+
+	Vector2 delta = gB - gA;
+
+	bias = delta*-(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step);
+
+	// apply accumulated impulse
+	A->apply_impulse(rA,-P);
+	if (B)
+		B->apply_impulse(rB,P);
+
+	return true;
+}
+
+void PinJoint2DSW::solve(float p_step){
+
+
+	// compute relative velocity
+	Vector2 vA = A->get_linear_velocity() - rA.cross(A->get_angular_velocity());
+
+	Vector2 rel_vel;
+	if (B)
+		rel_vel = B->get_linear_velocity() - rB.cross(B->get_angular_velocity()) - vA;
+	else
+		rel_vel = -vA;
+
+	Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness,softness) * P);
+
+	A->apply_impulse(rA,-impulse);
+	if (B)
+		B->apply_impulse(rB,impulse);
+
+
+	P += impulse;
+}
+
+
+PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) {
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
+	anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos;
+
+	softness=0;
+
+	p_body_a->add_constraint(this,0);
+	if (p_body_b)
+		p_body_b->add_constraint(this,1);
+
+}
+
+
+
+PinJoint2DSW::~PinJoint2DSW() {
+
+	if (A)
+		A->remove_constraint(this);
+	if (B)
+		B->remove_constraint(this);
+
+}
+
+
+
+#endif
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+
+static inline void
+k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2)
+{
+	// calculate mass matrix
+	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
+	real_t k11, k12, k21, k22;
+	real_t m_sum = a->get_inv_mass() + b->get_inv_mass();
+
+	// start with I*m_sum
+	k11 = m_sum; k12 = 0.0f;
+	k21 = 0.0f;  k22 = m_sum;
+
+	// add the influence from r1
+	real_t a_i_inv = a->get_inv_inertia();
+	real_t r1xsq =  r1.x * r1.x * a_i_inv;
+	real_t r1ysq =  r1.y * r1.y * a_i_inv;
+	real_t r1nxy = -r1.x * r1.y * a_i_inv;
+	k11 += r1ysq; k12 += r1nxy;
+	k21 += r1nxy; k22 += r1xsq;
+
+	// add the influnce from r2
+	real_t b_i_inv = b->get_inv_inertia();
+	real_t r2xsq =  r2.x * r2.x * b_i_inv;
+	real_t r2ysq =  r2.y * r2.y * b_i_inv;
+	real_t r2nxy = -r2.x * r2.y * b_i_inv;
+	k11 += r2ysq; k12 += r2nxy;
+	k21 += r2nxy; k22 += r2xsq;
+
+	// invert
+	real_t determinant = k11*k22 - k12*k21;
+	ERR_FAIL_COND(determinant== 0.0);
+
+	real_t det_inv = 1.0f/determinant;
+	*k1 = Vector2( k22*det_inv, -k12*det_inv);
+	*k2 = Vector2(-k21*det_inv,  k11*det_inv);
+}
+
+static _FORCE_INLINE_ Vector2
+mult_k(const Vector2& vr, const Vector2 &k1, const Vector2 &k2)
+{
+	return Vector2(vr.dot(k1), vr.dot(k2));
+}
+
+bool GrooveJoint2DSW::setup(float p_step) {
+
+
+	// calculate endpoints in worldspace
+	Vector2 ta = A->get_transform().xform(A_groove_1);
+	Vector2 tb = A->get_transform().xform(A_groove_2);
+	Space2DSW *space=A->get_space();
+
+	// calculate axis
+	Vector2 n = -(tb - ta).tangent().normalized();
+	real_t d = ta.dot(n);
+
+	xf_normal = n;
+	rB = B->get_transform().basis_xform(B_anchor);
+
+	// calculate tangential distance along the axis of rB
+	real_t td = (B->get_transform().get_origin() + rB).cross(n);
+	// calculate clamping factor and rB
+	if(td <= ta.cross(n)){
+		clamp = 1.0f;
+		rA = ta - A->get_transform().get_origin();
+	} else if(td >= tb.cross(n)){
+		clamp = -1.0f;
+		rA = tb - A->get_transform().get_origin();
+	} else {
+		clamp = 0.0f;
+		//joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
+		rA =  ((-n.tangent() * -td) + n*d) - A->get_transform().get_origin();
+	}
+
+	// Calculate mass tensor
+	k_tensor(A, B, rA, rB, &k1, &k2);
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// calculate bias velocity
+//	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+//	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
+
+
+	Vector2 delta = (B->get_transform().get_origin() +rB) - (A->get_transform().get_origin() + rA);
+	float _b = get_bias();
+	_b=0.001;
+	gbias=(delta*-(_b==0?space->get_constraint_bias():_b)*(1.0/p_step)).clamped(get_max_bias());
+
+	// apply accumulated impulse
+	A->apply_impulse(rA,-jn_acc);
+	B->apply_impulse(rB,jn_acc);
+
+	correct=true;
+	return true;
+}
+
+void GrooveJoint2DSW::solve(float p_step){
+
+
+	// compute impulse
+	Vector2 vr = relative_velocity(A, B, rA,rB);
+
+	Vector2 j = mult_k(gbias-vr, k1, k2);
+	Vector2 jOld = jn_acc;
+	j+=jOld;
+
+	jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : xf_normal.project(j)).clamped(jn_max);
+
+	j = jn_acc - jOld;
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+}
+
+
+GrooveJoint2DSW::GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
+
+	A=p_body_a;
+	B=p_body_b;
+
+	A_groove_1 = A->get_inv_transform().xform(p_a_groove1);
+	A_groove_2 = A->get_inv_transform().xform(p_a_groove2);
+	B_anchor=B->get_inv_transform().xform(p_b_anchor);
+	A_groove_normal = -(A_groove_2 - A_groove_1).normalized().tangent();
+
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+
+}
+
+GrooveJoint2DSW::~GrooveJoint2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+}
+
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+
+bool DampedSpringJoint2DSW::setup(float p_step) {
+
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B->get_transform().basis_xform(anchor_B);
+
+	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA) ;
+	real_t dist = delta.length();
+
+	if (dist)
+		n=delta/dist;
+	else
+		n=Vector2();
+
+	real_t k = k_scalar(A, B, rA, rB, n);
+	n_mass = 1.0f/k;
+
+	target_vrn = 0.0f;
+	v_coef = 1.0f - Math::exp(-damping*(p_step)*k);
+
+	// apply spring force
+	real_t f_spring = (rest_length - dist) * stiffness;
+	Vector2 j = n * f_spring*(p_step);
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+
+
+	return true;
+}
+
+void DampedSpringJoint2DSW::solve(float p_step) {
+
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn;
+
+	// compute velocity loss from drag
+	// not 100% certain this is derived correctly, though it makes sense
+	real_t v_damp = -vrn*v_coef;
+	target_vrn = vrn + v_damp;
+	Vector2 j=n*v_damp*n_mass;
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+
+}
+
+void DampedSpringJoint2DSW::set_param(Physics2DServer::DampedStringParam p_param, real_t p_value) {
+
+	switch(p_param) {
+
+		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
+
+			rest_length=p_value;
+		} break;
+		case Physics2DServer::DAMPED_STRING_DAMPING: {
+
+			damping=p_value;
+		} break;
+		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
+
+			stiffness=p_value;
+		} break;
+	}
+
+}
+
+real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const{
+
+	switch(p_param) {
+
+		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
+
+			return rest_length;
+		} break;
+		case Physics2DServer::DAMPED_STRING_DAMPING: {
+
+			return damping;
+		} break;
+		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
+
+			return stiffness;
+		} break;
+	}
+
+	ERR_FAIL_V(0);
+}
+
+
+DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
+
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = A->get_inv_transform().xform(p_anchor_a);
+	anchor_B = B->get_inv_transform().xform(p_anchor_b);
+
+	rest_length=p_anchor_a.distance_to(p_anchor_b);
+	stiffness=20;
+	damping=1.5;
+
+
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+
+}
+
+DampedSpringJoint2DSW::~DampedSpringJoint2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+
+}
+
+
diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h
new file mode 100644
index 000000000..0a9bf3425
--- /dev/null
+++ b/servers/physics_2d/joints_2d_sw.h
@@ -0,0 +1,210 @@
+/*************************************************************************/
+/*  joints_2d_sw.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 JOINTS_2D_SW_H
+#define JOINTS_2D_SW_H
+
+#include "constraint_2d_sw.h"
+#include "body_2d_sw.h"
+
+
+
+class Joint2DSW : public Constraint2DSW {
+
+	real_t max_force;
+	real_t bias;
+	real_t max_bias;
+public:
+
+	_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; }
+	_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
+
+	_FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; }
+	_FORCE_INLINE_ real_t get_bias() const { return bias; }
+
+	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; }
+	_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
+
+	virtual Physics2DServer::JointType get_type() const=0;
+	Joint2DSW(Body2DSW **p_body_ptr=NULL,int p_body_count=0) : Constraint2DSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; };
+
+};
+#if 0
+
+class PinJoint2DSW : public Joint2DSW {
+
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+	real_t dist;
+	real_t jn_acc;
+	real_t jn_max;
+	real_t max_distance;
+	real_t mass_normal;
+	real_t bias;
+
+	Vector2 rA,rB;
+	Vector2 n; //normal
+	bool correct;
+
+
+public:
+
+	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_PIN; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b=NULL);
+	~PinJoint2DSW();
+};
+
+#else
+
+class PinJoint2DSW : public Joint2DSW {
+
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+	Matrix32 M;
+	Vector2 rA,rB;
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+	Vector2 bias;
+	Vector2 P;
+	real_t softness;
+
+public:
+
+	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_PIN; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b=NULL);
+	~PinJoint2DSW();
+};
+
+
+#endif
+class GrooveJoint2DSW : public Joint2DSW {
+
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+	Vector2 A_groove_1;
+	Vector2 A_groove_2;
+	Vector2 A_groove_normal;
+	Vector2 B_anchor;
+	Vector2 jn_acc;
+	Vector2 gbias;
+	real_t jn_max;
+	real_t clamp;
+	Vector2 xf_normal;
+	Vector2 rA,rB;
+	Vector2 k1,k2;
+
+
+	bool correct;
+
+public:
+
+	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_GROOVE; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b);
+	~GrooveJoint2DSW();
+};
+
+
+class DampedSpringJoint2DSW : public Joint2DSW {
+
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+
+	real_t rest_length;
+	real_t damping;
+	real_t stiffness;
+
+	Vector2 rA,rB;
+	Vector2 n;
+	real_t n_mass;
+	real_t target_vrn;
+	real_t v_coef;
+
+public:
+
+	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_DAMPED_SPRING; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+	void set_param(Physics2DServer::DampedStringParam p_param, real_t p_value);
+	real_t get_param(Physics2DServer::DampedStringParam p_param) const;
+
+	DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b);
+	~DampedSpringJoint2DSW();
+};
+
+
+#endif // JOINTS_2D_SW_H
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
new file mode 100644
index 000000000..70e3d843b
--- /dev/null
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -0,0 +1,1046 @@
+/*************************************************************************/
+/*  physics_2d_server_sw.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 "physics_2d_server_sw.h"
+#include "broad_phase_2d_basic.h"
+#include "broad_phase_2d_hash_grid.h"
+
+RID Physics2DServerSW::shape_create(ShapeType p_shape) {
+
+	Shape2DSW *shape=NULL;
+	switch(p_shape) {
+
+		case SHAPE_LINE: {
+
+			shape=memnew( LineShape2DSW );
+		} break;
+		case SHAPE_RAY: {
+
+			shape=memnew( RayShape2DSW );
+		} break;
+		case SHAPE_SEGMENT: {
+
+			shape=memnew( SegmentShape2DSW);
+		} break;
+		case SHAPE_CIRCLE: {
+
+			shape=memnew( CircleShape2DSW);
+		} break;
+		case SHAPE_RECTANGLE: {
+
+			shape=memnew( RectangleShape2DSW);
+		} break;
+		case SHAPE_CAPSULE: {
+
+			shape=memnew( CapsuleShape2DSW );
+		} break;
+		case SHAPE_CONVEX_POLYGON: {
+
+			shape=memnew( ConvexPolygonShape2DSW );
+		} break;
+		case SHAPE_CONCAVE_POLYGON: {
+
+			shape=memnew( ConcavePolygonShape2DSW );
+		} break;
+		case SHAPE_CUSTOM: {
+
+			ERR_FAIL_V(RID());
+
+		} break;
+
+	}
+
+	RID id = shape_owner.make_rid(shape);
+	shape->set_self(id);
+
+	return id;
+};
+
+void Physics2DServerSW::shape_set_data(RID p_shape, const Variant& p_data) {
+
+	Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	shape->set_data(p_data);
+
+
+};
+
+
+void Physics2DServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
+
+	Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	shape->set_custom_bias(p_bias);
+
+}
+
+
+Physics2DServer::ShapeType Physics2DServerSW::shape_get_type(RID p_shape) const {
+
+	const Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,SHAPE_CUSTOM);
+	return shape->get_type();
+
+};
+
+Variant Physics2DServerSW::shape_get_data(RID p_shape) const {
+
+	const Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,Variant());
+	ERR_FAIL_COND_V(!shape->is_configured(),Variant());
+	return shape->get_data();
+
+};
+
+real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const {
+
+	const Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,0);
+	return shape->get_custom_bias();
+
+}
+
+
+RID Physics2DServerSW::space_create() {
+
+	Space2DSW *space = memnew( Space2DSW );
+	RID id = space_owner.make_rid(space);
+	space->set_self(id);
+	RID area_id = area_create();
+	Area2DSW *area = area_owner.get(area_id);
+	ERR_FAIL_COND_V(!area,RID());
+	space->set_default_area(area);
+	area->set_space(space);
+	area->set_priority(-1);
+
+	return id;
+};
+
+void Physics2DServerSW::space_set_active(RID p_space,bool p_active) {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+	if (p_active)
+		active_spaces.insert(space);
+	else
+		active_spaces.erase(space);
+}
+
+bool Physics2DServerSW::space_is_active(RID p_space) const {
+
+	const Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,false);
+
+	return active_spaces.has(space);
+
+}
+
+void Physics2DServerSW::space_set_param(RID p_space,SpaceParameter p_param, real_t p_value) {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+
+	space->set_param(p_param,p_value);
+
+}
+
+real_t Physics2DServerSW::space_get_param(RID p_space,SpaceParameter p_param) const {
+
+	const Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,0);
+	return space->get_param(p_param);
+}
+
+Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space) {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,NULL);
+	if (/*doing_sync ||*/ space->is_locked()) {
+
+		ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification.");
+		ERR_FAIL_V(NULL);
+	}
+
+	return space->get_direct_state();
+}
+
+RID Physics2DServerSW::area_create() {
+
+	Area2DSW *area = memnew( Area2DSW );
+	RID rid = area_owner.make_rid(area);
+	area->set_self(rid);
+	return rid;
+};
+
+void Physics2DServerSW::area_set_space(RID p_area, RID p_space) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	Space2DSW *space=NULL;
+	if (p_space.is_valid()) {
+		space = space_owner.get(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	area->set_space(space);
+
+};
+
+RID Physics2DServerSW::area_get_space(RID p_area) const {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,RID());
+
+	Space2DSW *space = area->get_space();
+	if (!space)
+		return RID();
+	return space->get_self();
+};
+
+void Physics2DServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) {
+
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_space_override_mode(p_mode);
+}
+
+Physics2DServer::AreaSpaceOverrideMode Physics2DServerSW::area_get_space_override_mode(RID p_area) const {
+
+	const Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,AREA_SPACE_OVERRIDE_DISABLED);
+
+	return area->get_space_override_mode();
+}
+
+
+void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	area->add_shape(shape,p_transform);
+
+}
+
+void Physics2DServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	ERR_FAIL_COND(!shape->is_configured());
+
+	area->set_shape(p_shape_idx,shape);
+
+}
+void Physics2DServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_shape_transform(p_shape_idx,p_transform);
+}
+
+int Physics2DServerSW::area_get_shape_count(RID p_area) const {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,-1);
+
+	return area->get_shape_count();
+
+}
+RID Physics2DServerSW::area_get_shape(RID p_area, int p_shape_idx) const {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,RID());
+
+	Shape2DSW *shape = area->get_shape(p_shape_idx);
+	ERR_FAIL_COND_V(!shape,RID());
+
+	return shape->get_self();
+}
+Matrix32 Physics2DServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,Matrix32());
+
+	return area->get_shape_transform(p_shape_idx);
+}
+
+void Physics2DServerSW::area_remove_shape(RID p_area, int p_shape_idx) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->remove_shape(p_shape_idx);
+}
+
+void Physics2DServerSW::area_clear_shapes(RID p_area) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	while(area->get_shape_count())
+		area->remove_shape(0);
+
+}
+
+void Physics2DServerSW::area_attach_object_instance_ID(RID p_area,ObjectID p_ID) {
+
+	if (space_owner.owns(p_area)) {
+		Space2DSW *space=space_owner.get(p_area);
+		p_area=space->get_default_area()->get_self();
+	}
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_instance_id(p_ID);
+
+}
+ObjectID Physics2DServerSW::area_get_object_instance_ID(RID p_area) const {
+
+	if (space_owner.owns(p_area)) {
+		Space2DSW *space=space_owner.get(p_area);
+		p_area=space->get_default_area()->get_self();
+	}
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,0);
+	return area->get_instance_id();
+
+
+}
+
+
+void Physics2DServerSW::area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value) {
+
+	if (space_owner.owns(p_area)) {
+		Space2DSW *space=space_owner.get(p_area);
+		p_area=space->get_default_area()->get_self();
+	}
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_param(p_param,p_value);
+
+};
+
+
+void Physics2DServerSW::area_set_transform(RID p_area, const Matrix32& p_transform) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_transform(p_transform);
+
+};
+
+Variant Physics2DServerSW::area_get_param(RID p_area,AreaParameter p_param) const {
+
+	if (space_owner.owns(p_area)) {
+		Space2DSW *space=space_owner.get(p_area);
+		p_area=space->get_default_area()->get_self();
+	}
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,Variant());
+
+	return area->get_param(p_param);
+};
+
+Matrix32 Physics2DServerSW::area_get_transform(RID p_area) const {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area,Matrix32());
+
+	return area->get_transform();
+};
+
+void Physics2DServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method);
+
+
+}
+
+
+/* BODY API */
+
+RID Physics2DServerSW::body_create(BodyMode p_mode,bool p_init_sleeping) {
+
+	Body2DSW *body = memnew( Body2DSW );
+	if (p_mode!=BODY_MODE_RIGID)
+		body->set_mode(p_mode);
+	if (p_init_sleeping)
+		body->set_state(BODY_STATE_SLEEPING,p_init_sleeping);
+	RID rid = body_owner.make_rid(body);
+	body->set_self(rid);
+	return rid;
+};
+
+
+void Physics2DServerSW::body_set_space(RID p_body, RID p_space) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	Space2DSW *space=NULL;
+	if (p_space.is_valid()) {
+		space = space_owner.get(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	body->set_space(space);
+
+};
+
+RID Physics2DServerSW::body_get_space(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,RID());
+
+	Space2DSW *space = body->get_space();
+	if (!space)
+		return RID();
+	return space->get_self();
+};
+
+
+void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_mode(p_mode);
+};
+
+Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body, BodyMode p_mode) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,BODY_MODE_STATIC);
+
+	return body->get_mode();
+};
+
+void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	body->add_shape(shape,p_transform);
+
+}
+
+void Physics2DServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	Shape2DSW *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	ERR_FAIL_COND(!shape->is_configured());
+
+	body->set_shape(p_shape_idx,shape);
+
+}
+void Physics2DServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_shape_transform(p_shape_idx,p_transform);
+}
+
+int Physics2DServerSW::body_get_shape_count(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,-1);
+
+	return body->get_shape_count();
+
+}
+RID Physics2DServerSW::body_get_shape(RID p_body, int p_shape_idx) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,RID());
+
+	Shape2DSW *shape = body->get_shape(p_shape_idx);
+	ERR_FAIL_COND_V(!shape,RID());
+
+	return shape->get_self();
+}
+Matrix32 Physics2DServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,Matrix32());
+
+	return body->get_shape_transform(p_shape_idx);
+}
+
+void Physics2DServerSW::body_remove_shape(RID p_body, int p_shape_idx) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->remove_shape(p_shape_idx);
+}
+
+void Physics2DServerSW::body_clear_shapes(RID p_body) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	while(body->get_shape_count())
+		body->remove_shape(0);
+
+}
+
+
+void Physics2DServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	ERR_FAIL_INDEX(p_shape_idx,body->get_shape_count());
+
+	body->set_shape_as_trigger(p_shape_idx,p_enable);
+
+}
+
+bool Physics2DServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const {
+
+	const Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,false);
+
+	ERR_FAIL_INDEX_V(p_shape_idx,body->get_shape_count(),false);
+
+	return body->is_shape_set_as_trigger(p_shape_idx);
+
+}
+
+
+void Physics2DServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_continuous_collision_detection(p_enable);
+
+}
+
+bool Physics2DServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,false);
+
+	return body->is_continuous_collision_detection_enabled();
+}
+
+void Physics2DServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_instance_id(p_ID);
+
+};
+
+uint32_t Physics2DServerSW::body_get_object_instance_ID(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+
+	return body->get_instance_id();
+};
+
+
+void Physics2DServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+};
+
+uint32_t Physics2DServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+
+	return 0;
+};
+
+void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_param(p_param,p_value);
+};
+
+float Physics2DServerSW::body_get_param(RID p_body, BodyParameter p_param) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+
+	return body->get_param(p_param);
+};
+
+
+void Physics2DServerSW::body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->simulate_motion(p_new_transform,last_step);
+
+};
+
+void Physics2DServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_state(p_state,p_variant);
+};
+
+Variant Physics2DServerSW::body_get_state(RID p_body, BodyState p_state) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,Variant());
+
+	return body->get_state(p_state);
+};
+
+
+void Physics2DServerSW::body_set_applied_force(RID p_body, const Vector2& p_force) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_applied_force(p_force);
+};
+
+Vector2 Physics2DServerSW::body_get_applied_force(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,Vector2());
+	return body->get_applied_force();
+};
+
+void Physics2DServerSW::body_set_applied_torque(RID p_body, float p_torque) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_applied_torque(p_torque);
+};
+
+float Physics2DServerSW::body_get_applied_torque(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+
+	return body->get_applied_torque();
+};
+
+void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->apply_impulse(p_pos,p_impulse);
+};
+
+void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	Vector2 v = body->get_linear_velocity();
+	Vector2 axis = p_axis_velocity.normalized();
+	v-=axis*axis.dot(v);
+	v+=p_axis_velocity;
+	body->set_linear_velocity(v);
+
+};
+
+void Physics2DServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_exception(p_body_b);
+
+};
+
+void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->remove_exception(p_body);
+
+};
+
+void Physics2DServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	for(int i=0;i<body->get_exceptions().size();i++) {
+		p_exceptions->push_back(body->get_exceptions()[i]);
+	}
+
+};
+
+void Physics2DServerSW::body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+};
+
+float Physics2DServerSW::body_get_contacts_reported_depth_treshold(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+	return 0;
+};
+
+void Physics2DServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_omit_force_integration(p_omit);
+};
+
+bool Physics2DServerSW::body_is_omitting_force_integration(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,false);
+	return body->get_omit_force_integration();
+};
+
+void Physics2DServerSW::body_set_max_contacts_reported(RID p_body, int p_contacts) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_max_contacts_reported(p_contacts);
+}
+
+int Physics2DServerSW::body_get_max_contacts_reported(RID p_body) const {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,-1);
+	return body->get_max_contacts_reported();
+}
+
+void Physics2DServerSW::body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata) {
+
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_force_integration_callback(p_receiver?p_receiver->get_instance_ID():ObjectID(0),p_method,p_udata);
+
+}
+
+
+/* JOINT API */
+
+void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
+
+	Joint2DSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+
+	switch(p_param) {
+		case JOINT_PARAM_BIAS: joint->set_bias(p_value); break;
+		case JOINT_PARAM_MAX_BIAS: joint->set_max_bias(p_value); break;
+		case JOINT_PARAM_MAX_FORCE: joint->set_max_force(p_value); break;
+	}
+
+
+}
+
+real_t Physics2DServerSW::joint_get_param(RID p_joint,JointParam p_param) const {
+
+	const Joint2DSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint,-1);
+
+	switch(p_param) {
+		case JOINT_PARAM_BIAS: return joint->get_bias(); break;
+		case JOINT_PARAM_MAX_BIAS: return joint->get_max_bias(); break;
+		case JOINT_PARAM_MAX_FORCE: return joint->get_max_force(); break;
+	}
+
+	return 0;
+}
+
+
+RID Physics2DServerSW::pin_joint_create(const Vector2& p_pos,RID p_body_a,RID p_body_b) {
+
+	Body2DSW *A=body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A,RID());
+	Body2DSW *B=NULL;
+	if (body_owner.owns(p_body_b)) {
+		B=body_owner.get(p_body_b);
+		ERR_FAIL_COND_V(!B,RID());
+	}
+
+	Joint2DSW *joint = memnew( PinJoint2DSW(p_pos,A,B) );
+	RID self = joint_owner.make_rid(joint);
+	joint->set_self(self);
+
+	return self;
+}
+
+RID Physics2DServerSW::groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b) {
+
+
+	Body2DSW *A=body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A,RID());
+
+	Body2DSW *B=body_owner.get(p_body_b);
+	ERR_FAIL_COND_V(!B,RID());
+
+	Joint2DSW *joint = memnew( GrooveJoint2DSW(p_a_groove1,p_a_groove2,p_b_anchor,A,B) );
+	RID self = joint_owner.make_rid(joint);
+	joint->set_self(self);
+	return self;
+
+
+}
+
+RID Physics2DServerSW::damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b) {
+
+	Body2DSW *A=body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A,RID());
+
+	Body2DSW *B=body_owner.get(p_body_b);
+	ERR_FAIL_COND_V(!B,RID());
+
+	Joint2DSW *joint = memnew( DampedSpringJoint2DSW(p_anchor_a,p_anchor_b,A,B) );
+	RID self = joint_owner.make_rid(joint);
+	joint->set_self(self);
+	return self;
+
+}
+
+void Physics2DServerSW::damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value) {
+
+
+	Joint2DSW *j = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!j);
+	ERR_FAIL_COND(j->get_type()!=JOINT_DAMPED_SPRING);
+
+	DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW*>(j);
+	dsj->set_param(p_param,p_value);
+}
+
+real_t Physics2DServerSW::damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const {
+
+	Joint2DSW *j = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!j,0);
+	ERR_FAIL_COND_V(j->get_type()!=JOINT_DAMPED_SPRING,0);
+
+	DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW*>(j);
+	return dsj->get_param(p_param);
+}
+
+Physics2DServer::JointType Physics2DServerSW::joint_get_type(RID p_joint) const {
+
+
+	Joint2DSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint,JOINT_PIN);
+
+	return joint->get_type();
+}
+
+
+
+void Physics2DServerSW::free(RID p_rid) {
+
+	if (shape_owner.owns(p_rid)) {
+
+		Shape2DSW *shape = shape_owner.get(p_rid);
+
+		while(shape->get_owners().size()) {
+			ShapeOwner2DSW *so=shape->get_owners().front()->key();
+			so->remove_shape(shape);
+		}
+
+		shape_owner.free(p_rid);
+		memdelete(shape);
+	} else if (body_owner.owns(p_rid)) {
+
+		Body2DSW *body = body_owner.get(p_rid);
+
+//		if (body->get_state_query())
+//			_clear_query(body->get_state_query());
+
+//		if (body->get_direct_state_query())
+//			_clear_query(body->get_direct_state_query());
+
+		body->set_space(NULL);
+
+
+		while( body->get_shape_count() ) {
+
+			body->remove_shape(0);
+		}
+
+		while (body->get_constraint_map().size()) {
+			RID self = body->get_constraint_map().front()->key()->get_self();
+			ERR_FAIL_COND(!self.is_valid());
+			free(self);
+		}
+
+		body_owner.free(p_rid);
+		memdelete(body);
+
+	} else if (area_owner.owns(p_rid)) {
+
+		Area2DSW *area = area_owner.get(p_rid);
+
+//		if (area->get_monitor_query())
+//			_clear_query(area->get_monitor_query());
+
+		area->set_space(NULL);
+
+		while( area->get_shape_count() ) {
+
+			area->remove_shape(0);
+		}
+
+		area_owner.free(p_rid);
+		memdelete(area);
+	} else if (space_owner.owns(p_rid)) {
+
+		Space2DSW *space = space_owner.get(p_rid);
+
+		while(space->get_objects().size()) {
+			CollisionObject2DSW *co = (CollisionObject2DSW *)space->get_objects().front()->get();
+			co->set_space(NULL);
+		}
+
+		active_spaces.erase(space);
+		free(space->get_default_area()->get_self());
+		space_owner.free(p_rid);
+		memdelete(space);
+	} else if (joint_owner.owns(p_rid)) {
+
+		Joint2DSW *joint = joint_owner.get(p_rid);
+
+		joint_owner.free(p_rid);
+		memdelete(joint);
+
+	} else {
+
+		ERR_EXPLAIN("Invalid ID");
+		ERR_FAIL();
+	}
+
+
+};
+
+void Physics2DServerSW::set_active(bool p_active) {
+
+	active=p_active;
+};
+
+void Physics2DServerSW::init() {
+
+	doing_sync=true;
+	last_step=0.001;
+	iterations=8;// 8?
+	stepper = memnew( Step2DSW );
+	direct_state = memnew( Physics2DDirectBodyStateSW );
+};
+
+
+void Physics2DServerSW::step(float p_step) {
+
+
+	if (!active)
+		return;
+
+	doing_sync=false;
+
+	last_step=p_step;
+	Physics2DDirectBodyStateSW::singleton->step=p_step;
+	for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+
+		stepper->step((Space2DSW*)E->get(),p_step,iterations);
+	}
+};
+
+void Physics2DServerSW::sync() {
+
+};
+
+void Physics2DServerSW::flush_queries() {
+
+	if (!active)
+		return;
+
+	doing_sync=true;
+	for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+
+		Space2DSW *space=(Space2DSW *)E->get();
+		space->call_queries();
+	}
+
+};
+
+
+
+void Physics2DServerSW::finish() {
+
+	memdelete(stepper);
+	memdelete(direct_state);
+};
+
+
+Physics2DServerSW::Physics2DServerSW() {
+
+	BroadPhase2DSW::create_func=BroadPhase2DHashGrid::_create;
+//	BroadPhase2DSW::create_func=BroadPhase2DBasic::_create;
+
+	active=true;
+};
+
+Physics2DServerSW::~Physics2DServerSW() {
+
+};
+
+
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
new file mode 100644
index 000000000..f6665da73
--- /dev/null
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -0,0 +1,215 @@
+/*************************************************************************/
+/*  physics_2d_server_sw.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 PHYSICS_2D_SERVER_SW
+#define PHYSICS_2D_SERVER_SW
+
+
+#include "servers/physics_2d_server.h"
+#include "shape_2d_sw.h"
+#include "space_2d_sw.h"
+#include "step_2d_sw.h"
+#include "joints_2d_sw.h"
+
+
+class Physics2DServerSW : public Physics2DServer {
+
+	OBJ_TYPE( Physics2DServerSW, Physics2DServer );
+
+friend class Physics2DDirectSpaceStateSW;
+	bool active;
+	int iterations;
+	bool doing_sync;
+	real_t last_step;
+
+	Step2DSW *stepper;
+	Set<const Space2DSW*> active_spaces;
+
+	Physics2DDirectBodyStateSW *direct_state;
+
+	mutable RID_Owner<Shape2DSW> shape_owner;
+	mutable RID_Owner<Space2DSW> space_owner;
+	mutable RID_Owner<Area2DSW> area_owner;
+	mutable RID_Owner<Body2DSW> body_owner;
+	mutable RID_Owner<Joint2DSW> joint_owner;
+
+//	void _clear_query(Query2DSW *p_query);
+public:
+
+	virtual RID shape_create(ShapeType p_shape);
+	virtual void shape_set_data(RID p_shape, const Variant& p_data);
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
+
+	virtual ShapeType shape_get_type(RID p_shape) const;
+	virtual Variant shape_get_data(RID p_shape) const;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
+
+	/* SPACE API */
+
+	virtual RID space_create();
+	virtual void space_set_active(RID p_space,bool p_active);
+	virtual bool space_is_active(RID p_space) const;
+
+	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value);
+	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const;
+
+	// this function only works on fixed process, errors and returns null otherwise
+	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space);
+
+
+	/* AREA API */
+
+	virtual RID area_create();
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode);
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const;
+
+	virtual void area_set_space(RID p_area, RID p_space);
+	virtual RID area_get_space(RID p_area) const;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform=Matrix32());
+	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape);
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform);
+
+	virtual int area_get_shape_count(RID p_area) const;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
+	virtual Matrix32 area_get_shape_transform(RID p_area, int p_shape_idx) const;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx);
+	virtual void area_clear_shapes(RID p_area);
+
+	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID);
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const;
+
+	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value);
+	virtual void area_set_transform(RID p_area, const Matrix32& p_transform);
+
+	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
+	virtual Matrix32 area_get_transform(RID p_area) const;
+
+	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
+
+
+	/* BODY API */
+
+	// create a body of a given type
+	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false);
+
+	virtual void body_set_space(RID p_body, RID p_space);
+	virtual RID body_get_space(RID p_body) const;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode);
+	virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32());
+	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape);
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform);
+
+	virtual int body_get_shape_count(RID p_body) const;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
+	virtual Matrix32 body_get_shape_transform(RID p_body, int p_shape_idx) const;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx);
+	virtual void body_clear_shapes(RID p_body);
+
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable);
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const;
+
+	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID);
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable);
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const;
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
+
+	//advanced simulation
+	virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform);
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant);
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
+
+	virtual void body_set_applied_force(RID p_body, const Vector2& p_force);
+	virtual Vector2 body_get_applied_force(RID p_body) const;
+
+	virtual void body_set_applied_torque(RID p_body, float p_torque);
+	virtual float body_get_applied_torque(RID p_body) const;
+
+	virtual void body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse);
+	virtual void body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity);
+
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
+
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold);
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const;
+
+	virtual void body_set_omit_force_integration(RID p_body,bool p_omit);
+	virtual bool body_is_omitting_force_integration(RID p_body) const;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
+	virtual int body_get_max_contacts_reported(RID p_body) const;
+
+	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
+
+	/* JOINT API */
+
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
+	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const;
+
+	virtual RID pin_joint_create(const Vector2& p_pos,RID p_body_a,RID p_body_b=RID());
+	virtual RID groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b);
+	virtual RID damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b=RID());
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value);
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const;
+
+	virtual JointType joint_get_type(RID p_joint) const;
+
+	/* MISC */
+
+	virtual void free(RID p_rid);
+
+	virtual void set_active(bool p_active);
+	virtual void init();
+	virtual void step(float p_step);
+	virtual void sync();
+	virtual void flush_queries();
+	virtual void finish();
+
+	Physics2DServerSW();
+	~Physics2DServerSW();
+
+};
+
+#endif
+
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
new file mode 100644
index 000000000..5ad05a18a
--- /dev/null
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -0,0 +1,1085 @@
+/*************************************************************************/
+/*  shape_2d_sw.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 "shape_2d_sw.h"

+#include "geometry.h"

+#include "sort.h"

+

+#define _SEGMENT_IS_VALID_SUPPORT_TRESHOLD 0.99998

+

+

+void Shape2DSW::configure(const Rect2& p_aabb) {

+	aabb=p_aabb;

+	configured=true;

+	for (Map<ShapeOwner2DSW*,int>::Element *E=owners.front();E;E=E->next()) {

+		ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key();

+		co->_shape_changed();

+	}

+}

+

+

+Vector2 Shape2DSW::get_support(const Vector2& p_normal) const {

+

+	Vector2 res[2];

+	int amnt;

+	get_supports(p_normal,res,amnt);

+	return res[0];

+}

+

+void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {

+

+	Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);

+	if (E) {

+		E->get()++;

+	} else {

+		owners[p_owner]=1;

+	}

+}

+

+void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){

+

+	Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);

+	ERR_FAIL_COND(!E);

+	E->get()--;

+	if (E->get()==0) {

+		owners.erase(E);

+	}

+

+}

+

+bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{

+

+	return owners.has(p_owner);

+

+}

+

+const Map<ShapeOwner2DSW*,int>& Shape2DSW::get_owners() const{

+	return owners;

+}

+

+

+Shape2DSW::Shape2DSW() {

+

+	custom_bias=0;

+	configured=false;

+}

+

+

+Shape2DSW::~Shape2DSW() {

+

+	ERR_FAIL_COND(owners.size());

+}

+

+

+/*********************************************************/

+/*********************************************************/

+/*********************************************************/

+

+

+

+void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {

+

+	r_amount=0;

+}

+

+bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {

+

+	Vector2 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;

+	}

+

+	r_point = p_begin + segment * -dist;

+	r_normal=normal;

+

+	return true;

+}

+

+real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const {

+

+	return 0;

+}

+

+void LineShape2DSW::set_data(const Variant& p_data) {

+

+	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY);

+	Array arr = p_data;

+	ERR_FAIL_COND(arr.size()!=2);

+	normal=arr[0];

+	d=arr[1];

+	configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2)));

+

+}

+

+Variant LineShape2DSW::get_data() const {

+

+	Array arr;

+	arr.resize(2);

+	arr[0]=normal;

+	arr[1]=d;

+	return arr;

+}

+

+/*********************************************************/

+/*********************************************************/

+/*********************************************************/

+

+

+

+void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {

+

+

+	r_amount=1;

+

+	if (p_normal.y>0)

+		*r_supports=Vector2(0,length);

+	else

+		*r_supports=Vector2();

+

+}

+

+bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {

+

+	return false; //rays can't be intersected

+

+}

+

+real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const {

+

+	return 0; //rays are mass-less

+}

+

+void RayShape2DSW::set_data(const Variant& p_data) {

+

+	length=p_data;

+	configure(Rect2(0,0,0.001,length));

+}

+

+Variant RayShape2DSW::get_data() const {

+

+	return length;

+}

+

+

+/*********************************************************/

+/*********************************************************/

+/*********************************************************/

+

+

+

+void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {

+

+	if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)  {

+		r_supports[0]=a;

+		r_supports[1]=b;

+		r_amount=2;

+		return;

+

+	}

+

+	float dp=p_normal.dot(b-a);

+	if (dp>0)

+		*r_supports=b;

+	else

+		*r_supports=a;

+	r_amount=1;

+

+}

+

+bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {

+

+	if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point))

+		return false;

+

+	Vector2 d = p_end-p_begin;

+	if (n.dot(p_begin) > n.dot(a)) {

+		r_normal=n;

+	} else {

+		r_normal=-n;

+	}

+

+	return true;

+}

+

+real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const {

+

+	real_t l = b.distance_to(a);

+	Vector2 ofs = (a+b)*0.5;

+

+	return p_mass*(l*l/12.0f + ofs.length_squared());

+}

+

+void SegmentShape2DSW::set_data(const Variant& p_data) {

+

+	ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2);

+

+	Rect2 r = p_data;

+	a=r.pos;

+	b=r.size;

+	n=(b-a).tangent();

+

+	Rect2 aabb;

+	aabb.pos=a;

+	aabb.expand_to(b);

+	if (aabb.size.x==0)

+		aabb.size.x=0.001;

+	if (aabb.size.y==0)

+		aabb.size.y=0.001;

+	configure(aabb);

+}

+

+Variant SegmentShape2DSW::get_data() const {

+

+	Rect2 r;

+	r.pos=a;

+	r.size=b;

+	return r;

+}

+

+/*********************************************************/

+/*********************************************************/

+/*********************************************************/

+

+

+

+void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {

+

+	r_amount=1;

+	*r_supports=p_normal*radius;

+

+}

+

+bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {

+

+

+	Vector2 line_vec = p_end - p_begin;

+

+	real_t a, b, c;

+

+	a = line_vec.dot(line_vec);

+	b = 2 * p_begin.dot(line_vec);

+	c = p_begin.dot(p_begin) - radius * radius;

+

+	real_t sqrtterm = b*b - 4*a*c;

+

+	if(sqrtterm < 0)

+		return false;

+	sqrtterm = Math::sqrt(sqrtterm);

+	real_t res = ( -b - sqrtterm ) / (2 * a);

+

+	if (res <0 || res >1+CMP_EPSILON) {

+		return false;

+	}

+

+	r_point=p_begin+line_vec*res;

+	r_normal=r_point.normalized();

+	return true;

+}

+

+real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const {

+

+	return radius*radius;

+}

+

+void CircleShape2DSW::set_data(const Variant& p_data) {

+

+	ERR_FAIL_COND(!p_data.is_num());

+	radius=p_data;

+	configure(Rect2(-radius,-radius,radius*2,radius*2));

+}

+

+Variant CircleShape2DSW::get_data() const {

+

+	return radius;

+}

+

+

+/*********************************************************/

+/*********************************************************/

+/*********************************************************/

+

+

+

+void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {

+

+	for(int i=0;i<2;i++) {

+

+		Vector2 ag;

+		ag[i]=1.0;

+		float dp = ag.dot(p_normal);

+		if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)

+			continue;

+

+		float sgn = dp>0 ? 1.0 : -1.0;

+

+		r_amount=2;

+

+		r_supports[0][i]=half_extents[i]*sgn;

+		r_supports[0][i^1]=half_extents[i^1];

+

+		r_supports[1][i]=half_extents[i]*sgn;

+		r_supports[1][i^1]=-half_extents[i^1];

+

+		return;

+

+

+	}

+

+	/* USE POINT */

+

+	r_amount=1;

+	r_supports[0]=Vector2(

+		(p_normal.x<0) ? -half_extents.x : half_extents.x,

+		(p_normal.y<0) ? -half_extents.y : half_extents.y

+	);

+

+}

+

+bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {

+

+

+	return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);

+}

+

+real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const {

+

+	Vector2 he2=half_extents*2;

+	return p_mass*he2.dot(he2)/12.0f;

+}

+

+void RectangleShape2DSW::set_data(const Variant& p_data) {

+

+	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2);

+

+	half_extents=p_data;

+	configure(Rect2(-half_extents,half_extents*2.0));

+}

+

+Variant RectangleShape2DSW::get_data() const {

+

+	return half_extents;

+}

+

+

+

+/*********************************************************/

+/*********************************************************/

+/*********************************************************/

+

+

+

+void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {

+

+	Vector2 n=p_normal;

+

+	float d = n.y;

+

+	if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {

+

+		// make it flat

+		n.y=0.0;

+		n.normalize();

+		n*=radius;

+

+		r_amount=2;

+		r_supports[0]=n;

+		r_supports[0].y+=height*0.5;

+		r_supports[1]=n;

+		r_supports[1].y-=height*0.5;

+

+	} else {

+

+		float h = (d > 0) ? height : -height;

+

+		n*=radius;

+		n.y += h*0.5;

+		r_amount=1;

+		*r_supports=n;

+

+	}

+}

+

+bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {

+

+

+	float d = 1e10;

+	Vector2 n = (p_end-p_begin).normalized();

+	bool collided=false;

+

+	//try spheres

+	for(int i=0;i<2;i++) {

+

+		Vector2 begin = p_begin;

+		Vector2 end = p_end;

+		float ofs = (i==0)?-height*0.5:height*0.5;

+		begin.y+=ofs;

+		end.y+=ofs;

+

+		Vector2 line_vec = end - begin;

+

+		real_t a, b, c;

+

+		a = line_vec.dot(line_vec);

+		b = 2 * begin.dot(line_vec);

+		c = begin.dot(begin) - radius * radius;

+

+		real_t sqrtterm = b*b - 4*a*c;

+

+		if(sqrtterm < 0)

+			continue;

+

+		sqrtterm = Math::sqrt(sqrtterm);

+		real_t res = ( -b - sqrtterm ) / (2 * a);

+

+		if (res <0 || res >1+CMP_EPSILON) {

+			continue;

+		}

+

+		Vector2 point = begin+line_vec*res;

+		Vector2 pointf(point.x,point.y-ofs);

+		real_t pd = n.dot(pointf);

+		if (pd<d) {

+			r_point=pointf;

+			r_normal=point.normalized();

+			d=pd;

+			collided=true;

+		}

+	}

+

+

+	Vector2 rpos,rnorm;

+	if (Rect2( Point2(-radius,-height*0.5), Size2(radius*2.0,height) ).intersects_segment(p_begin,p_end,&rpos,&rnorm)) {

+

+		real_t pd = n.dot(rpos);

+		if (pd<d) {

+			r_point=rpos;

+			r_normal=rnorm;

+			d=pd;

+			collided=true;

+		}

+	}

+

+	//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);

+	return collided; //todo

+}

+

+real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass) const {

+

+	Vector2 he2(radius*2,height+radius*2);

+	return p_mass*he2.dot(he2)/12.0f;

+}

+

+void CapsuleShape2DSW::set_data(const Variant& p_data) {

+

+	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2);

+

+	if (p_data.get_type()==Variant::ARRAY) {

+		Array arr=p_data;

+		ERR_FAIL_COND(arr.size()!=2);

+		height=arr[0];

+		radius=arr[1];

+	} else {

+

+		Point2 p = p_data;

+		radius=p.x;

+		height=p.y;

+	}

+

+	Point2 he(radius,height*0.5+radius);

+	configure(Rect2(-he,he*2));

+

+}

+

+Variant CapsuleShape2DSW::get_data() const {

+

+	return Point2(height,radius);

+}

+

+

+

+/*********************************************************/

+/*********************************************************/

+/*********************************************************/

+

+

+

+void ConvexPolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {

+

+	int support_idx=-1;

+	real_t d=-1e10;

+

+	for(int i=0;i<point_count;i++) {

+

+		//test point

+		real_t ld = p_normal.dot(points[i].pos);

+		if (ld>d) {

+			support_idx=i;

+			d=ld;

+		}

+

+		//test segment

+		if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {

+

+			r_amount=2;

+			r_supports[0]=points[i].pos;

+			r_supports[1]=points[(i+1)%point_count].pos;

+			return;

+		}

+	}

+

+	ERR_FAIL_COND(support_idx==-1);

+

+	r_amount=1;

+	r_supports[0]=points[support_idx].pos;

+

+}

+

+bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {

+

+	Vector2 n = (p_end-p_begin).normalized();

+	real_t d=1e10;

+	bool inters=false;

+

+	for(int i=0;i<point_count;i++) {

+

+		//hmm crap.. no can do..

+		//if (d.dot(points[i].normal)>=0)

+		//	continue;

+

+

+		Vector2 res;

+

+		if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res))

+			continue;

+

+		float nd = n.dot(res);

+		if (nd<d) {

+

+			d=nd;

+			r_point=res;

+			r_normal=points[i].normal;

+			inters=true;

+		}

+

+	}

+

+

+	if (inters) {

+

+		if (n.dot(r_normal)>0)

+			r_normal=-r_normal;

+	}

+

+	//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);

+	return inters; //todo

+}

+

+real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const {

+

+	Rect2 aabb;

+	aabb.pos=points[0].pos;

+	for(int i=0;i<point_count;i++) {

+

+		aabb.expand_to(points[i].pos);

+	}

+

+	return p_mass*aabb.size.dot(aabb.size)/12.0f;

+}

+

+void ConvexPolygonShape2DSW::set_data(const Variant& p_data) {

+

+	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);

+

+

+	if (points)

+		memdelete_arr(points);

+	points=NULL;

+	point_count=0;

+

+	if (p_data.get_type()==Variant::VECTOR2_ARRAY) {

+		DVector<Vector2> arr=p_data;

+		ERR_FAIL_COND(arr.size()==0);

+		point_count=arr.size();

+		points = memnew_arr(Point,point_count);

+		DVector<Vector2>::Read r = arr.read();

+

+		for(int i=0;i<point_count;i++) {

+			points[i].pos=r[i];

+		}

+

+		for(int i=0;i<point_count;i++) {

+

+			Vector2 p = points[i].pos;

+			Vector2 pn = points[(i+1)%point_count].pos;

+			points[i].normal=(pn-p).tangent().normalized();

+		}

+	} else {

+

+		DVector<real_t> dvr = p_data;

+		point_count=dvr.size()/4;

+		ERR_FAIL_COND(point_count==0);

+

+		points = memnew_arr(Point,point_count);

+		DVector<real_t>::Read r = dvr.read();

+

+		for(int i=0;i<point_count;i++) {

+

+			int idx=i<<2;

+			points[i].pos.x=r[idx+0];

+			points[i].pos.y=r[idx+1];

+			points[i].normal.x=r[idx+2];

+			points[i].normal.y=r[idx+3];

+

+		}

+	}

+

+

+	ERR_FAIL_COND(point_count==0);

+	Rect2 aabb;

+	aabb.pos=points[0].pos;

+	for(int i=1;i<point_count;i++)

+		aabb.expand_to(points[i].pos);

+

+	configure(aabb);

+}

+

+Variant ConvexPolygonShape2DSW::get_data() const {

+

+	DVector<Vector2> dvr;

+

+	dvr.resize(point_count);

+

+	for(int i=0;i<point_count;i++) {	

+		dvr.set(i,points[i].pos);

+	}

+

+	return dvr;

+}

+

+

+ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {

+

+	points=NULL;

+	point_count=0;

+

+}

+

+ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW(){

+

+	if (points)

+		memdelete_arr(points);

+

+}

+

+//////////////////////////////////////////////////

+

+

+void ConcavePolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {

+

+	real_t d=-1e10;

+	int idx=-1;

+	for(int i=0;i<points.size();i++) {

+

+		real_t ld = p_normal.dot(points[i]);

+		if (ld>d) {

+			d=ld;

+			idx=i;

+		}

+	}

+

+

+	r_amount=1;

+	ERR_FAIL_COND(idx==-1);

+	*r_supports=points[idx];

+

+}

+

+bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{

+

+	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_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,

+

+

+	};

+

+	Vector2 n = (p_end-p_begin).normalized();

+	real_t d=1e10;

+	bool inters=false;

+

+	//for(int i=0;i<bvh_depth;i++)

+	//	stack[i]=0;

+

+	int level=0;

+

+	const Segment *segmentptr=&segments[0];

+	const Vector2 *pointptr=&points[0];

+	const BVH *bvhptr = &bvh[0];

+	int pos=bvh.size()-1;

+

+

+	stack[0]=0;

+	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);

+				if (!valid) {

+

+					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;

+

+				} else {

+

+					if (b.left<0) {

+

+						const Segment &s=segmentptr[ b.right ];

+						Vector2 a = pointptr[ s.points[0] ];

+						Vector2 b = pointptr[ s.points[1] ];

+

+

+						Vector2 res;

+

+						if (Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&res)) {

+

+							float nd = n.dot(res);

+							if (nd<d) {

+

+								d=nd;

+								r_point=res;

+								r_normal=(b-a).tangent().normalized();

+								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;

+

+}

+

+

+

+int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) {

+

+	if (p_len==1) {

+

+		bvh_depth=MAX(p_depth,bvh_depth);

+		bvh.push_back(*p_bvh);

+		return bvh.size()-1;

+	}

+

+	//else sort best

+

+	Rect2 global_aabb=p_bvh[0].aabb;

+	for(int i=1;i<p_len;i++) {

+		global_aabb=global_aabb.merge(p_bvh[i].aabb);

+	}

+

+	if (global_aabb.size.x > global_aabb.size.y) {

+

+		SortArray<BVH,BVH_CompareX> sort;

+		sort.sort(p_bvh,p_len);

+

+	} else {

+

+		SortArray<BVH,BVH_CompareY> sort;

+		sort.sort(p_bvh,p_len);

+

+	}

+

+	int median = p_len/2;

+

+

+	BVH node;

+	node.aabb=global_aabb;

+	int node_idx = bvh.size();

+	bvh.push_back(node);

+

+	int l = _generate_bvh(p_bvh,median,p_depth+1);

+	int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1);

+	bvh[node_idx].left=l;

+	bvh[node_idx].right=r;

+

+	return node_idx;

+

+}

+

+void ConcavePolygonShape2DSW::set_data(const Variant& p_data) {

+

+	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);

+

+	segments.clear();;

+	points.clear();;

+	bvh.clear();;

+	bvh_depth=1;

+

+	Rect2 aabb;

+

+	if (p_data.get_type()==Variant::VECTOR2_ARRAY) {

+

+		DVector<Vector2> p2arr = p_data;

+		int len = p2arr.size();

+		DVector<Vector2>::Read arr = p2arr.read();

+

+

+		Map<Point2,int> pointmap;

+		for(int i=0;i<len;i+=2) {

+

+			Point2 p1 =arr[i];

+			Point2 p2 =arr[i+1];

+			int idx_p1,idx_p2;

+			if (p1==p2)

+				continue; //don't want it

+

+			if (pointmap.has(p1)) {

+				idx_p1=pointmap[p1];

+			} else {

+				idx_p1=pointmap.size();

+				pointmap[p1]=idx_p1;

+			}

+

+			if (pointmap.has(p2)) {

+				idx_p2=pointmap[p2];

+			} else {

+				idx_p2=pointmap.size();

+				pointmap[p2]=idx_p2;

+			}

+

+			Segment s;

+			s.points[0]=idx_p1;

+			s.points[1]=idx_p2;

+			segments.push_back(s);

+		}

+

+		points.resize(pointmap.size());

+		aabb.pos=pointmap.front()->key();

+		for(Map<Point2,int>::Element *E=pointmap.front();E;E=E->next()) {

+

+			aabb.expand_to(E->key());

+			points[E->get()]=E->key();

+		}

+

+		Vector<BVH> main_vbh;

+		main_vbh.resize(segments.size());

+		for(int i=0;i<main_vbh.size();i++) {

+

+

+			main_vbh[i].aabb.pos=points[segments[i].points[0]];

+			main_vbh[i].aabb.expand_to(points[segments[i].points[1]]);

+			main_vbh[i].left=-1;

+			main_vbh[i].right=i;

+		}

+

+		_generate_bvh(&main_vbh[0],main_vbh.size(),1);

+

+

+	} else {

+		//dictionary with arrays

+

+	}

+

+

+	configure(aabb);

+}

+Variant ConcavePolygonShape2DSW::get_data() const {

+

+

+	DVector<Vector2> rsegments;

+	int len = segments.size();

+	rsegments.resize(len*2);

+	DVector<Vector2>::Write w = rsegments.write();

+	for(int i=0;i<len;i++) {

+

+		w[(i<<1)+0]=points[segments[i].points[0]];

+		w[(i<<1)+1]=points[segments[i].points[1]];

+	}

+

+	w=DVector<Vector2>::Write();

+

+	return rsegments;

+}

+

+void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const {

+

+	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_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,

+

+

+	};

+

+	//for(int i=0;i<bvh_depth;i++)

+	//	stack[i]=0;

+

+

+	int level=0;

+

+	const Segment *segmentptr=&segments[0];

+	const Vector2 *pointptr=&points[0];

+	const BVH *bvhptr = &bvh[0];

+	int pos=bvh.size()-1;

+

+

+	stack[0]=0;

+	while(true) {

+

+		uint32_t node = stack[level]&NODE_IDX_MASK;

+		const BVH &b = bvhptr[ node ];

+

+		switch(stack[level]>>VISITED_BIT_SHIFT) {

+			case TEST_AABB_BIT: {

+

+

+				bool valid = p_local_aabb.intersects(b.aabb);

+				if (!valid) {

+

+					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;

+

+				} else {

+

+					if (b.left<0) {

+

+						const Segment &s=segmentptr[ b.right ];

+						Vector2 a = pointptr[ s.points[0] ];

+						Vector2 b = pointptr[ s.points[1] ];

+

+						SegmentShape2DSW ss(a,b,(b-a).tangent().normalized());

+

+						p_callback(p_userdata,&ss);

+						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)

+					return;

+				else

+					level--;

+

+			} continue;

+		}

+	}

+

+}

+

+

diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
new file mode 100644
index 000000000..20e76fad5
--- /dev/null
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -0,0 +1,442 @@
+/*************************************************************************/
+/*  shape_2d_sw.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 SHAPE_2D_2DSW_H

+#define SHAPE_2D_2DSW_H

+

+#include "servers/physics_2d_server.h"

+/*

+

+SHAPE_LINE, ///< plane:"plane"

+SHAPE_SEGMENT, ///< float:"length"

+SHAPE_CIRCLE, ///< float:"radius"

+SHAPE_RECTANGLE, ///< vec3:"extents"

+SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"

+SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array)

+SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error

+

+*/

+

+class Shape2DSW;

+

+class ShapeOwner2DSW {

+public:

+

+	virtual void _shape_changed()=0;

+	virtual void remove_shape(Shape2DSW *p_shape)=0;

+

+	virtual ~ShapeOwner2DSW() {}

+};

+

+class Shape2DSW {

+

+	RID self;

+	Rect2 aabb;

+	bool configured;

+	real_t custom_bias;

+

+	Map<ShapeOwner2DSW*,int> owners;

+protected:

+

+	void configure(const Rect2& p_aabb);

+public:

+

+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }

+	_FORCE_INLINE_ RID get_self() const {return  self; }

+

+	virtual Physics2DServer::ShapeType get_type() const=0;

+

+	_FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }

+	_FORCE_INLINE_ bool is_configured() const { return configured; }

+

+	virtual bool is_concave() const { return false; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;

+	virtual Vector2 get_support(const Vector2& p_normal) const;

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0;

+	virtual real_t get_moment_of_inertia(float p_mass) const=0;

+

+	virtual void set_data(const Variant& p_data)=0;

+	virtual Variant get_data() const=0;

+

+	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }

+	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }

+

+	void add_owner(ShapeOwner2DSW *p_owner);

+	void remove_owner(ShapeOwner2DSW *p_owner);

+	bool is_owner(ShapeOwner2DSW *p_owner) const;

+	const Map<ShapeOwner2DSW*,int>& get_owners() const;

+

+	Shape2DSW();

+	virtual ~Shape2DSW();

+};

+

+

+class LineShape2DSW : public Shape2DSW {

+

+

+	Vector2 normal;

+	real_t d;

+

+public:

+

+	_FORCE_INLINE_ Vector2 get_normal() const { return normal; }

+	_FORCE_INLINE_ real_t get_d() const { return d; }

+

+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_LINE; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;

+	virtual real_t get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {

+		//real large

+		r_min=-1e10;

+		r_max=1e10;

+	}

+

+};

+

+

+class RayShape2DSW : public Shape2DSW {

+

+

+	real_t length;

+

+public:

+

+

+	_FORCE_INLINE_  real_t get_length() const { return length; }

+

+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;

+	virtual real_t get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {

+		//real large

+		r_max = p_normal.dot(p_transform.get_origin());

+		r_min = p_normal.dot(p_transform.xform(Vector2(0,length)));

+		if (r_max<r_min) {

+

+			SWAP(r_max,r_min);

+		}

+	}

+

+	_FORCE_INLINE_ RayShape2DSW() {}

+	_FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; }

+};

+

+

+class SegmentShape2DSW : public Shape2DSW {

+

+

+	Vector2 a;

+	Vector2 b;

+	Vector2 n;

+

+public:

+

+

+	_FORCE_INLINE_ const Vector2& get_a() const { return a; }

+	_FORCE_INLINE_ const Vector2& get_b() const { return b; }

+	_FORCE_INLINE_ const Vector2& get_normal() const { return n; }

+

+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_SEGMENT; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;

+	virtual real_t get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {

+		//real large

+		r_max = p_normal.dot(p_transform.xform(a));

+		r_min = p_normal.dot(p_transform.xform(b));

+		if (r_max<r_min) {

+

+			SWAP(r_max,r_min);

+		}

+	}

+

+	_FORCE_INLINE_ SegmentShape2DSW() {}

+	_FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; }

+};

+

+

+class CircleShape2DSW : public Shape2DSW {

+

+

+	real_t radius;

+

+public:

+

+	_FORCE_INLINE_ const real_t& get_radius() const { return radius; }

+

+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CIRCLE; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;

+	virtual real_t get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {

+		//real large

+		real_t d = p_normal.dot( p_transform.get_origin() );

+

+		// figure out scale at point

+		Vector2 local_normal = p_transform.basis_xform_inv(p_normal);

+		real_t scale = local_normal.length();

+

+		r_min = d - (radius) * scale;

+		r_max = d + (radius) * scale;

+	}

+

+};

+

+

+

+class RectangleShape2DSW : public Shape2DSW {

+

+

+	Vector2 half_extents;

+

+public:

+

+	_FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; }

+

+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RECTANGLE; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;

+	virtual real_t get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {

+		// no matter the angle, the box is mirrored anyway

+		Vector2 local_normal=p_transform.basis_xform_inv(p_normal);

+

+		float length = local_normal.abs().dot(half_extents);

+		float distance = p_normal.dot( p_transform.get_origin() );

+

+		r_min = distance - length;

+		r_max = distance + length;

+	}

+

+};

+

+class CapsuleShape2DSW : public Shape2DSW {

+

+

+	real_t radius;

+	real_t height;

+

+public:

+

+	_FORCE_INLINE_ const real_t& get_radius() const { return radius; }

+	_FORCE_INLINE_ const real_t& get_height() const { return height; }

+

+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CAPSULE; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;

+	virtual real_t get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {

+		// no matter the angle, the box is mirrored anyway

+		Vector2 n=p_transform.basis_xform_inv(p_normal).normalized();

+		float h = (n.y > 0) ? height : -height;

+

+		n *= radius;

+		n.y += h * 0.5;

+

+		r_max = p_normal.dot(p_transform.xform(n));

+		r_min = p_normal.dot(p_transform.xform(-n));

+

+		if (r_max<r_min) {

+

+			SWAP(r_max,r_min);

+		}

+

+		//ERR_FAIL_COND( r_max < r_min );

+	}

+

+};

+

+

+class ConvexPolygonShape2DSW : public Shape2DSW {

+

+

+	struct Point {

+

+		Vector2 pos;

+		Vector2 normal; //normal to next segment

+	};

+

+	Point *points;

+	int point_count;

+

+public:

+

+	_FORCE_INLINE_ int get_point_count() const { return point_count; }

+	_FORCE_INLINE_ const Vector2& get_point(int p_idx) const { return points[p_idx].pos; }

+	_FORCE_INLINE_ const Vector2& get_segment_normal(int p_idx) const { return points[p_idx].normal; }

+

+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;

+	virtual real_t get_moment_of_inertia(float p_mass) const;

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {

+		// no matter the angle, the box is mirrored anyway

+

+		r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));

+		for(int i=1;i<point_count;i++) {

+

+			float d = p_normal.dot(p_transform.xform(points[i].pos));

+			if (d>r_max)

+				r_max=d;

+			if (d<r_min)

+				r_min=d;

+

+		}

+

+	}

+

+

+	ConvexPolygonShape2DSW();

+	~ConvexPolygonShape2DSW();

+};

+

+

+class ConcaveShape2DSW : public Shape2DSW {

+

+public:

+

+	virtual bool is_concave() const { return true; }

+	typedef void (*Callback)(void* p_userdata,Shape2DSW *p_convex);

+

+	virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const=0;

+

+};

+

+class ConcavePolygonShape2DSW : public ConcaveShape2DSW {

+

+	struct Segment {

+

+		int points[2];

+	};

+

+	Vector<Segment> segments;

+	Vector<Point2> points;

+

+	struct BVH {

+

+		Rect2 aabb;

+		int left,right;

+	};

+

+

+	Vector<BVH> bvh;

+	int bvh_depth;

+

+

+	struct BVH_CompareX {

+

+		_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {

+

+			return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5);

+		}

+	};

+

+	struct BVH_CompareY {

+

+		_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {

+

+			return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5);

+		}

+	};

+

+	int _generate_bvh(BVH *p_bvh,int p_len,int p_depth);

+

+public:

+

+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; }

+

+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }

+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;

+

+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;

+

+	virtual real_t get_moment_of_inertia(float p_mass) const { return 0; }

+

+	virtual void set_data(const Variant& p_data);

+	virtual Variant get_data() const;

+

+	virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const;

+

+};

+

+

+#endif // SHAPE_2D_2DSW_H

diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
new file mode 100644
index 000000000..4fe53aeb4
--- /dev/null
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -0,0 +1,432 @@
+/*************************************************************************/
+/*  space_2d_sw.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 "space_2d_sw.h"

+#include "collision_solver_2d_sw.h"

+#include "physics_2d_server_sw.h"

+

+

+bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {

+

+

+	ERR_FAIL_COND_V(space->locked,false);

+

+	Vector2 begin,end;

+	Vector2 normal;

+	begin=p_from;

+	end=p_to;

+	normal=(end-begin).normalized();

+

+

+	int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);

+

+	//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision

+

+	bool collided=false;

+	Vector2 res_point,res_normal;

+	int res_shape;

+	const CollisionObject2DSW *res_obj;

+	real_t min_d=1e10;

+

+

+	for(int i=0;i<amount;i++) {

+

+		if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)

+			continue; //ignore area

+

+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))

+			continue;

+

+		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];

+

+		int shape_idx=space->intersection_query_subindex_results[i];

+		Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();

+

+		Vector2 local_from = inv_xform.xform(begin);

+		Vector2 local_to = inv_xform.xform(end);

+

+		/*local_from = col_obj->get_inv_transform().xform(begin);

+		local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from);

+

+		local_to = col_obj->get_inv_transform().xform(end);

+		local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/

+

+		const Shape2DSW *shape = col_obj->get_shape(shape_idx);

+

+		Vector2 shape_point,shape_normal;

+

+

+		if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {

+

+

+			//print_line("inters sgment!");

+			Matrix32 xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);

+			shape_point=xform.xform(shape_point);

+

+			real_t ld = normal.dot(shape_point);

+

+

+			if (ld<min_d) {

+

+				min_d=ld;

+				res_point=shape_point;

+				res_normal=inv_xform.basis_xform_inv(shape_normal).normalized();

+				res_shape=shape_idx;

+				res_obj=col_obj;

+				collided=true;

+			}

+		}

+

+	}

+

+	if (!collided)

+		return false;

+

+

+	r_result.collider_id=res_obj->get_instance_id();

+	if (r_result.collider_id!=0)

+		r_result.collider=ObjectDB::get_instance(r_result.collider_id);

+	r_result.normal=res_normal;

+	r_result.position=res_point;

+	r_result.rid=res_obj->get_self();

+	r_result.shape=res_shape;

+

+	return true;

+

+}

+

+

+int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {

+

+	if (p_result_max<=0)

+		return 0;

+

+	Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);

+	ERR_FAIL_COND_V(!shape,0);

+

+	Rect2 aabb = p_xform.xform(shape->get_aabb());

+

+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);

+

+	bool collided=false;

+	int cc=0;

+

+	for(int i=0;i<amount;i++) {

+

+		if (cc>=p_result_max)

+			break;

+

+		if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)

+			continue; //ignore area

+

+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))

+			continue;

+

+

+		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];

+		int shape_idx=space->intersection_query_subindex_results[i];

+

+		if (!CollisionSolver2DSW::solve_static(shape,p_xform,p_xform.affine_inverse(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), col_obj->get_inv_transform() * col_obj->get_shape_inv_transform(shape_idx),NULL,NULL,NULL))

+			continue;

+

+		r_results[cc].collider_id=col_obj->get_instance_id();

+		if (r_results[cc].collider_id!=0)

+			r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);

+		r_results[cc].rid=col_obj->get_self();

+		r_results[cc].shape=shape_idx;

+

+		cc++;

+

+	}

+

+	return cc;

+

+}

+

+Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {

+

+

+	space=NULL;

+}

+

+

+////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+

+

+

+

+

+

+

+

+

+void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self) {

+

+	CollisionObject2DSW::Type type_A=A->get_type();

+	CollisionObject2DSW::Type type_B=B->get_type();

+	if (type_A>type_B) {

+

+		SWAP(A,B);

+		SWAP(p_subindex_A,p_subindex_B);

+		SWAP(type_A,type_B);

+	}

+

+	Space2DSW *self = (Space2DSW*)p_self;

+

+	if (type_A==CollisionObject2DSW::TYPE_AREA) {

+

+

+		ERR_FAIL_COND_V(type_B!=CollisionObject2DSW::TYPE_BODY,NULL);

+		Area2DSW *area=static_cast<Area2DSW*>(A);

+		Body2DSW *body=static_cast<Body2DSW*>(B);

+

+		AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) );

+

+		return area_pair;

+	} else {

+

+

+		BodyPair2DSW *b = memnew( BodyPair2DSW((Body2DSW*)A,p_subindex_A,(Body2DSW*)B,p_subindex_B) );

+		return b;

+

+	}

+

+	return NULL;

+}

+

+void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) {

+

+

+

+	Space2DSW *self = (Space2DSW*)p_self;

+	Constraint2DSW *c = (Constraint2DSW*)p_data;

+	memdelete(c);

+}

+

+

+const SelfList<Body2DSW>::List& Space2DSW::get_active_body_list() const {

+

+	return active_list;

+}

+void Space2DSW::body_add_to_active_list(SelfList<Body2DSW>* p_body) {

+

+	active_list.add(p_body);

+}

+void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW>* p_body) {

+

+	active_list.remove(p_body);

+

+}

+

+void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body) {

+

+

+	inertia_update_list.add(p_body);

+}

+

+void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body) {

+

+	inertia_update_list.remove(p_body);

+}

+

+BroadPhase2DSW *Space2DSW::get_broadphase() {

+

+	return broadphase;

+}

+

+void Space2DSW::add_object(CollisionObject2DSW *p_object) {

+

+	ERR_FAIL_COND( objects.has(p_object) );

+	objects.insert(p_object);

+}

+

+void Space2DSW::remove_object(CollisionObject2DSW *p_object) {

+

+	ERR_FAIL_COND( !objects.has(p_object) );

+	objects.erase(p_object);

+}

+

+const Set<CollisionObject2DSW*> &Space2DSW::get_objects() const {

+

+	return objects;

+}

+

+void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW>* p_body) {

+

+	state_query_list.add(p_body);

+}

+void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW>* p_body) {

+

+	state_query_list.remove(p_body);

+}

+

+void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area) {

+

+	monitor_query_list.add(p_area);

+}

+void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area) {

+

+	monitor_query_list.remove(p_area);

+}

+

+void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW>* p_area) {

+

+	area_moved_list.add(p_area);

+}

+

+void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW>* p_area) {

+

+	area_moved_list.remove(p_area);

+}

+

+const SelfList<Area2DSW>::List& Space2DSW::get_moved_area_list() const {

+

+	return area_moved_list;

+}

+

+

+void Space2DSW::call_queries() {

+

+	while(state_query_list.first()) {

+

+		Body2DSW * b = state_query_list.first()->self();

+		b->call_queries();

+		state_query_list.remove(state_query_list.first());

+	}

+

+	while(monitor_query_list.first()) {

+

+		Area2DSW * a = monitor_query_list.first()->self();

+		a->call_queries();

+		monitor_query_list.remove(monitor_query_list.first());

+	}

+

+}

+

+void Space2DSW::setup() {

+

+

+	while(inertia_update_list.first()) {

+		inertia_update_list.first()->self()->update_inertias();

+		inertia_update_list.remove(inertia_update_list.first());

+	}

+

+

+}

+

+void Space2DSW::update() {

+

+	broadphase->update();

+

+}

+

+

+void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_value) {

+

+	switch(p_param) {

+

+		case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;

+		case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;

+		case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;

+		case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break;

+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break;

+		case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;

+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break;

+		case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;

+	}

+}

+

+real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {

+

+	switch(p_param) {

+

+		case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;

+		case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;

+		case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration;

+		case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold;

+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold;

+		case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;

+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;

+		case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;

+	}

+	return 0;

+}

+

+void Space2DSW::lock() {

+

+	locked=true;

+}

+

+void Space2DSW::unlock() {

+

+	locked=false;

+}

+

+bool Space2DSW::is_locked() const {

+

+	return locked;

+}

+

+Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() {

+

+	return direct_access;

+}

+

+Space2DSW::Space2DSW() {

+

+

+	locked=false;

+	contact_recycle_radius=0.01;

+	contact_max_separation=0.05;

+	contact_max_allowed_penetration= 0.01;

+

+	constraint_bias = 0.01;

+	body_linear_velocity_sleep_treshold=0.01;

+	body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI);

+	body_time_to_sleep=0.5;

+	body_angular_velocity_damp_ratio=15;

+

+

+	broadphase = BroadPhase2DSW::create_func();

+	broadphase->set_pair_callback(_broadphase_pair,this);

+	broadphase->set_unpair_callback(_broadphase_unpair,this);

+	area=NULL;

+

+	direct_access = memnew( Physics2DDirectSpaceStateSW );

+	direct_access->space=this;

+}

+

+Space2DSW::~Space2DSW() {

+

+	memdelete(broadphase);

+	memdelete( direct_access );

+}

+

+

+

diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
new file mode 100644
index 000000000..f65ec1442
--- /dev/null
+++ b/servers/physics_2d/space_2d_sw.h
@@ -0,0 +1,160 @@
+/*************************************************************************/
+/*  space_2d_sw.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 SPACE_2D_SW_H

+#define SPACE_2D_SW_H

+

+#include "typedefs.h"

+#include "hash_map.h"

+#include "body_2d_sw.h"

+#include "area_2d_sw.h"

+#include "body_pair_2d_sw.h"

+#include "area_pair_2d_sw.h"

+#include "broad_phase_2d_sw.h"

+#include "collision_object_2d_sw.h"

+

+

+class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState {

+

+	OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState );

+public:

+

+	Space2DSW *space;

+

+	bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);

+	int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);

+

+	Physics2DDirectSpaceStateSW();

+};

+

+

+

+class Space2DSW {

+

+

+	Physics2DDirectSpaceStateSW *direct_access;

+	RID self;

+

+	BroadPhase2DSW *broadphase;

+	SelfList<Body2DSW>::List active_list;

+	SelfList<Body2DSW>::List inertia_update_list;

+	SelfList<Body2DSW>::List state_query_list;

+	SelfList<Area2DSW>::List monitor_query_list;

+	SelfList<Area2DSW>::List area_moved_list;

+

+	static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self);

+	static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self);

+

+	Set<CollisionObject2DSW*> objects;

+

+	Area2DSW *area;

+

+	real_t contact_recycle_radius;

+	real_t contact_max_separation;

+	real_t contact_max_allowed_penetration;

+	real_t constraint_bias;

+

+	enum {

+

+		INTERSECTION_QUERY_MAX=2048

+	};

+

+	CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX];

+	int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];

+

+	float body_linear_velocity_sleep_treshold;

+	float body_angular_velocity_sleep_treshold;

+	float body_time_to_sleep;

+	float body_angular_velocity_damp_ratio;

+

+	bool locked;

+

+friend class Physics2DDirectSpaceStateSW;

+

+public:

+

+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }

+	_FORCE_INLINE_ RID get_self() const { return self; }

+

+	void set_default_area(Area2DSW *p_area) { area=p_area; }

+	Area2DSW *get_default_area() const { return area; }

+

+	const SelfList<Body2DSW>::List& get_active_body_list() const;

+	void body_add_to_active_list(SelfList<Body2DSW>* p_body);

+	void body_remove_from_active_list(SelfList<Body2DSW>* p_body);

+	void body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body);

+	void body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body);

+	void area_add_to_moved_list(SelfList<Area2DSW>* p_area);

+	void area_remove_from_moved_list(SelfList<Area2DSW>* p_area);

+	const SelfList<Area2DSW>::List& get_moved_area_list() const;

+

+

+

+

+	void body_add_to_state_query_list(SelfList<Body2DSW>* p_body);

+	void body_remove_from_state_query_list(SelfList<Body2DSW>* p_body);

+

+	void area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area);

+	void area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area);

+

+	BroadPhase2DSW *get_broadphase();

+

+	void add_object(CollisionObject2DSW *p_object);

+	void remove_object(CollisionObject2DSW *p_object);

+	const Set<CollisionObject2DSW*> &get_objects() const;

+

+	_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }

+	_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }

+	_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }

+	_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }

+	_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; }

+	_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }

+	_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }

+	_FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }

+

+

+	void update();

+	void setup();

+	void call_queries();

+

+

+	bool is_locked() const;

+	void lock();

+	void unlock();

+

+	void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value);

+	real_t get_param(Physics2DServer::SpaceParameter p_param) const;

+

+	Physics2DDirectSpaceStateSW *get_direct_state();

+

+	Space2DSW();

+	~Space2DSW();

+};

+

+

+#endif // SPACE_2D_SW_H

diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
new file mode 100644
index 000000000..9f41fc94e
--- /dev/null
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -0,0 +1,239 @@
+/*************************************************************************/
+/*  step_2d_sw.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 "step_2d_sw.h"
+
+
+void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island) {
+
+	p_body->set_island_step(_step);
+	p_body->set_island_next(*p_island);
+	*p_island=p_body;
+
+	for(Map<Constraint2DSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) {		
+
+		Constraint2DSW *c=(Constraint2DSW*)E->key();
+		if (c->get_island_step()==_step)
+			continue; //already processed
+		c->set_island_step(_step);
+		c->set_island_next(*p_constraint_island);
+		*p_constraint_island=c;
+
+
+		for(int i=0;i<c->get_body_count();i++) {
+			if (i==E->get())
+				continue;
+			Body2DSW *b = c->get_body_ptr()[i];
+			if (b->get_island_step()==_step || b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
+				continue; //no go
+			_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
+		}
+	}
+}
+
+void Step2DSW::_setup_island(Constraint2DSW *p_island,float p_delta) {
+
+	Constraint2DSW *ci=p_island;
+	while(ci) {
+		bool process = ci->setup(p_delta);
+		//todo remove from island if process fails
+		ci=ci->get_island_next();
+	}
+}
+
+void Step2DSW::_solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta){
+
+
+	for(int i=0;i<p_iterations;i++) {
+
+		Constraint2DSW *ci=p_island;
+		while(ci) {
+			ci->solve(p_delta);
+			ci=ci->get_island_next();
+		}
+	}
+}
+
+void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) {
+
+
+	bool can_sleep=true;
+
+	Body2DSW *b = p_island;
+	while(b) {
+
+		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
+			continue; //ignore for static
+
+		if (!b->sleep_test(p_delta))
+			can_sleep=false;
+
+		b=b->get_island_next();
+	}
+
+	//put all to sleep or wake up everyoen
+
+	b = p_island;
+	while(b) {
+
+		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
+			continue; //ignore for static
+
+		bool active = b->is_active();
+
+		if (active==can_sleep)
+			b->set_active(!can_sleep);
+
+		b=b->get_island_next();
+	}
+}
+
+void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
+
+
+	p_space->lock(); // can't access space during this
+
+	p_space->setup(); //update inertias, etc
+
+	const SelfList<Body2DSW>::List * body_list = &p_space->get_active_body_list();
+
+	/* INTEGRATE FORCES */
+	int active_count=0;
+
+	const SelfList<Body2DSW>*b = body_list->first();
+	while(b) {
+
+		b->self()->integrate_forces(p_delta);
+		b=b->next();
+		active_count++;
+	}
+
+	/* GENERATE CONSTRAINT ISLANDS */
+
+	Body2DSW *island_list=NULL;
+	Constraint2DSW *constraint_island_list=NULL;
+	b = body_list->first();
+
+	int island_count=0;
+
+	while(b) {
+		Body2DSW *body = b->self();
+
+
+		if (body->get_island_step()!=_step) {
+
+			Body2DSW *island=NULL;
+			Constraint2DSW *constraint_island=NULL;
+			_populate_island(body,&island,&constraint_island);
+
+			island->set_island_list_next(island_list);
+			island_list=island;
+
+			if (constraint_island) {
+				constraint_island->set_island_list_next(constraint_island_list);
+				constraint_island_list=constraint_island;
+				island_count++;
+			}
+
+		}
+		b=b->next();
+	}
+
+	const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
+
+
+
+	while(aml.first()) {
+		for(const Set<Constraint2DSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) {
+
+			Constraint2DSW*c=E->get();
+			if (c->get_island_step()==_step)
+				continue;
+			c->set_island_step(_step);
+			c->set_island_next(NULL);
+			c->set_island_list_next(constraint_island_list);
+			constraint_island_list=c;
+		}
+		p_space->area_remove_from_moved_list((SelfList<Area2DSW>*)aml.first()); //faster to remove here
+	}
+
+//	print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
+	/* SETUP CONSTRAINT ISLANDS */
+
+	{
+		Constraint2DSW *ci=constraint_island_list;
+		while(ci) {
+
+			_setup_island(ci,p_delta);
+			ci=ci->get_island_list_next();
+		}
+	}
+
+	/* SOLVE CONSTRAINT ISLANDS */
+
+	{
+		Constraint2DSW *ci=constraint_island_list;
+		while(ci) {
+			//iterating each island separatedly improves cache efficiency
+			_solve_island(ci,p_iterations,p_delta);
+			ci=ci->get_island_list_next();
+		}
+	}
+
+	/* INTEGRATE VELOCITIES */
+
+	b = body_list->first();
+	while(b) {
+
+		b->self()->integrate_velocities(p_delta);
+		b=b->next();
+	}
+
+	/* SLEEP / WAKE UP ISLANDS */
+
+	{
+		Body2DSW *bi=island_list;
+		while(bi) {
+
+			_check_suspend(bi,p_delta);
+			bi=bi->get_island_list_next();
+		}
+	}
+
+	p_space->update();
+	p_space->unlock();
+	_step++;
+
+
+
+}
+
+Step2DSW::Step2DSW() {
+
+	_step=1;
+}
diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h
new file mode 100644
index 000000000..91ac9d858
--- /dev/null
+++ b/servers/physics_2d/step_2d_sw.h
@@ -0,0 +1,48 @@
+/*************************************************************************/
+/*  step_2d_sw.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 STEP_2D_SW_H
+#define STEP_2D_SW_H
+
+#include "space_2d_sw.h"
+
+class Step2DSW {
+
+	uint64_t _step;
+
+	void _populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island);
+	void _setup_island(Constraint2DSW *p_island,float p_delta);
+	void _solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta);
+	void _check_suspend(Body2DSW *p_island,float p_delta);
+public:
+
+	void step(Space2DSW* p_space,float p_delta,int p_iterations);
+	Step2DSW();
+};
+
+#endif // STEP_2D_SW_H
diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp
new file mode 100644
index 000000000..cae9565c4
--- /dev/null
+++ b/servers/physics_2d_server.cpp
@@ -0,0 +1,417 @@
+/*************************************************************************/
+/*  physics_2d_server.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 "physics_2d_server.h"
+#include "print_string.h"
+Physics2DServer * Physics2DServer::singleton=NULL;
+
+
+void Physics2DDirectBodyState::integrate_forces() {
+
+	real_t step = get_step();
+	Vector2 lv = get_linear_velocity();
+	lv+=get_total_gravity() * step;
+
+	real_t av = get_angular_velocity();
+
+	float damp = 1.0 - step * get_total_density();
+
+	if (damp<0) // reached zero in the given time
+		damp=0;
+
+	lv*=damp;
+	av*=damp;
+
+	set_linear_velocity(lv);
+	set_angular_velocity(av);
+
+
+
+
+}
+
+Object* Physics2DDirectBodyState::get_contact_collider_object(int p_contact_idx) const {
+
+	ObjectID objid = get_contact_collider_id(p_contact_idx);
+	Object *obj = ObjectDB::get_instance( objid );
+	return obj;
+}
+
+Physics2DServer * Physics2DServer::get_singleton() {
+
+	return singleton;
+}
+
+void Physics2DDirectBodyState::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_total_gravity"),&Physics2DDirectBodyState::get_total_gravity);
+	ObjectTypeDB::bind_method(_MD("get_total_density"),&Physics2DDirectBodyState::get_total_density);
+
+	ObjectTypeDB::bind_method(_MD("get_inverse_mass"),&Physics2DDirectBodyState::get_inverse_mass);
+	ObjectTypeDB::bind_method(_MD("get_inverse_inertia"),&Physics2DDirectBodyState::get_inverse_inertia);
+
+	ObjectTypeDB::bind_method(_MD("set_linear_velocity","velocity"),&Physics2DDirectBodyState::set_linear_velocity);
+	ObjectTypeDB::bind_method(_MD("get_linear_velocity"),&Physics2DDirectBodyState::get_linear_velocity);
+
+	ObjectTypeDB::bind_method(_MD("set_angular_velocity","velocity"),&Physics2DDirectBodyState::set_angular_velocity);
+	ObjectTypeDB::bind_method(_MD("get_angular_velocity"),&Physics2DDirectBodyState::get_angular_velocity);
+
+	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&Physics2DDirectBodyState::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"),&Physics2DDirectBodyState::get_transform);
+
+	ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&Physics2DDirectBodyState::set_sleep_state);
+	ObjectTypeDB::bind_method(_MD("is_sleeping"),&Physics2DDirectBodyState::is_sleeping);
+
+	ObjectTypeDB::bind_method(_MD("get_contact_count"),&Physics2DDirectBodyState::get_contact_count);
+
+	ObjectTypeDB::bind_method(_MD("get_contact_local_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_local_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_normal","contact_idx"),&Physics2DDirectBodyState::get_contact_local_normal);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_shape","contact_idx"),&Physics2DDirectBodyState::get_contact_local_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider","contact_idx"),&Physics2DDirectBodyState::get_contact_collider);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_id","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_id);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_object","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_object);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_velocity_at_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_velocity_at_pos);
+	ObjectTypeDB::bind_method(_MD("get_step"),&Physics2DDirectBodyState::get_step);
+	ObjectTypeDB::bind_method(_MD("integrate_forces"),&Physics2DDirectBodyState::integrate_forces);
+	ObjectTypeDB::bind_method(_MD("get_space_state:Physics2DDirectSpaceState"),&Physics2DDirectBodyState::get_space_state);
+
+}
+
+Physics2DDirectBodyState::Physics2DDirectBodyState() {}
+
+///////////////////////////////////////////////////////
+
+
+
+Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+	RayResult inters;
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+	bool res = intersect_ray(p_from,p_to,inters,exclude,p_user_mask);
+
+	if (!res)
+		return Variant();
+
+	Dictionary d;
+	d["position"]=inters.position;
+	d["normal"]=inters.normal;
+	d["collider_id"]=inters.collider_id;
+	d["collider"]=inters.collider;
+	d["shape"]=inters.shape;
+	d["rid"]=inters.rid;
+
+	return d;
+}
+
+Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+	ERR_FAIL_INDEX_V(p_result_max,4096,Variant());
+	if (p_result_max<=0)
+		return Variant();
+
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+	ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
+
+	int rc = intersect_shape(p_shape,p_xform,res,p_result_max,exclude,p_user_mask);
+
+	if (rc==0)
+		return Variant();
+
+	Ref<Physics2DShapeQueryResult>  result = memnew( Physics2DShapeQueryResult );
+	result->result.resize(rc);
+	for(int i=0;i<rc;i++)
+		result->result[i]=res[i];
+
+	return result;
+
+}
+
+
+
+
+
+Physics2DDirectSpaceState::Physics2DDirectSpaceState() {
+
+
+
+}
+
+
+void Physics2DDirectSpaceState::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","umask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
+	ObjectTypeDB::bind_method(_MD("intersect_shape:Physics2DShapeQueryResult","shape","xform","result_max","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
+
+}
+
+
+int Physics2DShapeQueryResult::get_result_count() const {
+
+	return result.size();
+}
+RID Physics2DShapeQueryResult::get_result_rid(int p_idx) const {
+
+	return result[p_idx].rid;
+}
+ObjectID Physics2DShapeQueryResult::get_result_object_id(int p_idx) const {
+
+	return result[p_idx].collider_id;
+}
+Object* Physics2DShapeQueryResult::get_result_object(int p_idx) const {
+
+	return result[p_idx].collider;
+}
+int Physics2DShapeQueryResult::get_result_object_shape(int p_idx) const {
+
+	return result[p_idx].shape;
+}
+
+Physics2DShapeQueryResult::Physics2DShapeQueryResult() {
+
+
+}
+
+void Physics2DShapeQueryResult::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_result_count"),&Physics2DShapeQueryResult::get_result_count);
+	ObjectTypeDB::bind_method(_MD("get_result_rid","idx"),&Physics2DShapeQueryResult::get_result_rid);
+	ObjectTypeDB::bind_method(_MD("get_result_object_id","idx"),&Physics2DShapeQueryResult::get_result_object_id);
+	ObjectTypeDB::bind_method(_MD("get_result_object","idx"),&Physics2DShapeQueryResult::get_result_object);
+	ObjectTypeDB::bind_method(_MD("get_result_object_shape","idx"),&Physics2DShapeQueryResult::get_result_object_shape);
+
+
+}
+
+
+
+
+
+
+///////////////////////////////////////
+
+void Physics2DServer::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("shape_create","type"),&Physics2DServer::shape_create);
+	ObjectTypeDB::bind_method(_MD("shape_set_data","shape","data"),&Physics2DServer::shape_set_data);
+
+	ObjectTypeDB::bind_method(_MD("shape_get_type","shape"),&Physics2DServer::shape_get_type);
+	ObjectTypeDB::bind_method(_MD("shape_get_data","shape"),&Physics2DServer::shape_get_data);
+
+
+	ObjectTypeDB::bind_method(_MD("space_create"),&Physics2DServer::space_create);
+	ObjectTypeDB::bind_method(_MD("space_set_active","space","active"),&Physics2DServer::space_set_active);
+	ObjectTypeDB::bind_method(_MD("space_is_active","space"),&Physics2DServer::space_is_active);
+	ObjectTypeDB::bind_method(_MD("space_set_param","space","param","value"),&Physics2DServer::space_set_param);
+	ObjectTypeDB::bind_method(_MD("space_get_param","space","param"),&Physics2DServer::space_get_param);
+	ObjectTypeDB::bind_method(_MD("space_get_direct_state:Physics2DDirectSpaceState","space"),&Physics2DServer::space_get_direct_state);
+
+	ObjectTypeDB::bind_method(_MD("area_create"),&Physics2DServer::area_create);
+	ObjectTypeDB::bind_method(_MD("area_set_space","area","space"),&Physics2DServer::area_set_space);
+	ObjectTypeDB::bind_method(_MD("area_get_space","area"),&Physics2DServer::area_get_space);
+
+	ObjectTypeDB::bind_method(_MD("area_set_space_override_mode","area","mode"),&Physics2DServer::area_set_space_override_mode);
+	ObjectTypeDB::bind_method(_MD("area_get_space_override_mode","area"),&Physics2DServer::area_get_space_override_mode);
+
+	ObjectTypeDB::bind_method(_MD("area_add_shape","area","shape","transform"),&Physics2DServer::area_set_shape,DEFVAL(Matrix32()));
+	ObjectTypeDB::bind_method(_MD("area_set_shape","area","shape_idx","shape"),&Physics2DServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_set_shape_transform","area","shape_idx","transform"),&Physics2DServer::area_set_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_get_shape_count","area"),&Physics2DServer::area_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("area_get_shape","area","shape_idx"),&Physics2DServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_get_shape_transform","area","shape_idx"),&Physics2DServer::area_get_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_remove_shape","area","shape_idx"),&Physics2DServer::area_remove_shape);
+	ObjectTypeDB::bind_method(_MD("area_clear_shapes","area"),&Physics2DServer::area_clear_shapes);
+
+
+	ObjectTypeDB::bind_method(_MD("area_set_param","area","param","value"),&Physics2DServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_set_transform","area","transform"),&Physics2DServer::area_get_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_get_param","area","param"),&Physics2DServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_get_transform","area"),&Physics2DServer::area_get_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_attach_object_instance_ID","area","id"),&Physics2DServer::area_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("area_get_object_instance_ID","area"),&Physics2DServer::area_get_object_instance_ID);
+
+	ObjectTypeDB::bind_method(_MD("area_set_monitor_callback","receiver","method"),&Physics2DServer::area_set_monitor_callback);
+
+	ObjectTypeDB::bind_method(_MD("body_create","mode","init_sleeping"),&Physics2DServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false));
+
+	ObjectTypeDB::bind_method(_MD("body_set_space","body","space"),&Physics2DServer::body_set_space);
+	ObjectTypeDB::bind_method(_MD("body_get_space","body"),&Physics2DServer::body_get_space);
+
+	ObjectTypeDB::bind_method(_MD("body_set_mode","body","mode"),&Physics2DServer::body_set_mode);
+	ObjectTypeDB::bind_method(_MD("body_get_mode","body"),&Physics2DServer::body_get_mode);
+
+	ObjectTypeDB::bind_method(_MD("body_add_shape","body","shape","transform"),&Physics2DServer::body_add_shape,DEFVAL(Matrix32()));
+	ObjectTypeDB::bind_method(_MD("body_set_shape","body","shape_idx","shape"),&Physics2DServer::body_set_shape);
+	ObjectTypeDB::bind_method(_MD("body_set_shape_transform","body","shape_idx","transform"),&Physics2DServer::body_set_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("body_get_shape_count","body"),&Physics2DServer::body_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("body_get_shape","body","shape_idx"),&Physics2DServer::body_get_shape);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_transform","body","shape_idx"),&Physics2DServer::body_get_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("body_remove_shape","body","shape_idx"),&Physics2DServer::body_remove_shape);
+	ObjectTypeDB::bind_method(_MD("body_clear_shapes","body"),&Physics2DServer::body_clear_shapes);
+
+
+	ObjectTypeDB::bind_method(_MD("body_set_shape_as_trigger","body","shape_idx","enable"),&Physics2DServer::body_set_shape_as_trigger);
+	ObjectTypeDB::bind_method(_MD("body_is_shape_set_as_trigger","body","shape_idx"),&Physics2DServer::body_is_shape_set_as_trigger);
+
+	ObjectTypeDB::bind_method(_MD("body_attach_object_instance_ID","body","id"),&Physics2DServer::body_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID","body"),&Physics2DServer::body_get_object_instance_ID);
+
+
+	ObjectTypeDB::bind_method(_MD("body_set_enable_continuous_collision_detection","body","enable"),&Physics2DServer::body_set_enable_continuous_collision_detection);
+	ObjectTypeDB::bind_method(_MD("body_is_continuous_collision_detection_enabled","body"),&Physics2DServer::body_is_continuous_collision_detection_enabled);
+
+
+	//ObjectTypeDB::bind_method(_MD("body_set_user_flags","flags""),&Physics2DServer::body_set_shape,DEFVAL(Matrix32));
+	//ObjectTypeDB::bind_method(_MD("body_get_user_flags","body","shape_idx","shape"),&Physics2DServer::body_get_shape);
+
+	ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&Physics2DServer::body_set_param);
+	ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&Physics2DServer::body_get_param);
+
+	ObjectTypeDB::bind_method(_MD("body_static_simulate_motion","body","new_xform"),&Physics2DServer::body_static_simulate_motion);
+
+	ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&Physics2DServer::body_set_state);
+	ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&Physics2DServer::body_get_state);
+
+	ObjectTypeDB::bind_method(_MD("body_apply_impulse","body","pos","impulse"),&Physics2DServer::body_apply_impulse);
+	ObjectTypeDB::bind_method(_MD("body_set_axis_velocity","body","axis_velocity"),&Physics2DServer::body_set_axis_velocity);
+
+	ObjectTypeDB::bind_method(_MD("body_add_collision_exception","body","excepted_body"),&Physics2DServer::body_add_collision_exception);
+	ObjectTypeDB::bind_method(_MD("body_remove_collision_exception","body","excepted_body"),&Physics2DServer::body_remove_collision_exception);
+//	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+
+	ObjectTypeDB::bind_method(_MD("body_set_max_contacts_reported","body","amount"),&Physics2DServer::body_set_max_contacts_reported);
+	ObjectTypeDB::bind_method(_MD("body_get_max_contacts_reported","body"),&Physics2DServer::body_get_max_contacts_reported);
+
+	ObjectTypeDB::bind_method(_MD("body_set_omit_force_integration","body","enable"),&Physics2DServer::body_set_omit_force_integration);
+	ObjectTypeDB::bind_method(_MD("body_is_omitting_force_integration","body"),&Physics2DServer::body_is_omitting_force_integration);
+
+	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method"),&Physics2DServer::body_set_force_integration_callback);
+
+	/* JOINT API */
+
+	ObjectTypeDB::bind_method(_MD("joint_set_param","joint","param","value"),&Physics2DServer::joint_set_param);
+	ObjectTypeDB::bind_method(_MD("joint_get_param","joint","param"),&Physics2DServer::joint_get_param);
+
+	ObjectTypeDB::bind_method(_MD("pin_joint_create","anchor","body_a","body_b"),&Physics2DServer::pin_joint_create,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("groove_joint_create","groove1_a","groove2_a","anchor_b","body_a","body_b"),&Physics2DServer::groove_joint_create,DEFVAL(RID()),DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_spring_joint_create","anchor_a","anchor_b","body_a","body_b"),&Physics2DServer::damped_spring_joint_create,DEFVAL(RID()));
+
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_set_param","joint","param","value"),&Physics2DServer::damped_string_joint_set_param,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_get_param","joint","param"),&Physics2DServer::damped_string_joint_get_param);
+
+	ObjectTypeDB::bind_method(_MD("joint_get_type","joint"),&Physics2DServer::joint_get_type);
+
+	ObjectTypeDB::bind_method(_MD("free","rid"),&Physics2DServer::free);
+
+	ObjectTypeDB::bind_method(_MD("set_active","active"),&Physics2DServer::set_active);
+
+//	ObjectTypeDB::bind_method(_MD("init"),&Physics2DServer::init);
+//	ObjectTypeDB::bind_method(_MD("step"),&Physics2DServer::step);
+//	ObjectTypeDB::bind_method(_MD("sync"),&Physics2DServer::sync);
+	//ObjectTypeDB::bind_method(_MD("flush_queries"),&Physics2DServer::flush_queries);
+
+	BIND_CONSTANT( SHAPE_LINE );
+	BIND_CONSTANT( SHAPE_SEGMENT );
+	BIND_CONSTANT( SHAPE_CIRCLE );
+	BIND_CONSTANT( SHAPE_RECTANGLE );
+	BIND_CONSTANT( SHAPE_CAPSULE );
+	BIND_CONSTANT( SHAPE_CONVEX_POLYGON );
+	BIND_CONSTANT( SHAPE_CONCAVE_POLYGON );
+	BIND_CONSTANT( SHAPE_CUSTOM );
+
+	BIND_CONSTANT( AREA_PARAM_GRAVITY );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_VECTOR );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_IS_POINT );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_POINT_ATTENUATION );
+	BIND_CONSTANT( AREA_PARAM_DENSITY );
+	BIND_CONSTANT( AREA_PARAM_PRIORITY );
+
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE );
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_DISABLED );
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
+
+	BIND_CONSTANT( BODY_MODE_STATIC );
+	BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE );
+	BIND_CONSTANT( BODY_MODE_RIGID );
+	BIND_CONSTANT( BODY_MODE_CHARACTER );
+
+	BIND_CONSTANT( BODY_PARAM_BOUNCE );
+	BIND_CONSTANT( BODY_PARAM_FRICTION );
+	BIND_CONSTANT( BODY_PARAM_MASS );
+	BIND_CONSTANT( BODY_PARAM_MAX );
+
+	BIND_CONSTANT( BODY_STATE_TRANSFORM );
+	BIND_CONSTANT( BODY_STATE_LINEAR_VELOCITY );
+	BIND_CONSTANT( BODY_STATE_ANGULAR_VELOCITY );
+	BIND_CONSTANT( BODY_STATE_SLEEPING );
+	BIND_CONSTANT( BODY_STATE_CAN_SLEEP );
+
+	BIND_CONSTANT( JOINT_PIN );
+	BIND_CONSTANT( JOINT_GROOVE );
+	BIND_CONSTANT( JOINT_DAMPED_SPRING );
+
+	BIND_CONSTANT( DAMPED_STRING_REST_LENGTH );
+	BIND_CONSTANT( DAMPED_STRING_STIFFNESS );
+	BIND_CONSTANT( DAMPED_STRING_DAMPING );
+
+//	BIND_CONSTANT( TYPE_BODY );
+//	BIND_CONSTANT( TYPE_AREA );
+
+	BIND_CONSTANT( AREA_BODY_ADDED );
+	BIND_CONSTANT( AREA_BODY_REMOVED );
+
+
+}
+
+
+Physics2DServer::Physics2DServer() {
+
+	ERR_FAIL_COND( singleton!=NULL );
+	singleton=this;
+}
+
+Physics2DServer::~Physics2DServer() {
+
+	singleton=NULL;
+}
+
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
new file mode 100644
index 000000000..8ace6282f
--- /dev/null
+++ b/servers/physics_2d_server.h
@@ -0,0 +1,426 @@
+/*************************************************************************/
+/*  physics_2d_server.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 PHYSICS_2D_SERVER_H
+#define PHYSICS_2D_SERVER_H
+
+#include "object.h"
+#include "reference.h"
+
+class Physics2DDirectSpaceState;
+
+class Physics2DDirectBodyState : public Object {
+
+	OBJ_TYPE( Physics2DDirectBodyState, Object );
+protected:
+	static void _bind_methods();
+public:
+
+	virtual Vector2 get_total_gravity() const=0; // get gravity vector working on this body space/area
+	virtual float get_total_density() const=0; // get density of this body space/area
+
+	virtual float get_inverse_mass() const=0; // get the mass
+	virtual real_t get_inverse_inertia() const=0; // get density of this body space
+
+	virtual void set_linear_velocity(const Vector2& p_velocity)=0;
+	virtual Vector2 get_linear_velocity() const=0;
+
+	virtual void set_angular_velocity(real_t p_velocity)=0;
+	virtual real_t get_angular_velocity() const=0;
+
+	virtual void set_transform(const Matrix32& p_transform)=0;
+	virtual Matrix32 get_transform() const=0;
+
+	virtual void set_sleep_state(bool p_enable)=0;
+	virtual bool is_sleeping() const=0;
+
+	virtual int get_contact_count() const=0;
+
+	virtual Vector2 get_contact_local_pos(int p_contact_idx) const=0;
+	virtual Vector2 get_contact_local_normal(int p_contact_idx) const=0;
+	virtual int get_contact_local_shape(int p_contact_idx) const=0;
+
+	virtual RID get_contact_collider(int p_contact_idx) const=0;
+	virtual Vector2 get_contact_collider_pos(int p_contact_idx) const=0;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const=0;
+	virtual Object* get_contact_collider_object(int p_contact_idx) const;
+	virtual int get_contact_collider_shape(int p_contact_idx) const=0;
+	virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const=0;
+
+	virtual real_t get_step() const=0;
+	virtual void integrate_forces();
+
+	virtual Physics2DDirectSpaceState* get_space_state()=0;
+
+	Physics2DDirectBodyState();
+};
+
+
+class Physics2DShapeQueryResult;
+
+
+class Physics2DDirectSpaceState : public Object {
+
+	OBJ_TYPE( Physics2DDirectSpaceState, Object );
+
+	Variant _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
+	Variant _intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
+
+
+protected:
+	static void _bind_methods();
+
+public:
+
+	struct RayResult {
+
+		Vector2 position;
+		Vector2 normal;
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+	};
+
+	virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+	struct ShapeResult {
+
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+
+	};
+
+	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+	Physics2DDirectSpaceState();
+};
+
+
+class Physics2DShapeQueryResult : public Reference {
+
+	OBJ_TYPE( Physics2DShapeQueryResult, Reference );
+
+	Vector<Physics2DDirectSpaceState::ShapeResult> result;
+
+friend class Physics2DDirectSpaceState;
+
+protected:
+	static void _bind_methods();
+public:
+
+	int get_result_count() const;
+	RID get_result_rid(int p_idx) const;
+	ObjectID get_result_object_id(int p_idx) const;
+	Object* get_result_object(int p_idx) const;
+	int get_result_object_shape(int p_idx) const;
+
+	Physics2DShapeQueryResult();
+};
+
+
+class Physics2DServer : public Object {
+
+	OBJ_TYPE( Physics2DServer, Object );
+
+	static Physics2DServer * singleton;
+
+protected:
+	static void _bind_methods();
+
+public:
+
+	static Physics2DServer * get_singleton();
+
+	enum ShapeType {
+		SHAPE_LINE, ///< plane:"plane"
+		SHAPE_RAY, ///< float:"length"
+		SHAPE_SEGMENT, ///< float:"length"
+		SHAPE_CIRCLE, ///< float:"radius"
+		SHAPE_RECTANGLE, ///< vec3:"extents"
+		SHAPE_CAPSULE,
+		SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+		SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array)
+		SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+	};
+
+	virtual RID shape_create(ShapeType p_shape)=0;
+	virtual void shape_set_data(RID p_shape, const Variant& p_data)=0;
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias)=0;
+
+	virtual ShapeType shape_get_type(RID p_shape) const=0;
+	virtual Variant shape_get_data(RID p_shape) const=0;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0;
+
+
+	/* SPACE API */
+
+	virtual RID space_create()=0;
+	virtual void space_set_active(RID p_space,bool p_active)=0;
+	virtual bool space_is_active(RID p_space) const=0;
+
+	enum SpaceParameter {
+
+		SPACE_PARAM_CONTACT_RECYCLE_RADIUS,
+		SPACE_PARAM_CONTACT_MAX_SEPARATION,
+		SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION,
+		SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD,
+		SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD,
+		SPACE_PARAM_BODY_TIME_TO_SLEEP,
+		SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO,
+		SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
+	};
+
+	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value)=0;
+	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const=0;
+
+	// this function only works on fixed process, errors and returns null otherwise
+	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space)=0;
+
+
+	//missing space parameters
+
+	/* AREA API */
+
+	//missing attenuation? missing better override?
+
+
+
+	enum AreaParameter {
+		AREA_PARAM_GRAVITY,
+		AREA_PARAM_GRAVITY_VECTOR,
+		AREA_PARAM_GRAVITY_IS_POINT,
+		AREA_PARAM_GRAVITY_POINT_ATTENUATION,
+		AREA_PARAM_DENSITY,
+		AREA_PARAM_PRIORITY
+	};
+
+	virtual RID area_create()=0;
+
+	virtual void area_set_space(RID p_area, RID p_space)=0;
+	virtual RID area_get_space(RID p_area) const=0;
+
+
+	enum AreaSpaceOverrideMode {
+		AREA_SPACE_OVERRIDE_DISABLED,
+		AREA_SPACE_OVERRIDE_COMBINE,
+		AREA_SPACE_OVERRIDE_REPLACE,
+	};
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode)=0;
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const=0;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform=Matrix32())=0;
+	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape)=0;
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform)=0;
+
+	virtual int area_get_shape_count(RID p_area) const=0;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const=0;
+	virtual Matrix32 area_get_shape_transform(RID p_area, int p_shape_idx) const=0;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx)=0;
+	virtual void area_clear_shapes(RID p_area)=0;
+
+	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID)=0;
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const=0;
+
+	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value)=0;
+	virtual void area_set_transform(RID p_area, const Matrix32& p_transform)=0;
+
+	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const=0;
+	virtual Matrix32 area_get_transform(RID p_area) const=0;
+
+	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
+
+	/* BODY API */
+
+	//missing ccd?
+
+	enum BodyMode {
+		BODY_MODE_STATIC,
+		BODY_MODE_STATIC_ACTIVE,
+		BODY_MODE_RIGID,
+		//BODY_MODE_SOFT
+		BODY_MODE_CHARACTER
+	};
+
+	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0;
+
+	virtual void body_set_space(RID p_body, RID p_space)=0;
+	virtual RID body_get_space(RID p_body) const=0;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode)=0;
+	virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const=0;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32())=0;
+	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0;
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform)=0;
+
+	virtual int body_get_shape_count(RID p_body) const=0;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const=0;
+	virtual Matrix32 body_get_shape_transform(RID p_body, int p_shape_idx) const=0;
+
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable)=0;
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const=0;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx)=0;
+	virtual void body_clear_shapes(RID p_body)=0;
+
+	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0;
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const=0;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0;
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
+
+	// common body variables
+	enum BodyParameter {
+		BODY_PARAM_BOUNCE,
+		BODY_PARAM_FRICTION,
+		BODY_PARAM_MASS, ///< unused for static, always infinite
+		BODY_PARAM_MAX,
+	};
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0;
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const=0;
+
+	//advanced simulation
+	virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform)=0;
+
+	//state
+	enum BodyState {
+		BODY_STATE_TRANSFORM,
+		BODY_STATE_LINEAR_VELOCITY,
+		BODY_STATE_ANGULAR_VELOCITY,
+		BODY_STATE_SLEEPING,
+		BODY_STATE_CAN_SLEEP
+	};
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant)=0;
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const=0;
+
+	//do something about it
+	virtual void body_set_applied_force(RID p_body, const Vector2& p_force)=0;
+	virtual Vector2 body_get_applied_force(RID p_body) const=0;
+
+	virtual void body_set_applied_torque(RID p_body, float p_torque)=0;
+	virtual float body_get_applied_torque(RID p_body) const=0;
+
+	virtual void body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse)=0;
+	virtual void body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity)=0;
+
+	//fix
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b)=0;
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b)=0;
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts)=0;
+	virtual int body_get_max_contacts_reported(RID p_body) const=0;
+
+	//missing remove
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold)=0;
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const=0;
+
+	virtual void body_set_omit_force_integration(RID p_body,bool p_omit)=0;
+	virtual bool body_is_omitting_force_integration(RID p_body) const=0;
+
+	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
+
+	/* JOINT API */
+
+	enum JointType {
+
+		JOINT_PIN,
+		JOINT_GROOVE,
+		JOINT_DAMPED_SPRING
+	};
+
+	enum JointParam {
+		JOINT_PARAM_BIAS,
+		JOINT_PARAM_MAX_BIAS,
+		JOINT_PARAM_MAX_FORCE,
+	};
+
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value)=0;
+	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const=0;
+
+	virtual RID pin_joint_create(const Vector2& p_anchor,RID p_body_a,RID p_body_b=RID())=0;
+	virtual RID groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b)=0;
+	virtual RID damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b=RID())=0;
+
+	enum DampedStringParam {
+		DAMPED_STRING_REST_LENGTH,
+		DAMPED_STRING_STIFFNESS,
+		DAMPED_STRING_DAMPING
+	};
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value)=0;
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const=0;
+
+	virtual JointType joint_get_type(RID p_joint) const=0;	
+
+	/* QUERY API */
+
+	enum AreaBodyStatus {
+		AREA_BODY_ADDED,
+		AREA_BODY_REMOVED
+	};
+
+
+	/* MISC */
+
+	virtual void free(RID p_rid)=0;
+
+	virtual void set_active(bool p_active)=0;
+	virtual void init()=0;
+	virtual void step(float p_step)=0;
+	virtual void sync()=0;
+	virtual void flush_queries()=0;
+	virtual void finish()=0;
+
+	Physics2DServer();
+	~Physics2DServer();
+};
+
+VARIANT_ENUM_CAST( Physics2DServer::ShapeType );
+VARIANT_ENUM_CAST( Physics2DServer::SpaceParameter );
+VARIANT_ENUM_CAST( Physics2DServer::AreaParameter );
+VARIANT_ENUM_CAST( Physics2DServer::AreaSpaceOverrideMode );
+VARIANT_ENUM_CAST( Physics2DServer::BodyMode );
+VARIANT_ENUM_CAST( Physics2DServer::BodyParameter );
+VARIANT_ENUM_CAST( Physics2DServer::BodyState );
+VARIANT_ENUM_CAST( Physics2DServer::JointParam );
+VARIANT_ENUM_CAST( Physics2DServer::JointType );
+VARIANT_ENUM_CAST( Physics2DServer::DampedStringParam );
+//VARIANT_ENUM_CAST( Physics2DServer::ObjectType );
+VARIANT_ENUM_CAST( Physics2DServer::AreaBodyStatus );
+
+#endif
diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp
new file mode 100644
index 000000000..69a2adae7
--- /dev/null
+++ b/servers/physics_server.cpp
@@ -0,0 +1,420 @@
+/*************************************************************************/
+/*  physics_server.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 "physics_server.h"
+#include "print_string.h"
+PhysicsServer * PhysicsServer::singleton=NULL;
+
+
+void PhysicsDirectBodyState::integrate_forces() {
+
+	real_t step = get_step();
+	Vector3 lv = get_linear_velocity();
+	lv+=get_total_gravity() * step;
+
+	Vector3 av = get_angular_velocity();
+
+	float damp = 1.0 - step * get_total_density();
+
+	if (damp<0) // reached zero in the given time
+		damp=0;
+
+	lv*=damp;
+	av*=damp;
+
+	set_linear_velocity(lv);
+	set_angular_velocity(av);
+
+
+
+
+}
+
+Object* PhysicsDirectBodyState::get_contact_collider_object(int p_contact_idx) const {
+
+	ObjectID objid = get_contact_collider_id(p_contact_idx);
+	Object *obj = ObjectDB::get_instance( objid );
+	return obj;
+}
+
+PhysicsServer * PhysicsServer::get_singleton() {
+
+	return singleton;
+}
+
+void PhysicsDirectBodyState::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_total_gravity"),&PhysicsDirectBodyState::get_total_gravity);
+	ObjectTypeDB::bind_method(_MD("get_total_density"),&PhysicsDirectBodyState::get_total_density);
+
+	ObjectTypeDB::bind_method(_MD("get_inverse_mass"),&PhysicsDirectBodyState::get_inverse_mass);
+	ObjectTypeDB::bind_method(_MD("get_inverse_inertia"),&PhysicsDirectBodyState::get_inverse_inertia);
+
+	ObjectTypeDB::bind_method(_MD("set_linear_velocity","velocity"),&PhysicsDirectBodyState::set_linear_velocity);
+	ObjectTypeDB::bind_method(_MD("get_linear_velocity"),&PhysicsDirectBodyState::get_linear_velocity);
+
+	ObjectTypeDB::bind_method(_MD("set_angular_velocity","velocity"),&PhysicsDirectBodyState::set_angular_velocity);
+	ObjectTypeDB::bind_method(_MD("get_angular_velocity"),&PhysicsDirectBodyState::get_angular_velocity);
+
+	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&PhysicsDirectBodyState::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsDirectBodyState::get_transform);
+
+	ObjectTypeDB::bind_method(_MD("add_force","force","pos"),&PhysicsDirectBodyState::add_force);
+
+	ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&PhysicsDirectBodyState::set_sleep_state);
+	ObjectTypeDB::bind_method(_MD("is_sleeping"),&PhysicsDirectBodyState::is_sleeping);
+
+	ObjectTypeDB::bind_method(_MD("get_contact_count"),&PhysicsDirectBodyState::get_contact_count);
+
+	ObjectTypeDB::bind_method(_MD("get_contact_local_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_local_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_normal","contact_idx"),&PhysicsDirectBodyState::get_contact_local_normal);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_shape","contact_idx"),&PhysicsDirectBodyState::get_contact_local_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider","contact_idx"),&PhysicsDirectBodyState::get_contact_collider);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_id","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_id);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_object","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_object);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_velocity_at_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_velocity_at_pos);
+	ObjectTypeDB::bind_method(_MD("get_step"),&PhysicsDirectBodyState::get_step);
+	ObjectTypeDB::bind_method(_MD("integrate_forces"),&PhysicsDirectBodyState::integrate_forces);
+	ObjectTypeDB::bind_method(_MD("get_space_state:PhysicsDirectSpaceState"),&PhysicsDirectBodyState::get_space_state);
+
+}
+
+PhysicsDirectBodyState::PhysicsDirectBodyState() {}
+
+///////////////////////////////////////////////////////
+
+
+
+Variant PhysicsDirectSpaceState::_intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+	RayResult inters;
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+	bool res = intersect_ray(p_from,p_to,inters,exclude,p_user_mask);
+
+	if (!res)
+		return Variant();
+
+	Dictionary d;
+	d["position"]=inters.position;
+	d["normal"]=inters.normal;
+	d["collider_id"]=inters.collider_id;
+	d["collider"]=inters.collider;
+	d["shape"]=inters.shape;
+	d["rid"]=inters.rid;
+
+	return d;
+}
+
+Variant PhysicsDirectSpaceState::_intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+
+
+	ERR_FAIL_INDEX_V(p_result_max,4096,Variant());
+	if (p_result_max<=0)
+		return Variant();
+
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+	ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
+
+	int rc = intersect_shape(p_shape,p_xform,res,p_result_max,exclude,p_user_mask);
+
+	if (rc==0)
+		return Variant();
+
+	Ref<PhysicsShapeQueryResult>  result = memnew( PhysicsShapeQueryResult );
+	result->result.resize(rc);
+	for(int i=0;i<rc;i++)
+		result->result[i]=res[i];
+
+	return result;
+
+}
+
+
+
+
+
+PhysicsDirectSpaceState::PhysicsDirectSpaceState() {
+
+
+
+}
+
+
+void PhysicsDirectSpaceState::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("intersect_ray","from","to","exclude","umask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
+	ObjectTypeDB::bind_method(_MD("intersect_shape:PhysicsShapeQueryResult","shape","xform","result_max","exclude","umask"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
+
+}
+
+
+int PhysicsShapeQueryResult::get_result_count() const {
+
+	return result.size();
+}
+RID PhysicsShapeQueryResult::get_result_rid(int p_idx) const {
+
+	return result[p_idx].rid;
+}
+ObjectID PhysicsShapeQueryResult::get_result_object_id(int p_idx) const {
+
+	return result[p_idx].collider_id;
+}
+Object* PhysicsShapeQueryResult::get_result_object(int p_idx) const {
+
+	return result[p_idx].collider;
+}
+int PhysicsShapeQueryResult::get_result_object_shape(int p_idx) const {
+
+	return result[p_idx].shape;
+}
+
+PhysicsShapeQueryResult::PhysicsShapeQueryResult() {
+
+
+}
+
+void PhysicsShapeQueryResult::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_result_count"),&PhysicsShapeQueryResult::get_result_count);
+	ObjectTypeDB::bind_method(_MD("get_result_rid","idx"),&PhysicsShapeQueryResult::get_result_rid);
+	ObjectTypeDB::bind_method(_MD("get_result_object_id","idx"),&PhysicsShapeQueryResult::get_result_object_id);
+	ObjectTypeDB::bind_method(_MD("get_result_object","idx"),&PhysicsShapeQueryResult::get_result_object);
+	ObjectTypeDB::bind_method(_MD("get_result_object_shape","idx"),&PhysicsShapeQueryResult::get_result_object_shape);
+
+
+}
+
+
+
+
+
+
+///////////////////////////////////////
+
+void PhysicsServer::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("shape_create","type"),&PhysicsServer::shape_create);
+	ObjectTypeDB::bind_method(_MD("shape_set_data","shape","data"),&PhysicsServer::shape_set_data);
+
+	ObjectTypeDB::bind_method(_MD("shape_get_type","shape"),&PhysicsServer::shape_get_type);
+	ObjectTypeDB::bind_method(_MD("shape_get_data","shape"),&PhysicsServer::shape_get_data);
+
+
+	ObjectTypeDB::bind_method(_MD("space_create"),&PhysicsServer::space_create);
+	ObjectTypeDB::bind_method(_MD("space_set_active","space","active"),&PhysicsServer::space_set_active);
+	ObjectTypeDB::bind_method(_MD("space_is_active","space"),&PhysicsServer::space_is_active);
+	ObjectTypeDB::bind_method(_MD("space_set_param","space","param","value"),&PhysicsServer::space_set_param);
+	ObjectTypeDB::bind_method(_MD("space_get_param","space","param"),&PhysicsServer::space_get_param);
+	ObjectTypeDB::bind_method(_MD("space_get_direct_state:PhysicsDirectSpaceState","space"),&PhysicsServer::space_get_direct_state);
+
+	ObjectTypeDB::bind_method(_MD("area_create"),&PhysicsServer::area_create);
+	ObjectTypeDB::bind_method(_MD("area_set_space","area","space"),&PhysicsServer::area_set_space);
+	ObjectTypeDB::bind_method(_MD("area_get_space","area"),&PhysicsServer::area_get_space);
+
+	ObjectTypeDB::bind_method(_MD("area_set_space_override_mode","area","mode"),&PhysicsServer::area_set_space_override_mode);
+	ObjectTypeDB::bind_method(_MD("area_get_space_override_mode","area"),&PhysicsServer::area_get_space_override_mode);
+
+	ObjectTypeDB::bind_method(_MD("area_add_shape","area","shape","transform"),&PhysicsServer::area_set_shape,DEFVAL(Transform()));
+	ObjectTypeDB::bind_method(_MD("area_set_shape","area","shape_idx","shape"),&PhysicsServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_set_shape_transform","area","shape_idx","transform"),&PhysicsServer::area_set_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_get_shape_count","area"),&PhysicsServer::area_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("area_get_shape","area","shape_idx"),&PhysicsServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_get_shape_transform","area","shape_idx"),&PhysicsServer::area_get_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_remove_shape","area","shape_idx"),&PhysicsServer::area_remove_shape);
+	ObjectTypeDB::bind_method(_MD("area_clear_shapes","area"),&PhysicsServer::area_clear_shapes);
+
+
+	ObjectTypeDB::bind_method(_MD("area_set_param","area","param","value"),&PhysicsServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_set_transform","area","transform"),&PhysicsServer::area_get_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_get_param","area","param"),&PhysicsServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_get_transform","area"),&PhysicsServer::area_get_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_attach_object_instance_ID","area","id"),&PhysicsServer::area_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("area_get_object_instance_ID","area"),&PhysicsServer::area_get_object_instance_ID);
+
+	ObjectTypeDB::bind_method(_MD("area_set_monitor_callback","receiver","method"),&PhysicsServer::area_set_monitor_callback);
+
+	ObjectTypeDB::bind_method(_MD("body_create","mode","init_sleeping"),&PhysicsServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false));
+
+	ObjectTypeDB::bind_method(_MD("body_set_space","body","space"),&PhysicsServer::body_set_space);
+	ObjectTypeDB::bind_method(_MD("body_get_space","body"),&PhysicsServer::body_get_space);
+
+	ObjectTypeDB::bind_method(_MD("body_set_mode","body","mode"),&PhysicsServer::body_set_mode);
+	ObjectTypeDB::bind_method(_MD("body_get_mode","body"),&PhysicsServer::body_get_mode);
+
+	ObjectTypeDB::bind_method(_MD("body_add_shape","body","shape","transform"),&PhysicsServer::body_add_shape,DEFVAL(Transform()));
+	ObjectTypeDB::bind_method(_MD("body_set_shape","body","shape_idx","shape"),&PhysicsServer::body_set_shape);
+	ObjectTypeDB::bind_method(_MD("body_set_shape_transform","body","shape_idx","transform"),&PhysicsServer::body_set_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("body_get_shape_count","body"),&PhysicsServer::body_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("body_get_shape","body","shape_idx"),&PhysicsServer::body_get_shape);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_transform","body","shape_idx"),&PhysicsServer::body_get_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("body_remove_shape","body","shape_idx"),&PhysicsServer::body_remove_shape);
+	ObjectTypeDB::bind_method(_MD("body_clear_shapes","body"),&PhysicsServer::body_clear_shapes);
+
+	ObjectTypeDB::bind_method(_MD("body_attach_object_instance_ID","body","id"),&PhysicsServer::body_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID","body"),&PhysicsServer::body_get_object_instance_ID);
+
+
+	ObjectTypeDB::bind_method(_MD("body_set_enable_continuous_collision_detection","body","enable"),&PhysicsServer::body_set_enable_continuous_collision_detection);
+	ObjectTypeDB::bind_method(_MD("body_is_continuous_collision_detection_enabled","body"),&PhysicsServer::body_is_continuous_collision_detection_enabled);
+
+
+	//ObjectTypeDB::bind_method(_MD("body_set_user_flags","flags""),&PhysicsServer::body_set_shape,DEFVAL(Transform));
+	//ObjectTypeDB::bind_method(_MD("body_get_user_flags","body","shape_idx","shape"),&PhysicsServer::body_get_shape);
+
+	ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&PhysicsServer::body_set_param);
+	ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&PhysicsServer::body_get_param);
+
+	ObjectTypeDB::bind_method(_MD("body_static_simulate_motion","body","new_xform"),&PhysicsServer::body_static_simulate_motion);
+
+	ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&PhysicsServer::body_set_state);
+	ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&PhysicsServer::body_get_state);
+
+	ObjectTypeDB::bind_method(_MD("body_apply_impulse","body","pos","impulse"),&PhysicsServer::body_apply_impulse);
+	ObjectTypeDB::bind_method(_MD("body_set_axis_velocity","body","axis_velocity"),&PhysicsServer::body_set_axis_velocity);
+
+	ObjectTypeDB::bind_method(_MD("body_add_collision_exception","body","excepted_body"),&PhysicsServer::body_add_collision_exception);
+	ObjectTypeDB::bind_method(_MD("body_remove_collision_exception","body","excepted_body"),&PhysicsServer::body_remove_collision_exception);
+//	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+
+	ObjectTypeDB::bind_method(_MD("body_set_max_contacts_reported","body","amount"),&PhysicsServer::body_set_max_contacts_reported);
+	ObjectTypeDB::bind_method(_MD("body_get_max_contacts_reported","body"),&PhysicsServer::body_get_max_contacts_reported);
+
+	ObjectTypeDB::bind_method(_MD("body_set_omit_force_integration","body","enable"),&PhysicsServer::body_set_omit_force_integration);
+	ObjectTypeDB::bind_method(_MD("body_is_omitting_force_integration","body"),&PhysicsServer::body_is_omitting_force_integration);
+
+	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method","userdata"),&PhysicsServer::body_set_force_integration_callback,DEFVAL(Variant()));
+
+	/* JOINT API */
+/*
+	ObjectTypeDB::bind_method(_MD("joint_set_param","joint","param","value"),&PhysicsServer::joint_set_param);
+	ObjectTypeDB::bind_method(_MD("joint_get_param","joint","param"),&PhysicsServer::joint_get_param);
+
+	ObjectTypeDB::bind_method(_MD("pin_joint_create","anchor","body_a","body_b"),&PhysicsServer::pin_joint_create,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("groove_joint_create","groove1_a","groove2_a","anchor_b","body_a","body_b"),&PhysicsServer::groove_joint_create,DEFVAL(RID()),DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_spring_joint_create","anchor_a","anchor_b","body_a","body_b"),&PhysicsServer::damped_spring_joint_create,DEFVAL(RID()));
+
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_set_param","joint","param","value"),&PhysicsServer::damped_string_joint_set_param,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_get_param","joint","param"),&PhysicsServer::damped_string_joint_get_param);
+
+	ObjectTypeDB::bind_method(_MD("joint_get_type","joint"),&PhysicsServer::joint_get_type);
+*/
+	ObjectTypeDB::bind_method(_MD("free","rid"),&PhysicsServer::free);
+
+	ObjectTypeDB::bind_method(_MD("set_active","active"),&PhysicsServer::set_active);
+
+//	ObjectTypeDB::bind_method(_MD("init"),&PhysicsServer::init);
+//	ObjectTypeDB::bind_method(_MD("step"),&PhysicsServer::step);
+//	ObjectTypeDB::bind_method(_MD("sync"),&PhysicsServer::sync);
+	//ObjectTypeDB::bind_method(_MD("flush_queries"),&PhysicsServer::flush_queries);
+
+
+	BIND_CONSTANT( SHAPE_PLANE );
+	BIND_CONSTANT( SHAPE_RAY );
+	BIND_CONSTANT( SHAPE_SPHERE );
+	BIND_CONSTANT( SHAPE_BOX );
+	BIND_CONSTANT( SHAPE_CAPSULE );
+	BIND_CONSTANT( SHAPE_CONVEX_POLYGON );
+	BIND_CONSTANT( SHAPE_CONCAVE_POLYGON );
+	BIND_CONSTANT( SHAPE_HEIGHTMAP );
+	BIND_CONSTANT( SHAPE_CUSTOM );
+
+
+	BIND_CONSTANT( AREA_PARAM_GRAVITY );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_VECTOR );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_IS_POINT );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_POINT_ATTENUATION );
+	BIND_CONSTANT( AREA_PARAM_DENSITY );
+	BIND_CONSTANT( AREA_PARAM_PRIORITY );
+
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE );
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_DISABLED );
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
+
+	BIND_CONSTANT( BODY_MODE_STATIC );
+	BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE );
+	BIND_CONSTANT( BODY_MODE_RIGID );
+	BIND_CONSTANT( BODY_MODE_CHARACTER );
+
+	BIND_CONSTANT( BODY_PARAM_BOUNCE );
+	BIND_CONSTANT( BODY_PARAM_FRICTION );
+	BIND_CONSTANT( BODY_PARAM_MASS );
+	BIND_CONSTANT( BODY_PARAM_MAX );
+
+	BIND_CONSTANT( BODY_STATE_TRANSFORM );
+	BIND_CONSTANT( BODY_STATE_LINEAR_VELOCITY );
+	BIND_CONSTANT( BODY_STATE_ANGULAR_VELOCITY );
+	BIND_CONSTANT( BODY_STATE_SLEEPING );
+	BIND_CONSTANT( BODY_STATE_CAN_SLEEP );
+/*
+	BIND_CONSTANT( JOINT_PIN );
+	BIND_CONSTANT( JOINT_GROOVE );
+	BIND_CONSTANT( JOINT_DAMPED_SPRING );
+
+	BIND_CONSTANT( DAMPED_STRING_REST_LENGTH );
+	BIND_CONSTANT( DAMPED_STRING_STIFFNESS );
+	BIND_CONSTANT( DAMPED_STRING_DAMPING );
+*/
+//	BIND_CONSTANT( TYPE_BODY );
+//	BIND_CONSTANT( TYPE_AREA );
+
+	BIND_CONSTANT( AREA_BODY_ADDED );
+	BIND_CONSTANT( AREA_BODY_REMOVED );
+
+
+}
+
+
+PhysicsServer::PhysicsServer() {
+
+	ERR_FAIL_COND( singleton!=NULL );
+	singleton=this;
+}
+
+PhysicsServer::~PhysicsServer() {
+
+	singleton=NULL;
+}
+
diff --git a/servers/physics_server.h b/servers/physics_server.h
new file mode 100644
index 000000000..1fe477adc
--- /dev/null
+++ b/servers/physics_server.h
@@ -0,0 +1,429 @@
+/*************************************************************************/
+/*  physics_server.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 PHYSICS_SERVER_H
+#define PHYSICS_SERVER_H
+
+#include "object.h"
+#include "reference.h"
+
+class PhysicsDirectSpaceState;
+
+class PhysicsDirectBodyState : public Object {
+
+	OBJ_TYPE( PhysicsDirectBodyState, Object );
+protected:
+	static void _bind_methods();
+public:
+
+	virtual Vector3 get_total_gravity() const=0; // get gravity vector working on this body space/area
+	virtual float get_total_density() const=0; // get density of this body space/area
+
+	virtual float get_inverse_mass() const=0; // get the mass
+	virtual Vector3 get_inverse_inertia() const=0; // get density of this body space
+	virtual Matrix3 get_inverse_inertia_tensor() const=0; // get density of this body space
+
+	virtual void set_linear_velocity(const Vector3& p_velocity)=0;
+	virtual Vector3 get_linear_velocity() const=0;
+
+	virtual void set_angular_velocity(const Vector3& p_velocity)=0;
+	virtual Vector3 get_angular_velocity() const=0;
+
+	virtual void set_transform(const Transform& p_transform)=0;
+	virtual Transform get_transform() const=0;
+
+	virtual void add_force(const Vector3& p_force, const Vector3& p_pos)=0;
+
+	virtual void set_sleep_state(bool p_enable)=0;
+	virtual bool is_sleeping() const=0;
+
+	virtual int get_contact_count() const=0;
+
+	virtual Vector3 get_contact_local_pos(int p_contact_idx) const=0;
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const=0;
+	virtual int get_contact_local_shape(int p_contact_idx) const=0;
+
+	virtual RID get_contact_collider(int p_contact_idx) const=0;
+	virtual Vector3 get_contact_collider_pos(int p_contact_idx) const=0;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const=0;
+	virtual Object* get_contact_collider_object(int p_contact_idx) const;
+	virtual int get_contact_collider_shape(int p_contact_idx) const=0;
+	virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const=0;
+
+	virtual real_t get_step() const=0;
+	virtual void integrate_forces();
+
+	virtual PhysicsDirectSpaceState* get_space_state()=0;
+
+	PhysicsDirectBodyState();
+};
+
+
+class PhysicsShapeQueryResult;
+
+
+class PhysicsDirectSpaceState : public Object {
+
+	OBJ_TYPE( PhysicsDirectSpaceState, Object );
+
+	Variant _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
+	Variant _intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
+
+
+protected:
+	static void _bind_methods();
+
+public:
+
+	struct RayResult {
+
+		Vector3 position;
+		Vector3 normal;
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+	};
+
+	virtual bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+	struct ShapeResult {
+
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+
+	};
+
+	virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+	PhysicsDirectSpaceState();
+};
+
+
+class PhysicsShapeQueryResult : public Reference {
+
+	OBJ_TYPE( PhysicsShapeQueryResult, Reference );
+
+	Vector<PhysicsDirectSpaceState::ShapeResult> result;
+
+friend class PhysicsDirectSpaceState;
+
+protected:
+	static void _bind_methods();
+public:
+
+	int get_result_count() const;
+	RID get_result_rid(int p_idx) const;
+	ObjectID get_result_object_id(int p_idx) const;
+	Object* get_result_object(int p_idx) const;
+	int get_result_object_shape(int p_idx) const;
+
+	PhysicsShapeQueryResult();
+};
+
+
+class PhysicsServer : public Object {
+
+	OBJ_TYPE( PhysicsServer, Object );
+
+	static PhysicsServer * singleton;
+
+protected:
+	static void _bind_methods();
+
+public:
+
+	static PhysicsServer * get_singleton();
+
+	enum ShapeType {
+		SHAPE_PLANE, ///< plane:"plane"
+		SHAPE_RAY, ///< float:"length"
+		SHAPE_SPHERE, ///< float:"radius"
+		SHAPE_BOX, ///< vec3:"extents"
+		SHAPE_CAPSULE, ///< dict( float:"radius", float:"height"):capsule
+		SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+		SHAPE_CONCAVE_POLYGON, ///< vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
+		SHAPE_HEIGHTMAP, ///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights"
+		SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+	};
+
+	virtual RID shape_create(ShapeType p_shape)=0;
+	virtual void shape_set_data(RID p_shape, const Variant& p_data)=0;
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias)=0;
+
+	virtual ShapeType shape_get_type(RID p_shape) const=0;
+	virtual Variant shape_get_data(RID p_shape) const=0;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0;
+
+
+	/* SPACE API */
+
+	virtual RID space_create()=0;
+	virtual void space_set_active(RID p_space,bool p_active)=0;
+	virtual bool space_is_active(RID p_space) const=0;
+
+	enum SpaceParameter {
+
+		SPACE_PARAM_CONTACT_RECYCLE_RADIUS,
+		SPACE_PARAM_CONTACT_MAX_SEPARATION,
+		SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION,
+		SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD,
+		SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD,
+		SPACE_PARAM_BODY_TIME_TO_SLEEP,
+		SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO,
+		SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
+	};
+
+	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value)=0;
+	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const=0;
+
+	// this function only works on fixed process, errors and returns null otherwise
+	virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space)=0;
+
+
+	//missing space parameters
+
+	/* AREA API */
+
+	//missing attenuation? missing better override?
+
+
+
+	enum AreaParameter {
+		AREA_PARAM_GRAVITY,
+		AREA_PARAM_GRAVITY_VECTOR,
+		AREA_PARAM_GRAVITY_IS_POINT,
+		AREA_PARAM_GRAVITY_POINT_ATTENUATION,
+		AREA_PARAM_DENSITY,
+		AREA_PARAM_PRIORITY
+	};
+
+	virtual RID area_create()=0;
+
+	virtual void area_set_space(RID p_area, RID p_space)=0;
+	virtual RID area_get_space(RID p_area) const=0;
+
+
+	enum AreaSpaceOverrideMode {
+		AREA_SPACE_OVERRIDE_DISABLED,
+		AREA_SPACE_OVERRIDE_COMBINE,
+		AREA_SPACE_OVERRIDE_REPLACE,
+	};
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode)=0;
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const=0;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform& p_transform=Transform())=0;
+	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape)=0;
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform)=0;
+
+	virtual int area_get_shape_count(RID p_area) const=0;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const=0;
+	virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const=0;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx)=0;
+	virtual void area_clear_shapes(RID p_area)=0;
+
+	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID)=0;
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const=0;
+
+	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value)=0;
+	virtual void area_set_transform(RID p_area, const Transform& p_transform)=0;
+
+	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const=0;
+	virtual Transform area_get_transform(RID p_area) const=0;
+
+	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
+
+	/* BODY API */
+
+	//missing ccd?
+
+	enum BodyMode {
+		BODY_MODE_STATIC,
+		BODY_MODE_STATIC_ACTIVE,
+		BODY_MODE_RIGID,
+		//BODY_MODE_SOFT
+		BODY_MODE_CHARACTER
+	};
+
+	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0;
+
+	virtual void body_set_space(RID p_body, RID p_space)=0;
+	virtual RID body_get_space(RID p_body) const=0;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode)=0;
+	virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const=0;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform& p_transform=Transform())=0;
+	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0;
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform)=0;
+
+	virtual int body_get_shape_count(RID p_body) const=0;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const=0;
+	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const=0;
+
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable)=0;
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const=0;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx)=0;
+	virtual void body_clear_shapes(RID p_body)=0;
+
+	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0;
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const=0;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0;
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
+
+	// common body variables
+	enum BodyParameter {
+		BODY_PARAM_BOUNCE,
+		BODY_PARAM_FRICTION,
+		BODY_PARAM_MASS, ///< unused for static, always infinite
+		BODY_PARAM_MAX,
+	};
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0;
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const=0;
+
+	//advanced simulation
+	virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform)=0;
+
+	//state
+	enum BodyState {
+		BODY_STATE_TRANSFORM,
+		BODY_STATE_LINEAR_VELOCITY,
+		BODY_STATE_ANGULAR_VELOCITY,
+		BODY_STATE_SLEEPING,
+		BODY_STATE_CAN_SLEEP
+	};
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant)=0;
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const=0;
+
+	//do something about it
+	virtual void body_set_applied_force(RID p_body, const Vector3& p_force)=0;
+	virtual Vector3 body_get_applied_force(RID p_body) const=0;
+
+	virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque)=0;
+	virtual Vector3 body_get_applied_torque(RID p_body) const=0;
+
+	virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse)=0;
+	virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity)=0;
+
+	//fix
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b)=0;
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b)=0;
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts)=0;
+	virtual int body_get_max_contacts_reported(RID p_body) const=0;
+
+	//missing remove
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold)=0;
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const=0;
+
+	virtual void body_set_omit_force_integration(RID p_body,bool p_omit)=0;
+	virtual bool body_is_omitting_force_integration(RID p_body) const=0;
+
+	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
+
+	/* JOINT API */
+#if 0
+	enum JointType {
+
+		JOINT_PIN,
+		JOINT_GROOVE,
+		JOINT_DAMPED_SPRING
+	};
+
+	enum JointParam {
+		JOINT_PARAM_BIAS,
+		JOINT_PARAM_MAX_BIAS,
+		JOINT_PARAM_MAX_FORCE,
+	};
+
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value)=0;
+	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const=0;
+
+	virtual RID pin_joint_create(const Vector3& p_anchor,RID p_body_a,RID p_body_b=RID())=0;
+	virtual RID groove_joint_create(const Vector3& p_a_groove1,const Vector3& p_a_groove2, const Vector3& p_b_anchor, RID p_body_a,RID p_body_b)=0;
+	virtual RID damped_spring_joint_create(const Vector3& p_anchor_a,const Vector3& p_anchor_b,RID p_body_a,RID p_body_b=RID())=0;
+
+	enum DampedStringParam {
+		DAMPED_STRING_REST_LENGTH,
+		DAMPED_STRING_STIFFNESS,
+		DAMPED_STRING_DAMPING
+	};
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value)=0;
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const=0;
+
+	virtual JointType joint_get_type(RID p_joint) const=0;	
+#endif
+	/* QUERY API */
+
+	enum AreaBodyStatus {
+		AREA_BODY_ADDED,
+		AREA_BODY_REMOVED
+	};
+
+
+	/* MISC */
+
+	virtual void free(RID p_rid)=0;
+
+	virtual void set_active(bool p_active)=0;
+	virtual void init()=0;
+	virtual void step(float p_step)=0;
+	virtual void sync()=0;
+	virtual void flush_queries()=0;
+	virtual void finish()=0;
+
+	PhysicsServer();
+	~PhysicsServer();
+};
+
+VARIANT_ENUM_CAST( PhysicsServer::ShapeType );
+VARIANT_ENUM_CAST( PhysicsServer::SpaceParameter );
+VARIANT_ENUM_CAST( PhysicsServer::AreaParameter );
+VARIANT_ENUM_CAST( PhysicsServer::AreaSpaceOverrideMode );
+VARIANT_ENUM_CAST( PhysicsServer::BodyMode );
+VARIANT_ENUM_CAST( PhysicsServer::BodyParameter );
+VARIANT_ENUM_CAST( PhysicsServer::BodyState );
+//VARIANT_ENUM_CAST( PhysicsServer::JointParam );
+//VARIANT_ENUM_CAST( PhysicsServer::JointType );
+//VARIANT_ENUM_CAST( PhysicsServer::DampedStringParam );
+//VARIANT_ENUM_CAST( PhysicsServer::ObjectType );
+VARIANT_ENUM_CAST( PhysicsServer::AreaBodyStatus );
+
+#endif