30 files changed, 1603 insertions, 932 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_rb_resampler.cpp b/servers/audio/audio_rb_resampler.cpp
new file mode 100644
index 0000000000..d07d55f1b5
--- /dev/null
+++ b/servers/audio/audio_rb_resampler.cpp
@@ -0,0 +1,356 @@
+#include "audio_rb_resampler.h"
+
+
+int AudioRBResampler::get_channel_count() const {
+
+	if (!rb)
+		return 0;
+
+	return channels;
+}
+
+
+template<int C>
+uint32_t AudioRBResampler::_resample(int32_t *p_dest,int p_todo,int32_t p_increment) {
+
+	uint32_t read=offset&MIX_FRAC_MASK;
+
+	for (int i=0;i<p_todo;i++) {
+
+		offset = (offset + p_increment)&(((1<<(rb_bits+MIX_FRAC_BITS))-1));
+		read+=p_increment;
+		uint32_t pos = offset >> MIX_FRAC_BITS;
+		uint32_t frac = offset & MIX_FRAC_MASK;
+#ifndef FAST_AUDIO
+		ERR_FAIL_COND_V(pos>=rb_len,0);
+#endif
+		uint32_t pos_next = (pos+1)&rb_mask;
+		//printf("rb pos %i\n",pos);
+
+		// since this is a template with a known compile time value (C), conditionals go away when compiling.
+		if (C==1) {
+
+			int32_t v0 = rb[pos];
+			int32_t v0n=rb[pos_next];
+#ifndef FAST_AUDIO
+			v0+=(v0n-v0)*(int32_t)frac >> MIX_FRAC_BITS;
+#endif
+			v0<<=16;
+			p_dest[i]=v0;
+
+		}
+		if (C==2) {
+
+			int32_t v0 = rb[(pos<<1)+0];
+			int32_t v1 = rb[(pos<<1)+1];
+			int32_t v0n=rb[(pos_next<<1)+0];
+			int32_t v1n=rb[(pos_next<<1)+1];
+
+#ifndef FAST_AUDIO
+			v0+=(v0n-v0)*(int32_t)frac >> MIX_FRAC_BITS;
+			v1+=(v1n-v1)*(int32_t)frac >> MIX_FRAC_BITS;
+#endif
+			v0<<=16;
+			v1<<=16;
+			p_dest[(i<<1)+0]=v0;
+			p_dest[(i<<1)+1]=v1;
+
+		}
+
+		if (C==4) {
+
+			int32_t v0 = rb[(pos<<2)+0];
+			int32_t v1 = rb[(pos<<2)+1];
+			int32_t v2 = rb[(pos<<2)+2];
+			int32_t v3 = rb[(pos<<2)+3];
+			int32_t v0n = rb[(pos_next<<2)+0];
+			int32_t v1n=rb[(pos_next<<2)+1];
+			int32_t v2n=rb[(pos_next<<2)+2];
+			int32_t v3n=rb[(pos_next<<2)+3];
+
+#ifndef FAST_AUDIO
+			v0+=(v0n-v0)*(int32_t)frac >> MIX_FRAC_BITS;
+			v1+=(v1n-v1)*(int32_t)frac >> MIX_FRAC_BITS;
+			v2+=(v2n-v2)*(int32_t)frac >> MIX_FRAC_BITS;
+			v3+=(v3n-v3)*(int32_t)frac >> MIX_FRAC_BITS;
+#endif
+			v0<<=16;
+			v1<<=16;
+			v2<<=16;
+			v3<<=16;
+			p_dest[(i<<2)+0]=v0;
+			p_dest[(i<<2)+1]=v1;
+			p_dest[(i<<2)+2]=v2;
+			p_dest[(i<<2)+3]=v3;
+
+		}
+
+		if (C==6) {
+
+			int32_t v0 = rb[(pos*6)+0];
+			int32_t v1 = rb[(pos*6)+1];
+			int32_t v2 = rb[(pos*6)+2];
+			int32_t v3 = rb[(pos*6)+3];
+			int32_t v4 = rb[(pos*6)+4];
+			int32_t v5 = rb[(pos*6)+5];
+			int32_t v0n = rb[(pos_next*6)+0];
+			int32_t v1n=rb[(pos_next*6)+1];
+			int32_t v2n=rb[(pos_next*6)+2];
+			int32_t v3n=rb[(pos_next*6)+3];
+			int32_t v4n=rb[(pos_next*6)+4];
+			int32_t v5n=rb[(pos_next*6)+5];
+
+#ifndef FAST_AUDIO
+			v0+=(v0n-v0)*(int32_t)frac >> MIX_FRAC_BITS;
+			v1+=(v1n-v1)*(int32_t)frac >> MIX_FRAC_BITS;
+			v2+=(v2n-v2)*(int32_t)frac >> MIX_FRAC_BITS;
+			v3+=(v3n-v3)*(int32_t)frac >> MIX_FRAC_BITS;
+			v4+=(v4n-v4)*(int32_t)frac >> MIX_FRAC_BITS;
+			v5+=(v5n-v5)*(int32_t)frac >> MIX_FRAC_BITS;
+#endif
+			v0<<=16;
+			v1<<=16;
+			v2<<=16;
+			v3<<=16;
+			v4<<=16;
+			v5<<=16;
+			p_dest[(i*6)+0]=v0;
+			p_dest[(i*6)+1]=v1;
+			p_dest[(i*6)+2]=v2;
+			p_dest[(i*6)+3]=v3;
+			p_dest[(i*6)+4]=v4;
+			p_dest[(i*6)+5]=v5;
+
+		}
+
+
+	}
+
+
+	return read>>MIX_FRAC_BITS;//rb_read_pos=offset>>MIX_FRAC_BITS;
+
+}
+
+
+bool AudioRBResampler::mix(int32_t *p_dest, int p_frames) {
+
+
+	if (!rb)
+		return false;
+
+	int write_pos_cache=rb_write_pos;
+
+	int32_t increment=(src_mix_rate*MIX_FRAC_LEN)/target_mix_rate;
+
+	int rb_todo;
+
+	if (write_pos_cache==rb_read_pos) {
+		return false; //out of buffer
+
+	} else if (rb_read_pos<write_pos_cache) {
+
+		rb_todo=write_pos_cache-rb_read_pos; //-1?
+	} else {
+
+		rb_todo=(rb_len-rb_read_pos)+write_pos_cache; //-1?
+	}
+
+	int todo = MIN( ((int64_t(rb_todo)<<MIX_FRAC_BITS)/increment)+1, p_frames );
+#if 0
+	if (int(src_mix_rate)==target_mix_rate) {
+
+
+		if (channels==6) {
+
+			for(int i=0;i<p_frames;i++) {
+
+				int from = ((rb_read_pos+i)&rb_mask)*6;
+				int to = i*6;
+
+				p_dest[from+0]=int32_t(rb[to+0])<<16;
+				p_dest[from+1]=int32_t(rb[to+1])<<16;
+				p_dest[from+2]=int32_t(rb[to+2])<<16;
+				p_dest[from+3]=int32_t(rb[to+3])<<16;
+				p_dest[from+4]=int32_t(rb[to+4])<<16;
+				p_dest[from+5]=int32_t(rb[to+5])<<16;
+			}
+
+		} else {
+			int len=p_frames*channels;
+			int from=rb_read_pos*channels;
+			int mask=0;
+			switch(channels) {
+				case 1: mask=rb_len-1; break;
+				case 2: mask=(rb_len*2)-1; break;
+				case 4: mask=(rb_len*4)-1; break;
+			}
+
+			for(int i=0;i<len;i++) {
+
+				p_dest[i]=int32_t(rb[(from+i)&mask])<<16;
+			}
+		}
+
+		rb_read_pos = (rb_read_pos+p_frames)&rb_mask;
+	} else
+#endif
+	{
+
+		uint32_t read=0;
+		switch(channels) {
+			case 1: read=_resample<1>(p_dest,todo,increment); break;
+			case 2: read=_resample<2>(p_dest,todo,increment); break;
+			case 4: read=_resample<4>(p_dest,todo,increment); break;
+			case 6: read=_resample<6>(p_dest,todo,increment); break;
+		}
+#if 1
+		//end of stream, fadeout
+		int remaining = p_frames-todo;
+		if (remaining && todo>0) {
+
+			//print_line("fadeout");
+			for(int c=0;c<channels;c++) {
+
+				for(int i=0;i<todo;i++) {
+
+					int32_t samp = p_dest[i*channels+c]>>8;
+					uint32_t mul = (todo-i) * 256 /todo;
+					//print_line("mul: "+itos(i)+" "+itos(mul));
+					p_dest[i*channels+c]=samp*mul;
+				}
+
+			}
+
+		}
+
+#else
+		int remaining = p_frames-todo;
+		if (remaining && todo>0) {
+
+
+			for(int c=0;c<channels;c++) {
+
+				int32_t from = p_dest[(todo-1)*channels+c]>>8;
+
+				for(int i=0;i<remaining;i++) {
+
+					uint32_t mul = (remaining-i) * 256 /remaining;
+					p_dest[(todo+i)*channels+c]=from*mul;
+				}
+
+			}
+
+		}
+#endif
+
+		//zero out what remains there to avoid glitches
+		for(int i=todo*channels;i<int(p_frames)*channels;i++) {
+
+			p_dest[i]=0;
+		}
+
+		if (read>rb_todo)
+			read=rb_todo;
+
+		rb_read_pos = (rb_read_pos+read)&rb_mask;
+
+
+
+
+	}
+
+	return true;
+}
+
+
+Error AudioRBResampler::setup(int p_channels,int p_src_mix_rate,int p_target_mix_rate,int p_buffer_msec,int p_minbuff_needed) {
+
+	ERR_FAIL_COND_V(p_channels!=1 && p_channels!=2 && p_channels!=4 && p_channels!=6,ERR_INVALID_PARAMETER);
+
+
+	//float buffering_sec = int(GLOBAL_DEF("audio/stream_buffering_ms",500))/1000.0;
+	int desired_rb_bits =nearest_shift(MAX((p_buffer_msec/1000.0)*p_src_mix_rate,p_minbuff_needed));
+
+	bool recreate=!rb;
+
+	if (rb && (uint32_t(desired_rb_bits)!=rb_bits || channels!=uint32_t(p_channels))) {
+		//recreate
+
+		memdelete_arr(rb);
+		memdelete_arr(read_buf);
+		recreate=true;
+
+	}
+
+	if (recreate) {
+
+		channels=p_channels;
+		rb_bits=desired_rb_bits;
+		rb_len=(1<<rb_bits);
+		rb_mask=rb_len-1;
+		rb = memnew_arr( int16_t, rb_len * p_channels );
+		read_buf = memnew_arr( int16_t, rb_len * p_channels );
+
+	}
+
+	src_mix_rate=p_src_mix_rate;
+	target_mix_rate=p_target_mix_rate;
+	offset=0;
+	rb_read_pos=0;
+	rb_write_pos=0;
+
+	//avoid maybe strange noises upon load
+	for (int i=0;i<(rb_len*channels);i++) {
+
+		rb[i]=0;
+		read_buf[i]=0;
+	}
+
+	return OK;
+
+}
+
+void AudioRBResampler::clear() {
+
+	if (!rb)
+		return;
+
+	//should be stopped at this point but just in case
+	if (rb) {
+		memdelete_arr(rb);
+		memdelete_arr(read_buf);
+	}
+	rb=NULL;
+	offset=0;
+	rb_read_pos=0;
+	rb_write_pos=0;
+	read_buf=NULL;
+}
+
+AudioRBResampler::AudioRBResampler() {
+
+	rb=NULL;
+	offset=0;
+	read_buf=NULL;
+	rb_read_pos=0;
+	rb_write_pos=0;
+
+	rb_bits=0;
+	rb_len=0;
+	rb_mask=0;
+	read_buff_len=0;
+	channels=0;
+	src_mix_rate=0;
+	target_mix_rate=0;
+
+}
+
+AudioRBResampler::~AudioRBResampler() {
+
+	if (rb) {
+		memdelete_arr(rb);
+		memdelete_arr(read_buf);
+	}
+
+}
+
diff --git a/servers/audio/audio_rb_resampler.h b/servers/audio/audio_rb_resampler.h
new file mode 100644
index 0000000000..3c686374f4
--- /dev/null
+++ b/servers/audio/audio_rb_resampler.h
@@ -0,0 +1,133 @@
+#ifndef AUDIO_RB_RESAMPLER_H
+#define AUDIO_RB_RESAMPLER_H
+
+#include "typedefs.h"
+#include "os/memory.h"
+
+struct AudioRBResampler {
+
+	uint32_t rb_bits;
+	uint32_t rb_len;
+	uint32_t rb_mask;
+	uint32_t read_buff_len;
+	uint32_t channels;
+	uint32_t src_mix_rate;
+	uint32_t target_mix_rate;
+
+	volatile int rb_read_pos;
+	volatile int rb_write_pos;
+
+	int32_t offset; //contains the fractional remainder of the resampler
+	enum {
+		MIX_FRAC_BITS=13,
+		MIX_FRAC_LEN=(1<<MIX_FRAC_BITS),
+		MIX_FRAC_MASK=MIX_FRAC_LEN-1,
+	};
+
+	int16_t *read_buf;
+	int16_t *rb;
+
+
+	template<int C>
+	uint32_t _resample(int32_t *p_dest,int p_todo,int32_t p_increment);
+
+
+public:
+
+	_FORCE_INLINE_ void flush() {
+		rb_read_pos=0;
+		rb_write_pos=0;
+	}
+
+	_FORCE_INLINE_ bool is_ready() const{
+		return rb!=NULL;
+	}
+
+
+	_FORCE_INLINE_ int get_total() const {
+
+		return rb_len-1;
+	}
+
+	_FORCE_INLINE_ int get_todo() const { //return amount of frames to mix
+
+		int todo;
+		int read_pos_cache=rb_read_pos;
+
+		if (read_pos_cache==rb_write_pos) {
+			todo=rb_len-1;
+		} else if (read_pos_cache>rb_write_pos) {
+
+			todo=read_pos_cache-rb_write_pos-1;
+		} else {
+
+			todo=(rb_len-rb_write_pos)+read_pos_cache-1;
+		}
+
+		return todo;
+	}
+
+	_FORCE_INLINE_ int16_t *get_write_buffer() { return read_buf; }
+	_FORCE_INLINE_ void write(uint32_t p_frames) {
+
+		ERR_FAIL_COND(p_frames >= rb_len);
+
+		switch(channels) {
+			case 1: {
+
+				for(uint32_t i=0;i<p_frames;i++) {
+
+					rb[ rb_write_pos ] = read_buf[i];
+					rb_write_pos=(rb_write_pos+1)&rb_mask;
+				}
+			} break;
+			case 2: {
+
+				for(uint32_t i=0;i<p_frames;i++) {
+
+					rb[ (rb_write_pos<<1)+0 ] = read_buf[(i<<1)+0];
+					rb[ (rb_write_pos<<1)+1 ] = read_buf[(i<<1)+1];
+					rb_write_pos=(rb_write_pos+1)&rb_mask;
+				}
+			} break;
+			case 4: {
+
+				for(uint32_t i=0;i<p_frames;i++) {
+
+					rb[ (rb_write_pos<<2)+0 ] = read_buf[(i<<2)+0];
+					rb[ (rb_write_pos<<2)+1 ] = read_buf[(i<<2)+1];
+					rb[ (rb_write_pos<<2)+2 ] = read_buf[(i<<2)+2];
+					rb[ (rb_write_pos<<2)+3 ] = read_buf[(i<<2)+3];
+					rb_write_pos=(rb_write_pos+1)&rb_mask;
+				}
+			} break;
+			case 6: {
+
+				for(uint32_t i=0;i<p_frames;i++) {
+
+					rb[ (rb_write_pos*6)+0 ] = read_buf[(i*6)+0];
+					rb[ (rb_write_pos*6)+1 ] = read_buf[(i*6)+1];
+					rb[ (rb_write_pos*6)+2 ] = read_buf[(i*6)+2];
+					rb[ (rb_write_pos*6)+3 ] = read_buf[(i*6)+3];
+					rb[ (rb_write_pos*6)+4 ] = read_buf[(i*6)+4];
+					rb[ (rb_write_pos*6)+5 ] = read_buf[(i*6)+5];
+					rb_write_pos=(rb_write_pos+1)&rb_mask;
+				}
+			} break;
+
+
+		}
+
+	}
+
+	int get_channel_count() const;
+
+	Error setup(int p_channels, int p_src_mix_rate, int p_target_mix_rate, int p_buffer_msec, int p_minbuff_needed=-1);
+	void clear();
+	bool mix(int32_t *p_dest, int p_frames);
+
+	AudioRBResampler();
+	~AudioRBResampler();
+};
+
+#endif // AUDIO_RB_RESAMPLER_H
diff --git a/servers/audio/audio_server_sw.cpp b/servers/audio/audio_server_sw.cpp
index 8a3ab7ce70..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);
 
 }
@@ -830,10 +830,14 @@ void AudioServerSW::finish() {
 void AudioServerSW::_update_streams(bool p_thread) {
 
 	_THREAD_SAFE_METHOD_
-	for(List<Stream*>::Element *E=active_audio_streams.front();E;E=E->next()) {
+	for(List<Stream*>::Element *E=active_audio_streams.front();E;) { //stream might be removed durnig this callback
+
+		List<Stream*>::Element *N=E->next();
 
 		if (E->get()->audio_stream && p_thread == E->get()->audio_stream->can_update_mt())
 			E->get()->audio_stream->update();
+
+		E=N;
 	}
 
 }
@@ -916,7 +920,7 @@ float AudioServerSW::get_event_voice_global_volume_scale() const {
 
 double AudioServerSW::get_output_delay() const {
 
-	return _output_delay;
+	return _output_delay+AudioDriverSW::get_singleton()->get_latency();
 }
 
 double AudioServerSW::get_mix_time() const {
diff --git a/servers/audio/audio_server_sw.h b/servers/audio/audio_server_sw.h
index 77d2f2e8dd..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 );
@@ -256,6 +256,8 @@ public:
 	virtual void unlock()=0;
 	virtual void finish()=0;
 
+	virtual float get_latency() { return 0; }
+
 
 
 
diff --git a/servers/audio_server.cpp b/servers/audio_server.cpp
index 7c4f8b185a..c155f5204a 100644
--- a/servers/audio_server.cpp
+++ b/servers/audio_server.cpp
@@ -164,6 +164,7 @@ void AudioServer::_bind_methods() {
 	BIND_CONSTANT( REVERB_HALL );
 
 	GLOBAL_DEF("audio/stream_buffering_ms",500);
+	GLOBAL_DEF("audio/video_delay_compensation_ms",300);
 
 }
 
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/area_sw.cpp b/servers/physics/area_sw.cpp
index 01dfdd633f..e78f0699cb 100644
--- a/servers/physics/area_sw.cpp
+++ b/servers/physics/area_sw.cpp
@@ -83,6 +83,10 @@ void AreaSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
 
 	_shape_changed();
 
+	if (!moved_list.in_list() && get_space())
+		get_space()->area_add_to_moved_list(&moved_list);
+
+
 }
 
 void AreaSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method) {
@@ -103,6 +107,10 @@ void AreaSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method
 
 	_shape_changed();
 
+	if (!moved_list.in_list() && get_space())
+		get_space()->area_add_to_moved_list(&moved_list);
+
+
 }
 
 
diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h
index 0c5295d42a..40ccdaf370 100644
--- a/servers/physics/area_sw.h
+++ b/servers/physics/area_sw.h
@@ -206,6 +206,7 @@ void AreaSW::remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape,uint32
 	monitored_areas[bk].dec();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
+
 }
 
 
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index e2b2fa2051..da4c1b48d8 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -299,6 +299,16 @@ bool BodyPairSW::setup(float p_step) {
 
 		c.active=true;
 
+#ifdef DEBUG_ENABLED
+
+
+		if (space->is_debugging_contacts()) {
+			space->add_debug_contact(global_A+offset_A);
+			space->add_debug_contact(global_B+offset_A);
+		}
+#endif
+
+
 		int gather_A = A->can_report_contacts();
 		int gather_B = B->can_report_contacts();
 
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 521ffae0ea..a9a8042c19 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -201,6 +201,30 @@ PhysicsDirectSpaceState* PhysicsServerSW::space_get_direct_state(RID p_space) {
 	return space->get_direct_state();
 }
 
+void PhysicsServerSW::space_set_debug_contacts(RID p_space,int p_max_contacts) {
+
+	SpaceSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+	space->set_debug_contacts(p_max_contacts);
+
+}
+
+Vector<Vector3> PhysicsServerSW::space_get_contacts(RID p_space) const {
+
+	SpaceSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,Vector<Vector3>());
+	return space->get_debug_contacts();
+
+}
+
+int PhysicsServerSW::space_get_contact_count(RID p_space) const {
+
+	SpaceSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,0);
+	return space->get_debug_contact_count();
+
+}
+
 RID PhysicsServerSW::area_create() {
 
 	AreaSW *area = memnew( AreaSW );
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index 80007b8499..abbb057616 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -94,6 +94,9 @@ public:
 	// this function only works on fixed process, errors and returns null otherwise
 	virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space);
 
+	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts);
+	virtual Vector<Vector3> space_get_contacts(RID p_space) const;
+	virtual int space_get_contact_count(RID p_space) const;
 
 	/* AREA API */
 
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index d329a10f04..d36b004989 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -1,744 +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() {

-

-

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

-

-	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 16f5ad3c81..ac788ba93f 100644
--- a/servers/physics/space_sw.h
+++ b/servers/physics/space_sw.h
@@ -1,179 +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;

-

-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_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
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp
index 1a41cda482..a5a132020a 100644
--- a/servers/physics_2d/area_2d_sw.cpp
+++ b/servers/physics_2d/area_2d_sw.cpp
@@ -82,6 +82,10 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
 
 	_shape_changed();
 
+	if (!moved_list.in_list() && get_space())
+		get_space()->area_add_to_moved_list(&moved_list);
+
+
 }
 
 void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method) {
@@ -102,6 +106,10 @@ void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_meth
 
 	_shape_changed();
 
+	if (!moved_list.in_list() && get_space())
+		get_space()->area_add_to_moved_list(&moved_list);
+
+
 }
 
 
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index 6bfed134e6..eb3abbb267 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -379,7 +379,12 @@ bool BodyPair2DSW::setup(float p_step) {
 		}
 
 		c.active=true;
-
+#ifdef DEBUG_ENABLED
+		if (space->is_debugging_contacts()) {
+			space->add_debug_contact(global_A+offset_A);
+			space->add_debug_contact(global_B+offset_A);
+		}
+#endif
 		int gather_A = A->can_report_contacts();
 		int gather_B = B->can_report_contacts();
 
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index b446f4928a..14b4c09ebc 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -257,6 +257,30 @@ real_t Physics2DServerSW::space_get_param(RID p_space,SpaceParameter p_param) co
 	return space->get_param(p_param);
 }
 
+void Physics2DServerSW::space_set_debug_contacts(RID p_space,int p_max_contacts) {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+	space->set_debug_contacts(p_max_contacts);
+
+}
+
+Vector<Vector2> Physics2DServerSW::space_get_contacts(RID p_space) const {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,Vector<Vector2>());
+	return space->get_debug_contacts();
+
+}
+
+int Physics2DServerSW::space_get_contact_count(RID p_space) const {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,0);
+	return space->get_debug_contact_count();
+
+}
+
 Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space) {
 
 	Space2DSW *space = space_owner.get(p_space);
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index 6e875701b8..605e04ead8 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -102,6 +102,11 @@ public:
 	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;
 
+	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts);
+	virtual Vector<Vector2> space_get_contacts(RID p_space) const;
+	virtual int space_get_contact_count(RID p_space) const;
+
+
 	// this function only works on fixed process, errors and returns null otherwise
 	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space);
 
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index 0ddc8f16ec..54af3eeb99 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -94,6 +94,22 @@ public:
 		return physics_2d_server->space_get_direct_state(p_space);
 	}
 
+	FUNC2(space_set_debug_contacts,RID,int);
+	virtual Vector<Vector2> space_get_contacts(RID p_space) const {
+
+		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),Vector<Vector2>());
+		return physics_2d_server->space_get_contacts(p_space);
+
+	}
+
+	virtual int space_get_contact_count(RID p_space) const {
+
+		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),0);
+		return physics_2d_server->space_get_contact_count(p_space);
+
+	}
+
+
 
 	/* AREA API */
 
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 767ad9038d..a71e6c4bf5 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -1230,6 +1230,7 @@ void Space2DSW::call_queries() {
 
 void Space2DSW::setup() {
 
+	contact_debug_count=0;
 
 	while(inertia_update_list.first()) {
 		inertia_update_list.first()->self()->update_inertias();
@@ -1302,6 +1303,8 @@ Space2DSW::Space2DSW() {
 	active_objects=0;
 	island_count=0;
 
+	contact_debug_count=0;
+
 	locked=false;
 	contact_recycle_radius=0.01;
 	contact_max_separation=0.05;
@@ -1320,6 +1323,10 @@ Space2DSW::Space2DSW() {
 
 	direct_access = memnew( Physics2DDirectSpaceStateSW );
 	direct_access->space=this;
+
+
+
+
 }
 
 Space2DSW::~Space2DSW() {
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index abee8628fc..97ad3d7f80 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -103,6 +103,9 @@ class Space2DSW {
 
 	int _cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb);
 
+	Vector<Vector2> contact_debug;
+	int contact_debug_count;
+
 friend class Physics2DDirectSpaceStateSW;
 
 public:
@@ -169,6 +172,14 @@ public:
 
 	bool test_body_motion(Body2DSW *p_body, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result);
 
+
+	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 Vector2& p_contact) { if (contact_debug_count<contact_debug.size()) contact_debug[contact_debug_count++]=p_contact; }
+	_FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; }
+	_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
+
+
 	Physics2DDirectSpaceStateSW *get_direct_state();
 
 	Space2DSW();
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
index b24496880e..2d70337dc8 100644
--- a/servers/physics_2d_server.h
+++ b/servers/physics_2d_server.h
@@ -293,6 +293,9 @@ public:
 	// this function only works on fixed process, errors and returns null otherwise
 	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space)=0;
 
+	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts)=0;
+	virtual Vector<Vector2> space_get_contacts(RID p_space) const=0;
+	virtual int space_get_contact_count(RID p_space) const=0;
 
 	//missing space parameters
 
diff --git a/servers/physics_server.h b/servers/physics_server.h
index b82d4cf5da..75584966bb 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -286,6 +286,9 @@ public:
 	// this function only works on fixed process, errors and returns null otherwise
 	virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space)=0;
 
+	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts)=0;
+	virtual Vector<Vector3> space_get_contacts(RID p_space) const=0;
+	virtual int space_get_contact_count(RID p_space) const=0;
 
 	//missing space parameters
 
diff --git a/servers/visual/rasterizer.h b/servers/visual/rasterizer.h
index 0b115a4d1d..15c757665b 100644
--- a/servers/visual/rasterizer.h
+++ b/servers/visual/rasterizer.h
@@ -1022,6 +1022,7 @@ public:
 
 	virtual bool has_feature(VS::Features p_feature) const=0;
 
+	virtual void restore_framebuffer()=0;
 
 	virtual int get_render_info(VS::RenderInfo p_info)=0;
 
diff --git a/servers/visual/rasterizer_dummy.cpp b/servers/visual/rasterizer_dummy.cpp
index e32f47b3d8..c05438aef3 100644
--- a/servers/visual/rasterizer_dummy.cpp
+++ b/servers/visual/rasterizer_dummy.cpp
@@ -1948,6 +1948,9 @@ bool RasterizerDummy::has_feature(VS::Features p_feature) const {
 
 }
 
+void RasterizerDummy::restore_framebuffer() {
+
+}
 
 RasterizerDummy::RasterizerDummy() {
 
diff --git a/servers/visual/rasterizer_dummy.h b/servers/visual/rasterizer_dummy.h
index cc3c1724a4..9249ad6256 100644
--- a/servers/visual/rasterizer_dummy.h
+++ b/servers/visual/rasterizer_dummy.h
@@ -779,6 +779,7 @@ public:
 
 	virtual bool has_feature(VS::Features p_feature) const;
 
+	virtual void restore_framebuffer();
 
 	RasterizerDummy();
 	virtual ~RasterizerDummy();
diff --git a/servers/visual/visual_server_raster.cpp b/servers/visual/visual_server_raster.cpp
index b51f59050d..fbea60c3a6 100644
--- a/servers/visual/visual_server_raster.cpp
+++ b/servers/visual/visual_server_raster.cpp
@@ -454,6 +454,14 @@ AABB VisualServerRaster::mesh_get_custom_aabb(RID p_mesh) const {
 	return rasterizer->mesh_get_custom_aabb(p_mesh);
 }
 
+void VisualServerRaster::mesh_clear(RID p_mesh) {
+
+	ERR_FAIL_COND(!rasterizer->is_mesh(p_mesh));
+	while(rasterizer->mesh_get_surface_count(p_mesh)) {
+		rasterizer->mesh_remove_surface(p_mesh,0);
+	}
+}
+
 
 /* MULTIMESH */
 
@@ -7351,6 +7359,8 @@ void VisualServerRaster::_draw_cursors_and_margins() {
 		rasterizer->canvas_draw_rect(Rect2(cursors[i].pos, size), 0, Rect2(), tex, Color(1, 1, 1, 1));
 	};
 
+
+
 	if (black_image[MARGIN_LEFT].is_valid()) {
 		Size2 sz(rasterizer->texture_get_width(black_image[MARGIN_LEFT]),rasterizer->texture_get_height(black_image[MARGIN_LEFT]));
 		rasterizer->canvas_draw_rect(Rect2(0,0,black_margin[MARGIN_LEFT],window_h),0,Rect2(0,0,sz.x,sz.y),black_image[MARGIN_LEFT],Color(1,1,1));
@@ -7362,10 +7372,22 @@ void VisualServerRaster::_draw_cursors_and_margins() {
 		rasterizer->canvas_draw_rect(Rect2(window_w-black_margin[MARGIN_RIGHT],0,black_margin[MARGIN_RIGHT],window_h),0,Rect2(0,0,sz.x,sz.y),black_image[MARGIN_RIGHT],Color(1,1,1));
 	} else if (black_margin[MARGIN_RIGHT])
 		rasterizer->canvas_draw_rect(Rect2(window_w-black_margin[MARGIN_RIGHT],0,black_margin[MARGIN_RIGHT],window_h),0,Rect2(0,0,1,1),RID(),Color(0,0,0));
-	if (black_margin[MARGIN_TOP])
+
+	if (black_image[MARGIN_TOP].is_valid()) {
+		Size2 sz(rasterizer->texture_get_width(black_image[MARGIN_TOP]),rasterizer->texture_get_height(black_image[MARGIN_TOP]));
+		rasterizer->canvas_draw_rect(Rect2(0,0,window_w,black_margin[MARGIN_TOP]),0,Rect2(0,0,sz.x,sz.y),black_image[MARGIN_TOP],Color(1,1,1));
+
+	} else if (black_margin[MARGIN_TOP]) {
 		rasterizer->canvas_draw_rect(Rect2(0,0,window_w,black_margin[MARGIN_TOP]),0,Rect2(0,0,1,1),RID(),Color(0,0,0));
-	if (black_margin[MARGIN_BOTTOM])
+	}
+
+	if (black_image[MARGIN_BOTTOM].is_valid()) {
+
+		Size2 sz(rasterizer->texture_get_width(black_image[MARGIN_BOTTOM]),rasterizer->texture_get_height(black_image[MARGIN_BOTTOM]));
+		rasterizer->canvas_draw_rect(Rect2(0,window_h-black_margin[MARGIN_BOTTOM],window_w,black_margin[MARGIN_BOTTOM]),0,Rect2(0,0,sz.x,sz.y),black_image[MARGIN_BOTTOM],Color(1,1,1));
+	} else if (black_margin[MARGIN_BOTTOM]) {
 		rasterizer->canvas_draw_rect(Rect2(0,window_h-black_margin[MARGIN_BOTTOM],window_w,black_margin[MARGIN_BOTTOM]),0,Rect2(0,0,1,1),RID(),Color(0,0,0));
+	}
 
 	rasterizer->canvas_end_rect();
 };
@@ -7413,6 +7435,8 @@ void VisualServerRaster::set_boot_image(const Image& p_image, const Color& p_col
 	if (p_image.empty())
 		return;
 
+	rasterizer->restore_framebuffer();
+
 	rasterizer->begin_frame();
 
 	int window_w = OS::get_singleton()->get_video_mode(0).width;
diff --git a/servers/visual/visual_server_raster.h b/servers/visual/visual_server_raster.h
index 73298d58cd..3d8331984a 100644
--- a/servers/visual/visual_server_raster.h
+++ b/servers/visual/visual_server_raster.h
@@ -765,6 +765,7 @@ public:
 	virtual void mesh_set_custom_aabb(RID p_mesh,const AABB& p_aabb);
 	virtual AABB mesh_get_custom_aabb(RID p_mesh) const;
 
+	virtual void mesh_clear(RID p_mesh);
 
 	/* MULTIMESH API */
 
diff --git a/servers/visual/visual_server_wrap_mt.h b/servers/visual/visual_server_wrap_mt.h
index db03d82829..af88c9bdc9 100644
--- a/servers/visual/visual_server_wrap_mt.h
+++ b/servers/visual/visual_server_wrap_mt.h
@@ -199,6 +199,7 @@ public:
 
 	FUNC2(mesh_remove_surface,RID,int);
 	FUNC1RC(int,mesh_get_surface_count,RID);
+	FUNC1(mesh_clear,RID);
 
 
 	FUNC2(mesh_set_custom_aabb,RID,const AABB&);
diff --git a/servers/visual_server.h b/servers/visual_server.h
index dd71650801..ece61ec701 100644
--- a/servers/visual_server.h
+++ b/servers/visual_server.h
@@ -353,6 +353,8 @@ public:
 	virtual void mesh_set_custom_aabb(RID p_mesh,const AABB& p_aabb)=0;
 	virtual AABB mesh_get_custom_aabb(RID p_mesh) const=0;
 
+	virtual void mesh_clear(RID p_mesh)=0;
+
 	/* MULTIMESH API */
 
 	virtual RID multimesh_create()=0;