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

Conflicts:
	platform/windows/detect.py

6 files changed, 941 insertions, 937 deletions

diff --git a/servers/audio/audio_mixer_sw.cpp b/servers/audio/audio_mixer_sw.cpp
index 788eec0d4a..791f31719e 100644
--- a/servers/audio/audio_mixer_sw.cpp
+++ b/servers/audio/audio_mixer_sw.cpp
@@ -765,6 +765,10 @@ AudioMixer::ChannelID AudioMixerSW::channel_alloc(RID p_sample) {
 	c.mix.increment=1;
 	//zero everything when this errors
 	for(int i=0;i<4;i++) {
+		c.mix.vol[i]=0;
+		c.mix.reverb_vol[i]=0;
+		c.mix.chorus_vol[i]=0;
+
 		c.mix.old_vol[i]=0;
 		c.mix.old_reverb_vol[i]=0;
 		c.mix.old_chorus_vol[i]=0;
diff --git a/servers/audio/audio_server_sw.cpp b/servers/audio/audio_server_sw.cpp
index 9aaaa240c3..417a582da6 100644
--- a/servers/audio/audio_server_sw.cpp
+++ b/servers/audio/audio_server_sw.cpp
@@ -455,12 +455,12 @@ void AudioServerSW::voice_play(RID p_voice, RID p_sample) {
 
 }
 
-void AudioServerSW::voice_set_volume(RID p_voice, float p_db) {
+void AudioServerSW::voice_set_volume(RID p_voice, float p_volume) {
 
 	VoiceRBSW::Command cmd;
 	cmd.type=VoiceRBSW::Command::CMD_SET_VOLUME;
 	cmd.voice=p_voice;
-	cmd.volume.volume=p_db;
+	cmd.volume.volume=p_volume;
 	voice_rb.push_command(cmd);
 
 }
diff --git a/servers/audio/audio_server_sw.h b/servers/audio/audio_server_sw.h
index 91146e64e6..c566e129c1 100644
--- a/servers/audio/audio_server_sw.h
+++ b/servers/audio/audio_server_sw.h
@@ -146,7 +146,7 @@ public:
 
 	virtual void voice_play(RID p_voice, RID p_sample);
 
-	virtual void voice_set_volume(RID p_voice, float p_db);
+	virtual void voice_set_volume(RID p_voice, float p_volume);
 	virtual void voice_set_pan(RID p_voice, float p_pan, float p_depth=0,float height=0); //pan and depth go from -1 to 1
 	virtual void voice_set_filter(RID p_voice, FilterType p_type, float p_cutoff, float p_resonance,float p_gain=0);
 	virtual void voice_set_chorus(RID p_voice, float p_chorus );
diff --git a/servers/audio_server.h b/servers/audio_server.h
index f54698a1e3..cd3e920f31 100644
--- a/servers/audio_server.h
+++ b/servers/audio_server.h
@@ -210,7 +210,7 @@ public:
 
 	virtual void voice_play(RID p_voice, RID p_sample)=0;
 
-	virtual void voice_set_volume(RID p_voice, float p_gain)=0;
+	virtual void voice_set_volume(RID p_voice, float p_volume)=0;
 	virtual void voice_set_pan(RID p_voice, float p_pan, float p_depth=0,float height=0)=0; //pan and depth go from -1 to 1
 	virtual void voice_set_filter(RID p_voice, FilterType p_type, float p_cutoff, float p_resonance, float p_gain=0)=0;
 	virtual void voice_set_chorus(RID p_voice, float p_chorus )=0;
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index a4fe1dd3fc..d36b004989 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -1,746 +1,746 @@
-/*************************************************************************/

-/*  space_sw.cpp                                                         */

-/*************************************************************************/

-/*                       This file is part of:                           */

-/*                           GODOT ENGINE                                */

-/*                    http://www.godotengine.org                         */

-/*************************************************************************/

-/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur.                 */

-/*                                                                       */

-/* Permission is hereby granted, free of charge, to any person obtaining */

-/* a copy of this software and associated documentation files (the       */

-/* "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 "globals.h"

-#include "space_sw.h"

-#include "collision_solver_sw.h"

-#include "physics_server_sw.h"

-

-

-_FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) {

-

-	if ((p_object->get_layer_mask()&p_layer_mask)==0)

-		return false;

-

-	if (p_object->get_type()==CollisionObjectSW::TYPE_AREA && !(p_type_mask&PhysicsDirectSpaceState::TYPE_MASK_AREA))

-		return false;

-

-	BodySW *body = static_cast<BodySW*>(p_object);

-

-	return (1<<body->get_mode())&p_type_mask;

-

-}

-

-

-bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_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 (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))

-			continue;

-

-		if (!(static_cast<CollisionObjectSW*>(space->intersection_query_results[i])->is_ray_pickable()))

-			continue;

-

-		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);

-	else

-		r_result.collider=NULL;

-	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,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_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 (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))

-			continue;

-

-		//area cant be picked by ray (default)

-

-		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,p_margin,0))

-			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);

-		else

-			r_results[cc].collider=NULL;

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

-		r_results[cc].shape=shape_idx;

-

-		cc++;

-

-	}

-

-	return cc;

-

-}

-

-

-bool PhysicsDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,ShapeRestInfo *r_info) {

-

-

-

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

-	ERR_FAIL_COND_V(!shape,false);

-

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

-	aabb=aabb.merge(AABB(aabb.pos+p_motion,aabb.size)); //motion

-	aabb=aabb.grow(p_margin);

-

-	//if (p_motion!=Vector3())

-	//	print_line(p_motion);

-

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

-

-	float best_safe=1;

-	float best_unsafe=1;

-

-	Transform xform_inv = p_xform.affine_inverse();

-	MotionShapeSW mshape;

-	mshape.shape=shape;

-	mshape.motion=xform_inv.basis.xform(p_motion);

-

-	bool best_first=true;

-

-	Vector3 closest_A,closest_B;

-

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

-

-

-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))

-			continue;

-

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

-			continue; //ignore excluded

-

-

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

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

-

-		Vector3 point_A,point_B;

-		Vector3 sep_axis=p_motion.normalized();

-

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

-		//test initial overlap, does it collide if going all the way?

-		if (CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {

-			//print_line("failed motion cast (no collision)");

-			continue;

-		}

-

-

-		//test initial overlap

-#if 0

-		if (CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,NULL,NULL,&sep_axis)) {

-			print_line("failed initial cast (collision at begining)");

-			return false;

-		}

-#else

-		sep_axis=p_motion.normalized();

-

-		if (!CollisionSolverSW::solve_distance(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {

-			//print_line("failed motion cast (no collision)");

-			return false;

-		}

-#endif

-

-

-		//just do kinematic solving

-		float low=0;

-		float hi=1;

-		Vector3 mnormal=p_motion.normalized();

-

-		for(int i=0;i<8;i++) { //steps should be customizable..

-

-			Transform xfa = p_xform;

-			float ofs = (low+hi)*0.5;

-

-			Vector3 sep=mnormal; //important optimization for this to work fast enough

-

-			mshape.motion=xform_inv.basis.xform(p_motion*ofs);

-

-			Vector3 lA,lB;

-

-			bool collided = !CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,lA,lB,aabb,&sep);

-

-			if (collided) {

-

-				//print_line(itos(i)+": "+rtos(ofs));

-				hi=ofs;

-			} else {

-

-				point_A=lA;

-				point_B=lB;

-				low=ofs;

-			}

-		}

-

-		if (low<best_safe) {

-			best_first=true; //force reset

-			best_safe=low;

-			best_unsafe=hi;

-		}

-

-		if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low<=best_safe))) {

-			closest_A=point_A;

-			closest_B=point_B;

-			r_info->collider_id=col_obj->get_instance_id();

-			r_info->rid=col_obj->get_self();

-			r_info->shape=shape_idx;

-			r_info->point=closest_B;

-			r_info->normal=(closest_A-closest_B).normalized();

-			best_first=false;

-			if (col_obj->get_type()==CollisionObjectSW::TYPE_BODY) {

-				const BodySW *body=static_cast<const BodySW*>(col_obj);

-				r_info->linear_velocity= body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);

-			}

-

-		}

-

-

-	}

-

-	p_closest_safe=best_safe;

-	p_closest_unsafe=best_unsafe;	

-

-	return true;

-}

-

-bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_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_shape_xform.xform(shape->get_aabb());

-	aabb=aabb.grow(p_margin);

-

-	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;

-	r_result_count=0;

-

-	PhysicsServerSW::CollCbkData cbk;

-	cbk.max=p_result_max;

-	cbk.amount=0;

-	cbk.ptr=r_results;

-	CollisionSolverSW::CallbackResult cbkres=NULL;

-

-	PhysicsServerSW::CollCbkData *cbkptr=NULL;

-	if (p_result_max>0) {

-		cbkptr=&cbk;

-		cbkres=PhysicsServerSW::_shape_col_cbk;

-	}

-

-

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

-

-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))

-			continue;

-

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

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

-

-		if (p_exclude.has( col_obj->get_self() )) {

-			continue;

-		}

-

-		//print_line("AGAINST: "+itos(col_obj->get_self().get_id())+":"+itos(shape_idx));

-		//print_line("THE ABBB: "+(col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).xform(col_obj->get_shape(shape_idx)->get_aabb()));

-

-		if (CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),cbkres,cbkptr,NULL,p_margin)) {

-			collided=true;

-		}

-

-	}

-

-	r_result_count=cbk.amount;

-

-	return collided;

-

-}

-

-

-struct _RestCallbackData {

-

-	const CollisionObjectSW *object;

-	const CollisionObjectSW *best_object;

-	int shape;

-	int best_shape;

-	Vector3 best_contact;

-	Vector3 best_normal;

-	float best_len;

-};

-

-static void _rest_cbk_result(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {

-

-

-	_RestCallbackData *rd=(_RestCallbackData*)p_userdata;

-

-	Vector3 contact_rel = p_point_B - p_point_A;

-	float len = contact_rel.length();

-	if (len <= rd->best_len)

-		return;

-

-	rd->best_len=len;

-	rd->best_contact=p_point_B;

-	rd->best_normal=contact_rel/len;

-	rd->best_object=rd->object;

-	rd->best_shape=rd->shape;

-

-}

-bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {

-

-

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

-	ERR_FAIL_COND_V(!shape,0);

-

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

-	aabb=aabb.grow(p_margin);

-

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

-

-	_RestCallbackData rcd;

-	rcd.best_len=0;

-	rcd.best_object=NULL;

-	rcd.best_shape=0;

-

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

-

-

-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))

-			continue;

-

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

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

-

-		if (p_exclude.has( col_obj->get_self() ))

-			continue;

-

-		rcd.object=col_obj;

-		rcd.shape=shape_idx;

-		bool sc = CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),_rest_cbk_result,&rcd,NULL,p_margin);

-		if (!sc)

-			continue;

-

-

-	}

-

-	if (rcd.best_len==0)

-		return false;

-

-	r_info->collider_id=rcd.best_object->get_instance_id();

-	r_info->shape=rcd.best_shape;

-	r_info->normal=rcd.best_normal;

-	r_info->point=rcd.best_contact;

-	r_info->rid=rcd.best_object->get_self();

-	if (rcd.best_object->get_type()==CollisionObjectSW::TYPE_BODY) {

-

-		const BodySW *body = static_cast<const BodySW*>(rcd.best_object);

-		Vector3 rel_vec = r_info->point-body->get_transform().get_origin();

-		r_info->linear_velocity = body->get_linear_velocity() +

-				(body->get_angular_velocity()).cross(body->get_transform().origin-rcd.best_contact);// * mPos);

-

-

-	} else {

-		r_info->linear_velocity=Vector3();

-	}

-

-	return true;

-}

-

-

-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;

-

-	self->collision_pairs++;

-

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

-

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

-		if (type_B==CollisionObjectSW::TYPE_AREA) {

-

-			AreaSW *area_b=static_cast<AreaSW*>(B);

-			Area2PairSW *area2_pair = memnew(Area2PairSW(area_b,p_subindex_B,area,p_subindex_A) );

-			return area2_pair;

-		} else {

-

-			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;

-	self->collision_pairs--;

-	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() {

-

-	contact_debug_count=0;

-	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_threshold=p_value; break;

-		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_threshold=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_threshold;

-		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_threshold;

-		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() {

-

-	collision_pairs=0;

-	active_objects=0;

-	island_count=0;

-	contact_debug_count=0;

-

-	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_threshold=GLOBAL_DEF("physics/sleep_threshold_linear",0.1);

-	body_angular_velocity_sleep_threshold=GLOBAL_DEF("physics/sleep_threshold_angular", (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 );

-}

-

-

-

+/*************************************************************************/
+/*  space_sw.cpp                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "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 "globals.h"
+#include "space_sw.h"
+#include "collision_solver_sw.h"
+#include "physics_server_sw.h"
+
+
+_FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) {
+
+	if ((p_object->get_layer_mask()&p_layer_mask)==0)
+		return false;
+
+	if (p_object->get_type()==CollisionObjectSW::TYPE_AREA && !(p_type_mask&PhysicsDirectSpaceState::TYPE_MASK_AREA))
+		return false;
+
+	BodySW *body = static_cast<BodySW*>(p_object);
+
+	return (1<<body->get_mode())&p_type_mask;
+
+}
+
+
+bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_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 (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		if (!(static_cast<CollisionObjectSW*>(space->intersection_query_results[i])->is_ray_pickable()))
+			continue;
+
+		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);
+	else
+		r_result.collider=NULL;
+	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,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_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 (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		//area cant be picked by ray (default)
+
+		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,p_margin,0))
+			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);
+		else
+			r_results[cc].collider=NULL;
+		r_results[cc].rid=col_obj->get_self();
+		r_results[cc].shape=shape_idx;
+
+		cc++;
+
+	}
+
+	return cc;
+
+}
+
+
+bool PhysicsDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,ShapeRestInfo *r_info) {
+
+
+
+	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,false);
+
+	AABB aabb = p_xform.xform(shape->get_aabb());
+	aabb=aabb.merge(AABB(aabb.pos+p_motion,aabb.size)); //motion
+	aabb=aabb.grow(p_margin);
+
+	//if (p_motion!=Vector3())
+	//	print_line(p_motion);
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	float best_safe=1;
+	float best_unsafe=1;
+
+	Transform xform_inv = p_xform.affine_inverse();
+	MotionShapeSW mshape;
+	mshape.shape=shape;
+	mshape.motion=xform_inv.basis.xform(p_motion);
+
+	bool best_first=true;
+
+	Vector3 closest_A,closest_B;
+
+	for(int i=0;i<amount;i++) {
+
+
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+			continue; //ignore excluded
+
+
+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		Vector3 point_A,point_B;
+		Vector3 sep_axis=p_motion.normalized();
+
+		Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+		//test initial overlap, does it collide if going all the way?
+		if (CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {
+			//print_line("failed motion cast (no collision)");
+			continue;
+		}
+
+
+		//test initial overlap
+#if 0
+		if (CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,NULL,NULL,&sep_axis)) {
+			print_line("failed initial cast (collision at begining)");
+			return false;
+		}
+#else
+		sep_axis=p_motion.normalized();
+
+		if (!CollisionSolverSW::solve_distance(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {
+			//print_line("failed motion cast (no collision)");
+			return false;
+		}
+#endif
+
+
+		//just do kinematic solving
+		float low=0;
+		float hi=1;
+		Vector3 mnormal=p_motion.normalized();
+
+		for(int i=0;i<8;i++) { //steps should be customizable..
+
+			Transform xfa = p_xform;
+			float ofs = (low+hi)*0.5;
+
+			Vector3 sep=mnormal; //important optimization for this to work fast enough
+
+			mshape.motion=xform_inv.basis.xform(p_motion*ofs);
+
+			Vector3 lA,lB;
+
+			bool collided = !CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,lA,lB,aabb,&sep);
+
+			if (collided) {
+
+				//print_line(itos(i)+": "+rtos(ofs));
+				hi=ofs;
+			} else {
+
+				point_A=lA;
+				point_B=lB;
+				low=ofs;
+			}
+		}
+
+		if (low<best_safe) {
+			best_first=true; //force reset
+			best_safe=low;
+			best_unsafe=hi;
+		}
+
+		if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low<=best_safe))) {
+			closest_A=point_A;
+			closest_B=point_B;
+			r_info->collider_id=col_obj->get_instance_id();
+			r_info->rid=col_obj->get_self();
+			r_info->shape=shape_idx;
+			r_info->point=closest_B;
+			r_info->normal=(closest_A-closest_B).normalized();
+			best_first=false;
+			if (col_obj->get_type()==CollisionObjectSW::TYPE_BODY) {
+				const BodySW *body=static_cast<const BodySW*>(col_obj);
+				r_info->linear_velocity= body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
+			}
+
+		}
+
+
+	}
+
+	p_closest_safe=best_safe;
+	p_closest_unsafe=best_unsafe;	
+
+	return true;
+}
+
+bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_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_shape_xform.xform(shape->get_aabb());
+	aabb=aabb.grow(p_margin);
+
+	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;
+	r_result_count=0;
+
+	PhysicsServerSW::CollCbkData cbk;
+	cbk.max=p_result_max;
+	cbk.amount=0;
+	cbk.ptr=r_results;
+	CollisionSolverSW::CallbackResult cbkres=NULL;
+
+	PhysicsServerSW::CollCbkData *cbkptr=NULL;
+	if (p_result_max>0) {
+		cbkptr=&cbk;
+		cbkres=PhysicsServerSW::_shape_col_cbk;
+	}
+
+
+	for(int i=0;i<amount;i++) {
+
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		if (p_exclude.has( col_obj->get_self() )) {
+			continue;
+		}
+
+		//print_line("AGAINST: "+itos(col_obj->get_self().get_id())+":"+itos(shape_idx));
+		//print_line("THE ABBB: "+(col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).xform(col_obj->get_shape(shape_idx)->get_aabb()));
+
+		if (CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),cbkres,cbkptr,NULL,p_margin)) {
+			collided=true;
+		}
+
+	}
+
+	r_result_count=cbk.amount;
+
+	return collided;
+
+}
+
+
+struct _RestCallbackData {
+
+	const CollisionObjectSW *object;
+	const CollisionObjectSW *best_object;
+	int shape;
+	int best_shape;
+	Vector3 best_contact;
+	Vector3 best_normal;
+	float best_len;
+};
+
+static void _rest_cbk_result(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
+
+
+	_RestCallbackData *rd=(_RestCallbackData*)p_userdata;
+
+	Vector3 contact_rel = p_point_B - p_point_A;
+	float len = contact_rel.length();
+	if (len <= rd->best_len)
+		return;
+
+	rd->best_len=len;
+	rd->best_contact=p_point_B;
+	rd->best_normal=contact_rel/len;
+	rd->best_object=rd->object;
+	rd->best_shape=rd->shape;
+
+}
+bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
+
+
+	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,0);
+
+	AABB aabb = p_shape_xform.xform(shape->get_aabb());
+	aabb=aabb.grow(p_margin);
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	_RestCallbackData rcd;
+	rcd.best_len=0;
+	rcd.best_object=NULL;
+	rcd.best_shape=0;
+
+	for(int i=0;i<amount;i++) {
+
+
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		if (p_exclude.has( col_obj->get_self() ))
+			continue;
+
+		rcd.object=col_obj;
+		rcd.shape=shape_idx;
+		bool sc = CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),_rest_cbk_result,&rcd,NULL,p_margin);
+		if (!sc)
+			continue;
+
+
+	}
+
+	if (rcd.best_len==0)
+		return false;
+
+	r_info->collider_id=rcd.best_object->get_instance_id();
+	r_info->shape=rcd.best_shape;
+	r_info->normal=rcd.best_normal;
+	r_info->point=rcd.best_contact;
+	r_info->rid=rcd.best_object->get_self();
+	if (rcd.best_object->get_type()==CollisionObjectSW::TYPE_BODY) {
+
+		const BodySW *body = static_cast<const BodySW*>(rcd.best_object);
+		Vector3 rel_vec = r_info->point-body->get_transform().get_origin();
+		r_info->linear_velocity = body->get_linear_velocity() +
+				(body->get_angular_velocity()).cross(body->get_transform().origin-rcd.best_contact);// * mPos);
+
+
+	} else {
+		r_info->linear_velocity=Vector3();
+	}
+
+	return true;
+}
+
+
+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;
+
+	self->collision_pairs++;
+
+	if (type_A==CollisionObjectSW::TYPE_AREA) {
+
+		AreaSW *area=static_cast<AreaSW*>(A);
+		if (type_B==CollisionObjectSW::TYPE_AREA) {
+
+			AreaSW *area_b=static_cast<AreaSW*>(B);
+			Area2PairSW *area2_pair = memnew(Area2PairSW(area_b,p_subindex_B,area,p_subindex_A) );
+			return area2_pair;
+		} else {
+
+			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;
+	self->collision_pairs--;
+	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() {
+
+	contact_debug_count=0;
+	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_threshold=p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_threshold=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_threshold;
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_threshold;
+		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() {
+
+	collision_pairs=0;
+	active_objects=0;
+	island_count=0;
+	contact_debug_count=0;
+
+	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_threshold=GLOBAL_DEF("physics/sleep_threshold_linear",0.1);
+	body_angular_velocity_sleep_threshold=GLOBAL_DEF("physics/sleep_threshold_angular", (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
index e88f61d881..ac788ba93f 100644
--- a/servers/physics/space_sw.h
+++ b/servers/physics/space_sw.h
@@ -1,187 +1,187 @@
-/*************************************************************************/

-/*  space_sw.h                                                           */

-/*************************************************************************/

-/*                       This file is part of:                           */

-/*                           GODOT ENGINE                                */

-/*                    http://www.godotengine.org                         */

-/*************************************************************************/

-/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur.                 */

-/*                                                                       */

-/* Permission is hereby granted, free of charge, to any person obtaining */

-/* a copy of this software and associated documentation files (the       */

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

-

-	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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);

-	virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);

-	virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,ShapeRestInfo *r_info=NULL);

-	virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);

-	virtual bool rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);

-

-	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_threshold;

-	float body_angular_velocity_sleep_threshold;

-	float body_time_to_sleep;

-	float body_angular_velocity_damp_ratio;

-

-	bool locked;

-

-	int island_count;

-	int active_objects;

-	int collision_pairs;

-

-	RID static_global_body;

-

-	Vector<Vector3> contact_debug;

-	int contact_debug_count;

-

-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_threshold; }

-	_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_threshold; }

-	_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;

-

-	void set_island_count(int p_island_count) { island_count=p_island_count; }

-	int get_island_count() const { return island_count; }

-

-	void set_active_objects(int p_active_objects) { active_objects=p_active_objects; }

-	int get_active_objects() const { return active_objects; }

-

-	int get_collision_pairs() const { return collision_pairs; }

-

-	PhysicsDirectSpaceStateSW *get_direct_state();

-

-	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }

-	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); }

-	_FORCE_INLINE_ void add_debug_contact(const Vector3& p_contact) { if (contact_debug_count<contact_debug.size()) contact_debug[contact_debug_count++]=p_contact; }

-	_FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; }

-	_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }

-

-	void set_static_global_body(RID p_body) { static_global_body=p_body; }

-	RID get_static_global_body() { return static_global_body; }

-

-

-	SpaceSW();

-	~SpaceSW();

-};

-

-

-#endif // SPACE__SW_H

+/*************************************************************************/
+/*  space_sw.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2015 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "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;
+
+	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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,ShapeRestInfo *r_info=NULL);
+	virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual bool rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+
+	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_threshold;
+	float body_angular_velocity_sleep_threshold;
+	float body_time_to_sleep;
+	float body_angular_velocity_damp_ratio;
+
+	bool locked;
+
+	int island_count;
+	int active_objects;
+	int collision_pairs;
+
+	RID static_global_body;
+
+	Vector<Vector3> contact_debug;
+	int contact_debug_count;
+
+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_threshold; }
+	_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_threshold; }
+	_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;
+
+	void set_island_count(int p_island_count) { island_count=p_island_count; }
+	int get_island_count() const { return island_count; }
+
+	void set_active_objects(int p_active_objects) { active_objects=p_active_objects; }
+	int get_active_objects() const { return active_objects; }
+
+	int get_collision_pairs() const { return collision_pairs; }
+
+	PhysicsDirectSpaceStateSW *get_direct_state();
+
+	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }
+	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); }
+	_FORCE_INLINE_ void add_debug_contact(const Vector3& p_contact) { if (contact_debug_count<contact_debug.size()) contact_debug[contact_debug_count++]=p_contact; }
+	_FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; }
+	_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
+
+	void set_static_global_body(RID p_body) { static_global_body=p_body; }
+	RID get_static_global_body() { return static_global_body; }
+
+
+	SpaceSW();
+	~SpaceSW();
+};
+
+
+#endif // SPACE__SW_H