GODOT IS OPEN SOURCE

servers/SCsub

@@ -0,0 +1,19 @@
+Import('env')
+
+env.servers_sources=[]
+env.add_source_files(env.servers_sources,"*.cpp")
+
+Export('env')
+
+SConscript('physics/SCsub');
+SConscript('physics_2d/SCsub');
+SConscript('visual/SCsub');
+SConscript('audio/SCsub');
+SConscript('spatial_sound/SCsub');
+SConscript('spatial_sound_2d/SCsub');
+
+lib = env.Library("servers",env.servers_sources)
+
+env.Prepend(LIBS=[lib])
+
+

servers/audio/SCsub

@@ -0,0 +1,7 @@
+Import('env')
+
+env.add_source_files(env.servers_sources,"*.cpp")
+
+Export('env')
+
+

servers/audio/audio_driver_dummy.cpp

@@ -0,0 +1,146 @@
+/*************************************************************************/
+/*  audio_driver_dummy.cpp                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "audio_driver_dummy.h"
+
+#include "globals.h"
+#include "os/os.h"
+
+
+
+Error AudioDriverDummy::init() {
+
+	active=false;
+	thread_exited=false;
+	exit_thread=false;
+	pcm_open = false;
+	samples_in = NULL;
+
+
+	mix_rate = 44100;
+	output_format = OUTPUT_STEREO;
+	channels = 2;
+
+	int latency = GLOBAL_DEF("audio/output_latency",25);
+	buffer_size = nearest_power_of_2( latency * mix_rate / 1000 );
+
+	samples_in = memnew_arr(int32_t, buffer_size*channels);
+
+	mutex=Mutex::create();
+	thread = Thread::create(AudioDriverDummy::thread_func, this);
+
+	return OK;
+};
+
+void AudioDriverDummy::thread_func(void* p_udata) {
+
+	AudioDriverDummy* ad = (AudioDriverDummy*)p_udata;
+
+	uint64_t usdelay = (ad->buffer_size / float(ad->mix_rate))*1000000;
+
+
+	while (!ad->exit_thread) {
+
+
+		if (!ad->active) {
+
+		} else {
+
+			ad->lock();
+
+			ad->audio_server_process(ad->buffer_size, ad->samples_in);
+
+			ad->unlock();
+
+		};
+
+		OS::get_singleton()->delay_usec(usdelay);
+
+	};
+
+	ad->thread_exited=true;
+
+};
+
+void AudioDriverDummy::start() {
+
+	active = true;
+};
+
+int AudioDriverDummy::get_mix_rate() const {
+
+	return mix_rate;
+};
+
+AudioDriverSW::OutputFormat AudioDriverDummy::get_output_format() const {
+
+	return output_format;
+};
+void AudioDriverDummy::lock() {
+
+	if (!thread || !mutex)
+		return;
+	mutex->lock();
+};
+void AudioDriverDummy::unlock() {
+
+	if (!thread || !mutex)
+		return;
+	mutex->unlock();
+};
+
+void AudioDriverDummy::finish() {
+
+	if (!thread)
+		return;
+
+	exit_thread = true;
+	Thread::wait_to_finish(thread);
+
+	if (samples_in) {
+		memdelete_arr(samples_in);
+	};
+
+	memdelete(thread);
+	if (mutex)
+		memdelete(mutex);
+	thread = NULL;
+};
+
+AudioDriverDummy::AudioDriverDummy() {
+
+	mutex = NULL;
+	thread=NULL;
+
+};
+
+AudioDriverDummy::~AudioDriverDummy() {
+
+};
+
+

servers/audio/audio_driver_dummy.h

@@ -0,0 +1,76 @@
+/*************************************************************************/
+/*  audio_driver_dummy.h                                                 */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AUDIO_DRIVER_DUMMY_H
+#define AUDIO_DRIVER_DUMMY_H
+
+#include "servers/audio/audio_server_sw.h"
+
+#include "core/os/thread.h"
+#include "core/os/mutex.h"
+
+
+class AudioDriverDummy : public AudioDriverSW {
+
+	Thread* thread;
+	Mutex* mutex;
+
+	int32_t* samples_in;
+
+	static void thread_func(void* p_udata);
+	int buffer_size;
+
+	unsigned int mix_rate;
+	OutputFormat output_format;
+
+	int channels;
+
+	bool active;
+	bool thread_exited;
+	mutable bool exit_thread;
+	bool pcm_open;
+
+public:
+
+	const char* get_name() const {
+		return "Dummy";
+	};
+
+	virtual Error init();
+	virtual void start();
+	virtual int get_mix_rate() const;
+	virtual OutputFormat get_output_format() const;
+	virtual void lock();
+	virtual void unlock();
+	virtual void finish();
+
+	AudioDriverDummy();
+	~AudioDriverDummy();
+};
+
+#endif

servers/audio/audio_filter_sw.cpp

@@ -0,0 +1,286 @@
+/*************************************************************************/
+/*  audio_filter_sw.cpp                                                  */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "audio_filter_sw.h"
+
+void AudioFilterSW::set_mode(Mode p_mode) {
+
+	mode = p_mode;
+}
+void AudioFilterSW::set_cutoff(float p_cutoff) {
+
+	cutoff=p_cutoff;
+}
+void AudioFilterSW::set_resonance(float p_resonance) {
+
+	resonance=p_resonance;
+}
+
+void AudioFilterSW::set_gain(float p_gain) {
+
+	gain=p_gain;
+}
+
+void AudioFilterSW::set_sampling_rate(float p_srate) {
+
+	sampling_rate=p_srate;
+}
+
+
+void AudioFilterSW::prepare_coefficients(Coeffs *p_coeffs) {
+
+	int sr_limit = (sampling_rate/2)+512;
+
+
+	double final_cutoff=(cutoff>sr_limit)?sr_limit:cutoff;
+	if (final_cutoff<1) //avoid crapness
+		final_cutoff=1; //dont allow less than this
+
+
+
+	double omega=2.0*Math_PI*final_cutoff/sampling_rate;
+
+	double sin_v=Math::sin(omega);
+	double cos_v=Math::cos(omega);
+
+	double Q=resonance;
+	if (Q<=0.0) {
+		Q=0.0001;
+	}
+
+
+	if (mode==BANDPASS)
+		Q*=2.0;
+	else if (mode==PEAK)
+		Q*=3.0;
+
+	double  tmpgain=gain;
+
+	if (tmpgain<0.001)
+		tmpgain=0.001;
+
+	if (stages>1) {
+
+		Q=(Q>1.0 ? Math::pow(Q,1.0/stages) : Q);
+		tmpgain = Math::pow(tmpgain,1.0/(stages+1));
+	}
+	double alpha = sin_v/(2*Q);
+
+	double a0 = 1.0 + alpha;
+
+	switch (mode) {
+
+		case LOWPASS: {
+
+			p_coeffs->b0=  (1.0 - cos_v)/2.0 ;
+			p_coeffs->b1=   1.0 - cos_v    ;
+			p_coeffs->b2=  (1.0 - cos_v)/2.0 ;
+			p_coeffs->a1=  -2.0*cos_v;
+			p_coeffs->a2=   1.0 - alpha  ;
+		} break;
+
+
+		case HIGHPASS: {
+
+			p_coeffs->b0 =  (1.0 + cos_v)/2.0;
+			p_coeffs->b1 = -(1.0 + cos_v);
+			p_coeffs->b2 =  (1.0 + cos_v)/2.0;
+			p_coeffs->a1 =  -2.0*cos_v;
+			p_coeffs->a2 =   1.0 - alpha;
+		} break;
+
+		case BANDPASS: {
+
+			p_coeffs->b0 =  alpha*sqrt(Q+1);
+			p_coeffs->b1 =  0.0 ;
+			p_coeffs->b2 =  -alpha*sqrt(Q+1);
+			p_coeffs->a1 =  -2.0*cos_v;
+			p_coeffs->a2 =   1.0 - alpha;
+		} break;
+
+		case NOTCH: {
+
+			p_coeffs->b0 =   1.0;
+			p_coeffs->b1 =  -2.0*cos_v;
+			p_coeffs->b2 =   1.0;
+			p_coeffs->a1 =  -2.0*cos_v;
+			p_coeffs->a2 =   1.0 - alpha;
+		} break;
+		case PEAK: {
+			p_coeffs->b0 =  (1.0+alpha*tmpgain);
+			p_coeffs->b1 =  (-2.0*cos_v);
+			p_coeffs->b2 =  (1.0-alpha*tmpgain);
+			p_coeffs->a1 =  -2*cos_v;
+			p_coeffs->a2 =  (1-alpha/tmpgain);
+		} break;
+		case BANDLIMIT: {
+			//this one is extra tricky
+			double hicutoff=resonance;
+			double centercutoff = (cutoff+resonance)/2.0;
+			double bandwidth=(Math::log(centercutoff)-Math::log(hicutoff))/Math::log(2);
+			omega=2.0*Math_PI*centercutoff/sampling_rate;
+			alpha = Math::sin(omega)*Math::sinh( Math::log(2)/2 * bandwidth * omega/Math::sin(omega) );
+			a0=1+alpha;
+
+			p_coeffs->b0 =  alpha;
+			p_coeffs->b1 =  0;
+			p_coeffs->b2 =  -alpha;
+			p_coeffs->a1 =  -2*Math::cos(omega);
+			p_coeffs->a2 =  1-alpha;
+
+		} break;
+		case LOWSHELF: {
+
+			double tmpq  = Math::sqrt(Q);
+			if (tmpq<=0)
+				tmpq=0.001;
+			alpha = sin_v / (2 * tmpq);
+			double beta  = Math::sqrt(tmpgain) / tmpq;
+
+			a0=(tmpgain+1.0)+(tmpgain-1.0)*cos_v+beta*sin_v;
+			p_coeffs->b0=tmpgain*((tmpgain+1.0)-(tmpgain-1.0)*cos_v+beta*sin_v);
+			p_coeffs->b1=2.0*tmpgain*((tmpgain-1.0)-(tmpgain+1.0)*cos_v);
+			p_coeffs->b2=tmpgain*((tmpgain+1.0)-(tmpgain-1.0)*cos_v-beta*sin_v);
+			p_coeffs->a1=-2.0*((tmpgain-1.0)+(tmpgain+1.0)*cos_v);
+			p_coeffs->a2=((tmpgain+1.0)+(tmpgain-1.0)*cos_v-beta*sin_v);
+
+		} break;
+		case HIGHSHELF: {
+			double tmpq= Math::sqrt(Q);
+			if (tmpq<=0)
+				tmpq=0.001;
+			alpha = sin_v / (2 * tmpq);
+			double beta  = Math::sqrt(tmpgain) / tmpq;
+
+			a0=(tmpgain+1.0)-(tmpgain-1.0)*cos_v+beta*sin_v;
+			p_coeffs->b0=tmpgain*((tmpgain+1.0)+(tmpgain-1.0)*cos_v+beta*sin_v);
+			p_coeffs->b1=-2.0*tmpgain*((tmpgain-1.0)+(tmpgain+1.0)*cos_v);
+			p_coeffs->b2=tmpgain*((tmpgain+1.0)+(tmpgain-1.0)*cos_v-beta*sin_v);
+			p_coeffs->a1=2.0*((tmpgain-1.0)-(tmpgain+1.0)*cos_v);
+			p_coeffs->a2=((tmpgain+1.0)-(tmpgain-1.0)*cos_v-beta*sin_v);
+
+
+		} break;
+
+    };
+
+    p_coeffs->b0/=a0;
+    p_coeffs->b1/=a0;
+    p_coeffs->b2/=a0;
+    p_coeffs->a1/=0.0-a0;
+    p_coeffs->a2/=0.0-a0;
+
+    //undenormalise
+/*    p_coeffs->b0=undenormalise(p_coeffs->b0);
+    p_coeffs->b1=undenormalise(p_coeffs->b1);
+    p_coeffs->b2=undenormalise(p_coeffs->b2);
+    p_coeffs->a1=undenormalise(p_coeffs->a1);
+    p_coeffs->a2=undenormalise(p_coeffs->a2);*/
+
+}
+
+void AudioFilterSW::set_stages(int p_stages) { //adjust for multiple stages
+
+	stages=p_stages;
+}
+
+/* Fouriertransform kernel to obtain response */
+
+float AudioFilterSW::get_response(float p_freq,Coeffs *p_coeffs) {
+
+	float freq=p_freq / sampling_rate * Math_PI * 2.0f;
+
+	float cx=p_coeffs->b0,cy=0.0;
+
+	cx += cos(freq) * p_coeffs->b1;
+	cy -= sin(freq) * p_coeffs->b1;
+	cx += cos(2*freq) * p_coeffs->b2;
+	cy -= sin(2*freq) * p_coeffs->b2;
+
+
+	float H=cx*cx+cy*cy;
+	cx=1.0;
+	cy=0.0;
+
+
+	cx -= cos(freq) * p_coeffs->a1;
+	cy += sin(freq) * p_coeffs->a1;
+	cx -= cos(2*freq) * p_coeffs->a2;
+	cy += sin(2*freq) * p_coeffs->a2;
+
+
+	H=H/(cx*cx+cy*cy);
+	return H;
+}
+
+
+AudioFilterSW::AudioFilterSW() {
+
+
+	sampling_rate=44100;
+	resonance=0.5;
+	cutoff=5000;
+	gain=1.0;
+	mode=LOWPASS;
+	stages=1;
+}
+
+AudioFilterSW::Processor::Processor() {
+
+	set_filter(NULL);
+
+}
+
+void AudioFilterSW::Processor::set_filter(AudioFilterSW * p_filter) {
+
+	ha1=ha2=hb1=hb2=0;
+	filter=p_filter;
+}
+
+void AudioFilterSW::Processor::update_coeffs() {
+
+	if (!filter)
+		return;
+
+	filter->prepare_coefficients(&coeffs);
+
+}
+
+void AudioFilterSW::Processor::process(float *p_samples,int p_amount, int p_stride) {
+
+	if (!filter)
+		return;
+
+	for (int i=0;i<p_amount;i++) {
+
+		process_one(*p_samples);
+		p_samples+=p_stride;
+	}
+
+
+}

servers/audio/audio_filter_sw.h

@@ -0,0 +1,119 @@
+/*************************************************************************/
+/*  audio_filter_sw.h                                                    */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AUDIO_FILTER_SW_H
+#define AUDIO_FILTER_SW_H
+
+
+#include "math_funcs.h"
+
+class AudioFilterSW {
+public:
+
+	struct Coeffs {
+
+		float a1,a2;
+		float b0,b1,b2;
+
+		//bool operator==(const Coeffs &p_rv) { return (FLOATS_EQ(a1,p_rv.a1) && FLOATS_EQ(a2,p_rv.a2) && FLOATS_EQ(b1,p_rv.b1) && FLOATS_EQ(b2,p_rv.b2) && FLOATS_EQ(b0,p_rv.b0) ); }
+		Coeffs() { a1=a2=b0=b1=b2=0.0; }
+	};
+
+	enum Mode {
+		BANDPASS,
+		HIGHPASS,
+		LOWPASS,
+		NOTCH,
+		PEAK,
+		BANDLIMIT,
+		LOWSHELF,
+		HIGHSHELF
+
+	};
+
+	class Processor { // simple filter processor
+
+		AudioFilterSW * filter;
+		Coeffs coeffs;
+		float ha1,ha2,hb1,hb2; //history
+	public:
+		void set_filter(AudioFilterSW * p_filter);
+		void process(float *p_samples,int p_amount, int p_stride=1);
+		void update_coeffs();
+		inline void process_one(float& p_sample);
+
+		Processor();
+	};
+
+private:
+
+
+	float cutoff;
+	float resonance;
+	float gain;
+	float sampling_rate;
+	int stages;
+	Mode mode;
+
+
+
+public:
+
+	float get_response(float p_freq,Coeffs *p_coeffs);
+
+	void set_mode(Mode p_mode);
+	void set_cutoff(float p_cutoff);
+	void set_resonance(float p_resonance);
+	void set_gain(float p_gain);
+	void set_sampling_rate(float p_srate);
+	void set_stages(int p_stages); //adjust for multiple stages
+
+	void prepare_coefficients(Coeffs *p_coeffs);
+
+	AudioFilterSW();
+
+};
+
+
+
+
+/* inline methods */
+
+
+void AudioFilterSW::Processor::process_one(float &p_val) {
+
+	float pre=p_val;
+	p_val = (p_val * coeffs.b0 + hb1 * coeffs.b1  + hb2 * coeffs.b2 + ha1 * coeffs.a1 + ha2 * coeffs.a2);
+	ha2=ha1;
+	hb2=hb1;
+	hb1=pre;
+	ha1=p_val;
+}
+
+
+#endif // AUDIO_FILTER_SW_H

servers/audio/audio_mixer_sw.h

@@ -0,0 +1,248 @@
+/*************************************************************************/
+/*  audio_mixer_sw.h                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AUDIO_MIXER_SW_H
+#define AUDIO_MIXER_SW_H
+
+#include "servers/audio_server.h"
+#include "servers/audio/sample_manager_sw.h"
+#include "servers/audio/audio_filter_sw.h"
+#include "servers/audio/reverb_sw.h"
+
+class AudioMixerSW : public AudioMixer {
+public:
+
+	enum InterpolationType {
+
+		INTERPOLATION_RAW,
+		INTERPOLATION_LINEAR,
+		INTERPOLATION_CUBIC
+	};
+
+	enum MixChannels {
+
+		MIX_STEREO=2,
+		MIX_QUAD=4
+	};
+
+	typedef void (*MixStepCallback)(void*);
+
+private:
+	SampleManagerSW *sample_manager;
+
+	enum {
+
+		MAX_CHANNELS=64,
+		// fixed point defs
+
+		MIX_FRAC_BITS=13,
+		MIX_FRAC_LEN=(1<<MIX_FRAC_BITS),
+		MIX_FRAC_MASK=MIX_FRAC_LEN-1,
+		MIX_VOL_FRAC_BITS=12,
+		MIX_VOLRAMP_FRAC_BITS=16,
+		MIX_VOLRAMP_FRAC_LEN=(1<<MIX_VOLRAMP_FRAC_BITS),
+		MIX_VOLRAMP_FRAC_MASK=MIX_VOLRAMP_FRAC_LEN-1,
+		MIX_FILTER_FRAC_BITS=16,
+		MIX_FILTER_RAMP_FRAC_BITS=8,
+		MIX_VOL_MOVE_TO_24=4,
+		MAX_REVERBS=4
+	};
+
+	struct Channel {
+
+		RID sample;
+		struct Mix {
+			int64_t offset;
+			int32_t increment;
+
+			int32_t vol[4];
+			int32_t reverb_vol[4];
+			int32_t chorus_vol[4];
+
+			int32_t old_vol[4];
+			int32_t old_reverb_vol[4];
+			int32_t old_chorus_vol[4];
+
+
+			struct Filter { //history (stereo)
+				float ha[2],hb[2];
+			} filter_l,filter_r;
+
+		} mix;
+
+		float vol;
+		float pan;
+		float depth;
+		float height;
+
+		float chorus_send;
+		ReverbRoomType reverb_room;
+		float reverb_send;
+		int speed;
+		int check;
+		bool positional;
+
+		bool had_prev_reverb;
+		bool had_prev_chorus;
+		bool had_prev_vol;
+
+		struct Filter {
+
+			bool dirty;
+
+			FilterType type;
+			float cutoff;
+			float resonance;
+			float gain;
+
+			struct Coefs {
+
+				float a1,a2,b0,b1,b2; // fixed point coefficients
+			} coefs,old_coefs;
+
+		} filter;
+
+		bool first_mix;
+		bool active;
+		Channel() { active=false; check=-1; first_mix=false; filter.dirty=true; filter.type=FILTER_NONE; filter.cutoff=8000; filter.resonance=0; filter.gain=0; }
+	};
+
+	Channel channels[MAX_CHANNELS];
+
+	uint32_t mix_rate;
+	bool fx_enabled;
+	InterpolationType interpolation_type;
+
+	int mix_chunk_bits;
+	int mix_chunk_size;
+	int mix_chunk_mask;
+
+	int32_t *mix_buffer;
+	int32_t *zero_buffer; // fx feed when no input was mixed
+
+	struct ResamplerState {
+
+		uint32_t amount;
+		int32_t increment;
+
+
+		int32_t pos;
+
+
+		int32_t vol[4];
+		int32_t reverb_vol[4];
+		int32_t chorus_vol[4];
+
+		int32_t vol_inc[4];
+		int32_t reverb_vol_inc[4];
+		int32_t chorus_vol_inc[4];
+
+
+		Channel::Mix::Filter *filter_l;
+		Channel::Mix::Filter *filter_r;
+		Channel::Filter::Coefs coefs;
+		Channel::Filter::Coefs coefs_inc;
+
+		int32_t *reverb_buffer;
+	};
+
+
+
+	template<class Depth,bool is_stereo,bool use_filter,bool use_fx,InterpolationType type,MixChannels>
+	_FORCE_INLINE_ void do_resample(const Depth* p_src, int32_t *p_dst, ResamplerState *p_state);
+
+	MixChannels mix_channels;
+
+	void mix_channel(Channel& p_channel);
+	int mix_chunk_left;
+	void mix_chunk();	
+
+	float channel_nrg;
+	int channel_id_count;
+	bool inside_mix;
+	MixStepCallback step_callback;
+	void *step_udata;
+	_FORCE_INLINE_ int _get_channel(ChannelID p_channel) const;
+
+	int max_reverbs;
+	struct ReverbState {
+
+		bool used_in_chunk;
+		bool enabled;
+		ReverbSW *reverb;
+		int frames_idle;
+		int32_t *buffer; //reverb is sent here
+		ReverbState() { enabled=false; frames_idle=0; used_in_chunk=false; }
+	};
+
+	ReverbState *reverb_state;
+
+
+public:
+
+
+	virtual ChannelID channel_alloc(RID p_sample);
+
+	virtual void channel_set_volume(ChannelID p_channel, float p_gain);
+	virtual void channel_set_pan(ChannelID p_channel, float p_pan, float p_depth=0,float height=0); //pan and depth go from -1 to 1
+	virtual void channel_set_filter(ChannelID p_channel, FilterType p_type, float p_cutoff, float p_resonance, float p_gain=1.0);
+	virtual void channel_set_chorus(ChannelID p_channel, float p_chorus );
+	virtual void channel_set_reverb(ChannelID p_channel, ReverbRoomType p_room_type, float p_reverb);
+	virtual void channel_set_mix_rate(ChannelID p_channel, int p_mix_rate);
+	virtual void channel_set_positional(ChannelID p_channel, bool p_positional);
+
+	virtual float channel_get_volume(ChannelID p_channel) const;
+	virtual float channel_get_pan(ChannelID p_channel) const; //pan and depth go from -1 to 1
+	virtual float channel_get_pan_depth(ChannelID p_channel) const; //pan and depth go from -1 to 1
+	virtual float channel_get_pan_height(ChannelID p_channel) const; //pan and depth go from -1 to 1
+	virtual FilterType channel_get_filter_type(ChannelID p_channel) const;
+	virtual float channel_get_filter_cutoff(ChannelID p_channel) const;
+	virtual float channel_get_filter_resonance(ChannelID p_channel) const;
+	virtual float channel_get_filter_gain(ChannelID p_channel) const;
+
+	virtual float channel_get_chorus(ChannelID p_channel) const;
+	virtual ReverbRoomType channel_get_reverb_type(ChannelID p_channel) const;
+	virtual float channel_get_reverb(ChannelID p_channel) const;
+
+	virtual int channel_get_mix_rate(ChannelID p_channel) const;
+	virtual bool channel_is_positional(ChannelID p_channel) const;
+
+	virtual bool channel_is_valid(ChannelID p_channel) const;
+
+	virtual void channel_free(ChannelID p_channel);
+
+	int mix(int32_t *p_buffer,int p_frames); //return amount of mixsteps
+	uint64_t get_step_usecs() const;
+
+	virtual void set_mixer_volume(float p_volume);
+
+	AudioMixerSW(SampleManagerSW *p_sample_manager,int p_desired_latency_ms,int p_mix_rate,MixChannels p_mix_channels,bool p_use_fx=true,InterpolationType p_interp=INTERPOLATION_LINEAR,MixStepCallback p_step_callback=NULL,void *p_callback_udata=NULL);
+	~AudioMixerSW();
+};
+
+#endif // AUDIO_MIXER_SW_H

servers/audio/audio_server_sw.h

@@ -0,0 +1,279 @@
+/*************************************************************************/
+/*  audio_server_sw.h                                                    */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AUDIO_SERVER_SW_H
+#define AUDIO_SERVER_SW_H
+
+#include "servers/audio_server.h"
+#include "servers/audio/audio_mixer_sw.h"
+#include "servers/audio/voice_rb_sw.h"
+#include "self_list.h"
+#include "os/thread_safe.h"
+#include "os/thread.h"
+class AudioServerSW : public AudioServer {
+
+	OBJ_TYPE( AudioServerSW, AudioServer );
+
+	_THREAD_SAFE_CLASS_
+
+	enum {
+		INTERNAL_BUFFER_SIZE=4096,
+		STREAM_SCALE_BITS=12
+
+	};
+
+	SampleManagerSW *sample_manager;
+	AudioMixerSW *mixer;
+
+	virtual AudioMixer *get_mixer();
+	virtual void audio_mixer_chunk_callback(int p_frames);
+
+	struct Voice {
+
+		float volume;
+		volatile bool active;
+		SelfList<Voice> active_item;
+		AudioMixer::ChannelID channel;
+
+
+		Voice () : active_item(this) { channel=AudioMixer::INVALID_CHANNEL; active=false;}
+	};
+
+	mutable RID_Owner<Voice> voice_owner;
+	SelfList<Voice>::List active_list;
+
+	struct Stream {
+		bool active;		
+		List<Stream*>::Element *E;
+		AudioStream *audio_stream;
+		EventStream *event_stream;
+		float volume_scale;
+	};
+
+	List<Stream*> active_audio_streams;
+
+	//List<Stream*> event_streams;
+
+	int32_t * internal_buffer;
+	int internal_buffer_channels;
+	int32_t * stream_buffer;
+
+	mutable RID_Owner<Stream> stream_owner;
+
+	float stream_volume;
+	float stream_volume_scale;
+	float fx_volume_scale;
+	float event_voice_volume_scale;
+	float peak_left,peak_right;
+	uint32_t max_peak;
+
+	VoiceRBSW voice_rb;
+
+	bool exit_update_thread;
+	Thread *thread;
+	static void _thread_func(void *self);
+
+	void _update_streams(bool p_thread);
+	void driver_process_chunk(int p_frames,int32_t *p_buffer);
+
+	AudioMixerSW::InterpolationType mixer_interp;
+	bool mixer_use_fx;
+	uint64_t mixer_step_usecs;
+
+	static void _mixer_callback(void *p_udata);
+friend class AudioDriverSW;
+	void driver_process(int p_frames,int32_t *p_buffer);
+public:
+
+
+	/* SAMPLE API */
+
+	virtual RID sample_create(SampleFormat p_format, bool p_stereo, int p_length);
+
+	virtual void sample_set_description(RID p_sample, const String& p_description);
+	virtual String sample_get_description(RID p_sample, const String& p_description) const;
+
+	virtual SampleFormat sample_get_format(RID p_sample) const;
+	virtual bool sample_is_stereo(RID p_sample) const;
+	virtual int sample_get_length(RID p_sample) const;
+	const void* sample_get_data_ptr(RID p_sample) const;
+
+	virtual void sample_set_data(RID p_sample, const DVector<uint8_t>& p_buffer);
+	virtual const DVector<uint8_t> sample_get_data(RID p_sample) const;
+
+	virtual void sample_set_mix_rate(RID p_sample,int p_rate);
+	virtual int sample_get_mix_rate(RID p_sample) const;
+
+	virtual void sample_set_loop_format(RID p_sample,SampleLoopFormat p_format);
+	virtual SampleLoopFormat sample_get_loop_format(RID p_sample) const;
+
+	virtual void sample_set_loop_begin(RID p_sample,int p_pos);
+	virtual int sample_get_loop_begin(RID p_sample) const;
+
+	virtual void sample_set_loop_end(RID p_sample,int p_pos);
+	virtual int sample_get_loop_end(RID p_sample) const;
+
+	/* VOICE API */
+
+	virtual RID voice_create();
+
+	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_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 );
+	virtual void voice_set_reverb(RID p_voice, ReverbRoomType p_room_type, float p_reverb);
+	virtual void voice_set_mix_rate(RID p_voice, int p_mix_rate);
+	virtual void voice_set_positional(RID p_voice, bool p_positional);
+
+	virtual float voice_get_volume(RID p_voice) const;
+	virtual float voice_get_pan(RID p_voice) const; //pan and depth go from -1 to 1
+	virtual float voice_get_pan_depth(RID p_voice) const; //pan and depth go from -1 to 1
+	virtual float voice_get_pan_height(RID p_voice) const; //pan and depth go from -1 to 1
+	virtual FilterType voice_get_filter_type(RID p_voice) const;
+	virtual float voice_get_filter_cutoff(RID p_voice) const;
+	virtual float voice_get_filter_resonance(RID p_voice) const;
+	virtual float voice_get_chorus(RID p_voice) const;
+	virtual ReverbRoomType voice_get_reverb_type(RID p_voice) const;
+	virtual float voice_get_reverb(RID p_voice) const;
+
+	virtual int voice_get_mix_rate(RID p_voice) const;
+	virtual bool voice_is_positional(RID p_voice) const;
+
+	virtual void voice_stop(RID p_voice);
+	virtual bool voice_is_active(RID p_voice) const;
+
+	/* STREAM API */
+
+	virtual RID audio_stream_create(AudioStream *p_stream);
+	virtual RID event_stream_create(EventStream *p_stream);
+
+	virtual void stream_set_active(RID p_stream, bool p_active);
+	virtual bool stream_is_active(RID p_stream) const;
+
+	virtual void stream_set_volume_scale(RID p_stream, float p_scale);
+	virtual float stream_set_volume_scale(RID p_stream) const;
+
+	virtual void free(RID p_id);
+
+	virtual void init();
+	virtual void finish();
+	virtual void update();
+
+	virtual void lock();
+	virtual void unlock();
+	virtual int get_default_channel_count() const;
+	virtual int get_default_mix_rate() const;
+
+	void set_mixer_params(AudioMixerSW::InterpolationType p_interp, bool p_use_fx);
+
+	virtual void set_stream_global_volume_scale(float p_volume);
+	virtual void set_fx_global_volume_scale(float p_volume);
+	virtual void set_event_voice_global_volume_scale(float p_volume);
+
+
+	virtual float get_stream_global_volume_scale() const;
+	virtual float get_fx_global_volume_scale() const;
+	virtual float get_event_voice_global_volume_scale() const;
+
+	virtual uint32_t read_output_peak() const;
+
+	virtual double get_mix_time() const; //useful for video -> audio sync
+
+	AudioServerSW(SampleManagerSW *p_sample_manager);
+	~AudioServerSW();
+
+};
+
+
+class AudioDriverSW {
+
+
+	static AudioDriverSW *singleton;
+	uint64_t _last_mix_time;
+	uint64_t _mix_amount;
+
+
+protected:
+
+	void audio_server_process(int p_frames,int32_t *p_buffer,bool p_update_mix_time=true);
+	void update_mix_time(int p_frames);
+
+public:
+
+
+	double get_mix_time() const; //useful for video -> audio sync
+
+	enum OutputFormat {
+
+		OUTPUT_MONO,
+		OUTPUT_STEREO,
+		OUTPUT_QUAD,
+		OUTPUT_5_1
+	};
+
+	static AudioDriverSW *get_singleton();
+	void set_singleton();
+
+	virtual const char* get_name() const=0;
+
+	virtual Error init()=0;
+	virtual void start()=0;
+	virtual int get_mix_rate() const =0;
+	virtual OutputFormat get_output_format() const=0;
+	virtual void lock()=0;
+	virtual void unlock()=0;
+	virtual void finish()=0;
+
+
+
+
+	AudioDriverSW();
+	virtual ~AudioDriverSW() {};
+};
+
+
+
+class AudioDriverManagerSW {
+
+	enum {
+
+		MAX_DRIVERS=10
+	};
+
+	static AudioDriverSW *drivers[MAX_DRIVERS];
+	static int driver_count;
+public:
+
+	static void add_driver(AudioDriverSW *p_driver);
+	static int get_driver_count();
+	static AudioDriverSW *get_driver(int p_driver);
+};
+
+#endif // AUDIO_SERVER_SW_H

servers/audio/reverb_buffers_sw.cpp

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

servers/audio/reverb_buffers_sw.h

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

servers/audio/reverb_sw.cpp

@@ -0,0 +1,569 @@
+/*************************************************************************/
+/*  reverb_sw.cpp                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "reverb_sw.h"
+#include "stdlib.h"
+#include "print_string.h"
+#define SETMIN( x, y ) (x) = MIN ( (x), (y) )
+#define rangeloop( c, min, max )       \
+	for ( (c) = (min) ; (c) < (max) ; (c)++ )
+
+#define ABSDIFF(x, y)\
+	( ((x) < (y)) ? ((y) - (x)) : ((x) - (y)) )
+
+
+#ifdef bleh_MSC_VER
+
+#if _MSC_VER >= 1400
+	_FORCE_INLINE_ int32_tMULSHIFT_S32 (
+		int32_t Factor1,
+		int32_t Factor2,
+		uint8_t Bits
+					) {
+
+			return __ll_rshift (
+					__emul ( Factor1, Factor2 ),
+			Bits
+						);
+					}
+#endif
+
+#else
+#define MULSHIFT_S32( Factor1, Factor2, Bits )\
+			( (int) (( (int64_t)(Factor1) * (Factor2) ) >> (Bits)) )
+#endif
+
+
+
+struct ReverbParamsSW {
+	unsigned int BufferSize;		// Required buffer size
+	int gLPF;			// Coefficient
+	int gEcho0;			// Coefficient
+	int gEcho1;			// Coefficient
+	int gEcho2;			// Coefficient
+	int gEcho3;			// Coefficient
+	int gWall;			// Coefficient
+	int gReva;			// Coefficient
+	int gRevb;			// Coefficient
+	int gInputL;		// Coefficient
+	int gInputR;		// Coefficient
+	unsigned int nRevaOldL;		// Offset
+	unsigned int nRevaOldR;		// Offset
+	unsigned int nRevbOldL;		// Offset
+	unsigned int nRevbOldR;		// Offset
+	unsigned int nLwlNew;		// Offset
+	unsigned int nRwrNew;		// Offset
+	unsigned int nEcho0L;		// Offset
+	unsigned int nEcho0R;		// Offset
+	unsigned int nEcho1L;		// Offset
+	unsigned int nEcho1R;		// Offset
+	unsigned int nLwlOld;		// Offset
+	unsigned int nRwrOld;		// Offset
+	unsigned int nLwrNew;		// Offset
+	unsigned int nRwlNew;		// Offset
+	unsigned int nEcho2L;		// Offset
+	unsigned int nEcho2R;		// Offset
+	unsigned int nEcho3L;		// Offset
+	unsigned int nEcho3R;		// Offset
+	unsigned int nLwrOld;		// Offset
+	unsigned int nRwlOld;		// Offset
+	unsigned int nRevaNewL;		// Offset
+	unsigned int nRevaNewR;		// Offset
+	unsigned int nRevbNewL;		// Offset
+	unsigned int nRevbNewR;		// Offset
+};
+
+static ReverbParamsSW reverb_params_Room = {
+	0x26C0/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x6D80,		0x54B8,		-0x4130,	0x0000,		0x0000,		-0x4580,
+//	gReva		gRevb		gInputL		gInputR
+	0x5800,		0x5300,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x01B4 - 0x007D,	0x0136 - 0x007D,	0x00B8 - 0x005B,	0x005C - 0x005B,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x04D6,		0x0333,		0x03F0,		0x0227,		0x0374,		0x01EF,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x0334,		0x01B5,		0x0000,		0x0000,		0x0000,		0x0000,		0x0000,		0x0000,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x0000,		0x0000,		0x01B4,		0x0136,		0x00B8,		0x005C
+};
+
+static ReverbParamsSW reverb_params_StudioSmall = {
+	0x1F40/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x70F0,		0x4FA8,		-0x4320,	0x4410,		-0x3F10,	-0x6400,
+//	gReva		gRevb		gInputL		gInputR
+	0x5280,		0x4EC0,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x00B4 - 0x0033,	0x0080 - 0x0033,	0x004C - 0x0025,	0x0026 - 0x0025,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x03E4,		0x031B,		0x03A4,		0x02AF,		0x0372,		0x0266,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x031C,		0x025D,		0x025C,		0x018E,		0x022F,		0x0135,		0x01D2,		0x00B7,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x018F,		0x00B5,		0x00B4,		0x0080,		0x004C,		0x0026
+};
+
+static ReverbParamsSW reverb_params_StudioMedium = {
+	0x4840/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x70F0,		0x4FA8,		-0x4320,	0x4510,		-0x4110,	-0x4B40,
+//	gReva		gRevb		gInputL		gInputR
+	0x5280,		0x4EC0,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x0264 - 0x00B1,	0x01B2 - 0x00B1,	0x0100 - 0x007F,	0x0080 - 0x007F,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x0904,		0x076B,		0x0824,		0x065F,		0x07A2,		0x0616,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x076C,		0x05ED,		0x05EC,		0x042E,		0x050F,		0x0305,		0x0462,		0x02B7,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x042F,		0x0265,		0x0264,		0x01B2,		0x0100,		0x0080
+};
+
+static ReverbParamsSW reverb_params_StudioLarge = {
+	0x6FE0/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x6F60,		0x4FA8,		-0x4320,	0x4510,		-0x4110,	-0x5980,
+//	gReva		gRevb		gInputL		gInputR
+	0x5680,		0x52C0,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x031C - 0x00E3,	0x0238 - 0x00E3,	0x0154 -  0x00A9,	0x00AA - 0x00A9,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x0DFB,		0x0B58,		0x0D09,		0x0A3C,		0x0BD9,		0x0973,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x0B59,		0x08DA,		0x08D9,		0x05E9,		0x07EC,		0x04B0,		0x06EF,		0x03D2,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x05EA,		0x031D,		0x031C,		0x0238,		0x0154,		0x00AA
+};
+
+static ReverbParamsSW reverb_params_Hall = {
+	0xADE0/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x6000,		0x5000,		0x4C00,		-0x4800,	-0x4400,	-0x4000,
+//	gReva		gRevb		gInputL		gInputR
+	0x6000,		0x5C00,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x05C0 - 0x01A5,	0x041A - 0x01A5,	0x0274 - 0x0139,	0x013A -  0x0139,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x15BA,		0x11BB,		0x14C2,		0x10BD,		0x11BC,		0x0DC1,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x11C0,		0x0DC3,		0x0DC0,		0x09C1,		0x0BC4,		0x07C1,		0x0A00,		0x06CD,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x09C2,		0x05C1,		0x05C0,		0x041A,		0x0274,		0x013A
+};
+
+static ReverbParamsSW reverb_params_SpaceEcho = {
+	0xF6C0/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x7E00,		0x5000,		-0x4C00,	-0x5000,	0x4C00,		-0x5000,
+//	gReva		gRevb		gInputL		gInputR
+	0x6000,		0x5400,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x0AE0 - 0x033D,	0x07A2 - 0x033D,	0x0464 - 0x0231,	0x0232 - 0x0231,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x1ED6,		0x1A31,		0x1D14,		0x183B,		0x1BC2,		0x16B2,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x1A32,		0x15EF,		0x15EE,		0x1055,		0x1334,		0x0F2D,		0x11F6,		0x0C5D,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x1056,		0x0AE1,		0x0AE0,		0x07A2,		0x0464,		0x0232
+};
+
+static ReverbParamsSW reverb_params_Echo = {
+	0x18040/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x7FFF,		0x7FFF,		0x0000,		0x0000,		0x0000,		-0x7F00,
+//	gReva		gRevb		gInputL		gInputR
+	0x0000,		0x0000,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x1004 - 0x0001,	0x1002 - 0x0001,	0x0004 - 0x0001,	0x0002 - 0x0001,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x1FFF,		0x0FFF,		0x1005,		0x0005,		0x0000,		0x0000,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x1005,		0x0005,		0x0000,		0x0000,		0x0000,		0x0000,		0x0000,		0x0000,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x0000,		0x0000,		0x1004,		0x1002,		0x0004,		0x0002
+};
+
+static ReverbParamsSW reverb_params_Delay = {
+	0x18040/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x7FFF,		0x7FFF,		0x0000,		0x0000,		0x0000,		0x0000,
+//	gReva		gRevb		gInputL		gInputR
+	0x0000,		0x0000,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x1004 - 0x0001,	0x1002 - 0x0001,	0x0004 - 0x0001,	0x0002 - 0x0001,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x1FFF,		0x0FFF,		0x1005,		0x0005,		0x0000,		0x0000,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x1005,		0x0005,		0x0000,		0x0000,		0x0000,		0x0000,		0x0000,		0x0000,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x0000,		0x0000,		0x1004,		0x1002,		0x0004,		0x0002
+};
+
+static ReverbParamsSW reverb_params_HalfEcho = {
+	0x3C00/2,
+//	gLPF		gEcho0		gEcho1		gEcho2		gEcho3		gWall
+	0x70F0,		0x4FA8,		-0x4320,	0x4510,		-0x4110,	-0x7B00,
+//	gReva		gRevb		gInputL		gInputR
+	0x5F80,		0x54C0,		-0x8000,	-0x8000,
+//	nRevaOldL			nRevaOldR			nRevbOldL			nRevbOldR
+	0x0058 - 0x0017,	0x0040 - 0x0017,	0x0028 - 0x0013,	0x0014 - 0x0013,
+//	nLwlNew		nRwrNew		nEcho0L		nEcho0R		nEcho1L		nEcho1R
+	0x0371,		0x02AF,		0x02E5,		0x01DF,		0x02B0,		0x01D7,
+//	nLwlOld		nRwrOld		nLwrNew		nRwlNew		nEcho2L		nEcho2R		nEcho3L		nEcho3R
+	0x0358,		0x026A,		0x01D6,		0x011E,		0x012D,		0x00B1,		0x011F,		0x0059,
+//	nLwrOld		nRwlOld		nRevaNewL	nRevaNewR	nRevbNewL	nRevbNewR
+	0x01A0,		0x00E3,		0x0058,		0x0040,		0x0028,		0x0014
+};
+
+
+static ReverbParamsSW * reverb_param_modes[] = {
+	&reverb_params_Room,
+	&reverb_params_StudioSmall,
+	&reverb_params_StudioMedium,
+	&reverb_params_StudioLarge,
+	&reverb_params_Hall,
+	&reverb_params_SpaceEcho,
+	&reverb_params_Echo,
+	&reverb_params_Delay,
+	&reverb_params_HalfEcho,
+};
+
+bool ReverbSW::process(int *p_input,int *p_output,int p_frames,int p_stereo_stride) {
+
+	if (!reverb_buffer)
+		return false;
+
+
+		//
+	// p_input must point to a non-looping buffer.
+	// BOTH p_input and p_output must be touched (use ClearModuleBuffer).
+
+	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>LOCAL MACROS <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۲
+
+#undef LM_SETSRCOFFSET
+#define LM_SETSRCOFFSET(x)								\
+			(x) = current_params->x + Offset;									\
+			if ( (x) >= reverb_buffer_size ) {						\
+				(x) -= reverb_buffer_size;							\
+	}												\
+			SETMIN ( aSample, reverb_buffer_size - (x) );
+
+/*
+#undef LM_SETSRCOFFSET2
+#define LM_SETSRCOFFSET2(x,y)							\
+			(x) = ((y) << 3) >> HZShift;					\
+			(x) += Offset;									\
+			if ( (x) >= reverb_buffer_size ) {						\
+				(x) -= reverb_buffer_size;							\
+	}												\
+			SETMIN ( aSample, reverb_buffer_size - (x) );
+*/
+#undef LM_SRCADVANCE
+#define LM_SRCADVANCE(x)								\
+			(x) += aSample;
+
+#undef LM_MUL
+#define LM_MUL(x,y)										\
+MULSHIFT_S32 ( x, current_params->y, 15 )
+
+#undef LM_REVERB
+#define LM_REVERB(x) reverb_buffer[ (x) + cSample ]
+
+	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>LOCAL VARIABLES <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۲
+
+	unsigned int Offset;
+
+	int lwl, lwr, rwl, rwr;
+//	unsigned char HZShift;
+
+	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>CODE <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۲
+
+
+	lwl = state.lwl;
+	lwr = state.lwr;
+	rwl = state.rwl;
+	rwr = state.rwr;
+	Offset = state.Offset;
+
+	int max=0;
+
+	while ( p_frames ) {
+
+		// Offsets
+
+		unsigned int nLwlOld;
+		unsigned int nRwrOld;
+		unsigned int nLwlNew;
+		unsigned int nRwrNew;
+
+		unsigned int nLwrOld;
+		unsigned int nRwlOld;
+		unsigned int nLwrNew;
+		unsigned int nRwlNew;
+
+		unsigned int nEcho0L;
+		unsigned int nEcho1L;
+		unsigned int nEcho2L;
+		unsigned int nEcho3L;
+
+		unsigned int nEcho0R;
+		unsigned int nEcho1R;
+		unsigned int nEcho2R;
+		unsigned int nEcho3R;
+
+		unsigned int nRevaOldL;
+		unsigned int nRevaOldR;
+		unsigned int nRevbOldL;
+		unsigned int nRevbOldR;
+
+		unsigned int nRevaNewL;
+		unsigned int nRevaNewR;
+		unsigned int nRevbNewL;
+		unsigned int nRevbNewR;
+
+		// Other variables
+
+		unsigned int aSample = p_frames;
+
+		// Set initial offsets
+
+		LM_SETSRCOFFSET ( nLwlOld );
+		LM_SETSRCOFFSET ( nRwrOld );
+		LM_SETSRCOFFSET ( nLwlNew );
+		LM_SETSRCOFFSET ( nRwrNew );
+		LM_SETSRCOFFSET ( nLwrOld );
+		LM_SETSRCOFFSET ( nRwlOld );
+		LM_SETSRCOFFSET ( nLwrNew );
+		LM_SETSRCOFFSET ( nRwlNew );
+		LM_SETSRCOFFSET ( nEcho0L );
+		LM_SETSRCOFFSET ( nEcho1L );
+		LM_SETSRCOFFSET ( nEcho2L );
+		LM_SETSRCOFFSET ( nEcho3L );
+		LM_SETSRCOFFSET ( nEcho0R );
+		LM_SETSRCOFFSET ( nEcho1R );
+		LM_SETSRCOFFSET ( nEcho2R );
+		LM_SETSRCOFFSET ( nEcho3R );
+		LM_SETSRCOFFSET ( nRevaOldL );
+		LM_SETSRCOFFSET ( nRevaOldR );
+		LM_SETSRCOFFSET ( nRevbOldL );
+		LM_SETSRCOFFSET ( nRevbOldR );
+		LM_SETSRCOFFSET ( nRevaNewL );
+		LM_SETSRCOFFSET ( nRevaNewR );
+		LM_SETSRCOFFSET ( nRevbNewL );
+		LM_SETSRCOFFSET ( nRevbNewR );
+
+		//SETMIN ( aSample, p_output.Size - p_output.Offset );
+
+		for (unsigned int cSample=0;cSample<aSample;cSample++) {
+
+			int tempL0, tempL1, tempR0, tempR1;
+
+			tempL1 = p_input[(cSample<<p_stereo_stride)]>>8;
+			tempR1 = p_input[(cSample<<p_stereo_stride) + 1]>>8;
+
+			tempL0 = LM_MUL ( tempL1, gInputL );
+			tempR0 = LM_MUL ( tempR1, gInputR );
+
+			/*
+			Left -> Wall -> Left Reflection
+			*/
+			tempL1 = tempL0 + LM_MUL ( LM_REVERB( nLwlOld ), gWall );
+			tempR1 = tempR0 + LM_MUL ( LM_REVERB( nRwrOld ), gWall );
+			lwl += LM_MUL ( tempL1 - lwl, gLPF );
+			rwr += LM_MUL ( tempR1 - rwr, gLPF );
+			LM_REVERB( nLwlNew ) = lwl;
+			LM_REVERB( nRwrNew ) = rwr;
+			/*
+			Left -> Wall -> Right Reflection
+			*/
+			tempL1 = tempL0 + LM_MUL ( LM_REVERB( nRwlOld ), gWall );
+			tempR1 = tempR0 + LM_MUL ( LM_REVERB( nLwrOld ), gWall );
+			lwr += LM_MUL ( tempL1 - lwr, gLPF );
+			rwl += LM_MUL ( tempR1 - rwl, gLPF );
+			LM_REVERB( nLwrNew ) = lwr;
+			LM_REVERB( nRwlNew ) = rwl;
+			/*
+			Early Echo(Early Reflection)
+			*/
+			tempL0 =
+					LM_MUL ( LM_REVERB( nEcho0L ), gEcho0 ) +
+					LM_MUL ( LM_REVERB( nEcho1L ), gEcho1 ) +
+					LM_MUL ( LM_REVERB( nEcho2L ), gEcho2 ) +
+					LM_MUL ( LM_REVERB( nEcho3L ), gEcho3 );
+			tempR0 =
+					LM_MUL ( LM_REVERB( nEcho0R ), gEcho0 ) +
+					LM_MUL ( LM_REVERB( nEcho1R ), gEcho1 ) +
+					LM_MUL ( LM_REVERB( nEcho2R ), gEcho2 ) +
+					LM_MUL ( LM_REVERB( nEcho3R ), gEcho3 );
+			/*
+			Late Reverb
+			*/
+			tempL1 = LM_REVERB( nRevaOldL );
+			tempR1 = LM_REVERB( nRevaOldR );
+			tempL0 -= LM_MUL ( tempL1, gReva );
+			tempR0 -= LM_MUL ( tempR1, gReva );
+			LM_REVERB( nRevaNewL ) = tempL0;
+			LM_REVERB( nRevaNewR ) = tempR0;
+			tempL0 = LM_MUL ( tempL0, gReva ) + tempL1;
+			tempR0 = LM_MUL ( tempR0, gReva ) + tempR1;
+			tempL1 = LM_REVERB( nRevbOldL );
+			tempR1 = LM_REVERB( nRevbOldR );
+			tempL0 -= LM_MUL ( tempL1, gRevb );
+			tempR0 -= LM_MUL ( tempR1, gRevb );
+			LM_REVERB( nRevbNewL ) = tempL0;
+			LM_REVERB( nRevbNewR ) = tempR0;
+			tempL0 = LM_MUL ( tempL0, gRevb ) + tempL1;
+			tempR0 = LM_MUL ( tempR0, gRevb ) + tempR1;
+			/*
+			Output
+			*/
+
+			max|=abs(tempL0);
+			max|=abs(tempR0);
+
+			p_output[(cSample<<p_stereo_stride)] += tempL0<<8;
+			p_output[(cSample<<p_stereo_stride) + 1] += tempR0<<8;
+
+		}
+
+		// Advance offsets
+
+		Offset += aSample;
+		if ( Offset >= reverb_buffer_size ) { Offset -= reverb_buffer_size; }
+
+		p_input += aSample << p_stereo_stride;
+		p_output += aSample << p_stereo_stride;
+
+		p_frames -= aSample;
+	}
+
+	state.lwl = lwl;
+	state.lwr = lwr;
+	state.rwl = rwl;
+	state.rwr = rwr;
+	state.Offset = Offset;
+
+	return (max&0x7FFFFF00)!=0; // audio was mixed?
+}
+
+void ReverbSW::adjust_current_params() {
+
+	*current_params=*reverb_param_modes[mode];
+
+	uint32_t maxofs=0;
+
+#define LM_CONFIG_PARAM( x )\
+	current_params->x=(int)( ( (int64_t)current_params->x*(int64_t)mix_rate*8L)/(int64_t)44100);\
+	if (current_params->x>maxofs)\
+		maxofs=current_params->x;
+
+
+	LM_CONFIG_PARAM ( nLwlOld );
+	LM_CONFIG_PARAM ( nRwrOld );
+	LM_CONFIG_PARAM ( nLwlNew );
+	LM_CONFIG_PARAM ( nRwrNew );
+	LM_CONFIG_PARAM ( nLwrOld );
+	LM_CONFIG_PARAM ( nRwlOld );
+	LM_CONFIG_PARAM ( nLwrNew );
+	LM_CONFIG_PARAM ( nRwlNew );
+	LM_CONFIG_PARAM ( nEcho0L );
+	LM_CONFIG_PARAM ( nEcho1L );
+	LM_CONFIG_PARAM ( nEcho2L );
+	LM_CONFIG_PARAM ( nEcho3L );
+	LM_CONFIG_PARAM ( nEcho0R );
+	LM_CONFIG_PARAM ( nEcho1R );
+	LM_CONFIG_PARAM ( nEcho2R );
+	LM_CONFIG_PARAM ( nEcho3R );
+	LM_CONFIG_PARAM ( nRevaOldL );
+	LM_CONFIG_PARAM ( nRevaOldR );
+	LM_CONFIG_PARAM ( nRevbOldL );
+	LM_CONFIG_PARAM ( nRevbOldR );
+	LM_CONFIG_PARAM ( nRevaNewL );
+	LM_CONFIG_PARAM ( nRevaNewR );
+	LM_CONFIG_PARAM ( nRevbNewL );
+	LM_CONFIG_PARAM ( nRevbNewR );
+
+	int needed_buffer_size=maxofs+1;
+	if (reverb_buffer)
+		memdelete_arr(reverb_buffer);
+
+	reverb_buffer = memnew_arr(int,needed_buffer_size);
+	reverb_buffer_size=needed_buffer_size;
+
+	for (uint32_t i=0;i<reverb_buffer_size;i++)
+		reverb_buffer[i]=0;
+
+	state.reset();
+
+
+}
+
+void ReverbSW::set_mode(ReverbMode p_mode) {
+
+	if (mode==p_mode)
+		return;
+
+	mode=p_mode;
+
+	adjust_current_params();
+}
+
+void ReverbSW::set_mix_rate(int p_mix_rate) {
+
+	if (p_mix_rate==mix_rate)
+		return;
+
+	mix_rate=p_mix_rate;
+
+	adjust_current_params();
+
+}
+
+
+ReverbSW::ReverbSW() {
+
+	reverb_buffer=0;
+	reverb_buffer_size=0;
+	mode=REVERB_MODE_ROOM;
+	mix_rate=1;
+	current_params = memnew(ReverbParamsSW);
+}
+
+
+ReverbSW::~ReverbSW() {
+
+
+	if (reverb_buffer)
+		memdelete_arr(reverb_buffer);
+
+	memdelete(current_params);
+
+}
+

servers/audio/reverb_sw.h

@@ -0,0 +1,84 @@
+/*************************************************************************/
+/*  reverb_sw.h                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef REVERB_SW_H
+#define REVERB_SW_H
+
+#include "typedefs.h"
+#include "os/memory.h"
+
+class ReverbParamsSW;
+
+class ReverbSW {
+public:
+	enum ReverbMode {
+		REVERB_MODE_ROOM,
+		REVERB_MODE_STUDIO_SMALL,
+		REVERB_MODE_STUDIO_MEDIUM,
+		REVERB_MODE_STUDIO_LARGE,
+		REVERB_MODE_HALL,
+		REVERB_MODE_SPACE_ECHO,
+		REVERB_MODE_ECHO,
+		REVERB_MODE_DELAY,
+		REVERB_MODE_HALF_ECHO
+	};
+
+private:
+	struct State {
+		int lwl;
+		int lwr;
+		int rwl;
+		int rwr;
+		unsigned int Offset;
+		void reset() { lwl=0; lwr=0; rwl=0; rwr=0; Offset=0; }
+		State() { reset(); }
+	} state;
+
+	ReverbParamsSW *current_params;
+
+
+	int *reverb_buffer;
+	unsigned int reverb_buffer_size;
+	ReverbMode mode;
+	int mix_rate;
+
+	void adjust_current_params();
+
+public:
+
+
+	void set_mode(ReverbMode p_mode);
+	bool process(int *p_input,int *p_output,int p_frames,int p_stereo_stride=1); // return tru if audio was created
+	void set_mix_rate(int p_mix_rate);
+
+	ReverbSW();
+	~ReverbSW();
+
+};
+
+#endif

servers/audio/sample_manager_sw.cpp

@@ -0,0 +1,280 @@
+/*************************************************************************/
+/*  sample_manager_sw.cpp                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "sample_manager_sw.h"
+
+#include "print_string.h"
+
+SampleManagerSW::~SampleManagerSW()
+{
+}
+
+
+
+RID SampleManagerMallocSW::sample_create(AS::SampleFormat p_format, bool p_stereo, int p_length) {
+
+	ERR_EXPLAIN("IMA-ADPCM and STEREO are not a valid combination for sample format.");
+	ERR_FAIL_COND_V( p_format == AS::SAMPLE_FORMAT_IMA_ADPCM && p_stereo,RID());
+	Sample *s = memnew( Sample );
+	int datalen = p_length;
+	if (p_stereo)
+		datalen*=2;
+	if (p_format==AS::SAMPLE_FORMAT_PCM16)
+		datalen*=2;
+	else if (p_format==AS::SAMPLE_FORMAT_IMA_ADPCM)
+		datalen/=2;
+#define SAMPLE_EXTRA 16
+
+	s->data = memalloc(datalen+SAMPLE_EXTRA); //help the interpolator by allocating a little more..
+	for(int i=0;i<SAMPLE_EXTRA;i++) {
+
+		uint8_t *data = (uint8_t*)s->data;
+		data[datalen+i]=0;
+	}
+	if (!s->data) {
+
+		memdelete(s);
+		ERR_EXPLAIN("Cannot allocate sample of requested size.");
+		ERR_FAIL_V(RID());
+	}
+
+	s->format=p_format;
+	s->length=p_length;
+	s->length_bytes=datalen;
+	s->stereo=p_stereo;
+	s->loop_begin=0;
+	s->loop_end=0;
+	s->loop_format=AS::SAMPLE_LOOP_NONE;
+	s->mix_rate=44100;
+
+	AudioServer::get_singleton()->lock();
+	RID rid = sample_owner.make_rid(s);
+	AudioServer::get_singleton()->unlock();
+
+	return rid;
+}
+
+void SampleManagerMallocSW::sample_set_description(RID p_sample, const String& p_description) {
+
+	Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND(!s);
+
+	s->description=p_description;
+}
+
+String SampleManagerMallocSW::sample_get_description(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,String());
+
+	return s->description;
+}
+
+
+AS::SampleFormat SampleManagerMallocSW::sample_get_format(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,AS::SAMPLE_FORMAT_PCM8);
+
+	return s->format;
+}
+
+bool SampleManagerMallocSW::sample_is_stereo(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,false);
+
+	return s->stereo;
+
+}
+int SampleManagerMallocSW::sample_get_length(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,-1);
+
+	return s->length;
+}
+
+void SampleManagerMallocSW::sample_set_data(RID p_sample, const DVector<uint8_t>& p_buffer) {
+
+	Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND(!s);
+
+	int buff_size=p_buffer.size();
+	ERR_FAIL_COND(buff_size==0);
+
+	ERR_EXPLAIN("Sample buffer size does not match sample size.");
+	ERR_FAIL_COND(s->length_bytes!=buff_size);
+	DVector<uint8_t>::Read buffer_r=p_buffer.read();
+	const uint8_t *src = buffer_r.ptr();
+	uint8_t *dst = (uint8_t*)s->data;
+
+	for(int i=0;i<s->length_bytes;i++) {
+
+		dst[i]=src[i];
+	}
+
+}
+
+const DVector<uint8_t> SampleManagerMallocSW::sample_get_data(RID p_sample) const {
+
+	Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,DVector<uint8_t>());
+
+	DVector<uint8_t> ret_buffer;
+	ret_buffer.resize(s->length_bytes);
+	DVector<uint8_t>::Write buffer_w=ret_buffer.write();
+	uint8_t *dst = buffer_w.ptr();
+	const uint8_t *src = (const uint8_t*)s->data;
+
+	for(int i=0;i<s->length_bytes;i++) {
+
+		dst[i]=src[i];
+	}
+
+	buffer_w = DVector<uint8_t>::Write(); //unlock
+
+	return ret_buffer;
+}
+
+void *SampleManagerMallocSW::sample_get_data_ptr(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,NULL);
+
+	return s->data;
+
+}
+
+void SampleManagerMallocSW::sample_set_mix_rate(RID p_sample,int p_rate) {
+
+	ERR_FAIL_COND(p_rate<1);
+
+	Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND(!s);
+
+	s->mix_rate=p_rate;
+
+
+}
+int SampleManagerMallocSW::sample_get_mix_rate(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,-1);
+
+	return s->mix_rate;
+
+}
+void SampleManagerMallocSW::sample_set_loop_format(RID p_sample,AS::SampleLoopFormat p_format) {
+
+	Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND(!s);
+
+	s->loop_format=p_format;
+
+}
+AS::SampleLoopFormat SampleManagerMallocSW::sample_get_loop_format(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,AS::SAMPLE_LOOP_NONE);
+
+	return s->loop_format;
+}
+
+void SampleManagerMallocSW::sample_set_loop_begin(RID p_sample,int p_pos) {
+
+	Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND(!s);
+	ERR_FAIL_INDEX(p_pos,s->length);
+
+	s->loop_begin=p_pos;
+}
+int SampleManagerMallocSW::sample_get_loop_begin(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,-1);
+
+	return s->loop_begin;
+}
+
+void SampleManagerMallocSW::sample_set_loop_end(RID p_sample,int p_pos) {
+
+	Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND(!s);
+	if (p_pos>s->length)
+		p_pos=s->length;
+	s->loop_end=p_pos;
+
+}
+int SampleManagerMallocSW::sample_get_loop_end(RID p_sample) const {
+
+	const Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND_V(!s,-1);
+
+	return s->loop_end;
+}
+
+bool SampleManagerMallocSW::is_sample(RID p_sample) const {
+
+	return sample_owner.owns(p_sample);
+
+}
+void SampleManagerMallocSW::free(RID p_sample) {
+
+	Sample *s = sample_owner.get(p_sample);
+	ERR_FAIL_COND(!s);
+	AudioServer::get_singleton()->lock();
+	sample_owner.free(p_sample);
+	AudioServer::get_singleton()->unlock();
+
+	memfree(s->data);
+	memdelete(s);
+
+}
+
+SampleManagerMallocSW::SampleManagerMallocSW() {
+
+
+}
+
+SampleManagerMallocSW::~SampleManagerMallocSW() {
+
+	// check for sample leakage
+	List<RID> owned_list;
+	sample_owner.get_owned_list(&owned_list);
+
+	while(owned_list.size()) {
+
+		Sample *s = sample_owner.get(owned_list.front()->get());
+		String err="Leaked sample of size: "+itos(s->length_bytes)+" description: "+s->description;
+		ERR_PRINT(err.utf8().get_data());
+		free(owned_list.front()->get());
+		owned_list.pop_front();
+	}
+
+}

servers/audio/sample_manager_sw.h

@@ -0,0 +1,129 @@
+/*************************************************************************/
+/*  sample_manager_sw.h                                                  */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef SAMPLE_MANAGER_SW_H
+#define SAMPLE_MANAGER_SW_H
+
+#include "servers/audio_server.h"
+
+class SampleManagerSW {
+public:
+
+	/* SAMPLE API */
+
+	virtual RID sample_create(AS::SampleFormat p_format, bool p_stereo, int p_length)=0;
+
+	virtual void sample_set_description(RID p_sample, const String& p_description)=0;
+	virtual String sample_get_description(RID p_sample) const=0;
+
+	virtual AS::SampleFormat sample_get_format(RID p_sample) const=0;
+	virtual bool sample_is_stereo(RID p_sample) const=0;
+	virtual int sample_get_length(RID p_sample) const=0;
+
+	virtual void sample_set_data(RID p_sample, const DVector<uint8_t>& p_buffer)=0;
+	virtual const DVector<uint8_t> sample_get_data(RID p_sample) const=0;
+
+	virtual void *sample_get_data_ptr(RID p_sample) const=0;
+
+	virtual void sample_set_mix_rate(RID p_sample,int p_rate)=0;
+	virtual int sample_get_mix_rate(RID p_sample) const=0;
+
+	virtual void sample_set_loop_format(RID p_sample,AS::SampleLoopFormat p_format)=0;
+	virtual AS::SampleLoopFormat sample_get_loop_format(RID p_sample) const=0;
+
+	virtual void sample_set_loop_begin(RID p_sample,int p_pos)=0;
+	virtual int sample_get_loop_begin(RID p_sample) const=0;
+
+	virtual void sample_set_loop_end(RID p_sample,int p_pos)=0;
+	virtual int sample_get_loop_end(RID p_sample) const=0;
+
+	virtual bool is_sample(RID) const=0;
+	virtual void free(RID p_sample)=0;
+
+
+
+	virtual ~SampleManagerSW();
+};
+
+
+class SampleManagerMallocSW : public SampleManagerSW {
+
+
+	struct Sample {
+
+		void *data;
+		int length;
+		int length_bytes;
+		AS::SampleFormat format;
+		bool stereo;
+		AS::SampleLoopFormat loop_format;
+		int loop_begin;
+		int loop_end;
+		int mix_rate;
+		String description;
+	};
+
+	mutable RID_Owner<Sample> sample_owner;
+public:
+
+	/* SAMPLE API */
+
+	virtual RID sample_create(AS::SampleFormat p_format, bool p_stereo, int p_length);
+
+	virtual void sample_set_description(RID p_sample, const String& p_description);
+	virtual String sample_get_description(RID p_sample) const;
+
+	virtual AS::SampleFormat sample_get_format(RID p_sample) const;
+	virtual bool sample_is_stereo(RID p_sample) const;
+	virtual int sample_get_length(RID p_sample) const;
+
+	virtual void sample_set_data(RID p_sample, const DVector<uint8_t>& p_buffer);
+	virtual const DVector<uint8_t> sample_get_data(RID p_sample) const;
+
+	virtual void *sample_get_data_ptr(RID p_sample) const;
+
+	virtual void sample_set_mix_rate(RID p_sample,int p_rate);
+	virtual int sample_get_mix_rate(RID p_sample) const;
+
+	virtual void sample_set_loop_format(RID p_sample,AS::SampleLoopFormat p_format);
+	virtual AS::SampleLoopFormat sample_get_loop_format(RID p_sample) const;
+
+	virtual void sample_set_loop_begin(RID p_sample,int p_pos);
+	virtual int sample_get_loop_begin(RID p_sample) const;
+
+	virtual void sample_set_loop_end(RID p_sample,int p_pos);
+	virtual int sample_get_loop_end(RID p_sample) const;
+
+	virtual bool is_sample(RID) const;
+	virtual void free(RID p_sample);
+
+	SampleManagerMallocSW();
+	virtual ~SampleManagerMallocSW();
+};
+
+#endif // SAMPLE_MANAGER_SW_H

servers/audio/voice_rb_sw.cpp

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

servers/audio/voice_rb_sw.h

@@ -0,0 +1,146 @@
+/*************************************************************************/
+/*  voice_rb_sw.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef VOICE_RB_SW_H
+#define VOICE_RB_SW_H
+
+#include "servers/audio_server.h"
+#include "os/os.h"
+class VoiceRBSW {
+public:
+
+	enum {
+		VOICE_RB_SIZE=1024
+	};
+
+	struct Command {
+
+		enum Type {
+			CMD_NONE,
+			CMD_PLAY,
+			CMD_STOP,
+			CMD_SET_VOLUME,
+			CMD_SET_PAN,
+			CMD_SET_FILTER,
+			CMD_SET_CHORUS,
+			CMD_SET_REVERB,
+			CMD_SET_MIX_RATE,
+			CMD_SET_POSITIONAL,
+			CMD_CHANGE_ALL_FX_VOLUMES
+		};
+
+		Type type;
+		RID voice;
+
+		struct {
+
+			RID sample;
+
+		} play;
+
+		union {
+
+			struct {
+
+				float volume;
+			} volume;
+
+			struct {
+
+				float pan,depth,height;
+			} pan;
+
+			struct {
+
+				AS::FilterType type;
+				float cutoff;
+				float resonance;
+				float gain;
+			} filter;
+
+			struct {
+				float send;
+			} chorus;
+			struct {
+				float send;
+				AS::ReverbRoomType room;
+			} reverb;
+
+			struct {
+
+				int mix_rate;
+			} mix_rate;
+
+			struct {
+
+				bool positional;
+			} positional;
+
+		};
+
+		Command() { type=CMD_NONE; }
+
+	};
+private:
+
+	Command voice_cmd_rb[VOICE_RB_SIZE];
+	volatile int read_pos;
+	volatile int write_pos;
+
+public:
+
+	_FORCE_INLINE_ bool commands_left() const { return read_pos!=write_pos; }
+	_FORCE_INLINE_ Command pop_command() {
+		ERR_FAIL_COND_V( read_pos==write_pos, Command() );
+		Command cmd=voice_cmd_rb[read_pos];
+		read_pos=(read_pos+1)%VOICE_RB_SIZE;
+		return cmd;
+	}
+	_FORCE_INLINE_ void push_command(const Command& p_command) {
+
+		bool full = ((write_pos+1)%VOICE_RB_SIZE)==read_pos;
+		if (full) {
+#ifdef DEBUG_ENABLED
+			if (OS::get_singleton()->is_stdout_verbose()) {
+				ERR_EXPLAIN("Audio Ring Buffer Full (too many commands");
+				ERR_FAIL_COND( ((write_pos+1)%VOICE_RB_SIZE)==read_pos);
+			}
+#endif
+			return;
+		}
+
+		voice_cmd_rb[write_pos]=p_command;
+		write_pos=(write_pos+1)%VOICE_RB_SIZE;
+
+	}
+
+	VoiceRBSW() { read_pos=write_pos=0; }
+
+};
+
+#endif // VOICE_RB_SW_H

servers/audio_server.cpp

@@ -0,0 +1,178 @@
+/*************************************************************************/
+/*  audio_server.cpp                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "audio_server.h"
+#include "globals.h"
+
+void AudioMixer::audio_mixer_chunk_call(int p_frames) {
+
+	AudioServer::get_singleton()->audio_mixer_chunk_callback(p_frames);
+}
+
+AudioMixer *AudioServer::EventStream::get_mixer() const {
+
+	return AudioServer::get_singleton()->get_mixer();
+}
+
+AudioServer *AudioServer::singleton=NULL;
+
+AudioServer *AudioServer::get_singleton() {
+
+	return singleton;
+}
+
+void AudioServer::sample_set_signed_data(RID p_sample, const DVector<float>& p_buffer) {
+
+	int len = p_buffer.size();
+	ERR_FAIL_COND( len == 0 );
+
+	DVector<uint8_t> data;
+	data.resize(len*2);
+	DVector<uint8_t>::Write w=data.write();
+
+	int16_t *samples = (int16_t*)w.ptr();
+
+	DVector<float>::Read r = p_buffer.read();
+
+	for(int i=0;i<len;i++) {
+
+		float sample = r[i];
+		sample = Math::floor( sample * (1<<16) );
+		if (sample<-32768)
+			sample=-32768;
+		else if (sample>32767)
+			sample=32767;
+		samples[i]=sample;
+	}
+
+	w = DVector<uint8_t>::Write();
+
+	sample_set_data(p_sample,data);
+
+
+}
+
+void AudioServer::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("sample_create","format","stereo","length"), &AudioServer::sample_create );
+	ObjectTypeDB::bind_method(_MD("sample_set_description","sample","description"), &AudioServer::sample_set_description );
+	ObjectTypeDB::bind_method(_MD("sample_get_description","sample"), &AudioServer::sample_get_description );
+
+	ObjectTypeDB::bind_method(_MD("sample_get_format","sample"), &AudioServer::sample_get_format );
+	ObjectTypeDB::bind_method(_MD("sample_is_stereo","sample"), &AudioServer::sample_is_stereo );
+	ObjectTypeDB::bind_method(_MD("sample_get_length","sample"), &AudioServer::sample_get_length );
+
+	ObjectTypeDB::bind_method(_MD("sample_set_signed_data","sample","data"), &AudioServer::sample_set_signed_data );
+	ObjectTypeDB::bind_method(_MD("sample_set_data","sample"), &AudioServer::sample_set_data );
+	ObjectTypeDB::bind_method(_MD("sample_get_data","sample"), &AudioServer::sample_get_data );
+
+	ObjectTypeDB::bind_method(_MD("sample_set_mix_rate","sample","mix_rate"), &AudioServer::sample_set_mix_rate );
+	ObjectTypeDB::bind_method(_MD("sample_get_mix_rate","sample"), &AudioServer::sample_get_mix_rate );
+
+	ObjectTypeDB::bind_method(_MD("sample_set_loop_format","sample","loop_format"), &AudioServer::sample_set_loop_format );
+	ObjectTypeDB::bind_method(_MD("sample_get_loop_format","sample"), &AudioServer::sample_get_loop_format );
+
+
+	ObjectTypeDB::bind_method(_MD("sample_set_loop_begin","sample","pos"), &AudioServer::sample_set_loop_begin );
+	ObjectTypeDB::bind_method(_MD("sample_get_loop_begin","sample"), &AudioServer::sample_get_loop_begin );
+
+	ObjectTypeDB::bind_method(_MD("sample_set_loop_end","sample","pos"), &AudioServer::sample_set_loop_end );
+	ObjectTypeDB::bind_method(_MD("sample_get_loop_end","sample"), &AudioServer::sample_get_loop_end );
+
+
+
+	ObjectTypeDB::bind_method(_MD("voice_create"), &AudioServer::voice_create );
+	ObjectTypeDB::bind_method(_MD("voice_play","voice","sample"), &AudioServer::voice_play );
+	ObjectTypeDB::bind_method(_MD("voice_set_volume","voice","volume"), &AudioServer::voice_set_volume );
+	ObjectTypeDB::bind_method(_MD("voice_set_pan","voice","pan","depth","height"), &AudioServer::voice_set_pan,DEFVAL(0),DEFVAL(0) );
+	ObjectTypeDB::bind_method(_MD("voice_set_filter","voice","type","cutoff","resonance","gain"), &AudioServer::voice_set_filter,DEFVAL(0) );
+	ObjectTypeDB::bind_method(_MD("voice_set_chorus","voice","chorus"), &AudioServer::voice_set_chorus );
+	ObjectTypeDB::bind_method(_MD("voice_set_reverb","voice","room","reverb"), &AudioServer::voice_set_reverb );
+	ObjectTypeDB::bind_method(_MD("voice_set_mix_rate","voice","rate"), &AudioServer::voice_set_mix_rate );
+	ObjectTypeDB::bind_method(_MD("voice_set_positional","voice","enabled"), &AudioServer::voice_set_positional );
+
+
+	ObjectTypeDB::bind_method(_MD("voice_get_volume","voice"), &AudioServer::voice_get_volume );
+	ObjectTypeDB::bind_method(_MD("voice_get_pan","voice"), &AudioServer::voice_get_pan );
+	ObjectTypeDB::bind_method(_MD("voice_get_pan_height","voice"), &AudioServer::voice_get_pan_height );
+	ObjectTypeDB::bind_method(_MD("voice_get_pan_depth","voice"), &AudioServer::voice_get_pan_depth );
+	ObjectTypeDB::bind_method(_MD("voice_get_filter_type","voice"), &AudioServer::voice_get_filter_type );
+	ObjectTypeDB::bind_method(_MD("voice_get_filter_cutoff","voice"), &AudioServer::voice_get_filter_cutoff );
+	ObjectTypeDB::bind_method(_MD("voice_get_filter_resonance","voice"), &AudioServer::voice_get_filter_resonance );
+	ObjectTypeDB::bind_method(_MD("voice_get_chorus","voice"), &AudioServer::voice_get_chorus );
+	ObjectTypeDB::bind_method(_MD("voice_get_reverb_type","voice"), &AudioServer::voice_get_reverb_type );
+	ObjectTypeDB::bind_method(_MD("voice_get_reverb","voice"), &AudioServer::voice_get_reverb );
+	ObjectTypeDB::bind_method(_MD("voice_get_mix_rate","voice"), &AudioServer::voice_get_mix_rate );
+	ObjectTypeDB::bind_method(_MD("voice_is_positional","voice"), &AudioServer::voice_is_positional );
+
+	ObjectTypeDB::bind_method(_MD("voice_stop","voice"), &AudioServer::voice_stop );
+
+	ObjectTypeDB::bind_method(_MD("free","rid"), &AudioServer::free );
+
+	ObjectTypeDB::bind_method(_MD("set_stream_global_volume_scale","scale"), &AudioServer::set_stream_global_volume_scale );
+	ObjectTypeDB::bind_method(_MD("get_stream_global_volume_scale"), &AudioServer::get_stream_global_volume_scale );
+
+	ObjectTypeDB::bind_method(_MD("set_fx_global_volume_scale","scale"), &AudioServer::set_fx_global_volume_scale );
+	ObjectTypeDB::bind_method(_MD("get_fx_global_volume_scale"), &AudioServer::get_fx_global_volume_scale );
+
+	ObjectTypeDB::bind_method(_MD("set_event_voice_global_volume_scale","scale"), &AudioServer::set_event_voice_global_volume_scale );
+	ObjectTypeDB::bind_method(_MD("get_event_voice_global_volume_scale"), &AudioServer::get_event_voice_global_volume_scale );
+
+	BIND_CONSTANT( SAMPLE_FORMAT_PCM8 );
+	BIND_CONSTANT( SAMPLE_FORMAT_PCM16 );
+	BIND_CONSTANT( SAMPLE_FORMAT_IMA_ADPCM );
+
+	BIND_CONSTANT( SAMPLE_LOOP_NONE );
+	BIND_CONSTANT( SAMPLE_LOOP_FORWARD );
+	BIND_CONSTANT( SAMPLE_LOOP_PING_PONG );
+
+	BIND_CONSTANT( FILTER_NONE );
+	BIND_CONSTANT( FILTER_LOWPASS );
+	BIND_CONSTANT( FILTER_BANDPASS );
+	BIND_CONSTANT( FILTER_HIPASS );
+	BIND_CONSTANT( FILTER_NOTCH );
+	BIND_CONSTANT( FILTER_BANDLIMIT ); ///< cutoff is LP resonace is HP
+
+	BIND_CONSTANT( REVERB_SMALL );
+	BIND_CONSTANT( REVERB_MEDIUM );
+	BIND_CONSTANT( REVERB_LARGE );
+	BIND_CONSTANT( REVERB_HALL );
+
+	GLOBAL_DEF("audio/stream_buffering_ms",500);
+
+}
+
+AudioServer::AudioServer() {
+
+	singleton=this;
+}
+
+AudioServer::~AudioServer() {
+
+
+}

servers/audio_server.h

@@ -0,0 +1,289 @@
+/*************************************************************************/
+/*  audio_server.h                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AUDIO_SERVER_H
+#define AUDIO_SERVER_H
+
+#include "variant.h"
+#include "object.h"
+
+class AudioMixer {
+protected:
+
+	void audio_mixer_chunk_call(int p_frames);
+public:
+
+	enum {
+
+		INVALID_CHANNEL=0xFFFFFFFF
+	};
+
+	typedef uint32_t ChannelID;
+
+	/* CHANNEL API */
+
+	enum FilterType {
+		FILTER_NONE,
+		FILTER_LOWPASS,
+		FILTER_BANDPASS,
+		FILTER_HIPASS,
+		FILTER_NOTCH,
+		FILTER_PEAK,
+		FILTER_BANDLIMIT, ///< cutoff is LP resonace is HP
+		FILTER_LOW_SHELF,
+		FILTER_HIGH_SHELF
+
+	};
+
+	enum ReverbRoomType {
+
+		REVERB_SMALL,
+		REVERB_MEDIUM,
+		REVERB_LARGE,
+		REVERB_HALL
+	};
+
+	virtual ChannelID channel_alloc(RID p_sample)=0;
+
+	virtual void channel_set_volume(ChannelID p_channel, float p_gain)=0;
+	virtual void channel_set_pan(ChannelID p_channel, float p_pan, float p_depth=0,float height=0)=0; //pan and depth go from -1 to 1
+	virtual void channel_set_filter(ChannelID p_channel, FilterType p_type, float p_cutoff, float p_resonance, float p_gain=1.0)=0;
+	virtual void channel_set_chorus(ChannelID p_channel, float p_chorus )=0;
+	virtual void channel_set_reverb(ChannelID p_channel, ReverbRoomType p_room_type, float p_reverb)=0;
+	virtual void channel_set_mix_rate(ChannelID p_channel, int p_mix_rate)=0;
+	virtual void channel_set_positional(ChannelID p_channel, bool p_positional)=0;
+
+	virtual float channel_get_volume(ChannelID p_channel) const=0;
+	virtual float channel_get_pan(ChannelID p_channel) const=0; //pan and depth go from -1 to 1
+	virtual float channel_get_pan_depth(ChannelID p_channel) const=0; //pan and depth go from -1 to 1
+	virtual float channel_get_pan_height(ChannelID p_channel) const=0; //pan and depth go from -1 to 1
+	virtual FilterType channel_get_filter_type(ChannelID p_channel) const=0;
+	virtual float channel_get_filter_cutoff(ChannelID p_channel) const=0;
+	virtual float channel_get_filter_resonance(ChannelID p_channel) const=0;
+	virtual float channel_get_filter_gain(ChannelID p_channel) const=0;
+	virtual float channel_get_chorus(ChannelID p_channel) const=0;
+	virtual ReverbRoomType channel_get_reverb_type(ChannelID p_channel) const=0;
+	virtual float channel_get_reverb(ChannelID p_channel) const=0;
+
+	virtual int channel_get_mix_rate(ChannelID p_channel) const=0;
+	virtual bool channel_is_positional(ChannelID p_channel) const=0;
+	virtual bool channel_is_valid(ChannelID p_channel) const=0;
+
+
+	virtual void channel_free(ChannelID p_channel)=0;
+
+	virtual void set_mixer_volume(float p_volume)=0;
+
+
+	virtual ~AudioMixer() {}
+};
+
+
+class AudioServer : public Object {
+
+	OBJ_TYPE( AudioServer, Object );
+
+	static AudioServer *singleton;
+protected:
+friend class AudioStream;
+friend class EventStream;
+friend class AudioMixer;
+
+	virtual AudioMixer *get_mixer()=0;
+	virtual void audio_mixer_chunk_callback(int p_frames)=0;
+
+	static void _bind_methods();
+public:
+
+
+	class EventStream {
+	protected:
+		AudioMixer *get_mixer() const;
+	public:
+		virtual void update(uint64_t p_usec)=0;
+
+		virtual ~EventStream() {}
+	};
+
+	class AudioStream {
+	public:
+		virtual int get_channel_count() const=0;
+		virtual void set_mix_rate(int p_rate)=0; //notify the stream of the mix rate
+		virtual bool mix(int32_t *p_buffer,int p_frames)=0;
+		virtual void update()=0;
+		virtual bool can_update_mt() const { return true; }
+		virtual ~AudioStream() {}
+	};
+
+
+	enum SampleFormat {
+
+		SAMPLE_FORMAT_PCM8,
+		SAMPLE_FORMAT_PCM16,
+		SAMPLE_FORMAT_IMA_ADPCM
+	};
+
+	enum SampleLoopFormat {
+		SAMPLE_LOOP_NONE,
+		SAMPLE_LOOP_FORWARD,
+		SAMPLE_LOOP_PING_PONG // not supported in every platform
+
+	};
+
+	/* SAMPLE API */
+
+	virtual RID sample_create(SampleFormat p_format, bool p_stereo, int p_length)=0;
+
+	virtual void sample_set_description(RID p_sample, const String& p_description)=0;
+	virtual String sample_get_description(RID p_sample, const String& p_description) const=0;
+
+	virtual SampleFormat sample_get_format(RID p_sample) const=0;
+	virtual bool sample_is_stereo(RID p_sample) const=0;
+	virtual int sample_get_length(RID p_sample) const=0;
+	virtual const void* sample_get_data_ptr(RID p_sample) const=0;
+
+	virtual void sample_set_signed_data(RID p_sample, const DVector<float>& p_buffer);
+	virtual void sample_set_data(RID p_sample, const DVector<uint8_t>& p_buffer)=0;
+	virtual const DVector<uint8_t> sample_get_data(RID p_sample) const=0;
+
+	virtual void sample_set_mix_rate(RID p_sample,int p_rate)=0;
+	virtual int sample_get_mix_rate(RID p_sample) const=0;
+
+	virtual void sample_set_loop_format(RID p_sample,SampleLoopFormat p_format)=0;
+	virtual SampleLoopFormat sample_get_loop_format(RID p_sample) const=0;
+
+	virtual void sample_set_loop_begin(RID p_sample,int p_pos)=0;
+	virtual int sample_get_loop_begin(RID p_sample) const=0;
+
+	virtual void sample_set_loop_end(RID p_sample,int p_pos)=0;
+	virtual int sample_get_loop_end(RID p_sample) const=0;
+
+
+	/* VOICE API */
+
+	enum FilterType {
+		FILTER_NONE,
+		FILTER_LOWPASS,
+		FILTER_BANDPASS,
+		FILTER_HIPASS,
+		FILTER_NOTCH,
+		FILTER_PEAK,
+		FILTER_BANDLIMIT, ///< cutoff is LP resonace is HP
+		FILTER_LOW_SHELF,
+		FILTER_HIGH_SHELF
+	};
+
+	enum ReverbRoomType {
+
+		REVERB_SMALL,
+		REVERB_MEDIUM,
+		REVERB_LARGE,
+		REVERB_HALL
+	};
+
+	virtual RID voice_create()=0;
+
+	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_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;
+	virtual void voice_set_reverb(RID p_voice, ReverbRoomType p_room_type, float p_reverb)=0;
+	virtual void voice_set_mix_rate(RID p_voice, int p_mix_rate)=0;
+	virtual void voice_set_positional(RID p_voice, bool p_positional)=0;
+
+	virtual float voice_get_volume(RID p_voice) const=0;
+	virtual float voice_get_pan(RID p_voice) const=0; //pan and depth go from -1 to 1
+	virtual float voice_get_pan_depth(RID p_voice) const=0; //pan and depth go from -1 to 1
+	virtual float voice_get_pan_height(RID p_voice) const=0; //pan and depth go from -1 to 1
+	virtual FilterType voice_get_filter_type(RID p_voice) const=0;
+	virtual float voice_get_filter_cutoff(RID p_voice) const=0;
+	virtual float voice_get_filter_resonance(RID p_voice) const=0;
+	virtual float voice_get_chorus(RID p_voice) const=0;
+	virtual ReverbRoomType voice_get_reverb_type(RID p_voice) const=0;
+	virtual float voice_get_reverb(RID p_voice) const=0;
+
+	virtual int voice_get_mix_rate(RID p_voice) const=0;
+	virtual bool voice_is_positional(RID p_voice) const=0;
+
+	virtual void voice_stop(RID p_voice)=0;
+	virtual bool voice_is_active(RID p_voice) const=0;
+
+	/* STREAM API */
+
+	virtual RID audio_stream_create(AudioStream *p_stream)=0;
+	virtual RID event_stream_create(EventStream *p_stream)=0;
+
+	virtual void stream_set_active(RID p_stream, bool p_active)=0;
+	virtual bool stream_is_active(RID p_stream) const=0;
+
+	virtual void stream_set_volume_scale(RID p_stream, float p_scale)=0;
+	virtual float stream_set_volume_scale(RID p_stream) const=0;
+
+	/* Audio Physics API */
+
+	virtual void free(RID p_id)=0;
+
+	virtual void init()=0;
+	virtual void finish()=0;
+	virtual void update()=0;
+
+	/* MISC config */
+
+	virtual void lock()=0;
+	virtual void unlock()=0;
+	virtual int get_default_channel_count() const=0;
+	virtual int get_default_mix_rate() const=0;
+
+	virtual void set_stream_global_volume_scale(float p_volume)=0;
+	virtual void set_fx_global_volume_scale(float p_volume)=0;
+	virtual void set_event_voice_global_volume_scale(float p_volume)=0;
+
+	virtual float get_stream_global_volume_scale() const=0;
+	virtual float get_fx_global_volume_scale() const=0;
+	virtual float get_event_voice_global_volume_scale() const=0;
+
+	virtual uint32_t read_output_peak() const=0;
+
+	static AudioServer *get_singleton();
+
+	virtual double get_mix_time() const=0; //useful for video -> audio sync
+
+	AudioServer();
+	virtual ~AudioServer();
+};
+
+VARIANT_ENUM_CAST( AudioServer::SampleFormat );
+VARIANT_ENUM_CAST( AudioServer::SampleLoopFormat );
+VARIANT_ENUM_CAST( AudioServer::FilterType );
+VARIANT_ENUM_CAST( AudioServer::ReverbRoomType );
+
+typedef AudioServer AS;
+
+#endif // AUDIO_SERVER_H

servers/physics/SCsub

@@ -0,0 +1,7 @@
+Import('env')
+
+env.add_source_files(env.servers_sources,"*.cpp")
+
+Export('env')
+
+

servers/physics/area_pair_sw.cpp

@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  area_pair_sw.cpp                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "area_pair_sw.h"
+#include "collision_solver_sw.h"
+
+
+bool AreaPairSW::setup(float p_step) {
+
+	bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),NULL,this);
+
+	if (result!=colliding) {
+
+		if (result) {
+
+			if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+				body->add_area(area);
+			if (area->has_monitor_callback())
+				area->add_body_to_query(body,body_shape,area_shape);
+
+		} else {
+
+			if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+				body->remove_area(area);
+			if (area->has_monitor_callback())
+				area->remove_body_from_query(body,body_shape,area_shape);
+
+		}
+
+		colliding=result;
+
+	}
+
+	return false; //never do any post solving
+}
+
+void AreaPairSW::solve(float p_step) {
+
+
+}
+
+
+AreaPairSW::AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape) {
+
+
+	body=p_body;
+	area=p_area;
+	body_shape=p_body_shape;
+	area_shape=p_area_shape;
+	colliding=false;
+	body->add_constraint(this,0);
+	area->add_constraint(this);
+
+}
+
+AreaPairSW::~AreaPairSW() {
+
+	if (colliding) {
+
+		if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+			body->remove_area(area);
+		if (area->has_monitor_callback())
+			area->remove_body_from_query(body,body_shape,area_shape);
+
+
+	}
+	body->remove_constraint(this);
+	area->remove_constraint(this);
+}

servers/physics/area_pair_sw.h

@@ -0,0 +1,53 @@
+/*************************************************************************/
+/*  area_pair_sw.h                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AREA_PAIR_SW_H
+#define AREA_PAIR_SW_H
+
+#include "constraint_sw.h"
+#include "body_sw.h"
+#include "area_sw.h"
+
+class AreaPairSW : public ConstraintSW {
+
+	BodySW *body;
+	AreaSW *area;
+	int body_shape;
+	int area_shape;
+	bool colliding;
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape);
+	~AreaPairSW();
+};
+
+#endif // AREA_PAIR__SW_H
+

servers/physics/area_sw.cpp

@@ -0,0 +1,192 @@
+/*************************************************************************/
+/*  area_sw.cpp                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "area_sw.h"
+#include "space_sw.h"
+#include "body_sw.h"
+
+AreaSW::BodyKey::BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
+
+void AreaSW::_shapes_changed() {
+
+
+}
+
+void AreaSW::set_transform(const Transform& p_transform) {
+
+	if (!moved_list.in_list() && get_space())
+		get_space()->area_add_to_moved_list(&moved_list);
+
+	_set_transform(p_transform);
+}
+
+void AreaSW::set_space(SpaceSW *p_space) {
+
+	if (get_space()) {
+		if (monitor_query_list.in_list())
+			get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+		if (moved_list.in_list())
+			get_space()->area_remove_from_moved_list(&moved_list);
+
+	}
+
+	monitored_bodies.clear();
+
+	_set_space(p_space);
+}
+
+
+void AreaSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
+
+
+	if (p_id==monitor_callback_id) {
+		monitor_callback_method=p_method;
+		return;
+	}
+
+	_unregister_shapes();
+
+	monitor_callback_id=p_id;
+	monitor_callback_method=p_method;
+
+	monitored_bodies.clear();
+
+	_shape_changed();
+
+}
+
+
+
+void AreaSW::set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) {
+	bool do_override=p_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override==(space_override_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED))
+		return;
+	_unregister_shapes();
+	space_override_mode=p_mode;
+	_shape_changed();
+}
+
+void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value) {
+
+	switch(p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:  point_attenuation=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_DENSITY: density=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_PRIORITY: priority=p_value; ; break;
+	}
+
+
+}
+
+Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const {
+
+
+	switch(p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY: return gravity;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector;
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return  point_attenuation;
+		case PhysicsServer::AREA_PARAM_DENSITY: return density;
+		case PhysicsServer::AREA_PARAM_PRIORITY: return priority;
+	}
+
+	return Variant();
+}
+
+
+void AreaSW::_queue_monitor_update() {
+
+	ERR_FAIL_COND(!get_space());
+
+	if (!monitor_query_list.in_list())
+		get_space()->area_add_to_monitor_query_list(&monitor_query_list);
+
+
+}
+
+void AreaSW::call_queries() {
+
+	if (monitor_callback_id && !monitored_bodies.empty()) {
+
+		Variant res[5];
+		Variant *resptr[5];
+		for(int i=0;i<5;i++)
+			resptr[i]=&res[i];
+
+		Object *obj = ObjectDB::get_instance(monitor_callback_id);
+		if (!obj) {
+			monitored_bodies.clear();
+			monitor_callback_id=0;
+			return;
+		}
+
+
+
+		for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) {
+
+			if (E->get().state==0)
+				continue; //nothing happened
+
+			res[0]=E->get().state>0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED;
+			res[1]=E->key().rid;
+			res[2]=E->key().instance_id;
+			res[3]=E->key().body_shape;
+			res[4]=E->key().area_shape;
+
+			Variant::CallError ce;
+			obj->call(monitor_callback_method,(const Variant**)resptr,5,ce);
+		}
+	}
+
+	monitored_bodies.clear();
+
+	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+
+}
+
+AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this),  moved_list(this) {
+
+	_set_static(true); //areas are never active
+	space_override_mode=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED;
+	gravity=9.80665;
+	gravity_vector=Vector3(0,-1,0);
+	gravity_is_point=false;
+	point_attenuation=1;
+	density=0.1;
+	priority=0;
+
+
+}
+
+AreaSW::~AreaSW() {
+
+
+}
+

servers/physics/area_sw.h

@@ -0,0 +1,172 @@
+/*************************************************************************/
+/*  area_sw.h                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AREA_SW_H
+#define AREA_SW_H
+
+#include "servers/physics_server.h"
+#include "collision_object_sw.h"
+#include "self_list.h"
+//#include "servers/physics/query_sw.h"
+
+class SpaceSW;
+class BodySW;
+class ConstraintSW;
+
+class AreaSW : public CollisionObjectSW{
+
+
+	PhysicsServer::AreaSpaceOverrideMode space_override_mode;
+	float gravity;
+	Vector3 gravity_vector;
+	bool gravity_is_point;
+	float point_attenuation;
+	float density;
+	int priority;
+
+	ObjectID monitor_callback_id;
+	StringName monitor_callback_method;
+
+	SelfList<AreaSW> monitor_query_list;
+	SelfList<AreaSW> moved_list;
+
+	struct BodyKey {
+
+		RID rid;
+		ObjectID instance_id;
+		uint32_t body_shape;
+		uint32_t area_shape;
+
+		_FORCE_INLINE_ bool operator<( const BodyKey& p_key) const {
+
+			if (rid==p_key.rid) {
+
+				if (body_shape==p_key.body_shape) {
+
+					return area_shape < p_key.area_shape;
+				} else
+					return body_shape < p_key.area_shape;
+			} else
+				return rid < p_key.rid;
+
+		}
+
+		_FORCE_INLINE_ BodyKey() {}
+		BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	};
+
+	struct BodyState {
+
+		int state;
+		_FORCE_INLINE_ void inc() { state++; }
+		_FORCE_INLINE_ void dec() { state--; }
+		_FORCE_INLINE_ BodyState() { state=0; }
+	};
+
+	Map<BodyKey,BodyState> monitored_bodies;
+
+	//virtual void shape_changed_notify(ShapeSW *p_shape);
+	//virtual void shape_deleted_notify(ShapeSW *p_shape);
+
+	Set<ConstraintSW*> constraints;
+
+
+	virtual void _shapes_changed();
+	void _queue_monitor_update();
+
+public:
+
+	//_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; }
+	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
+
+	void set_monitor_callback(ObjectID p_id, const StringName& p_method);
+	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; }
+
+	_FORCE_INLINE_ void add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+
+	void set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value);
+	Variant get_param(PhysicsServer::AreaParameter p_param) const;
+
+	void set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode);
+	PhysicsServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; }
+
+	_FORCE_INLINE_ void set_gravity(float p_gravity) { gravity=p_gravity; }
+	_FORCE_INLINE_ float get_gravity() const { return gravity; }
+
+	_FORCE_INLINE_ void set_gravity_vector(const Vector3& p_gravity) { gravity_vector=p_gravity; }
+	_FORCE_INLINE_ Vector3 get_gravity_vector() const { return gravity_vector; }
+
+	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; }
+	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
+
+	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; }
+	_FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; }
+
+	_FORCE_INLINE_ void set_density(float p_density) { density=p_density; }
+	_FORCE_INLINE_ float get_density() const { return density; }
+
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; }
+	_FORCE_INLINE_ int get_priority() const { return priority; }
+
+	_FORCE_INLINE_ void add_constraint( ConstraintSW* p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; }
+
+	void set_transform(const Transform& p_transform);
+
+	void set_space(SpaceSW *p_space);
+
+
+	void call_queries();
+
+	AreaSW();
+	~AreaSW();
+};
+
+void AreaSW::add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+
+	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	monitored_bodies[bk].inc();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
+void AreaSW::remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+
+	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	monitored_bodies[bk].dec();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
+
+
+
+
+
+
+#endif // AREA__SW_H

servers/physics/body_pair_sw.cpp

@@ -0,0 +1,442 @@
+/*************************************************************************/
+/*  body_pair_sw.cpp                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "body_pair_sw.h"
+#include "collision_solver_sw.h"
+#include "space_sw.h"
+
+
+/*
+#define NO_ACCUMULATE_IMPULSES
+#define NO_SPLIT_IMPULSES
+
+#define NO_FRICTION
+*/
+
+#define NO_TANGENTIALS
+/* BODY PAIR */
+
+
+//#define ALLOWED_PENETRATION 0.01
+#define RELAXATION_TIMESTEPS 3
+#define MIN_VELOCITY 0.0001
+
+void BodyPairSW::_contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
+
+	BodyPairSW* pair  = (BodyPairSW*)p_userdata;
+	pair->contact_added_callback(p_point_A,p_point_B);
+
+}
+
+void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B) {
+
+	// check if we already have the contact
+
+	//Vector3 local_A = A->get_inv_transform().xform(p_point_A);
+	//Vector3 local_B = B->get_inv_transform().xform(p_point_B);
+
+	Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
+	Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B-offset_B);
+
+
+
+	int new_index = contact_count;
+
+	ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) );
+
+	Contact contact;
+
+	contact.acc_normal_impulse=0;
+	contact.acc_bias_impulse=0;
+	contact.acc_tangent_impulse=Vector3();
+	contact.local_A=local_A;
+	contact.local_B=local_B;
+	contact.normal=(p_point_A-p_point_B).normalized();
+
+
+
+	// attempt to determine if the contact will be reused
+	real_t contact_recycle_radius=space->get_contact_recycle_radius();
+
+	for (int i=0;i<contact_count;i++) {
+
+		Contact& c = contacts[i];
+		if (
+		c.local_A.distance_squared_to( local_A ) < (contact_recycle_radius*contact_recycle_radius) &&
+		c.local_B.distance_squared_to( local_B ) < (contact_recycle_radius*contact_recycle_radius) ) {
+
+				contact.acc_normal_impulse=c.acc_normal_impulse;
+				contact.acc_bias_impulse=c.acc_bias_impulse;
+				contact.acc_tangent_impulse=c.acc_tangent_impulse;
+				new_index=i;
+				break;
+		}
+	}
+
+	// figure out if the contact amount must be reduced to fit the new contact
+
+	if (new_index == MAX_CONTACTS) {
+
+		// remove the contact with the minimum depth
+
+		int least_deep=-1;
+		float min_depth=1e10;
+
+		for (int i=0;i<=contact_count;i++) {
+
+			Contact& c = (i==contact_count)?contact:contacts[i];
+			Vector3 global_A = A->get_transform().basis.xform(c.local_A);
+			Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B;
+
+			Vector3 axis = global_A - global_B;
+			float depth = axis.dot( c.normal );
+
+			if (depth<min_depth) {
+
+				min_depth=depth;
+				least_deep=i;
+			}
+		}
+
+		ERR_FAIL_COND(least_deep==-1);
+
+		if (least_deep < contact_count) { //replace the last deep contact by the new one
+
+			contacts[least_deep]=contact;
+		}
+
+		return;
+	}
+
+	contacts[new_index]=contact;
+
+	if (new_index==contact_count) {
+
+		contact_count++;
+	}
+
+}
+
+void BodyPairSW::validate_contacts() {
+
+	//make sure to erase contacts that are no longer valid
+
+	real_t contact_max_separation=space->get_contact_max_separation();
+	for (int i=0;i<contact_count;i++) {
+
+		Contact& c = contacts[i];
+
+		Vector3 global_A = A->get_transform().basis.xform(c.local_A);
+		Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B;
+		Vector3 axis = global_A - global_B;
+		float depth = axis.dot( c.normal );
+
+		if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
+			// contact no longer needed, remove
+
+
+			if ((i+1) < contact_count) {
+				// swap with the last one
+				SWAP( contacts[i], contacts[ contact_count-1 ] );
+
+			}
+
+			i--;
+			contact_count--;
+		}
+	}
+}
+
+bool BodyPairSW::setup(float p_step) {
+
+
+	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
+
+	validate_contacts();
+
+	Vector3 offset_A = A->get_transform().get_origin();
+	Transform xform_Au = Transform(A->get_transform().basis,Vector3());
+	Transform xform_A = xform_Au * A->get_shape_transform(shape_A);
+
+	Transform xform_Bu = B->get_transform();
+	xform_Bu.origin-=offset_A;
+	Transform xform_B = xform_Bu * B->get_shape_transform(shape_B);
+
+	ShapeSW *shape_A_ptr=A->get_shape(shape_A);
+	ShapeSW *shape_B_ptr=B->get_shape(shape_B);
+
+	bool collided = CollisionSolverSW::solve_static(shape_A_ptr,xform_A,shape_B_ptr,xform_B,_contact_added_callback,this,&sep_axis);
+	this->collided=collided;
+
+	if (!collided)
+		return false;
+
+
+	//cannot collide
+	if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE)) {
+		return false;
+	}
+
+	real_t max_penetration = space->get_contact_max_allowed_penetration();
+
+	float bias = 0.3f;
+
+	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
+
+		if (shape_A_ptr->get_custom_bias()==0)
+			bias=shape_B_ptr->get_custom_bias();
+		else if (shape_B_ptr->get_custom_bias()==0)
+			bias=shape_A_ptr->get_custom_bias();
+		else
+			bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5;
+	}
+
+
+	real_t inv_dt = 1.0/p_step;
+
+	for(int i=0;i<contact_count;i++) {
+
+		Contact &c = contacts[i];
+		c.active=false;
+
+		Vector3 global_A = xform_Au.xform(c.local_A);
+		Vector3 global_B = xform_Bu.xform(c.local_B);
+
+		real_t depth = c.normal.dot(global_A - global_B);
+
+		if (depth<=0) {
+			c.active=false;
+			continue;
+		}
+
+		c.active=true;
+
+		int gather_A = A->can_report_contacts();
+		int gather_B = B->can_report_contacts();
+
+		c.rA = global_A;
+		c.rB = global_B-offset_B;
+
+		// contact query reporting...
+#if 0
+		if (A->get_body_type() == PhysicsServer::BODY_CHARACTER)
+			static_cast<CharacterBodySW*>(A)->report_character_contact( global_A, global_B, B );
+		if (B->get_body_type() == PhysicsServer::BODY_CHARACTER)
+			static_cast<CharacterBodySW*>(B)->report_character_contact( global_B, global_A, A );
+		if (A->has_contact_query())
+			A->report_contact( global_A, global_B, B );
+		if (B->has_contact_query())
+			B->report_contact( global_B, global_A, A );
+#endif
+
+		if (A->can_report_contacts()) {
+			Vector3 crB = A->get_angular_velocity().cross( c.rA ) + A->get_linear_velocity();
+			A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB);
+		}
+
+		if (B->can_report_contacts()) {
+			Vector3 crA = A->get_angular_velocity().cross( c.rB ) + A->get_linear_velocity();
+			B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA);
+		}
+
+		c.active=true;
+
+		// Precompute normal mass, tangent mass, and bias.
+		Vector3 inertia_A = A->get_inv_inertia_tensor().xform( c.rA.cross( c.normal ) );
+		Vector3 inertia_B = B->get_inv_inertia_tensor().xform( c.rB.cross( c.normal ) );
+		real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
+		kNormal += c.normal.dot( inertia_A.cross(c.rA ) ) + c.normal.dot( inertia_B.cross( c.rB ));
+		c.mass_normal = 1.0f / kNormal;
+
+#if 1
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+
+#else
+		if (depth > max_penetration) {
+			c.bias = (depth - max_penetration) * (1.0/(p_step*(1.0/RELAXATION_TIMESTEPS)));
+		} else {
+			float approach = -0.1f * (depth - max_penetration) / (CMP_EPSILON + max_penetration);
+			approach = CLAMP( approach, CMP_EPSILON, 1.0 );
+			c.bias = approach * (depth - max_penetration) * (1.0/p_step);
+		}
+#endif
+		c.depth=depth;
+
+		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
+		A->apply_impulse( c.rA, -j_vec );
+		B->apply_impulse( c.rB, j_vec );
+		c.acc_bias_impulse=0;
+		Vector3 jb_vec = c.normal * c.acc_bias_impulse;
+		A->apply_bias_impulse( c.rA, -jb_vec );
+		B->apply_bias_impulse( c.rB, jb_vec );
+
+	}
+
+	return true;
+}
+
+void BodyPairSW::solve(float p_step) {
+
+	if (!collided)
+		return;
+
+
+	for(int i=0;i<contact_count;i++) {
+
+		Contact &c = contacts[i];
+		if (!c.active)
+			continue;
+
+		c.active=false; //try to deactivate, will activate itself if still needed
+
+		//bias impule
+
+		Vector3 crbA = A->get_biased_angular_velocity().cross( c.rA );
+		Vector3 crbB = B->get_biased_angular_velocity().cross( c.rB );
+		Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
+
+		real_t vbn = dbv.dot(c.normal);
+
+		if (Math::abs(-vbn+c.bias)>MIN_VELOCITY) {
+
+			real_t jbn = (-vbn + c.bias)*c.mass_normal;
+			real_t jbnOld = c.acc_bias_impulse;
+			c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
+
+			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
+
+
+			A->apply_bias_impulse(c.rA,-jb);
+			B->apply_bias_impulse(c.rB, jb);
+
+			c.active=true;
+		}
+
+
+		Vector3 crA = A->get_angular_velocity().cross( c.rA );
+		Vector3 crB = B->get_angular_velocity().cross( c.rB );
+		Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
+
+		//normal impule
+		real_t vn = dv.dot(c.normal);
+
+		if (Math::abs(vn)>MIN_VELOCITY) {
+
+			real_t bounce=0;
+			real_t jn = (-bounce -vn)*c.mass_normal;
+			real_t jnOld = c.acc_normal_impulse;
+			c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
+
+
+			Vector3 j =c.normal * (c.acc_normal_impulse - jnOld);
+
+
+			A->apply_impulse(c.rA,-j);
+			B->apply_impulse(c.rB, j);
+
+			c.active=true;
+		}
+
+		//friction impule
+
+		real_t friction = A->get_friction() * B->get_friction();
+
+		Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross( c.rA );
+		Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross( c.rB );
+
+		Vector3 dtv = lvB - lvA;
+		real_t tn = c.normal.dot(dtv);
+
+		// tangential velocity
+		Vector3 tv = dtv - c.normal * tn;
+		real_t tvl = tv.length();
+
+		if (tvl > MIN_VELOCITY) {
+
+			tv /= tvl;
+
+			Vector3 temp1 = A->get_inv_inertia_tensor().xform( c.rA.cross( tv ) );
+			Vector3 temp2 = B->get_inv_inertia_tensor().xform( c.rB.cross( tv ) );
+
+			real_t t = -tvl /
+				(A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
+
+			Vector3 jt = t * tv;
+
+
+			Vector3 jtOld = c.acc_tangent_impulse;
+			c.acc_tangent_impulse += jt;
+
+			real_t fi_len = c.acc_tangent_impulse.length();
+			real_t jtMax = c.acc_normal_impulse * friction;
+
+			if (fi_len > CMP_EPSILON && fi_len > jtMax) {
+
+				c.acc_tangent_impulse*=jtMax / fi_len;
+			}
+
+			jt = c.acc_tangent_impulse - jtOld;
+
+
+			A->apply_impulse( c.rA, -jt );
+			B->apply_impulse( c.rB, jt );
+
+			c.active=true;
+
+		}
+
+
+	}
+
+}
+
+
+
+
+
+BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B) : ConstraintSW(_arr,2) {
+
+	A=p_A;
+	B=p_B;
+	shape_A=p_shape_A;
+	shape_B=p_shape_B;
+	space=A->get_space();
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+	contact_count=0;
+	collided=false;
+
+}
+
+
+BodyPairSW::~BodyPairSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+
+}

servers/physics/body_pair_sw.h

@@ -0,0 +1,97 @@
+/*************************************************************************/
+/*  body_pair_sw.h                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BODY_PAIR_SW_H
+#define BODY_PAIR_SW_H
+
+#include "body_sw.h"
+#include "constraint_sw.h"
+
+class BodyPairSW : public ConstraintSW {
+	enum {
+
+		MAX_CONTACTS=4
+	};
+
+	union {
+		struct {
+			BodySW *A;
+			BodySW *B;
+		};
+
+		BodySW *_arr[2];
+	};
+
+	int shape_A;
+	int shape_B;
+
+
+	struct Contact {
+
+		Vector3 position;
+		Vector3 normal;
+		Vector3 local_A, local_B;
+		real_t acc_normal_impulse;	// accumulated normal impulse (Pn)
+		Vector3 acc_tangent_impulse;	// accumulated tangent impulse (Pt)
+		real_t acc_bias_impulse;	// accumulated normal impulse for position bias (Pnb)
+		real_t mass_normal;
+		real_t bias;
+
+		real_t depth;
+		bool active;
+		Vector3 rA,rB;
+	};
+
+	Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
+
+	Vector3 sep_axis;
+	Contact contacts[MAX_CONTACTS];
+	int contact_count;
+	bool collided;
+	int cc;
+
+
+	static void _contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata);
+
+	void contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B);
+
+	void validate_contacts();
+
+	SpaceSW *space;
+
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B);
+	~BodyPairSW();
+
+};
+
+#endif // BODY_PAIR__SW_H

servers/physics/body_sw.cpp

@@ -0,0 +1,631 @@
+/*************************************************************************/
+/*  body_sw.cpp                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "body_sw.h"
+#include "space_sw.h"
+#include "area_sw.h"
+
+void BodySW::_update_inertia() {
+
+	if (get_space() && !inertia_update_list.in_list())
+		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+
+}
+
+
+void BodySW::_update_inertia_tensor() {
+
+	Matrix3 tb = get_transform().basis;
+	tb.scale(_inv_inertia);
+	_inv_inertia_tensor = tb * get_transform().basis.transposed();
+
+}
+
+void BodySW::update_inertias() {
+
+	//update shapes and motions
+
+	switch(mode) {
+
+		case PhysicsServer::BODY_MODE_RIGID: {
+
+			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
+			float total_area=0;
+
+			for (int i=0;i<get_shape_count();i++) {
+
+				total_area+=get_shape_aabb(i).get_area();
+			}
+
+			Vector3 _inertia;
+
+
+			for (int i=0;i<get_shape_count();i++) {
+
+				const ShapeSW* shape=get_shape(i);
+
+				float area=get_shape_aabb(i).get_area();
+
+				float mass = area * this->mass / total_area;
+
+				_inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin();
+
+			}
+
+			if (_inertia!=Vector3())
+				_inv_inertia=_inertia.inverse();
+			else
+				_inv_inertia=Vector3();
+
+			if (mass)
+				_inv_mass=1.0/mass;
+			else
+				_inv_mass=0;
+
+		} break;
+
+		case PhysicsServer::BODY_MODE_STATIC_ACTIVE:
+		case PhysicsServer::BODY_MODE_STATIC: {
+
+			_inv_inertia=Vector3();
+			_inv_mass=0;
+		} break;
+		case PhysicsServer::BODY_MODE_CHARACTER: {
+
+			_inv_inertia=Vector3();
+			_inv_mass=1.0/mass;
+
+		} break;
+	}
+	_update_inertia_tensor();
+
+	//_update_shapes();
+
+}
+
+
+
+void BodySW::set_active(bool p_active) {
+
+	if (active==p_active)
+		return;
+
+	active=p_active;
+	if (!p_active) {
+		if (get_space())
+			get_space()->body_remove_from_active_list(&active_list);
+	} else {
+		if (mode==PhysicsServer::BODY_MODE_STATIC)
+			return; //static bodies can't become active
+		if (get_space())
+			get_space()->body_add_to_active_list(&active_list);
+
+		//still_time=0;
+	}
+/*
+	if (!space)
+		return;
+
+	for(int i=0;i<get_shape_count();i++) {
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			get_space()->get_broadphase()->set_active(s.bpid,active);
+		}
+	}
+*/
+}
+
+
+
+void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) {
+
+	switch(p_param) {
+		case PhysicsServer::BODY_PARAM_BOUNCE: {
+
+			bounce=p_value;
+		} break;
+		case PhysicsServer::BODY_PARAM_FRICTION: {
+
+			friction=p_value;
+		} break;
+		case PhysicsServer::BODY_PARAM_MASS: {
+			ERR_FAIL_COND(p_value<=0);
+			mass=p_value;
+			_update_inertia();
+
+		} break;
+		default:{}
+	}
+}
+
+float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
+
+	switch(p_param) {
+		case PhysicsServer::BODY_PARAM_BOUNCE: {
+
+			return bounce;
+		} break;
+		case PhysicsServer::BODY_PARAM_FRICTION: {
+
+			return friction;
+		} break;
+		case PhysicsServer::BODY_PARAM_MASS: {
+			return mass;
+		} break;
+		default:{}
+	}
+
+	return 0;
+}
+
+void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
+
+	mode=p_mode;
+
+	switch(p_mode) {
+		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
+		case PhysicsServer::BODY_MODE_STATIC:
+		case PhysicsServer::BODY_MODE_STATIC_ACTIVE: {
+
+			_set_inv_transform(get_transform().affine_inverse());
+			_inv_mass=0;
+			_set_static(p_mode==PhysicsServer::BODY_MODE_STATIC);
+			set_active(p_mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE);
+			linear_velocity=Vector3();
+			angular_velocity=Vector3();
+		} break;
+		case PhysicsServer::BODY_MODE_RIGID: {
+
+			_inv_mass=mass>0?(1.0/mass):0;
+			_set_static(false);
+			simulated_motion=false; //jic
+
+		} break;
+		case PhysicsServer::BODY_MODE_CHARACTER: {
+
+			_inv_mass=mass>0?(1.0/mass):0;
+			_set_static(false);
+			simulated_motion=false; //jic
+		} break;
+	}
+
+	_update_inertia();
+	//if (get_space())
+//		_update_queries();
+
+}
+PhysicsServer::BodyMode BodySW::get_mode() const {
+
+	return mode;
+}
+
+void BodySW::_shapes_changed() {
+
+	_update_inertia();
+}
+
+void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) {
+
+	switch(p_state)	{
+		case PhysicsServer::BODY_STATE_TRANSFORM: {
+
+
+			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) {
+				_set_transform(p_variant);
+				_set_inv_transform(get_transform().affine_inverse());
+				wakeup_neighbours();
+			} else {
+				Transform t = p_variant;
+				t.orthonormalize();
+				_set_transform(t);
+				_set_inv_transform(get_transform().inverse());
+
+			}
+
+		} break;
+		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
+
+			//if (mode==PhysicsServer::BODY_MODE_STATIC)
+			//	break;
+			linear_velocity=p_variant;
+		} break;
+		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
+			//if (mode!=PhysicsServer::BODY_MODE_RIGID)
+			//	break;
+			angular_velocity=p_variant;
+
+		} break;
+		case PhysicsServer::BODY_STATE_SLEEPING: {
+			//?
+			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)
+				break;
+			bool do_sleep=p_variant;
+			if (do_sleep) {
+				linear_velocity=Vector3();
+				//biased_linear_velocity=Vector3();
+				angular_velocity=Vector3();
+				//biased_angular_velocity=Vector3();
+				set_active(false);
+			} else {
+				if (mode!=PhysicsServer::BODY_MODE_STATIC)
+					set_active(true);
+			}
+		} break;
+		case PhysicsServer::BODY_STATE_CAN_SLEEP: {
+			can_sleep=p_variant;
+			if (mode==PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep)
+				set_active(true);
+
+		} break;
+	}
+
+}
+Variant BodySW::get_state(PhysicsServer::BodyState p_state) const {
+
+	switch(p_state)	{
+		case PhysicsServer::BODY_STATE_TRANSFORM: {
+			return get_transform();
+		} break;
+		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
+			return linear_velocity;
+		} break;
+		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
+			return angular_velocity;
+		} break;
+		case PhysicsServer::BODY_STATE_SLEEPING: {
+			return !is_active();
+		} break;
+		case PhysicsServer::BODY_STATE_CAN_SLEEP: {
+			return can_sleep;
+		} break;
+	}
+
+	return Variant();
+}
+
+
+void BodySW::set_space(SpaceSW *p_space){
+
+	if (get_space()) {
+
+		if (inertia_update_list.in_list())
+			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
+		if (active_list.in_list())
+			get_space()->body_remove_from_active_list(&active_list);
+		if (direct_state_query_list.in_list())
+			get_space()->body_remove_from_state_query_list(&direct_state_query_list);
+
+	}
+
+	_set_space(p_space);
+
+	if (get_space()) {
+
+		_update_inertia();
+		if (active)
+			get_space()->body_add_to_active_list(&active_list);
+//		_update_queries();
+		//if (is_active()) {
+		//	active=false;
+		//	set_active(true);
+		//}
+
+	}
+
+}
+
+void BodySW::_compute_area_gravity(const AreaSW *p_area) {
+
+	if (p_area->is_gravity_point()) {
+
+		gravity = (p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity();
+
+	} else {
+		gravity = p_area->get_gravity_vector() * p_area->get_gravity();
+	}
+}
+
+void BodySW::integrate_forces(real_t p_step) {
+
+
+	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)
+		return;
+
+	AreaSW *current_area = get_space()->get_default_area();
+	ERR_FAIL_COND(!current_area);
+
+	int prio = current_area->get_priority();
+	int ac = areas.size();
+	if (ac) {
+		const AreaCMP *aa = &areas[0];
+		for(int i=0;i<ac;i++) {
+			if (aa[i].area->get_priority() > prio) {
+				current_area=aa[i].area;
+				prio=current_area->get_priority();
+			}
+		}
+	}
+
+	_compute_area_gravity(current_area);
+	density=current_area->get_density();
+
+	if (!omit_force_integration) {
+		//overriden by direct state query
+
+		Vector3 force=gravity*mass;
+		force+=applied_force;
+		Vector3 torque=applied_torque;
+
+		real_t damp = 1.0 - p_step * density;
+
+		if (damp<0) // reached zero in the given time
+			damp=0;
+
+		real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();
+
+		if (angular_damp<0) // reached zero in the given time
+			angular_damp=0;
+
+		linear_velocity*=damp;
+		angular_velocity*=angular_damp;
+
+		linear_velocity+=_inv_mass * force * p_step;
+		angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step;
+	}
+
+	applied_force=Vector3();
+	applied_torque=Vector3();
+
+	//motion=linear_velocity*p_step;
+
+	biased_angular_velocity=Vector3();
+	biased_linear_velocity=Vector3();
+
+	if (continuous_cd) //shapes temporarily extend for raycast
+		_update_shapes_with_motion(linear_velocity*p_step);
+
+	current_area=NULL; // clear the area, so it is set in the next frame
+	contact_count=0;
+
+}
+
+void BodySW::integrate_velocities(real_t p_step) {
+
+	if (mode==PhysicsServer::BODY_MODE_STATIC)
+		return;
+
+	if (mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) {
+		if (fi_callback)
+			get_space()->body_add_to_state_query_list(&direct_state_query_list);
+		return;
+	}
+
+	Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity;
+
+
+
+	float ang_vel = total_angular_velocity.length();
+	Transform transform = get_transform();
+
+
+	if (ang_vel!=0.0) {
+		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
+		Matrix3 rot( ang_vel_axis, -ang_vel*p_step );
+		transform.basis = rot * transform.basis;
+		transform.orthonormalize();
+	}
+
+	Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity;
+
+
+	transform.origin+=total_linear_velocity * p_step;
+
+	_set_transform(transform);
+	_set_inv_transform(get_transform().inverse());
+
+	_update_inertia_tensor();
+
+	if (fi_callback) {
+
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	}
+
+}
+
+
+void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {
+
+	Transform inv_xform = p_xform.affine_inverse();
+	if (!get_space()) {
+		_set_transform(p_xform);
+		_set_inv_transform(inv_xform);
+
+		return;
+	}
+
+	//compute a FAKE linear velocity - this is easy
+
+	linear_velocity=(p_xform.origin - get_transform().origin)/p_step;
+
+	//compute a FAKE angular velocity, not so easy
+	Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized();
+	Vector3 axis;
+	float angle;
+
+	rot.get_axis_and_angle(axis,angle);
+	axis.normalize();
+	angular_velocity=axis.normalized() * (angle/p_step);
+	linear_velocity = (p_xform.origin - get_transform().origin)/p_step;
+
+	if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	simulated_motion=true;
+	_set_transform(p_xform);
+
+
+}
+
+void BodySW::wakeup_neighbours() {
+
+	for(Map<ConstraintSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
+
+		const ConstraintSW *c=E->key();
+		BodySW **n = c->get_body_ptr();
+		int bc=c->get_body_count();
+
+		for(int i=0;i<bc;i++) {
+
+			if (i==E->get())
+				continue;
+			BodySW *b = n[i];
+			if (b->mode!=PhysicsServer::BODY_MODE_RIGID)
+				continue;
+
+			if (!b->is_active())
+				b->set_active(true);
+		}
+	}
+}
+
+void BodySW::call_queries() {
+
+
+	if (fi_callback) {
+
+		PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;
+		dbs->body=this;
+
+		Variant v=dbs;
+
+		Object *obj = ObjectDB::get_instance(fi_callback->id);
+		if (!obj) {
+
+			set_force_integration_callback(0,StringName());
+		} else {
+			const Variant *vp[2]={&v,&fi_callback->udata};
+
+			Variant::CallError ce;
+			int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2;
+			obj->call(fi_callback->method,vp,argc,ce);
+		}
+
+
+	}
+
+	if (simulated_motion) {
+
+	//	linear_velocity=Vector3();
+	//	angular_velocity=0;
+		simulated_motion=false;
+	}
+}
+
+
+bool BodySW::sleep_test(real_t p_step)  {
+
+	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)
+		return true; //
+	else if (mode==PhysicsServer::BODY_MODE_CHARACTER)
+		return !active; // characters don't sleep unless asked to sleep
+	else if (!can_sleep)
+		return false;
+
+
+
+
+	if (Math::abs(angular_velocity.length())<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {
+
+		still_time+=p_step;
+
+		return still_time > get_space()->get_body_time_to_sleep();
+	} else {
+
+		still_time=0; //maybe this should be set to 0 on set_active?
+		return false;
+	}
+}
+
+
+void BodySW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {
+
+	if (fi_callback) {
+
+		memdelete(fi_callback);
+		fi_callback=NULL;
+	}
+
+
+	if (p_id!=0) {
+
+		fi_callback=memnew(ForceIntegrationCallback);
+		fi_callback->id=p_id;
+		fi_callback->method=p_method;
+		fi_callback->udata=p_udata;
+	}
+
+}
+
+BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
+
+
+	mode=PhysicsServer::BODY_MODE_RIGID;
+	active=true;
+
+	mass=1;
+//	_inv_inertia=Transform();
+	_inv_mass=1;
+	bounce=0;
+	friction=1;
+	omit_force_integration=false;
+//	applied_torque=0;
+	island_step=0;
+	island_next=NULL;
+	island_list_next=NULL;
+	_set_static(false);
+	density=0;
+	contact_count=0;
+	simulated_motion=false;
+	still_time=0;
+	continuous_cd=false;
+	can_sleep=false;
+	fi_callback=NULL;
+
+}
+
+BodySW::~BodySW() {
+
+	if (fi_callback)
+		memdelete(fi_callback);
+}
+
+PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL;
+
+PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() {
+
+	return body->get_space()->get_direct_state();
+}

servers/physics/body_sw.h

@@ -0,0 +1,348 @@
+/*************************************************************************/
+/*  body_sw.h                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BODY_SW_H
+#define BODY_SW_H
+
+#include "collision_object_sw.h"
+#include "vset.h"
+#include "area_sw.h"
+
+class ConstraintSW;
+
+
+class BodySW : public CollisionObjectSW {
+
+
+	PhysicsServer::BodyMode mode;
+
+	Vector3 linear_velocity;
+	Vector3 angular_velocity;
+
+	Vector3 biased_linear_velocity;
+	Vector3 biased_angular_velocity;
+	real_t mass;
+	real_t bounce;
+	real_t friction;
+
+	real_t _inv_mass;
+	Vector3 _inv_inertia;
+	Matrix3 _inv_inertia_tensor;
+
+	Vector3 gravity;
+	real_t density;
+
+	real_t still_time;
+
+	Vector3 applied_force;
+	Vector3 applied_torque;
+
+	SelfList<BodySW> active_list;
+	SelfList<BodySW> inertia_update_list;
+	SelfList<BodySW> direct_state_query_list;
+
+	VSet<RID> exceptions;
+	bool omit_force_integration;
+	bool active;
+	bool simulated_motion;
+	bool continuous_cd;
+	bool can_sleep;
+	void _update_inertia();
+	virtual void _shapes_changed();
+
+	Map<ConstraintSW*,int> constraint_map;
+
+	struct AreaCMP {
+
+		AreaSW *area;
+		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }
+		_FORCE_INLINE_ AreaCMP() {}
+		_FORCE_INLINE_ AreaCMP(AreaSW *p_area) { area=p_area;}
+	};
+
+
+	VSet<AreaCMP> areas;
+
+	struct Contact {
+
+
+		Vector3 local_pos;
+		Vector3 local_normal;
+		float depth;
+		int local_shape;
+		Vector3 collider_pos;
+		int collider_shape;
+		ObjectID collider_instance_id;
+		RID collider;
+		Vector3 collider_velocity_at_pos;
+	};
+
+	Vector<Contact> contacts; //no contacts by default
+	int contact_count;
+
+	struct ForceIntegrationCallback {
+
+		ObjectID id;
+		StringName method;
+		Variant udata;
+	};
+
+	ForceIntegrationCallback *fi_callback;
+
+
+	uint64_t island_step;
+	BodySW *island_next;
+	BodySW *island_list_next;
+
+	_FORCE_INLINE_ void _compute_area_gravity(const AreaSW *p_area);
+
+	_FORCE_INLINE_ void _update_inertia_tensor();
+
+friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose
+
+public:
+
+
+	void set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata=Variant());
+
+
+	_FORCE_INLINE_ void add_area(AreaSW *p_area) { areas.insert(AreaCMP(p_area)); }
+	_FORCE_INLINE_ void remove_area(AreaSW *p_area) { areas.erase(AreaCMP(p_area)); }
+
+	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }
+	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
+
+	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
+	_FORCE_INLINE_ void add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, float p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos);
+
+
+	_FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
+	_FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
+	_FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
+	_FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}
+
+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+
+	_FORCE_INLINE_ BodySW* get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(BodySW* p_next) { island_next=p_next; }
+
+	_FORCE_INLINE_ BodySW* get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(BodySW* p_next) { island_list_next=p_next; }
+
+	_FORCE_INLINE_ void add_constraint(ConstraintSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
+	_FORCE_INLINE_ void remove_constraint(ConstraintSW* p_constraint) { constraint_map.erase(p_constraint); }
+	const Map<ConstraintSW*,int>& get_constraint_map() const { return constraint_map; }
+
+	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
+	_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
+
+	_FORCE_INLINE_ void set_linear_velocity(const Vector3& p_velocity) {linear_velocity=p_velocity; }
+	_FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; }
+
+	_FORCE_INLINE_ void set_angular_velocity(const Vector3& p_velocity) { angular_velocity=p_velocity; }
+	_FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; }
+
+	_FORCE_INLINE_ const Vector3& get_biased_linear_velocity() const { return biased_linear_velocity; }
+	_FORCE_INLINE_ const Vector3& get_biased_angular_velocity() const { return biased_angular_velocity; }
+
+	_FORCE_INLINE_ void apply_impulse(const Vector3& p_pos, const Vector3& p_j) {
+
+		linear_velocity += p_j * _inv_mass;
+		angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) );
+	}
+
+	_FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& p_j) {
+
+		biased_linear_velocity += p_j * _inv_mass;
+		biased_angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) );
+	}
+
+	_FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) {
+
+		angular_velocity += _inv_inertia_tensor.xform(p_j);
+	}
+
+	_FORCE_INLINE_ void add_force(const Vector3& p_force, const Vector3& p_pos) {
+
+		applied_force += p_force;
+		applied_torque += p_pos.cross(p_force);
+	}
+
+	void set_active(bool p_active);
+	_FORCE_INLINE_ bool is_active() const { return active; }
+
+	void set_param(PhysicsServer::BodyParameter p_param, float);
+	float get_param(PhysicsServer::BodyParameter p_param) const;
+
+	void set_mode(PhysicsServer::BodyMode p_mode);
+	PhysicsServer::BodyMode get_mode() const;
+
+	void set_state(PhysicsServer::BodyState p_state, const Variant& p_variant);
+	Variant get_state(PhysicsServer::BodyState p_state) const;
+
+	void set_applied_force(const Vector3& p_force) { applied_force=p_force; }
+	Vector3 get_applied_force() const { return applied_force; }
+
+	void set_applied_torque(const Vector3& p_torque) { applied_torque=p_torque; }
+	Vector3 get_applied_torque() const { return applied_torque; }
+
+	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; }
+	_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
+
+	void set_space(SpaceSW *p_space);
+
+	void update_inertias();
+
+	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
+	_FORCE_INLINE_ Vector3 get_inv_inertia() const { return _inv_inertia; }
+	_FORCE_INLINE_ Matrix3 get_inv_inertia_tensor() const { return _inv_inertia_tensor; }
+	_FORCE_INLINE_ real_t get_friction() const { return friction; }
+	_FORCE_INLINE_ Vector3 get_gravity() const { return gravity; }
+	_FORCE_INLINE_ real_t get_density() const { return density; }
+	_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
+
+	void integrate_forces(real_t p_step);
+	void integrate_velocities(real_t p_step);
+
+	void simulate_motion(const Transform& p_xform,real_t p_step);
+	void call_queries();
+	void wakeup_neighbours();
+
+	bool sleep_test(real_t p_step);
+
+	BodySW();
+	~BodySW();
+
+};
+
+
+//add contact inline
+
+void BodySW::add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, float p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos) {
+
+	int c_max=contacts.size();
+
+	if (c_max==0)
+		return;
+
+	Contact *c = &contacts[0];
+
+
+	int idx=-1;
+
+	if (contact_count<c_max) {
+		idx=contact_count++;
+	} else {
+
+		float least_depth=1e20;
+		int least_deep=-1;
+		for(int i=0;i<c_max;i++) {
+
+			if (i==0 || c[i].depth<least_depth) {
+				least_deep=i;
+				least_depth=c[i].depth;
+			}
+		}
+
+		if (least_deep>=0 && least_depth<p_depth) {
+
+			idx=least_deep;
+		}
+		if (idx==-1)
+			return; //none least deepe than this
+	}
+
+	c[idx].local_pos=p_local_pos;
+	c[idx].local_normal=p_local_normal;
+	c[idx].depth=p_depth;
+	c[idx].local_shape=p_local_shape;
+	c[idx].collider_pos=p_collider_pos;
+	c[idx].collider_shape=p_collider_shape;
+	c[idx].collider_instance_id=p_collider_instance_id;
+	c[idx].collider=p_collider;
+	c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;
+
+}
+
+
+class PhysicsDirectBodyStateSW : public PhysicsDirectBodyState {
+
+	OBJ_TYPE( PhysicsDirectBodyStateSW, PhysicsDirectBodyState );
+
+public:
+
+	static PhysicsDirectBodyStateSW *singleton;
+	BodySW *body;
+	real_t step;
+
+	virtual Vector3 get_total_gravity() const {  return body->get_gravity();  } // get gravity vector working on this body space/area
+	virtual float get_total_density() const {  return body->get_density();  } // get density of this body space/area
+
+	virtual float get_inverse_mass() const {  return body->get_inv_mass();  } // get the mass
+	virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia();   } // get density of this body space
+	virtual Matrix3 get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor();   } // get density of this body space
+
+	virtual void set_linear_velocity(const Vector3& p_velocity) {  body->set_linear_velocity(p_velocity);  }
+	virtual Vector3 get_linear_velocity() const {  return body->get_linear_velocity();  }
+
+	virtual void set_angular_velocity(const Vector3& p_velocity) {  body->set_angular_velocity(p_velocity);  }
+	virtual Vector3 get_angular_velocity() const {  return body->get_angular_velocity();  }
+
+	virtual void set_transform(const Transform& p_transform) {  body->set_state(PhysicsServer::BODY_STATE_TRANSFORM,p_transform);  }
+	virtual Transform get_transform() const {  return body->get_transform();  }
+
+	virtual void add_force(const Vector3& p_force, const Vector3& p_pos) {  body->add_force(p_force,p_pos); }
+
+	virtual void set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }
+	virtual bool is_sleeping() const {  return !body->is_active();  }
+
+	virtual int get_contact_count() const {  return body->contact_count;  }
+
+	virtual Vector3 get_contact_local_pos(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());
+		return body->contacts[p_contact_idx].local_pos;
+	}
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());   return body->contacts[p_contact_idx].local_normal;   }
+	virtual int get_contact_local_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1);   return body->contacts[p_contact_idx].local_shape;   }
+
+	virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID());   return body->contacts[p_contact_idx].collider;   }
+	virtual Vector3 get_contact_collider_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());   return body->contacts[p_contact_idx].collider_pos;  }
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0);   return body->contacts[p_contact_idx].collider_instance_id;   }
+	virtual int get_contact_collider_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape;  }
+	virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].collider_velocity_at_pos;  }
+
+	virtual PhysicsDirectSpaceState* get_space_state();
+
+
+	virtual real_t get_step() const { return step; }
+	PhysicsDirectBodyStateSW() { singleton=this; body=NULL; }
+};
+
+
+#endif // BODY__SW_H

servers/physics/broad_phase_basic.cpp

@@ -0,0 +1,216 @@
+/*************************************************************************/
+/*  broad_phase_basic.cpp                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "broad_phase_basic.h"
+#include "list.h"
+#include "print_string.h"
+BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object_, int p_subindex) {
+
+	if (p_object_==NULL) {
+
+		ERR_FAIL_COND_V(p_object_==NULL,0);
+	}
+
+	current++;
+
+	Element e;
+	e.owner=p_object_;
+	e._static=false;
+	e.subindex=p_subindex;
+
+	element_map[current]=e;
+	return current;
+}
+
+void BroadPhaseBasic::move(ID p_id, const AABB& p_aabb) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	E->get().aabb=p_aabb;
+
+}
+void BroadPhaseBasic::set_static(ID p_id, bool p_static) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	E->get()._static=p_static;
+
+}
+void BroadPhaseBasic::remove(ID p_id) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	List<PairKey> to_erase;
+	//unpair must be done immediately on removal to avoid potential invalid pointers
+	for (Map<PairKey,void*>::Element *F=pair_map.front();F;F=F->next()) {
+
+		if (F->key().a==p_id || F->key().b==p_id) {
+
+			if (unpair_callback) {
+				Element *elem_A=&element_map[F->key().a];
+				Element *elem_B=&element_map[F->key().b];
+				unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,F->get(),unpair_userdata);
+			}
+			to_erase.push_back(F->key());
+		}
+	}
+	while(to_erase.size()) {
+
+		pair_map.erase(to_erase.front()->get());
+		to_erase.pop_front();
+	}
+	element_map.erase(E);
+
+}
+
+CollisionObjectSW *BroadPhaseBasic::get_object(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,NULL);
+	return E->get().owner;
+
+}
+bool BroadPhaseBasic::is_static(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,false);
+	return E->get()._static;
+
+}
+int BroadPhaseBasic::get_subindex(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,-1);
+	return E->get().subindex;
+}
+
+int BroadPhaseBasic::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+
+	int rc=0;
+
+	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+
+		const AABB aabb=E->get().aabb;
+		if (aabb.intersects_segment(p_from,p_to)) {
+
+			p_results[rc]=E->get().owner;
+			p_result_indices[rc]=E->get().subindex;
+			rc++;
+			if (rc>=p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+
+}
+int BroadPhaseBasic::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+
+	int rc=0;
+
+	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+
+		const AABB aabb=E->get().aabb;
+		if (aabb.intersects(p_aabb)) {
+
+			p_results[rc]=E->get().owner;
+			p_result_indices[rc]=E->get().subindex;
+			rc++;
+			if (rc>=p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+}
+
+void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
+
+	pair_userdata=p_userdata;
+	pair_callback=p_pair_callback;
+
+}
+void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) {
+
+	unpair_userdata=p_userdata;
+	unpair_callback=p_pair_callback;
+
+}
+
+void BroadPhaseBasic::update() {
+
+	// recompute pairs
+	for(Map<ID,Element>::Element *I=element_map.front();I;I=I->next()) {
+
+		for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) {
+
+			Element *elem_A=&I->get();
+			Element *elem_B=&J->get();
+
+			if (elem_A->owner == elem_B->owner)
+				continue;
+
+
+			bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static );
+
+			PairKey key(I->key(),J->key());
+
+			Map<PairKey,void*>::Element *E=pair_map.find(key);
+
+			if (!pair_ok && E) {
+				if (unpair_callback)
+					unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,E->get(),unpair_userdata);
+				pair_map.erase(key);
+			}
+
+			if (pair_ok && !E) {
+
+				void *data=NULL;
+				if (pair_callback)
+					data=pair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,unpair_userdata);
+				pair_map.insert(key,data);
+			}
+		}
+	}
+
+}
+
+BroadPhaseSW *BroadPhaseBasic::_create() {
+
+	return memnew( BroadPhaseBasic );
+}
+
+BroadPhaseBasic::BroadPhaseBasic() {
+
+	current=1;
+	unpair_callback=NULL;
+	unpair_userdata=NULL;
+	pair_callback=NULL;
+	pair_userdata=NULL;
+
+}

servers/physics/broad_phase_basic.h

@@ -0,0 +1,101 @@
+/*************************************************************************/
+/*  broad_phase_basic.h                                                  */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BROAD_PHASE_BASIC_H
+#define BROAD_PHASE_BASIC_H
+
+#include "broad_phase_sw.h"
+#include "map.h"
+
+class BroadPhaseBasic : public BroadPhaseSW {
+
+	struct Element {
+
+		CollisionObjectSW *owner;
+		bool _static;
+		AABB aabb;
+		int subindex;
+	};
+
+
+	Map<ID,Element> element_map;
+
+	ID current;
+
+	struct PairKey {
+
+		union {
+			struct {
+				ID a;
+				ID b;
+			};
+			uint64_t key;
+		};
+
+		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+			return key < p_key.key;
+		}
+
+		PairKey() { key=0; }
+		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
+
+	};
+
+	Map<PairKey,void*> pair_map;
+
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+public:
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0);
+	virtual void move(ID p_id, const AABB& p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObjectSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+
+	virtual void update();
+
+	static BroadPhaseSW *_create();
+	BroadPhaseBasic();
+};
+
+#endif // BROAD_PHASE_BASIC_H

servers/physics/broad_phase_octree.cpp

@@ -0,0 +1,133 @@
+/*************************************************************************/
+/*  broad_phase_octree.cpp                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "broad_phase_octree.h"
+#include "collision_object_sw.h"
+
+ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) {
+
+	ID oid = octree.create(p_object,AABB(),p_subindex,false,1<<p_object->get_type(),0);
+	return oid;
+}
+
+void BroadPhaseOctree::move(ID p_id, const AABB& p_aabb){
+
+	octree.move(p_id,p_aabb);
+}
+
+void BroadPhaseOctree::set_static(ID p_id, bool p_static){
+
+	CollisionObjectSW *it = octree.get(p_id);
+	octree.set_pairable(p_id,p_static?false:true,1<<it->get_type(),p_static?0:0xFFFFF); //pair everything, don't care 1?
+
+}
+void BroadPhaseOctree::remove(ID p_id){
+
+	octree.erase(p_id);
+}
+
+CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const{
+
+	CollisionObjectSW *it = octree.get(p_id);
+	ERR_FAIL_COND_V(!it,NULL);
+	return it;
+}
+bool BroadPhaseOctree::is_static(ID p_id) const{
+
+	return !octree.is_pairable(p_id);
+}
+int BroadPhaseOctree::get_subindex(ID p_id) const{
+
+	return octree.get_subindex(p_id);
+}
+
+int BroadPhaseOctree::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices){
+
+	return octree.cull_segment(p_from,p_to,p_results,p_max_results,p_result_indices);
+}
+
+int BroadPhaseOctree::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+
+	return octree.cull_AABB(p_aabb,p_results,p_max_results,p_result_indices);
+
+}
+
+
+void* BroadPhaseOctree::_pair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B) {
+
+	BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self);
+	if (!bpo->pair_callback)
+		return NULL;
+
+	return bpo->pair_callback(p_object_A,subindex_A,p_object_B,subindex_B,bpo->pair_userdata);
+
+}
+
+void BroadPhaseOctree::_unpair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B,void*pairdata) {
+
+	BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self);
+	if (!bpo->unpair_callback)
+		return;
+
+	bpo->unpair_callback(p_object_A,subindex_A,p_object_B,subindex_B,pairdata,bpo->unpair_userdata);
+
+}
+
+
+void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback,void *p_userdata){
+
+	pair_callback=p_pair_callback;
+	pair_userdata=p_userdata;
+
+}
+void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata){
+
+	unpair_callback=p_unpair_callback;
+	unpair_userdata=p_userdata;
+
+}
+
+void BroadPhaseOctree::update() {
+	// does.. not?
+}
+
+BroadPhaseSW *BroadPhaseOctree::_create() {
+
+	return memnew( BroadPhaseOctree );
+}
+
+BroadPhaseOctree::BroadPhaseOctree() {
+	octree.set_pair_callback(_pair_callback,this);
+	octree.set_unpair_callback(_unpair_callback,this);
+	pair_callback=NULL;
+	pair_userdata=NULL;
+	pair_callback=NULL;
+	unpair_userdata=NULL;
+}
+
+

servers/physics/broad_phase_octree.h

@@ -0,0 +1,73 @@
+/*************************************************************************/
+/*  broad_phase_octree.h                                                 */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BROAD_PHASE_OCTREE_H
+#define BROAD_PHASE_OCTREE_H
+
+#include "broad_phase_sw.h"
+#include "octree.h"
+
+class BroadPhaseOctree : public BroadPhaseSW {
+
+
+	Octree<CollisionObjectSW,true> octree;
+
+	static void* _pair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int);
+	static void _unpair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int,void*);
+
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+public:
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0);
+	virtual void move(ID p_id, const AABB& p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObjectSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+
+	virtual void update();
+
+	static BroadPhaseSW *_create();
+	BroadPhaseOctree();
+};
+
+#endif // BROAD_PHASE_OCTREE_H

servers/physics/broad_phase_sw.cpp

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

servers/physics/broad_phase_sw.h

@@ -0,0 +1,73 @@
+/*************************************************************************/
+/*  broad_phase_sw.h                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BROAD_PHASE_SW_H
+#define BROAD_PHASE_SW_H
+
+#include "math_funcs.h"
+#include "aabb.h"
+
+class CollisionObjectSW;
+
+
+class BroadPhaseSW {
+
+public:
+	typedef BroadPhaseSW* (*CreateFunction)();
+
+	static CreateFunction create_func;
+
+	typedef uint32_t ID;
+
+
+	typedef void* (*PairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_userdata);
+	typedef void (*UnpairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_userdata);
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0)=0;
+	virtual void move(ID p_id, const AABB& p_aabb)=0;
+	virtual void set_static(ID p_id, bool p_static)=0;
+	virtual void remove(ID p_id)=0;
+
+	virtual CollisionObjectSW *get_object(ID p_id) const=0;
+	virtual bool is_static(ID p_id) const=0;
+	virtual int get_subindex(ID p_id) const=0;
+
+	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0;
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0;
+
+	virtual void update()=0;
+
+	virtual ~BroadPhaseSW();
+
+};
+
+#endif // BROAD_PHASE__SW_H

servers/physics/collision_object_sw.cpp

@@ -0,0 +1,219 @@
+/*************************************************************************/
+/*  collision_object_sw.cpp                                              */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "collision_object_sw.h"
+#include "space_sw.h"
+
+void CollisionObjectSW::add_shape(ShapeSW *p_shape,const Transform& p_transform) {
+
+	Shape s;
+	s.shape=p_shape;
+	s.xform=p_transform;
+	s.xform_inv=s.xform.affine_inverse();
+	s.bpid=0; //needs update
+	shapes.push_back(s);
+	p_shape->add_owner(this);
+	_update_shapes();
+	_shapes_changed();
+
+}
+
+void CollisionObjectSW::set_shape(int p_index,ShapeSW *p_shape){
+
+	ERR_FAIL_INDEX(p_index,shapes.size());
+	shapes[p_index].shape->remove_owner(this);
+	shapes[p_index].shape=p_shape;
+
+	p_shape->add_owner(this);
+	_update_shapes();
+	_shapes_changed();
+
+}
+void CollisionObjectSW::set_shape_transform(int p_index,const Transform& p_transform){
+
+	ERR_FAIL_INDEX(p_index,shapes.size());
+
+	shapes[p_index].xform=p_transform;
+	shapes[p_index].xform_inv=p_transform.affine_inverse();
+	_update_shapes();
+	_shapes_changed();
+}
+
+void CollisionObjectSW::remove_shape(ShapeSW *p_shape) {
+
+	//remove a shape, all the times it appears
+	for(int i=0;i<shapes.size();i++) {
+
+		if (shapes[i].shape==p_shape) {
+			remove_shape(i);
+			i--;
+		}
+	}
+}
+
+void CollisionObjectSW::remove_shape(int p_index){
+
+	//remove anything from shape to be erased to end, so subindices don't change
+	ERR_FAIL_INDEX(p_index,shapes.size());
+	for(int i=p_index;i<shapes.size();i++) {
+
+		if (shapes[i].bpid==0)
+			continue;
+		//should never get here with a null owner
+		space->get_broadphase()->remove(shapes[i].bpid);
+		shapes[i].bpid=0;
+	}
+	shapes[p_index].shape->remove_owner(this);
+	shapes.remove(p_index);
+
+	_shapes_changed();
+
+}
+
+void CollisionObjectSW::_set_static(bool p_static) {
+	if (_static==p_static)
+		return;
+	_static=p_static;
+
+	if (!space)
+		return;
+	for(int i=0;i<get_shape_count();i++) {
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+	}
+
+}
+
+void CollisionObjectSW::_unregister_shapes() {
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			space->get_broadphase()->remove(s.bpid);
+			s.bpid=0;
+		}
+	}
+
+}
+
+void CollisionObjectSW::_update_shapes() {
+
+	if (!space)
+		return;
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid==0) {
+			s.bpid=space->get_broadphase()->create(this,i);
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+
+		//not quite correct, should compute the next matrix..
+		AABB shape_aabb=s.shape->get_aabb();
+		Transform xform = transform * s.xform;
+		shape_aabb=xform.xform(shape_aabb);
+		s.aabb_cache=shape_aabb;
+		s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 );
+
+
+		space->get_broadphase()->move(s.bpid,s.aabb_cache);
+	}
+
+}
+
+void CollisionObjectSW::_update_shapes_with_motion(const Vector3& p_motion) {
+
+
+	if (!space)
+		return;
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid==0) {
+			s.bpid=space->get_broadphase()->create(this,i);
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+
+		//not quite correct, should compute the next matrix..
+		AABB shape_aabb=s.shape->get_aabb();
+		Transform xform = transform * s.xform;
+		shape_aabb=xform.xform(shape_aabb);
+		shape_aabb=shape_aabb.merge(AABB( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion
+		s.aabb_cache=shape_aabb;
+
+		space->get_broadphase()->move(s.bpid,shape_aabb);
+	}
+
+
+}
+
+void CollisionObjectSW::_set_space(SpaceSW *p_space) {
+
+	if (space) {
+
+		space->remove_object(this);
+
+		for(int i=0;i<shapes.size();i++) {
+
+			Shape &s=shapes[i];
+			if (s.bpid) {
+				space->get_broadphase()->remove(s.bpid);
+				s.bpid=0;
+			}
+		}
+
+	}
+
+	space=p_space;
+
+	if (space) {
+
+		space->add_object(this);
+		_update_shapes();
+	}
+
+}
+
+void CollisionObjectSW::_shape_changed() {
+
+	_update_shapes();
+	_shapes_changed();
+}
+
+CollisionObjectSW::CollisionObjectSW(Type p_type) {
+
+	_static=true;
+	type=p_type;
+	space=NULL;
+	instance_id=0;
+}

servers/physics/collision_object_sw.h

@@ -0,0 +1,119 @@
+/*************************************************************************/
+/*  collision_object_sw.h                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef COLLISION_OBJECT_SW_H
+#define COLLISION_OBJECT_SW_H
+
+#include "shape_sw.h"
+#include "servers/physics_server.h"
+#include "self_list.h"
+#include "broad_phase_sw.h"
+
+class SpaceSW;
+
+class CollisionObjectSW : public ShapeOwnerSW {
+public:
+	enum Type {
+		TYPE_AREA,
+		TYPE_BODY
+	};
+private:
+
+	Type type;
+	RID self;
+	ObjectID instance_id;
+
+	struct Shape {
+
+		Transform xform;
+		Transform xform_inv;
+		BroadPhaseSW::ID bpid;
+		AABB aabb_cache; //for rayqueries
+		ShapeSW *shape;
+	};
+
+	Vector<Shape> shapes;
+	SpaceSW *space;
+	Transform transform;
+	Transform inv_transform;
+	bool _static;
+
+	void _update_shapes();
+
+protected:
+
+
+	void _update_shapes_with_motion(const Vector3& p_motion);
+	void _unregister_shapes();
+
+	_FORCE_INLINE_ void _set_transform(const Transform& p_transform) { transform=p_transform; _update_shapes(); }
+	_FORCE_INLINE_ void _set_inv_transform(const Transform& p_transform) { inv_transform=p_transform; }
+	void _set_static(bool p_static);
+
+	virtual void _shapes_changed()=0;
+	void _set_space(SpaceSW *space);
+
+	CollisionObjectSW(Type p_type);
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ void set_instance_id(const ObjectID& p_instance_id) { instance_id=p_instance_id; }
+	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
+
+	void _shape_changed();
+
+	_FORCE_INLINE_ Type get_type() const { return type; }
+	void add_shape(ShapeSW *p_shape,const Transform& p_transform=Transform());
+	void set_shape(int p_index,ShapeSW *p_shape);
+	void set_shape_transform(int p_index,const Transform& p_transform);
+	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
+	_FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { return shapes[p_index].shape; }
+	_FORCE_INLINE_ const Transform& get_shape_transform(int p_index) const { return shapes[p_index].xform; }
+	_FORCE_INLINE_ const Transform& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
+	_FORCE_INLINE_ const AABB& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
+
+	_FORCE_INLINE_ Transform get_transform() const { return transform; }
+	_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
+	_FORCE_INLINE_ SpaceSW* get_space() const { return space; }
+
+
+
+	void remove_shape(ShapeSW *p_shape);
+	void remove_shape(int p_index);
+
+	virtual void set_space(SpaceSW *p_space)=0;
+
+	_FORCE_INLINE_ bool is_static() const { return _static;  }
+
+	virtual ~CollisionObjectSW() {}
+
+};
+
+#endif // COLLISION_OBJECT_SW_H

servers/physics/collision_solver_sat.h

@@ -0,0 +1,37 @@
+/*************************************************************************/
+/*  collision_solver_sat.h                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef COLLISION_SOLVER_SAT_H
+#define COLLISION_SOLVER_SAT_H
+
+#include "collision_solver_sw.h"
+
+
+bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL);
+
+#endif // COLLISION_SOLVER_SAT_H

servers/physics/collision_solver_sw.cpp

@@ -0,0 +1,241 @@
+/*************************************************************************/
+/*  collision_solver_sw.cpp                                              */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "collision_solver_sw.h"
+#include "collision_solver_sat.h"
+
+#include "gjk_epa.h"
+#include "collision_solver_sat.h"
+
+
+#define collision_solver sat_calculate_penetration
+//#define collision_solver gjk_epa_calculate_penetration
+
+
+bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+	const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A);
+	if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE)
+		return false;
+	Plane p = p_transform_A.xform(plane->get_plane());
+
+	static const int max_supports = 16;
+	Vector3 supports[max_supports];
+	int support_count;
+
+	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count);
+
+	bool found=false;
+
+	for(int i=0;i<support_count;i++) {
+
+		supports[i] = p_transform_B.xform( supports[i] );
+		if (p.distance_to(supports[i])>=0)
+			continue;
+		found=true;
+
+		Vector3 support_A = p.project(supports[i]);
+
+		if (p_result_callback) {
+			if (p_swap_result)
+				p_result_callback(supports[i],support_A,p_userdata);
+			else
+				p_result_callback(support_A,supports[i],p_userdata);
+		}
+
+	}
+
+
+	return found;
+}
+
+bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+
+	const RayShapeSW *ray = static_cast<const RayShapeSW*>(p_shape_A);
+
+	Vector3 from = p_transform_A.origin;
+	Vector3 to = from+p_transform_A.basis.get_axis(2)*ray->get_length();
+	Vector3 support_A=to;
+
+	Transform ai = p_transform_B.affine_inverse();
+
+	from = ai.xform(from);
+	to = ai.xform(to);
+
+	Vector3 p,n;
+	if (!p_shape_B->intersect_segment(from,to,p,n))
+		return false;
+
+	Vector3 support_B=p_transform_B.xform(p);
+
+	if (p_result_callback) {
+		if (p_swap_result)
+			p_result_callback(support_B,support_A,p_userdata);
+		else
+			p_result_callback(support_A,support_B,p_userdata);
+	}
+	return true;
+}
+
+struct _ConcaveCollisionInfo {
+
+	const Transform *transform_A;
+	const ShapeSW *shape_A;
+	const Transform *transform_B;
+	CollisionSolverSW::CallbackResult result_callback;
+	void *userdata;
+	bool swap_result;
+	bool collided;
+	int aabb_tests;
+	int collisions;
+
+};
+
+void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
+
+
+	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata);
+	cinfo.aabb_tests++;
+
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result );
+	if (!collided)
+		return;
+
+	cinfo.collided=true;
+	cinfo.collisions++;
+
+}
+
+bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+
+	const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B);
+
+	_ConcaveCollisionInfo cinfo;
+	cinfo.transform_A=&p_transform_A;
+	cinfo.shape_A=p_shape_A;
+	cinfo.transform_B=&p_transform_B;
+	cinfo.result_callback=p_result_callback;
+	cinfo.userdata=p_userdata;
+	cinfo.swap_result=p_swap_result;
+	cinfo.collided=false;
+	cinfo.collisions=0;
+
+	cinfo.aabb_tests=0;
+
+	Transform rel_transform = p_transform_A;
+	rel_transform.origin-=p_transform_B.origin;
+
+	//quickly compute a local AABB
+
+	AABB local_aabb;
+	for(int i=0;i<3;i++) {
+
+	     Vector3 axis( p_transform_B.basis.get_axis(i) );
+	     float axis_scale = 1.0/axis.length();
+	     axis*=axis_scale;
+
+	     float smin,smax;
+	     p_shape_A->project_range(axis,rel_transform,smin,smax);
+	     smin*=axis_scale;
+	     smax*=axis_scale;
+
+	     local_aabb.pos[i]=smin;
+	     local_aabb.size[i]=smax-smin;
+	}
+
+	concave_B->cull(local_aabb,concave_callback,&cinfo);
+
+
+	return cinfo.collided;
+}
+
+
+bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis) {
+
+
+	PhysicsServer::ShapeType type_A=p_shape_A->get_type();
+	PhysicsServer::ShapeType type_B=p_shape_B->get_type();
+	bool concave_A=p_shape_A->is_concave();
+	bool concave_B=p_shape_B->is_concave();
+
+	bool swap = false;
+
+	if (type_A>type_B) {
+		SWAP(type_A,type_B);
+		SWAP(concave_A,concave_B);
+		swap=true;
+	}
+
+	if (type_A==PhysicsServer::SHAPE_PLANE) {
+
+		if (type_B==PhysicsServer::SHAPE_PLANE)
+			return false;
+		if (type_B==PhysicsServer::SHAPE_RAY) {
+			return false;
+		}
+
+		if (swap) {
+			return solve_static_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+		} else {
+			return solve_static_plane(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+		}
+
+	} else if (type_A==PhysicsServer::SHAPE_RAY) {
+
+		if (type_B==PhysicsServer::SHAPE_RAY)
+			return false;
+
+		if (swap) {
+			return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+		} else {
+			return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+		}
+
+	} else if (concave_B) {
+
+
+		if (concave_A)
+			return false;
+
+		if (!swap)
+			return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+		else
+			return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+
+
+
+	} else {
+
+		return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis);
+	}
+
+
+	return false;
+}

servers/physics/collision_solver_sw.h

@@ -0,0 +1,52 @@
+/*************************************************************************/
+/*  collision_solver_sw.h                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef COLLISION_SOLVER_SW_H
+#define COLLISION_SOLVER_SW_H
+
+#include "shape_sw.h"
+
+class CollisionSolverSW
+{
+public:
+	typedef void (*CallbackResult)(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata);
+private:
+
+	static void concave_callback(void *p_userdata, ShapeSW *p_convex);
+	static bool solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+	static bool solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+	static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+
+public:
+
+
+	static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL);
+
+};
+
+#endif // COLLISION_SOLVER__SW_H

servers/physics/constraint_sw.cpp

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

servers/physics/constraint_sw.h

@@ -0,0 +1,72 @@
+/*************************************************************************/
+/*  constraint_sw.h                                                      */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef CONSTRAINT_SW_H
+#define CONSTRAINT_SW_H
+
+#include "body_sw.h"
+
+class ConstraintSW {
+
+	BodySW **_body_ptr;
+	int _body_count;
+	uint64_t island_step;
+	ConstraintSW *island_next;
+	ConstraintSW *island_list_next;
+
+
+	RID self;
+
+protected:
+	ConstraintSW(BodySW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; }
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+
+
+	_FORCE_INLINE_ ConstraintSW* get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(ConstraintSW* p_next) { island_next=p_next; }
+
+	_FORCE_INLINE_ ConstraintSW* get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(ConstraintSW* p_next) { island_list_next=p_next; }
+
+	_FORCE_INLINE_ BodySW **get_body_ptr() const { return _body_ptr; }
+	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
+
+
+	virtual bool setup(float p_step)=0;
+	virtual void solve(float p_step)=0;
+
+	virtual ~ConstraintSW() {}
+};
+
+#endif // CONSTRAINT__SW_H

servers/physics/gjk_epa.cpp

@@ -0,0 +1,900 @@
+/*************************************************************************/
+/*  gjk_epa.cpp                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "gjk_epa.h"
+
+/*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/
+
+	// Config
+
+/* GJK	*/ 
+#define GJK_MAX_ITERATIONS	128
+#define GJK_ACCURARY		((real_t)0.0001)
+#define GJK_MIN_DISTANCE	((real_t)0.0001)
+#define GJK_DUPLICATED_EPS	((real_t)0.0001)
+#define GJK_SIMPLEX2_EPS	((real_t)0.0)
+#define GJK_SIMPLEX3_EPS	((real_t)0.0)
+#define GJK_SIMPLEX4_EPS	((real_t)0.0)
+
+/* EPA	*/ 
+#define EPA_MAX_VERTICES	64
+#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
+#define EPA_MAX_ITERATIONS	255
+#define EPA_ACCURACY		((real_t)0.0001)
+#define EPA_FALLBACK		(10*EPA_ACCURACY)
+#define EPA_PLANE_EPS		((real_t)0.00001)
+#define EPA_INSIDE_EPS		((real_t)0.01)
+
+namespace GjkEpa2 {
+
+
+struct sResults	{
+	enum eStatus {
+		Separated,		/* Shapes doesnt penetrate */ 
+		Penetrating,	/* Shapes are penetrating */ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+	} status;
+		
+	Vector3	witnesses[2];
+	Vector3	normal;
+	real_t	distance;
+};
+
+// Shorthands
+typedef unsigned int	U;
+typedef unsigned char	U1;
+
+// MinkowskiDiff
+struct	MinkowskiDiff {
+
+	const ShapeSW* m_shapes[2];
+
+	Transform transform_A;
+	Transform transform_B;
+
+	// i wonder how this could be sped up... if it can
+	_FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const {
+		return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) );
+	}
+	
+	_FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const {
+		return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) );
+	}
+	
+	_FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const {
+		return ( Support0 ( d )-Support1 ( -d ) );
+	}
+	
+	_FORCE_INLINE_ Vector3	Support ( const Vector3& d,U index ) const
+	{
+		if ( index )
+			return ( Support1 ( d ) );
+		else
+			return ( Support0 ( d ) );
+	}
+};
+
+typedef	MinkowskiDiff tShape;
+
+
+// GJK
+struct	GJK
+{
+	/* Types		*/ 
+	struct	sSV
+	{
+		Vector3	d,w;
+	};
+	struct	sSimplex
+	{
+		sSV*		c[4];
+		real_t	p[4];
+		U			rank;
+	};
+	struct	eStatus	{ enum _ {
+		Valid,
+		Inside,
+		Failed		};};
+		/* Fields		*/ 
+		tShape			m_shape;
+		Vector3		m_ray;
+		real_t		m_distance;
+		sSimplex		m_simplices[2];
+		sSV				m_store[4];
+		sSV*			m_free[4];
+		U				m_nfree;
+		U				m_current;
+		sSimplex*		m_simplex;
+		eStatus::_		m_status;
+		/* Methods		*/ 
+		GJK()
+		{
+			Initialize();
+		}
+		void				Initialize()
+		{
+			m_ray		=	Vector3(0,0,0);
+			m_nfree		=	0;
+			m_status	=	eStatus::Failed;
+			m_current	=	0;
+			m_distance	=	0;
+		}
+		eStatus::_			Evaluate(const tShape& shapearg,const Vector3& guess)
+		{
+			U			iterations=0;
+			real_t	sqdist=0;
+			real_t	alpha=0;
+			Vector3	lastw[4];
+			U			clastw=0;
+			/* Initialize solver		*/ 
+			m_free[0]			=	&m_store[0];
+			m_free[1]			=	&m_store[1];
+			m_free[2]			=	&m_store[2];
+			m_free[3]			=	&m_store[3];
+			m_nfree				=	4;
+			m_current			=	0;
+			m_status			=	eStatus::Valid;
+			m_shape				=	shapearg;
+			m_distance			=	0;
+			/* Initialize simplex		*/ 
+			m_simplices[0].rank	=	0;
+			m_ray				=	guess;
+			const real_t	sqrl=	m_ray.length_squared();
+			appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0));
+			m_simplices[0].p[0]	=	1;
+			m_ray				=	m_simplices[0].c[0]->w;	
+			sqdist				=	sqrl;
+			lastw[0]			=
+				lastw[1]			=
+				lastw[2]			=
+				lastw[3]			=	m_ray;
+			/* Loop						*/ 
+			do	{
+				const U		next=1-m_current;
+				sSimplex&	cs=m_simplices[m_current];
+				sSimplex&	ns=m_simplices[next];
+				/* Check zero							*/ 
+				const real_t	rl=m_ray.length();
+				if(rl<GJK_MIN_DISTANCE)
+				{/* Touching or inside				*/ 
+					m_status=eStatus::Inside;
+					break;
+				}
+				/* Append new vertice in -'v' direction	*/ 
+				appendvertice(cs,-m_ray);
+				const Vector3&	w=cs.c[cs.rank-1]->w;
+				bool				found=false;
+				for(U i=0;i<4;++i)
+				{
+					if((w-lastw[i]).length_squared()<GJK_DUPLICATED_EPS)
+					{ found=true;break; }
+				}
+				if(found)
+				{/* Return old simplex				*/ 
+					removevertice(m_simplices[m_current]);
+					break;
+				}
+				else
+				{/* Update lastw					*/ 
+					lastw[clastw=(clastw+1)&3]=w;
+				}
+				/* Check for termination				*/ 
+				const real_t	omega=vec3_dot(m_ray,w)/rl;
+				alpha=MAX(omega,alpha);
+				if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+				{/* Return old simplex				*/ 
+					removevertice(m_simplices[m_current]);
+					break;
+				}		
+				/* Reduce simplex						*/ 
+				real_t	weights[4];
+				U			mask=0;
+				switch(cs.rank)
+				{
+				case	2:	sqdist=projectorigin(	cs.c[0]->w,
+								cs.c[1]->w,
+								weights,mask);break;
+				case	3:	sqdist=projectorigin(	cs.c[0]->w,
+								cs.c[1]->w,
+								cs.c[2]->w,
+								weights,mask);break;
+				case	4:	sqdist=projectorigin(	cs.c[0]->w,
+								cs.c[1]->w,
+								cs.c[2]->w,
+								cs.c[3]->w,
+								weights,mask);break;
+				}
+				if(sqdist>=0)
+				{/* Valid	*/ 
+					ns.rank		=	0;
+					m_ray		=	Vector3(0,0,0);
+					m_current	=	next;
+					for(U i=0,ni=cs.rank;i<ni;++i)
+					{
+						if(mask&(1<<i))
+						{
+							ns.c[ns.rank]		=	cs.c[i];
+							ns.p[ns.rank++]		=	weights[i];
+							m_ray				+=	cs.c[i]->w*weights[i];
+						}
+						else
+						{
+							m_free[m_nfree++]	=	cs.c[i];
+						}
+					}
+					if(mask==15) m_status=eStatus::Inside;
+				}
+				else
+				{/* Return old simplex				*/ 
+					removevertice(m_simplices[m_current]);
+					break;
+				}
+				m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
+			} while(m_status==eStatus::Valid);
+			m_simplex=&m_simplices[m_current];
+			switch(m_status)
+			{
+			case	eStatus::Valid:		m_distance=m_ray.length();break;
+			case	eStatus::Inside:	m_distance=0;break;
+			default: {}
+			}	
+			return(m_status);
+		}
+		bool					EncloseOrigin()
+		{
+			switch(m_simplex->rank)
+			{
+			case	1:
+				{
+					for(U i=0;i<3;++i)
+					{
+						Vector3		axis=Vector3(0,0,0);
+						axis[i]=1;
+						appendvertice(*m_simplex, axis);
+						if(EncloseOrigin())	return(true);
+						removevertice(*m_simplex);
+						appendvertice(*m_simplex,-axis);
+						if(EncloseOrigin())	return(true);
+						removevertice(*m_simplex);
+					}
+				}
+				break;
+			case	2:
+				{
+					const Vector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
+					for(U i=0;i<3;++i)
+					{
+						Vector3		axis=Vector3(0,0,0);
+						axis[i]=1;
+						const Vector3	p=vec3_cross(d,axis);
+						if(p.length_squared()>0)
+						{
+							appendvertice(*m_simplex, p);
+							if(EncloseOrigin())	return(true);
+							removevertice(*m_simplex);
+							appendvertice(*m_simplex,-p);
+							if(EncloseOrigin())	return(true);
+							removevertice(*m_simplex);
+						}
+					}
+				}
+				break;
+			case	3:
+				{
+					const Vector3	n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+						m_simplex->c[2]->w-m_simplex->c[0]->w);
+					if(n.length_squared()>0)
+					{
+						appendvertice(*m_simplex,n);
+						if(EncloseOrigin())	return(true);
+						removevertice(*m_simplex);
+						appendvertice(*m_simplex,-n);
+						if(EncloseOrigin())	return(true);
+						removevertice(*m_simplex);
+					}
+				}
+				break;
+			case	4:
+				{
+					if(Math::abs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
+						m_simplex->c[1]->w-m_simplex->c[3]->w,
+						m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
+						return(true);
+				}
+				break;
+			}
+			return(false);
+		}
+		/* Internals	*/ 
+		void				getsupport(const Vector3& d,sSV& sv) const
+		{
+			sv.d	=	d/d.length();
+			sv.w	=	m_shape.Support(sv.d);
+		}
+		void				removevertice(sSimplex& simplex)
+		{
+			m_free[m_nfree++]=simplex.c[--simplex.rank];
+		}
+		void				appendvertice(sSimplex& simplex,const Vector3& v)
+		{
+			simplex.p[simplex.rank]=0;
+			simplex.c[simplex.rank]=m_free[--m_nfree];
+			getsupport(v,*simplex.c[simplex.rank++]);
+		}
+		static real_t		det(const Vector3& a,const Vector3& b,const Vector3& c)
+		{
+			return(	a.y*b.z*c.x+a.z*b.x*c.y-
+				a.x*b.z*c.y-a.y*b.x*c.z+
+				a.x*b.y*c.z-a.z*b.y*c.x);
+		}
+		static real_t		projectorigin(	const Vector3& a,
+			const Vector3& b,
+			real_t* w,U& m)
+		{
+			const Vector3	d=b-a;
+			const real_t	l=d.length_squared();
+			if(l>GJK_SIMPLEX2_EPS)
+			{
+				const real_t	t(l>0?-vec3_dot(a,d)/l:0);
+				if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length_squared()); }
+				else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length_squared()); }
+				else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); }
+			}
+			return(-1);
+		}
+		static real_t		projectorigin(	const Vector3& a,
+			const Vector3& b,
+			const Vector3& c,
+			real_t* w,U& m)
+		{
+			static const U		imd3[]={1,2,0};
+			const Vector3*	vt[]={&a,&b,&c};
+			const Vector3		dl[]={a-b,b-c,c-a};
+			const Vector3		n=vec3_cross(dl[0],dl[1]);
+			const real_t		l=n.length_squared();
+			if(l>GJK_SIMPLEX3_EPS)
+			{
+				real_t	mindist=-1;
+				real_t	subw[2];
+				U			subm;
+				for(U i=0;i<3;++i)
+				{
+					if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0)
+					{
+						const U			j=imd3[i];
+						const real_t	subd(projectorigin(*vt[i],*vt[j],subw,subm));
+						if((mindist<0)||(subd<mindist))
+						{
+							mindist		=	subd;
+							m			=	static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
+							w[i]		=	subw[0];
+							w[j]		=	subw[1];
+							w[imd3[j]]	=	0;				
+						}
+					}
+				}
+				if(mindist<0)
+				{
+					const real_t	d=vec3_dot(a,n);	
+					const real_t	s=Math::sqrt(l);
+					const Vector3	p=n*(d/l);
+					mindist	=	p.length_squared();
+					m		=	7;
+					w[0]	=	(vec3_cross(dl[1],b-p)).length()/s;
+					w[1]	=	(vec3_cross(dl[2],c-p)).length()/s;
+					w[2]	=	1-(w[0]+w[1]);
+				}
+				return(mindist);
+			}
+			return(-1);
+		}
+		static real_t		projectorigin(	const Vector3& a,
+			const Vector3& b,
+			const Vector3& c,
+			const Vector3& d,
+			real_t* w,U& m)
+		{
+			static const U		imd3[]={1,2,0};
+			const Vector3*	vt[]={&a,&b,&c,&d};
+			const Vector3		dl[]={a-d,b-d,c-d};
+			const real_t		vl=det(dl[0],dl[1],dl[2]);
+			const bool			ng=(vl*vec3_dot(a,vec3_cross(b-c,a-b)))<=0;
+			if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS))
+			{
+				real_t	mindist=-1;
+				real_t	subw[3];
+				U			subm;
+				for(U i=0;i<3;++i)
+				{
+					const U			j=imd3[i];
+					const real_t	s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j]));
+					if(s>0)
+					{
+						const real_t	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
+						if((mindist<0)||(subd<mindist))
+						{
+							mindist		=	subd;
+							m			=	static_cast<U>((subm&1?1<<i:0)+
+								(subm&2?1<<j:0)+
+								(subm&4?8:0));
+							w[i]		=	subw[0];
+							w[j]		=	subw[1];
+							w[imd3[j]]	=	0;
+							w[3]		=	subw[2];
+						}
+					}
+				}
+				if(mindist<0)
+				{
+					mindist	=	0;
+					m		=	15;
+					w[0]	=	det(c,b,d)/vl;
+					w[1]	=	det(a,c,d)/vl;
+					w[2]	=	det(b,a,d)/vl;
+					w[3]	=	1-(w[0]+w[1]+w[2]);
+				}
+				return(mindist);
+			}
+			return(-1);
+		}
+};
+
+	// EPA
+	struct	EPA
+	{
+		/* Types		*/ 
+		typedef	GJK::sSV	sSV;
+		struct	sFace
+		{
+			Vector3	n;
+			real_t	d;
+			real_t	p;
+			sSV*		c[3];
+			sFace*		f[3];
+			sFace*		l[2];
+			U1			e[3];
+			U1			pass;
+		};
+		struct	sList
+		{
+			sFace*		root;
+			U			count;
+			sList() : root(0),count(0)	{}
+		};
+		struct	sHorizon
+		{
+			sFace*		cf;
+			sFace*		ff;
+			U			nf;
+			sHorizon() : cf(0),ff(0),nf(0)	{}
+		};
+		struct	eStatus { enum _ {
+			Valid,
+			Touching,
+			Degenerated,
+			NonConvex,
+			InvalidHull,		
+			OutOfFaces,
+			OutOfVertices,
+			AccuraryReached,
+			FallBack,
+			Failed		};};
+			/* Fields		*/ 
+			eStatus::_		m_status;
+			GJK::sSimplex	m_result;
+			Vector3		m_normal;
+			real_t		m_depth;
+			sSV				m_sv_store[EPA_MAX_VERTICES];
+			sFace			m_fc_store[EPA_MAX_FACES];
+			U				m_nextsv;
+			sList			m_hull;
+			sList			m_stock;
+			/* Methods		*/ 
+			EPA()
+			{
+				Initialize();	
+			}
+
+
+			static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
+			{
+				fa->e[ea]=(U1)eb;fa->f[ea]=fb;
+				fb->e[eb]=(U1)ea;fb->f[eb]=fa;
+			}
+			static inline void		append(sList& list,sFace* face)
+			{
+				face->l[0]	=	0;
+				face->l[1]	=	list.root;
+				if(list.root) list.root->l[0]=face;
+				list.root	=	face;
+				++list.count;
+			}
+			static inline void		remove(sList& list,sFace* face)
+			{
+				if(face->l[1]) face->l[1]->l[0]=face->l[0];
+				if(face->l[0]) face->l[0]->l[1]=face->l[1];
+				if(face==list.root) list.root=face->l[1];
+				--list.count;
+			}
+
+
+			void				Initialize()
+			{
+				m_status	=	eStatus::Failed;
+				m_normal	=	Vector3(0,0,0);
+				m_depth		=	0;
+				m_nextsv	=	0;
+				for(U i=0;i<EPA_MAX_FACES;++i)
+				{
+					append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
+				}
+			}
+			eStatus::_			Evaluate(GJK& gjk,const Vector3& guess)
+			{
+				GJK::sSimplex&	simplex=*gjk.m_simplex;
+				if((simplex.rank>1)&&gjk.EncloseOrigin())
+				{
+
+					/* Clean up				*/ 
+					while(m_hull.root)
+					{
+						sFace*	f = m_hull.root;
+						remove(m_hull,f);
+						append(m_stock,f);
+					}
+					m_status	=	eStatus::Valid;
+					m_nextsv	=	0;
+					/* Orient simplex		*/ 
+					if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
+						simplex.c[1]->w-simplex.c[3]->w,
+						simplex.c[2]->w-simplex.c[3]->w)<0)
+					{
+						SWAP(simplex.c[0],simplex.c[1]);
+						SWAP(simplex.p[0],simplex.p[1]);
+					}
+					/* Build initial hull	*/ 
+					sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
+						newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
+						newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
+						newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
+					if(m_hull.count==4)
+					{
+						sFace*		best=findbest();
+						sFace		outer=*best;
+						U			pass=0;
+						U			iterations=0;
+						bind(tetra[0],0,tetra[1],0);
+						bind(tetra[0],1,tetra[2],0);
+						bind(tetra[0],2,tetra[3],0);
+						bind(tetra[1],1,tetra[3],2);
+						bind(tetra[1],2,tetra[2],1);
+						bind(tetra[2],2,tetra[3],1);
+						m_status=eStatus::Valid;
+						for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+						{
+							if(m_nextsv<EPA_MAX_VERTICES)
+							{	
+								sHorizon		horizon;
+								sSV*			w=&m_sv_store[m_nextsv++];
+								bool			valid=true;					
+								best->pass	=	(U1)(++pass);
+								gjk.getsupport(best->n,*w);
+								const real_t	wdist=vec3_dot(best->n,w->w)-best->d;
+								if(wdist>EPA_ACCURACY)
+								{
+									for(U j=0;(j<3)&&valid;++j)
+									{
+										valid&=expand(	pass,w,
+											best->f[j],best->e[j],
+											horizon);
+									}
+									if(valid&&(horizon.nf>=3))
+									{
+										bind(horizon.cf,1,horizon.ff,2);
+										remove(m_hull,best);
+										append(m_stock,best);
+										best=findbest();
+										if(best->p>=outer.p) outer=*best;
+									} else { m_status=eStatus::InvalidHull;break; }
+								} else { m_status=eStatus::AccuraryReached;break; }
+							} else { m_status=eStatus::OutOfVertices;break; }
+						}
+						const Vector3	projection=outer.n*outer.d;
+						m_normal	=	outer.n;
+						m_depth		=	outer.d;
+						m_result.rank	=	3;
+						m_result.c[0]	=	outer.c[0];
+						m_result.c[1]	=	outer.c[1];
+						m_result.c[2]	=	outer.c[2];
+						m_result.p[0]	=	vec3_cross(	outer.c[1]->w-projection,
+							outer.c[2]->w-projection).length();
+						m_result.p[1]	=	vec3_cross(	outer.c[2]->w-projection,
+							outer.c[0]->w-projection).length();
+						m_result.p[2]	=	vec3_cross(	outer.c[0]->w-projection,
+							outer.c[1]->w-projection).length();
+						const real_t	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
+						m_result.p[0]	/=	sum;
+						m_result.p[1]	/=	sum;
+						m_result.p[2]	/=	sum;
+						return(m_status);
+					}
+				}
+				/* Fallback		*/ 
+				m_status	=	eStatus::FallBack;
+				m_normal	=	-guess;
+				const real_t	nl=m_normal.length();
+				if(nl>0)
+					m_normal	=	m_normal/nl;
+				else
+					m_normal	=	Vector3(1,0,0);
+				m_depth	=	0;
+				m_result.rank=1;
+				m_result.c[0]=simplex.c[0];
+				m_result.p[0]=1;	
+				return(m_status);
+			}
+			sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
+			{
+				if(m_stock.root)
+				{
+					sFace*	face=m_stock.root;
+					remove(m_stock,face);
+					append(m_hull,face);
+					face->pass	=	0;
+					face->c[0]	=	a;
+					face->c[1]	=	b;
+					face->c[2]	=	c;
+					face->n		=	vec3_cross(b->w-a->w,c->w-a->w);
+					const real_t	l=face->n.length();
+					const bool		v=l>EPA_ACCURACY;
+					face->p		=	MIN(MIN(
+						vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)),
+						vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))),
+						vec3_dot(c->w,vec3_cross(face->n,c->w-a->w)))	/
+						(v?l:1);
+					face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
+					if(v)
+					{
+						face->d		=	vec3_dot(a->w,face->n)/l;
+						face->n		/=	l;
+						if(forced||(face->d>=-EPA_PLANE_EPS))
+						{
+							return(face);
+						} else m_status=eStatus::NonConvex;
+					} else m_status=eStatus::Degenerated;
+					remove(m_hull,face);
+					append(m_stock,face);
+					return(0);
+				}
+				m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
+				return(0);
+			}
+			sFace*				findbest()
+			{
+				sFace*		minf=m_hull.root;
+				real_t	mind=minf->d*minf->d;
+				real_t	maxp=minf->p;
+				for(sFace* f=minf->l[1];f;f=f->l[1])
+				{
+					const real_t	sqd=f->d*f->d;
+					if((f->p>=maxp)&&(sqd<mind))
+					{
+						minf=f;
+						mind=sqd;
+						maxp=f->p;
+					}
+				}
+				return(minf);
+			}
+			bool				expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
+			{
+				static const U	i1m3[]={1,2,0};
+				static const U	i2m3[]={2,0,1};
+				if(f->pass!=pass)
+				{
+					const U	e1=i1m3[e];
+					if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+					{
+						sFace*	nf=newface(f->c[e1],f->c[e],w,false);
+						if(nf)
+						{
+							bind(nf,0,f,e);
+							if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
+							horizon.cf=nf;
+							++horizon.nf;
+							return(true);
+						}
+					}
+					else
+					{
+						const U	e2=i2m3[e];
+						f->pass		=	(U1)pass;
+						if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
+							expand(pass,w,f->f[e2],f->e[e2],horizon))
+						{
+							remove(m_hull,f);
+							append(m_stock,f);
+							return(true);
+						}
+					}
+				}
+				return(false);
+			}
+
+	};
+
+	//
+	static void	Initialize(	const ShapeSW* shape0,const Transform& wtrs0,
+		const ShapeSW* shape1,const Transform& wtrs1,
+		sResults& results,
+		tShape& shape,
+		bool withmargins)
+	{
+		/* Results		*/ 
+		results.witnesses[0]	=
+			results.witnesses[1]	=	Vector3(0,0,0);
+		results.status			=	sResults::Separated;
+		/* Shape		*/ 
+		shape.m_shapes[0]		=	shape0;
+		shape.m_shapes[1]		=	shape1;
+		shape.transform_A		=	wtrs0;
+		shape.transform_B		=	wtrs1;
+		
+	}
+
+
+
+//
+// Api
+//
+
+//
+
+//
+bool Distance(	const ShapeSW*	shape0,
+									  const Transform&		wtrs0,
+									  const ShapeSW*	shape1,
+									  const Transform&		wtrs1,
+									  const Vector3&		guess,
+									  sResults&				results)
+{
+	tShape			shape;
+	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
+	GJK				gjk;
+	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
+	if(gjk_status==GJK::eStatus::Valid)
+	{
+		Vector3	w0=Vector3(0,0,0);
+		Vector3	w1=Vector3(0,0,0);
+		for(U i=0;i<gjk.m_simplex->rank;++i)
+		{
+			const real_t	p=gjk.m_simplex->p[i];
+			w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
+			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+		}
+		results.witnesses[0]	=	wtrs0.xform(w0);
+		results.witnesses[1]	=	wtrs0.xform(w1);
+		results.normal			=	w0-w1;
+		results.distance		=	results.normal.length();
+		results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
+		return(true);
+	}
+	else
+	{
+		results.status	=	gjk_status==GJK::eStatus::Inside?
+			sResults::Penetrating	:
+		sResults::GJK_Failed	;
+		return(false);
+	}
+}
+
+//
+bool Penetration(	const ShapeSW*	shape0,
+									 const Transform&		wtrs0,
+									 const ShapeSW*	shape1,
+									 const Transform&		wtrs1,
+									 const Vector3&		guess,
+									 sResults&				results
+									)
+{
+	tShape			shape;
+	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
+	GJK				gjk;	
+	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,-guess);
+	switch(gjk_status)
+	{
+	case	GJK::eStatus::Inside:
+		{
+			EPA				epa;
+			EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
+			if(epa_status!=EPA::eStatus::Failed)
+			{
+				Vector3	w0=Vector3(0,0,0);
+				for(U i=0;i<epa.m_result.rank;++i)
+				{
+					w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
+				}
+				results.status			=	sResults::Penetrating;
+				results.witnesses[0]	=	w0;
+				results.witnesses[1]	=	w0-epa.m_normal*epa.m_depth;
+				results.normal			=	-epa.m_normal;
+				results.distance		=	-epa.m_depth;
+				return(true);
+			} else results.status=sResults::EPA_Failed;
+		}
+		break;
+	case	GJK::eStatus::Failed:
+		results.status=sResults::GJK_Failed;
+		break;
+	default: {}
+	}
+	return(false);
+}
+
+
+/* Symbols cleanup		*/ 
+
+#undef GJK_MAX_ITERATIONS
+#undef GJK_ACCURARY
+#undef GJK_MIN_DISTANCE
+#undef GJK_DUPLICATED_EPS
+#undef GJK_SIMPLEX2_EPS
+#undef GJK_SIMPLEX3_EPS
+#undef GJK_SIMPLEX4_EPS
+
+#undef EPA_MAX_VERTICES
+#undef EPA_MAX_FACES
+#undef EPA_MAX_ITERATIONS
+#undef EPA_ACCURACY
+#undef EPA_FALLBACK
+#undef EPA_PLANE_EPS
+#undef EPA_INSIDE_EPS
+
+
+} // end of namespace
+
+
+
+bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap ) {
+
+	GjkEpa2::sResults res;
+	
+	if (GjkEpa2::Penetration(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) {
+		if (p_result_callback) {
+			if (p_swap)
+				p_result_callback(res.witnesses[1],res.witnesses[0],p_userdata);
+			else
+				p_result_callback(res.witnesses[0],res.witnesses[1],p_userdata);
+		}
+		return true;
+	} 
+	
+	return false;
+}
+

servers/physics/gjk_epa.h

@@ -0,0 +1,40 @@
+/*************************************************************************/
+/*  gjk_epa.h                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef GJK_EPA_H
+#define GJK_EPA_H
+
+#include "shape_sw.h"
+/**
+	@author Juan Linietsky <reduzio@gmail.com>
+*/
+#include "collision_solver_sw.h"
+
+bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false);
+
+#endif

servers/physics/joints_sw.cpp

@@ -0,0 +1,450 @@
+/*************************************************************************/
+/*  joints_sw.cpp                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "joints_sw.h"
+#include "space_sw.h"
+
+#if 0
+
+//based on chipmunk joint constraints
+
+/* Copyright (c) 2007 Scott Lembcke
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+static inline real_t k_scalar(Body2DSW *a,Body2DSW *b,const Vector2& rA, const Vector2& rB, const Vector2& n) {
+
+
+	real_t value=0;
+
+
+	{
+		value+=a->get_inv_mass();
+		real_t rcn = rA.cross(n);
+		value+=a->get_inv_inertia() * rcn * rcn;
+	}
+
+	if (b) {
+
+		value+=b->get_inv_mass();
+		real_t rcn = rB.cross(n);
+		value+=b->get_inv_inertia() * rcn * rcn;
+	}
+
+	return value;
+
+}
+
+
+bool PinJoint2DSW::setup(float p_step) {
+
+	Space2DSW *space = A->get_space();
+	ERR_FAIL_COND_V(!space,false;)
+	rA = A->get_transform().xform(anchor_A);
+	rB = B?B->get_transform().xform(anchor_B):anchor_B;
+
+	Vector2 delta = rB - rA;
+
+	rA-= A->get_transform().get_origin();
+	if (B)
+		rB-=B->get_transform().get_origin();
+
+
+	real_t jdist = delta.length();
+	correct=false;
+	if (jdist==0)
+		return false; // do not correct
+
+	correct=true;
+
+	n = delta / jdist;
+
+	// calculate mass normal
+	mass_normal = 1.0f/k_scalar(A, B, rA, rB, n);
+
+	// calculate bias velocity
+	//real_t maxBias = joint->constraint.maxBias;
+	bias = -(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)*(jdist-dist);
+	bias = CLAMP(bias, -get_max_bias(), +get_max_bias());
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// apply accumulated impulse
+	Vector2 j = n * jn_acc;
+	A->apply_impulse(rA,-j);
+	if (B)
+		B->apply_impulse(rB,j);
+
+	return true;
+}
+
+
+static inline Vector2
+relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB){
+	Vector2 sum = a->get_linear_velocity() -rA.tangent() * a->get_angular_velocity();
+	if (b)
+		return (b->get_linear_velocity() -rB.tangent() * b->get_angular_velocity()) - sum;
+	else
+		return -sum;
+}
+
+static inline real_t
+normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n){
+	return relative_velocity(a, b, rA, rB).dot(n);
+}
+
+
+void PinJoint2DSW::solve(float p_step){
+
+	if (!correct)
+		return;
+
+	Vector2 ln = n;
+
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A,B, rA, rB, ln);
+
+	// compute normal impulse
+	real_t jn = (bias - vrn)*mass_normal;
+	real_t jnOld = jn_acc;
+	jn_acc = CLAMP(jnOld + jn,-jn_max,jn_max); //cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
+	jn = jn_acc - jnOld;
+
+	Vector2 j = jn*ln;
+
+	A->apply_impulse(rA,-j);
+	if (B)
+		B->apply_impulse(rB,j);
+
+}
+
+
+PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) {
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
+	anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos;
+
+	jn_acc=0;
+	dist=0;
+
+	p_body_a->add_constraint(this,0);
+	if (p_body_b)
+		p_body_b->add_constraint(this,1);
+
+}
+
+PinJoint2DSW::~PinJoint2DSW() {
+
+	if (A)
+		A->remove_constraint(this);
+	if (B)
+		B->remove_constraint(this);
+
+}
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+
+static inline void
+k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2)
+{
+	// calculate mass matrix
+	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
+	real_t k11, k12, k21, k22;
+	real_t m_sum = a->get_inv_mass() + b->get_inv_mass();
+
+	// start with I*m_sum
+	k11 = m_sum; k12 = 0.0f;
+	k21 = 0.0f;  k22 = m_sum;
+
+	// add the influence from r1
+	real_t a_i_inv = a->get_inv_inertia();
+	real_t r1xsq =  r1.x * r1.x * a_i_inv;
+	real_t r1ysq =  r1.y * r1.y * a_i_inv;
+	real_t r1nxy = -r1.x * r1.y * a_i_inv;
+	k11 += r1ysq; k12 += r1nxy;
+	k21 += r1nxy; k22 += r1xsq;
+
+	// add the influnce from r2
+	real_t b_i_inv = b->get_inv_inertia();
+	real_t r2xsq =  r2.x * r2.x * b_i_inv;
+	real_t r2ysq =  r2.y * r2.y * b_i_inv;
+	real_t r2nxy = -r2.x * r2.y * b_i_inv;
+	k11 += r2ysq; k12 += r2nxy;
+	k21 += r2nxy; k22 += r2xsq;
+
+	// invert
+	real_t determinant = k11*k22 - k12*k21;
+	ERR_FAIL_COND(determinant== 0.0);
+
+	real_t det_inv = 1.0f/determinant;
+	*k1 = Vector2( k22*det_inv, -k12*det_inv);
+	*k2 = Vector2(-k21*det_inv,  k11*det_inv);
+}
+
+static _FORCE_INLINE_ Vector2
+mult_k(const Vector2& vr, const Vector2 &k1, const Vector2 &k2)
+{
+	return Vector2(vr.dot(k1), vr.dot(k2));
+}
+
+bool GrooveJoint2DSW::setup(float p_step) {
+
+
+	// calculate endpoints in worldspace
+	Vector2 ta = A->get_transform().xform(A_groove_1);
+	Vector2 tb = A->get_transform().xform(A_groove_2);
+	Space2DSW *space=A->get_space();
+
+	// calculate axis
+	Vector2 n = -(tb - ta).tangent().normalized();
+	real_t d = ta.dot(n);
+
+	xf_normal = n;
+	rB = B->get_transform().basis_xform(B_anchor);
+
+	// calculate tangential distance along the axis of rB
+	real_t td = (B->get_transform().get_origin() + rB).cross(n);
+	// calculate clamping factor and rB
+	if(td <= ta.cross(n)){
+		clamp = 1.0f;
+		rA = ta - A->get_transform().get_origin();
+	} else if(td >= tb.cross(n)){
+		clamp = -1.0f;
+		rA = tb - A->get_transform().get_origin();
+	} else {
+		clamp = 0.0f;
+		//joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
+		rA =  ((-n.tangent() * -td) + n*d) - A->get_transform().get_origin();
+	}
+
+	// Calculate mass tensor
+	k_tensor(A, B, rA, rB, &k1, &k2);
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// calculate bias velocity
+//	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+//	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
+
+
+	Vector2 delta = (B->get_transform().get_origin() +rB) - (A->get_transform().get_origin() + rA);
+	gbias=(delta*-(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)).clamped(get_max_bias());
+
+	// apply accumulated impulse
+	A->apply_impulse(rA,-jn_acc);
+	B->apply_impulse(rB,jn_acc);
+
+	correct=true;
+	return true;
+}
+
+void GrooveJoint2DSW::solve(float p_step){
+
+
+	// compute impulse
+	Vector2 vr = relative_velocity(A, B, rA,rB);
+
+	Vector2 j = mult_k(gbias-vr, k1, k2);
+	Vector2 jOld = jn_acc;
+	j+=jOld;
+
+	jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : xf_normal.project(j)).clamped(jn_max);
+
+	j = jn_acc - jOld;
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+}
+
+
+GrooveJoint2DSW::GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
+
+	A=p_body_a;
+	B=p_body_b;
+
+	A_groove_1 = A->get_inv_transform().xform(p_a_groove1);
+	A_groove_2 = A->get_inv_transform().xform(p_a_groove2);
+	B_anchor=B->get_inv_transform().xform(p_b_anchor);
+	A_groove_normal = -(A_groove_2 - A_groove_1).normalized().tangent();
+
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+
+}
+
+GrooveJoint2DSW::~GrooveJoint2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+}
+
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+
+bool DampedSpringJoint2DSW::setup(float p_step) {
+
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B->get_transform().basis_xform(anchor_B);
+
+	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA) ;
+	real_t dist = delta.length();
+
+	if (dist)
+		n=delta/dist;
+	else
+		n=Vector2();
+
+	real_t k = k_scalar(A, B, rA, rB, n);
+	n_mass = 1.0f/k;
+
+	target_vrn = 0.0f;
+	v_coef = 1.0f - Math::exp(-damping*(p_step)*k);
+
+	// apply spring force
+	real_t f_spring = (rest_length - dist) * stiffness;
+	Vector2 j = n * f_spring*(p_step);
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+
+
+	return true;
+}
+
+void DampedSpringJoint2DSW::solve(float p_step) {
+
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn;
+
+	// compute velocity loss from drag
+	// not 100% certain this is derived correctly, though it makes sense
+	real_t v_damp = -vrn*v_coef;
+	target_vrn = vrn + v_damp;
+	Vector2 j=n*v_damp*n_mass;
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+
+}
+
+void DampedSpringJoint2DSW::set_param(Physics2DServer::DampedStringParam p_param, real_t p_value) {
+
+	switch(p_param) {
+
+		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
+
+			rest_length=p_value;
+		} break;
+		case Physics2DServer::DAMPED_STRING_DAMPING: {
+
+			damping=p_value;
+		} break;
+		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
+
+			stiffness=p_value;
+		} break;
+	}
+
+}
+
+real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const{
+
+	switch(p_param) {
+
+		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
+
+			return rest_length;
+		} break;
+		case Physics2DServer::DAMPED_STRING_DAMPING: {
+
+			return damping;
+		} break;
+		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
+
+			return stiffness;
+		} break;
+	}
+
+	ERR_FAIL_V(0);
+}
+
+
+DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
+
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = A->get_inv_transform().xform(p_anchor_a);
+	anchor_B = B->get_inv_transform().xform(p_anchor_b);
+
+	rest_length=p_anchor_a.distance_to(p_anchor_b);
+	stiffness=20;
+	damping=1.5;
+
+
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+
+}
+
+DampedSpringJoint2DSW::~DampedSpringJoint2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+
+}
+
+
+#endif

servers/physics/joints_sw.h

@@ -0,0 +1,176 @@
+/*************************************************************************/
+/*  joints_sw.h                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef JOINTS_SW_H
+#define JOINTS_SW_H
+
+#include "constraint_sw.h"
+#include "body_sw.h"
+
+
+
+class JointSW : public ConstraintSW {
+
+	real_t max_force;
+	real_t bias;
+	real_t max_bias;
+public:
+
+	_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; }
+	_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
+
+	_FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; }
+	_FORCE_INLINE_ real_t get_bias() const { return bias; }
+
+	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; }
+	_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
+
+//	virtual PhysicsServer::JointType get_type() const=0;
+	JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; };
+
+};
+
+#if 0
+class PinJointSW : public JointSW {
+
+	union {
+		struct {
+			BodySW *A;
+			BodySW *B;
+		};
+
+		BodySW *_arr[2];
+	};
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+	real_t dist;
+	real_t jn_acc;
+	real_t jn_max;
+	real_t max_distance;
+	real_t mass_normal;
+	real_t bias;
+
+	Vector2 rA,rB;
+	Vector2 n; //normal
+	bool correct;
+
+
+public:
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	PinJointSW(const Vector2& p_pos,BodySW* p_body_a,BodySW* p_body_b=NULL);
+	~PinJointSW();
+};
+
+
+class GrooveJointSW : public JointSW {
+
+	union {
+		struct {
+			BodySW *A;
+			BodySW *B;
+		};
+
+		BodySW *_arr[2];
+	};
+
+	Vector2 A_groove_1;
+	Vector2 A_groove_2;
+	Vector2 A_groove_normal;
+	Vector2 B_anchor;
+	Vector2 jn_acc;
+	Vector2 gbias;
+	real_t jn_max;
+	real_t clamp;
+	Vector2 xf_normal;
+	Vector2 rA,rB;
+	Vector2 k1,k2;
+
+
+	bool correct;
+
+public:
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_GROOVE; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	GrooveJointSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, BodySW* p_body_a,BodySW* p_body_b);
+	~GrooveJointSW();
+};
+
+
+class DampedSpringJointSW : public JointSW {
+
+	union {
+		struct {
+			BodySW *A;
+			BodySW *B;
+		};
+
+		BodySW *_arr[2];
+	};
+
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+
+	real_t rest_length;
+	real_t damping;
+	real_t stiffness;
+
+	Vector2 rA,rB;
+	Vector2 n;
+	real_t n_mass;
+	real_t target_vrn;
+	real_t v_coef;
+
+public:
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_DAMPED_SPRING; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+	void set_param(PhysicsServer::DampedStringParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer::DampedStringParam p_param) const;
+
+	DampedSpringJointSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, BodySW* p_body_a,BodySW* p_body_b);
+	~DampedSpringJointSW();
+};
+#endif
+
+#endif // JOINTS__SW_H

servers/physics/physics_server_sw.h

@@ -0,0 +1,215 @@
+/*************************************************************************/
+/*  physics_server_sw.h                                                  */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef PHYSICS_SERVER_SW
+#define PHYSICS_SERVER_SW
+
+
+#include "servers/physics_server.h"
+#include "shape_sw.h"
+#include "space_sw.h"
+#include "step_sw.h"
+#include "joints_sw.h"
+
+
+class PhysicsServerSW : public PhysicsServer {
+
+	OBJ_TYPE( PhysicsServerSW, PhysicsServer );
+
+friend class PhysicsDirectSpaceStateSW;
+	bool active;
+	int iterations;
+	bool doing_sync;
+	real_t last_step;
+
+	StepSW *stepper;
+	Set<const SpaceSW*> active_spaces;
+
+	PhysicsDirectBodyStateSW *direct_state;
+
+	mutable RID_Owner<ShapeSW> shape_owner;
+	mutable RID_Owner<SpaceSW> space_owner;
+	mutable RID_Owner<AreaSW> area_owner;
+	mutable RID_Owner<BodySW> body_owner;
+	mutable RID_Owner<JointSW> joint_owner;
+
+//	void _clear_query(QuerySW *p_query);
+public:
+
+	virtual RID shape_create(ShapeType p_shape);
+	virtual void shape_set_data(RID p_shape, const Variant& p_data);
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
+
+	virtual ShapeType shape_get_type(RID p_shape) const;
+	virtual Variant shape_get_data(RID p_shape) const;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
+
+	/* SPACE API */
+
+	virtual RID space_create();
+	virtual void space_set_active(RID p_space,bool p_active);
+	virtual bool space_is_active(RID p_space) const;
+
+	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value);
+	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const;
+
+	// this function only works on fixed process, errors and returns null otherwise
+	virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space);
+
+
+	/* AREA API */
+
+	virtual RID area_create();
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode);
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const;
+
+	virtual void area_set_space(RID p_area, RID p_space);
+	virtual RID area_get_space(RID p_area) const;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform& p_transform=Transform());
+	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape);
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform);
+
+	virtual int area_get_shape_count(RID p_area) const;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
+	virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx);
+	virtual void area_clear_shapes(RID p_area);
+
+	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID);
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const;
+
+	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value);
+	virtual void area_set_transform(RID p_area, const Transform& p_transform);
+
+	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
+	virtual Transform area_get_transform(RID p_area) const;
+
+	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
+
+
+	/* BODY API */
+
+	// create a body of a given type
+	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false);
+
+	virtual void body_set_space(RID p_body, RID p_space);
+	virtual RID body_get_space(RID p_body) const;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode);
+	virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform& p_transform=Transform());
+	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape);
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform);
+
+	virtual int body_get_shape_count(RID p_body) const;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
+	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const;
+
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable);
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx);
+	virtual void body_clear_shapes(RID p_body);
+
+	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID);
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable);
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const;
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
+
+	//advanced simulation
+	virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform);
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant);
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
+
+	virtual void body_set_applied_force(RID p_body, const Vector3& p_force);
+	virtual Vector3 body_get_applied_force(RID p_body) const;
+
+	virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque);
+	virtual Vector3 body_get_applied_torque(RID p_body) const;
+
+	virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse);
+	virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity);
+
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
+
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold);
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const;
+
+	virtual void body_set_omit_force_integration(RID p_body,bool p_omit);
+	virtual bool body_is_omitting_force_integration(RID p_body) const;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
+	virtual int body_get_max_contacts_reported(RID p_body) const;
+
+	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
+
+	/* JOINT API */
+#if 0
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
+	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const;
+
+	virtual RID pin_joint_create(const Vector3& p_pos,RID p_body_a,RID p_body_b=RID());
+	virtual RID groove_joint_create(const Vector3& p_a_groove1,const Vector3& p_a_groove2, const Vector3& p_b_anchor, RID p_body_a,RID p_body_b);
+	virtual RID damped_spring_joint_create(const Vector3& p_anchor_a,const Vector3& p_anchor_b,RID p_body_a,RID p_body_b=RID());
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value);
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const;
+
+	virtual JointType joint_get_type(RID p_joint) const;
+#endif
+	/* MISC */
+
+	virtual void free(RID p_rid);
+
+	virtual void set_active(bool p_active);
+	virtual void init();
+	virtual void step(float p_step);
+	virtual void sync();
+	virtual void flush_queries();
+	virtual void finish();
+
+	PhysicsServerSW();
+	~PhysicsServerSW();
+
+};
+
+#endif
+

servers/physics/shape_sw.h

@@ -0,0 +1,430 @@
+/*************************************************************************/
+/*  shape_sw.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef SHAPE_SW_H
+#define SHAPE_SW_H
+
+#include "servers/physics_server.h"
+#include "bsp_tree.h"
+#include "geometry.h"
+/*
+
+SHAPE_LINE, ///< plane:"plane"
+SHAPE_SEGMENT, ///< float:"length"
+SHAPE_CIRCLE, ///< float:"radius"
+SHAPE_RECTANGLE, ///< vec3:"extents"
+SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
+SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+
+*/
+
+class ShapeSW;
+
+class ShapeOwnerSW {
+public:
+
+	virtual void _shape_changed()=0;
+	virtual void remove_shape(ShapeSW *p_shape)=0;
+
+	virtual ~ShapeOwnerSW() {}
+};
+
+
+class ShapeSW {
+
+	RID self;
+	AABB aabb;
+	bool configured;
+	real_t custom_bias;
+
+	Map<ShapeOwnerSW*,int> owners;
+protected:
+
+	void configure(const AABB& p_aabb);
+public:
+
+	enum {
+		MAX_SUPPORTS=8
+	};
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const {return  self; }
+
+	virtual PhysicsServer::ShapeType get_type() const=0;
+
+	_FORCE_INLINE_ AABB get_aabb() const { return aabb; }
+	_FORCE_INLINE_ bool is_configured() const { return configured; }
+
+	virtual bool is_concave() const { return false; }
+
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const=0;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const=0;
+
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0;
+	virtual Vector3 get_moment_of_inertia(float p_mass) const=0;
+
+	virtual void set_data(const Variant& p_data)=0;
+	virtual Variant get_data() const=0;
+
+	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
+	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
+
+	void add_owner(ShapeOwnerSW *p_owner);
+	void remove_owner(ShapeOwnerSW *p_owner);
+	bool is_owner(ShapeOwnerSW *p_owner) const;
+	const Map<ShapeOwnerSW*,int>& get_owners() const;
+
+	ShapeSW();
+	virtual ~ShapeSW();
+};
+
+
+class ConcaveShapeSW : public ShapeSW {
+
+public:
+
+	virtual bool is_concave() const { return true; }
+	typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex);
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
+
+	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
+
+	ConcaveShapeSW() {}
+};
+
+class PlaneShapeSW : public ShapeSW {
+
+	Plane plane;
+
+	void _setup(const Plane& p_plane);
+public:
+
+	Plane get_plane() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; }
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
+
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	PlaneShapeSW();
+};
+
+class RayShapeSW : public ShapeSW {
+
+	float length;
+
+	void _setup(float p_length);
+public:
+
+	float get_length() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	RayShapeSW();
+};
+
+class SphereShapeSW : public ShapeSW {
+
+	real_t radius;
+
+	void _setup(real_t p_radius);
+public:
+
+	real_t get_radius() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }
+
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	SphereShapeSW();
+};
+
+class BoxShapeSW : public ShapeSW {
+
+	Vector3 half_extents;
+	void _setup(const Vector3& p_half_extents);
+public:
+
+	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
+
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	BoxShapeSW();
+};
+
+class CapsuleShapeSW : public ShapeSW {
+
+	real_t height;
+	real_t radius;
+
+
+	void _setup(real_t p_height,real_t p_radius);
+public:
+
+	_FORCE_INLINE_ real_t get_height() const { return height; }
+	_FORCE_INLINE_ real_t get_radius() const { return radius; }
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }
+
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	CapsuleShapeSW();
+};
+
+struct ConvexPolygonShapeSW : public ShapeSW {
+
+	Geometry::MeshData mesh;
+
+	void _setup(const Vector<Vector3>& p_vertices);
+public:
+
+	const Geometry::MeshData& get_mesh() const { return mesh; }
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
+
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	ConvexPolygonShapeSW();
+
+};
+
+
+struct _VolumeSW_BVH;
+struct FaceShapeSW;
+
+struct ConcavePolygonShapeSW : public ConcaveShapeSW {
+	// always a trimesh
+
+	struct Face {
+
+		Vector3 normal;
+		int indices[3];
+	};
+
+	DVector<Face> faces;
+	DVector<Vector3> vertices;
+
+	struct BVH {
+
+		AABB aabb;
+		int left;
+		int right;
+
+		int face_index;
+	};
+
+	DVector<BVH> bvh;
+
+	struct _CullParams {
+
+		AABB aabb;
+		Callback callback;
+		void *userdata;
+		const Face *faces;
+		const Vector3 *vertices;
+		const BVH *bvh;
+		FaceShapeSW *face;
+	};
+
+	struct _SegmentCullParams {
+
+		Vector3 from;
+		Vector3 to;
+		const Face *faces;
+		const Vector3 *vertices;
+		const BVH *bvh;
+
+		Vector3 result;
+		Vector3 normal;
+		real_t min_d;
+		int collisions;
+
+	};
+
+	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
+	void _cull(int p_idx,_CullParams *p_params) const;
+
+	void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx);
+
+
+	void _setup(DVector<Vector3> p_faces);
+public:
+
+	DVector<Vector3> get_faces() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
+
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	ConcavePolygonShapeSW();
+
+};
+
+
+struct HeightMapShapeSW : public ConcaveShapeSW {
+
+	DVector<real_t> heights;
+	int width;
+	int depth;
+	float cell_size;
+
+//	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
+//	void _cull(int p_idx,_CullParams *p_params) const;
+
+	void _setup(DVector<float> p_heights,int p_width,int p_depth,float p_cell_size);
+public:
+
+	DVector<real_t> get_heights() const;
+	int get_width() const;
+	int get_depth() const;
+	float get_cell_size() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; }
+
+	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	HeightMapShapeSW();
+
+};
+
+//used internally
+struct FaceShapeSW : public ShapeSW {
+
+	Vector3 normal; //cache
+	Vector3 vertex[3];
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
+
+	const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; }
+
+	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	Vector3 get_support(const Vector3& p_normal) const;
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+	bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+	Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data) {}
+	virtual Variant get_data() const { return Variant(); }
+
+	FaceShapeSW();
+};
+
+
+
+
+
+struct _ShapeTestConvexBSPSW {
+
+	const BSP_Tree *bsp;
+	const ShapeSW *shape;
+	Transform transform;
+
+	_FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const {
+
+		shape->project_range(p_normal,transform,r_min,r_max);
+	}
+
+};
+
+
+
+
+#endif // SHAPESW_H

servers/physics/space_sw.cpp

@@ -0,0 +1,429 @@
+/*************************************************************************/
+/*  space_sw.cpp                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "space_sw.h"
+#include "collision_solver_sw.h"
+#include "physics_server_sw.h"
+
+
+bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+
+	ERR_FAIL_COND_V(space->locked,false);
+
+	Vector3 begin,end;
+	Vector3 normal;
+	begin=p_from;
+	end=p_to;
+	normal=(end-begin).normalized();
+
+
+	int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+
+	//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
+
+	bool collided=false;
+	Vector3 res_point,res_normal;
+	int res_shape;
+	const CollisionObjectSW *res_obj;
+	real_t min_d=1e10;
+
+
+	for(int i=0;i<amount;i++) {
+
+		if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)
+			continue; //ignore area
+
+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+			continue;
+
+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+
+		int shape_idx=space->intersection_query_subindex_results[i];
+		Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
+
+		Vector3 local_from = inv_xform.xform(begin);
+		Vector3 local_to = inv_xform.xform(end);
+
+		const ShapeSW *shape = col_obj->get_shape(shape_idx);
+
+		Vector3 shape_point,shape_normal;
+
+		if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
+
+			Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+			shape_point=xform.xform(shape_point);
+
+			real_t ld = normal.dot(shape_point);
+
+
+			if (ld<min_d) {
+
+				min_d=ld;
+				res_point=shape_point;
+				res_normal=inv_xform.basis.xform_inv(shape_normal).normalized();
+				res_shape=shape_idx;
+				res_obj=col_obj;
+				collided=true;
+			}
+		}
+
+	}
+
+	if (!collided)
+		return false;
+
+
+	r_result.collider_id=res_obj->get_instance_id();
+	if (r_result.collider_id!=0)
+		r_result.collider=ObjectDB::get_instance(r_result.collider_id);
+	r_result.normal=res_normal;
+	r_result.position=res_point;
+	r_result.rid=res_obj->get_self();
+	r_result.shape=res_shape;
+
+	return true;
+
+}
+
+
+int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+	if (p_result_max<=0)
+		return 0;
+
+	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,0);
+
+	AABB aabb = p_xform.xform(shape->get_aabb());
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	bool collided=false;
+	int cc=0;
+
+	//Transform ai = p_xform.affine_inverse();
+
+	for(int i=0;i<amount;i++) {
+
+		if (cc>=p_result_max)
+			break;
+
+		if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)
+			continue; //ignore area
+
+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+			continue;
+
+
+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL))
+			continue;
+
+		r_results[cc].collider_id=col_obj->get_instance_id();
+		if (r_results[cc].collider_id!=0)
+			r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
+		r_results[cc].rid=col_obj->get_self();
+		r_results[cc].shape=shape_idx;
+
+		cc++;
+
+	}
+
+	return cc;
+
+}
+
+PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
+
+
+	space=NULL;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+
+void* SpaceSW::_broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self) {
+
+	CollisionObjectSW::Type type_A=A->get_type();
+	CollisionObjectSW::Type type_B=B->get_type();
+	if (type_A>type_B) {
+
+		SWAP(A,B);
+		SWAP(p_subindex_A,p_subindex_B);
+		SWAP(type_A,type_B);
+	}
+
+	SpaceSW *self = (SpaceSW*)p_self;
+
+	if (type_A==CollisionObjectSW::TYPE_AREA) {
+
+
+		ERR_FAIL_COND_V(type_B!=CollisionObjectSW::TYPE_BODY,NULL);
+		AreaSW *area=static_cast<AreaSW*>(A);
+		BodySW *body=static_cast<BodySW*>(B);
+
+
+		AreaPairSW *area_pair = memnew(AreaPairSW(body,p_subindex_B,area,p_subindex_A) );
+
+		return area_pair;
+	} else {
+
+
+		BodyPairSW *b = memnew( BodyPairSW((BodySW*)A,p_subindex_A,(BodySW*)B,p_subindex_B) );
+		return b;
+
+	}
+
+	return NULL;
+}
+
+void SpaceSW::_broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self) {
+
+
+
+	SpaceSW *self = (SpaceSW*)p_self;
+	ConstraintSW *c = (ConstraintSW*)p_data;
+	memdelete(c);
+}
+
+
+const SelfList<BodySW>::List& SpaceSW::get_active_body_list() const {
+
+	return active_list;
+}
+void SpaceSW::body_add_to_active_list(SelfList<BodySW>* p_body) {
+
+	active_list.add(p_body);
+}
+void SpaceSW::body_remove_from_active_list(SelfList<BodySW>* p_body) {
+
+	active_list.remove(p_body);
+
+}
+
+void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW>* p_body) {
+
+
+	inertia_update_list.add(p_body);
+}
+
+void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW>* p_body) {
+
+	inertia_update_list.remove(p_body);
+}
+
+BroadPhaseSW *SpaceSW::get_broadphase() {
+
+	return broadphase;
+}
+
+void SpaceSW::add_object(CollisionObjectSW *p_object) {
+
+	ERR_FAIL_COND( objects.has(p_object) );
+	objects.insert(p_object);
+}
+
+void SpaceSW::remove_object(CollisionObjectSW *p_object) {
+
+	ERR_FAIL_COND( !objects.has(p_object) );
+	objects.erase(p_object);
+}
+
+const Set<CollisionObjectSW*> &SpaceSW::get_objects() const {
+
+	return objects;
+}
+
+void SpaceSW::body_add_to_state_query_list(SelfList<BodySW>* p_body) {
+
+	state_query_list.add(p_body);
+}
+void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW>* p_body) {
+
+	state_query_list.remove(p_body);
+}
+
+void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW>* p_area) {
+
+	monitor_query_list.add(p_area);
+}
+void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area) {
+
+	monitor_query_list.remove(p_area);
+}
+
+void SpaceSW::area_add_to_moved_list(SelfList<AreaSW>* p_area) {
+
+	area_moved_list.add(p_area);
+}
+
+void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW>* p_area) {
+
+	area_moved_list.remove(p_area);
+}
+
+const SelfList<AreaSW>::List& SpaceSW::get_moved_area_list() const {
+
+	return area_moved_list;
+}
+
+
+
+
+void SpaceSW::call_queries() {
+
+	while(state_query_list.first()) {
+
+		BodySW * b = state_query_list.first()->self();
+		b->call_queries();
+		state_query_list.remove(state_query_list.first());
+	}
+
+	while(monitor_query_list.first()) {
+
+		AreaSW * a = monitor_query_list.first()->self();
+		a->call_queries();
+		monitor_query_list.remove(monitor_query_list.first());
+	}
+
+}
+
+void SpaceSW::setup() {
+
+
+	while(inertia_update_list.first()) {
+		inertia_update_list.first()->self()->update_inertias();
+		inertia_update_list.remove(inertia_update_list.first());
+	}
+
+
+}
+
+void SpaceSW::update() {
+
+	broadphase->update();
+
+}
+
+
+void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {
+
+	switch(p_param) {
+
+		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;
+		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break;
+		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;
+	}
+}
+
+real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const {
+
+	switch(p_param) {
+
+		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;
+		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;
+		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration;
+		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold;
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold;
+		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;
+		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
+	}
+	return 0;
+}
+
+void SpaceSW::lock() {
+
+	locked=true;
+}
+
+void SpaceSW::unlock() {
+
+	locked=false;
+}
+
+bool SpaceSW::is_locked() const {
+
+	return locked;
+}
+
+PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() {
+
+	return direct_access;
+}
+
+SpaceSW::SpaceSW() {
+
+
+	locked=false;
+	contact_recycle_radius=0.01;
+	contact_max_separation=0.05;
+	contact_max_allowed_penetration= 0.01;
+
+	constraint_bias = 0.01;
+	body_linear_velocity_sleep_treshold=0.01;
+	body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI);
+	body_time_to_sleep=0.5;
+	body_angular_velocity_damp_ratio=10;
+
+
+	broadphase = BroadPhaseSW::create_func();
+	broadphase->set_pair_callback(_broadphase_pair,this);
+	broadphase->set_unpair_callback(_broadphase_unpair,this);
+	area=NULL;
+
+	direct_access = memnew( PhysicsDirectSpaceStateSW );
+	direct_access->space=this;
+}
+
+SpaceSW::~SpaceSW() {
+
+	memdelete(broadphase);
+	memdelete( direct_access );
+}
+
+
+

servers/physics/space_sw.h

@@ -0,0 +1,157 @@
+/*************************************************************************/
+/*  space_sw.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef SPACE_SW_H
+#define SPACE_SW_H
+
+#include "typedefs.h"
+#include "hash_map.h"
+#include "body_sw.h"
+#include "area_sw.h"
+#include "body_pair_sw.h"
+#include "area_pair_sw.h"
+#include "broad_phase_sw.h"
+#include "collision_object_sw.h"
+
+
+class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState {
+
+	OBJ_TYPE( PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState );
+public:
+
+	SpaceSW *space;
+
+	bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+	int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+
+	PhysicsDirectSpaceStateSW();
+};
+
+
+
+class SpaceSW {
+
+
+	PhysicsDirectSpaceStateSW *direct_access;
+	RID self;
+
+	BroadPhaseSW *broadphase;
+	SelfList<BodySW>::List active_list;
+	SelfList<BodySW>::List inertia_update_list;
+	SelfList<BodySW>::List state_query_list;
+	SelfList<AreaSW>::List monitor_query_list;
+	SelfList<AreaSW>::List area_moved_list;
+
+	static void* _broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self);
+	static void _broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self);
+
+	Set<CollisionObjectSW*> objects;
+
+	AreaSW *area;
+
+	real_t contact_recycle_radius;
+	real_t contact_max_separation;
+	real_t contact_max_allowed_penetration;
+	real_t constraint_bias;
+
+	enum {
+
+		INTERSECTION_QUERY_MAX=2048
+	};
+
+	CollisionObjectSW *intersection_query_results[INTERSECTION_QUERY_MAX];
+	int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
+
+	float body_linear_velocity_sleep_treshold;
+	float body_angular_velocity_sleep_treshold;
+	float body_time_to_sleep;
+	float body_angular_velocity_damp_ratio;
+
+	bool locked;
+
+friend class PhysicsDirectSpaceStateSW;
+
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	void set_default_area(AreaSW *p_area) { area=p_area; }
+	AreaSW *get_default_area() const { return area; }
+
+	const SelfList<BodySW>::List& get_active_body_list() const;
+	void body_add_to_active_list(SelfList<BodySW>* p_body);
+	void body_remove_from_active_list(SelfList<BodySW>* p_body);
+	void body_add_to_inertia_update_list(SelfList<BodySW>* p_body);
+	void body_remove_from_inertia_update_list(SelfList<BodySW>* p_body);
+
+	void body_add_to_state_query_list(SelfList<BodySW>* p_body);
+	void body_remove_from_state_query_list(SelfList<BodySW>* p_body);
+
+	void area_add_to_monitor_query_list(SelfList<AreaSW>* p_area);
+	void area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area);
+	void area_add_to_moved_list(SelfList<AreaSW>* p_area);
+	void area_remove_from_moved_list(SelfList<AreaSW>* p_area);
+	const SelfList<AreaSW>::List& get_moved_area_list() const;
+
+	BroadPhaseSW *get_broadphase();
+
+	void add_object(CollisionObjectSW *p_object);
+	void remove_object(CollisionObjectSW *p_object);
+	const Set<CollisionObjectSW*> &get_objects() const;
+
+	_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
+	_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
+	_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
+	_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
+	_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; }
+	_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }
+	_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
+	_FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }
+
+
+	void update();
+	void setup();
+	void call_queries();
+
+
+	bool is_locked() const;
+	void lock();
+	void unlock();
+
+	void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);
+	real_t get_param(PhysicsServer::SpaceParameter p_param) const;
+
+	PhysicsDirectSpaceStateSW *get_direct_state();
+
+	SpaceSW();
+	~SpaceSW();
+};
+
+
+#endif // SPACE__SW_H

servers/physics/step_sw.cpp

@@ -0,0 +1,237 @@
+/*************************************************************************/
+/*  step_sw.cpp                                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "step_sw.h"
+
+
+void StepSW::_populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island) {
+
+	p_body->set_island_step(_step);
+	p_body->set_island_next(*p_island);
+	*p_island=p_body;
+
+	for(Map<ConstraintSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) {
+
+		ConstraintSW *c=(ConstraintSW*)E->key();
+		if (c->get_island_step()==_step)
+			continue; //already processed
+		c->set_island_step(_step);
+		c->set_island_next(*p_constraint_island);
+		*p_constraint_island=c;
+
+
+		for(int i=0;i<c->get_body_count();i++) {
+			if (i==E->get())
+				continue;
+			BodySW *b = c->get_body_ptr()[i];
+			if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+				continue; //no go
+			_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
+		}
+	}
+}
+
+void StepSW::_setup_island(ConstraintSW *p_island,float p_delta) {
+
+	ConstraintSW *ci=p_island;
+	while(ci) {
+		bool process = ci->setup(p_delta);
+		//todo remove from island if process fails
+		ci=ci->get_island_next();
+	}
+}
+
+void StepSW::_solve_island(ConstraintSW *p_island,int p_iterations,float p_delta){
+
+	for(int i=0;i<p_iterations;i++) {
+
+		ConstraintSW *ci=p_island;
+		while(ci) {
+			ci->solve(p_delta);
+			ci=ci->get_island_next();
+		}
+	}
+}
+
+void StepSW::_check_suspend(BodySW *p_island,float p_delta) {
+
+
+	bool can_sleep=true;
+
+	BodySW *b = p_island;
+	while(b) {
+
+		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+			continue; //ignore for static
+
+		if (!b->sleep_test(p_delta))
+			can_sleep=false;
+
+		b=b->get_island_next();
+	}
+
+	//put all to sleep or wake up everyoen
+
+	b = p_island;
+	while(b) {
+
+		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+			continue; //ignore for static
+
+		bool active = b->is_active();
+
+		if (active==can_sleep)
+			b->set_active(!can_sleep);
+
+		b=b->get_island_next();
+	}
+}
+
+void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) {
+
+	p_space->lock(); // can't access space during this
+
+	p_space->setup(); //update inertias, etc
+
+	const SelfList<BodySW>::List * body_list = &p_space->get_active_body_list();
+
+	/* INTEGRATE FORCES */
+	int active_count=0;
+
+	const SelfList<BodySW>*b = body_list->first();
+	while(b) {
+
+		b->self()->integrate_forces(p_delta);
+		b=b->next();
+		active_count++;
+	}
+
+
+
+	/* GENERATE CONSTRAINT ISLANDS */
+
+	BodySW *island_list=NULL;
+	ConstraintSW *constraint_island_list=NULL;
+	b = body_list->first();
+
+	int island_count=0;
+
+	while(b) {
+		BodySW *body = b->self();
+
+
+		if (body->get_island_step()!=_step) {
+
+			BodySW *island=NULL;
+			ConstraintSW *constraint_island=NULL;
+			_populate_island(body,&island,&constraint_island);
+
+			island->set_island_list_next(island_list);
+			island_list=island;
+
+			if (constraint_island) {
+				constraint_island->set_island_list_next(constraint_island_list);
+				constraint_island_list=constraint_island;
+				island_count++;
+			}
+
+		}
+		b=b->next();
+	}
+
+	const SelfList<AreaSW>::List &aml = p_space->get_moved_area_list();
+
+	while(aml.first()) {
+		for(const Set<ConstraintSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) {
+
+			ConstraintSW*c=E->get();
+			if (c->get_island_step()==_step)
+				continue;
+			c->set_island_step(_step);
+			c->set_island_next(NULL);
+			c->set_island_list_next(constraint_island_list);
+			constraint_island_list=c;
+		}
+		p_space->area_remove_from_moved_list((SelfList<AreaSW>*)aml.first()); //faster to remove here
+	}
+
+//	print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
+	/* SETUP CONSTRAINT ISLANDS */
+
+	{
+		ConstraintSW *ci=constraint_island_list;
+		while(ci) {
+
+			_setup_island(ci,p_delta);
+			ci=ci->get_island_list_next();
+		}
+	}
+
+	/* SOLVE CONSTRAINT ISLANDS */
+
+	{
+		ConstraintSW *ci=constraint_island_list;
+		while(ci) {
+			//iterating each island separatedly improves cache efficiency
+			_solve_island(ci,p_iterations,p_delta);
+			ci=ci->get_island_list_next();
+		}
+	}
+
+	/* INTEGRATE VELOCITIES */
+
+	b = body_list->first();
+	while(b) {
+
+		b->self()->integrate_velocities(p_delta);
+		b=b->next();
+	}
+
+	/* SLEEP / WAKE UP ISLANDS */
+
+	{
+		BodySW *bi=island_list;
+		while(bi) {
+
+			_check_suspend(bi,p_delta);
+			bi=bi->get_island_list_next();
+		}
+	}
+
+	p_space->update();
+	p_space->unlock();
+	_step++;
+
+
+
+}
+
+StepSW::StepSW() {
+
+	_step=1;
+}

servers/physics/step_sw.h

@@ -0,0 +1,48 @@
+/*************************************************************************/
+/*  step_sw.h                                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef STEP_SW_H
+#define STEP_SW_H
+
+#include "space_sw.h"
+
+class StepSW {
+
+	uint64_t _step;
+
+	void _populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island);
+	void _setup_island(ConstraintSW *p_island,float p_delta);
+	void _solve_island(ConstraintSW *p_island,int p_iterations,float p_delta);
+	void _check_suspend(BodySW *p_island,float p_delta);
+public:
+
+	void step(SpaceSW* p_space,float p_delta,int p_iterations);
+	StepSW();
+};
+
+#endif // STEP__SW_H

servers/physics_2d/SCsub

@@ -0,0 +1,4 @@
+Import('env')
+
+env.add_source_files(env.servers_sources,"*.cpp")
+

servers/physics_2d/area_2d_sw.cpp

@@ -0,0 +1,193 @@
+/*************************************************************************/
+/*  area_2d_sw.cpp                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "area_2d_sw.h"
+#include "space_2d_sw.h"
+#include "body_2d_sw.h"
+
+Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
+
+void Area2DSW::_shapes_changed() {
+
+
+}
+
+void Area2DSW::set_transform(const Matrix32& p_transform) {
+
+	if (!moved_list.in_list() && get_space())
+		get_space()->area_add_to_moved_list(&moved_list);
+
+	_set_transform(p_transform);
+}
+
+void Area2DSW::set_space(Space2DSW *p_space) {
+
+	if (get_space()) {
+		if (monitor_query_list.in_list())
+			get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+		if (moved_list.in_list())
+			get_space()->area_remove_from_moved_list(&moved_list);
+
+	}
+
+	monitored_bodies.clear();
+
+	_set_space(p_space);
+}
+
+
+void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
+
+
+	if (p_id==monitor_callback_id) {
+		monitor_callback_method=p_method;
+		return;
+	}
+
+	_unregister_shapes();
+
+	monitor_callback_id=p_id;
+	monitor_callback_method=p_method;
+
+	monitored_bodies.clear();
+
+	_shape_changed();
+
+}
+
+
+
+void Area2DSW::set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode) {
+	bool do_override=p_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override==space_override_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+		return;
+	_unregister_shapes();
+	space_override_mode=p_mode;
+	_shape_changed();
+}
+
+void Area2DSW::set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value) {
+
+	switch(p_param) {
+		case Physics2DServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:  point_attenuation=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_DENSITY: density=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_PRIORITY: priority=p_value; ; break;
+	}
+
+
+}
+
+Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const {
+
+
+	switch(p_param) {
+		case Physics2DServer::AREA_PARAM_GRAVITY: return gravity;
+		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector;
+		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point;
+		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return  point_attenuation;
+		case Physics2DServer::AREA_PARAM_DENSITY: return density;
+		case Physics2DServer::AREA_PARAM_PRIORITY: return priority;
+	}
+
+	return Variant();
+}
+
+
+void Area2DSW::_queue_monitor_update() {
+
+	ERR_FAIL_COND(!get_space());
+
+	if (!monitor_query_list.in_list())
+		get_space()->area_add_to_monitor_query_list(&monitor_query_list);
+
+
+}
+
+void Area2DSW::call_queries() {
+
+	if (monitor_callback_id && !monitored_bodies.empty()) {
+
+		Variant res[5];
+		Variant *resptr[5];
+		for(int i=0;i<5;i++)
+			resptr[i]=&res[i];
+
+		Object *obj = ObjectDB::get_instance(monitor_callback_id);
+		if (!obj) {
+			monitored_bodies.clear();
+			monitor_callback_id=0;
+			return;
+		}
+
+
+
+		for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) {
+
+			if (E->get().state==0)
+				continue; //nothing happened
+
+			res[0]=E->get().state>0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
+			res[1]=E->key().rid;
+			res[2]=E->key().instance_id;
+			res[3]=E->key().body_shape;
+			res[4]=E->key().area_shape;
+
+			Variant::CallError ce;
+			obj->call(monitor_callback_method,(const Variant**)resptr,5,ce);
+		}
+	}
+
+	monitored_bodies.clear();
+
+	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
+
+}
+
+Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this)  {
+
+	_set_static(true); //areas are never active
+	space_override_mode=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
+	gravity=9.80665;
+	gravity_vector=Vector2(0,-1);
+	gravity_is_point=false;
+	point_attenuation=1;
+	density=0.1;
+	priority=0;
+	monitor_callback_id=0;
+
+
+}
+
+Area2DSW::~Area2DSW() {
+
+
+}
+

servers/physics_2d/area_2d_sw.h

@@ -0,0 +1,172 @@
+/*************************************************************************/
+/*  area_2d_sw.h                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AREA_2D_SW_H
+#define AREA_2D_SW_H
+
+#include "servers/physics_2d_server.h"
+#include "collision_object_2d_sw.h"
+#include "self_list.h"
+//#include "servers/physics/query_sw.h"
+
+class Space2DSW;
+class Body2DSW;
+class Constraint2DSW;
+
+
+class Area2DSW : public CollisionObject2DSW{
+
+
+	Physics2DServer::AreaSpaceOverrideMode space_override_mode;
+	float gravity;
+	Vector2 gravity_vector;
+	bool gravity_is_point;
+	float point_attenuation;
+	float density;
+	int priority;
+
+	ObjectID monitor_callback_id;
+	StringName monitor_callback_method;
+
+	SelfList<Area2DSW> monitor_query_list;
+	SelfList<Area2DSW> moved_list;
+
+	struct BodyKey {
+
+		RID rid;
+		ObjectID instance_id;
+		uint32_t body_shape;
+		uint32_t area_shape;
+
+		_FORCE_INLINE_ bool operator<( const BodyKey& p_key) const {
+
+			if (rid==p_key.rid) {
+
+				if (body_shape==p_key.body_shape) {
+
+					return area_shape < p_key.area_shape;
+				} else
+					return body_shape < p_key.area_shape;
+			} else
+				return rid < p_key.rid;
+
+		}
+
+		_FORCE_INLINE_ BodyKey() {}
+		BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	};
+
+	struct BodyState {
+
+		int state;
+		_FORCE_INLINE_ void inc() { state++; }
+		_FORCE_INLINE_ void dec() { state--; }
+		_FORCE_INLINE_ BodyState() { state=0; }
+	};
+
+	Map<BodyKey,BodyState> monitored_bodies;
+
+	//virtual void shape_changed_notify(Shape2DSW *p_shape);
+	//virtual void shape_deleted_notify(Shape2DSW *p_shape);
+	Set<Constraint2DSW*> constraints;
+
+
+	virtual void _shapes_changed();
+	void _queue_monitor_update();
+
+public:
+
+	//_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; }
+	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
+
+	void set_monitor_callback(ObjectID p_id, const StringName& p_method);
+	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; }
+
+	_FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+
+	void set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value);
+	Variant get_param(Physics2DServer::AreaParameter p_param) const;
+
+	void set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode);
+	Physics2DServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; }
+
+	_FORCE_INLINE_ void set_gravity(float p_gravity) { gravity=p_gravity; }
+	_FORCE_INLINE_ float get_gravity() const { return gravity; }
+
+	_FORCE_INLINE_ void set_gravity_vector(const Vector2& p_gravity) { gravity_vector=p_gravity; }
+	_FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; }
+
+	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; }
+	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
+
+	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; }
+	_FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; }
+
+	_FORCE_INLINE_ void set_density(float p_density) { density=p_density; }
+	_FORCE_INLINE_ float get_density() const { return density; }
+
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; }
+	_FORCE_INLINE_ int get_priority() const { return priority; }
+
+	_FORCE_INLINE_ void add_constraint( Constraint2DSW* p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint( Constraint2DSW* p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const Set<Constraint2DSW*>& get_constraints() const { return constraints; }
+
+	void set_transform(const Matrix32& p_transform);
+
+	void set_space(Space2DSW *p_space);
+
+
+	void call_queries();
+
+	Area2DSW();
+	~Area2DSW();
+};
+
+void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+
+	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	monitored_bodies[bk].inc();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
+void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+
+	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	monitored_bodies[bk].dec();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
+
+
+
+
+
+
+#endif // AREA_2D_SW_H

servers/physics_2d/area_pair_2d_sw.cpp

@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  area_pair_2d_sw.cpp                                                  */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "area_pair_2d_sw.h"
+#include "collision_solver_2d_sw.h"
+
+
+bool AreaPair2DSW::setup(float p_step) {
+
+	bool result = CollisionSolver2DSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),body->get_shape_inv_transform(body_shape) * body->get_inv_transform(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),area->get_shape_inv_transform(area_shape) * area->get_inv_transform(),NULL,this);
+
+	if (result!=colliding) {
+
+		if (result) {
+
+			if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+				body->add_area(area);
+			if (area->has_monitor_callback())
+				area->add_body_to_query(body,body_shape,area_shape);
+
+		} else {
+
+			if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+				body->remove_area(area);
+			if (area->has_monitor_callback())
+				area->remove_body_from_query(body,body_shape,area_shape);
+
+		}
+
+		colliding=result;
+
+	}
+
+	return false; //never do any post solving
+}
+
+void AreaPair2DSW::solve(float p_step) {
+
+
+}
+
+
+AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape) {
+
+
+	body=p_body;
+	area=p_area;
+	body_shape=p_body_shape;
+	area_shape=p_area_shape;
+	colliding=false;
+	body->add_constraint(this,0);
+	area->add_constraint(this);
+
+}
+
+AreaPair2DSW::~AreaPair2DSW() {
+
+	if (colliding) {
+
+		if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+			body->remove_area(area);
+		if (area->has_monitor_callback())
+			area->remove_body_from_query(body,body_shape,area_shape);
+
+
+	}
+	body->remove_constraint(this);
+	area->remove_constraint(this);
+}

servers/physics_2d/area_pair_2d_sw.h

@@ -0,0 +1,53 @@
+/*************************************************************************/
+/*  area_pair_2d_sw.h                                                    */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef AREA_PAIR_2D_SW_H
+#define AREA_PAIR_2D_SW_H
+
+#include "constraint_2d_sw.h"
+#include "body_2d_sw.h"
+#include "area_2d_sw.h"
+
+class AreaPair2DSW : public Constraint2DSW {
+
+	Body2DSW *body;
+	Area2DSW *area;
+	int body_shape;
+	int area_shape;
+	bool colliding;
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape);
+	~AreaPair2DSW();
+};
+
+#endif // AREA_PAIR_2D_SW_H
+

servers/physics_2d/body_2d_sw.cpp

@@ -0,0 +1,609 @@
+/*************************************************************************/
+/*  body_2d_sw.cpp                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "body_2d_sw.h"
+#include "space_2d_sw.h"
+#include "area_2d_sw.h"
+
+void Body2DSW::_update_inertia() {
+
+	if (get_space() && !inertia_update_list.in_list())
+		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+
+}
+
+
+
+void Body2DSW::update_inertias() {
+
+	//update shapes and motions
+
+	switch(mode) {
+
+		case Physics2DServer::BODY_MODE_RIGID: {
+
+			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
+			float total_area=0;
+
+			for (int i=0;i<get_shape_count();i++) {
+
+				total_area+=get_shape_aabb(i).get_area();
+			}
+
+			real_t _inertia=0;
+
+			for (int i=0;i<get_shape_count();i++) {
+
+				const Shape2DSW* shape=get_shape(i);
+
+				float area=get_shape_aabb(i).get_area();
+
+				float mass = area * this->mass / total_area;
+
+				_inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin().length_squared();
+
+			}
+
+			if (_inertia!=0)
+				_inv_inertia=1.0/_inertia;
+			else
+				_inv_inertia=0.0; //wathever
+
+			if (mass)
+				_inv_mass=1.0/mass;
+			else
+				_inv_mass=0;
+
+		} break;
+		case Physics2DServer::BODY_MODE_STATIC_ACTIVE:
+		case Physics2DServer::BODY_MODE_STATIC: {
+
+			_inv_inertia=0;
+			_inv_mass=0;
+		} break;
+		case Physics2DServer::BODY_MODE_CHARACTER: {
+
+			_inv_inertia=0;
+			_inv_mass=1.0/mass;
+
+		} break;
+	}
+	//_update_inertia_tensor();
+
+	//_update_shapes();
+
+}
+
+
+
+void Body2DSW::set_active(bool p_active) {
+
+	if (active==p_active)
+		return;
+
+	active=p_active;
+	if (!p_active) {
+		if (get_space())
+			get_space()->body_remove_from_active_list(&active_list);
+	} else {
+		if (mode==Physics2DServer::BODY_MODE_STATIC)
+			return; //static bodies can't become active
+		if (get_space())
+			get_space()->body_add_to_active_list(&active_list);
+
+		//still_time=0;
+	}
+/*
+	if (!space)
+		return;
+
+	for(int i=0;i<get_shape_count();i++) {
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			get_space()->get_broadphase()->set_active(s.bpid,active);
+		}
+	}
+*/
+}
+
+
+
+void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value) {
+
+	switch(p_param) {
+		case Physics2DServer::BODY_PARAM_BOUNCE: {
+
+			bounce=p_value;
+		} break;
+		case Physics2DServer::BODY_PARAM_FRICTION: {
+
+			friction=p_value;
+		} break;
+		case Physics2DServer::BODY_PARAM_MASS: {
+			ERR_FAIL_COND(p_value<=0);
+			mass=p_value;
+			_update_inertia();
+
+		} break;
+		default:{}
+	}
+}
+
+float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
+
+	switch(p_param) {
+		case Physics2DServer::BODY_PARAM_BOUNCE: {
+
+			return bounce;
+		} break;
+		case Physics2DServer::BODY_PARAM_FRICTION: {
+
+			return friction;
+		} break;
+		case Physics2DServer::BODY_PARAM_MASS: {
+			return mass;
+		} break;
+		default:{}
+	}
+
+	return 0;
+}
+
+void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {
+
+	mode=p_mode;
+
+	switch(p_mode) {
+	//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
+		case Physics2DServer::BODY_MODE_STATIC:
+		case Physics2DServer::BODY_MODE_STATIC_ACTIVE: {
+
+			_set_inv_transform(get_transform().affine_inverse());
+			_inv_mass=0;
+			_set_static(p_mode==Physics2DServer::BODY_MODE_STATIC);
+			set_active(p_mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE);
+			linear_velocity=Vector2();
+			angular_velocity=0;
+		} break;
+		case Physics2DServer::BODY_MODE_RIGID: {
+
+			_inv_mass=mass>0?(1.0/mass):0;
+			_set_static(false);
+			simulated_motion=false; //jic
+
+		} break;
+		case Physics2DServer::BODY_MODE_CHARACTER: {
+
+			_inv_mass=mass>0?(1.0/mass):0;
+			_set_static(false);
+			simulated_motion=false; //jic
+		} break;
+	}
+
+	_update_inertia();
+	//if (get_space())
+//		_update_queries();
+
+}
+Physics2DServer::BodyMode Body2DSW::get_mode() const {
+
+	return mode;
+}
+
+void Body2DSW::_shapes_changed() {
+
+	_update_inertia();
+	wakeup_neighbours();
+}
+
+void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) {
+
+	switch(p_state)	{
+		case Physics2DServer::BODY_STATE_TRANSFORM: {
+
+
+			if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
+				_set_transform(p_variant);
+				_set_inv_transform(get_transform().affine_inverse());
+				wakeup_neighbours();
+			} else {
+				Matrix32 t = p_variant;
+				t.orthonormalize();
+				_set_transform(t);
+				_set_inv_transform(get_transform().inverse());
+
+			}
+
+		} break;
+		case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
+
+			//if (mode==Physics2DServer::BODY_MODE_STATIC)
+			//	break;
+			linear_velocity=p_variant;
+
+		} break;
+		case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
+			//if (mode!=Physics2DServer::BODY_MODE_RIGID)
+			//	break;
+			angular_velocity=p_variant;
+
+		} break;
+		case Physics2DServer::BODY_STATE_SLEEPING: {
+			//?
+			if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
+				break;
+			bool do_sleep=p_variant;
+			if (do_sleep) {
+				linear_velocity=Vector2();
+				//biased_linear_velocity=Vector3();
+				angular_velocity=0;
+				//biased_angular_velocity=Vector3();
+				set_active(false);
+			} else {
+				if (mode!=Physics2DServer::BODY_MODE_STATIC)
+					set_active(true);
+			}
+		} break;
+		case Physics2DServer::BODY_STATE_CAN_SLEEP: {
+			can_sleep=p_variant;
+			if (mode==Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep)
+				set_active(true);
+
+		} break;
+	}
+
+}
+Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const {
+
+	switch(p_state)	{
+		case Physics2DServer::BODY_STATE_TRANSFORM: {
+			return get_transform();
+		} break;
+		case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
+			return linear_velocity;
+		} break;
+		case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
+			return angular_velocity;
+		} break;
+		case Physics2DServer::BODY_STATE_SLEEPING: {
+			return !is_active();
+		} break;
+		case Physics2DServer::BODY_STATE_CAN_SLEEP: {
+			return can_sleep;
+		} break;
+	}
+
+	return Variant();
+}
+
+
+void Body2DSW::set_space(Space2DSW *p_space){
+
+	if (get_space()) {
+
+		wakeup_neighbours();
+
+		if (inertia_update_list.in_list())
+			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
+		if (active_list.in_list())
+			get_space()->body_remove_from_active_list(&active_list);
+		if (direct_state_query_list.in_list())
+			get_space()->body_remove_from_state_query_list(&direct_state_query_list);
+
+	}
+
+	_set_space(p_space);
+
+	if (get_space()) {
+
+		_update_inertia();
+		if (active)
+			get_space()->body_add_to_active_list(&active_list);
+//		_update_queries();
+		//if (is_active()) {
+		//	active=false;
+		//	set_active(true);
+		//}
+
+	}
+
+}
+
+void Body2DSW::_compute_area_gravity(const Area2DSW *p_area) {
+
+	if (p_area->is_gravity_point()) {
+
+		gravity = (p_area->get_transform().get_origin()+p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity();
+
+	} else {
+		gravity = p_area->get_gravity_vector() * p_area->get_gravity();
+	}
+}
+
+void Body2DSW::integrate_forces(real_t p_step) {
+
+	if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
+		return;
+
+	Area2DSW *current_area = get_space()->get_default_area();
+	ERR_FAIL_COND(!current_area);
+
+	int prio = current_area->get_priority();
+	int ac = areas.size();
+	if (ac) {
+		const AreaCMP *aa = &areas[0];
+		for(int i=0;i<ac;i++) {
+			if (aa[i].area->get_priority() > prio) {
+				current_area=aa[i].area;
+				prio=current_area->get_priority();
+			}
+		}
+	}
+
+	_compute_area_gravity(current_area);
+	density=current_area->get_density();
+
+	if (!omit_force_integration) {
+		//overriden by direct state query
+
+		Vector2 force=gravity*mass;
+		force+=applied_force;
+		real_t torque=applied_torque;
+
+		real_t damp = 1.0 - p_step * density;
+
+		if (damp<0) // reached zero in the given time
+			damp=0;
+
+		real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();
+
+		if (angular_damp<0) // reached zero in the given time
+			angular_damp=0;
+
+		linear_velocity*=damp;
+		angular_velocity*=angular_damp;
+
+		linear_velocity+=_inv_mass * force * p_step;
+		angular_velocity+=_inv_inertia * torque * p_step;
+	}
+
+
+	//motion=linear_velocity*p_step;
+
+	biased_angular_velocity=0;
+	biased_linear_velocity=Vector2();
+
+	if (continuous_cd) //shapes temporarily extend for raycast
+		_update_shapes_with_motion(linear_velocity*p_step);
+
+	current_area=NULL; // clear the area, so it is set in the next frame
+	contact_count=0;
+
+}
+
+void Body2DSW::integrate_velocities(real_t p_step) {
+
+	if (mode==Physics2DServer::BODY_MODE_STATIC)
+		return;
+	if (mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
+		if (fi_callback)
+			get_space()->body_add_to_state_query_list(&direct_state_query_list);
+		return;
+	}
+
+	real_t total_angular_velocity = angular_velocity+biased_angular_velocity;
+	Vector2 total_linear_velocity=linear_velocity+biased_linear_velocity;
+
+	real_t angle = get_transform().get_rotation() - total_angular_velocity * p_step;
+	Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
+
+
+	_set_transform(Matrix32(angle,pos));
+	_set_inv_transform(get_transform().inverse());
+
+
+	if (fi_callback) {
+
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	}
+
+	//_update_inertia_tensor();
+}
+
+
+void Body2DSW::simulate_motion(const Matrix32& p_xform,real_t p_step) {
+
+	Matrix32 inv_xform = p_xform.affine_inverse();
+	if (!get_space()) {
+		_set_transform(p_xform);
+		_set_inv_transform(inv_xform);
+
+		return;
+	}
+
+
+
+	linear_velocity = (p_xform.elements[2] - get_transform().elements[2])/p_step;
+
+	real_t rot = inv_xform.basis_xform(get_transform().elements[1]).atan2();
+	angular_velocity = rot / p_step;
+
+	if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	simulated_motion=true;
+	_set_transform(p_xform);
+
+
+}
+
+void Body2DSW::wakeup_neighbours() {
+
+
+
+	for(Map<Constraint2DSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
+
+		const Constraint2DSW *c=E->key();
+		Body2DSW **n = c->get_body_ptr();
+		int bc=c->get_body_count();
+
+		for(int i=0;i<bc;i++) {
+
+			if (i==E->get())
+				continue;
+			Body2DSW *b = n[i];
+			if (b->mode!=Physics2DServer::BODY_MODE_RIGID)
+				continue;
+
+			if (!b->is_active())
+				b->set_active(true);
+		}
+	}
+}
+
+void Body2DSW::call_queries() {
+
+
+	if (fi_callback) {
+
+		Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton;
+		dbs->body=this;
+
+		Variant v=dbs;
+		const Variant *vp[2]={&v,&fi_callback->callback_udata};
+
+		Object *obj = ObjectDB::get_instance(fi_callback->id);
+		if (!obj) {
+
+			set_force_integration_callback(NULL,StringName());
+		} else {
+			Variant::CallError ce;			
+			if (fi_callback->callback_udata.get_type()) {
+
+				obj->call(fi_callback->method,vp,2,ce);
+
+			} else {
+				obj->call(fi_callback->method,vp,1,ce);
+			}
+		}
+
+
+	}
+
+	if (simulated_motion) {
+
+	//	linear_velocity=Vector2();
+	//	angular_velocity=0;
+		simulated_motion=false;
+	}
+}
+
+
+bool Body2DSW::sleep_test(real_t p_step)  {
+
+	if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_STATIC_ACTIVE)
+		return true; //
+	else if (mode==Physics2DServer::BODY_MODE_CHARACTER)
+		return !active; // characters don't sleep unless asked to sleep
+	else if (!can_sleep)
+		return false;
+
+
+
+
+	if (Math::abs(angular_velocity)<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {
+
+		still_time+=p_step;
+
+		return still_time > get_space()->get_body_time_to_sleep();
+	} else {
+
+		still_time=0; //maybe this should be set to 0 on set_active?
+		return false;
+	}
+}
+
+
+void Body2DSW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {
+
+	if (fi_callback) {
+
+		memdelete(fi_callback);
+		fi_callback=NULL;
+	}
+
+
+	if (p_id!=0) {
+
+		fi_callback=memnew(ForceIntegrationCallback);
+		fi_callback->id=p_id;
+		fi_callback->method=p_method;
+		fi_callback->callback_udata=p_udata;
+	}
+
+}
+
+Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
+
+
+	mode=Physics2DServer::BODY_MODE_RIGID;
+	active=true;
+	angular_velocity=0;
+	biased_angular_velocity=0;
+	mass=1;
+	_inv_inertia=0;
+	_inv_mass=1;
+	bounce=0;
+	friction=1;
+	omit_force_integration=false;
+	applied_torque=0;
+	island_step=0;
+	island_next=NULL;
+	island_list_next=NULL;
+	_set_static(false);
+	density=0;
+	contact_count=0;
+	simulated_motion=false;
+	still_time=0;
+	continuous_cd=false;
+	can_sleep=false;
+	fi_callback=NULL;
+
+}
+
+Body2DSW::~Body2DSW() {
+
+	if (fi_callback)
+		memdelete(fi_callback);
+}
+
+Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton=NULL;
+
+Physics2DDirectSpaceState* Physics2DDirectBodyStateSW::get_space_state() {
+
+	return body->get_space()->get_direct_state();
+}

servers/physics_2d/body_2d_sw.h

@@ -0,0 +1,334 @@
+/*************************************************************************/
+/*  body_2d_sw.h                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BODY_2D_SW_H
+#define BODY_2D_SW_H
+
+#include "collision_object_2d_sw.h"
+#include "vset.h"
+#include "area_2d_sw.h"
+
+class Constraint2DSW;
+
+
+class Body2DSW : public CollisionObject2DSW {
+
+
+	Physics2DServer::BodyMode mode;
+
+	Vector2 biased_linear_velocity;
+	real_t biased_angular_velocity;
+
+	Vector2 linear_velocity;
+	real_t angular_velocity;
+
+	real_t mass;
+	real_t bounce;
+	real_t friction;
+
+	real_t _inv_mass;
+	real_t _inv_inertia;
+
+	Vector2 gravity;
+	real_t density;
+
+	real_t still_time;
+
+	Vector2 applied_force;
+	real_t applied_torque;
+
+	SelfList<Body2DSW> active_list;
+	SelfList<Body2DSW> inertia_update_list;
+	SelfList<Body2DSW> direct_state_query_list;
+
+	VSet<RID> exceptions;
+	bool omit_force_integration;
+	bool active;
+	bool simulated_motion;
+	bool continuous_cd;
+	bool can_sleep;
+	void _update_inertia();
+	virtual void _shapes_changed();
+
+
+	Map<Constraint2DSW*,int> constraint_map;
+
+	struct AreaCMP {
+
+		Area2DSW *area;
+		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }
+		_FORCE_INLINE_ AreaCMP() {}
+		_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;}
+	};
+
+
+	VSet<AreaCMP> areas;
+
+	struct Contact {
+
+
+		Vector2 local_pos;
+		Vector2 local_normal;
+		float depth;
+		int local_shape;
+		Vector2 collider_pos;
+		int collider_shape;
+		ObjectID collider_instance_id;
+		RID collider;
+		Vector2 collider_velocity_at_pos;
+	};
+
+	Vector<Contact> contacts; //no contacts by default
+	int contact_count;
+
+	struct ForceIntegrationCallback {
+
+		ObjectID id;
+		StringName method;
+		Variant callback_udata;
+	};
+
+	ForceIntegrationCallback *fi_callback;
+
+
+	uint64_t island_step;
+	Body2DSW *island_next;
+	Body2DSW *island_list_next;
+
+	_FORCE_INLINE_ void _compute_area_gravity(const Area2DSW *p_area);
+
+friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose
+
+public:
+
+
+	void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant());
+
+
+	_FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); }
+	_FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); }
+
+	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }
+	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
+
+	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
+	_FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos);
+
+
+	_FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
+	_FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
+	_FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
+	_FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}
+
+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+
+	_FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; }
+
+	_FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; }
+
+	_FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
+	_FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); }
+	const Map<Constraint2DSW*,int>& get_constraint_map() const { return constraint_map; }
+
+	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
+	_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
+
+	_FORCE_INLINE_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; }
+	_FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
+
+	_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; }
+	_FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
+
+	_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; }
+	_FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
+
+	_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; }
+	_FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
+
+	_FORCE_INLINE_ void apply_impulse(const Vector2& p_pos, const Vector2& p_j) {
+
+		linear_velocity += p_j * _inv_mass;
+		angular_velocity += _inv_inertia * p_pos.cross(p_j);
+	}
+
+	_FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) {
+
+		biased_linear_velocity += p_j * _inv_mass;
+		biased_angular_velocity += _inv_inertia * p_pos.cross(p_j);
+	}
+
+	void set_active(bool p_active);
+	_FORCE_INLINE_ bool is_active() const { return active; }
+
+	void set_param(Physics2DServer::BodyParameter p_param, float);
+	float get_param(Physics2DServer::BodyParameter p_param) const;
+
+	void set_mode(Physics2DServer::BodyMode p_mode);
+	Physics2DServer::BodyMode get_mode() const;
+
+	void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant);
+	Variant get_state(Physics2DServer::BodyState p_state) const;
+
+	void set_applied_force(const Vector2& p_force) { applied_force=p_force; }
+	Vector2 get_applied_force() const { return applied_force; }
+
+	void set_applied_torque(real_t p_torque) { applied_torque=p_torque; }
+	real_t get_applied_torque() const { return applied_torque; }
+
+	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; }
+	_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
+
+	void set_space(Space2DSW *p_space);
+
+	void update_inertias();
+
+	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
+	_FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; }
+	_FORCE_INLINE_ real_t get_friction() const { return friction; }
+	_FORCE_INLINE_ Vector2 get_gravity() const { return gravity; }
+	_FORCE_INLINE_ real_t get_density() const { return density; }
+
+	void integrate_forces(real_t p_step);
+	void integrate_velocities(real_t p_step);
+
+	void simulate_motion(const Matrix32& p_xform,real_t p_step);
+	void call_queries();
+	void wakeup_neighbours();
+
+	bool sleep_test(real_t p_step);
+
+	Body2DSW();
+	~Body2DSW();
+
+};
+
+
+//add contact inline
+
+void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) {
+
+	int c_max=contacts.size();
+
+	if (c_max==0)
+		return;
+
+	Contact *c = &contacts[0];
+
+
+	int idx=-1;
+
+	if (contact_count<c_max) {
+		idx=contact_count++;
+	} else {
+
+		float least_depth=1e20;
+		int least_deep=-1;
+		for(int i=0;i<c_max;i++) {
+
+			if (i==0 || c[i].depth<least_depth) {
+				least_deep=i;
+				least_depth=c[i].depth;
+			}
+		}
+
+		if (least_deep>=0 && least_depth<p_depth) {
+
+			idx=least_deep;
+		}
+		if (idx==-1)
+			return; //none least deepe than this
+	}
+
+	c[idx].local_pos=p_local_pos;
+	c[idx].local_normal=p_local_normal;
+	c[idx].depth=p_depth;
+	c[idx].local_shape=p_local_shape;
+	c[idx].collider_pos=p_collider_pos;
+	c[idx].collider_shape=p_collider_shape;
+	c[idx].collider_instance_id=p_collider_instance_id;
+	c[idx].collider=p_collider;
+	c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;
+
+}
+
+
+class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {
+
+	OBJ_TYPE( Physics2DDirectBodyStateSW, Physics2DDirectBodyState );
+
+public:
+
+	static Physics2DDirectBodyStateSW *singleton;
+	Body2DSW *body;
+	real_t step;
+
+	virtual Vector2 get_total_gravity() const {  return body->get_gravity();  } // get gravity vector working on this body space/area
+	virtual float get_total_density() const {  return body->get_density();  } // get density of this body space/area
+
+	virtual float get_inverse_mass() const {  return body->get_inv_mass();  } // get the mass
+	virtual real_t get_inverse_inertia() const { return body->get_inv_inertia();   } // get density of this body space
+
+	virtual void set_linear_velocity(const Vector2& p_velocity) {  body->set_linear_velocity(p_velocity);  }
+	virtual Vector2 get_linear_velocity() const {  return body->get_linear_velocity();  }
+
+	virtual void set_angular_velocity(real_t p_velocity) {  body->set_angular_velocity(p_velocity);  }
+	virtual real_t get_angular_velocity() const {  return body->get_angular_velocity();  }
+
+	virtual void set_transform(const Matrix32& p_transform) {  body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform);  }
+	virtual Matrix32 get_transform() const {  return body->get_transform();  }
+
+	virtual void set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }
+	virtual bool is_sleeping() const {  return !body->is_active();  }
+
+	virtual int get_contact_count() const {  return body->contact_count;  }
+
+	virtual Vector2 get_contact_local_pos(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());
+		return body->contacts[p_contact_idx].local_pos;
+	}
+	virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());   return body->contacts[p_contact_idx].local_normal;   }
+	virtual int get_contact_local_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1);   return body->contacts[p_contact_idx].local_shape;   }
+
+	virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID());   return body->contacts[p_contact_idx].collider;   }
+	virtual Vector2 get_contact_collider_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());   return body->contacts[p_contact_idx].collider_pos;  }
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0);   return body->contacts[p_contact_idx].collider_instance_id;   }
+	virtual int get_contact_collider_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape;  }
+	virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos;  }
+
+	virtual Physics2DDirectSpaceState* get_space_state();
+
+
+	virtual real_t get_step() const { return step; }
+	Physics2DDirectBodyStateSW() { singleton=this; body=NULL; }
+};
+
+
+#endif // BODY_2D_SW_H

servers/physics_2d/body_pair_2d_sw.cpp

@@ -0,0 +1,435 @@
+/*************************************************************************/
+/*  body_pair_2d_sw.cpp                                                  */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "body_pair_2d_sw.h"
+#include "collision_solver_2d_sw.h"
+#include "space_2d_sw.h"
+
+
+#define POSITION_CORRECTION
+#define ACCUMULATE_IMPULSES
+
+void BodyPair2DSW::_add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self) {
+
+	BodyPair2DSW *self = (BodyPair2DSW *)p_self;
+
+	self->_contact_added_callback(p_point_A,p_point_B);
+
+}
+
+void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B) {
+
+	// check if we already have the contact
+
+
+	Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
+	Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B-offset_B);
+
+	int new_index = contact_count;
+
+	ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) );
+
+	Contact contact;
+
+	contact.acc_normal_impulse=0;
+	contact.acc_bias_impulse=0;
+	contact.acc_tangent_impulse=0;
+	contact.local_A=local_A;
+	contact.local_B=local_B;
+	contact.normal=(p_point_A-p_point_B).normalized();
+
+	// attempt to determine if the contact will be reused
+
+	real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius();
+
+	for (int i=0;i<contact_count;i++) {
+
+		Contact& c = contacts[i];
+		if (
+		c.local_A.distance_squared_to( local_A ) < (recycle_radius_2) &&
+		c.local_B.distance_squared_to( local_B ) < (recycle_radius_2) ) {
+
+			contact.acc_normal_impulse=c.acc_normal_impulse;
+			contact.acc_tangent_impulse=c.acc_tangent_impulse;
+			contact.acc_bias_impulse=c.acc_bias_impulse;
+			new_index=i;
+			break;
+		}
+	}
+
+	// figure out if the contact amount must be reduced to fit the new contact
+
+	if (new_index == MAX_CONTACTS) {
+
+		// remove the contact with the minimum depth
+
+		int least_deep=-1;
+		real_t min_depth=1e10;
+
+
+		for (int i=0;i<=contact_count;i++) {
+
+			Contact& c = (i==contact_count)?contact:contacts[i];
+			Vector2 global_A = A->get_transform().basis_xform(c.local_A);
+			Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
+
+			Vector2 axis = global_A - global_B;
+			float depth = axis.dot( c.normal );
+
+
+			if (depth<min_depth) {
+
+				min_depth=depth;
+				least_deep=i;
+			}
+		}
+
+		ERR_FAIL_COND(least_deep==-1);
+
+		if (least_deep < contact_count) { //replace the last deep contact by the new one
+
+			contacts[least_deep]=contact;
+		}
+
+		return;
+	}
+
+	contacts[new_index]=contact;
+
+	if (new_index==contact_count) {
+
+		contact_count++;
+	}
+
+}
+
+void BodyPair2DSW::_validate_contacts() {
+
+	//make sure to erase contacts that are no longer valid
+
+	real_t max_separation = space->get_contact_max_separation();
+	real_t max_separation2 = max_separation*max_separation;
+
+	for (int i=0;i<contact_count;i++) {
+
+		Contact& c = contacts[i];
+
+		Vector2 global_A = A->get_transform().basis_xform(c.local_A);
+		Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
+		Vector2 axis = global_A - global_B;
+		float depth = axis.dot( c.normal );
+
+		if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
+			// contact no longer needed, remove
+
+
+			if ((i+1) < contact_count) {
+				// swap with the last one
+				SWAP( contacts[i], contacts[ contact_count-1 ] );
+
+			}
+
+			i--;
+			contact_count--;
+		}
+	}
+}
+
+
+bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result) {
+
+	Vector2 motion = p_A->get_linear_velocity()*p_step;
+	real_t mlen = motion.length();
+	if (mlen<CMP_EPSILON)
+		return false;
+
+	Vector2 mnormal = motion / mlen;
+
+	real_t min,max;
+	p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max);
+	if (mlen < (max-min)*0.3)  //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
+		return false;
+
+	//cast a segment from support in motion normal, in the same direction of motion by motion length
+	int a;
+	Vector2 s[2];
+	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a);
+	Vector2 from = p_xform_A.xform(s[0]);
+	Vector2 to = from + motion;
+
+	Vector2 local_from = p_xform_inv_B.xform(from);
+	Vector2 local_to = p_xform_inv_B.xform(to);
+
+	Vector2 rpos,rnorm;
+	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm))
+		return false;
+
+	//ray hit something
+
+	Vector2 contact_A = to;
+	Vector2 contact_B = p_xform_B.xform(rpos);
+
+	//create a contact
+
+	if (p_swap_result)
+		_contact_added_callback(contact_B,contact_A);
+	else
+		_contact_added_callback(contact_A,contact_B);
+
+
+	return true;
+}
+
+bool BodyPair2DSW::setup(float p_step) {
+
+
+	//use local A coordinates to avoid numerical issues on collision detection
+	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
+
+	_validate_contacts();
+
+	//cannot collide
+	if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=Physics2DServer::BODY_MODE_STATIC_ACTIVE)) {
+		collided=false;
+
+		return false;
+	}
+	Vector2 offset_A = A->get_transform().get_origin();
+	Matrix32 xform_Au = A->get_transform().untranslated();
+	Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A);
+	Matrix32 xform_inv_A = xform_A.affine_inverse();
+
+	Matrix32 xform_Bu = B->get_transform();
+	xform_Bu.elements[2]-=A->get_transform().get_origin();
+	Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B);
+	Matrix32 xform_inv_B = xform_B.affine_inverse();
+
+	Shape2DSW *shape_A_ptr=A->get_shape(shape_A);
+	Shape2DSW *shape_B_ptr=B->get_shape(shape_B);
+
+	collided = CollisionSolver2DSW::solve_static(shape_A_ptr,xform_A,xform_inv_A,shape_B_ptr,xform_B,xform_inv_B,_add_contact,this,&sep_axis);
+	if (!collided) {
+
+		//test ccd (currently just a raycast)
+		if (A->is_continuous_collision_detection_enabled() && A->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
+			if (_test_ccd(p_step,A,shape_A,xform_A,xform_inv_A,B,shape_B,xform_B,xform_inv_B))
+				collided=true;
+		}
+
+		if (B->is_continuous_collision_detection_enabled() && B->get_type()>Physics2DServer::BODY_MODE_STATIC_ACTIVE) {
+			if (_test_ccd(p_step,B,shape_B,xform_B,xform_inv_B,A,shape_A,xform_A,xform_inv_A,true))
+				collided=true;
+		}
+
+		if (!collided)
+			return false;
+
+	}
+
+
+
+	real_t max_penetration = space->get_contact_max_allowed_penetration();
+
+	float bias = 0.3f;
+	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
+
+		if (shape_A_ptr->get_custom_bias()==0)
+			bias=shape_B_ptr->get_custom_bias();
+		else if (shape_B_ptr->get_custom_bias()==0)
+			bias=shape_A_ptr->get_custom_bias();
+		else
+			bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5;
+	}
+
+
+	cc=0;
+
+
+	real_t inv_dt = 1.0/p_step;
+	for (int i = 0; i < contact_count; i++) {
+
+		Contact& c = contacts[i];
+
+		Vector2 global_A = xform_Au.xform(c.local_A);
+		Vector2 global_B = xform_Bu.xform(c.local_B);
+
+		real_t depth = c.normal.dot(global_A - global_B);
+
+		if (depth<=0) {
+			c.active=false;
+			continue;
+		}
+
+		c.active=true;
+
+		int gather_A = A->can_report_contacts();
+		int gather_B = B->can_report_contacts();
+
+		c.rA = global_A;
+		c.rB = global_B-offset_B;
+
+		if (gather_A | gather_B) {
+
+			//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+
+			global_A+=offset_A;
+			global_B+=offset_A;
+
+			if (gather_A) {
+				Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+				A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB+B->get_linear_velocity());
+			}
+			if (gather_B) {
+
+				Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
+				B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA+A->get_linear_velocity());
+			}
+		}
+
+
+		// Precompute normal mass, tangent mass, and bias.
+		real_t rnA = c.rA.dot(c.normal);
+		real_t rnB = c.rB.dot(c.normal);
+		real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
+		kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB);
+		c.mass_normal = 1.0f / kNormal;
+
+		Vector2 tangent = c.normal.tangent();
+		real_t rtA = c.rA.dot(tangent);
+		real_t rtB = c.rB.dot(tangent);
+		real_t kTangent = A->get_inv_mass() + B->get_inv_mass();
+		kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB);
+		c.mass_tangent = 1.0f /  kTangent;
+
+
+
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+		c.depth=depth;
+		//c.acc_bias_impulse=0;
+
+
+#ifdef ACCUMULATE_IMPULSES
+		{
+			// Apply normal + friction impulse
+			Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
+
+
+			A->apply_impulse(c.rA,-P);
+			B->apply_impulse(c.rB, P);
+		}
+
+#endif
+	}
+
+	return true;
+}
+
+void BodyPair2DSW::solve(float p_step) {
+
+	if (!collided)
+		return;
+
+	for (int i = 0; i < contact_count; ++i) {
+
+		Contact& c = contacts[i];
+		cc++;
+
+		if (!c.active)
+			continue;
+
+
+		// Relative velocity at contact
+
+		Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
+		Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+		Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
+
+		Vector2 crbA( -A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x );
+		Vector2 crbB( -B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x );
+		Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
+
+		real_t vn = dv.dot(c.normal);
+		real_t vbn = dbv.dot(c.normal);
+		Vector2 tangent = c.normal.tangent();
+		real_t vt = dv.dot(tangent);
+
+
+		real_t jbn = (c.bias - vbn)*c.mass_normal;
+		real_t jbnOld = c.acc_bias_impulse;
+		c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
+
+		Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
+
+		A->apply_bias_impulse(c.rA,-jb);
+		B->apply_bias_impulse(c.rB, jb);
+
+		real_t bounce=0;
+		real_t jn = -(bounce + vn)*c.mass_normal;
+		real_t jnOld = c.acc_normal_impulse;
+		c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
+
+
+		real_t friction = A->get_friction() * B->get_friction();
+
+		real_t jtMax = friction*c.acc_normal_impulse;
+		real_t jt = -vt*c.mass_tangent;
+		real_t jtOld = c.acc_tangent_impulse;
+		c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax);
+
+		Vector2 j =c.normal * (c.acc_normal_impulse - jnOld) + tangent * ( c.acc_tangent_impulse - jtOld );
+
+
+		A->apply_impulse(c.rA,-j);
+		B->apply_impulse(c.rB, j);
+
+
+	}
+}
+
+
+BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B) : Constraint2DSW(_arr,2) {
+
+	A=p_A;
+	B=p_B;
+	shape_A=p_shape_A;
+	shape_B=p_shape_B;
+	space=A->get_space();
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+	contact_count=0;
+	collided=false;
+
+}
+
+
+BodyPair2DSW::~BodyPair2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+
+}

servers/physics_2d/body_pair_2d_sw.h

@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  body_pair_2d_sw.h                                                    */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BODY_PAIR_2D_SW_H
+#define BODY_PAIR_2D_SW_H
+
+#include "body_2d_sw.h"
+#include "constraint_2d_sw.h"
+
+class BodyPair2DSW : public Constraint2DSW {
+
+	enum {
+		MAX_CONTACTS=2
+	};
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+	int shape_A;
+	int shape_B;
+
+	Space2DSW *space;
+
+	struct Contact {
+
+		Vector2 position;
+		Vector2 normal;
+		Vector2 local_A, local_B;
+		real_t acc_normal_impulse;	// accumulated normal impulse (Pn)
+		real_t acc_tangent_impulse;	// accumulated tangent impulse (Pt)
+		real_t acc_bias_impulse;	// accumulated normal impulse for position bias (Pnb)
+		real_t mass_normal, mass_tangent;
+		real_t bias;
+
+		real_t depth;
+		bool active;
+		Vector2 rA,rB;
+	};
+
+	Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection
+
+	Vector2 sep_axis;
+	Contact contacts[MAX_CONTACTS];
+	int contact_count;
+	bool collided;
+	int cc;
+
+
+	bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,const Matrix32& p_xform_inv_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,const Matrix32& p_xform_inv_B,bool p_swap_result=false);
+	void _validate_contacts();
+	static void _add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self);
+	_FORCE_INLINE_ void _contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B);
+
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B);
+	~BodyPair2DSW();
+
+};
+
+#endif // BODY_PAIR_2D_SW_H

servers/physics_2d/broad_phase_2d_basic.cpp

@@ -0,0 +1,192 @@
+/*************************************************************************/
+/*  broad_phase_2d_basic.cpp                                             */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "broad_phase_2d_basic.h"
+
+ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) {
+
+
+	current++;
+
+	Element e;
+	e.owner=p_object_;
+	e._static=false;
+	e.subindex=p_subindex;
+
+	element_map[current]=e;
+	return current;
+}
+
+void BroadPhase2DBasic::move(ID p_id, const Rect2& p_aabb) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	E->get().aabb=p_aabb;
+
+}
+void BroadPhase2DBasic::set_static(ID p_id, bool p_static) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	E->get()._static=p_static;
+
+}
+void BroadPhase2DBasic::remove(ID p_id) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+	element_map.erase(E);
+
+}
+
+CollisionObject2DSW *BroadPhase2DBasic::get_object(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,NULL);
+	return E->get().owner;
+
+}
+bool BroadPhase2DBasic::is_static(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,false);
+	return E->get()._static;
+
+}
+int BroadPhase2DBasic::get_subindex(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,-1);
+	return E->get().subindex;
+}
+
+int BroadPhase2DBasic::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+
+	int rc=0;
+
+	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+
+		const Rect2 aabb=E->get().aabb;
+		if (aabb.intersects_segment(p_from,p_to)) {
+
+			p_results[rc]=E->get().owner;
+			p_result_indices[rc]=E->get().subindex;
+			rc++;
+			if (rc>=p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+
+}
+int BroadPhase2DBasic::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+
+	int rc=0;
+
+	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+
+		const Rect2 aabb=E->get().aabb;
+		if (aabb.intersects(p_aabb)) {
+
+			p_results[rc]=E->get().owner;
+			p_result_indices[rc]=E->get().subindex;
+			rc++;
+			if (rc>=p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+}
+
+void BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
+
+	pair_userdata=p_userdata;
+	pair_callback=p_pair_callback;
+
+}
+void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) {
+
+	unpair_userdata=p_userdata;
+	unpair_callback=p_pair_callback;
+
+}
+
+void BroadPhase2DBasic::update() {
+
+	// recompute pairs
+	for(Map<ID,Element>::Element *I=element_map.front();I;I=I->next()) {
+
+		for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) {
+
+			Element *elem_A=&I->get();
+			Element *elem_B=&J->get();
+
+			if (elem_A->owner == elem_B->owner)
+				continue;
+
+
+			bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static );
+
+			PairKey key(I->key(),J->key());
+
+			Map<PairKey,void*>::Element *E=pair_map.find(key);
+
+			if (!pair_ok && E) {
+				if (unpair_callback)
+					unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,E->get(),unpair_userdata);
+				pair_map.erase(key);
+			}
+
+			if (pair_ok && !E) {
+
+				void *data=NULL;
+				if (pair_callback)
+					data=pair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,unpair_userdata);
+				pair_map.insert(key,data);
+			}
+		}
+	}
+
+}
+
+BroadPhase2DSW *BroadPhase2DBasic::_create() {
+
+	return memnew( BroadPhase2DBasic );
+}
+
+BroadPhase2DBasic::BroadPhase2DBasic() {
+
+	current=1;
+	unpair_callback=NULL;
+	unpair_userdata=NULL;
+	pair_callback=NULL;
+	pair_userdata=NULL;
+
+}

servers/physics_2d/broad_phase_2d_basic.h

@@ -0,0 +1,100 @@
+/*************************************************************************/
+/*  broad_phase_2d_basic.h                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BROAD_PHASE_2D_BASIC_H
+#define BROAD_PHASE_2D_BASIC_H
+
+#include "space_2d_sw.h"
+#include "map.h"
+class BroadPhase2DBasic : public BroadPhase2DSW {
+
+	struct Element {
+
+		CollisionObject2DSW *owner;
+		bool _static;
+		Rect2 aabb;
+		int subindex;
+	};
+
+
+	Map<ID,Element> element_map;
+
+	ID current;
+
+	struct PairKey {
+
+		union {
+			struct {
+				ID a;
+				ID b;
+			};
+			uint64_t key;
+		};
+
+		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+			return key < p_key.key;
+		}
+
+		PairKey() { key=0; }
+		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
+
+	};
+
+	Map<PairKey,void*> pair_map;
+
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+public:
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0);
+	virtual void move(ID p_id, const Rect2& p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObject2DSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+
+	virtual void update();
+
+	static BroadPhase2DSW *_create();
+	BroadPhase2DBasic();
+};
+
+#endif // BROAD_PHASE_2D_BASIC_H

servers/physics_2d/broad_phase_2d_hash_grid.cpp

@@ -0,0 +1,665 @@
+/*************************************************************************/
+/*  broad_phase_2d_hash_grid.cpp                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "broad_phase_2d_hash_grid.h"
+#include "globals.h"
+
+void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element* p_with) {
+
+	Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
+
+	ERR_FAIL_COND(p_elem->_static && p_with->_static);
+
+	if (!E) {
+
+		PairData *pd = memnew( PairData );
+		p_elem->paired[p_with]=pd;
+		p_with->paired[p_elem]=pd;
+	} else {
+		E->get()->rc++;
+	}
+
+}
+
+void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element* p_with) {
+
+	Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
+
+	ERR_FAIL_COND(!E); //this should really be paired..
+
+	E->get()->rc--;
+
+	if (E->get()->rc==0) {
+
+		if (E->get()->colliding) {
+			//uncollide
+			if (unpair_callback) {
+				unpair_callback(p_elem->owner,p_elem->subindex,p_with->owner,p_with->subindex,E->get()->ud,unpair_userdata);
+			}
+
+
+		}
+
+		memdelete(E->get());
+		p_elem->paired.erase(E);
+		p_with->paired.erase(p_elem);
+	}
+
+
+}
+
+void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
+
+	for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) {
+
+		bool pairing = p_elem->aabb.intersects( E->key()->aabb );
+
+		if (pairing!=E->get()->colliding) {
+
+			if (pairing) {
+
+				if (pair_callback) {
+					E->get()->ud=pair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,pair_userdata);
+				}
+			} else {
+
+				if (unpair_callback) {
+					unpair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,E->get()->ud,unpair_userdata);
+				}
+
+			}
+
+			E->get()->colliding=pairing;
+		}
+	}
+}
+
+void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
+
+
+	Point2i from = (p_rect.pos/cell_size).floor();
+	Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
+
+	for(int i=from.x;i<=to.x;i++) {
+
+
+		for(int j=from.y;j<=to.y;j++) {
+
+			PosKey pk;
+			pk.x=i;
+			pk.y=j;
+
+			uint32_t idx = pk.hash() % hash_table_size;
+			PosBin *pb = hash_table[idx];
+
+			while (pb) {
+
+				if (pb->key == pk) {
+					break;
+				}
+
+				pb=pb->next;
+			}
+
+
+			bool entered=false;
+
+			if (!pb) {
+				//does not exist, create!
+				pb = memnew( PosBin );
+				pb->key=pk;
+				pb->next=hash_table[idx];
+				hash_table[idx]=pb;
+			}
+
+
+
+			if (p_static) {
+				if (pb->static_object_set[p_elem].inc()==1) {
+					entered=true;
+				}
+			} else {
+				if (pb->object_set[p_elem].inc()==1) {
+
+					entered=true;
+				}
+			}
+
+			if (entered) {
+
+				for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
+
+					if (E->key()->owner==p_elem->owner)
+						continue;
+					_pair_attempt(p_elem,E->key());
+				}
+
+				if (!p_static) {
+
+					for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
+
+						if (E->key()->owner==p_elem->owner)
+							continue;
+						_pair_attempt(p_elem,E->key());
+					}
+				}
+			}
+
+		}
+
+	}
+
+
+}
+
+
+void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
+
+
+	Point2i from = (p_rect.pos/cell_size).floor();
+	Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
+
+	for(int i=from.x;i<=to.x;i++) {
+
+		for(int j=from.y;j<=to.y;j++) {
+
+			PosKey pk;
+			pk.x=i;
+			pk.y=j;
+
+			uint32_t idx = pk.hash() % hash_table_size;
+			PosBin *pb = hash_table[idx];
+
+			while (pb) {
+
+				if (pb->key == pk) {
+					break;
+				}
+
+				pb=pb->next;
+			}
+
+			ERR_CONTINUE(!pb); //should exist!!
+
+			bool exited=false;
+
+
+			if (p_static) {
+				if (pb->static_object_set[p_elem].dec()==0) {
+
+					pb->static_object_set.erase(p_elem);
+					exited=true;
+
+				}
+			} else {
+				if (pb->object_set[p_elem].dec()==0) {
+
+					pb->object_set.erase(p_elem);
+					exited=true;
+
+				}
+			}
+
+			if (exited) {
+
+				for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
+
+					if (E->key()->owner==p_elem->owner)
+						continue;
+					_unpair_attempt(p_elem,E->key());
+
+				}
+
+				if (!p_static) {
+
+					for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
+
+						if (E->key()->owner==p_elem->owner)
+							continue;
+						_unpair_attempt(p_elem,E->key());
+					}
+				}
+			}
+
+			if (pb->object_set.empty() && pb->static_object_set.empty()) {
+
+				if (hash_table[idx]==pb) {
+					hash_table[idx]=pb->next;
+				} else {
+
+					PosBin *px = hash_table[idx];
+
+					while (px) {
+
+						if (px->next==pb) {
+							px->next=pb->next;
+							break;
+						}
+
+						px=px->next;
+					}
+
+					ERR_CONTINUE(!px);
+				}
+
+				memdelete(pb);
+
+			}
+		}
+
+	}
+
+}
+
+
+BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) {
+
+	current++;
+
+	Element e;
+	e.owner=p_object;
+	e._static=false;
+	e.subindex=p_subindex;
+	e.self=current;
+	e.pass=0;
+
+	element_map[current]=e;
+	return current;
+
+}
+
+void BroadPhase2DHashGrid::move(ID p_id, const Rect2& p_aabb) {
+
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+
+	Element &e=E->get();
+
+	if (p_aabb==e.aabb)
+		return;
+
+
+	if (p_aabb!=Rect2()) {
+
+		_enter_grid(&e,p_aabb,e._static);
+	}
+
+	if (e.aabb!=Rect2()) {
+
+		_exit_grid(&e,e.aabb,e._static);
+	}
+
+	e.aabb=p_aabb;
+
+	_check_motion(&e);
+
+	e.aabb=p_aabb;
+
+}
+void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+
+	Element &e=E->get();
+
+	if (e._static==p_static)
+		return;
+
+	if (e.aabb!=Rect2())
+		_exit_grid(&e,e.aabb,e._static);
+
+	e._static=p_static;
+
+	if (e.aabb!=Rect2()) {
+		_enter_grid(&e,e.aabb,e._static);
+		_check_motion(&e);
+	}
+
+}
+void BroadPhase2DHashGrid::remove(ID p_id) {
+
+	Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+
+	Element &e=E->get();
+
+	if (e.aabb!=Rect2())
+		_exit_grid(&e,e.aabb,e._static);
+
+	element_map.erase(p_id);
+
+}
+
+CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,NULL);
+	return E->get().owner;
+
+}
+bool BroadPhase2DHashGrid::is_static(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,false);
+	return E->get()._static;
+
+}
+int BroadPhase2DHashGrid::get_subindex(ID p_id) const {
+
+	const Map<ID,Element>::Element *E=element_map.find(p_id);
+	ERR_FAIL_COND_V(!E,-1);
+	return E->get().subindex;
+}
+
+void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) {
+
+
+	PosKey pk;
+	pk.x=p_cell.x;
+	pk.y=p_cell.y;
+
+	uint32_t idx = pk.hash() % hash_table_size;
+	PosBin *pb = hash_table[idx];
+
+	while (pb) {
+
+		if (pb->key == pk) {
+			break;
+		}
+
+		pb=pb->next;
+	}
+
+	if (!pb)
+		return;
+
+
+
+	for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
+
+
+		if (index>=p_max_results)
+			break;
+		if (E->key()->pass==pass)
+			continue;
+
+		E->key()->pass=pass;
+		p_results[index]=E->key()->owner;
+		p_result_indices[index]=E->key()->subindex;
+		index++;
+
+	}
+
+	for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
+
+
+		if (index>=p_max_results)
+			break;
+		if (E->key()->pass==pass)
+			continue;
+
+		E->key()->pass=pass;
+		p_results[index]=E->key()->owner;
+		p_result_indices[index]=E->key()->subindex;
+		index++;
+
+	}
+}
+
+int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+
+	pass++;
+
+	Vector2 dir = (p_to-p_from);
+	if (dir==Vector2())
+		return 0;
+	//avoid divisions by zero
+	dir.normalize();
+	if (dir.x==0.0)
+		dir.x=0.000001;
+	if (dir.y==0.0)
+		dir.y=0.000001;
+	Vector2 delta = dir.abs();
+
+	delta.x=cell_size/delta.x;
+	delta.y=cell_size/delta.y;
+
+	Point2i pos = p_from.floor() / cell_size;
+	Point2i end = p_to.floor() / cell_size;
+	Point2i step = Vector2( SGN(dir.x), SGN(dir.y));
+
+	Vector2 max;
+
+	if (dir.x<0)
+		max.x= (Math::floor(pos.x)*cell_size - p_from.x) / dir.x;
+	else
+		max.x= (Math::floor(pos.x + 1)*cell_size - p_from.x) / dir.x;
+
+	if (dir.y<0)
+		max.y= (Math::floor(pos.y)*cell_size - p_from.y) / dir.y;
+	else
+		max.y= (Math::floor(pos.y + 1)*cell_size - p_from.y) / dir.y;
+
+	int cullcount=0;
+	_cull(pos,p_results,p_max_results,p_result_indices,cullcount);
+
+	bool reached_x=false;
+	bool reached_y=false;
+
+	while(true) {
+
+		if (max.x < max.y) {
+
+			max.x+=delta.x;
+			pos.x+=step.x;
+		} else {
+
+			max.y+=delta.y;
+			pos.y+=step.y;
+
+		}
+
+		if (step.x>0) {
+			if (pos.x>=end.x)
+				reached_x=true;
+		} else if (pos.x<=end.x) {
+
+			reached_x=true;
+		}
+
+		if (step.y>0) {
+			if (pos.y>=end.y)
+				reached_y=true;
+		} else if (pos.y<=end.y) {
+
+			reached_y=true;
+		}
+
+		_cull(pos,p_results,p_max_results,p_result_indices,cullcount);
+
+		if (reached_x && reached_y)
+			break;
+
+	}
+
+	return cullcount;
+}
+
+
+int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+
+
+	return 0;
+}
+
+void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
+
+	pair_callback=p_pair_callback;
+	pair_userdata=p_userdata;
+
+}
+void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata) {
+
+	unpair_callback=p_unpair_callback;
+	unpair_userdata=p_userdata;
+
+}
+
+void BroadPhase2DHashGrid::update() {
+
+
+}
+
+BroadPhase2DSW *BroadPhase2DHashGrid::_create() {
+
+	return memnew( BroadPhase2DHashGrid );
+}
+
+
+BroadPhase2DHashGrid::BroadPhase2DHashGrid() {
+
+	hash_table_size = GLOBAL_DEF("physics_2d/bp_hash_table_size",4096);
+	hash_table_size = Math::larger_prime(hash_table_size);
+	hash_table = memnew_arr( PosBin*, hash_table_size);
+
+	cell_size = GLOBAL_DEF("physics_2d/cell_size",128);
+
+	for(int i=0;i<hash_table_size;i++)
+		hash_table[i]=NULL;
+	pass=1;
+
+	current=0;
+}
+
+BroadPhase2DHashGrid::~BroadPhase2DHashGrid() {
+
+	for(int i=0;i<hash_table_size;i++) {
+		while(hash_table[i]) {
+			PosBin *pb=hash_table[i];
+			hash_table[i]=pb->next;
+			memdelete(pb);
+		}
+	}
+
+	memdelete_arr( hash_table );
+
+
+}
+
+
+
+/* 3D version of voxel traversal:
+
+public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth)
+{
+    // Implementation is based on:
+    // "A Fast Voxel Traversal Algorithm for Ray Tracing"
+    // John Amanatides, Andrew Woo
+    // http://www.cse.yorku.ca/~amana/research/grid.pdf
+    // http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf
+
+    // NOTES:
+    // * This code assumes that the ray's position and direction are in 'cell coordinates', which means
+    //   that one unit equals one cell in all directions.
+    // * When the ray doesn't start within the voxel grid, calculate the first position at which the
+    //   ray could enter the grid. If it never enters the grid, there is nothing more to do here.
+    // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
+    // * The Point3D structure is a simple structure having three integer fields (X, Y and Z).
+
+    // The cell in which the ray starts.
+    Point3D start = GetCellAt(ray.Position);        // Rounds the position's X, Y and Z down to the nearest integer values.
+    int x = start.X;
+    int y = start.Y;
+    int z = start.Z;
+
+    // Determine which way we go.
+    int stepX = Math.Sign(ray.Direction.X);
+    int stepY = Math.Sign(ray.Direction.Y);
+    int stepZ = Math.Sign(ray.Direction.Z);
+
+    // Calculate cell boundaries. When the step (i.e. direction sign) is positive,
+    // the next boundary is AFTER our current position, meaning that we have to add 1.
+    // Otherwise, it is BEFORE our current position, in which case we add nothing.
+    Point3D cellBoundary = new Point3D(
+	x + (stepX > 0 ? 1 : 0),
+	y + (stepY > 0 ? 1 : 0),
+	z + (stepZ > 0 ? 1 : 0));
+
+    // NOTE: For the following calculations, the result will be Single.PositiveInfinity
+    // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand
+    // value of the division also equals zero, the result is Single.NaN, which is not OK.
+
+    // Determine how far we can travel along the ray before we hit a voxel boundary.
+    Vector3 tMax = new Vector3(
+	(cellBoundary.X - ray.Position.X) / ray.Direction.X,    // Boundary is a plane on the YZ axis.
+	(cellBoundary.Y - ray.Position.Y) / ray.Direction.Y,    // Boundary is a plane on the XZ axis.
+	(cellBoundary.Z - ray.Position.Z) / ray.Direction.Z);    // Boundary is a plane on the XY axis.
+    if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity;
+    if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity;
+    if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity;
+
+    // Determine how far we must travel along the ray before we have crossed a gridcell.
+    Vector3 tDelta = new Vector3(
+	stepX / ray.Direction.X,                    // Crossing the width of a cell.
+	stepY / ray.Direction.Y,                    // Crossing the height of a cell.
+	stepZ / ray.Direction.Z);                    // Crossing the depth of a cell.
+    if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity;
+    if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity;
+    if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity;
+
+    // For each step, determine which distance to the next voxel boundary is lowest (i.e.
+    // which voxel boundary is nearest) and walk that way.
+    for (int i = 0; i < maxDepth; i++)
+    {
+	// Return it.
+	yield return new Point3D(x, y, z);
+
+	// Do the next step.
+	if (tMax.X < tMax.Y && tMax.X < tMax.Z)
+	{
+	    // tMax.X is the lowest, an YZ cell boundary plane is nearest.
+	    x += stepX;
+	    tMax.X += tDelta.X;
+	}
+	else if (tMax.Y < tMax.Z)
+	{
+	    // tMax.Y is the lowest, an XZ cell boundary plane is nearest.
+	    y += stepY;
+	    tMax.Y += tDelta.Y;
+	}
+	else
+	{
+	    // tMax.Z is the lowest, an XY cell boundary plane is nearest.
+	    z += stepZ;
+	    tMax.Z += tDelta.Z;
+	}
+    }
+
+    */

servers/physics_2d/broad_phase_2d_hash_grid.h

@@ -0,0 +1,192 @@
+/*************************************************************************/
+/*  broad_phase_2d_hash_grid.h                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BROAD_PHASE_2D_HASH_GRID_H
+#define BROAD_PHASE_2D_HASH_GRID_H
+
+#include "broad_phase_2d_sw.h"
+#include "map.h"
+
+class BroadPhase2DHashGrid : public BroadPhase2DSW {
+
+
+	struct PairData {
+
+		bool colliding;
+		int rc;
+		void *ud;
+		PairData() { colliding=false; rc=1; ud=NULL; }
+	};
+
+	struct Element {
+
+		ID self;
+		CollisionObject2DSW *owner;
+		bool _static;
+		Rect2 aabb;
+		int subindex;
+		uint64_t pass;
+		Map<Element*,PairData*> paired;
+
+	};
+
+
+	Map<ID,Element> element_map;
+
+	ID current;
+
+	uint64_t pass;
+
+
+	struct PairKey {
+
+		union {
+			struct {
+				ID a;
+				ID b;
+			};
+			uint64_t key;
+		};
+
+		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+			return key < p_key.key;
+		}
+
+		PairKey() { key=0; }
+		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
+
+	};
+
+
+	Map<PairKey,PairData> pair_map;
+
+	int cell_size;
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+	void _enter_grid(Element* p_elem, const Rect2& p_rect,bool p_static);
+	void _exit_grid(Element* p_elem, const Rect2& p_rect,bool p_static);
+	_FORCE_INLINE_ void _cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index);
+
+
+	struct PosKey {
+
+		union {
+			struct {
+				int32_t x;
+				int32_t y;
+			};
+			uint64_t key;
+		};
+
+
+		_FORCE_INLINE_ uint32_t hash() const {
+			uint64_t k=key;
+			k = (~k) + (k << 18); // k = (k << 18) - k - 1;
+			k = k ^ (k >> 31);
+			k = k * 21; // k = (k + (k << 2)) + (k << 4);
+			k = k ^ (k >> 11);
+			k = k + (k << 6);
+			k = k ^ (k >> 22);
+			return k;
+		}
+
+		bool operator==(const PosKey& p_key) const { return key==p_key.key; }
+		_FORCE_INLINE_ bool operator<(const PosKey& p_key) const {
+			return key < p_key.key;
+		}
+
+	};
+
+	struct RC {
+
+		int ref;
+
+		_FORCE_INLINE_ int inc() {
+			ref++;
+			return ref;
+		}
+		_FORCE_INLINE_ int dec() {
+			ref--;
+			return ref;
+		}
+
+		_FORCE_INLINE_ RC() {
+			ref=0;
+		}
+	};
+
+	struct PosBin {
+
+		PosKey key;
+		Map<Element*,RC> object_set;
+		Map<Element*,RC> static_object_set;
+		PosBin *next;
+	};
+
+
+	uint32_t hash_table_size;
+	PosBin **hash_table;
+
+	void _pair_attempt(Element *p_elem, Element* p_with);
+	void _unpair_attempt(Element *p_elem, Element* p_with);
+	void _check_motion(Element *p_elem);
+
+
+public:
+
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0);
+	virtual void move(ID p_id, const Rect2& p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObject2DSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+
+	virtual void update();
+
+
+	static BroadPhase2DSW *_create();
+
+	BroadPhase2DHashGrid();
+	~BroadPhase2DHashGrid();
+
+
+};
+
+#endif // BROAD_PHASE_2D_HASH_GRID_H

servers/physics_2d/broad_phase_2d_sw.cpp

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

servers/physics_2d/broad_phase_2d_sw.h

@@ -0,0 +1,73 @@
+/*************************************************************************/
+/*  broad_phase_2d_sw.h                                                  */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef BROAD_PHASE_2D_SW_H
+#define BROAD_PHASE_2D_SW_H
+
+#include "math_funcs.h"
+#include "math_2d.h"
+
+class CollisionObject2DSW;
+
+
+class BroadPhase2DSW {
+
+public:
+	typedef BroadPhase2DSW* (*CreateFunction)();
+
+	static CreateFunction create_func;
+
+	typedef uint32_t ID;
+
+
+	typedef void* (*PairCallback)(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_userdata);
+	typedef void (*UnpairCallback)(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_userdata);
+
+	// 0 is an invalid ID
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0)=0;
+	virtual void move(ID p_id, const Rect2& p_aabb)=0;
+	virtual void set_static(ID p_id, bool p_static)=0;
+	virtual void remove(ID p_id)=0;
+
+	virtual CollisionObject2DSW *get_object(ID p_id) const=0;
+	virtual bool is_static(ID p_id) const=0;
+	virtual int get_subindex(ID p_id) const=0;
+
+	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+
+	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0;
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0;
+
+	virtual void update()=0;
+
+	virtual ~BroadPhase2DSW();
+
+};
+
+#endif // BROAD_PHASE_2D_SW_H

servers/physics_2d/collision_object_2d_sw.cpp

@@ -0,0 +1,220 @@
+/*************************************************************************/
+/*  collision_object_2d_sw.cpp                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "collision_object_2d_sw.h"
+#include "space_2d_sw.h"
+
+void CollisionObject2DSW::add_shape(Shape2DSW *p_shape,const Matrix32& p_transform) {
+
+	Shape s;
+	s.shape=p_shape;
+	s.xform=p_transform;
+	s.xform_inv=s.xform.affine_inverse();
+	s.bpid=0; //needs update
+	s.trigger=false;
+	shapes.push_back(s);
+	p_shape->add_owner(this);
+	_update_shapes();
+	_shapes_changed();
+
+}
+
+void CollisionObject2DSW::set_shape(int p_index,Shape2DSW *p_shape){
+
+	ERR_FAIL_INDEX(p_index,shapes.size());
+	shapes[p_index].shape->remove_owner(this);
+	shapes[p_index].shape=p_shape;
+
+	p_shape->add_owner(this);
+	_update_shapes();
+	_shapes_changed();
+
+}
+void CollisionObject2DSW::set_shape_transform(int p_index,const Matrix32& p_transform){
+
+	ERR_FAIL_INDEX(p_index,shapes.size());
+
+	shapes[p_index].xform=p_transform;
+	shapes[p_index].xform_inv=p_transform.affine_inverse();
+	_update_shapes();
+	_shapes_changed();
+}
+
+void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) {
+
+	//remove a shape, all the times it appears
+	for(int i=0;i<shapes.size();i++) {
+
+		if (shapes[i].shape==p_shape) {
+			remove_shape(i);
+			i--;
+		}
+	}
+}
+
+void CollisionObject2DSW::remove_shape(int p_index){
+
+	//remove anything from shape to be erased to end, so subindices don't change
+	ERR_FAIL_INDEX(p_index,shapes.size());
+	for(int i=p_index;i<shapes.size();i++) {
+
+		if (shapes[i].bpid==0)
+			continue;
+		//should never get here with a null owner
+		space->get_broadphase()->remove(shapes[i].bpid);
+		shapes[i].bpid=0;
+	}
+	shapes[p_index].shape->remove_owner(this);
+	shapes.remove(p_index);
+
+	_shapes_changed();
+
+}
+
+void CollisionObject2DSW::_set_static(bool p_static) {
+	if (_static==p_static)
+		return;
+	_static=p_static;
+
+	if (!space)
+		return;
+	for(int i=0;i<get_shape_count();i++) {
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+	}
+
+}
+
+void CollisionObject2DSW::_unregister_shapes() {
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			space->get_broadphase()->remove(s.bpid);
+			s.bpid=0;
+		}
+	}
+
+}
+
+void CollisionObject2DSW::_update_shapes() {
+
+	if (!space)
+		return;
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid==0) {
+			s.bpid=space->get_broadphase()->create(this,i);
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+
+		//not quite correct, should compute the next matrix..
+		Rect2 shape_aabb=s.shape->get_aabb();
+		Matrix32 xform = transform * s.xform;
+		shape_aabb=xform.xform(shape_aabb);
+		s.aabb_cache=shape_aabb;
+		s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 );
+
+
+		space->get_broadphase()->move(s.bpid,s.aabb_cache);
+	}
+
+}
+
+void CollisionObject2DSW::_update_shapes_with_motion(const Vector2& p_motion) {
+
+
+	if (!space)
+		return;
+
+	for(int i=0;i<shapes.size();i++) {
+
+		Shape &s=shapes[i];
+		if (s.bpid==0) {
+			s.bpid=space->get_broadphase()->create(this,i);
+			space->get_broadphase()->set_static(s.bpid,_static);
+		}
+
+		//not quite correct, should compute the next matrix..
+		Rect2 shape_aabb=s.shape->get_aabb();
+		Matrix32 xform = transform * s.xform;
+		shape_aabb=xform.xform(shape_aabb);
+		shape_aabb=shape_aabb.merge(Rect2( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion
+		s.aabb_cache=shape_aabb;
+
+		space->get_broadphase()->move(s.bpid,shape_aabb);
+	}
+
+
+}
+
+void CollisionObject2DSW::_set_space(Space2DSW *p_space) {
+
+	if (space) {
+
+		space->remove_object(this);
+
+		for(int i=0;i<shapes.size();i++) {
+
+			Shape &s=shapes[i];
+			if (s.bpid) {
+				space->get_broadphase()->remove(s.bpid);
+				s.bpid=0;
+			}
+		}
+
+	}
+
+	space=p_space;
+
+	if (space) {
+
+		space->add_object(this);
+		_update_shapes();
+	}
+
+}
+
+void CollisionObject2DSW::_shape_changed() {
+
+	_update_shapes();
+	_shapes_changed();
+}
+
+CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
+
+	_static=true;
+	type=p_type;
+	space=NULL;
+	instance_id=0;
+}

servers/physics_2d/collision_object_2d_sw.h

@@ -0,0 +1,123 @@
+/*************************************************************************/
+/*  collision_object_2d_sw.h                                             */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef COLLISION_OBJECT_2D_SW_H
+#define COLLISION_OBJECT_2D_SW_H
+
+#include "shape_2d_sw.h"
+#include "servers/physics_2d_server.h"
+#include "self_list.h"
+#include "broad_phase_2d_sw.h"
+
+class Space2DSW;
+
+class CollisionObject2DSW : public ShapeOwner2DSW {
+public:
+	enum Type {
+		TYPE_AREA,
+		TYPE_BODY
+	};
+private:
+
+	Type type;
+	RID self;
+	ObjectID instance_id;
+
+	struct Shape {
+
+		Matrix32 xform;
+		Matrix32 xform_inv;
+		BroadPhase2DSW::ID bpid;
+		Rect2 aabb_cache; //for rayqueries
+		Shape2DSW *shape;
+		bool trigger;
+		Shape() { trigger=false; }
+	};
+
+	Vector<Shape> shapes;
+	Space2DSW *space;
+	Matrix32 transform;
+	Matrix32 inv_transform;
+	bool _static;
+
+	void _update_shapes();
+
+protected:
+
+
+	void _update_shapes_with_motion(const Vector2& p_motion);
+	void _unregister_shapes();
+
+	_FORCE_INLINE_ void _set_transform(const Matrix32& p_transform) { transform=p_transform; _update_shapes(); }
+	_FORCE_INLINE_ void _set_inv_transform(const Matrix32& p_transform) { inv_transform=p_transform; }
+	void _set_static(bool p_static);
+
+	virtual void _shapes_changed()=0;
+	void _set_space(Space2DSW *space);
+
+	CollisionObject2DSW(Type p_type);
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ void set_instance_id(const ObjectID& p_instance_id) { instance_id=p_instance_id; }
+	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
+
+	void _shape_changed();
+
+	_FORCE_INLINE_ Type get_type() const { return type; }
+	void add_shape(Shape2DSW *p_shape,const Matrix32& p_transform=Matrix32());
+	void set_shape(int p_index,Shape2DSW *p_shape);
+	void set_shape_transform(int p_index,const Matrix32& p_transform);
+	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
+	_FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const { return shapes[p_index].shape; }
+	_FORCE_INLINE_ const Matrix32& get_shape_transform(int p_index) const { return shapes[p_index].xform; }
+	_FORCE_INLINE_ const Matrix32& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
+	_FORCE_INLINE_ const Rect2& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
+
+	_FORCE_INLINE_ Matrix32 get_transform() const { return transform; }
+	_FORCE_INLINE_ Matrix32 get_inv_transform() const { return inv_transform; }
+	_FORCE_INLINE_ Space2DSW* get_space() const { return space; }
+
+	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx,bool p_enable) { shapes[p_idx].trigger=p_enable; }
+	_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
+
+
+	void remove_shape(Shape2DSW *p_shape);
+	void remove_shape(int p_index);
+
+	virtual void set_space(Space2DSW *p_space)=0;
+
+	_FORCE_INLINE_ bool is_static() const { return _static;  }
+
+	virtual ~CollisionObject2DSW() {}
+
+};
+
+#endif // COLLISION_OBJECT_2D_SW_H

servers/physics_2d/collision_solver_2d_sat.h

@@ -0,0 +1,37 @@
+/*************************************************************************/
+/*  collision_solver_2d_sat.h                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef COLLISION_SOLVER_2D_SAT_H
+#define COLLISION_SOLVER_2D_SAT_H
+
+#include "collision_solver_2d_sw.h"
+
+
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL);
+
+#endif // COLLISION_SOLVER_2D_SAT_H

servers/physics_2d/collision_solver_2d_sw.cpp

@@ -0,0 +1,309 @@
+/*************************************************************************/
+/*  collision_solver_2d_sw.cpp                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "collision_solver_2d_sw.h"
+#include "collision_solver_2d_sat.h"
+
+
+#define collision_solver sat_2d_calculate_penetration
+//#define collision_solver gjk_epa_calculate_penetration
+
+
+bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+
+	const LineShape2DSW *line = static_cast<const LineShape2DSW*>(p_shape_A);
+	if (p_shape_B->get_type()==Physics2DServer::SHAPE_LINE)
+		return false;
+
+
+	Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized();
+	Vector2 p = p_transform_A.xform(line->get_normal()*line->get_d());
+	real_t d = n.dot(p);
+
+	Vector2 supports[2];
+	int support_count;
+
+	p_shape_B->get_supports(p_transform_inv_B.basis_xform(-n).normalized(),supports,support_count);
+
+	bool found=false;
+
+
+	for(int i=0;i<support_count;i++) {
+
+		supports[i] = p_transform_B.xform( supports[i] );
+		real_t pd = n.dot(supports[i]);
+		if (pd>=d)
+			continue;
+		found=true;
+
+		Vector2 support_A = supports[i] - n*(pd-d);
+
+		if (p_result_callback) {
+			if (p_swap_result)
+				p_result_callback(supports[i],support_A,p_userdata);
+			else
+				p_result_callback(support_A,supports[i],p_userdata);
+		}
+
+	}
+
+
+	return found;
+}
+
+bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
+
+
+
+	const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A);
+	if (p_shape_B->get_type()==Physics2DServer::SHAPE_RAY)
+		return false;
+
+	Vector2 from = p_transform_A.get_origin();
+	Vector2 to = from+p_transform_A[1]*ray->get_length();
+	Vector2 support_A=to;
+
+	from = p_transform_inv_B.xform(from);
+	to = p_transform_inv_B.xform(to);
+
+	Vector2 p,n;
+	if (!p_shape_B->intersect_segment(from,to,p,n)) {
+
+		if (sep_axis)
+			*sep_axis=p_transform_A[1].normalized();
+		return false;
+	}
+
+
+	Vector2 support_B=p_transform_B.xform(p);
+
+	if (p_result_callback) {
+		if (p_swap_result)
+			p_result_callback(support_B,support_A,p_userdata);
+		else
+			p_result_callback(support_A,support_B,p_userdata);
+	}
+	return true;
+
+}
+
+/*
+bool CollisionSolver2DSW::solve_ray(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_inverse_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+
+
+	const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A);
+
+	Vector2 from = p_transform_A.origin;
+	Vector2 to = from+p_transform_A.basis.get_axis(2)*ray->get_length();
+	Vector2 support_A=to;
+
+	from = p_inverse_B.xform(from);
+	to = p_inverse_B.xform(to);
+
+	Vector2 p,n;
+	if (!p_shape_B->intersect_segment(from,to,&p,&n))
+		return false;
+
+	Vector2 support_B=p_transform_B.xform(p);
+
+	if (p_result_callback) {
+		if (p_swap_result)
+			p_result_callback(support_B,support_A,p_userdata);
+		else
+			p_result_callback(support_A,support_B,p_userdata);
+	}
+	return true;
+}
+*/
+
+struct _ConcaveCollisionInfo2D {
+
+	const Matrix32 *transform_A;
+	const Matrix32 *transform_inv_A;
+	const Shape2DSW *shape_A;
+	const Matrix32 *transform_B;
+	const Matrix32 *transform_inv_B;
+	CollisionSolver2DSW::CallbackResult result_callback;
+	void *userdata;
+	bool swap_result;
+	bool collided;
+	int aabb_tests;
+	int collisions;
+	Vector2 *sep_axis;
+
+};
+
+void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) {
+
+
+
+	_ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D*)(p_userdata);
+	cinfo.aabb_tests++;
+	if (!cinfo.result_callback && cinfo.collided)
+		return; //already collided and no contacts requested, don't test anymore
+
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, *cinfo.transform_inv_A, p_convex,*cinfo.transform_B,*cinfo.transform_inv_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis );
+	if (!collided)
+		return;
+
+
+	cinfo.collided=true;
+	cinfo.collisions++;
+
+}
+
+bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
+
+
+	const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B);
+
+	_ConcaveCollisionInfo2D cinfo;
+	cinfo.transform_A=&p_transform_A;
+	cinfo.transform_inv_A=&p_transform_inv_A;
+	cinfo.shape_A=p_shape_A;
+	cinfo.transform_B=&p_transform_B;
+	cinfo.transform_inv_B=&p_transform_inv_B;
+	cinfo.result_callback=p_result_callback;
+	cinfo.userdata=p_userdata;
+	cinfo.swap_result=p_swap_result;
+	cinfo.collided=false;
+	cinfo.collisions=0;
+	cinfo.sep_axis=sep_axis;
+
+	cinfo.aabb_tests=0;
+
+	Matrix32 rel_transform = p_transform_A;
+	rel_transform.elements[2]-=p_transform_B.elements[2];
+
+	//quickly compute a local Rect2
+
+	Rect2 local_aabb;
+	for(int i=0;i<2;i++) {
+
+	     Vector2 axis( p_transform_B.elements[i] );
+	     float axis_scale = 1.0/axis.length();
+	     axis*=axis_scale;
+
+	     float smin,smax;
+	     p_shape_A->project_rangev(axis,rel_transform,smin,smax);
+	     smin*=axis_scale;
+	     smax*=axis_scale;
+
+	     local_aabb.pos[i]=smin;
+	     local_aabb.size[i]=smax-smin;
+	}
+
+	concave_B->cull(local_aabb,concave_callback,&cinfo);
+
+
+//	print_line("Rect2 TESTS: "+itos(cinfo.aabb_tests));	
+	return cinfo.collided;
+}
+
+
+bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) {
+
+
+
+	Physics2DServer::ShapeType type_A=p_shape_A->get_type();
+	Physics2DServer::ShapeType type_B=p_shape_B->get_type();
+	bool concave_A=p_shape_A->is_concave();
+	bool concave_B=p_shape_B->is_concave();
+
+	bool swap = false;
+
+	if (type_A>type_B) {
+		SWAP(type_A,type_B);
+		SWAP(concave_A,concave_B);
+		swap=true;
+	}
+
+	if (type_A==Physics2DServer::SHAPE_LINE) {
+
+		if (type_B==Physics2DServer::SHAPE_LINE || type_B==Physics2DServer::SHAPE_RAY) {
+			return false;
+		//if (type_B==Physics2DServer::SHAPE_RAY) {
+		//	return false;
+		}
+
+		if (swap) {
+			return solve_static_line(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true);
+		} else {
+			return solve_static_line(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false);
+		}
+
+	/*} else if (type_A==Physics2DServer::SHAPE_RAY) {
+
+		if (type_B==Physics2DServer::SHAPE_RAY)
+			return false;
+
+		if (swap) {
+			return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_inverse_A,p_result_callback,p_userdata,true);
+		} else {
+			return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_inverse_B,p_result_callback,p_userdata,false);
+		}
+*/
+	} else if (type_A==Physics2DServer::SHAPE_RAY) {
+
+		if (type_B==Physics2DServer::SHAPE_RAY) {
+
+			return false; //no ray-ray
+		}
+
+
+		if (swap) {
+			return solve_raycast(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis);
+		} else {
+			return solve_raycast(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis);
+		}
+
+
+	} else if (concave_B) {
+
+
+		if (concave_A)
+			return false;
+
+		if (!swap)
+			return solve_concave(p_shape_A,p_transform_A,p_transform_inv_A,p_shape_B,p_transform_B,p_transform_inv_B,p_result_callback,p_userdata,false,sep_axis);
+		else
+			return solve_concave(p_shape_B,p_transform_B,p_transform_inv_B,p_shape_A,p_transform_A,p_transform_inv_A,p_result_callback,p_userdata,true,sep_axis);
+
+
+
+	} else {
+
+
+		return collision_solver(p_shape_A, p_transform_A, p_transform_inv_A, p_shape_B, p_transform_B, p_transform_inv_B, p_result_callback,p_userdata,false,sep_axis);
+	}
+
+
+	return false;
+}
+

servers/physics_2d/collision_solver_2d_sw.h

@@ -0,0 +1,52 @@
+/*************************************************************************/
+/*  collision_solver_2d_sw.h                                             */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef COLLISION_SOLVER_2D_SW_H
+#define COLLISION_SOLVER_2D_SW_H
+
+#include "shape_2d_sw.h"
+
+class CollisionSolver2DSW {
+public:
+	typedef void (*CallbackResult)(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
+private:
+	static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+	static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
+	static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
+	static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_transform_inv_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_transform_inv_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
+
+
+
+public:
+
+	static bool solve_static(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Matrix32& p_inverse_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Matrix32& p_inverse_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL);
+
+
+};
+
+#endif // COLLISION_SOLVER_2D_SW_H

servers/physics_2d/constraint_2d_sw.cpp

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

servers/physics_2d/constraint_2d_sw.h

@@ -0,0 +1,72 @@
+/*************************************************************************/
+/*  constraint_2d_sw.h                                                   */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef CONSTRAINT_2D_SW_H
+#define CONSTRAINT_2D_SW_H
+
+#include "body_2d_sw.h"
+
+class Constraint2DSW {
+
+	Body2DSW **_body_ptr;
+	int _body_count;
+	uint64_t island_step;
+	Constraint2DSW *island_next;
+	Constraint2DSW *island_list_next;
+
+
+	RID self;
+
+protected:
+	Constraint2DSW(Body2DSW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; }
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+
+
+	_FORCE_INLINE_ Constraint2DSW* get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(Constraint2DSW* p_next) { island_next=p_next; }
+
+	_FORCE_INLINE_ Constraint2DSW* get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(Constraint2DSW* p_next) { island_list_next=p_next; }
+
+	_FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; }
+	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
+
+
+	virtual bool setup(float p_step)=0;
+	virtual void solve(float p_step)=0;
+
+	virtual ~Constraint2DSW() {}
+};
+
+#endif // CONSTRAINT_2D_SW_H

servers/physics_2d/joints_2d_sw.cpp

@@ -0,0 +1,574 @@
+/*************************************************************************/
+/*  joints_2d_sw.cpp                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "joints_2d_sw.h"
+#include "space_2d_sw.h"
+
+//based on chipmunk joint constraints
+
+/* Copyright (c) 2007 Scott Lembcke
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+static inline real_t k_scalar(Body2DSW *a,Body2DSW *b,const Vector2& rA, const Vector2& rB, const Vector2& n) {
+
+
+	real_t value=0;
+
+
+	{
+		value+=a->get_inv_mass();
+		real_t rcn = rA.cross(n);
+		value+=a->get_inv_inertia() * rcn * rcn;
+	}
+
+	if (b) {
+
+		value+=b->get_inv_mass();
+		real_t rcn = rB.cross(n);
+		value+=b->get_inv_inertia() * rcn * rcn;
+	}
+
+	return value;
+
+}
+
+static inline Vector2
+relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB){
+	Vector2 sum = a->get_linear_velocity() -rA.tangent() * a->get_angular_velocity();
+	if (b)
+		return (b->get_linear_velocity() -rB.tangent() * b->get_angular_velocity()) - sum;
+	else
+		return -sum;
+}
+
+static inline real_t
+normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n){
+	return relative_velocity(a, b, rA, rB).dot(n);
+}
+
+#if 0
+
+bool PinJoint2DSW::setup(float p_step) {
+
+	Space2DSW *space = A->get_space();
+	ERR_FAIL_COND_V(!space,false;)
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B?B->get_transform().basis_xform(anchor_B):anchor_B;
+
+	Vector2 gA = A->get_transform().get_origin();
+	Vector2 gB = B?B->get_transform().get_origin():Vector2();
+
+	Vector2 delta = gB - gA;
+	delta = (delta+rB) -rA;
+
+	real_t jdist = delta.length();
+	correct=false;
+	if (jdist==0)
+		return false; // do not correct
+
+	correct=true;
+
+	n = delta / jdist;
+
+	// calculate mass normal
+	mass_normal = 1.0f/k_scalar(A, B, rA, rB, n);
+
+	// calculate bias velocity
+	//real_t maxBias = joint->constraint.maxBias;
+	bias = -(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)*(jdist-dist);
+	bias = CLAMP(bias, -get_max_bias(), +get_max_bias());
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// apply accumulated impulse
+	Vector2 j = n * jn_acc;
+	A->apply_impulse(rA,-j);
+	if (B)
+		B->apply_impulse(rB,j);
+
+	print_line("setup");
+	return true;
+}
+
+
+
+void PinJoint2DSW::solve(float p_step){
+
+	if (!correct)
+		return;
+
+	Vector2 ln = n;
+
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A,B, rA, rB, ln);
+
+	// compute normal impulse
+	real_t jn = (bias - vrn)*mass_normal;
+	real_t jnOld = jn_acc;
+	jn_acc = CLAMP(jnOld + jn,-jn_max,jn_max); //cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
+	jn = jn_acc - jnOld;
+	print_line("jn_acc: "+rtos(jn_acc));
+	Vector2 j = jn*ln;
+
+	A->apply_impulse(rA,-j);
+	if (B)
+		B->apply_impulse(rB,j);
+
+}
+
+
+PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) {
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
+	anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos;
+
+	jn_acc=0;
+	dist=0;
+
+	p_body_a->add_constraint(this,0);
+	if (p_body_b)
+		p_body_b->add_constraint(this,1);
+
+}
+
+PinJoint2DSW::~PinJoint2DSW() {
+
+	if (A)
+		A->remove_constraint(this);
+	if (B)
+		B->remove_constraint(this);
+
+}
+
+#else
+
+
+bool PinJoint2DSW::setup(float p_step) {
+
+	Space2DSW *space = A->get_space();
+	ERR_FAIL_COND_V(!space,false;)
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B?B->get_transform().basis_xform(anchor_B):anchor_B;
+#if 0
+	Vector2 gA = rA+A->get_transform().get_origin();
+	Vector2 gB = B?rB+B->get_transform().get_origin():rB;
+
+	VectorB delta = gB - gA;
+
+	real_t jdist = delta.length();
+	correct=false;
+	if (jdist==0)
+		return false; // do not correct
+#endif
+
+	// deltaV = deltaV0 + K * impulse
+	// invM = [(1/m1 + 1/m2) * eye(2) - skew(rA) * invI1 * skew(rA) - skew(rB) * invI2 * skew(rB)]
+	//      = [1/m1+1/m2     0    ] + invI1 * [rA.y*rA.y -rA.x*rA.y] + invI2 * [rA.y*rA.y -rA.x*rA.y]
+	//        [    0     1/m1+1/m2]           [-rA.x*rA.y rA.x*rA.x]           [-rA.x*rA.y rA.x*rA.x]
+
+	real_t B_inv_mass = B?B->get_inv_mass():0.0;
+
+
+	Matrix32 K1;
+	K1[0].x = A->get_inv_mass() + B_inv_mass;	K1[1].x = 0.0f;
+	K1[0].y = 0.0f;					K1[1].y = A->get_inv_mass() + B_inv_mass;
+
+	Matrix32 K2;
+	K2[0].x =  A->get_inv_inertia() * rA.y * rA.y;		K2[1].x = -A->get_inv_inertia() * rA.x * rA.y;
+	K2[0].y = -A->get_inv_inertia() * rA.x * rA.y;		K2[1].y =  A->get_inv_inertia() * rA.x * rA.x;
+
+	Matrix32 K;
+	K[0]= K1[0] + K2[0];
+	K[1]= K1[1] + K2[1];
+
+	if (B) {
+
+		Matrix32 K3;
+		K3[0].x =  B->get_inv_inertia() * rB.y * rB.y;		K3[1].x = -B->get_inv_inertia() * rB.x * rB.y;
+		K3[0].y = -B->get_inv_inertia() * rB.x * rB.y;		K3[1].y =  B->get_inv_inertia() * rB.x * rB.x;
+
+		K[0]+=K3[0];
+		K[1]+=K3[1];
+	}
+
+	K[0].x += softness;
+	K[1].y += softness;
+
+	M = K.affine_inverse();
+
+	Vector2 gA = rA+A->get_transform().get_origin();
+	Vector2 gB = B?rB+B->get_transform().get_origin():rB;
+
+	Vector2 delta = gB - gA;
+
+	bias = delta*-(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step);
+
+	// apply accumulated impulse
+	A->apply_impulse(rA,-P);
+	if (B)
+		B->apply_impulse(rB,P);
+
+	return true;
+}
+
+void PinJoint2DSW::solve(float p_step){
+
+
+	// compute relative velocity
+	Vector2 vA = A->get_linear_velocity() - rA.cross(A->get_angular_velocity());
+
+	Vector2 rel_vel;
+	if (B)
+		rel_vel = B->get_linear_velocity() - rB.cross(B->get_angular_velocity()) - vA;
+	else
+		rel_vel = -vA;
+
+	Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness,softness) * P);
+
+	A->apply_impulse(rA,-impulse);
+	if (B)
+		B->apply_impulse(rB,impulse);
+
+
+	P += impulse;
+}
+
+
+PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) {
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
+	anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos;
+
+	softness=0;
+
+	p_body_a->add_constraint(this,0);
+	if (p_body_b)
+		p_body_b->add_constraint(this,1);
+
+}
+
+
+
+PinJoint2DSW::~PinJoint2DSW() {
+
+	if (A)
+		A->remove_constraint(this);
+	if (B)
+		B->remove_constraint(this);
+
+}
+
+
+
+#endif
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+
+static inline void
+k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2)
+{
+	// calculate mass matrix
+	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
+	real_t k11, k12, k21, k22;
+	real_t m_sum = a->get_inv_mass() + b->get_inv_mass();
+
+	// start with I*m_sum
+	k11 = m_sum; k12 = 0.0f;
+	k21 = 0.0f;  k22 = m_sum;
+
+	// add the influence from r1
+	real_t a_i_inv = a->get_inv_inertia();
+	real_t r1xsq =  r1.x * r1.x * a_i_inv;
+	real_t r1ysq =  r1.y * r1.y * a_i_inv;
+	real_t r1nxy = -r1.x * r1.y * a_i_inv;
+	k11 += r1ysq; k12 += r1nxy;
+	k21 += r1nxy; k22 += r1xsq;
+
+	// add the influnce from r2
+	real_t b_i_inv = b->get_inv_inertia();
+	real_t r2xsq =  r2.x * r2.x * b_i_inv;
+	real_t r2ysq =  r2.y * r2.y * b_i_inv;
+	real_t r2nxy = -r2.x * r2.y * b_i_inv;
+	k11 += r2ysq; k12 += r2nxy;
+	k21 += r2nxy; k22 += r2xsq;
+
+	// invert
+	real_t determinant = k11*k22 - k12*k21;
+	ERR_FAIL_COND(determinant== 0.0);
+
+	real_t det_inv = 1.0f/determinant;
+	*k1 = Vector2( k22*det_inv, -k12*det_inv);
+	*k2 = Vector2(-k21*det_inv,  k11*det_inv);
+}
+
+static _FORCE_INLINE_ Vector2
+mult_k(const Vector2& vr, const Vector2 &k1, const Vector2 &k2)
+{
+	return Vector2(vr.dot(k1), vr.dot(k2));
+}
+
+bool GrooveJoint2DSW::setup(float p_step) {
+
+
+	// calculate endpoints in worldspace
+	Vector2 ta = A->get_transform().xform(A_groove_1);
+	Vector2 tb = A->get_transform().xform(A_groove_2);
+	Space2DSW *space=A->get_space();
+
+	// calculate axis
+	Vector2 n = -(tb - ta).tangent().normalized();
+	real_t d = ta.dot(n);
+
+	xf_normal = n;
+	rB = B->get_transform().basis_xform(B_anchor);
+
+	// calculate tangential distance along the axis of rB
+	real_t td = (B->get_transform().get_origin() + rB).cross(n);
+	// calculate clamping factor and rB
+	if(td <= ta.cross(n)){
+		clamp = 1.0f;
+		rA = ta - A->get_transform().get_origin();
+	} else if(td >= tb.cross(n)){
+		clamp = -1.0f;
+		rA = tb - A->get_transform().get_origin();
+	} else {
+		clamp = 0.0f;
+		//joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
+		rA =  ((-n.tangent() * -td) + n*d) - A->get_transform().get_origin();
+	}
+
+	// Calculate mass tensor
+	k_tensor(A, B, rA, rB, &k1, &k2);
+
+	// compute max impulse
+	jn_max = get_max_force() * p_step;
+
+	// calculate bias velocity
+//	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+//	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
+
+
+	Vector2 delta = (B->get_transform().get_origin() +rB) - (A->get_transform().get_origin() + rA);
+	float _b = get_bias();
+	_b=0.001;
+	gbias=(delta*-(_b==0?space->get_constraint_bias():_b)*(1.0/p_step)).clamped(get_max_bias());
+
+	// apply accumulated impulse
+	A->apply_impulse(rA,-jn_acc);
+	B->apply_impulse(rB,jn_acc);
+
+	correct=true;
+	return true;
+}
+
+void GrooveJoint2DSW::solve(float p_step){
+
+
+	// compute impulse
+	Vector2 vr = relative_velocity(A, B, rA,rB);
+
+	Vector2 j = mult_k(gbias-vr, k1, k2);
+	Vector2 jOld = jn_acc;
+	j+=jOld;
+
+	jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : xf_normal.project(j)).clamped(jn_max);
+
+	j = jn_acc - jOld;
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+}
+
+
+GrooveJoint2DSW::GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
+
+	A=p_body_a;
+	B=p_body_b;
+
+	A_groove_1 = A->get_inv_transform().xform(p_a_groove1);
+	A_groove_2 = A->get_inv_transform().xform(p_a_groove2);
+	B_anchor=B->get_inv_transform().xform(p_b_anchor);
+	A_groove_normal = -(A_groove_2 - A_groove_1).normalized().tangent();
+
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+
+}
+
+GrooveJoint2DSW::~GrooveJoint2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+}
+
+
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+//////////////////////////////////////////////
+
+
+bool DampedSpringJoint2DSW::setup(float p_step) {
+
+	rA = A->get_transform().basis_xform(anchor_A);
+	rB = B->get_transform().basis_xform(anchor_B);
+
+	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA) ;
+	real_t dist = delta.length();
+
+	if (dist)
+		n=delta/dist;
+	else
+		n=Vector2();
+
+	real_t k = k_scalar(A, B, rA, rB, n);
+	n_mass = 1.0f/k;
+
+	target_vrn = 0.0f;
+	v_coef = 1.0f - Math::exp(-damping*(p_step)*k);
+
+	// apply spring force
+	real_t f_spring = (rest_length - dist) * stiffness;
+	Vector2 j = n * f_spring*(p_step);
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+
+
+	return true;
+}
+
+void DampedSpringJoint2DSW::solve(float p_step) {
+
+	// compute relative velocity
+	real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn;
+
+	// compute velocity loss from drag
+	// not 100% certain this is derived correctly, though it makes sense
+	real_t v_damp = -vrn*v_coef;
+	target_vrn = vrn + v_damp;
+	Vector2 j=n*v_damp*n_mass;
+
+	A->apply_impulse(rA,-j);
+	B->apply_impulse(rB,j);
+
+}
+
+void DampedSpringJoint2DSW::set_param(Physics2DServer::DampedStringParam p_param, real_t p_value) {
+
+	switch(p_param) {
+
+		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
+
+			rest_length=p_value;
+		} break;
+		case Physics2DServer::DAMPED_STRING_DAMPING: {
+
+			damping=p_value;
+		} break;
+		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
+
+			stiffness=p_value;
+		} break;
+	}
+
+}
+
+real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const{
+
+	switch(p_param) {
+
+		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
+
+			return rest_length;
+		} break;
+		case Physics2DServer::DAMPED_STRING_DAMPING: {
+
+			return damping;
+		} break;
+		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
+
+			return stiffness;
+		} break;
+	}
+
+	ERR_FAIL_V(0);
+}
+
+
+DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
+
+
+	A=p_body_a;
+	B=p_body_b;
+	anchor_A = A->get_inv_transform().xform(p_anchor_a);
+	anchor_B = B->get_inv_transform().xform(p_anchor_b);
+
+	rest_length=p_anchor_a.distance_to(p_anchor_b);
+	stiffness=20;
+	damping=1.5;
+
+
+	A->add_constraint(this,0);
+	B->add_constraint(this,1);
+
+}
+
+DampedSpringJoint2DSW::~DampedSpringJoint2DSW() {
+
+	A->remove_constraint(this);
+	B->remove_constraint(this);
+
+}
+
+

servers/physics_2d/joints_2d_sw.h

@@ -0,0 +1,210 @@
+/*************************************************************************/
+/*  joints_2d_sw.h                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef JOINTS_2D_SW_H
+#define JOINTS_2D_SW_H
+
+#include "constraint_2d_sw.h"
+#include "body_2d_sw.h"
+
+
+
+class Joint2DSW : public Constraint2DSW {
+
+	real_t max_force;
+	real_t bias;
+	real_t max_bias;
+public:
+
+	_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; }
+	_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
+
+	_FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; }
+	_FORCE_INLINE_ real_t get_bias() const { return bias; }
+
+	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; }
+	_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
+
+	virtual Physics2DServer::JointType get_type() const=0;
+	Joint2DSW(Body2DSW **p_body_ptr=NULL,int p_body_count=0) : Constraint2DSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; };
+
+};
+#if 0
+
+class PinJoint2DSW : public Joint2DSW {
+
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+	real_t dist;
+	real_t jn_acc;
+	real_t jn_max;
+	real_t max_distance;
+	real_t mass_normal;
+	real_t bias;
+
+	Vector2 rA,rB;
+	Vector2 n; //normal
+	bool correct;
+
+
+public:
+
+	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_PIN; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b=NULL);
+	~PinJoint2DSW();
+};
+
+#else
+
+class PinJoint2DSW : public Joint2DSW {
+
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+	Matrix32 M;
+	Vector2 rA,rB;
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+	Vector2 bias;
+	Vector2 P;
+	real_t softness;
+
+public:
+
+	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_PIN; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b=NULL);
+	~PinJoint2DSW();
+};
+
+
+#endif
+class GrooveJoint2DSW : public Joint2DSW {
+
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+	Vector2 A_groove_1;
+	Vector2 A_groove_2;
+	Vector2 A_groove_normal;
+	Vector2 B_anchor;
+	Vector2 jn_acc;
+	Vector2 gbias;
+	real_t jn_max;
+	real_t clamp;
+	Vector2 xf_normal;
+	Vector2 rA,rB;
+	Vector2 k1,k2;
+
+
+	bool correct;
+
+public:
+
+	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_GROOVE; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+
+	GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b);
+	~GrooveJoint2DSW();
+};
+
+
+class DampedSpringJoint2DSW : public Joint2DSW {
+
+	union {
+		struct {
+			Body2DSW *A;
+			Body2DSW *B;
+		};
+
+		Body2DSW *_arr[2];
+	};
+
+
+	Vector2 anchor_A;
+	Vector2 anchor_B;
+
+	real_t rest_length;
+	real_t damping;
+	real_t stiffness;
+
+	Vector2 rA,rB;
+	Vector2 n;
+	real_t n_mass;
+	real_t target_vrn;
+	real_t v_coef;
+
+public:
+
+	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_DAMPED_SPRING; }
+
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
+
+	void set_param(Physics2DServer::DampedStringParam p_param, real_t p_value);
+	real_t get_param(Physics2DServer::DampedStringParam p_param) const;
+
+	DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b);
+	~DampedSpringJoint2DSW();
+};
+
+
+#endif // JOINTS_2D_SW_H

servers/physics_2d/physics_2d_server_sw.h

@@ -0,0 +1,215 @@
+/*************************************************************************/
+/*  physics_2d_server_sw.h                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef PHYSICS_2D_SERVER_SW
+#define PHYSICS_2D_SERVER_SW
+
+
+#include "servers/physics_2d_server.h"
+#include "shape_2d_sw.h"
+#include "space_2d_sw.h"
+#include "step_2d_sw.h"
+#include "joints_2d_sw.h"
+
+
+class Physics2DServerSW : public Physics2DServer {
+
+	OBJ_TYPE( Physics2DServerSW, Physics2DServer );
+
+friend class Physics2DDirectSpaceStateSW;
+	bool active;
+	int iterations;
+	bool doing_sync;
+	real_t last_step;
+
+	Step2DSW *stepper;
+	Set<const Space2DSW*> active_spaces;
+
+	Physics2DDirectBodyStateSW *direct_state;
+
+	mutable RID_Owner<Shape2DSW> shape_owner;
+	mutable RID_Owner<Space2DSW> space_owner;
+	mutable RID_Owner<Area2DSW> area_owner;
+	mutable RID_Owner<Body2DSW> body_owner;
+	mutable RID_Owner<Joint2DSW> joint_owner;
+
+//	void _clear_query(Query2DSW *p_query);
+public:
+
+	virtual RID shape_create(ShapeType p_shape);
+	virtual void shape_set_data(RID p_shape, const Variant& p_data);
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
+
+	virtual ShapeType shape_get_type(RID p_shape) const;
+	virtual Variant shape_get_data(RID p_shape) const;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
+
+	/* SPACE API */
+
+	virtual RID space_create();
+	virtual void space_set_active(RID p_space,bool p_active);
+	virtual bool space_is_active(RID p_space) const;
+
+	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value);
+	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const;
+
+	// this function only works on fixed process, errors and returns null otherwise
+	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space);
+
+
+	/* AREA API */
+
+	virtual RID area_create();
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode);
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const;
+
+	virtual void area_set_space(RID p_area, RID p_space);
+	virtual RID area_get_space(RID p_area) const;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform=Matrix32());
+	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape);
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform);
+
+	virtual int area_get_shape_count(RID p_area) const;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
+	virtual Matrix32 area_get_shape_transform(RID p_area, int p_shape_idx) const;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx);
+	virtual void area_clear_shapes(RID p_area);
+
+	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID);
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const;
+
+	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value);
+	virtual void area_set_transform(RID p_area, const Matrix32& p_transform);
+
+	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
+	virtual Matrix32 area_get_transform(RID p_area) const;
+
+	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
+
+
+	/* BODY API */
+
+	// create a body of a given type
+	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false);
+
+	virtual void body_set_space(RID p_body, RID p_space);
+	virtual RID body_get_space(RID p_body) const;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode);
+	virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32());
+	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape);
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform);
+
+	virtual int body_get_shape_count(RID p_body) const;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
+	virtual Matrix32 body_get_shape_transform(RID p_body, int p_shape_idx) const;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx);
+	virtual void body_clear_shapes(RID p_body);
+
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable);
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const;
+
+	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID);
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable);
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const;
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
+
+	//advanced simulation
+	virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform);
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant);
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
+
+	virtual void body_set_applied_force(RID p_body, const Vector2& p_force);
+	virtual Vector2 body_get_applied_force(RID p_body) const;
+
+	virtual void body_set_applied_torque(RID p_body, float p_torque);
+	virtual float body_get_applied_torque(RID p_body) const;
+
+	virtual void body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse);
+	virtual void body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity);
+
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
+
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold);
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const;
+
+	virtual void body_set_omit_force_integration(RID p_body,bool p_omit);
+	virtual bool body_is_omitting_force_integration(RID p_body) const;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
+	virtual int body_get_max_contacts_reported(RID p_body) const;
+
+	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
+
+	/* JOINT API */
+
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
+	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const;
+
+	virtual RID pin_joint_create(const Vector2& p_pos,RID p_body_a,RID p_body_b=RID());
+	virtual RID groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b);
+	virtual RID damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b=RID());
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value);
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const;
+
+	virtual JointType joint_get_type(RID p_joint) const;
+
+	/* MISC */
+
+	virtual void free(RID p_rid);
+
+	virtual void set_active(bool p_active);
+	virtual void init();
+	virtual void step(float p_step);
+	virtual void sync();
+	virtual void flush_queries();
+	virtual void finish();
+
+	Physics2DServerSW();
+	~Physics2DServerSW();
+
+};
+
+#endif
+

servers/physics_2d/shape_2d_sw.h

@@ -0,0 +1,442 @@
+/*************************************************************************/
+/*  shape_2d_sw.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef SHAPE_2D_2DSW_H
+#define SHAPE_2D_2DSW_H
+
+#include "servers/physics_2d_server.h"
+/*
+
+SHAPE_LINE, ///< plane:"plane"
+SHAPE_SEGMENT, ///< float:"length"
+SHAPE_CIRCLE, ///< float:"radius"
+SHAPE_RECTANGLE, ///< vec3:"extents"
+SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array)
+SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+
+*/
+
+class Shape2DSW;
+
+class ShapeOwner2DSW {
+public:
+
+	virtual void _shape_changed()=0;
+	virtual void remove_shape(Shape2DSW *p_shape)=0;
+
+	virtual ~ShapeOwner2DSW() {}
+};
+
+class Shape2DSW {
+
+	RID self;
+	Rect2 aabb;
+	bool configured;
+	real_t custom_bias;
+
+	Map<ShapeOwner2DSW*,int> owners;
+protected:
+
+	void configure(const Rect2& p_aabb);
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const {return  self; }
+
+	virtual Physics2DServer::ShapeType get_type() const=0;
+
+	_FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
+	_FORCE_INLINE_ bool is_configured() const { return configured; }
+
+	virtual bool is_concave() const { return false; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
+	virtual Vector2 get_support(const Vector2& p_normal) const;
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0;
+	virtual real_t get_moment_of_inertia(float p_mass) const=0;
+
+	virtual void set_data(const Variant& p_data)=0;
+	virtual Variant get_data() const=0;
+
+	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
+	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
+
+	void add_owner(ShapeOwner2DSW *p_owner);
+	void remove_owner(ShapeOwner2DSW *p_owner);
+	bool is_owner(ShapeOwner2DSW *p_owner) const;
+	const Map<ShapeOwner2DSW*,int>& get_owners() const;
+
+	Shape2DSW();
+	virtual ~Shape2DSW();
+};
+
+
+class LineShape2DSW : public Shape2DSW {
+
+
+	Vector2 normal;
+	real_t d;
+
+public:
+
+	_FORCE_INLINE_ Vector2 get_normal() const { return normal; }
+	_FORCE_INLINE_ real_t get_d() const { return d; }
+
+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_LINE; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		r_min=-1e10;
+		r_max=1e10;
+	}
+
+};
+
+
+class RayShape2DSW : public Shape2DSW {
+
+
+	real_t length;
+
+public:
+
+
+	_FORCE_INLINE_  real_t get_length() const { return length; }
+
+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		r_max = p_normal.dot(p_transform.get_origin());
+		r_min = p_normal.dot(p_transform.xform(Vector2(0,length)));
+		if (r_max<r_min) {
+
+			SWAP(r_max,r_min);
+		}
+	}
+
+	_FORCE_INLINE_ RayShape2DSW() {}
+	_FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; }
+};
+
+
+class SegmentShape2DSW : public Shape2DSW {
+
+
+	Vector2 a;
+	Vector2 b;
+	Vector2 n;
+
+public:
+
+
+	_FORCE_INLINE_ const Vector2& get_a() const { return a; }
+	_FORCE_INLINE_ const Vector2& get_b() const { return b; }
+	_FORCE_INLINE_ const Vector2& get_normal() const { return n; }
+
+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_SEGMENT; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		r_max = p_normal.dot(p_transform.xform(a));
+		r_min = p_normal.dot(p_transform.xform(b));
+		if (r_max<r_min) {
+
+			SWAP(r_max,r_min);
+		}
+	}
+
+	_FORCE_INLINE_ SegmentShape2DSW() {}
+	_FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; }
+};
+
+
+class CircleShape2DSW : public Shape2DSW {
+
+
+	real_t radius;
+
+public:
+
+	_FORCE_INLINE_ const real_t& get_radius() const { return radius; }
+
+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CIRCLE; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+		//real large
+		real_t d = p_normal.dot( p_transform.get_origin() );
+
+		// figure out scale at point
+		Vector2 local_normal = p_transform.basis_xform_inv(p_normal);
+		real_t scale = local_normal.length();
+
+		r_min = d - (radius) * scale;
+		r_max = d + (radius) * scale;
+	}
+
+};
+
+
+
+class RectangleShape2DSW : public Shape2DSW {
+
+
+	Vector2 half_extents;
+
+public:
+
+	_FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; }
+
+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RECTANGLE; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+		// no matter the angle, the box is mirrored anyway
+		Vector2 local_normal=p_transform.basis_xform_inv(p_normal);
+
+		float length = local_normal.abs().dot(half_extents);
+		float distance = p_normal.dot( p_transform.get_origin() );
+
+		r_min = distance - length;
+		r_max = distance + length;
+	}
+
+};
+
+class CapsuleShape2DSW : public Shape2DSW {
+
+
+	real_t radius;
+	real_t height;
+
+public:
+
+	_FORCE_INLINE_ const real_t& get_radius() const { return radius; }
+	_FORCE_INLINE_ const real_t& get_height() const { return height; }
+
+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CAPSULE; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+		// no matter the angle, the box is mirrored anyway
+		Vector2 n=p_transform.basis_xform_inv(p_normal).normalized();
+		float h = (n.y > 0) ? height : -height;
+
+		n *= radius;
+		n.y += h * 0.5;
+
+		r_max = p_normal.dot(p_transform.xform(n));
+		r_min = p_normal.dot(p_transform.xform(-n));
+
+		if (r_max<r_min) {
+
+			SWAP(r_max,r_min);
+		}
+
+		//ERR_FAIL_COND( r_max < r_min );
+	}
+
+};
+
+
+class ConvexPolygonShape2DSW : public Shape2DSW {
+
+
+	struct Point {
+
+		Vector2 pos;
+		Vector2 normal; //normal to next segment
+	};
+
+	Point *points;
+	int point_count;
+
+public:
+
+	_FORCE_INLINE_ int get_point_count() const { return point_count; }
+	_FORCE_INLINE_ const Vector2& get_point(int p_idx) const { return points[p_idx].pos; }
+	_FORCE_INLINE_ const Vector2& get_segment_normal(int p_idx) const { return points[p_idx].normal; }
+
+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+		// no matter the angle, the box is mirrored anyway
+
+		r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));
+		for(int i=1;i<point_count;i++) {
+
+			float d = p_normal.dot(p_transform.xform(points[i].pos));
+			if (d>r_max)
+				r_max=d;
+			if (d<r_min)
+				r_min=d;
+
+		}
+
+	}
+
+
+	ConvexPolygonShape2DSW();
+	~ConvexPolygonShape2DSW();
+};
+
+
+class ConcaveShape2DSW : public Shape2DSW {
+
+public:
+
+	virtual bool is_concave() const { return true; }
+	typedef void (*Callback)(void* p_userdata,Shape2DSW *p_convex);
+
+	virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
+
+};
+
+class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
+
+	struct Segment {
+
+		int points[2];
+	};
+
+	Vector<Segment> segments;
+	Vector<Point2> points;
+
+	struct BVH {
+
+		Rect2 aabb;
+		int left,right;
+	};
+
+
+	Vector<BVH> bvh;
+	int bvh_depth;
+
+
+	struct BVH_CompareX {
+
+		_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
+
+			return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5);
+		}
+	};
+
+	struct BVH_CompareY {
+
+		_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
+
+			return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5);
+		}
+	};
+
+	int _generate_bvh(BVH *p_bvh,int p_len,int p_depth);
+
+public:
+
+	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; }
+
+	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
+	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+
+	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+
+	virtual real_t get_moment_of_inertia(float p_mass) const { return 0; }
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const;
+
+};
+
+
+#endif // SHAPE_2D_2DSW_H

servers/physics_2d/space_2d_sw.cpp

@@ -0,0 +1,432 @@
+/*************************************************************************/
+/*  space_2d_sw.cpp                                                      */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "space_2d_sw.h"
+#include "collision_solver_2d_sw.h"
+#include "physics_2d_server_sw.h"
+
+
+bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+
+	ERR_FAIL_COND_V(space->locked,false);
+
+	Vector2 begin,end;
+	Vector2 normal;
+	begin=p_from;
+	end=p_to;
+	normal=(end-begin).normalized();
+
+
+	int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
+
+	bool collided=false;
+	Vector2 res_point,res_normal;
+	int res_shape;
+	const CollisionObject2DSW *res_obj;
+	real_t min_d=1e10;
+
+
+	for(int i=0;i<amount;i++) {
+
+		if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
+			continue; //ignore area
+
+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+			continue;
+
+		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+
+		int shape_idx=space->intersection_query_subindex_results[i];
+		Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
+
+		Vector2 local_from = inv_xform.xform(begin);
+		Vector2 local_to = inv_xform.xform(end);
+
+		/*local_from = col_obj->get_inv_transform().xform(begin);
+		local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from);
+
+		local_to = col_obj->get_inv_transform().xform(end);
+		local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/
+
+		const Shape2DSW *shape = col_obj->get_shape(shape_idx);
+
+		Vector2 shape_point,shape_normal;
+
+
+		if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
+
+
+			//print_line("inters sgment!");
+			Matrix32 xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+			shape_point=xform.xform(shape_point);
+
+			real_t ld = normal.dot(shape_point);
+
+
+			if (ld<min_d) {
+
+				min_d=ld;
+				res_point=shape_point;
+				res_normal=inv_xform.basis_xform_inv(shape_normal).normalized();
+				res_shape=shape_idx;
+				res_obj=col_obj;
+				collided=true;
+			}
+		}
+
+	}
+
+	if (!collided)
+		return false;
+
+
+	r_result.collider_id=res_obj->get_instance_id();
+	if (r_result.collider_id!=0)
+		r_result.collider=ObjectDB::get_instance(r_result.collider_id);
+	r_result.normal=res_normal;
+	r_result.position=res_point;
+	r_result.rid=res_obj->get_self();
+	r_result.shape=res_shape;
+
+	return true;
+
+}
+
+
+int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+	if (p_result_max<=0)
+		return 0;
+
+	Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,0);
+
+	Rect2 aabb = p_xform.xform(shape->get_aabb());
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	bool collided=false;
+	int cc=0;
+
+	for(int i=0;i<amount;i++) {
+
+		if (cc>=p_result_max)
+			break;
+
+		if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
+			continue; //ignore area
+
+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+			continue;
+
+
+		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		if (!CollisionSolver2DSW::solve_static(shape,p_xform,p_xform.affine_inverse(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), col_obj->get_inv_transform() * col_obj->get_shape_inv_transform(shape_idx),NULL,NULL,NULL))
+			continue;
+
+		r_results[cc].collider_id=col_obj->get_instance_id();
+		if (r_results[cc].collider_id!=0)
+			r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
+		r_results[cc].rid=col_obj->get_self();
+		r_results[cc].shape=shape_idx;
+
+		cc++;
+
+	}
+
+	return cc;
+
+}
+
+Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {
+
+
+	space=NULL;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+
+void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self) {
+
+	CollisionObject2DSW::Type type_A=A->get_type();
+	CollisionObject2DSW::Type type_B=B->get_type();
+	if (type_A>type_B) {
+
+		SWAP(A,B);
+		SWAP(p_subindex_A,p_subindex_B);
+		SWAP(type_A,type_B);
+	}
+
+	Space2DSW *self = (Space2DSW*)p_self;
+
+	if (type_A==CollisionObject2DSW::TYPE_AREA) {
+
+
+		ERR_FAIL_COND_V(type_B!=CollisionObject2DSW::TYPE_BODY,NULL);
+		Area2DSW *area=static_cast<Area2DSW*>(A);
+		Body2DSW *body=static_cast<Body2DSW*>(B);
+
+		AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) );
+
+		return area_pair;
+	} else {
+
+
+		BodyPair2DSW *b = memnew( BodyPair2DSW((Body2DSW*)A,p_subindex_A,(Body2DSW*)B,p_subindex_B) );
+		return b;
+
+	}
+
+	return NULL;
+}
+
+void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) {
+
+
+
+	Space2DSW *self = (Space2DSW*)p_self;
+	Constraint2DSW *c = (Constraint2DSW*)p_data;
+	memdelete(c);
+}
+
+
+const SelfList<Body2DSW>::List& Space2DSW::get_active_body_list() const {
+
+	return active_list;
+}
+void Space2DSW::body_add_to_active_list(SelfList<Body2DSW>* p_body) {
+
+	active_list.add(p_body);
+}
+void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW>* p_body) {
+
+	active_list.remove(p_body);
+
+}
+
+void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body) {
+
+
+	inertia_update_list.add(p_body);
+}
+
+void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body) {
+
+	inertia_update_list.remove(p_body);
+}
+
+BroadPhase2DSW *Space2DSW::get_broadphase() {
+
+	return broadphase;
+}
+
+void Space2DSW::add_object(CollisionObject2DSW *p_object) {
+
+	ERR_FAIL_COND( objects.has(p_object) );
+	objects.insert(p_object);
+}
+
+void Space2DSW::remove_object(CollisionObject2DSW *p_object) {
+
+	ERR_FAIL_COND( !objects.has(p_object) );
+	objects.erase(p_object);
+}
+
+const Set<CollisionObject2DSW*> &Space2DSW::get_objects() const {
+
+	return objects;
+}
+
+void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW>* p_body) {
+
+	state_query_list.add(p_body);
+}
+void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW>* p_body) {
+
+	state_query_list.remove(p_body);
+}
+
+void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area) {
+
+	monitor_query_list.add(p_area);
+}
+void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area) {
+
+	monitor_query_list.remove(p_area);
+}
+
+void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW>* p_area) {
+
+	area_moved_list.add(p_area);
+}
+
+void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW>* p_area) {
+
+	area_moved_list.remove(p_area);
+}
+
+const SelfList<Area2DSW>::List& Space2DSW::get_moved_area_list() const {
+
+	return area_moved_list;
+}
+
+
+void Space2DSW::call_queries() {
+
+	while(state_query_list.first()) {
+
+		Body2DSW * b = state_query_list.first()->self();
+		b->call_queries();
+		state_query_list.remove(state_query_list.first());
+	}
+
+	while(monitor_query_list.first()) {
+
+		Area2DSW * a = monitor_query_list.first()->self();
+		a->call_queries();
+		monitor_query_list.remove(monitor_query_list.first());
+	}
+
+}
+
+void Space2DSW::setup() {
+
+
+	while(inertia_update_list.first()) {
+		inertia_update_list.first()->self()->update_inertias();
+		inertia_update_list.remove(inertia_update_list.first());
+	}
+
+
+}
+
+void Space2DSW::update() {
+
+	broadphase->update();
+
+}
+
+
+void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_value) {
+
+	switch(p_param) {
+
+		case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;
+		case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break;
+		case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;
+	}
+}
+
+real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {
+
+	switch(p_param) {
+
+		case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;
+		case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;
+		case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration;
+		case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold;
+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold;
+		case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;
+		case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
+	}
+	return 0;
+}
+
+void Space2DSW::lock() {
+
+	locked=true;
+}
+
+void Space2DSW::unlock() {
+
+	locked=false;
+}
+
+bool Space2DSW::is_locked() const {
+
+	return locked;
+}
+
+Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() {
+
+	return direct_access;
+}
+
+Space2DSW::Space2DSW() {
+
+
+	locked=false;
+	contact_recycle_radius=0.01;
+	contact_max_separation=0.05;
+	contact_max_allowed_penetration= 0.01;
+
+	constraint_bias = 0.01;
+	body_linear_velocity_sleep_treshold=0.01;
+	body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI);
+	body_time_to_sleep=0.5;
+	body_angular_velocity_damp_ratio=15;
+
+
+	broadphase = BroadPhase2DSW::create_func();
+	broadphase->set_pair_callback(_broadphase_pair,this);
+	broadphase->set_unpair_callback(_broadphase_unpair,this);
+	area=NULL;
+
+	direct_access = memnew( Physics2DDirectSpaceStateSW );
+	direct_access->space=this;
+}
+
+Space2DSW::~Space2DSW() {
+
+	memdelete(broadphase);
+	memdelete( direct_access );
+}
+
+
+

servers/physics_2d/space_2d_sw.h

@@ -0,0 +1,160 @@
+/*************************************************************************/
+/*  space_2d_sw.h                                                        */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef SPACE_2D_SW_H
+#define SPACE_2D_SW_H
+
+#include "typedefs.h"
+#include "hash_map.h"
+#include "body_2d_sw.h"
+#include "area_2d_sw.h"
+#include "body_pair_2d_sw.h"
+#include "area_pair_2d_sw.h"
+#include "broad_phase_2d_sw.h"
+#include "collision_object_2d_sw.h"
+
+
+class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState {
+
+	OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState );
+public:
+
+	Space2DSW *space;
+
+	bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+	int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+
+	Physics2DDirectSpaceStateSW();
+};
+
+
+
+class Space2DSW {
+
+
+	Physics2DDirectSpaceStateSW *direct_access;
+	RID self;
+
+	BroadPhase2DSW *broadphase;
+	SelfList<Body2DSW>::List active_list;
+	SelfList<Body2DSW>::List inertia_update_list;
+	SelfList<Body2DSW>::List state_query_list;
+	SelfList<Area2DSW>::List monitor_query_list;
+	SelfList<Area2DSW>::List area_moved_list;
+
+	static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self);
+	static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self);
+
+	Set<CollisionObject2DSW*> objects;
+
+	Area2DSW *area;
+
+	real_t contact_recycle_radius;
+	real_t contact_max_separation;
+	real_t contact_max_allowed_penetration;
+	real_t constraint_bias;
+
+	enum {
+
+		INTERSECTION_QUERY_MAX=2048
+	};
+
+	CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX];
+	int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
+
+	float body_linear_velocity_sleep_treshold;
+	float body_angular_velocity_sleep_treshold;
+	float body_time_to_sleep;
+	float body_angular_velocity_damp_ratio;
+
+	bool locked;
+
+friend class Physics2DDirectSpaceStateSW;
+
+public:
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	void set_default_area(Area2DSW *p_area) { area=p_area; }
+	Area2DSW *get_default_area() const { return area; }
+
+	const SelfList<Body2DSW>::List& get_active_body_list() const;
+	void body_add_to_active_list(SelfList<Body2DSW>* p_body);
+	void body_remove_from_active_list(SelfList<Body2DSW>* p_body);
+	void body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body);
+	void body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body);
+	void area_add_to_moved_list(SelfList<Area2DSW>* p_area);
+	void area_remove_from_moved_list(SelfList<Area2DSW>* p_area);
+	const SelfList<Area2DSW>::List& get_moved_area_list() const;
+
+
+
+
+	void body_add_to_state_query_list(SelfList<Body2DSW>* p_body);
+	void body_remove_from_state_query_list(SelfList<Body2DSW>* p_body);
+
+	void area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area);
+	void area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area);
+
+	BroadPhase2DSW *get_broadphase();
+
+	void add_object(CollisionObject2DSW *p_object);
+	void remove_object(CollisionObject2DSW *p_object);
+	const Set<CollisionObject2DSW*> &get_objects() const;
+
+	_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
+	_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
+	_FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
+	_FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
+	_FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; }
+	_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }
+	_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
+	_FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }
+
+
+	void update();
+	void setup();
+	void call_queries();
+
+
+	bool is_locked() const;
+	void lock();
+	void unlock();
+
+	void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value);
+	real_t get_param(Physics2DServer::SpaceParameter p_param) const;
+
+	Physics2DDirectSpaceStateSW *get_direct_state();
+
+	Space2DSW();
+	~Space2DSW();
+};
+
+
+#endif // SPACE_2D_SW_H

servers/physics_2d/step_2d_sw.cpp

@@ -0,0 +1,239 @@
+/*************************************************************************/
+/*  step_2d_sw.cpp                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "step_2d_sw.h"
+
+
+void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island) {
+
+	p_body->set_island_step(_step);
+	p_body->set_island_next(*p_island);
+	*p_island=p_body;
+
+	for(Map<Constraint2DSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) {		
+
+		Constraint2DSW *c=(Constraint2DSW*)E->key();
+		if (c->get_island_step()==_step)
+			continue; //already processed
+		c->set_island_step(_step);
+		c->set_island_next(*p_constraint_island);
+		*p_constraint_island=c;
+
+
+		for(int i=0;i<c->get_body_count();i++) {
+			if (i==E->get())
+				continue;
+			Body2DSW *b = c->get_body_ptr()[i];
+			if (b->get_island_step()==_step || b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
+				continue; //no go
+			_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
+		}
+	}
+}
+
+void Step2DSW::_setup_island(Constraint2DSW *p_island,float p_delta) {
+
+	Constraint2DSW *ci=p_island;
+	while(ci) {
+		bool process = ci->setup(p_delta);
+		//todo remove from island if process fails
+		ci=ci->get_island_next();
+	}
+}
+
+void Step2DSW::_solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta){
+
+
+	for(int i=0;i<p_iterations;i++) {
+
+		Constraint2DSW *ci=p_island;
+		while(ci) {
+			ci->solve(p_delta);
+			ci=ci->get_island_next();
+		}
+	}
+}
+
+void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) {
+
+
+	bool can_sleep=true;
+
+	Body2DSW *b = p_island;
+	while(b) {
+
+		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
+			continue; //ignore for static
+
+		if (!b->sleep_test(p_delta))
+			can_sleep=false;
+
+		b=b->get_island_next();
+	}
+
+	//put all to sleep or wake up everyoen
+
+	b = p_island;
+	while(b) {
+
+		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
+			continue; //ignore for static
+
+		bool active = b->is_active();
+
+		if (active==can_sleep)
+			b->set_active(!can_sleep);
+
+		b=b->get_island_next();
+	}
+}
+
+void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
+
+
+	p_space->lock(); // can't access space during this
+
+	p_space->setup(); //update inertias, etc
+
+	const SelfList<Body2DSW>::List * body_list = &p_space->get_active_body_list();
+
+	/* INTEGRATE FORCES */
+	int active_count=0;
+
+	const SelfList<Body2DSW>*b = body_list->first();
+	while(b) {
+
+		b->self()->integrate_forces(p_delta);
+		b=b->next();
+		active_count++;
+	}
+
+	/* GENERATE CONSTRAINT ISLANDS */
+
+	Body2DSW *island_list=NULL;
+	Constraint2DSW *constraint_island_list=NULL;
+	b = body_list->first();
+
+	int island_count=0;
+
+	while(b) {
+		Body2DSW *body = b->self();
+
+
+		if (body->get_island_step()!=_step) {
+
+			Body2DSW *island=NULL;
+			Constraint2DSW *constraint_island=NULL;
+			_populate_island(body,&island,&constraint_island);
+
+			island->set_island_list_next(island_list);
+			island_list=island;
+
+			if (constraint_island) {
+				constraint_island->set_island_list_next(constraint_island_list);
+				constraint_island_list=constraint_island;
+				island_count++;
+			}
+
+		}
+		b=b->next();
+	}
+
+	const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
+
+
+
+	while(aml.first()) {
+		for(const Set<Constraint2DSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) {
+
+			Constraint2DSW*c=E->get();
+			if (c->get_island_step()==_step)
+				continue;
+			c->set_island_step(_step);
+			c->set_island_next(NULL);
+			c->set_island_list_next(constraint_island_list);
+			constraint_island_list=c;
+		}
+		p_space->area_remove_from_moved_list((SelfList<Area2DSW>*)aml.first()); //faster to remove here
+	}
+
+//	print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
+	/* SETUP CONSTRAINT ISLANDS */
+
+	{
+		Constraint2DSW *ci=constraint_island_list;
+		while(ci) {
+
+			_setup_island(ci,p_delta);
+			ci=ci->get_island_list_next();
+		}
+	}
+
+	/* SOLVE CONSTRAINT ISLANDS */
+
+	{
+		Constraint2DSW *ci=constraint_island_list;
+		while(ci) {
+			//iterating each island separatedly improves cache efficiency
+			_solve_island(ci,p_iterations,p_delta);
+			ci=ci->get_island_list_next();
+		}
+	}
+
+	/* INTEGRATE VELOCITIES */
+
+	b = body_list->first();
+	while(b) {
+
+		b->self()->integrate_velocities(p_delta);
+		b=b->next();
+	}
+
+	/* SLEEP / WAKE UP ISLANDS */
+
+	{
+		Body2DSW *bi=island_list;
+		while(bi) {
+
+			_check_suspend(bi,p_delta);
+			bi=bi->get_island_list_next();
+		}
+	}
+
+	p_space->update();
+	p_space->unlock();
+	_step++;
+
+
+
+}
+
+Step2DSW::Step2DSW() {
+
+	_step=1;
+}

servers/physics_2d/step_2d_sw.h

@@ -0,0 +1,48 @@
+/*************************************************************************/
+/*  step_2d_sw.h                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef STEP_2D_SW_H
+#define STEP_2D_SW_H
+
+#include "space_2d_sw.h"
+
+class Step2DSW {
+
+	uint64_t _step;
+
+	void _populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island);
+	void _setup_island(Constraint2DSW *p_island,float p_delta);
+	void _solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta);
+	void _check_suspend(Body2DSW *p_island,float p_delta);
+public:
+
+	void step(Space2DSW* p_space,float p_delta,int p_iterations);
+	Step2DSW();
+};
+
+#endif // STEP_2D_SW_H

servers/physics_2d_server.cpp

@@ -0,0 +1,417 @@
+/*************************************************************************/
+/*  physics_2d_server.cpp                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "physics_2d_server.h"
+#include "print_string.h"
+Physics2DServer * Physics2DServer::singleton=NULL;
+
+
+void Physics2DDirectBodyState::integrate_forces() {
+
+	real_t step = get_step();
+	Vector2 lv = get_linear_velocity();
+	lv+=get_total_gravity() * step;
+
+	real_t av = get_angular_velocity();
+
+	float damp = 1.0 - step * get_total_density();
+
+	if (damp<0) // reached zero in the given time
+		damp=0;
+
+	lv*=damp;
+	av*=damp;
+
+	set_linear_velocity(lv);
+	set_angular_velocity(av);
+
+
+
+
+}
+
+Object* Physics2DDirectBodyState::get_contact_collider_object(int p_contact_idx) const {
+
+	ObjectID objid = get_contact_collider_id(p_contact_idx);
+	Object *obj = ObjectDB::get_instance( objid );
+	return obj;
+}
+
+Physics2DServer * Physics2DServer::get_singleton() {
+
+	return singleton;
+}
+
+void Physics2DDirectBodyState::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_total_gravity"),&Physics2DDirectBodyState::get_total_gravity);
+	ObjectTypeDB::bind_method(_MD("get_total_density"),&Physics2DDirectBodyState::get_total_density);
+
+	ObjectTypeDB::bind_method(_MD("get_inverse_mass"),&Physics2DDirectBodyState::get_inverse_mass);
+	ObjectTypeDB::bind_method(_MD("get_inverse_inertia"),&Physics2DDirectBodyState::get_inverse_inertia);
+
+	ObjectTypeDB::bind_method(_MD("set_linear_velocity","velocity"),&Physics2DDirectBodyState::set_linear_velocity);
+	ObjectTypeDB::bind_method(_MD("get_linear_velocity"),&Physics2DDirectBodyState::get_linear_velocity);
+
+	ObjectTypeDB::bind_method(_MD("set_angular_velocity","velocity"),&Physics2DDirectBodyState::set_angular_velocity);
+	ObjectTypeDB::bind_method(_MD("get_angular_velocity"),&Physics2DDirectBodyState::get_angular_velocity);
+
+	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&Physics2DDirectBodyState::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"),&Physics2DDirectBodyState::get_transform);
+
+	ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&Physics2DDirectBodyState::set_sleep_state);
+	ObjectTypeDB::bind_method(_MD("is_sleeping"),&Physics2DDirectBodyState::is_sleeping);
+
+	ObjectTypeDB::bind_method(_MD("get_contact_count"),&Physics2DDirectBodyState::get_contact_count);
+
+	ObjectTypeDB::bind_method(_MD("get_contact_local_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_local_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_normal","contact_idx"),&Physics2DDirectBodyState::get_contact_local_normal);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_shape","contact_idx"),&Physics2DDirectBodyState::get_contact_local_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider","contact_idx"),&Physics2DDirectBodyState::get_contact_collider);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_id","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_id);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_object","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_object);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_velocity_at_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_velocity_at_pos);
+	ObjectTypeDB::bind_method(_MD("get_step"),&Physics2DDirectBodyState::get_step);
+	ObjectTypeDB::bind_method(_MD("integrate_forces"),&Physics2DDirectBodyState::integrate_forces);
+	ObjectTypeDB::bind_method(_MD("get_space_state:Physics2DDirectSpaceState"),&Physics2DDirectBodyState::get_space_state);
+
+}
+
+Physics2DDirectBodyState::Physics2DDirectBodyState() {}
+
+///////////////////////////////////////////////////////
+
+
+
+Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+	RayResult inters;
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+	bool res = intersect_ray(p_from,p_to,inters,exclude,p_user_mask);
+
+	if (!res)
+		return Variant();
+
+	Dictionary d;
+	d["position"]=inters.position;
+	d["normal"]=inters.normal;
+	d["collider_id"]=inters.collider_id;
+	d["collider"]=inters.collider;
+	d["shape"]=inters.shape;
+	d["rid"]=inters.rid;
+
+	return d;
+}
+
+Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+	ERR_FAIL_INDEX_V(p_result_max,4096,Variant());
+	if (p_result_max<=0)
+		return Variant();
+
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+	ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
+
+	int rc = intersect_shape(p_shape,p_xform,res,p_result_max,exclude,p_user_mask);
+
+	if (rc==0)
+		return Variant();
+
+	Ref<Physics2DShapeQueryResult>  result = memnew( Physics2DShapeQueryResult );
+	result->result.resize(rc);
+	for(int i=0;i<rc;i++)
+		result->result[i]=res[i];
+
+	return result;
+
+}
+
+
+
+
+
+Physics2DDirectSpaceState::Physics2DDirectSpaceState() {
+
+
+
+}
+
+
+void Physics2DDirectSpaceState::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","umask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
+	ObjectTypeDB::bind_method(_MD("intersect_shape:Physics2DShapeQueryResult","shape","xform","result_max","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
+
+}
+
+
+int Physics2DShapeQueryResult::get_result_count() const {
+
+	return result.size();
+}
+RID Physics2DShapeQueryResult::get_result_rid(int p_idx) const {
+
+	return result[p_idx].rid;
+}
+ObjectID Physics2DShapeQueryResult::get_result_object_id(int p_idx) const {
+
+	return result[p_idx].collider_id;
+}
+Object* Physics2DShapeQueryResult::get_result_object(int p_idx) const {
+
+	return result[p_idx].collider;
+}
+int Physics2DShapeQueryResult::get_result_object_shape(int p_idx) const {
+
+	return result[p_idx].shape;
+}
+
+Physics2DShapeQueryResult::Physics2DShapeQueryResult() {
+
+
+}
+
+void Physics2DShapeQueryResult::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_result_count"),&Physics2DShapeQueryResult::get_result_count);
+	ObjectTypeDB::bind_method(_MD("get_result_rid","idx"),&Physics2DShapeQueryResult::get_result_rid);
+	ObjectTypeDB::bind_method(_MD("get_result_object_id","idx"),&Physics2DShapeQueryResult::get_result_object_id);
+	ObjectTypeDB::bind_method(_MD("get_result_object","idx"),&Physics2DShapeQueryResult::get_result_object);
+	ObjectTypeDB::bind_method(_MD("get_result_object_shape","idx"),&Physics2DShapeQueryResult::get_result_object_shape);
+
+
+}
+
+
+
+
+
+
+///////////////////////////////////////
+
+void Physics2DServer::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("shape_create","type"),&Physics2DServer::shape_create);
+	ObjectTypeDB::bind_method(_MD("shape_set_data","shape","data"),&Physics2DServer::shape_set_data);
+
+	ObjectTypeDB::bind_method(_MD("shape_get_type","shape"),&Physics2DServer::shape_get_type);
+	ObjectTypeDB::bind_method(_MD("shape_get_data","shape"),&Physics2DServer::shape_get_data);
+
+
+	ObjectTypeDB::bind_method(_MD("space_create"),&Physics2DServer::space_create);
+	ObjectTypeDB::bind_method(_MD("space_set_active","space","active"),&Physics2DServer::space_set_active);
+	ObjectTypeDB::bind_method(_MD("space_is_active","space"),&Physics2DServer::space_is_active);
+	ObjectTypeDB::bind_method(_MD("space_set_param","space","param","value"),&Physics2DServer::space_set_param);
+	ObjectTypeDB::bind_method(_MD("space_get_param","space","param"),&Physics2DServer::space_get_param);
+	ObjectTypeDB::bind_method(_MD("space_get_direct_state:Physics2DDirectSpaceState","space"),&Physics2DServer::space_get_direct_state);
+
+	ObjectTypeDB::bind_method(_MD("area_create"),&Physics2DServer::area_create);
+	ObjectTypeDB::bind_method(_MD("area_set_space","area","space"),&Physics2DServer::area_set_space);
+	ObjectTypeDB::bind_method(_MD("area_get_space","area"),&Physics2DServer::area_get_space);
+
+	ObjectTypeDB::bind_method(_MD("area_set_space_override_mode","area","mode"),&Physics2DServer::area_set_space_override_mode);
+	ObjectTypeDB::bind_method(_MD("area_get_space_override_mode","area"),&Physics2DServer::area_get_space_override_mode);
+
+	ObjectTypeDB::bind_method(_MD("area_add_shape","area","shape","transform"),&Physics2DServer::area_set_shape,DEFVAL(Matrix32()));
+	ObjectTypeDB::bind_method(_MD("area_set_shape","area","shape_idx","shape"),&Physics2DServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_set_shape_transform","area","shape_idx","transform"),&Physics2DServer::area_set_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_get_shape_count","area"),&Physics2DServer::area_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("area_get_shape","area","shape_idx"),&Physics2DServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_get_shape_transform","area","shape_idx"),&Physics2DServer::area_get_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_remove_shape","area","shape_idx"),&Physics2DServer::area_remove_shape);
+	ObjectTypeDB::bind_method(_MD("area_clear_shapes","area"),&Physics2DServer::area_clear_shapes);
+
+
+	ObjectTypeDB::bind_method(_MD("area_set_param","area","param","value"),&Physics2DServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_set_transform","area","transform"),&Physics2DServer::area_get_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_get_param","area","param"),&Physics2DServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_get_transform","area"),&Physics2DServer::area_get_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_attach_object_instance_ID","area","id"),&Physics2DServer::area_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("area_get_object_instance_ID","area"),&Physics2DServer::area_get_object_instance_ID);
+
+	ObjectTypeDB::bind_method(_MD("area_set_monitor_callback","receiver","method"),&Physics2DServer::area_set_monitor_callback);
+
+	ObjectTypeDB::bind_method(_MD("body_create","mode","init_sleeping"),&Physics2DServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false));
+
+	ObjectTypeDB::bind_method(_MD("body_set_space","body","space"),&Physics2DServer::body_set_space);
+	ObjectTypeDB::bind_method(_MD("body_get_space","body"),&Physics2DServer::body_get_space);
+
+	ObjectTypeDB::bind_method(_MD("body_set_mode","body","mode"),&Physics2DServer::body_set_mode);
+	ObjectTypeDB::bind_method(_MD("body_get_mode","body"),&Physics2DServer::body_get_mode);
+
+	ObjectTypeDB::bind_method(_MD("body_add_shape","body","shape","transform"),&Physics2DServer::body_add_shape,DEFVAL(Matrix32()));
+	ObjectTypeDB::bind_method(_MD("body_set_shape","body","shape_idx","shape"),&Physics2DServer::body_set_shape);
+	ObjectTypeDB::bind_method(_MD("body_set_shape_transform","body","shape_idx","transform"),&Physics2DServer::body_set_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("body_get_shape_count","body"),&Physics2DServer::body_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("body_get_shape","body","shape_idx"),&Physics2DServer::body_get_shape);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_transform","body","shape_idx"),&Physics2DServer::body_get_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("body_remove_shape","body","shape_idx"),&Physics2DServer::body_remove_shape);
+	ObjectTypeDB::bind_method(_MD("body_clear_shapes","body"),&Physics2DServer::body_clear_shapes);
+
+
+	ObjectTypeDB::bind_method(_MD("body_set_shape_as_trigger","body","shape_idx","enable"),&Physics2DServer::body_set_shape_as_trigger);
+	ObjectTypeDB::bind_method(_MD("body_is_shape_set_as_trigger","body","shape_idx"),&Physics2DServer::body_is_shape_set_as_trigger);
+
+	ObjectTypeDB::bind_method(_MD("body_attach_object_instance_ID","body","id"),&Physics2DServer::body_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID","body"),&Physics2DServer::body_get_object_instance_ID);
+
+
+	ObjectTypeDB::bind_method(_MD("body_set_enable_continuous_collision_detection","body","enable"),&Physics2DServer::body_set_enable_continuous_collision_detection);
+	ObjectTypeDB::bind_method(_MD("body_is_continuous_collision_detection_enabled","body"),&Physics2DServer::body_is_continuous_collision_detection_enabled);
+
+
+	//ObjectTypeDB::bind_method(_MD("body_set_user_flags","flags""),&Physics2DServer::body_set_shape,DEFVAL(Matrix32));
+	//ObjectTypeDB::bind_method(_MD("body_get_user_flags","body","shape_idx","shape"),&Physics2DServer::body_get_shape);
+
+	ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&Physics2DServer::body_set_param);
+	ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&Physics2DServer::body_get_param);
+
+	ObjectTypeDB::bind_method(_MD("body_static_simulate_motion","body","new_xform"),&Physics2DServer::body_static_simulate_motion);
+
+	ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&Physics2DServer::body_set_state);
+	ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&Physics2DServer::body_get_state);
+
+	ObjectTypeDB::bind_method(_MD("body_apply_impulse","body","pos","impulse"),&Physics2DServer::body_apply_impulse);
+	ObjectTypeDB::bind_method(_MD("body_set_axis_velocity","body","axis_velocity"),&Physics2DServer::body_set_axis_velocity);
+
+	ObjectTypeDB::bind_method(_MD("body_add_collision_exception","body","excepted_body"),&Physics2DServer::body_add_collision_exception);
+	ObjectTypeDB::bind_method(_MD("body_remove_collision_exception","body","excepted_body"),&Physics2DServer::body_remove_collision_exception);
+//	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+
+	ObjectTypeDB::bind_method(_MD("body_set_max_contacts_reported","body","amount"),&Physics2DServer::body_set_max_contacts_reported);
+	ObjectTypeDB::bind_method(_MD("body_get_max_contacts_reported","body"),&Physics2DServer::body_get_max_contacts_reported);
+
+	ObjectTypeDB::bind_method(_MD("body_set_omit_force_integration","body","enable"),&Physics2DServer::body_set_omit_force_integration);
+	ObjectTypeDB::bind_method(_MD("body_is_omitting_force_integration","body"),&Physics2DServer::body_is_omitting_force_integration);
+
+	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method"),&Physics2DServer::body_set_force_integration_callback);
+
+	/* JOINT API */
+
+	ObjectTypeDB::bind_method(_MD("joint_set_param","joint","param","value"),&Physics2DServer::joint_set_param);
+	ObjectTypeDB::bind_method(_MD("joint_get_param","joint","param"),&Physics2DServer::joint_get_param);
+
+	ObjectTypeDB::bind_method(_MD("pin_joint_create","anchor","body_a","body_b"),&Physics2DServer::pin_joint_create,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("groove_joint_create","groove1_a","groove2_a","anchor_b","body_a","body_b"),&Physics2DServer::groove_joint_create,DEFVAL(RID()),DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_spring_joint_create","anchor_a","anchor_b","body_a","body_b"),&Physics2DServer::damped_spring_joint_create,DEFVAL(RID()));
+
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_set_param","joint","param","value"),&Physics2DServer::damped_string_joint_set_param,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_get_param","joint","param"),&Physics2DServer::damped_string_joint_get_param);
+
+	ObjectTypeDB::bind_method(_MD("joint_get_type","joint"),&Physics2DServer::joint_get_type);
+
+	ObjectTypeDB::bind_method(_MD("free","rid"),&Physics2DServer::free);
+
+	ObjectTypeDB::bind_method(_MD("set_active","active"),&Physics2DServer::set_active);
+
+//	ObjectTypeDB::bind_method(_MD("init"),&Physics2DServer::init);
+//	ObjectTypeDB::bind_method(_MD("step"),&Physics2DServer::step);
+//	ObjectTypeDB::bind_method(_MD("sync"),&Physics2DServer::sync);
+	//ObjectTypeDB::bind_method(_MD("flush_queries"),&Physics2DServer::flush_queries);
+
+	BIND_CONSTANT( SHAPE_LINE );
+	BIND_CONSTANT( SHAPE_SEGMENT );
+	BIND_CONSTANT( SHAPE_CIRCLE );
+	BIND_CONSTANT( SHAPE_RECTANGLE );
+	BIND_CONSTANT( SHAPE_CAPSULE );
+	BIND_CONSTANT( SHAPE_CONVEX_POLYGON );
+	BIND_CONSTANT( SHAPE_CONCAVE_POLYGON );
+	BIND_CONSTANT( SHAPE_CUSTOM );
+
+	BIND_CONSTANT( AREA_PARAM_GRAVITY );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_VECTOR );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_IS_POINT );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_POINT_ATTENUATION );
+	BIND_CONSTANT( AREA_PARAM_DENSITY );
+	BIND_CONSTANT( AREA_PARAM_PRIORITY );
+
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE );
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_DISABLED );
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
+
+	BIND_CONSTANT( BODY_MODE_STATIC );
+	BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE );
+	BIND_CONSTANT( BODY_MODE_RIGID );
+	BIND_CONSTANT( BODY_MODE_CHARACTER );
+
+	BIND_CONSTANT( BODY_PARAM_BOUNCE );
+	BIND_CONSTANT( BODY_PARAM_FRICTION );
+	BIND_CONSTANT( BODY_PARAM_MASS );
+	BIND_CONSTANT( BODY_PARAM_MAX );
+
+	BIND_CONSTANT( BODY_STATE_TRANSFORM );
+	BIND_CONSTANT( BODY_STATE_LINEAR_VELOCITY );
+	BIND_CONSTANT( BODY_STATE_ANGULAR_VELOCITY );
+	BIND_CONSTANT( BODY_STATE_SLEEPING );
+	BIND_CONSTANT( BODY_STATE_CAN_SLEEP );
+
+	BIND_CONSTANT( JOINT_PIN );
+	BIND_CONSTANT( JOINT_GROOVE );
+	BIND_CONSTANT( JOINT_DAMPED_SPRING );
+
+	BIND_CONSTANT( DAMPED_STRING_REST_LENGTH );
+	BIND_CONSTANT( DAMPED_STRING_STIFFNESS );
+	BIND_CONSTANT( DAMPED_STRING_DAMPING );
+
+//	BIND_CONSTANT( TYPE_BODY );
+//	BIND_CONSTANT( TYPE_AREA );
+
+	BIND_CONSTANT( AREA_BODY_ADDED );
+	BIND_CONSTANT( AREA_BODY_REMOVED );
+
+
+}
+
+
+Physics2DServer::Physics2DServer() {
+
+	ERR_FAIL_COND( singleton!=NULL );
+	singleton=this;
+}
+
+Physics2DServer::~Physics2DServer() {
+
+	singleton=NULL;
+}
+

servers/physics_2d_server.h

@@ -0,0 +1,426 @@
+/*************************************************************************/
+/*  physics_2d_server.h                                                  */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef PHYSICS_2D_SERVER_H
+#define PHYSICS_2D_SERVER_H
+
+#include "object.h"
+#include "reference.h"
+
+class Physics2DDirectSpaceState;
+
+class Physics2DDirectBodyState : public Object {
+
+	OBJ_TYPE( Physics2DDirectBodyState, Object );
+protected:
+	static void _bind_methods();
+public:
+
+	virtual Vector2 get_total_gravity() const=0; // get gravity vector working on this body space/area
+	virtual float get_total_density() const=0; // get density of this body space/area
+
+	virtual float get_inverse_mass() const=0; // get the mass
+	virtual real_t get_inverse_inertia() const=0; // get density of this body space
+
+	virtual void set_linear_velocity(const Vector2& p_velocity)=0;
+	virtual Vector2 get_linear_velocity() const=0;
+
+	virtual void set_angular_velocity(real_t p_velocity)=0;
+	virtual real_t get_angular_velocity() const=0;
+
+	virtual void set_transform(const Matrix32& p_transform)=0;
+	virtual Matrix32 get_transform() const=0;
+
+	virtual void set_sleep_state(bool p_enable)=0;
+	virtual bool is_sleeping() const=0;
+
+	virtual int get_contact_count() const=0;
+
+	virtual Vector2 get_contact_local_pos(int p_contact_idx) const=0;
+	virtual Vector2 get_contact_local_normal(int p_contact_idx) const=0;
+	virtual int get_contact_local_shape(int p_contact_idx) const=0;
+
+	virtual RID get_contact_collider(int p_contact_idx) const=0;
+	virtual Vector2 get_contact_collider_pos(int p_contact_idx) const=0;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const=0;
+	virtual Object* get_contact_collider_object(int p_contact_idx) const;
+	virtual int get_contact_collider_shape(int p_contact_idx) const=0;
+	virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const=0;
+
+	virtual real_t get_step() const=0;
+	virtual void integrate_forces();
+
+	virtual Physics2DDirectSpaceState* get_space_state()=0;
+
+	Physics2DDirectBodyState();
+};
+
+
+class Physics2DShapeQueryResult;
+
+
+class Physics2DDirectSpaceState : public Object {
+
+	OBJ_TYPE( Physics2DDirectSpaceState, Object );
+
+	Variant _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
+	Variant _intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
+
+
+protected:
+	static void _bind_methods();
+
+public:
+
+	struct RayResult {
+
+		Vector2 position;
+		Vector2 normal;
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+	};
+
+	virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+	struct ShapeResult {
+
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+
+	};
+
+	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+	Physics2DDirectSpaceState();
+};
+
+
+class Physics2DShapeQueryResult : public Reference {
+
+	OBJ_TYPE( Physics2DShapeQueryResult, Reference );
+
+	Vector<Physics2DDirectSpaceState::ShapeResult> result;
+
+friend class Physics2DDirectSpaceState;
+
+protected:
+	static void _bind_methods();
+public:
+
+	int get_result_count() const;
+	RID get_result_rid(int p_idx) const;
+	ObjectID get_result_object_id(int p_idx) const;
+	Object* get_result_object(int p_idx) const;
+	int get_result_object_shape(int p_idx) const;
+
+	Physics2DShapeQueryResult();
+};
+
+
+class Physics2DServer : public Object {
+
+	OBJ_TYPE( Physics2DServer, Object );
+
+	static Physics2DServer * singleton;
+
+protected:
+	static void _bind_methods();
+
+public:
+
+	static Physics2DServer * get_singleton();
+
+	enum ShapeType {
+		SHAPE_LINE, ///< plane:"plane"
+		SHAPE_RAY, ///< float:"length"
+		SHAPE_SEGMENT, ///< float:"length"
+		SHAPE_CIRCLE, ///< float:"radius"
+		SHAPE_RECTANGLE, ///< vec3:"extents"
+		SHAPE_CAPSULE,
+		SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+		SHAPE_CONCAVE_POLYGON, ///< Vector2 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector2 array)
+		SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+	};
+
+	virtual RID shape_create(ShapeType p_shape)=0;
+	virtual void shape_set_data(RID p_shape, const Variant& p_data)=0;
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias)=0;
+
+	virtual ShapeType shape_get_type(RID p_shape) const=0;
+	virtual Variant shape_get_data(RID p_shape) const=0;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0;
+
+
+	/* SPACE API */
+
+	virtual RID space_create()=0;
+	virtual void space_set_active(RID p_space,bool p_active)=0;
+	virtual bool space_is_active(RID p_space) const=0;
+
+	enum SpaceParameter {
+
+		SPACE_PARAM_CONTACT_RECYCLE_RADIUS,
+		SPACE_PARAM_CONTACT_MAX_SEPARATION,
+		SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION,
+		SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD,
+		SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD,
+		SPACE_PARAM_BODY_TIME_TO_SLEEP,
+		SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO,
+		SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
+	};
+
+	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value)=0;
+	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const=0;
+
+	// this function only works on fixed process, errors and returns null otherwise
+	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space)=0;
+
+
+	//missing space parameters
+
+	/* AREA API */
+
+	//missing attenuation? missing better override?
+
+
+
+	enum AreaParameter {
+		AREA_PARAM_GRAVITY,
+		AREA_PARAM_GRAVITY_VECTOR,
+		AREA_PARAM_GRAVITY_IS_POINT,
+		AREA_PARAM_GRAVITY_POINT_ATTENUATION,
+		AREA_PARAM_DENSITY,
+		AREA_PARAM_PRIORITY
+	};
+
+	virtual RID area_create()=0;
+
+	virtual void area_set_space(RID p_area, RID p_space)=0;
+	virtual RID area_get_space(RID p_area) const=0;
+
+
+	enum AreaSpaceOverrideMode {
+		AREA_SPACE_OVERRIDE_DISABLED,
+		AREA_SPACE_OVERRIDE_COMBINE,
+		AREA_SPACE_OVERRIDE_REPLACE,
+	};
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode)=0;
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const=0;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform=Matrix32())=0;
+	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape)=0;
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform)=0;
+
+	virtual int area_get_shape_count(RID p_area) const=0;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const=0;
+	virtual Matrix32 area_get_shape_transform(RID p_area, int p_shape_idx) const=0;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx)=0;
+	virtual void area_clear_shapes(RID p_area)=0;
+
+	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID)=0;
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const=0;
+
+	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value)=0;
+	virtual void area_set_transform(RID p_area, const Matrix32& p_transform)=0;
+
+	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const=0;
+	virtual Matrix32 area_get_transform(RID p_area) const=0;
+
+	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
+
+	/* BODY API */
+
+	//missing ccd?
+
+	enum BodyMode {
+		BODY_MODE_STATIC,
+		BODY_MODE_STATIC_ACTIVE,
+		BODY_MODE_RIGID,
+		//BODY_MODE_SOFT
+		BODY_MODE_CHARACTER
+	};
+
+	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0;
+
+	virtual void body_set_space(RID p_body, RID p_space)=0;
+	virtual RID body_get_space(RID p_body) const=0;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode)=0;
+	virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const=0;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32())=0;
+	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0;
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform)=0;
+
+	virtual int body_get_shape_count(RID p_body) const=0;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const=0;
+	virtual Matrix32 body_get_shape_transform(RID p_body, int p_shape_idx) const=0;
+
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable)=0;
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const=0;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx)=0;
+	virtual void body_clear_shapes(RID p_body)=0;
+
+	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0;
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const=0;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0;
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
+
+	// common body variables
+	enum BodyParameter {
+		BODY_PARAM_BOUNCE,
+		BODY_PARAM_FRICTION,
+		BODY_PARAM_MASS, ///< unused for static, always infinite
+		BODY_PARAM_MAX,
+	};
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0;
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const=0;
+
+	//advanced simulation
+	virtual void body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform)=0;
+
+	//state
+	enum BodyState {
+		BODY_STATE_TRANSFORM,
+		BODY_STATE_LINEAR_VELOCITY,
+		BODY_STATE_ANGULAR_VELOCITY,
+		BODY_STATE_SLEEPING,
+		BODY_STATE_CAN_SLEEP
+	};
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant)=0;
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const=0;
+
+	//do something about it
+	virtual void body_set_applied_force(RID p_body, const Vector2& p_force)=0;
+	virtual Vector2 body_get_applied_force(RID p_body) const=0;
+
+	virtual void body_set_applied_torque(RID p_body, float p_torque)=0;
+	virtual float body_get_applied_torque(RID p_body) const=0;
+
+	virtual void body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse)=0;
+	virtual void body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity)=0;
+
+	//fix
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b)=0;
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b)=0;
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts)=0;
+	virtual int body_get_max_contacts_reported(RID p_body) const=0;
+
+	//missing remove
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold)=0;
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const=0;
+
+	virtual void body_set_omit_force_integration(RID p_body,bool p_omit)=0;
+	virtual bool body_is_omitting_force_integration(RID p_body) const=0;
+
+	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
+
+	/* JOINT API */
+
+	enum JointType {
+
+		JOINT_PIN,
+		JOINT_GROOVE,
+		JOINT_DAMPED_SPRING
+	};
+
+	enum JointParam {
+		JOINT_PARAM_BIAS,
+		JOINT_PARAM_MAX_BIAS,
+		JOINT_PARAM_MAX_FORCE,
+	};
+
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value)=0;
+	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const=0;
+
+	virtual RID pin_joint_create(const Vector2& p_anchor,RID p_body_a,RID p_body_b=RID())=0;
+	virtual RID groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b)=0;
+	virtual RID damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b=RID())=0;
+
+	enum DampedStringParam {
+		DAMPED_STRING_REST_LENGTH,
+		DAMPED_STRING_STIFFNESS,
+		DAMPED_STRING_DAMPING
+	};
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value)=0;
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const=0;
+
+	virtual JointType joint_get_type(RID p_joint) const=0;	
+
+	/* QUERY API */
+
+	enum AreaBodyStatus {
+		AREA_BODY_ADDED,
+		AREA_BODY_REMOVED
+	};
+
+
+	/* MISC */
+
+	virtual void free(RID p_rid)=0;
+
+	virtual void set_active(bool p_active)=0;
+	virtual void init()=0;
+	virtual void step(float p_step)=0;
+	virtual void sync()=0;
+	virtual void flush_queries()=0;
+	virtual void finish()=0;
+
+	Physics2DServer();
+	~Physics2DServer();
+};
+
+VARIANT_ENUM_CAST( Physics2DServer::ShapeType );
+VARIANT_ENUM_CAST( Physics2DServer::SpaceParameter );
+VARIANT_ENUM_CAST( Physics2DServer::AreaParameter );
+VARIANT_ENUM_CAST( Physics2DServer::AreaSpaceOverrideMode );
+VARIANT_ENUM_CAST( Physics2DServer::BodyMode );
+VARIANT_ENUM_CAST( Physics2DServer::BodyParameter );
+VARIANT_ENUM_CAST( Physics2DServer::BodyState );
+VARIANT_ENUM_CAST( Physics2DServer::JointParam );
+VARIANT_ENUM_CAST( Physics2DServer::JointType );
+VARIANT_ENUM_CAST( Physics2DServer::DampedStringParam );
+//VARIANT_ENUM_CAST( Physics2DServer::ObjectType );
+VARIANT_ENUM_CAST( Physics2DServer::AreaBodyStatus );
+
+#endif

servers/physics_server.cpp

@@ -0,0 +1,420 @@
+/*************************************************************************/
+/*  physics_server.cpp                                                   */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "physics_server.h"
+#include "print_string.h"
+PhysicsServer * PhysicsServer::singleton=NULL;
+
+
+void PhysicsDirectBodyState::integrate_forces() {
+
+	real_t step = get_step();
+	Vector3 lv = get_linear_velocity();
+	lv+=get_total_gravity() * step;
+
+	Vector3 av = get_angular_velocity();
+
+	float damp = 1.0 - step * get_total_density();
+
+	if (damp<0) // reached zero in the given time
+		damp=0;
+
+	lv*=damp;
+	av*=damp;
+
+	set_linear_velocity(lv);
+	set_angular_velocity(av);
+
+
+
+
+}
+
+Object* PhysicsDirectBodyState::get_contact_collider_object(int p_contact_idx) const {
+
+	ObjectID objid = get_contact_collider_id(p_contact_idx);
+	Object *obj = ObjectDB::get_instance( objid );
+	return obj;
+}
+
+PhysicsServer * PhysicsServer::get_singleton() {
+
+	return singleton;
+}
+
+void PhysicsDirectBodyState::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_total_gravity"),&PhysicsDirectBodyState::get_total_gravity);
+	ObjectTypeDB::bind_method(_MD("get_total_density"),&PhysicsDirectBodyState::get_total_density);
+
+	ObjectTypeDB::bind_method(_MD("get_inverse_mass"),&PhysicsDirectBodyState::get_inverse_mass);
+	ObjectTypeDB::bind_method(_MD("get_inverse_inertia"),&PhysicsDirectBodyState::get_inverse_inertia);
+
+	ObjectTypeDB::bind_method(_MD("set_linear_velocity","velocity"),&PhysicsDirectBodyState::set_linear_velocity);
+	ObjectTypeDB::bind_method(_MD("get_linear_velocity"),&PhysicsDirectBodyState::get_linear_velocity);
+
+	ObjectTypeDB::bind_method(_MD("set_angular_velocity","velocity"),&PhysicsDirectBodyState::set_angular_velocity);
+	ObjectTypeDB::bind_method(_MD("get_angular_velocity"),&PhysicsDirectBodyState::get_angular_velocity);
+
+	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&PhysicsDirectBodyState::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsDirectBodyState::get_transform);
+
+	ObjectTypeDB::bind_method(_MD("add_force","force","pos"),&PhysicsDirectBodyState::add_force);
+
+	ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&PhysicsDirectBodyState::set_sleep_state);
+	ObjectTypeDB::bind_method(_MD("is_sleeping"),&PhysicsDirectBodyState::is_sleeping);
+
+	ObjectTypeDB::bind_method(_MD("get_contact_count"),&PhysicsDirectBodyState::get_contact_count);
+
+	ObjectTypeDB::bind_method(_MD("get_contact_local_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_local_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_normal","contact_idx"),&PhysicsDirectBodyState::get_contact_local_normal);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_shape","contact_idx"),&PhysicsDirectBodyState::get_contact_local_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider","contact_idx"),&PhysicsDirectBodyState::get_contact_collider);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_id","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_id);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_object","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_object);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_velocity_at_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_velocity_at_pos);
+	ObjectTypeDB::bind_method(_MD("get_step"),&PhysicsDirectBodyState::get_step);
+	ObjectTypeDB::bind_method(_MD("integrate_forces"),&PhysicsDirectBodyState::integrate_forces);
+	ObjectTypeDB::bind_method(_MD("get_space_state:PhysicsDirectSpaceState"),&PhysicsDirectBodyState::get_space_state);
+
+}
+
+PhysicsDirectBodyState::PhysicsDirectBodyState() {}
+
+///////////////////////////////////////////////////////
+
+
+
+Variant PhysicsDirectSpaceState::_intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+	RayResult inters;
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+	bool res = intersect_ray(p_from,p_to,inters,exclude,p_user_mask);
+
+	if (!res)
+		return Variant();
+
+	Dictionary d;
+	d["position"]=inters.position;
+	d["normal"]=inters.normal;
+	d["collider_id"]=inters.collider_id;
+	d["collider"]=inters.collider;
+	d["shape"]=inters.shape;
+	d["rid"]=inters.rid;
+
+	return d;
+}
+
+Variant PhysicsDirectSpaceState::_intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+
+
+	ERR_FAIL_INDEX_V(p_result_max,4096,Variant());
+	if (p_result_max<=0)
+		return Variant();
+
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+	ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
+
+	int rc = intersect_shape(p_shape,p_xform,res,p_result_max,exclude,p_user_mask);
+
+	if (rc==0)
+		return Variant();
+
+	Ref<PhysicsShapeQueryResult>  result = memnew( PhysicsShapeQueryResult );
+	result->result.resize(rc);
+	for(int i=0;i<rc;i++)
+		result->result[i]=res[i];
+
+	return result;
+
+}
+
+
+
+
+
+PhysicsDirectSpaceState::PhysicsDirectSpaceState() {
+
+
+
+}
+
+
+void PhysicsDirectSpaceState::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("intersect_ray","from","to","exclude","umask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
+	ObjectTypeDB::bind_method(_MD("intersect_shape:PhysicsShapeQueryResult","shape","xform","result_max","exclude","umask"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
+
+}
+
+
+int PhysicsShapeQueryResult::get_result_count() const {
+
+	return result.size();
+}
+RID PhysicsShapeQueryResult::get_result_rid(int p_idx) const {
+
+	return result[p_idx].rid;
+}
+ObjectID PhysicsShapeQueryResult::get_result_object_id(int p_idx) const {
+
+	return result[p_idx].collider_id;
+}
+Object* PhysicsShapeQueryResult::get_result_object(int p_idx) const {
+
+	return result[p_idx].collider;
+}
+int PhysicsShapeQueryResult::get_result_object_shape(int p_idx) const {
+
+	return result[p_idx].shape;
+}
+
+PhysicsShapeQueryResult::PhysicsShapeQueryResult() {
+
+
+}
+
+void PhysicsShapeQueryResult::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_result_count"),&PhysicsShapeQueryResult::get_result_count);
+	ObjectTypeDB::bind_method(_MD("get_result_rid","idx"),&PhysicsShapeQueryResult::get_result_rid);
+	ObjectTypeDB::bind_method(_MD("get_result_object_id","idx"),&PhysicsShapeQueryResult::get_result_object_id);
+	ObjectTypeDB::bind_method(_MD("get_result_object","idx"),&PhysicsShapeQueryResult::get_result_object);
+	ObjectTypeDB::bind_method(_MD("get_result_object_shape","idx"),&PhysicsShapeQueryResult::get_result_object_shape);
+
+
+}
+
+
+
+
+
+
+///////////////////////////////////////
+
+void PhysicsServer::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("shape_create","type"),&PhysicsServer::shape_create);
+	ObjectTypeDB::bind_method(_MD("shape_set_data","shape","data"),&PhysicsServer::shape_set_data);
+
+	ObjectTypeDB::bind_method(_MD("shape_get_type","shape"),&PhysicsServer::shape_get_type);
+	ObjectTypeDB::bind_method(_MD("shape_get_data","shape"),&PhysicsServer::shape_get_data);
+
+
+	ObjectTypeDB::bind_method(_MD("space_create"),&PhysicsServer::space_create);
+	ObjectTypeDB::bind_method(_MD("space_set_active","space","active"),&PhysicsServer::space_set_active);
+	ObjectTypeDB::bind_method(_MD("space_is_active","space"),&PhysicsServer::space_is_active);
+	ObjectTypeDB::bind_method(_MD("space_set_param","space","param","value"),&PhysicsServer::space_set_param);
+	ObjectTypeDB::bind_method(_MD("space_get_param","space","param"),&PhysicsServer::space_get_param);
+	ObjectTypeDB::bind_method(_MD("space_get_direct_state:PhysicsDirectSpaceState","space"),&PhysicsServer::space_get_direct_state);
+
+	ObjectTypeDB::bind_method(_MD("area_create"),&PhysicsServer::area_create);
+	ObjectTypeDB::bind_method(_MD("area_set_space","area","space"),&PhysicsServer::area_set_space);
+	ObjectTypeDB::bind_method(_MD("area_get_space","area"),&PhysicsServer::area_get_space);
+
+	ObjectTypeDB::bind_method(_MD("area_set_space_override_mode","area","mode"),&PhysicsServer::area_set_space_override_mode);
+	ObjectTypeDB::bind_method(_MD("area_get_space_override_mode","area"),&PhysicsServer::area_get_space_override_mode);
+
+	ObjectTypeDB::bind_method(_MD("area_add_shape","area","shape","transform"),&PhysicsServer::area_set_shape,DEFVAL(Transform()));
+	ObjectTypeDB::bind_method(_MD("area_set_shape","area","shape_idx","shape"),&PhysicsServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_set_shape_transform","area","shape_idx","transform"),&PhysicsServer::area_set_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_get_shape_count","area"),&PhysicsServer::area_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("area_get_shape","area","shape_idx"),&PhysicsServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_get_shape_transform","area","shape_idx"),&PhysicsServer::area_get_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_remove_shape","area","shape_idx"),&PhysicsServer::area_remove_shape);
+	ObjectTypeDB::bind_method(_MD("area_clear_shapes","area"),&PhysicsServer::area_clear_shapes);
+
+
+	ObjectTypeDB::bind_method(_MD("area_set_param","area","param","value"),&PhysicsServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_set_transform","area","transform"),&PhysicsServer::area_get_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_get_param","area","param"),&PhysicsServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_get_transform","area"),&PhysicsServer::area_get_transform);
+
+	ObjectTypeDB::bind_method(_MD("area_attach_object_instance_ID","area","id"),&PhysicsServer::area_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("area_get_object_instance_ID","area"),&PhysicsServer::area_get_object_instance_ID);
+
+	ObjectTypeDB::bind_method(_MD("area_set_monitor_callback","receiver","method"),&PhysicsServer::area_set_monitor_callback);
+
+	ObjectTypeDB::bind_method(_MD("body_create","mode","init_sleeping"),&PhysicsServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false));
+
+	ObjectTypeDB::bind_method(_MD("body_set_space","body","space"),&PhysicsServer::body_set_space);
+	ObjectTypeDB::bind_method(_MD("body_get_space","body"),&PhysicsServer::body_get_space);
+
+	ObjectTypeDB::bind_method(_MD("body_set_mode","body","mode"),&PhysicsServer::body_set_mode);
+	ObjectTypeDB::bind_method(_MD("body_get_mode","body"),&PhysicsServer::body_get_mode);
+
+	ObjectTypeDB::bind_method(_MD("body_add_shape","body","shape","transform"),&PhysicsServer::body_add_shape,DEFVAL(Transform()));
+	ObjectTypeDB::bind_method(_MD("body_set_shape","body","shape_idx","shape"),&PhysicsServer::body_set_shape);
+	ObjectTypeDB::bind_method(_MD("body_set_shape_transform","body","shape_idx","transform"),&PhysicsServer::body_set_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("body_get_shape_count","body"),&PhysicsServer::body_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("body_get_shape","body","shape_idx"),&PhysicsServer::body_get_shape);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_transform","body","shape_idx"),&PhysicsServer::body_get_shape_transform);
+
+	ObjectTypeDB::bind_method(_MD("body_remove_shape","body","shape_idx"),&PhysicsServer::body_remove_shape);
+	ObjectTypeDB::bind_method(_MD("body_clear_shapes","body"),&PhysicsServer::body_clear_shapes);
+
+	ObjectTypeDB::bind_method(_MD("body_attach_object_instance_ID","body","id"),&PhysicsServer::body_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID","body"),&PhysicsServer::body_get_object_instance_ID);
+
+
+	ObjectTypeDB::bind_method(_MD("body_set_enable_continuous_collision_detection","body","enable"),&PhysicsServer::body_set_enable_continuous_collision_detection);
+	ObjectTypeDB::bind_method(_MD("body_is_continuous_collision_detection_enabled","body"),&PhysicsServer::body_is_continuous_collision_detection_enabled);
+
+
+	//ObjectTypeDB::bind_method(_MD("body_set_user_flags","flags""),&PhysicsServer::body_set_shape,DEFVAL(Transform));
+	//ObjectTypeDB::bind_method(_MD("body_get_user_flags","body","shape_idx","shape"),&PhysicsServer::body_get_shape);
+
+	ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&PhysicsServer::body_set_param);
+	ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&PhysicsServer::body_get_param);
+
+	ObjectTypeDB::bind_method(_MD("body_static_simulate_motion","body","new_xform"),&PhysicsServer::body_static_simulate_motion);
+
+	ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&PhysicsServer::body_set_state);
+	ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&PhysicsServer::body_get_state);
+
+	ObjectTypeDB::bind_method(_MD("body_apply_impulse","body","pos","impulse"),&PhysicsServer::body_apply_impulse);
+	ObjectTypeDB::bind_method(_MD("body_set_axis_velocity","body","axis_velocity"),&PhysicsServer::body_set_axis_velocity);
+
+	ObjectTypeDB::bind_method(_MD("body_add_collision_exception","body","excepted_body"),&PhysicsServer::body_add_collision_exception);
+	ObjectTypeDB::bind_method(_MD("body_remove_collision_exception","body","excepted_body"),&PhysicsServer::body_remove_collision_exception);
+//	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+
+	ObjectTypeDB::bind_method(_MD("body_set_max_contacts_reported","body","amount"),&PhysicsServer::body_set_max_contacts_reported);
+	ObjectTypeDB::bind_method(_MD("body_get_max_contacts_reported","body"),&PhysicsServer::body_get_max_contacts_reported);
+
+	ObjectTypeDB::bind_method(_MD("body_set_omit_force_integration","body","enable"),&PhysicsServer::body_set_omit_force_integration);
+	ObjectTypeDB::bind_method(_MD("body_is_omitting_force_integration","body"),&PhysicsServer::body_is_omitting_force_integration);
+
+	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method","userdata"),&PhysicsServer::body_set_force_integration_callback,DEFVAL(Variant()));
+
+	/* JOINT API */
+/*
+	ObjectTypeDB::bind_method(_MD("joint_set_param","joint","param","value"),&PhysicsServer::joint_set_param);
+	ObjectTypeDB::bind_method(_MD("joint_get_param","joint","param"),&PhysicsServer::joint_get_param);
+
+	ObjectTypeDB::bind_method(_MD("pin_joint_create","anchor","body_a","body_b"),&PhysicsServer::pin_joint_create,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("groove_joint_create","groove1_a","groove2_a","anchor_b","body_a","body_b"),&PhysicsServer::groove_joint_create,DEFVAL(RID()),DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_spring_joint_create","anchor_a","anchor_b","body_a","body_b"),&PhysicsServer::damped_spring_joint_create,DEFVAL(RID()));
+
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_set_param","joint","param","value"),&PhysicsServer::damped_string_joint_set_param,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_get_param","joint","param"),&PhysicsServer::damped_string_joint_get_param);
+
+	ObjectTypeDB::bind_method(_MD("joint_get_type","joint"),&PhysicsServer::joint_get_type);
+*/
+	ObjectTypeDB::bind_method(_MD("free","rid"),&PhysicsServer::free);
+
+	ObjectTypeDB::bind_method(_MD("set_active","active"),&PhysicsServer::set_active);
+
+//	ObjectTypeDB::bind_method(_MD("init"),&PhysicsServer::init);
+//	ObjectTypeDB::bind_method(_MD("step"),&PhysicsServer::step);
+//	ObjectTypeDB::bind_method(_MD("sync"),&PhysicsServer::sync);
+	//ObjectTypeDB::bind_method(_MD("flush_queries"),&PhysicsServer::flush_queries);
+
+
+	BIND_CONSTANT( SHAPE_PLANE );
+	BIND_CONSTANT( SHAPE_RAY );
+	BIND_CONSTANT( SHAPE_SPHERE );
+	BIND_CONSTANT( SHAPE_BOX );
+	BIND_CONSTANT( SHAPE_CAPSULE );
+	BIND_CONSTANT( SHAPE_CONVEX_POLYGON );
+	BIND_CONSTANT( SHAPE_CONCAVE_POLYGON );
+	BIND_CONSTANT( SHAPE_HEIGHTMAP );
+	BIND_CONSTANT( SHAPE_CUSTOM );
+
+
+	BIND_CONSTANT( AREA_PARAM_GRAVITY );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_VECTOR );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_IS_POINT );
+	BIND_CONSTANT( AREA_PARAM_GRAVITY_POINT_ATTENUATION );
+	BIND_CONSTANT( AREA_PARAM_DENSITY );
+	BIND_CONSTANT( AREA_PARAM_PRIORITY );
+
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE );
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_DISABLED );
+	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
+
+	BIND_CONSTANT( BODY_MODE_STATIC );
+	BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE );
+	BIND_CONSTANT( BODY_MODE_RIGID );
+	BIND_CONSTANT( BODY_MODE_CHARACTER );
+
+	BIND_CONSTANT( BODY_PARAM_BOUNCE );
+	BIND_CONSTANT( BODY_PARAM_FRICTION );
+	BIND_CONSTANT( BODY_PARAM_MASS );
+	BIND_CONSTANT( BODY_PARAM_MAX );
+
+	BIND_CONSTANT( BODY_STATE_TRANSFORM );
+	BIND_CONSTANT( BODY_STATE_LINEAR_VELOCITY );
+	BIND_CONSTANT( BODY_STATE_ANGULAR_VELOCITY );
+	BIND_CONSTANT( BODY_STATE_SLEEPING );
+	BIND_CONSTANT( BODY_STATE_CAN_SLEEP );
+/*
+	BIND_CONSTANT( JOINT_PIN );
+	BIND_CONSTANT( JOINT_GROOVE );
+	BIND_CONSTANT( JOINT_DAMPED_SPRING );
+
+	BIND_CONSTANT( DAMPED_STRING_REST_LENGTH );
+	BIND_CONSTANT( DAMPED_STRING_STIFFNESS );
+	BIND_CONSTANT( DAMPED_STRING_DAMPING );
+*/
+//	BIND_CONSTANT( TYPE_BODY );
+//	BIND_CONSTANT( TYPE_AREA );
+
+	BIND_CONSTANT( AREA_BODY_ADDED );
+	BIND_CONSTANT( AREA_BODY_REMOVED );
+
+
+}
+
+
+PhysicsServer::PhysicsServer() {
+
+	ERR_FAIL_COND( singleton!=NULL );
+	singleton=this;
+}
+
+PhysicsServer::~PhysicsServer() {
+
+	singleton=NULL;
+}
+

servers/physics_server.h

@@ -0,0 +1,429 @@
+/*************************************************************************/
+/*  physics_server.h                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef PHYSICS_SERVER_H
+#define PHYSICS_SERVER_H
+
+#include "object.h"
+#include "reference.h"
+
+class PhysicsDirectSpaceState;
+
+class PhysicsDirectBodyState : public Object {
+
+	OBJ_TYPE( PhysicsDirectBodyState, Object );
+protected:
+	static void _bind_methods();
+public:
+
+	virtual Vector3 get_total_gravity() const=0; // get gravity vector working on this body space/area
+	virtual float get_total_density() const=0; // get density of this body space/area
+
+	virtual float get_inverse_mass() const=0; // get the mass
+	virtual Vector3 get_inverse_inertia() const=0; // get density of this body space
+	virtual Matrix3 get_inverse_inertia_tensor() const=0; // get density of this body space
+
+	virtual void set_linear_velocity(const Vector3& p_velocity)=0;
+	virtual Vector3 get_linear_velocity() const=0;
+
+	virtual void set_angular_velocity(const Vector3& p_velocity)=0;
+	virtual Vector3 get_angular_velocity() const=0;
+
+	virtual void set_transform(const Transform& p_transform)=0;
+	virtual Transform get_transform() const=0;
+
+	virtual void add_force(const Vector3& p_force, const Vector3& p_pos)=0;
+
+	virtual void set_sleep_state(bool p_enable)=0;
+	virtual bool is_sleeping() const=0;
+
+	virtual int get_contact_count() const=0;
+
+	virtual Vector3 get_contact_local_pos(int p_contact_idx) const=0;
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const=0;
+	virtual int get_contact_local_shape(int p_contact_idx) const=0;
+
+	virtual RID get_contact_collider(int p_contact_idx) const=0;
+	virtual Vector3 get_contact_collider_pos(int p_contact_idx) const=0;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const=0;
+	virtual Object* get_contact_collider_object(int p_contact_idx) const;
+	virtual int get_contact_collider_shape(int p_contact_idx) const=0;
+	virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const=0;
+
+	virtual real_t get_step() const=0;
+	virtual void integrate_forces();
+
+	virtual PhysicsDirectSpaceState* get_space_state()=0;
+
+	PhysicsDirectBodyState();
+};
+
+
+class PhysicsShapeQueryResult;
+
+
+class PhysicsDirectSpaceState : public Object {
+
+	OBJ_TYPE( PhysicsDirectSpaceState, Object );
+
+	Variant _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
+	Variant _intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
+
+
+protected:
+	static void _bind_methods();
+
+public:
+
+	struct RayResult {
+
+		Vector3 position;
+		Vector3 normal;
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+	};
+
+	virtual bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+	struct ShapeResult {
+
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+
+	};
+
+	virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+	PhysicsDirectSpaceState();
+};
+
+
+class PhysicsShapeQueryResult : public Reference {
+
+	OBJ_TYPE( PhysicsShapeQueryResult, Reference );
+
+	Vector<PhysicsDirectSpaceState::ShapeResult> result;
+
+friend class PhysicsDirectSpaceState;
+
+protected:
+	static void _bind_methods();
+public:
+
+	int get_result_count() const;
+	RID get_result_rid(int p_idx) const;
+	ObjectID get_result_object_id(int p_idx) const;
+	Object* get_result_object(int p_idx) const;
+	int get_result_object_shape(int p_idx) const;
+
+	PhysicsShapeQueryResult();
+};
+
+
+class PhysicsServer : public Object {
+
+	OBJ_TYPE( PhysicsServer, Object );
+
+	static PhysicsServer * singleton;
+
+protected:
+	static void _bind_methods();
+
+public:
+
+	static PhysicsServer * get_singleton();
+
+	enum ShapeType {
+		SHAPE_PLANE, ///< plane:"plane"
+		SHAPE_RAY, ///< float:"length"
+		SHAPE_SPHERE, ///< float:"radius"
+		SHAPE_BOX, ///< vec3:"extents"
+		SHAPE_CAPSULE, ///< dict( float:"radius", float:"height"):capsule
+		SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+		SHAPE_CONCAVE_POLYGON, ///< vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
+		SHAPE_HEIGHTMAP, ///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights"
+		SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+	};
+
+	virtual RID shape_create(ShapeType p_shape)=0;
+	virtual void shape_set_data(RID p_shape, const Variant& p_data)=0;
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias)=0;
+
+	virtual ShapeType shape_get_type(RID p_shape) const=0;
+	virtual Variant shape_get_data(RID p_shape) const=0;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0;
+
+
+	/* SPACE API */
+
+	virtual RID space_create()=0;
+	virtual void space_set_active(RID p_space,bool p_active)=0;
+	virtual bool space_is_active(RID p_space) const=0;
+
+	enum SpaceParameter {
+
+		SPACE_PARAM_CONTACT_RECYCLE_RADIUS,
+		SPACE_PARAM_CONTACT_MAX_SEPARATION,
+		SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION,
+		SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD,
+		SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD,
+		SPACE_PARAM_BODY_TIME_TO_SLEEP,
+		SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO,
+		SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
+	};
+
+	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value)=0;
+	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const=0;
+
+	// this function only works on fixed process, errors and returns null otherwise
+	virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space)=0;
+
+
+	//missing space parameters
+
+	/* AREA API */
+
+	//missing attenuation? missing better override?
+
+
+
+	enum AreaParameter {
+		AREA_PARAM_GRAVITY,
+		AREA_PARAM_GRAVITY_VECTOR,
+		AREA_PARAM_GRAVITY_IS_POINT,
+		AREA_PARAM_GRAVITY_POINT_ATTENUATION,
+		AREA_PARAM_DENSITY,
+		AREA_PARAM_PRIORITY
+	};
+
+	virtual RID area_create()=0;
+
+	virtual void area_set_space(RID p_area, RID p_space)=0;
+	virtual RID area_get_space(RID p_area) const=0;
+
+
+	enum AreaSpaceOverrideMode {
+		AREA_SPACE_OVERRIDE_DISABLED,
+		AREA_SPACE_OVERRIDE_COMBINE,
+		AREA_SPACE_OVERRIDE_REPLACE,
+	};
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode)=0;
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const=0;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform& p_transform=Transform())=0;
+	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape)=0;
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform)=0;
+
+	virtual int area_get_shape_count(RID p_area) const=0;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const=0;
+	virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const=0;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx)=0;
+	virtual void area_clear_shapes(RID p_area)=0;
+
+	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID)=0;
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const=0;
+
+	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value)=0;
+	virtual void area_set_transform(RID p_area, const Transform& p_transform)=0;
+
+	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const=0;
+	virtual Transform area_get_transform(RID p_area) const=0;
+
+	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
+
+	/* BODY API */
+
+	//missing ccd?
+
+	enum BodyMode {
+		BODY_MODE_STATIC,
+		BODY_MODE_STATIC_ACTIVE,
+		BODY_MODE_RIGID,
+		//BODY_MODE_SOFT
+		BODY_MODE_CHARACTER
+	};
+
+	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0;
+
+	virtual void body_set_space(RID p_body, RID p_space)=0;
+	virtual RID body_get_space(RID p_body) const=0;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode)=0;
+	virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const=0;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform& p_transform=Transform())=0;
+	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0;
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform)=0;
+
+	virtual int body_get_shape_count(RID p_body) const=0;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const=0;
+	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const=0;
+
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable)=0;
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const=0;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx)=0;
+	virtual void body_clear_shapes(RID p_body)=0;
+
+	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0;
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const=0;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0;
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
+
+	// common body variables
+	enum BodyParameter {
+		BODY_PARAM_BOUNCE,
+		BODY_PARAM_FRICTION,
+		BODY_PARAM_MASS, ///< unused for static, always infinite
+		BODY_PARAM_MAX,
+	};
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0;
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const=0;
+
+	//advanced simulation
+	virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform)=0;
+
+	//state
+	enum BodyState {
+		BODY_STATE_TRANSFORM,
+		BODY_STATE_LINEAR_VELOCITY,
+		BODY_STATE_ANGULAR_VELOCITY,
+		BODY_STATE_SLEEPING,
+		BODY_STATE_CAN_SLEEP
+	};
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant)=0;
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const=0;
+
+	//do something about it
+	virtual void body_set_applied_force(RID p_body, const Vector3& p_force)=0;
+	virtual Vector3 body_get_applied_force(RID p_body) const=0;
+
+	virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque)=0;
+	virtual Vector3 body_get_applied_torque(RID p_body) const=0;
+
+	virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse)=0;
+	virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity)=0;
+
+	//fix
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b)=0;
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b)=0;
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts)=0;
+	virtual int body_get_max_contacts_reported(RID p_body) const=0;
+
+	//missing remove
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold)=0;
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const=0;
+
+	virtual void body_set_omit_force_integration(RID p_body,bool p_omit)=0;
+	virtual bool body_is_omitting_force_integration(RID p_body) const=0;
+
+	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
+
+	/* JOINT API */
+#if 0
+	enum JointType {
+
+		JOINT_PIN,
+		JOINT_GROOVE,
+		JOINT_DAMPED_SPRING
+	};
+
+	enum JointParam {
+		JOINT_PARAM_BIAS,
+		JOINT_PARAM_MAX_BIAS,
+		JOINT_PARAM_MAX_FORCE,
+	};
+
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value)=0;
+	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const=0;
+
+	virtual RID pin_joint_create(const Vector3& p_anchor,RID p_body_a,RID p_body_b=RID())=0;
+	virtual RID groove_joint_create(const Vector3& p_a_groove1,const Vector3& p_a_groove2, const Vector3& p_b_anchor, RID p_body_a,RID p_body_b)=0;
+	virtual RID damped_spring_joint_create(const Vector3& p_anchor_a,const Vector3& p_anchor_b,RID p_body_a,RID p_body_b=RID())=0;
+
+	enum DampedStringParam {
+		DAMPED_STRING_REST_LENGTH,
+		DAMPED_STRING_STIFFNESS,
+		DAMPED_STRING_DAMPING
+	};
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value)=0;
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const=0;
+
+	virtual JointType joint_get_type(RID p_joint) const=0;	
+#endif
+	/* QUERY API */
+
+	enum AreaBodyStatus {
+		AREA_BODY_ADDED,
+		AREA_BODY_REMOVED
+	};
+
+
+	/* MISC */
+
+	virtual void free(RID p_rid)=0;
+
+	virtual void set_active(bool p_active)=0;
+	virtual void init()=0;
+	virtual void step(float p_step)=0;
+	virtual void sync()=0;
+	virtual void flush_queries()=0;
+	virtual void finish()=0;
+
+	PhysicsServer();
+	~PhysicsServer();
+};
+
+VARIANT_ENUM_CAST( PhysicsServer::ShapeType );
+VARIANT_ENUM_CAST( PhysicsServer::SpaceParameter );
+VARIANT_ENUM_CAST( PhysicsServer::AreaParameter );
+VARIANT_ENUM_CAST( PhysicsServer::AreaSpaceOverrideMode );
+VARIANT_ENUM_CAST( PhysicsServer::BodyMode );
+VARIANT_ENUM_CAST( PhysicsServer::BodyParameter );
+VARIANT_ENUM_CAST( PhysicsServer::BodyState );
+//VARIANT_ENUM_CAST( PhysicsServer::JointParam );
+//VARIANT_ENUM_CAST( PhysicsServer::JointType );
+//VARIANT_ENUM_CAST( PhysicsServer::DampedStringParam );
+//VARIANT_ENUM_CAST( PhysicsServer::ObjectType );
+VARIANT_ENUM_CAST( PhysicsServer::AreaBodyStatus );
+
+#endif

servers/register_server_types.cpp

@@ -0,0 +1,68 @@
+/*************************************************************************/
+/*  register_server_types.cpp                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "register_server_types.h"
+#include "globals.h"
+
+#include "visual_server.h"
+#include "audio_server.h"
+#include "physics_server.h"
+#include "physics_2d_server.h"
+#include "spatial_sound_server.h"
+#include "spatial_sound_2d_server.h"
+
+void register_server_types() {
+
+	Globals::get_singleton()->add_singleton( Globals::Singleton("VisualServer",VisualServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("VS",VisualServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("AudioServer",AudioServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("AS",AudioServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("PhysicsServer",PhysicsServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("PS",PhysicsServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("Physics2DServer",Physics2DServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("PS2D",Physics2DServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("SpatialSoundServer",SpatialSound2DServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("SS",SpatialSound2DServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("SpatialSound2DServer",SpatialSound2DServer::get_singleton()) );
+	Globals::get_singleton()->add_singleton( Globals::Singleton("SS2D",SpatialSound2DServer::get_singleton()) );
+
+
+	ObjectTypeDB::register_virtual_type<Physics2DDirectBodyState>();
+	ObjectTypeDB::register_virtual_type<Physics2DDirectSpaceState>();
+	ObjectTypeDB::register_virtual_type<Physics2DShapeQueryResult>();
+
+	ObjectTypeDB::register_virtual_type<PhysicsDirectBodyState>();
+	ObjectTypeDB::register_virtual_type<PhysicsDirectSpaceState>();
+	ObjectTypeDB::register_virtual_type<PhysicsShapeQueryResult>();
+
+}
+
+void unregister_server_types(){
+
+
+}

servers/register_server_types.h

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

servers/spatial_sound/SCsub

@@ -0,0 +1,7 @@
+Import('env')
+
+env.add_source_files(env.servers_sources,"*.cpp")
+
+Export('env')
+
+

servers/spatial_sound/spatial_sound_server_sw.h

@@ -0,0 +1,249 @@
+/*************************************************/
+/*  spatial_sound_server_sw.h                    */
+/*************************************************/
+/*            This file is part of:              */
+/*                GODOT ENGINE                   */
+/*************************************************/
+/*       Source code within this file is:        */
+/*  (c) 2007-2010 Juan Linietsky, Ariel Manzur   */
+/*             All Rights Reserved.              */
+/*************************************************/
+
+#ifndef SPATIAL_SOUND_SERVER_SW_H
+#define SPATIAL_SOUND_SERVER_SW_H
+
+#include "servers/spatial_sound_server.h"
+#include "octree.h"
+#include "os/thread_safe.h"
+
+
+class SpatialSoundServerSW : public SpatialSoundServer {
+
+	OBJ_TYPE(SpatialSoundServerSW,SpatialSoundServer);
+
+	_THREAD_SAFE_CLASS_
+
+	enum {
+		MAX_CULL_ROOMS=128,
+	       INTERNAL_BUFFER_SIZE=4096,
+	       INTERNAL_BUFFER_MAX_CHANNELS=4,
+	       VOICE_IS_STREAM=-1
+
+	};
+
+
+	struct InternalAudioStream : public AudioServer::AudioStream {
+
+		::SpatialSoundServerSW *owner;
+		virtual int get_channel_count() const;
+		virtual void set_mix_rate(int p_rate); //notify the stream of the mix rate
+		virtual bool mix(int32_t *p_buffer,int p_frames);
+		virtual void update();
+	};
+
+	InternalAudioStream *internal_audio_stream;
+	RID internal_audio_stream_rid;
+	int32_t *internal_buffer;
+	int internal_buffer_channels;
+
+	bool internal_buffer_mix(int32_t *p_buffer,int p_frames);
+
+	struct Room;
+
+	struct Space {
+
+		RID default_room;
+		Set<RID> rooms;
+		Set<RID> sources;
+		Set<RID> listeners;
+
+		Octree<Room> octree;
+	};
+
+	mutable RID_Owner<Space> space_owner;
+
+	struct Room {
+		RID space;
+		Transform transform;
+		Transform inverse_transform;
+		BSP_Tree bounds;
+		RoomReverb reverb;
+		float params[ROOM_PARAM_MAX];
+		bool override_other_sources;
+		OctreeElementID octree_id;
+		int level;
+
+		Room();
+	};
+
+	mutable RID_Owner<Room> room_owner;
+
+
+
+	struct Source {
+
+		struct Voice {
+
+			RID voice_rid;
+			RID sample_rid;
+			bool active;
+			bool restart;
+			float pitch_scale;
+			float volume_scale;
+			int sample_mix_rate;
+
+
+			float last_volume;
+			float last_filter_gain;
+			float last_filter_cutoff;
+			Vector3 last_panning;
+			int last_mix_rate;
+			RoomReverb last_reverb_room;
+			float last_reverb_send;
+
+			Voice();
+			~Voice();
+		};
+
+		struct StreamData {
+
+
+			Vector3 panning;
+			RoomReverb reverb;
+			float reverb_send;
+			float volume;
+			float filter_gain;
+			float filter_cutoff;
+
+			struct FilterState {
+
+				float ha[2];
+				float hb[2];
+			} filter_state[4];
+
+			StreamData() {
+
+				reverb_send=0;
+				reverb=ROOM_REVERB_HALL;
+				volume=1.0;
+				filter_gain=1;
+				filter_cutoff=5000;
+
+			}
+		} stream_data;
+
+		RID space;
+		Transform transform;
+		float params[SOURCE_PARAM_MAX];
+		AudioServer::AudioStream *stream;
+		Vector<Voice> voices;
+		int last_voice;
+
+		Source();
+	};
+
+	mutable RID_Owner<Source> source_owner;
+
+	struct Listener {
+
+		RID space;
+		Transform transform;
+		float params[LISTENER_PARAM_MAX];
+
+		Listener();
+	};
+
+	mutable RID_Owner<Listener> listener_owner;
+	
+	struct ActiveVoice {
+
+		Source *source;
+		int voice;
+		bool operator<(const ActiveVoice& p_voice) const { return (voice==p_voice.voice)?(source<p_voice.source):(voice<p_voice.voice); }
+		ActiveVoice(Source *p_source=NULL,int p_voice=0) { source=p_source; voice=p_voice; }
+	};
+
+	Room *cull_rooms[MAX_CULL_ROOMS];
+
+	Set<Source*> streaming_sources;
+	Set<ActiveVoice> active_voices;
+
+	void _clean_up_owner(RID_OwnerBase *p_owner, const char *p_area);
+	void _update_sources();
+
+public:
+
+	/* SPACE */
+	virtual RID space_create();
+
+	/* ROOM */
+
+	virtual RID room_create();
+	virtual void room_set_space(RID p_room,RID p_space);
+	virtual RID room_get_space(RID p_room) const;
+
+	virtual void room_set_bounds(RID p_room, const BSP_Tree& p_bounds);
+	virtual BSP_Tree room_get_bounds(RID p_room) const;
+	virtual void room_set_transform(RID p_room, const Transform& p_transform);
+	virtual Transform room_get_transform(RID p_room) const;
+
+
+	virtual void room_set_param(RID p_room, RoomParam p_param, float p_value);
+	virtual float room_get_param(RID p_room, RoomParam p_param) const;
+
+	virtual void room_set_level(RID p_room, int p_level);
+	virtual int room_get_level(RID p_room) const;
+
+	virtual void room_set_reverb(RID p_room, RoomReverb p_reverb);
+	virtual RoomReverb room_get_reverb(RID p_room) const;
+
+	//useful for underwater or rooms with very strange conditions
+	virtual void room_set_force_params_to_all_sources(RID p_room, bool p_force);
+	virtual bool room_is_forcing_params_to_all_sources(RID p_room) const;
+
+	/* SOURCE */
+
+	virtual RID source_create(RID p_space);
+
+	virtual void source_set_polyphony(RID p_source,int p_voice_count);
+	virtual int source_get_polyphony(RID p_source) const;
+
+	virtual void source_set_transform(RID p_source, const Transform& p_transform);
+	virtual Transform source_get_transform(RID p_source) const;
+
+	virtual void source_set_param(RID p_source, SourceParam p_param, float p_value);
+	virtual float source_get_param(RID p_source, SourceParam p_param) const;
+
+	virtual void source_set_audio_stream(RID p_source, AudioServer::AudioStream *p_stream); //null to unset
+	virtual SourceVoiceID source_play_sample(RID p_source, RID p_sample, int p_mix_rate, int p_voice=SOURCE_NEXT_VOICE);
+	/* VOICES */
+	virtual void source_voice_set_pitch_scale(RID p_source, SourceVoiceID p_voice, float p_pitch_scale);
+	virtual void source_voice_set_volume_scale_db(RID p_source, SourceVoiceID p_voice, float p_volume);
+
+	virtual bool source_is_voice_active(RID p_source, SourceVoiceID p_voice) const;
+	virtual void source_stop_voice(RID p_source, SourceVoiceID p_voice);
+
+	/* LISTENER */
+
+	virtual RID listener_create();
+	virtual void listener_set_space(RID p_listener, RID p_space);
+
+	virtual void listener_set_transform(RID p_listener, const Transform& p_transform);
+	virtual Transform listener_get_transform(RID p_listener) const;
+
+	virtual void listener_set_param(RID p_listener, ListenerParam p_param, float p_value);
+	virtual float listener_get_param(RID p_listener, ListenerParam p_param) const;
+
+
+	/* MISC */
+
+	virtual void free(RID p_id);
+
+	virtual void init();
+	virtual void update(float p_delta);
+	virtual void finish();
+
+	SpatialSoundServerSW();
+};
+
+#endif // SPATIAL_SOUND_SERVER_SW_H

servers/spatial_sound_2d/SCsub

@@ -0,0 +1,7 @@
+Import('env')
+
+env.add_source_files(env.servers_sources,"*.cpp")
+
+Export('env')
+
+

servers/spatial_sound_2d/spatial_sound_2d_server_sw.h

@@ -0,0 +1,265 @@
+/*************************************************************************/
+/*  spatial_sound_2d_server_sw.h                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#ifndef SPATIAL_SOUND_2D_SERVER_SW_H
+#define SPATIAL_SOUND_2D_SERVER_SW_H
+
+#include "servers/spatial_sound_2d_server.h"
+
+#include "os/thread_safe.h"
+
+
+class SpatialSound2DServerSW : public SpatialSound2DServer {
+
+	OBJ_TYPE(SpatialSound2DServerSW,SpatialSound2DServer);
+
+	_THREAD_SAFE_CLASS_
+
+	enum {
+	       INTERNAL_BUFFER_SIZE=4096,
+	       INTERNAL_BUFFER_MAX_CHANNELS=4,
+	       VOICE_IS_STREAM=-1
+
+	};
+
+
+	struct InternalAudioStream : public AudioServer::AudioStream {
+
+		::SpatialSound2DServerSW *owner;
+		virtual int get_channel_count() const;
+		virtual void set_mix_rate(int p_rate); //notify the stream of the mix rate
+		virtual bool mix(int32_t *p_buffer,int p_frames);
+		virtual void update();
+	};
+
+	InternalAudioStream *internal_audio_stream;
+	RID internal_audio_stream_rid;
+	int32_t *internal_buffer;
+	int internal_buffer_channels;
+
+	bool internal_buffer_mix(int32_t *p_buffer,int p_frames);
+
+	struct Room;
+
+	struct Space {
+
+		RID default_room;
+		Set<RID> rooms;
+		Set<RID> sources;
+		Set<RID> listeners;
+
+		//Octree<Room> octree;
+	};
+
+	mutable RID_Owner<Space> space_owner;
+
+	struct Room {
+		RID space;
+		Matrix32 transform;
+		Matrix32 inverse_transform;
+		DVector<Point2> bounds;
+		RoomReverb reverb;
+		float params[ROOM_PARAM_MAX];
+		bool override_other_sources;
+		//OctreeElementID octree_id;
+		int level;
+
+		Room();
+	};
+
+	mutable RID_Owner<Room> room_owner;
+
+
+
+	struct Source {
+
+		struct Voice {
+
+			RID voice_rid;
+			RID sample_rid;
+			bool active;
+			bool restart;
+			float pitch_scale;
+			float volume_scale;
+			int sample_mix_rate;
+
+
+			float last_volume;
+			float last_filter_gain;
+			float last_filter_cutoff;
+			Vector2 last_panning;
+			int last_mix_rate;
+			RoomReverb last_reverb_room;
+			float last_reverb_send;
+
+			Voice();
+			~Voice();
+		};
+
+		struct StreamData {
+
+
+			Vector2 panning;
+			RoomReverb reverb;
+			float reverb_send;
+			float volume;
+			float filter_gain;
+			float filter_cutoff;
+
+			struct FilterState {
+
+				float ha[2];
+				float hb[2];
+			} filter_state[4];
+
+			StreamData() {
+
+				reverb_send=0;
+				reverb=ROOM_REVERB_HALL;
+				volume=1.0;
+				filter_gain=1;
+				filter_cutoff=5000;
+
+			}
+		} stream_data;
+
+		RID space;
+		Matrix32 transform;
+		float params[SOURCE_PARAM_MAX];
+		AudioServer::AudioStream *stream;
+		Vector<Voice> voices;
+		int last_voice;
+
+		Source();
+	};
+
+	mutable RID_Owner<Source> source_owner;
+
+	struct Listener {
+
+		RID space;
+		Matrix32 transform;
+		float params[LISTENER_PARAM_MAX];
+
+		Listener();
+	};
+
+	mutable RID_Owner<Listener> listener_owner;
+
+	struct ActiveVoice {
+
+		Source *source;
+		int voice;
+		bool operator<(const ActiveVoice& p_voice) const { return (voice==p_voice.voice)?(source<p_voice.source):(voice<p_voice.voice); }
+		ActiveVoice(Source *p_source=NULL,int p_voice=0) { source=p_source; voice=p_voice; }
+	};
+
+//	Room *cull_rooms[MAX_CULL_ROOMS];
+
+	Set<Source*> streaming_sources;
+	Set<ActiveVoice> active_voices;
+
+	void _clean_up_owner(RID_OwnerBase *p_owner, const char *p_area);
+	void _update_sources();
+
+public:
+
+	/* SPACE */
+	virtual RID space_create();
+
+	/* ROOM */
+
+	virtual RID room_create();
+	virtual void room_set_space(RID p_room,RID p_space);
+	virtual RID room_get_space(RID p_room) const;
+
+	virtual void room_set_bounds(RID p_room, const DVector<Point2>& p_bounds);
+	virtual DVector<Point2> room_get_bounds(RID p_room) const;
+	virtual void room_set_transform(RID p_room, const Matrix32& p_transform);
+	virtual Matrix32 room_get_transform(RID p_room) const;
+
+
+	virtual void room_set_param(RID p_room, RoomParam p_param, float p_value);
+	virtual float room_get_param(RID p_room, RoomParam p_param) const;
+
+	virtual void room_set_level(RID p_room, int p_level);
+	virtual int room_get_level(RID p_room) const;
+
+	virtual void room_set_reverb(RID p_room, RoomReverb p_reverb);
+	virtual RoomReverb room_get_reverb(RID p_room) const;
+
+	//useful for underwater or rooms with very strange conditions
+	virtual void room_set_force_params_to_all_sources(RID p_room, bool p_force);
+	virtual bool room_is_forcing_params_to_all_sources(RID p_room) const;
+
+	/* SOURCE */
+
+	virtual RID source_create(RID p_space);
+
+	virtual void source_set_polyphony(RID p_source,int p_voice_count);
+	virtual int source_get_polyphony(RID p_source) const;
+
+	virtual void source_set_transform(RID p_source, const Matrix32& p_transform);
+	virtual Matrix32 source_get_transform(RID p_source) const;
+
+	virtual void source_set_param(RID p_source, SourceParam p_param, float p_value);
+	virtual float source_get_param(RID p_source, SourceParam p_param) const;
+
+	virtual void source_set_audio_stream(RID p_source, AudioServer::AudioStream *p_stream); //null to unset
+	virtual SourceVoiceID source_play_sample(RID p_source, RID p_sample, int p_mix_rate, int p_voice=SOURCE_NEXT_VOICE);
+	/* VOICES */
+	virtual void source_voice_set_pitch_scale(RID p_source, SourceVoiceID p_voice, float p_pitch_scale);
+	virtual void source_voice_set_volume_scale_db(RID p_source, SourceVoiceID p_voice, float p_volume);
+
+	virtual bool source_is_voice_active(RID p_source, SourceVoiceID p_voice) const;
+	virtual void source_stop_voice(RID p_source, SourceVoiceID p_voice);
+
+	/* LISTENER */
+
+	virtual RID listener_create();
+	virtual void listener_set_space(RID p_listener, RID p_space);
+
+	virtual void listener_set_transform(RID p_listener, const Matrix32& p_transform);
+	virtual Matrix32 listener_get_transform(RID p_listener) const;
+
+	virtual void listener_set_param(RID p_listener, ListenerParam p_param, float p_value);
+	virtual float listener_get_param(RID p_listener, ListenerParam p_param) const;
+
+
+	/* MISC */
+
+	virtual void free(RID p_id);
+
+	virtual void init();
+	virtual void update(float p_delta);
+	virtual void finish();
+
+	SpatialSound2DServerSW();
+};
+
+#endif // SPATIAL_SOUND_2D_SERVER_SW_H