-improved physics ccd

-html5 exporter works again
-disable repeat on image loader by default
-can change shape offset en tileset, texture offset was broken

bin/tests/test_physics.cpp

@@ -90,7 +90,7 @@ protected:
 		PhysicsServer * ps = PhysicsServer::get_singleton();
 
 		RID mesh_instance = vs->instance_create2(type_mesh_map[p_shape],scenario);
-		RID body = ps->body_create(p_body,!p_active_default);
+		RID body = ps->body_create(p_body,!p_active_default);		
 		ps->body_set_space(body,space);
 		ps->body_set_param(body,PhysicsServer::BODY_PARAM_BOUNCE,0.5);
 		//todo set space

bin/tests/test_physics_2d.cpp

@@ -270,6 +270,7 @@ protected:
 		RID body = ps->body_create();
 		ps->body_add_shape(body,body_shape_data[p_shape].shape);
 		ps->body_set_space(body,space);
+		ps->body_set_continuous_collision_detection_mode(body,Physics2DServer::CCD_MODE_CAST_SHAPE);
 		ps->body_set_state(body,Physics2DServer::BODY_STATE_TRANSFORM,p_xform);
 
 //		print_line("add body with xform: "+p_xform);

core/globals.cpp

@@ -240,6 +240,7 @@ bool Globals::_load_resource_pack(const String& p_pack) {
 
 	//if data.pck is found, all directory access will be from here
 	DirAccess::make_default<DirAccessPack>(DirAccess::ACCESS_RESOURCES);
+	using_datapack=true;
 
 	return true;
 }
@@ -1327,6 +1328,10 @@ void Globals::set_disable_platform_override(bool p_disable) {
 	disable_platform_override=p_disable;
 }
 
+bool Globals::is_using_datapack() const {
+
+	return using_datapack;
+}
 
 void Globals::_bind_methods() {
 
@@ -1439,6 +1444,8 @@ Globals::Globals() {
 	custom_prop_info["render/mipmap_policy"]=PropertyInfo(Variant::INT,"render/mipmap_policy",PROPERTY_HINT_ENUM,"Allow,Allow For Po2,Disallow");
 	custom_prop_info["render/thread_model"]=PropertyInfo(Variant::INT,"render/thread_model",PROPERTY_HINT_ENUM,"Single-Unsafe,Single-Safe,Multi-Threaded");
 	set("display/emulate_touchscreen",false);
+
+	using_datapack=false;
 }
 
 

core/globals.h

@@ -69,6 +69,7 @@ protected:
 	String resource_path;
 	HashMap<String,PropertyInfo> custom_prop_info;
 	bool disable_platform_override;
+	bool using_datapack;
 
 	
 	bool _set(const StringName& p_name, const Variant& p_value);
@@ -127,6 +128,8 @@ public:
 
 	void register_global_defaults();
 
+	bool is_using_datapack() const;
+
 	Globals();	
 	~Globals();
 

core/io/file_access_compressed.cpp

@@ -26,396 +26,396 @@
 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
-#include "file_access_compressed.h"
-#include "print_string.h"
-void FileAccessCompressed::configure(const String& p_magic, Compression::Mode p_mode, int p_block_size) {
-
-	magic=p_magic.ascii().get_data();
-	if (magic.length()>4)
-		magic=magic.substr(0,4);
-	else {
-		while(magic.length()<4)
-			magic+=" ";
-	}
-
-	cmode=p_mode;
-	block_size=p_block_size;
-
-}
-
-
-#define WRITE_FIT(m_bytes) \
-{\
-if (write_pos+(m_bytes) > write_max) {\
-   write_max=write_pos+(m_bytes);\
-}\
-if (write_max > write_buffer_size) {\
-	write_buffer_size = nearest_power_of_2( write_max );\
-	buffer.resize(write_buffer_size);\
-	write_ptr=buffer.ptr();\
-}\
-}
-
-
-Error FileAccessCompressed::open_after_magic(FileAccess *p_base) {
-
-
-	f=p_base;
-	cmode=(Compression::Mode)f->get_32();
-	block_size=f->get_32();
-	read_total=f->get_32();
-	int bc = (read_total/block_size)+1;
-	int acc_ofs=f->get_pos()+bc*4;
-	int max_bs=0;
-	for(int i=0;i<bc;i++) {
-
-		ReadBlock rb;
-		rb.offset=acc_ofs;
-		rb.csize=f->get_32();
-		acc_ofs+=rb.csize;
-		max_bs=MAX(max_bs,rb.csize);
-		read_blocks.push_back(rb);
-
-
-	}
-
-	comp_buffer.resize(max_bs);
-	buffer.resize(block_size);
-	read_ptr=buffer.ptr();
-	f->get_buffer(comp_buffer.ptr(),read_blocks[0].csize);
-	at_end=false;
-	read_eof=false;
-	read_block_count=bc;
-	read_block_size=read_blocks.size()==1?read_total:block_size;
-
-	Compression::decompress(buffer.ptr(),read_block_size,comp_buffer.ptr(),read_blocks[0].csize,cmode);
-	read_block=0;
-	read_pos=0;
-
-	return OK;
-
-}
-
-Error FileAccessCompressed::_open(const String& p_path, int p_mode_flags){
-
-	ERR_FAIL_COND_V(p_mode_flags==READ_WRITE,ERR_UNAVAILABLE);
-
-	if (f)
-		close();
-
-
-	Error err;
-	f = FileAccess::open(p_path,p_mode_flags,&err);
-	if (err!=OK) {
-		//not openable
-
-		f=NULL;
-		return err;
-	}
-
-	if (p_mode_flags&WRITE) {
-
-		buffer.clear();
-		writing=true;
-		write_pos=0;
-		write_buffer_size=256;
-		buffer.resize(256);
-		write_max=0;
-		write_ptr=buffer.ptr();
-
-
-
-		//don't store anything else unless it's done saving!
-	} else {
-
-		char rmagic[5];
-		f->get_buffer((uint8_t*)rmagic,4);
-		rmagic[4]=0;
-		if (magic!=rmagic) {
-			memdelete(f);
-			f=NULL;
-			return ERR_FILE_UNRECOGNIZED;
-		}
-
-		open_after_magic(f);
-
-	}
-
-	return OK;
-
-}
-void FileAccessCompressed::close(){
-
-	if (!f)
-		return;
-
-
-	if (writing) {
-		//save block table and all compressed blocks
-
-		CharString mgc = magic.utf8();
-		f->store_buffer((const uint8_t*)mgc.get_data(),mgc.length()); //write header 4
-		f->store_32(cmode); //write compression mode 4
-		f->store_32(block_size); //write block size 4
-		f->store_32(write_max); //max amount of data written 4
-		int bc=(write_max/block_size)+1;
-
-		for(int i=0;i<bc;i++) {
-			f->store_32(0); //compressed sizes, will update later
-		}
-
-
-		Vector<int> block_sizes;
-		for(int i=0;i<bc;i++) {
-
-			int bl = i==(bc-1) ? write_max % block_size : block_size;
-			uint8_t *bp = &write_ptr[i*block_size];
-
-			Vector<uint8_t> cblock;
-			cblock.resize(Compression::get_max_compressed_buffer_size(bl,cmode));
-			int s = Compression::compress(cblock.ptr(),bp,bl,cmode);
-
-			f->store_buffer(cblock.ptr(),s);
-			block_sizes.push_back(s);
-		}
-
-		f->seek(16); //ok write block sizes
-		for(int i=0;i<bc;i++)
-			f->store_32(block_sizes[i]);
-		f->seek_end();
-		f->store_buffer((const uint8_t*)mgc.get_data(),mgc.length()); //magic at the end too
-
-		buffer.clear();
-
-	} else {
-
-
-		comp_buffer.clear();
-		buffer.clear();
-		read_blocks.clear();
-	}
-
-	memdelete(f);
-	f=NULL;
-
-}
-
-bool FileAccessCompressed::is_open() const{
-
-	return f!=NULL;
-}
-
-void FileAccessCompressed::seek(size_t p_position){
-
-	ERR_FAIL_COND(!f);
-	if (writing) {
-
-		ERR_FAIL_COND(p_position>write_max);
-
-		write_pos=p_position;
-
-	} else {
-
-		ERR_FAIL_COND(p_position>read_total);
-		if (p_position==read_total) {
-			at_end=true;
-		} else {
-
-			int block_idx = p_position/block_size;
-			if (block_idx!=read_block) {
-
-				read_block=block_idx;
-				f->seek(read_blocks[read_block].offset);
-				f->get_buffer(comp_buffer.ptr(),read_blocks[read_block].csize);
-				Compression::decompress(buffer.ptr(),read_blocks.size()==1?read_total:block_size,comp_buffer.ptr(),read_blocks[read_block].csize,cmode);
-				read_block_size=read_block==read_block_count-1?read_total%block_size:block_size;
-			}
-
-			read_pos=p_position%block_size;
-		}
-	}
-}
-
-
-void FileAccessCompressed::seek_end(int64_t p_position){
-
-	ERR_FAIL_COND(!f);
-	if (writing) {
-
-		seek(write_max+p_position);
-	} else {
-
-		seek(read_total+p_position);
-
-	}
-
-
-}
-size_t FileAccessCompressed::get_pos() const{
-
-	ERR_FAIL_COND_V(!f,0);
-	if (writing) {
-
-		return write_pos;
-	} else {
-
-		return read_block*block_size+read_pos;
-	}
-
-}
-size_t FileAccessCompressed::get_len() const{
-
-	ERR_FAIL_COND_V(!f,0);
-	if (writing) {
-
-		return write_max;
-	} else {
-		return read_total;
-	}
-}
-
-bool FileAccessCompressed::eof_reached() const{
-
-	ERR_FAIL_COND_V(!f,false);
-	if (writing) {
-		return false;
-	} else {
-		return read_eof;
-	}
-}
-
-uint8_t FileAccessCompressed::get_8() const{
-
-	ERR_FAIL_COND_V(writing,0);
-	ERR_FAIL_COND_V(!f,0);
-
-	if (at_end) {
-		read_eof=true;
-		return 0;
-	}
-
-	uint8_t ret = read_ptr[read_pos];
-
-	read_pos++;
-	if (read_pos>=read_block_size) {
-		read_block++;
-
-		if (read_block<read_block_count) {
-			//read another block of compressed data
-			f->get_buffer(comp_buffer.ptr(),read_blocks[read_block].csize);
-			Compression::decompress(buffer.ptr(),read_blocks.size()==1?read_total:block_size,comp_buffer.ptr(),read_blocks[read_block].csize,cmode);
-			read_block_size=read_block==read_block_count-1?read_total%block_size:block_size;
-			read_pos=0;
-
-		} else {
-			read_block--;
-			at_end=true;
-			ret =0;
-		}
-	}
-
-	return ret;
-
-}
-int FileAccessCompressed::get_buffer(uint8_t *p_dst, int p_length) const{
-
-	ERR_FAIL_COND_V(writing,0);
-	ERR_FAIL_COND_V(!f,0);
-
-	if (at_end) {
-		read_eof=true;
-		return 0;
-	}
-
-
-	for(int i=0;i<p_length;i++) {
-
-
-		p_dst[i]=read_ptr[read_pos];
-		read_pos++;
-		if (read_pos>=read_block_size) {
-			read_block++;
-
-			if (read_block<read_block_count) {
-				//read another block of compressed data
-				f->get_buffer(comp_buffer.ptr(),read_blocks[read_block].csize);
-				Compression::decompress(buffer.ptr(),read_blocks.size()==1?read_total:block_size,comp_buffer.ptr(),read_blocks[read_block].csize,cmode);
-				read_block_size=read_block==read_block_count-1?read_total%block_size:block_size;
-				read_pos=0;
-
-			} else {
-				read_block--;
-				at_end=true;
-				if (i<p_length-1)
-				 read_eof=true;
-				return i;
-
-			}
-		}
-
-	}
-
-	return p_length;
-
-}
-
-Error FileAccessCompressed::get_error() const{
-
-	return read_eof?ERR_FILE_EOF:OK;
-}
-
-void FileAccessCompressed::store_8(uint8_t p_dest){
-
-	ERR_FAIL_COND(!f);
-	ERR_FAIL_COND(!writing);
-
-	WRITE_FIT(1);
-	write_ptr[write_pos++]=p_dest;
-
-}
-
-bool FileAccessCompressed::file_exists(const String& p_name){
-
-	FileAccess *fa = FileAccess::open(p_name,FileAccess::READ);
-	if (!fa)
-		return false;
-	memdelete(fa);
-	return true;
-}
-
-uint64_t FileAccessCompressed::_get_modified_time(const String& p_file) {
-
-	if (f)
-		return f->get_modified_time(p_file);
-	else
-		return 0;
-}
-
-FileAccessCompressed::FileAccessCompressed() {
-
-	f=NULL;
-	magic="GCMP";
-	block_size=4096;
-	cmode=Compression::MODE_FASTLZ;
-	writing=false;
-	write_ptr=0;
-	write_buffer_size=0;
-	write_max=0;
-	block_size=0;
-	read_eof=false;
-	at_end=false;
-	read_total=0;
-	read_ptr=NULL;
-	read_block=0;
-	read_block_count=0;
-	read_block_size=0;
-	read_pos=0;
-
-}
-
-FileAccessCompressed::~FileAccessCompressed(){
-
-	if (f)
-		close();
-
-}
+#include "file_access_compressed.h"
+#include "print_string.h"
+void FileAccessCompressed::configure(const String& p_magic, Compression::Mode p_mode, int p_block_size) {
+
+	magic=p_magic.ascii().get_data();
+	if (magic.length()>4)
+		magic=magic.substr(0,4);
+	else {
+		while(magic.length()<4)
+			magic+=" ";
+	}
+
+	cmode=p_mode;
+	block_size=p_block_size;
+
+}
+
+
+#define WRITE_FIT(m_bytes) \
+{\
+if (write_pos+(m_bytes) > write_max) {\
+   write_max=write_pos+(m_bytes);\
+}\
+if (write_max > write_buffer_size) {\
+	write_buffer_size = nearest_power_of_2( write_max );\
+	buffer.resize(write_buffer_size);\
+	write_ptr=buffer.ptr();\
+}\
+}
+
+
+Error FileAccessCompressed::open_after_magic(FileAccess *p_base) {
+
+
+	f=p_base;
+	cmode=(Compression::Mode)f->get_32();
+	block_size=f->get_32();
+	read_total=f->get_32();
+	int bc = (read_total/block_size)+1;
+	int acc_ofs=f->get_pos()+bc*4;
+	int max_bs=0;
+	for(int i=0;i<bc;i++) {
+
+		ReadBlock rb;
+		rb.offset=acc_ofs;
+		rb.csize=f->get_32();
+		acc_ofs+=rb.csize;
+		max_bs=MAX(max_bs,rb.csize);
+		read_blocks.push_back(rb);
+
+
+	}
+
+	comp_buffer.resize(max_bs);
+	buffer.resize(block_size);
+	read_ptr=buffer.ptr();
+	f->get_buffer(comp_buffer.ptr(),read_blocks[0].csize);
+	at_end=false;
+	read_eof=false;
+	read_block_count=bc;
+	read_block_size=read_blocks.size()==1?read_total:block_size;
+
+	Compression::decompress(buffer.ptr(),read_block_size,comp_buffer.ptr(),read_blocks[0].csize,cmode);
+	read_block=0;
+	read_pos=0;
+
+	return OK;
+
+}
+
+Error FileAccessCompressed::_open(const String& p_path, int p_mode_flags){
+
+	ERR_FAIL_COND_V(p_mode_flags==READ_WRITE,ERR_UNAVAILABLE);
+
+	if (f)
+		close();
+
+
+	Error err;
+	f = FileAccess::open(p_path,p_mode_flags,&err);
+	if (err!=OK) {
+		//not openable
+
+		f=NULL;
+		return err;
+	}
+
+	if (p_mode_flags&WRITE) {
+
+		buffer.clear();
+		writing=true;
+		write_pos=0;
+		write_buffer_size=256;
+		buffer.resize(256);
+		write_max=0;
+		write_ptr=buffer.ptr();
+
+
+
+		//don't store anything else unless it's done saving!
+	} else {
+
+		char rmagic[5];
+		f->get_buffer((uint8_t*)rmagic,4);
+		rmagic[4]=0;
+		if (magic!=rmagic) {
+			memdelete(f);
+			f=NULL;
+			return ERR_FILE_UNRECOGNIZED;
+		}
+
+		open_after_magic(f);
+
+	}
+
+	return OK;
+
+}
+void FileAccessCompressed::close(){
+
+	if (!f)
+		return;
+
+
+	if (writing) {
+		//save block table and all compressed blocks
+
+		CharString mgc = magic.utf8();
+		f->store_buffer((const uint8_t*)mgc.get_data(),mgc.length()); //write header 4
+		f->store_32(cmode); //write compression mode 4
+		f->store_32(block_size); //write block size 4
+		f->store_32(write_max); //max amount of data written 4
+		int bc=(write_max/block_size)+1;
+
+		for(int i=0;i<bc;i++) {
+			f->store_32(0); //compressed sizes, will update later
+		}
+
+
+		Vector<int> block_sizes;
+		for(int i=0;i<bc;i++) {
+
+			int bl = i==(bc-1) ? write_max % block_size : block_size;
+			uint8_t *bp = &write_ptr[i*block_size];
+
+			Vector<uint8_t> cblock;
+			cblock.resize(Compression::get_max_compressed_buffer_size(bl,cmode));
+			int s = Compression::compress(cblock.ptr(),bp,bl,cmode);
+
+			f->store_buffer(cblock.ptr(),s);
+			block_sizes.push_back(s);
+		}
+
+		f->seek(16); //ok write block sizes
+		for(int i=0;i<bc;i++)
+			f->store_32(block_sizes[i]);
+		f->seek_end();
+		f->store_buffer((const uint8_t*)mgc.get_data(),mgc.length()); //magic at the end too
+
+		buffer.clear();
+
+	} else {
+
+
+		comp_buffer.clear();
+		buffer.clear();
+		read_blocks.clear();
+	}
+
+	memdelete(f);
+	f=NULL;
+
+}
+
+bool FileAccessCompressed::is_open() const{
+
+	return f!=NULL;
+}
+
+void FileAccessCompressed::seek(size_t p_position){
+
+	ERR_FAIL_COND(!f);
+	if (writing) {
+
+		ERR_FAIL_COND(p_position>write_max);
+
+		write_pos=p_position;
+
+	} else {
+
+		ERR_FAIL_COND(p_position>read_total);
+		if (p_position==read_total) {
+			at_end=true;
+		} else {
+
+			int block_idx = p_position/block_size;
+			if (block_idx!=read_block) {
+
+				read_block=block_idx;
+				f->seek(read_blocks[read_block].offset);
+				f->get_buffer(comp_buffer.ptr(),read_blocks[read_block].csize);
+				Compression::decompress(buffer.ptr(),read_blocks.size()==1?read_total:block_size,comp_buffer.ptr(),read_blocks[read_block].csize,cmode);
+				read_block_size=read_block==read_block_count-1?read_total%block_size:block_size;
+			}
+
+			read_pos=p_position%block_size;
+		}
+	}
+}
+
+
+void FileAccessCompressed::seek_end(int64_t p_position){
+
+	ERR_FAIL_COND(!f);
+	if (writing) {
+
+		seek(write_max+p_position);
+	} else {
+
+		seek(read_total+p_position);
+
+	}
+
+
+}
+size_t FileAccessCompressed::get_pos() const{
+
+	ERR_FAIL_COND_V(!f,0);
+	if (writing) {
+
+		return write_pos;
+	} else {
+
+		return read_block*block_size+read_pos;
+	}
+
+}
+size_t FileAccessCompressed::get_len() const{
+
+	ERR_FAIL_COND_V(!f,0);
+	if (writing) {
+
+		return write_max;
+	} else {
+		return read_total;
+	}
+}
+
+bool FileAccessCompressed::eof_reached() const{
+
+	ERR_FAIL_COND_V(!f,false);
+	if (writing) {
+		return false;
+	} else {
+		return read_eof;
+	}
+}
+
+uint8_t FileAccessCompressed::get_8() const{
+
+	ERR_FAIL_COND_V(writing,0);
+	ERR_FAIL_COND_V(!f,0);
+
+	if (at_end) {
+		read_eof=true;
+		return 0;
+	}
+
+	uint8_t ret = read_ptr[read_pos];
+
+	read_pos++;
+	if (read_pos>=read_block_size) {
+		read_block++;
+
+		if (read_block<read_block_count) {
+			//read another block of compressed data
+			f->get_buffer(comp_buffer.ptr(),read_blocks[read_block].csize);
+			Compression::decompress(buffer.ptr(),read_blocks.size()==1?read_total:block_size,comp_buffer.ptr(),read_blocks[read_block].csize,cmode);
+			read_block_size=read_block==read_block_count-1?read_total%block_size:block_size;
+			read_pos=0;
+
+		} else {
+			read_block--;
+			at_end=true;
+			ret =0;
+		}
+	}
+
+	return ret;
+
+}
+int FileAccessCompressed::get_buffer(uint8_t *p_dst, int p_length) const{
+
+	ERR_FAIL_COND_V(writing,0);
+	ERR_FAIL_COND_V(!f,0);
+
+	if (at_end) {
+		read_eof=true;
+		return 0;
+	}
+
+
+	for(int i=0;i<p_length;i++) {
+
+
+		p_dst[i]=read_ptr[read_pos];
+		read_pos++;
+		if (read_pos>=read_block_size) {
+			read_block++;
+
+			if (read_block<read_block_count) {
+				//read another block of compressed data
+				f->get_buffer(comp_buffer.ptr(),read_blocks[read_block].csize);
+				Compression::decompress(buffer.ptr(),read_blocks.size()==1?read_total:block_size,comp_buffer.ptr(),read_blocks[read_block].csize,cmode);
+				read_block_size=read_block==read_block_count-1?read_total%block_size:block_size;
+				read_pos=0;
+
+			} else {
+				read_block--;
+				at_end=true;
+				if (i<p_length-1)
+				 read_eof=true;
+				return i;
+
+			}
+		}
+
+	}
+
+	return p_length;
+
+}
+
+Error FileAccessCompressed::get_error() const{
+
+	return read_eof?ERR_FILE_EOF:OK;
+}
+
+void FileAccessCompressed::store_8(uint8_t p_dest){
+
+	ERR_FAIL_COND(!f);
+	ERR_FAIL_COND(!writing);
+
+	WRITE_FIT(1);
+	write_ptr[write_pos++]=p_dest;
+
+}
+
+bool FileAccessCompressed::file_exists(const String& p_name){
+
+	FileAccess *fa = FileAccess::open(p_name,FileAccess::READ);
+	if (!fa)
+		return false;
+	memdelete(fa);
+	return true;
+}
+
+uint64_t FileAccessCompressed::_get_modified_time(const String& p_file) {
+
+	if (f)
+		return f->get_modified_time(p_file);
+	else
+		return 0;
+}
+
+FileAccessCompressed::FileAccessCompressed() {
+
+	f=NULL;
+	magic="GCMP";
+	block_size=16384;
+	cmode=Compression::MODE_DEFLATE;
+	writing=false;
+	write_ptr=0;
+	write_buffer_size=0;
+	write_max=0;
+	block_size=0;
+	read_eof=false;
+	at_end=false;
+	read_total=0;
+	read_ptr=NULL;
+	read_block=0;
+	read_block_count=0;
+	read_block_size=0;
+	read_pos=0;
+
+}
+
+FileAccessCompressed::~FileAccessCompressed(){
+
+	if (f)
+		close();
+
+}

core/map.h

@@ -115,7 +115,7 @@ private:
 		Element* _nil;
 		int size_cache;
 		
-		_Data() {
+		_FORCE_INLINE_ _Data() {
 #ifdef GLOBALNIL_DISABLED
 			_nil = memnew_allocator( Element, A );
 			_nil->parent=_nil->left=_nil->right=_nil;

core/math/math_2d.cpp

@@ -255,6 +255,16 @@ Vector2 Vector2::cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, co
 
 }
 
+Vector2 Vector2::slide(const Vector2& p_vec) const {
+
+	return p_vec - *this * this->dot(p_vec);
+}
+Vector2 Vector2::reflect(const Vector2& p_vec) const {
+
+	return p_vec - *this * this->dot(p_vec) * 2.0;
+
+}
+
 
 bool Rect2::intersects_segment(const Point2& p_from, const Point2& p_to, Point2* r_pos,Point2* r_normal) const {
 

core/math/math_2d.h

@@ -105,6 +105,8 @@ struct Vector2 {
 	Vector2 cubic_interpolate(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const;
 	Vector2 cubic_interpolate_soft(const Vector2& p_b,const Vector2& p_pre_a, const Vector2& p_post_b,float p_t) const;
 
+	Vector2 slide(const Vector2& p_vec) const;
+	Vector2 reflect(const Vector2& p_vec) const;
 
 	Vector2 operator+(const Vector2& p_v) const;
 	void operator+=(const Vector2& p_v);

core/variant_call.cpp

@@ -298,6 +298,8 @@ static void _call_##m_type##_##m_method(Variant& r_ret,Variant& p_self,const Var
 	VCALL_LOCALMEM1R(Vector2,snapped);
 	VCALL_LOCALMEM0R(Vector2,get_aspect);
 	VCALL_LOCALMEM1R(Vector2,dot);
+	VCALL_LOCALMEM1R(Vector2,slide);
+	VCALL_LOCALMEM1R(Vector2,reflect);
 //	VCALL_LOCALMEM1R(Vector2,cross);
 
 	VCALL_LOCALMEM0R(Rect2,get_area);
@@ -1192,6 +1194,8 @@ _VariantCall::addfunc(Variant::m_vtype,Variant::m_ret,_SCS(#m_method),VCALL(m_cl
 	ADDFUNC1(VECTOR2,VECTOR2,Vector2,snapped,VECTOR2,"by",varray());
 	ADDFUNC0(VECTOR2,REAL,Vector2,get_aspect,varray());
 	ADDFUNC1(VECTOR2,REAL,Vector2,dot,VECTOR2,"with",varray());
+	ADDFUNC1(VECTOR2,REAL,Vector2,slide,VECTOR2,"vec",varray());
+	ADDFUNC1(VECTOR2,REAL,Vector2,reflect,VECTOR2,"vec",varray());
 	//ADDFUNC1(VECTOR2,REAL,Vector2,cross,VECTOR2,"with",varray());
 
 	ADDFUNC0(RECT2,REAL,Rect2,get_area,varray());

demos/2d/platformer/bullet.gd

@@ -15,6 +15,7 @@ func disable():
 
 func _ready():
 	# Initalization here
+	get_node("Timer").start()
 	pass
 
 

demos/2d/platformer/bullet.xml

@@ -1,14 +1,13 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<resource_file type="PackedScene" subresource_count="5" version="0.99" version_name="Godot Engine v0.99.3037-pre-beta">
-	<ext_resource path="res://bullet.png" type="Texture"></ext_resource>
-	<ext_resource path="res://bullet.gd" type="GDScript"></ext_resource>
-	<resource type="CircleShape2D" path="local://0">
-		<string name="resource/name"> "" </string>
+<resource_file type="PackedScene" subresource_count="5" version="1.0" version_name="Godot Engine v1.0.3917-beta1">
+	<ext_resource path="res://bullet.*" type="Texture"></ext_resource>
+	<ext_resource path="res://bullet.*" type="Script"></ext_resource>
+	<resource type="CircleShape2D" path="local://1">
 		<real name="custom_solver_bias"> 0 </real>
 		<real name="radius"> 10 </real>
-		<resource name="script/script"></resource>
+
 	</resource>
-	<resource type="Animation" path="local://1">
+	<resource type="Animation" path="local://2">
 		<string name="resource/name"> "shutdown" </string>
 		<real name="length"> 1.5 </real>
 		<bool name="loop"> False </bool>
@@ -16,13 +15,13 @@
 		<string name="tracks/0/type"> "value" </string>
 		<node_path name="tracks/0/path"> "particles:config/emitting" </node_path>
 		<int name="tracks/0/interp"> 1 </int>
-		<dictionary name="tracks/0/keys">
+		<dictionary name="tracks/0/keys" shared="false">
 			<string> "cont" </string>
 			<bool> False </bool>
 			<string> "transitions" </string>
 			<real_array  len="1"> 				1 </real_array>
 			<string> "values" </string>
-			<array  len="1">
+			<array  len="1" shared="false">
 				<bool> False </bool>
 			</array>
 			<string> "times" </string>
@@ -31,13 +30,13 @@
 		<string name="tracks/1/type"> "value" </string>
 		<node_path name="tracks/1/path"> "sprite:visibility/self_opacity" </node_path>
 		<int name="tracks/1/interp"> 1 </int>
-		<dictionary name="tracks/1/keys">
+		<dictionary name="tracks/1/keys" shared="false">
 			<string> "cont" </string>
 			<bool> True </bool>
 			<string> "transitions" </string>
 			<real_array  len="2"> 				1, 1 </real_array>
 			<string> "values" </string>
-			<array  len="2">
+			<array  len="2" shared="false">
 				<real> 1 </real>
 				<real> 0 </real>
 			</array>
@@ -47,14 +46,14 @@
 		<string name="tracks/2/type"> "method" </string>
 		<node_path name="tracks/2/path"> "." </node_path>
 		<int name="tracks/2/interp"> 1 </int>
-		<dictionary name="tracks/2/keys">
+		<dictionary name="tracks/2/keys" shared="false">
 			<string> "transitions" </string>
 			<real_array  len="1"> 				1 </real_array>
 			<string> "values" </string>
-			<array  len="1">
-				<dictionary>
+			<array  len="1" shared="false">
+				<dictionary  shared="false">
 					<string> "args" </string>
-					<array  len="0">
+					<array  len="0" shared="false">
 					</array>
 					<string> "method" </string>
 					<string> "queue_free" </string>
@@ -63,33 +62,25 @@
 			<string> "times" </string>
 			<real_array  len="1"> 				1.31 </real_array>
 		</dictionary>
-		<resource name="script/script"></resource>
+
 	</resource>
 	<main_resource>
-		<string name="resource/name"> "" </string>
-		<dictionary name="_bundled">
+		<dictionary name="_bundled" shared="false">
 			<string> "names" </string>
-			<string_array  len="109">
+			<string_array  len="111">
 				<string> "bullet" </string>
 				<string> "RigidBody2D" </string>
-				<string> "process/process" </string>
-				<string> "process/fixed_process" </string>
-				<string> "process/input" </string>
-				<string> "process/unhandled_input" </string>
-				<string> "process/mode" </string>
 				<string> "visibility/visible" </string>
-				<string> "visibility/toplevel" </string>
 				<string> "visibility/opacity" </string>
 				<string> "visibility/self_opacity" </string>
 				<string> "visibility/on_top" </string>
-				<string> "visibility/blend_mode" </string>
-				<string> "transform/notify" </string>
 				<string> "transform/pos" </string>
 				<string> "transform/rot" </string>
 				<string> "transform/scale" </string>
 				<string> "shape_count" </string>
 				<string> "shapes/0/shape" </string>
 				<string> "shapes/0/transform" </string>
+				<string> "shapes/0/trigger" </string>
 				<string> "mode" </string>
 				<string> "mass" </string>
 				<string> "friction" </string>
@@ -106,8 +97,10 @@
 				<string> "__meta__" </string>
 				<string> "particles" </string>
 				<string> "Particles2D" </string>
+				<string> "visibility/blend_mode" </string>
 				<string> "config/amount" </string>
 				<string> "config/lifetime" </string>
+				<string> "config/time_scale" </string>
 				<string> "config/preprocess" </string>
 				<string> "config/emit_timeout" </string>
 				<string> "config/emitting" </string>
@@ -115,11 +108,14 @@
 				<string> "config/half_extents" </string>
 				<string> "config/local_space" </string>
 				<string> "config/explosiveness" </string>
+				<string> "config/flip_h" </string>
+				<string> "config/flip_v" </string>
 				<string> "config/texture" </string>
 				<string> "params/direction" </string>
 				<string> "params/spread" </string>
 				<string> "params/linear_velocity" </string>
 				<string> "params/spin_velocity" </string>
+				<string> "params/orbit_velocity" </string>
 				<string> "params/gravity_direction" </string>
 				<string> "params/gravity_strength" </string>
 				<string> "params/radial_accel" </string>
@@ -132,6 +128,7 @@
 				<string> "randomness/spread" </string>
 				<string> "randomness/linear_velocity" </string>
 				<string> "randomness/spin_velocity" </string>
+				<string> "randomness/orbit_velocity" </string>
 				<string> "randomness/gravity_direction" </string>
 				<string> "randomness/gravity_strength" </string>
 				<string> "randomness/radial_accel" </string>
@@ -149,6 +146,7 @@
 				<string> "phase_2/color" </string>
 				<string> "phase_3/pos" </string>
 				<string> "phase_3/color" </string>
+				<string> "emission_points" </string>
 				<string> "sprite" </string>
 				<string> "Sprite" </string>
 				<string> "texture" </string>
@@ -164,6 +162,7 @@
 				<string> "region_rect" </string>
 				<string> "CollisionShape2D" </string>
 				<string> "shape" </string>
+				<string> "trigger" </string>
 				<string> "Timer" </string>
 				<string> "wait_time" </string>
 				<string> "one_shot" </string>
@@ -173,8 +172,9 @@
 				<string> "playback/process_mode" </string>
 				<string> "playback/default_blend_time" </string>
 				<string> "root/root" </string>
-				<string> "speed" </string>
 				<string> "anims/shutdown" </string>
+				<string> "playback/active" </string>
+				<string> "playback/speed" </string>
 				<string> "blend_times" </string>
 				<string> "autoplay" </string>
 				<string> "disable" </string>
@@ -187,99 +187,150 @@
 			<string> "node_count" </string>
 			<int> 6 </int>
 			<string> "variants" </string>
-			<array  len="27">
-				<bool> False </bool>
-				<int> 0 </int>
+			<array  len="27" shared="false">
 				<bool> True </bool>
 				<real> 1 </real>
 				<vector2> 0, 0 </vector2>
 				<real> 0 </real>
 				<vector2> 1, 1 </vector2>
 				<int> 1 </int>
-				<resource  resource_type="CircleShape2D" path="local://0">  </resource>
+				<resource  resource_type="Shape2D" path="local://1">  </resource>
 				<matrix32> 1, 0, 0, 1, 0, 0 </matrix32>
-				<resource  resource_type="GDScript" path="res://bullet.gd">  </resource>
-				<dictionary>
+				<bool> False </bool>
+				<int> 0 </int>
+				<int> 2 </int>
+				<resource  resource_type="Script" path="res://bullet.*">  </resource>
+				<dictionary  shared="false">
 					<string> "__editor_plugin_states__" </string>
-					<dictionary>
+					<dictionary  shared="false">
 						<string> "Script" </string>
-						<dictionary>
+						<dictionary  shared="false">
 							<string> "current" </string>
-							<int> 0 </int>
+							<int> 2 </int>
 							<string> "sources" </string>
-							<array  len="3">
+							<array  len="3" shared="false">
 								<string> "res://enemy.gd" </string>
 								<string> "res://player.gd" </string>
 								<string> "res://bullet.gd" </string>
 							</array>
 						</dictionary>
 						<string> "2D" </string>
-						<dictionary>
+						<dictionary  shared="false">
+							<string> "pixel_snap" </string>
+							<bool> False </bool>
 							<string> "zoom" </string>
 							<real> 3.424785 </real>
 							<string> "ofs" </string>
 							<vector2> -74.7573, -35.9676 </vector2>
 						</dictionary>
 						<string> "3D" </string>
-						<dictionary>
+						<dictionary  shared="false">
 							<string> "zfar" </string>
 							<real> 500 </real>
 							<string> "fov" </string>
 							<real> 45 </real>
-							<string> "window_mode" </string>
-							<int> 0 </int>
-							<string> "window_0" </string>
-							<dictionary>
-								<string> "distance" </string>
-								<real> 10.07268 </real>
-								<string> "default_light" </string>
-								<bool> True </bool>
-								<string> "x_rot" </string>
-								<real> 0.337 </real>
-								<string> "y_rot" </string>
-								<real> -0.575 </real>
-								<string> "show_grid" </string>
-								<bool> True </bool>
-								<string> "show_origin" </string>
-								<bool> True </bool>
-								<string> "pos" </string>
-								<vector3> 0, 0, 0 </vector3>
-							</dictionary>
+							<string> "viewports" </string>
+							<array  len="4" shared="false">
+								<dictionary  shared="false">
+									<string> "distance" </string>
+									<real> 4 </real>
+									<string> "x_rot" </string>
+									<real> 0 </real>
+									<string> "y_rot" </string>
+									<real> 0 </real>
+									<string> "use_orthogonal" </string>
+									<bool> False </bool>
+									<string> "use_environment" </string>
+									<bool> False </bool>
+									<string> "pos" </string>
+									<vector3> 0, 0, 0 </vector3>
+								</dictionary>
+								<dictionary  shared="false">
+									<string> "distance" </string>
+									<real> 4 </real>
+									<string> "x_rot" </string>
+									<real> 0 </real>
+									<string> "y_rot" </string>
+									<real> 0 </real>
+									<string> "use_orthogonal" </string>
+									<bool> False </bool>
+									<string> "use_environment" </string>
+									<bool> False </bool>
+									<string> "pos" </string>
+									<vector3> 0, 0, 0 </vector3>
+								</dictionary>
+								<dictionary  shared="false">
+									<string> "distance" </string>
+									<real> 4 </real>
+									<string> "x_rot" </string>
+									<real> 0 </real>
+									<string> "y_rot" </string>
+									<real> 0 </real>
+									<string> "use_orthogonal" </string>
+									<bool> False </bool>
+									<string> "use_environment" </string>
+									<bool> False </bool>
+									<string> "pos" </string>
+									<vector3> 0, 0, 0 </vector3>
+								</dictionary>
+								<dictionary  shared="false">
+									<string> "distance" </string>
+									<real> 4 </real>
+									<string> "x_rot" </string>
+									<real> 0 </real>
+									<string> "y_rot" </string>
+									<real> 0 </real>
+									<string> "use_orthogonal" </string>
+									<bool> False </bool>
+									<string> "use_environment" </string>
+									<bool> False </bool>
+									<string> "pos" </string>
+									<vector3> 0, 0, 0 </vector3>
+								</dictionary>
+							</array>
+							<string> "viewport_mode" </string>
+							<int> 1 </int>
+							<string> "default_light" </string>
+							<bool> True </bool>
+							<string> "show_grid" </string>
+							<bool> True </bool>
+							<string> "show_origin" </string>
+							<bool> True </bool>
 							<string> "znear" </string>
 							<real> 0.1 </real>
 						</dictionary>
 					</dictionary>
 					<string> "__editor_run_settings__" </string>
-					<dictionary>
+					<dictionary  shared="false">
 						<string> "custom_args" </string>
 						<string> "-l $scene" </string>
 						<string> "run_mode" </string>
 						<int> 0 </int>
 					</dictionary>
 					<string> "__editor_plugin_screen__" </string>
-					<string> "Script" </string>
+					<string> "2D" </string>
 				</dictionary>
 				<real> 0.559322 </real>
 				<int> 24 </int>
 				<real> 0.1 </real>
-				<resource  resource_type="Texture" path="res://bullet.png">  </resource>
+				<resource  resource_type="Texture" path="res://bullet.*">  </resource>
 				<real> 10 </real>
-				<int> 2 </int>
 				<color> 1, 1, 1, 1 </color>
 				<color> 1, 0, 0, 0 </color>
 				<color> 0, 0, 0, 1 </color>
-				<resource name=""></resource>				<rect2> 0, 0, 0, 0 </rect2>
+				<vector2_array  len="0"> 					 </vector2_array>
+				<rect2> 0, 0, 0, 0 </rect2>
 				<node_path> ".." </node_path>
-				<resource  resource_type="Animation" path="local://1">  </resource>
-				<array  len="0">
+				<resource  resource_type="Animation" path="local://2">  </resource>
+				<array  len="0" shared="false">
 				</array>
 				<string> "" </string>
 			</array>
 			<string> "nodes" </string>
-			<int_array  len="356"> 				-1, -1, 1, 0, -1, 32, 2, 0, 3, 0, 4, 0, 5, 0, 6, 1, 7, 2, 8, 0, 9, 3, 10, 3, 11, 2, 12, 1, 13, 2, 14, 4, 15, 5, 16, 6, 17, 7, 18, 8, 19, 9, 20, 1, 21, 3, 22, 3, 23, 5, 24, 0, 25, 0, 26, 1, 27, 0, 28, 2, 29, 2, 30, 4, 31, 5, 32, 10, 33, 11, 0, 0, 0, 35, 34, -1, 59, 2, 2, 3, 0, 4, 0, 5, 0, 6, 1, 7, 2, 8, 0, 9, 12, 10, 3, 11, 2, 12, 7, 13, 0, 14, 4, 15, 5, 16, 6, 36, 13, 37, 14, 38, 5, 39, 5, 40, 2, 41, 4, 42, 4, 43, 0, 44, 3, 45, 15, 46, 5, 47, 16, 48, 5, 49, 5, 50, 5, 51, 5, 52, 5, 53, 5, 54, 5, 55, 3, 56, 5, 57, 5, 58, 5, 59, 5, 60, 5, 61, 5, 62, 5, 63, 5, 64, 5, 65, 5, 66, 5, 67, 5, 68, 5, 69, 5, 70, 17, 71, 5, 72, 18, 73, 3, 74, 19, 75, 3, 76, 20, 77, 3, 78, 20, 32, 21, 0, 0, 0, 80, 79, -1, 27, 2, 0, 3, 0, 4, 0, 5, 0, 6, 1, 7, 2, 8, 0, 9, 3, 10, 3, 11, 2, 12, 1, 13, 0, 14, 4, 15, 5, 16, 6, 81, 15, 82, 2, 83, 4, 84, 0, 85, 0, 86, 7, 87, 7, 88, 1, 89, 18, 90, 0, 91, 22, 32, 21, 0, 0, 0, 92, 92, -1, 17, 2, 0, 3, 0, 4, 0, 5, 0, 6, 1, 7, 2, 8, 0, 9, 3, 10, 3, 11, 2, 12, 1, 13, 2, 14, 4, 15, 5, 16, 6, 93, 8, 32, 21, 0, 0, 0, 94, 94, -1, 9, 2, 0, 3, 0, 4, 0, 5, 0, 6, 1, 95, 3, 96, 2, 97, 2, 32, 21, 0, 0, 0, 99, 98, -1, 13, 2, 0, 3, 0, 4, 0, 5, 0, 6, 1, 100, 7, 101, 5, 102, 23, 103, 3, 104, 24, 105, 25, 106, 26, 32, 21, 0 </int_array>
+			<int_array  len="282"> 				-1, -1, 1, 0, -1, 25, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 9, 5, 10, 6, 11, 7, 12, 8, 13, 9, 14, 1, 15, 1, 16, 3, 17, 8, 18, 10, 19, 9, 20, 8, 21, 0, 22, 0, 23, 2, 24, 3, 25, 11, 26, 12, 0, 0, 0, 28, 27, -1, 57, 2, 0, 3, 13, 4, 1, 5, 0, 29, 5, 6, 2, 7, 3, 8, 4, 30, 14, 31, 15, 32, 1, 33, 3, 34, 3, 35, 0, 36, 2, 37, 2, 38, 8, 39, 1, 40, 8, 41, 8, 42, 16, 43, 3, 44, 17, 45, 3, 46, 3, 47, 3, 48, 3, 49, 3, 50, 3, 51, 3, 52, 3, 53, 1, 54, 3, 55, 3, 56, 3, 57, 3, 58, 3, 59, 3, 60, 3, 61, 3, 62, 3, 63, 3, 64, 3, 65, 3, 66, 3, 67, 3, 68, 3, 69, 10, 70, 3, 71, 18, 72, 1, 73, 19, 74, 1, 75, 20, 76, 1, 77, 20, 78, 21, 0, 0, 0, 80, 79, -1, 18, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 81, 16, 82, 0, 83, 2, 84, 8, 85, 8, 86, 5, 87, 5, 88, 9, 89, 18, 90, 8, 91, 22, 0, 0, 0, 92, 92, -1, 9, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 93, 6, 94, 8, 0, 0, 0, 95, 95, -1, 3, 96, 1, 97, 0, 98, 8, 0, 0, 0, 100, 99, -1, 8, 101, 5, 102, 3, 103, 23, 104, 24, 105, 0, 106, 1, 107, 25, 108, 26, 0 </int_array>
 			<string> "conns" </string>
-			<int_array  len="6"> 				4, 0, 108, 107, 2, 0 </int_array>
+			<int_array  len="6"> 				4, 0, 110, 109, 2, 0 </int_array>
 		</dictionary>
-		<resource name="script/script"></resource>
+
 	</main_resource>
 </resource_file>

demos/2d/platformer/enemy.gd

@@ -95,3 +95,4 @@ func _ready():
 	rc_right=get_node("raycast_right")
 
 
+

demos/2d/platformer/moving_platform.xml

@@ -10,7 +10,7 @@
 	<main_resource>
 		<dictionary name="_bundled" shared="false">
 			<string> "names" </string>
-			<string_array  len="39">
+			<string_array  len="46">
 				<string> "moving_platform" </string>
 				<string> "Node2D" </string>
 				<string> "visibility/visible" </string>
@@ -25,16 +25,23 @@
 				<string> "motion" </string>
 				<string> "cycle" </string>
 				<string> "platform" </string>
-				<string> "StaticBody2D" </string>
+				<string> "RigidBody2D" </string>
 				<string> "shape_count" </string>
 				<string> "shapes/0/shape" </string>
 				<string> "shapes/0/transform" </string>
 				<string> "shapes/0/trigger" </string>
-				<string> "simulate_motion" </string>
-				<string> "constant_linear_velocity" </string>
-				<string> "constant_angular_velocity" </string>
+				<string> "mode" </string>
+				<string> "mass" </string>
 				<string> "friction" </string>
 				<string> "bounce" </string>
+				<string> "custom_integrator" </string>
+				<string> "continuous_cd" </string>
+				<string> "contacts_reported" </string>
+				<string> "contact_monitor" </string>
+				<string> "active" </string>
+				<string> "can_sleep" </string>
+				<string> "velocity/linear" </string>
+				<string> "velocity/angular" </string>
 				<string> "Sprite" </string>
 				<string> "texture" </string>
 				<string> "centered" </string>
@@ -58,7 +65,7 @@
 			<string> "node_count" </string>
 			<int> 4 </int>
 			<string> "variants" </string>
-			<array  len="16" shared="false">
+			<array  len="17" shared="false">
 				<bool> True </bool>
 				<real> 1 </real>
 				<vector2> 0, 0 </vector2>
@@ -180,14 +187,15 @@
 				<resource  resource_type="Shape2D" path="local://1">  </resource>
 				<matrix32> 1, -0, 0, 1, 0, 0 </matrix32>
 				<bool> False </bool>
-				<resource  resource_type="Texture" path="res://moving_platform.*">  </resource>
+				<int> 3 </int>
 				<int> 0 </int>
+				<resource  resource_type="Texture" path="res://moving_platform.*">  </resource>
 				<color> 1, 1, 1, 1 </color>
 				<rect2> 0, 0, 0, 0 </rect2>
 				<vector2_array  len="4"> 					-88, -24, 88, -24, 88, 24, -88, 24 </vector2_array>
 			</array>
 			<string> "nodes" </string>
-			<int_array  len="136"> 				-1, -1, 1, 0, -1, 11, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 9, 5, 10, 6, 11, 2, 12, 1, 0, 0, 0, 14, 13, -1, 16, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 15, 7, 16, 8, 17, 9, 18, 10, 19, 0, 20, 2, 21, 3, 22, 1, 23, 3, 0, 1, 0, 24, 24, -1, 18, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 25, 11, 26, 0, 27, 2, 28, 10, 29, 10, 30, 7, 31, 7, 32, 12, 33, 13, 34, 10, 35, 14, 0, 1, 0, 36, 36, -1, 9, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 37, 12, 38, 15, 0 </int_array>
+			<int_array  len="150"> 				-1, -1, 1, 0, -1, 11, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 9, 5, 10, 6, 11, 2, 12, 1, 0, 0, 0, 14, 13, -1, 23, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 15, 7, 16, 8, 17, 9, 18, 10, 19, 11, 20, 1, 21, 1, 22, 3, 23, 10, 24, 10, 25, 12, 26, 10, 27, 0, 28, 0, 29, 2, 30, 3, 0, 1, 0, 31, 31, -1, 18, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 32, 13, 33, 0, 34, 2, 35, 10, 36, 10, 37, 7, 38, 7, 39, 12, 40, 14, 41, 10, 42, 15, 0, 1, 0, 43, 43, -1, 9, 2, 0, 3, 1, 4, 1, 5, 0, 6, 2, 7, 3, 8, 4, 44, 12, 45, 16, 0 </int_array>
 			<string> "conns" </string>
 			<int_array  len="0"> 				 </int_array>
 		</dictionary>

doc/base/classes.xml

@@ -11027,6 +11027,9 @@
 			<return type="Vector2">
 			</return>
 			<description>
+			Return the global, unscaled, screen pointer coordinates.
+			If the 2D viewport has been scaled, it may not work well
+			with [Camera] or controls.
 			</description>
 		</method>
 		<method name="get_mouse_speed" qualifiers="const" >
@@ -15411,6 +15414,7 @@
 	<brief_description>
 	</brief_description>
 	<description>
+	explain ownership, and that node does not need to own itself
 	</description>
 	<methods>
 		<method name="pack"  >
@@ -15419,6 +15423,8 @@
 			<argument index="0" name="path" type="Node">
 			</argument>
 			<description>
+			Pack will ignore any sub-nodes not owned by given
+			node. See [Node.set_owner].
 			</description>
 		</method>
 		<method name="instance" qualifiers="const" >
@@ -17490,7 +17496,7 @@
 		</constant>
 		<constant name="BODY_MODE_STATIC" value="0">
 		</constant>
-		<constant name="BODY_MODE_STATIC_ACTIVE" value="1">
+		<constant name="BODY_MODE_KINEMATIC" value="1">
 		</constant>
 		<constant name="BODY_MODE_RIGID" value="2">
 		</constant>
@@ -18420,7 +18426,7 @@
 		</constant>
 		<constant name="BODY_MODE_STATIC" value="0">
 		</constant>
-		<constant name="BODY_MODE_STATIC_ACTIVE" value="1">
+		<constant name="BODY_MODE_KINEMATIC" value="1">
 		</constant>
 		<constant name="BODY_MODE_RIGID" value="2">
 		</constant>
@@ -20861,7 +20867,7 @@
 	<constants>
 		<constant name="MODE_STATIC" value="1">
 		</constant>
-		<constant name="MODE_STATIC_ACTIVE" value="3">
+		<constant name="MODE_KINEMATIC" value="3">
 		</constant>
 		<constant name="MODE_RIGID" value="0">
 		</constant>
@@ -21143,7 +21149,7 @@
 		<constant name="MODE_STATIC" value="1">
 			Static mode (does not move, can't be moved).
 		</constant>
-		<constant name="MODE_STATIC_ACTIVE" value="3">
+		<constant name="MODE_KINEMATIC" value="3">
 		</constant>
 		<constant name="MODE_RIGID" value="0">
 			Rigid body, can move and rotate.

drivers/gles2/rasterizer_gles2.cpp

@@ -7511,7 +7511,7 @@ void RasterizerGLES2::init() {
 	pvr_supported=extensions.has("GL_IMG_texture_compression_pvrtc");
 	etc_supported=extensions.has("GL_OES_compressed_ETC1_RGB8_texture");
 	use_depth24 = extensions.has("GL_OES_depth24");
-	s3tc_supported = extensions.has("GL_EXT_texture_compression_dxt1") || extensions.has("GL_EXT_texture_compression_s3tc");
+	s3tc_supported = extensions.has("GL_EXT_texture_compression_dxt1") || extensions.has("GL_EXT_texture_compression_s3tc") || extensions.has("WEBGL_compressed_texture_s3tc");
 	use_half_float = extensions.has("GL_OES_vertex_half_float");
 
 

main/main.cpp

@@ -1037,18 +1037,24 @@ bool Main::start() {
 
 				if (!absolute) {
 
-					int sep=local_game_path.find_last("/");
+					if (Globals::get_singleton()->is_using_datapack()) {
 
-					if (sep==-1) {
-						DirAccess *da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
-						local_game_path=da->get_current_dir()+"/"+local_game_path;
-						memdelete(da)						;
-					} else {
+						local_game_path="res://"+local_game_path;
 
-						DirAccess *da = DirAccess::open(local_game_path.substr(0,sep));
-						if (da) {
-							local_game_path=da->get_current_dir()+"/"+local_game_path.substr(sep+1,local_game_path.length());;
-							memdelete(da);
+					} else {
+						int sep=local_game_path.find_last("/");
+
+						if (sep==-1) {
+							DirAccess *da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
+							local_game_path=da->get_current_dir()+"/"+local_game_path;
+							memdelete(da)						;
+						} else {
+
+							DirAccess *da = DirAccess::open(local_game_path.substr(0,sep));
+							if (da) {
+								local_game_path=da->get_current_dir()+"/"+local_game_path.substr(sep+1,local_game_path.length());;
+								memdelete(da);
+							}
 						}
 					}
 

platform/android/.old/os_android.cpp

@@ -223,6 +223,7 @@ void OS_Android::process_touch(int p_what,int p_pointer, const Vector<TouchPos>&
 				ev.mouse_button.y=touch[0].pos.y;
 				ev.mouse_button.global_x=touch[0].pos.x;
 				ev.mouse_button.global_y=touch[0].pos.y;
+				input->set_mouse_pos(Point2(ev.mouse_button.x,ev.mouse_button.y));
 				main_loop->input_event(ev);
 
 
@@ -259,6 +260,7 @@ void OS_Android::process_touch(int p_what,int p_pointer, const Vector<TouchPos>&
 				ev.mouse_button.global_x=touch[0].pos.x;
 				ev.mouse_button.global_y=touch[0].pos.y;
 				last_mouse=touch[0].pos;
+				input->set_mouse_pos(Point2(ev.mouse_button.x,ev.mouse_button.y));
 				main_loop->input_event(ev);
 			}
 

platform/iphone/app_delegate.mm

@@ -165,7 +165,7 @@ static int frame_count = 0;
 	printf("**************** app delegate init\n");
 	CGRect rect = [[UIScreen mainScreen] bounds];
 
-	[application setStatusBarHidden:YES animation:NO];
+	[application setStatusBarHidden:YES withAnimation:UIStatusBarAnimationNone];
 	// disable idle timer
 	application.idleTimerDisabled = YES;
 

platform/javascript/export/export.cpp

@@ -178,10 +178,11 @@ static void _fix_files(Vector<uint8_t>& html,uint64_t p_data_size) {
 	}
 
 	CharString cs = strnew.utf8();
-	html.resize(cs.size());
-	for(int i=9;i<cs.size();i++) {
+	html.resize(cs.length());
+	for(int i=9;i<cs.length();i++) {
 		html[i]=cs[i];
 	}
+
 }
 
 struct JSExportData {

scene/2d/physics_body_2d.cpp

@@ -70,27 +70,7 @@ real_t StaticBody2D::get_constant_angular_velocity() const {
 
 	return constant_angular_velocity;
 }
-
-
-void StaticBody2D::_state_notify(Object *p_object) {
-
-	if (!pre_xform)
-		return;
-
-	Physics2DDirectBodyState *p2d = (Physics2DDirectBodyState*)p_object;
-	setting=true;
-
-	Matrix32 new_xform = p2d->get_transform();
-	*pre_xform=new_xform;
-	set_block_transform_notify(true);
-	set_global_transform(new_xform);
-	set_block_transform_notify(false);
-
-	setting=false;
-
-
-}
-
+#if 0
 void StaticBody2D::_update_xform() {
 
 	if (!pre_xform || !pending)
@@ -112,58 +92,7 @@ void StaticBody2D::_update_xform() {
 	pending=false;
 
 }
-
-void StaticBody2D::_notification(int p_what) {
-
-	switch(p_what) {
-
-		case NOTIFICATION_ENTER_SCENE: {
-
-			if (pre_xform)
-				*pre_xform = get_global_transform();
-			pending=false;
-		} break;
-		case NOTIFICATION_TRANSFORM_CHANGED: {
-
-			if (simulating_motion && !pending && is_inside_scene() && !setting && !get_scene()->is_editor_hint()) {
-
-
-				call_deferred(SceneStringNames::get_singleton()->_update_xform);
-				pending=true;
-			}
-
-		} break;
-	}
-
-
-}
-
-void StaticBody2D::set_simulate_motion(bool p_enable) {
-
-	if (p_enable==simulating_motion)
-		return;
-	simulating_motion=p_enable;
-
-	if (p_enable) {
-		pre_xform = memnew( Matrix32 );
-		if (is_inside_scene())
-			*pre_xform=get_transform();
-//		query = Physics2DServer::get_singleton()->query_create(this,"_state_notify",Variant());
-	//	Physics2DServer::get_singleton()->query_body_direct_state(query,get_rid());
-		Physics2DServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_state_notify");
-
-	} else {
-		memdelete( pre_xform );
-		pre_xform=NULL;
-		Physics2DServer::get_singleton()->body_set_force_integration_callback(get_rid(),NULL,StringName());
-		pending=false;
-	}
-}
-
-bool StaticBody2D::is_simulating_motion() const {
-
-	return simulating_motion;
-}
+#endif
 
 
 void StaticBody2D::set_friction(real_t p_friction){
@@ -194,10 +123,6 @@ real_t StaticBody2D::get_bounce() const{
 
 void StaticBody2D::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("set_simulate_motion","enabled"),&StaticBody2D::set_simulate_motion);
-	ObjectTypeDB::bind_method(_MD("is_simulating_motion"),&StaticBody2D::is_simulating_motion);
-	ObjectTypeDB::bind_method(_MD("_update_xform"),&StaticBody2D::_update_xform);
-	ObjectTypeDB::bind_method(_MD("_state_notify"),&StaticBody2D::_state_notify);
 	ObjectTypeDB::bind_method(_MD("set_constant_linear_velocity","vel"),&StaticBody2D::set_constant_linear_velocity);
 	ObjectTypeDB::bind_method(_MD("set_constant_angular_velocity","vel"),&StaticBody2D::set_constant_angular_velocity);
 	ObjectTypeDB::bind_method(_MD("get_constant_linear_velocity"),&StaticBody2D::get_constant_linear_velocity);
@@ -208,7 +133,6 @@ void StaticBody2D::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("set_bounce","bounce"),&StaticBody2D::set_bounce);
 	ObjectTypeDB::bind_method(_MD("get_bounce"),&StaticBody2D::get_bounce);
 
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL,"simulate_motion"),_SCS("set_simulate_motion"),_SCS("is_simulating_motion"));
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2,"constant_linear_velocity"),_SCS("set_constant_linear_velocity"),_SCS("get_constant_linear_velocity"));
 	ADD_PROPERTY(PropertyInfo(Variant::REAL,"constant_angular_velocity"),_SCS("set_constant_angular_velocity"),_SCS("get_constant_angular_velocity"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"friction",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_friction"),_SCS("get_friction"));
@@ -217,10 +141,6 @@ void StaticBody2D::_bind_methods() {
 
 StaticBody2D::StaticBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_STATIC) {
 
-	simulating_motion=false;
-	pre_xform=NULL;
-	setting=false;
-	pending=false;
 	constant_angular_velocity=0;
 	bounce=0;
 	friction=1;
@@ -230,10 +150,6 @@ StaticBody2D::StaticBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_STATIC)
 
 StaticBody2D::~StaticBody2D() {
 
-	if (pre_xform)
-		memdelete(pre_xform);
-	//if (query.is_valid())
-	//	Physics2DServer::get_singleton()->free(query);
 }
 
 
@@ -385,7 +301,7 @@ void RigidBody2D::_direct_state_changed(Object *p_state) {
 			toadd[i].id=obj;
 			toadd[i].shape=shape;
 
-			bool found=false;
+//			bool found=false;
 
 			Map<ObjectID,BodyState>::Element *E=contact_monitor->body_map.find(obj);
 			if (!E) {
@@ -437,7 +353,8 @@ void RigidBody2D::_direct_state_changed(Object *p_state) {
 	}
 
 	set_block_transform_notify(true); // don't want notify (would feedback loop)
-	set_global_transform(state->get_transform());
+	if (mode!=MODE_KINEMATIC)
+		set_global_transform(state->get_transform());
 	linear_velocity=state->get_linear_velocity();
 	angular_velocity=state->get_angular_velocity();
 	active=!state->is_sleeping();
@@ -448,10 +365,6 @@ void RigidBody2D::_direct_state_changed(Object *p_state) {
 	state=NULL;
 }
 
-void RigidBody2D::_notification(int p_what) {
-
-
-}
 
 void RigidBody2D::set_mode(Mode p_mode) {
 
@@ -467,9 +380,9 @@ void RigidBody2D::set_mode(Mode p_mode) {
 			Physics2DServer::get_singleton()->body_set_mode(get_rid(),Physics2DServer::BODY_MODE_STATIC);
 
 		} break;
-		case MODE_STATIC_ACTIVE: {
+		case MODE_KINEMATIC: {
 
-			Physics2DServer::get_singleton()->body_set_mode(get_rid(),Physics2DServer::BODY_MODE_STATIC_ACTIVE);
+			Physics2DServer::get_singleton()->body_set_mode(get_rid(),Physics2DServer::BODY_MODE_KINEMATIC);
 
 		} break;
 		case MODE_CHARACTER: {
@@ -643,16 +556,17 @@ Vector2 RigidBody2D::get_applied_force() const {
 	return Physics2DServer::get_singleton()->body_get_applied_force(get_rid());
 };
 
-void RigidBody2D::set_use_continuous_collision_detection(bool p_enable) {
 
-	ccd=p_enable;
-	Physics2DServer::get_singleton()->body_set_enable_continuous_collision_detection(get_rid(),p_enable);
-}
+void RigidBody2D::set_continuous_collision_detection_mode(CCDMode p_mode) {
+
+	ccd_mode=p_mode;
+	Physics2DServer::get_singleton()->body_set_continuous_collision_detection_mode(get_rid(),Physics2DServer::CCDMode(p_mode));
 
-bool RigidBody2D::is_using_continuous_collision_detection() const {
+}
 
+RigidBody2D::CCDMode RigidBody2D::get_continuous_collision_detection_mode() const {
 
-	return ccd;
+	return ccd_mode;
 }
 
 
@@ -716,8 +630,8 @@ void RigidBody2D::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("set_contact_monitor","enabled"),&RigidBody2D::set_contact_monitor);
 	ObjectTypeDB::bind_method(_MD("is_contact_monitor_enabled"),&RigidBody2D::is_contact_monitor_enabled);
 
-	ObjectTypeDB::bind_method(_MD("set_use_continuous_collision_detection","enable"),&RigidBody2D::set_use_continuous_collision_detection);
-	ObjectTypeDB::bind_method(_MD("is_using_continuous_collision_detection"),&RigidBody2D::is_using_continuous_collision_detection);
+	ObjectTypeDB::bind_method(_MD("set_continuous_collision_detection_mode","mode"),&RigidBody2D::set_continuous_collision_detection_mode);
+	ObjectTypeDB::bind_method(_MD("get_continuous_collision_detection_mode"),&RigidBody2D::get_continuous_collision_detection_mode);
 
 	ObjectTypeDB::bind_method(_MD("set_axis_velocity","axis_velocity"),&RigidBody2D::set_axis_velocity);
 	ObjectTypeDB::bind_method(_MD("apply_impulse","pos","impulse"),&RigidBody2D::apply_impulse);
@@ -737,13 +651,13 @@ void RigidBody2D::_bind_methods() {
 
 	BIND_VMETHOD(MethodInfo("_integrate_forces",PropertyInfo(Variant::OBJECT,"state:Physics2DDirectBodyState")));
 
-	ADD_PROPERTY( PropertyInfo(Variant::INT,"mode",PROPERTY_HINT_ENUM,"Rigid,Static,Character,Static Active"),_SCS("set_mode"),_SCS("get_mode"));
+	ADD_PROPERTY( PropertyInfo(Variant::INT,"mode",PROPERTY_HINT_ENUM,"Rigid,Static,Character,Kinematic"),_SCS("set_mode"),_SCS("get_mode"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"mass",PROPERTY_HINT_EXP_RANGE,"0.01,65535,0.01"),_SCS("set_mass"),_SCS("get_mass"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"weight",PROPERTY_HINT_EXP_RANGE,"0.01,65535,0.01",PROPERTY_USAGE_EDITOR),_SCS("set_weight"),_SCS("get_weight"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"friction",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_friction"),_SCS("get_friction"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"bounce",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_bounce"),_SCS("get_bounce"));
 	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"custom_integrator"),_SCS("set_use_custom_integrator"),_SCS("is_using_custom_integrator"));
-	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"continuous_cd"),_SCS("set_use_continuous_collision_detection"),_SCS("is_using_continuous_collision_detection"));
+	ADD_PROPERTY( PropertyInfo(Variant::INT,"continuous_cd",PROPERTY_HINT_ENUM,"Disabled,Cast Ray,Cast Shape"),_SCS("set_continuous_collision_detection_mode"),_SCS("get_continuous_collision_detection_mode"));
 	ADD_PROPERTY( PropertyInfo(Variant::INT,"contacts_reported"),_SCS("set_max_contacts_reported"),_SCS("get_max_contacts_reported"));
 	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"contact_monitor"),_SCS("set_contact_monitor"),_SCS("is_contact_monitor_enabled"));
 	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"active"),_SCS("set_active"),_SCS("is_active"));
@@ -757,9 +671,14 @@ void RigidBody2D::_bind_methods() {
 	ADD_SIGNAL( MethodInfo("body_exit",PropertyInfo(Variant::OBJECT,"body")));
 
 	BIND_CONSTANT( MODE_STATIC );
-	BIND_CONSTANT( MODE_STATIC_ACTIVE );
+	BIND_CONSTANT( MODE_KINEMATIC );
 	BIND_CONSTANT( MODE_RIGID );
 	BIND_CONSTANT( MODE_CHARACTER );
+
+	BIND_CONSTANT( CCD_MODE_DISABLED );
+	BIND_CONSTANT( CCD_MODE_CAST_RAY );
+	BIND_CONSTANT( CCD_MODE_CAST_SHAPE );
+
 }
 
 RigidBody2D::RigidBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_RIGID) {
@@ -774,7 +693,7 @@ RigidBody2D::RigidBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_RIGID) {
 
 	angular_velocity=0;
 	active=true;
-	ccd=false;
+	ccd_mode=CCD_MODE_DISABLED;
 
 	custom_integrator=false;
 	contact_monitor=NULL;
@@ -790,5 +709,401 @@ RigidBody2D::~RigidBody2D() {
 
 
 
+}
+
+//////////////////////////
+
+
+Variant KinematicBody2D::_get_collider() const {
+
+	ObjectID oid=get_collider();
+	if (oid==0)
+		return Variant();
+	Object *obj = ObjectDB::get_instance(oid);
+	if (!obj)
+		return Variant();
+
+	Reference *ref = obj->cast_to<Reference>();
+	if (ref) {
+		return Ref<Reference>(ref);
+	}
+
+	return obj;
+}
+
+
+bool KinematicBody2D::_ignores_mode(Physics2DServer::BodyMode p_mode) const {
+
+	switch(p_mode) {
+		case Physics2DServer::BODY_MODE_STATIC: return !collide_static;
+		case Physics2DServer::BODY_MODE_KINEMATIC: return !collide_kinematic;
+		case Physics2DServer::BODY_MODE_RIGID: return !collide_rigid;
+		case Physics2DServer::BODY_MODE_CHARACTER: return !collide_character;
+	}
+
+	return true;
+}
+
+bool KinematicBody2D::is_trapped() const {
+
+	ERR_FAIL_COND_V(!is_inside_scene(),false);
+
+	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
+	ERR_FAIL_COND_V(!dss,false);
+
+	const int max_shapes=32;
+	Physics2DDirectSpaceState::ShapeResult sr[max_shapes];
+
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+
+	for(int i=0;i<get_shape_count();i++) {
+
+
+		int res = dss->intersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),sr,max_shapes,exclude);
+
+		for(int j=0;j<res;j++) {
+
+			Physics2DServer::BodyMode bm = Physics2DServer::get_singleton()->body_get_mode(sr[j].rid);
+			if (!_ignores_mode(bm)) {
+				return true; //it's indeed trapped
+			}
+
+		}
+
+	}
+
+	return false;
+
+}
+void KinematicBody2D::untrap() {
+
+	//this is reaaaaaaaaally wild, will probably only work for simple cases
+	ERR_FAIL_COND(!is_inside_scene());
+
+	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
+	ERR_FAIL_COND(!dss);
+
+	const int max_shapes=32;
+	Physics2DDirectSpaceState::ShapeResult sr[max_shapes];
+	const int max_contacts=8;
+	Vector2 pairs[max_contacts*2];
+
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+	Vector2 untrap_vec;
+
+	for(int i=0;i<get_shape_count();i++) {
+
+		Matrix32 shape_xform = get_global_transform() * get_shape_transform(i);
+		int res = dss->intersect_shape(get_shape(i)->get_rid(), shape_xform,Vector2(),sr,max_shapes,exclude);
+
+		for(int j=0;j<res;j++) {
+
+			Physics2DServer::BodyMode bm = Physics2DServer::get_singleton()->body_get_mode(sr[j].rid);
+			if (_ignores_mode(bm)) {
+				exclude.insert(sr[j].rid);
+			} else {
+
+				int rc;
+				bool c = Physics2DServer::get_singleton()->body_collide_shape(sr[j].rid,sr[j].shape,get_shape(i)->get_rid(),shape_xform,Vector2(),pairs,max_contacts,rc);
+
+				if (c) {
+
+					for(int k=0;k<rc;k++) {
+
+						untrap_vec+=pairs[k*2+0]-pairs[k*2+1];
+					}
+				}
+
+			}
+
+		}
+
+	}
+
+	untrap_vec += untrap_vec.normalized()*margin;
+
+
+	Matrix32 gt = get_global_transform();
+	gt.elements[2]+=untrap_vec;
+	set_global_transform(gt);
+
+}
+
+Vector2 KinematicBody2D::move(const Vector2& p_motion) {
+
+	colliding=false;
+	ERR_FAIL_COND_V(!is_inside_scene(),Vector2());
+	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
+	ERR_FAIL_COND_V(!dss,Vector2());
+	const int max_shapes=32;
+	Physics2DDirectSpaceState::ShapeResult sr[max_shapes];
+
+	float best_travel = 1e20;
+	Physics2DDirectSpaceState::MotionCastCollision mcc_final;
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+	print_line("pos: "+get_global_pos());
+	print_line("mlen: "+p_motion);
+
+	if (!collide_static || ! collide_rigid || !collide_character || !collide_kinematic) {
+		//fill exclude list..
+		for(int i=0;i<get_shape_count();i++) {
+
+
+			int res = dss->intersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),p_motion,sr,max_shapes,exclude);
+
+			for(int j=0;j<res;j++) {
+
+				Physics2DServer::BodyMode bm = Physics2DServer::get_singleton()->body_get_mode(sr[j].rid);
+				if (_ignores_mode(bm)) {
+					exclude.insert(sr[j].rid);
+				} else {
+				//	print_line("DANGER???");
+				}
+			}
+		}
+	}
+
+	for(int i=0;i<get_shape_count();i++) {
+
+		Physics2DDirectSpaceState::MotionCastCollision mcc;
+
+		bool res = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, mcc,exclude,0);
+		if (res==false)
+			continue;
+		if (mcc.travel<=0) {
+			//uh it's trapped
+			colliding=false;
+			return p_motion;
+		}
+		if (mcc.travel < best_travel) {
+
+			mcc_final=mcc;
+			best_travel=mcc.travel;
+		}
+	}
+
+	float motion;
+	Vector2 motion_ret;
+	Vector2 push;
+	if (best_travel>1) {
+		//not collided
+		colliding=false;
+		motion=p_motion.length(); //no stopped
+	} else {
+
+		colliding=true;
+		collision=mcc_final.point;
+		normal=mcc_final.normal;
+		collider=mcc_final.collider_id;
+		Vector2 mnormal=p_motion.normalized();
+
+		float sine = Math::abs(mnormal.dot(normal));
+		float retreat=0;
+		motion = p_motion.length()*mcc_final.travel;
+
+		if (sine==0) {
+			//something odd going on, do not allow motion?
+
+			retreat=motion;
+
+		} else {
+
+			retreat = margin/sine;
+			if (retreat>motion)
+				retreat=motion;
+		}
+
+		motion_ret=p_motion.normalized() * ( p_motion.length() - motion);
+		motion-=retreat;
+
+
+	}
+
+	Matrix32 gt = get_global_transform();
+	gt.elements[2]+=p_motion.normalized()*motion;
+	set_global_transform(gt);
+
+	return motion_ret;
+
+}
+
+Vector2 KinematicBody2D::move_to(const Vector2& p_position) {
+
+	return move(p_position-get_global_pos());
+}
+
+bool KinematicBody2D::can_move_to(const Vector2& p_position) {
+
+	ERR_FAIL_COND_V(!is_inside_scene(),false);
+	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
+	ERR_FAIL_COND_V(!dss,false);
+
+	const int max_shapes=32;
+	Physics2DDirectSpaceState::ShapeResult sr[max_shapes];
+
+	Vector2 motion = p_position-get_global_pos();
+
+	Physics2DDirectSpaceState::MotionCastCollision mcc_final;
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+	//fill exclude list..
+	for(int i=0;i<get_shape_count();i++) {
+
+
+		int res = dss->intersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),motion,sr,max_shapes,exclude);
+
+		for(int j=0;j<res;j++) {
+
+			Physics2DServer::BodyMode bm = Physics2DServer::get_singleton()->body_get_mode(sr[j].rid);
+			if (_ignores_mode(bm)) {
+				exclude.insert(sr[j].rid);
+				continue;
+			}
+
+			return false; //omg collided
+
+		}
+	}
+
+	return true;
+}
+
+bool KinematicBody2D::is_colliding() const {
+
+	ERR_FAIL_COND_V(!is_inside_scene(),false);
+
+	return colliding;
+}
+Vector2 KinematicBody2D::get_collision_pos() const {
+
+	ERR_FAIL_COND_V(!colliding,Vector2());
+	return collision;
+
+}
+Vector2 KinematicBody2D::get_collision_normal() const {
+
+	ERR_FAIL_COND_V(!colliding,Vector2());
+	return normal;
+
+}
+ObjectID KinematicBody2D::get_collider() const {
+
+	ERR_FAIL_COND_V(!colliding,0);
+	return collider;
+}
+
+void KinematicBody2D::set_collide_with_static_bodies(bool p_enable) {
+
+	collide_static=p_enable;
+}
+bool KinematicBody2D::can_collide_with_static_bodies() const {
+
+	return collide_static;
+}
+
+void KinematicBody2D::set_collide_with_rigid_bodies(bool p_enable) {
+
+	collide_rigid=p_enable;
+
+}
+bool KinematicBody2D::can_collide_with_rigid_bodies() const {
+
+
+	return collide_rigid;
+}
+
+void KinematicBody2D::set_collide_with_kinematic_bodies(bool p_enable) {
+
+	collide_kinematic=p_enable;
+
+}
+bool KinematicBody2D::can_collide_with_kinematic_bodies() const {
+
+	return collide_kinematic;
+}
+
+void KinematicBody2D::set_collide_with_character_bodies(bool p_enable) {
+
+	collide_character=p_enable;
+}
+bool KinematicBody2D::can_collide_with_character_bodies() const {
+
+	return collide_character;
+}
+
+void KinematicBody2D::set_collision_margin(float p_margin) {
+
+	margin=p_margin;
+}
+
+float KinematicBody2D::get_collision_margin() const{
+
+	return margin;
+}
+
+void KinematicBody2D::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("is_trapped"),&KinematicBody2D::is_trapped);
+	ObjectTypeDB::bind_method(_MD("untrap"),&KinematicBody2D::untrap);
+
+	ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody2D::move);
+	ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody2D::move_to);
+
+	ObjectTypeDB::bind_method(_MD("can_move_to","position"),&KinematicBody2D::can_move_to);
+
+	ObjectTypeDB::bind_method(_MD("is_colliding"),&KinematicBody2D::is_colliding);
+
+	ObjectTypeDB::bind_method(_MD("get_collision_pos"),&KinematicBody2D::get_collision_pos);
+	ObjectTypeDB::bind_method(_MD("get_collision_normal"),&KinematicBody2D::get_collision_normal);
+	ObjectTypeDB::bind_method(_MD("get_collider:Object"),&KinematicBody2D::get_collider);
+
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_static_bodies","enable"),&KinematicBody2D::set_collide_with_static_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_static_bodies"),&KinematicBody2D::can_collide_with_static_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_kinematic_bodies","enable"),&KinematicBody2D::set_collide_with_kinematic_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_kinematic_bodies"),&KinematicBody2D::can_collide_with_kinematic_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_rigid_bodies","enable"),&KinematicBody2D::set_collide_with_rigid_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_rigid_bodies"),&KinematicBody2D::can_collide_with_rigid_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_character_bodies","enable"),&KinematicBody2D::set_collide_with_character_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_character_bodies"),&KinematicBody2D::can_collide_with_character_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collision_margin","pixels"),&KinematicBody2D::set_collision_margin);
+	ObjectTypeDB::bind_method(_MD("get_collision_margin","pixels"),&KinematicBody2D::get_collision_margin);
+
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/static"),_SCS("set_collide_with_static_bodies"),_SCS("can_collide_with_static_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/kinematic"),_SCS("set_collide_with_kinematic_bodies"),_SCS("can_collide_with_kinematic_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/rigid"),_SCS("set_collide_with_rigid_bodies"),_SCS("can_collide_with_rigid_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/character"),_SCS("set_collide_with_character_bodies"),_SCS("can_collide_with_character_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.01,256,0.01"),_SCS("set_collision_margin"),_SCS("get_collision_margin"));
+
+
+}
+
+KinematicBody2D::KinematicBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_KINEMATIC){
+
+	collide_static=true;
+	collide_rigid=true;
+	collide_kinematic=true;
+	collide_character=true;
+
+	colliding=false;
+	collider=0;
+
+	margin=1;
+}
+KinematicBody2D::~KinematicBody2D()  {
+
+
 }
 

scene/2d/physics_body_2d.h

@@ -52,15 +52,8 @@ class StaticBody2D : public PhysicsBody2D {
 
 	OBJ_TYPE(StaticBody2D,PhysicsBody2D);
 
-	Matrix32 *pre_xform;
-	//RID query;
-	bool setting;
-	bool pending;
-	bool simulating_motion;
 	Vector2 constant_linear_velocity;
 	real_t constant_angular_velocity;
-	void _update_xform();
-	void _state_notify(Object *p_object);
 
 	real_t bounce;
 	real_t friction;
@@ -68,7 +61,6 @@ class StaticBody2D : public PhysicsBody2D {
 
 protected:
 
-	void _notification(int p_what);
 	static void _bind_methods();
 
 public:
@@ -79,8 +71,6 @@ public:
 	void set_bounce(real_t p_bounce);
 	real_t get_bounce() const;
 
-	void set_simulate_motion(bool p_enable);
-	bool is_simulating_motion() const;
 
 	void set_constant_linear_velocity(const Vector2& p_vel);
 	void set_constant_angular_velocity(real_t p_vel);
@@ -102,8 +92,15 @@ public:
 		MODE_RIGID,
 		MODE_STATIC,
 		MODE_CHARACTER,
-		MODE_STATIC_ACTIVE,
+		MODE_KINEMATIC,
+	};
+
+	enum CCDMode {
+		CCD_MODE_DISABLED,
+		CCD_MODE_CAST_RAY,
+		CCD_MODE_CAST_SHAPE,
 	};
+
 private:
 
 	bool can_sleep;
@@ -117,13 +114,14 @@ private:
 	Vector2 linear_velocity;
 	real_t angular_velocity;
 	bool active;
-	bool ccd;
 
 
 	int max_contacts_reported;
 
 	bool custom_integrator;
 
+	CCDMode ccd_mode;
+
 
 	struct ShapePair {
 
@@ -173,7 +171,6 @@ private:
 
 protected:
 
-	void _notification(int p_what);
 	static void _bind_methods();
 public:
 
@@ -215,8 +212,8 @@ public:
 	void set_max_contacts_reported(int p_amount);
 	int get_max_contacts_reported() const;
 
-	void set_use_continuous_collision_detection(bool p_enable);
-	bool is_using_continuous_collision_detection() const;
+	void set_continuous_collision_detection_mode(CCDMode p_mode);
+	CCDMode get_continuous_collision_detection_mode() const;
 
 	void apply_impulse(const Vector2& p_pos, const Vector2& p_impulse);
 
@@ -229,4 +226,65 @@ public:
 };
 
 VARIANT_ENUM_CAST(RigidBody2D::Mode);
+VARIANT_ENUM_CAST(RigidBody2D::CCDMode);
+
+
+
+class KinematicBody2D : public PhysicsBody2D {
+
+	OBJ_TYPE(KinematicBody2D,PhysicsBody2D);
+
+	float margin;
+	bool collide_static;
+	bool collide_rigid;
+	bool collide_kinematic;
+	bool collide_character;
+
+	bool colliding;
+	Vector2 collision;
+	Vector2 normal;
+	ObjectID collider;
+
+
+	Variant _get_collider() const;
+
+	_FORCE_INLINE_ bool _ignores_mode(Physics2DServer::BodyMode) const;
+protected:
+
+	static void _bind_methods();
+public:
+
+	bool is_trapped() const;
+	void untrap();
+
+	Vector2 move(const Vector2& p_motion);
+	Vector2 move_to(const Vector2& p_position);
+
+	bool can_move_to(const Vector2& p_position);
+	bool is_colliding() const;
+	Vector2 get_collision_pos() const;
+	Vector2 get_collision_normal() const;
+	ObjectID get_collider() const;
+
+	void set_collide_with_static_bodies(bool p_enable);
+	bool can_collide_with_static_bodies() const;
+
+	void set_collide_with_rigid_bodies(bool p_enable);
+	bool can_collide_with_rigid_bodies() const;
+
+	void set_collide_with_kinematic_bodies(bool p_enable);
+	bool can_collide_with_kinematic_bodies() const;
+
+	void set_collide_with_character_bodies(bool p_enable);
+	bool can_collide_with_character_bodies() const;
+
+	void set_collision_margin(float p_margin);
+	float get_collision_margin() const;
+
+	KinematicBody2D();
+	~KinematicBody2D();
+
+};
+
+
 #endif // PHYSICS_BODY_2D_H

scene/2d/sprite.cpp

@@ -105,7 +105,7 @@ void Sprite::set_texture(const Ref<Texture>& p_texture) {
 	}
 	texture=p_texture;
 	if (texture.is_valid()) {
-		texture->set_flags(texture->get_flags()&(~Texture::FLAG_REPEAT)); //remove repeat from texture, it looks bad in sprites
+		texture->set_flags(texture->get_flags()); //remove repeat from texture, it looks bad in sprites
 		texture->connect(CoreStringNames::get_singleton()->changed,this,SceneStringNames::get_singleton()->update);
 	}
 	update();

scene/2d/tile_map.cpp

@@ -181,7 +181,7 @@ void TileMap::_update_dirty_quadrants() {
 			if (!tile_set->has_tile(c.id))
 				continue;
 			Ref<Texture> tex = tile_set->tile_get_texture(c.id);
-			Vector2 tile_ofs = tile_set->tile_get_offset(c.id);
+			Vector2 tile_ofs = tile_set->tile_get_texture_offset(c.id);
 
 			Vector2 offset = Point2( E->key().x, E->key().y )*cell_size - q.pos;
 
@@ -215,6 +215,7 @@ void TileMap::_update_dirty_quadrants() {
 				rect.size.y=-rect.size.y;
 
 
+			rect.pos+=tile_ofs;
 			if (r==Rect2()) {
 
 				tex->draw_rect(q.canvas_item,rect);
@@ -231,8 +232,9 @@ void TileMap::_update_dirty_quadrants() {
 				Ref<Shape2D> shape = shapes[i];
 				if (shape.is_valid()) {
 
+					Vector2 shape_ofs = tile_set->tile_get_shape_offset(c.id);
 					Matrix32 xform;
-					xform.set_origin(offset.floor());
+					xform.set_origin(offset.floor()+shape_ofs);
 					if (c.flip_h) {
 						xform.elements[0]=-xform.elements[0];
 						xform.elements[2].x+=s.x;
@@ -242,6 +244,7 @@ void TileMap::_update_dirty_quadrants() {
 						xform.elements[2].y+=s.y;
 					}
 
+
 					ps->body_add_shape(q.static_body,shape->get_rid(),xform);
 				}
 			}

scene/3d/physics_body.cpp

@@ -434,9 +434,9 @@ void RigidBody::set_mode(Mode p_mode) {
 			PhysicsServer::get_singleton()->body_set_mode(get_rid(),PhysicsServer::BODY_MODE_CHARACTER);
 
 		} break;
-		case MODE_STATIC_ACTIVE: {
+		case MODE_KINEMATIC: {
 
-			PhysicsServer::get_singleton()->body_set_mode(get_rid(),PhysicsServer::BODY_MODE_STATIC_ACTIVE);
+			PhysicsServer::get_singleton()->body_set_mode(get_rid(),PhysicsServer::BODY_MODE_KINEMATIC);
 		} break;
 
 	}
@@ -684,7 +684,7 @@ void RigidBody::_bind_methods() {
 
 	BIND_VMETHOD(MethodInfo("_integrate_forces",PropertyInfo(Variant::OBJECT,"state:PhysicsDirectBodyState")));
 
-	ADD_PROPERTY( PropertyInfo(Variant::INT,"mode",PROPERTY_HINT_ENUM,"Rigid,Static,Character,Static Active"),_SCS("set_mode"),_SCS("get_mode"));
+	ADD_PROPERTY( PropertyInfo(Variant::INT,"mode",PROPERTY_HINT_ENUM,"Rigid,Static,Character,Kinematic"),_SCS("set_mode"),_SCS("get_mode"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"mass",PROPERTY_HINT_EXP_RANGE,"0.01,65535,0.01"),_SCS("set_mass"),_SCS("get_mass"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"weight",PROPERTY_HINT_EXP_RANGE,"0.01,65535,0.01",PROPERTY_USAGE_EDITOR),_SCS("set_weight"),_SCS("get_weight"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"friction",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_friction"),_SCS("get_friction"));
@@ -704,7 +704,7 @@ void RigidBody::_bind_methods() {
 	ADD_SIGNAL( MethodInfo("body_exit",PropertyInfo(Variant::OBJECT,"body")));
 
 	BIND_CONSTANT( MODE_STATIC );
-	BIND_CONSTANT( MODE_STATIC_ACTIVE );
+	BIND_CONSTANT( MODE_KINEMATIC );
 	BIND_CONSTANT( MODE_RIGID );
 	BIND_CONSTANT( MODE_CHARACTER );
 }

scene/3d/physics_body.h

@@ -92,7 +92,7 @@ public:
 		MODE_RIGID,
 		MODE_STATIC,
 		MODE_CHARACTER,
-		MODE_STATIC_ACTIVE,
+		MODE_KINEMATIC,
 	};
 private:
 

scene/io/resource_format_image.cpp

@@ -135,7 +135,7 @@ RES ResourceFormatLoaderImage::load(const String &p_path,const String& p_origina
 			flags|=Texture::FLAG_FILTER;
 		if (bool(GLOBAL_DEF("texture_import/gen_mipmaps",true)))
 			flags|=Texture::FLAG_MIPMAPS;
-		if (bool(GLOBAL_DEF("texture_import/repeat",true)))
+		if (bool(GLOBAL_DEF("texture_import/repeat",false)))
 			flags|=Texture::FLAG_REPEAT;
 
 

scene/register_scene_types.cpp

@@ -452,6 +452,7 @@ void register_scene_types() {
 	ObjectTypeDB::register_virtual_type<PhysicsBody2D>();
 	ObjectTypeDB::register_type<StaticBody2D>();
 	ObjectTypeDB::register_type<RigidBody2D>();
+	//ObjectTypeDB::register_type<KinematicBody2D>();
 	ObjectTypeDB::register_type<Area2D>();
 	ObjectTypeDB::register_type<CollisionShape2D>();
 	ObjectTypeDB::register_type<CollisionPolygon2D>();

scene/resources/shape_2d.cpp

@@ -47,20 +47,82 @@ real_t Shape2D::get_custom_solver_bias() const{
 }
 
 
+bool Shape2D::collide_with_motion(const Matrix32& p_local_xform, const Vector2& p_local_motion, const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform, const Vector2 &p_shape_motion) {
+
+	ERR_FAIL_COND_V(p_shape.is_null(),false);
+	int r;
+	return Physics2DServer::get_singleton()->shape_collide(get_rid(),p_local_xform,p_local_motion,p_shape->get_rid(),p_shape_xform,p_shape_motion,NULL,0,r);
+}
+
+bool Shape2D::collide(const Matrix32& p_local_xform,  const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform){
+	ERR_FAIL_COND_V(p_shape.is_null(),false);
+	int r;
+	return Physics2DServer::get_singleton()->shape_collide(get_rid(),p_local_xform,Vector2(),p_shape->get_rid(),p_shape_xform,Vector2(),NULL,0,r);
+
+
+}
+
+Variant Shape2D::collide_with_motion_and_get_contacts(const Matrix32& p_local_xform, const Vector2& p_local_motion, const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform, const Vector2 &p_shape_motion){
+
+	ERR_FAIL_COND_V(p_shape.is_null(),Variant());
+	const int max_contacts = 16;
+	Vector2 result[max_contacts*2];
+	int contacts=0;
+
+	if (!Physics2DServer::get_singleton()->shape_collide(get_rid(),p_local_xform,p_local_motion,p_shape->get_rid(),p_shape_xform,p_shape_motion,result,max_contacts,contacts))
+		return Variant();
+
+	Array results;
+	results.resize(contacts*2);
+	for(int i=0;i<contacts;i++) {
+		results[i]=result[i];
+	}
+
+	return results;
+
+}
+Variant Shape2D::collide_and_get_contacts(const Matrix32& p_local_xform,  const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform){
+
+	ERR_FAIL_COND_V(p_shape.is_null(),Variant());
+	const int max_contacts = 16;
+	Vector2 result[max_contacts*2];
+	int contacts=0;
+
+	if (!Physics2DServer::get_singleton()->shape_collide(get_rid(),p_local_xform,Vector2(),p_shape->get_rid(),p_shape_xform,Vector2(),result,max_contacts,contacts))
+		return Variant();
+
+	Array results;
+	results.resize(contacts*2);
+	for(int i=0;i<contacts;i++) {
+		results[i]=result[i];
+	}
+
+	return results;
+
+
+}
+
 void Shape2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_custom_solver_bias","bias"),&Shape2D::set_custom_solver_bias);
 	ObjectTypeDB::bind_method(_MD("get_custom_solver_bias"),&Shape2D::get_custom_solver_bias);
+	ObjectTypeDB::bind_method(_MD("collide","local_xform","with_shape:Shape2D","shape_xform"),&Shape2D::collide);
+	ObjectTypeDB::bind_method(_MD("collide_with_motion","local_xform","local_motion","with_shape:Shape2D","shape_xform","shape_motion"),&Shape2D::collide_with_motion);
+	ObjectTypeDB::bind_method(_MD("collide_and_get_contacts:var","local_xform","with_shape:Shape2D","shape_xform"),&Shape2D::collide_and_get_contacts);
+	ObjectTypeDB::bind_method(_MD("collide_with_motion_and_get_contacts:var","local_xform","local_motion","with_shape:Shape2D","shape_xform","shape_motion"),&Shape2D::collide_and_get_contacts);
 
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"custom_solver_bias",PROPERTY_HINT_RANGE,"0,1,0.001"),_SCS("set_custom_solver_bias"),_SCS("get_custom_solver_bias"));
 }
 
+
+
 Shape2D::Shape2D(const RID& p_rid) {
 	shape=p_rid;
 	custom_bias=0;
 }
 
 
+
 Shape2D::~Shape2D() {
 
 	Physics2DServer::get_singleton()->free(shape);

scene/resources/shape_2d.h

@@ -47,6 +47,12 @@ public:
 	void set_custom_solver_bias(real_t p_bias);
 	real_t get_custom_solver_bias() const;
 
+	bool collide_with_motion(const Matrix32& p_local_xform, const Vector2& p_local_motion, const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform, const Vector2 &p_p_shape_motion);
+	bool collide(const Matrix32& p_local_xform,  const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform);
+
+	Variant collide_with_motion_and_get_contacts(const Matrix32& p_local_xform, const Vector2& p_local_motion, const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform, const Vector2 &p_p_shape_motion);
+	Variant collide_and_get_contacts(const Matrix32& p_local_xform,  const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform);
+
 	virtual RID get_rid() const;
 	Shape2D();
 	~Shape2D();

scene/resources/tile_set.cpp

@@ -44,8 +44,10 @@ bool TileSet::_set(const StringName& p_name, const Variant& p_value) {
 		tile_set_name(id,p_value);
 	else if (what=="texture")
 		tile_set_texture(id,p_value);
-	else if (what=="offset")
-		tile_set_offset(id,p_value);
+	else if (what=="tex_offset")
+		tile_set_texture_offset(id,p_value);
+	else if (what=="shape_offset")
+		tile_set_shape_offset(id,p_value);
 	else if (what=="region")
 		tile_set_region(id,p_value);
 	else if (what=="shape")
@@ -75,8 +77,10 @@ bool TileSet::_get(const StringName& p_name,Variant &r_ret) const{
 		r_ret=tile_get_name(id);
 	else if (what=="texture")
 		r_ret=tile_get_texture(id);
-	else if (what=="offset")
-		r_ret=tile_get_offset(id);
+	else if (what=="tex_offset")
+		r_ret=tile_get_texture_offset(id);
+	else if (what=="shape_offset")
+		r_ret=tile_get_shape_offset(id);
 	else if (what=="region")
 		r_ret=tile_get_region(id);
 	else if (what=="shape")
@@ -98,7 +102,8 @@ void TileSet::_get_property_list( List<PropertyInfo> *p_list) const{
 		String pre = itos(id)+"/";
 		p_list->push_back(PropertyInfo(Variant::STRING,pre+"name"));
 		p_list->push_back(PropertyInfo(Variant::OBJECT,pre+"texture",PROPERTY_HINT_RESOURCE_TYPE,"Texture"));
-		p_list->push_back(PropertyInfo(Variant::VECTOR2,pre+"offset"));
+		p_list->push_back(PropertyInfo(Variant::VECTOR2,pre+"tex_offset"));
+		p_list->push_back(PropertyInfo(Variant::VECTOR2,pre+"shape_offset"));
 		p_list->push_back(PropertyInfo(Variant::RECT2,pre+"region"));
 		p_list->push_back(PropertyInfo(Variant::OBJECT,pre+"shape",PROPERTY_HINT_RESOURCE_TYPE,"Shape2D",PROPERTY_USAGE_EDITOR));
 		p_list->push_back(PropertyInfo(Variant::ARRAY,pre+"shapes",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_NOEDITOR));
@@ -129,20 +134,34 @@ Ref<Texture> TileSet::tile_get_texture(int p_id) const {
 
 }
 
-void TileSet::tile_set_offset(int p_id,const Vector2 &p_offset) {
+void TileSet::tile_set_texture_offset(int p_id,const Vector2 &p_offset) {
 
 	ERR_FAIL_COND(!tile_map.has(p_id));
 	tile_map[p_id].offset=p_offset;
 	emit_changed();
 }
 
-Vector2 TileSet::tile_get_offset(int p_id) const {
+Vector2 TileSet::tile_get_texture_offset(int p_id) const {
 
 	ERR_FAIL_COND_V(!tile_map.has(p_id),Vector2());
 	return tile_map[p_id].offset;
 
 }
 
+void TileSet::tile_set_shape_offset(int p_id,const Vector2 &p_offset) {
+
+	ERR_FAIL_COND(!tile_map.has(p_id));
+	tile_map[p_id].shape_offset=p_offset;
+	emit_changed();
+}
+
+Vector2 TileSet::tile_get_shape_offset(int p_id) const {
+
+	ERR_FAIL_COND_V(!tile_map.has(p_id),Vector2());
+	return tile_map[p_id].shape_offset;
+
+}
+
 void TileSet::tile_set_region(int p_id,const Rect2 &p_region) {
 
 	ERR_FAIL_COND(!tile_map.has(p_id));
@@ -289,8 +308,10 @@ void TileSet::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("tile_get_name","id"),&TileSet::tile_get_name);
 	ObjectTypeDB::bind_method(_MD("tile_set_texture","id","texture:Texture"),&TileSet::tile_set_texture);
 	ObjectTypeDB::bind_method(_MD("tile_get_texture:Texture","id"),&TileSet::tile_get_texture);
-	ObjectTypeDB::bind_method(_MD("tile_set_offset","id","offset"),&TileSet::tile_set_offset);
-	ObjectTypeDB::bind_method(_MD("tile_get_offset","id"),&TileSet::tile_get_offset);
+	ObjectTypeDB::bind_method(_MD("tile_set_texture_offset","id","texture_offset"),&TileSet::tile_set_texture_offset);
+	ObjectTypeDB::bind_method(_MD("tile_get_texture_offset","id"),&TileSet::tile_get_texture_offset);
+	ObjectTypeDB::bind_method(_MD("tile_set_shape_offset","id","shape_offset"),&TileSet::tile_set_shape_offset);
+	ObjectTypeDB::bind_method(_MD("tile_get_shape_offset","id"),&TileSet::tile_get_shape_offset);
 	ObjectTypeDB::bind_method(_MD("tile_set_region","id","region"),&TileSet::tile_set_region);
 	ObjectTypeDB::bind_method(_MD("tile_get_region","id"),&TileSet::tile_get_region);
 	ObjectTypeDB::bind_method(_MD("tile_set_shape","id","shape:Shape2D"),&TileSet::tile_set_shape);

scene/resources/tile_set.h

@@ -42,6 +42,7 @@ class TileSet : public Resource {
 		String name;
 		Ref<Texture> texture;
 		Vector2 offset;
+		Vector2 shape_offset;
 		Rect2i region;
 		Vector<Ref<Shape2D> > shapes;
 	};
@@ -70,8 +71,11 @@ public:
 	void tile_set_texture(int p_id, const Ref<Texture> &p_texture);
 	Ref<Texture> tile_get_texture(int p_id) const;
 
-	void tile_set_offset(int p_id,const Vector2 &p_offset);
-	Vector2 tile_get_offset(int p_id) const;
+	void tile_set_texture_offset(int p_id,const Vector2 &p_offset);
+	Vector2 tile_get_texture_offset(int p_id) const;
+
+	void tile_set_shape_offset(int p_id,const Vector2 &p_offset);
+	Vector2 tile_get_shape_offset(int p_id) const;
 
 	void tile_set_region(int p_id,const Rect2 &p_region);
 	Rect2 tile_get_region(int p_id) const;

servers/physics/body_pair_sw.cpp

@@ -198,7 +198,7 @@ bool BodyPairSW::setup(float p_step) {
 
 
 	//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)) {
+	if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
 		return false;
 	}
 

servers/physics/body_sw.cpp

@@ -26,606 +26,606 @@
 /* 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();
-}
+#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_KINEMATIC:
+		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_KINEMATIC: {
+
+			_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_KINEMATIC);
+			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_KINEMATIC) {
+				_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_KINEMATIC)
+				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_KINEMATIC)
+		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_KINEMATIC) {
+		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_KINEMATIC)
+		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_2d/area_2d_sw.cpp

@@ -84,7 +84,7 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
 
 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)
+	if (do_override==(space_override_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED))
 		return;
 	_unregister_shapes();
 	space_override_mode=p_mode;

servers/physics_2d/area_pair_2d_sw.cpp

@@ -32,7 +32,7 @@
 
 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);
+	bool result = CollisionSolver2DSW::solve(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),Vector2(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),Vector2(),NULL,this);
 
 	if (result!=colliding) {
 

servers/physics_2d/body_2d_sw.cpp

@@ -26,584 +26,589 @@
 /* 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();
-}
+#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_KINEMATIC:
+		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_KINEMATIC: {
+
+			_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_KINEMATIC);
+			linear_velocity=Vector2();
+			angular_velocity=0;
+		} break;
+		case Physics2DServer::BODY_MODE_RIGID: {
+
+			_inv_mass=mass>0?(1.0/mass):0;
+			_set_static(false);
+
+		} break;
+		case Physics2DServer::BODY_MODE_CHARACTER: {
+
+			_inv_mass=mass>0?(1.0/mass):0;
+			_set_static(false);
+		} 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_KINEMATIC) {
+				new_transform=p_variant;
+				//wakeup_neighbours();
+				set_active(true);
+			} else if (mode==Physics2DServer::BODY_MODE_STATIC) {
+				_set_transform(p_variant);
+				_set_inv_transform(get_transform().affine_inverse());
+				wakeup_neighbours();
+			} else {
+				Matrix32 t = p_variant;
+				t.orthonormalize();
+				new_transform=get_transform(); //used as old to compute motion
+				_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_KINEMATIC)
+				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)
+		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();
+
+	Vector2 motion;
+	bool do_motion=false;
+
+	if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
+
+		//compute motion, angular and etc. velocities from prev transform
+		linear_velocity = (new_transform.elements[2] - get_transform().elements[2])/p_step;
+
+		real_t rot = new_transform.affine_inverse().basis_xform(get_transform().elements[1]).atan2();
+		angular_velocity = rot / p_step;
+
+		motion = new_transform.elements[2] - get_transform().elements[2];
+		do_motion=true;
+
+		for(int i=0;i<get_shape_count();i++) {
+			set_shape_kinematic_advance(i,Vector2());
+			set_shape_kinematic_retreat(i,0);
+		}
+
+	} else {
+		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;
+		}
+
+		if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED) {
+
+			motion = new_transform.get_origin() - get_transform().get_origin();
+			//linear_velocity*p_step;
+			do_motion=true;
+		}
+	}
+
+
+	//motion=linear_velocity*p_step;
+
+	biased_angular_velocity=0;
+	biased_linear_velocity=Vector2();
+
+	if (do_motion) {//shapes temporarily extend for raycast
+		_update_shapes_with_motion(motion);
+	}
+
+	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 (fi_callback)
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+
+	if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
+
+		_set_transform(new_transform,false);
+		_set_inv_transform(new_transform.affine_inverse());				;
+		if (linear_velocity==Vector2() && angular_velocity==0)
+			set_active(false); //stopped moving, deactivate
+		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),continuous_cd_mode==Physics2DServer::CCD_MODE_DISABLED);
+	_set_inv_transform(get_transform().inverse());
+
+	if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED)
+		new_transform=get_transform();
+
+	//_update_inertia_tensor();
+}
+
+
+
+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(0,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);
+			}
+		}
+
+
+	}
+
+}
+
+
+bool Body2DSW::sleep_test(real_t p_step)  {
+
+	if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC)
+		return true; //
+	else if (mode==Physics2DServer::BODY_MODE_CHARACTER)
+		return !active; // characters and kinematic bodies 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;
+
+	still_time=0;
+	continuous_cd_mode=Physics2DServer::CCD_MODE_DISABLED;
+	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

@@ -26,309 +26,321 @@
 /* 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
+#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;
+	Physics2DServer::CCDMode continuous_cd_mode;
+	bool omit_force_integration;
+	bool active;
+	bool can_sleep;
+	void _update_inertia();
+	virtual void _shapes_changed();
+	Matrix32 new_transform;
+
+
+	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_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode=p_mode; }
+	_FORCE_INLINE_ Physics2DServer::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
+
+	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; }
+	_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
+
+	void integrate_forces(real_t p_step);
+	void integrate_velocities(real_t p_step);
+
+	_FORCE_INLINE_ Vector2 get_motion() const {
+
+		if (mode>Physics2DServer::BODY_MODE_KINEMATIC) {
+			return new_transform.get_origin() - get_transform().get_origin();
+		} else if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
+			return  get_transform().get_origin() -new_transform.get_origin(); //kinematic simulates forward
+		}
+		return Vector2();
+	}
+
+	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

@@ -46,7 +46,6 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector
 
 	// 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);
 
@@ -61,6 +60,7 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector
 	contact.acc_tangent_impulse=0;
 	contact.local_A=local_A;
 	contact.local_B=local_B;
+	contact.reused=true;
 	contact.normal=(p_point_A-p_point_B).normalized();
 
 	// attempt to determine if the contact will be reused
@@ -77,7 +77,7 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector
 			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;
+			new_index=i;			
 			break;
 		}
 	}
@@ -139,12 +139,26 @@ void BodyPair2DSW::_validate_contacts() {
 
 		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 );
+		bool erase=false;
+		if (c.reused==false) {
+			//was left behind in previous frame
+			erase=true;
+		} else {
+			c.reused=false;
+
+			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) {
+				erase=true;
+			}
+		}
 
-		if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
+		if (erase) {
 			// contact no longer needed, remove
 
 
@@ -161,7 +175,9 @@ void BodyPair2DSW::_validate_contacts() {
 }
 
 
-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) {
+bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result) {
+
+
 
 	Vector2 motion = p_A->get_linear_velocity()*p_step;
 	real_t mlen = motion.length();
@@ -172,18 +188,24 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat
 
 	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
+	bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction
+
+	if (fast_object) { //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
+	//support is the worst case collision point, so real collision happened before
 	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);
+	Matrix32 from_inv = p_xform_B.affine_inverse();
+
+	Vector2 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
+	Vector2 local_to = from_inv.xform(to);
 
 	Vector2 rpos,rnorm;
 	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm))
@@ -191,8 +213,11 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat
 
 	//ray hit something
 
+
+	Vector2 hitpos = p_xform_B.xform(rpos);
+
 	Vector2 contact_A = to;
-	Vector2 contact_B = p_xform_B.xform(rpos);
+	Vector2 contact_B = hitpos;
 
 	//create a contact
 
@@ -208,41 +233,50 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat
 bool BodyPair2DSW::setup(float p_step) {
 
 
+	//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_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC)) {
+		collided=false;
+		return false;
+	}
+
 	//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);
+	Vector2 motion_A,motion_B;
+
+	if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) {
+		motion_A=A->get_motion();
+	} 
+	if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) {
+		motion_B=B->get_motion();
+	} 
+	//faster to set than to check..
+
+	collided = CollisionSolver2DSW::solve(shape_A_ptr,xform_A,motion_A,shape_B_ptr,xform_B,motion_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))
+
+		if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) {
+			if (_test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_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))
+		if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) {
+			if (_test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A,true))
 				collided=true;
 		}
 
@@ -251,8 +285,6 @@ bool BodyPair2DSW::setup(float p_step) {
 
 	}
 
-
-
 	real_t max_penetration = space->get_contact_max_allowed_penetration();
 
 	float bias = 0.3f;
@@ -280,7 +312,7 @@ bool BodyPair2DSW::setup(float p_step) {
 
 		real_t depth = c.normal.dot(global_A - global_B);
 
-		if (depth<=0) {
+		if (depth<=0 || !c.reused) {
 			c.active=false;
 			continue;
 		}
@@ -311,7 +343,6 @@ bool BodyPair2DSW::setup(float p_step) {
 			}
 		}
 
-
 		// Precompute normal mass, tangent mass, and bias.
 		real_t rnA = c.rA.dot(c.normal);
 		real_t rnB = c.rB.dot(c.normal);
@@ -373,6 +404,7 @@ void BodyPair2DSW::solve(float p_step) {
 		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();
@@ -388,7 +420,7 @@ void BodyPair2DSW::solve(float p_step) {
 		A->apply_bias_impulse(c.rA,-jb);
 		B->apply_bias_impulse(c.rB, jb);
 
-		real_t bounce=0;
+		real_t bounce=MAX(A->get_bounce(),B->get_bounce());
 		real_t jn = -(bounce + vn)*c.mass_normal;
 		real_t jnOld = c.acc_normal_impulse;
 		c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
@@ -403,7 +435,6 @@ void BodyPair2DSW::solve(float p_step) {
 
 		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);
 

servers/physics_2d/body_pair_2d_sw.h

@@ -65,6 +65,7 @@ class BodyPair2DSW : public Constraint2DSW {
 		real_t depth;
 		bool active;
 		Vector2 rA,rB;
+		bool reused;
 	};
 
 	Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection
@@ -76,7 +77,7 @@ class BodyPair2DSW : public Constraint2DSW {
 	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);
+	bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_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);

servers/physics_2d/broad_phase_2d_hash_grid.cpp

@@ -378,7 +378,8 @@ int BroadPhase2DHashGrid::get_subindex(ID p_id) const {
 	return E->get().subindex;
 }
 
-void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) {
+template<bool use_aabb,bool use_segment>
+void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) {
 
 
 	PosKey pk;
@@ -411,10 +412,18 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_re
 			continue;
 
 		E->key()->pass=pass;
+
+		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
+			continue;
+
+		if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
+			continue;
+
 		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()) {
@@ -425,6 +434,12 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,CollisionObject2DSW** p_re
 		if (E->key()->pass==pass)
 			continue;
 
+		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
+			continue;
+
+		if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
+			continue;
+
 		E->key()->pass=pass;
 		p_results[index]=E->key()->owner;
 		p_result_indices[index]=E->key()->subindex;
@@ -468,7 +483,7 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t
 		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);
+	_cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount);
 
 	bool reached_x=false;
 	bool reached_y=false;
@@ -502,7 +517,7 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t
 			reached_y=true;
 		}
 
-		_cull(pos,p_results,p_max_results,p_result_indices,cullcount);
+		_cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount);
 
 		if (reached_x && reached_y)
 			break;
@@ -515,8 +530,22 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t
 
 int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
 
+	pass++;
+
+	Point2i from = (p_aabb.pos/cell_size).floor();
+	Point2i to = ((p_aabb.pos+p_aabb.size)/cell_size).floor();
+	int cullcount=0;
+
+	for(int i=from.x;i<=to.x;i++) {
+
+		for(int j=from.y;j<=to.y;j++) {
 
-	return 0;
+			_cull<true,false>(Point2i(i,j),p_aabb,Point2(),Point2(),p_results,p_max_results,p_result_indices,cullcount);
+		}
+
+	}
+
+	return cullcount;
 }
 
 void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {

servers/physics_2d/broad_phase_2d_hash_grid.h

@@ -94,7 +94,8 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 
 	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);
+	template<bool use_aabb,bool use_segment>
+	_FORCE_INLINE_ void _cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index);
 
 
 	struct PosKey {

servers/physics_2d/collision_object_2d_sw.cpp

@@ -130,6 +130,7 @@ void CollisionObject2DSW::_update_shapes() {
 	if (!space)
 		return;
 
+
 	for(int i=0;i<shapes.size();i++) {
 
 		Shape &s=shapes[i];

servers/physics_2d/collision_object_2d_sw.h

@@ -55,8 +55,10 @@ private:
 		BroadPhase2DSW::ID bpid;
 		Rect2 aabb_cache; //for rayqueries
 		Shape2DSW *shape;
+		Vector2 kinematic_advance;
+		float kinematic_retreat;
 		bool trigger;
-		Shape() { trigger=false; }
+		Shape() { trigger=false; kinematic_retreat=0; }
 	};
 
 	Vector<Shape> shapes;
@@ -73,7 +75,7 @@ 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_transform(const Matrix32& p_transform, bool p_update_shapes=true) { transform=p_transform; if (p_update_shapes) {_update_shapes();} }
 	_FORCE_INLINE_ void _set_inv_transform(const Matrix32& p_transform) { inv_transform=p_transform; }
 	void _set_static(bool p_static);
 
@@ -101,6 +103,12 @@ public:
 	_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_ void set_shape_kinematic_advance(int p_index,const Vector2& p_advance) { shapes[p_index].kinematic_advance=p_advance; }
+	_FORCE_INLINE_ Vector2 get_shape_kinematic_advance(int p_index) const { return shapes[p_index].kinematic_advance; }
+
+	_FORCE_INLINE_ void set_shape_kinematic_retreat(int p_index,float p_retreat) { shapes[p_index].kinematic_retreat=p_retreat; }
+	_FORCE_INLINE_ float get_shape_kinematic_retreat(int p_index) const { return shapes[p_index].kinematic_retreat; }
+
 	_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; }
@@ -109,6 +117,7 @@ public:
 	_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);
 

servers/physics_2d/collision_solver_2d_sat.cpp

@@ -321,19 +321,19 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po
 
 
 
-template<class ShapeA, class ShapeB>
+template<class ShapeA, class ShapeB,bool castA=false,bool castB=false>
 class SeparatorAxisTest2D {
 
 	const ShapeA *shape_A;
 	const ShapeB *shape_B;
 	const Matrix32 *transform_A;
 	const Matrix32 *transform_B;
-	const Matrix32 *transform_inv_A;
-	const Matrix32 *transform_inv_B;
 	real_t best_depth;
 	Vector2 best_axis;
 	int best_axis_count;
 	int best_axis_index;
+	Vector2 motion_A;
+	Vector2 motion_B;
 	_CollectorCallback2D *callback;
 
 public:
@@ -351,6 +351,29 @@ public:
 		return true;
 	}
 
+	_FORCE_INLINE_ bool test_cast() {
+
+		if (castA) {
+
+			Vector2 na = motion_A.normalized();
+			if (!test_axis(na))
+				return false;
+			if (!test_axis(na.tangent()))
+				return false;
+		}
+
+		if (castB) {
+
+			Vector2 nb = motion_B.normalized();
+			if (!test_axis(nb))
+				return false;
+			if (!test_axis(nb.tangent()))
+				return false;
+		}
+
+		return true;
+	}
+
 	_FORCE_INLINE_ bool test_axis(const Vector2& p_axis) {
 
 		Vector2 axis=p_axis;
@@ -364,8 +387,15 @@ public:
 
 		real_t min_A,max_A,min_B,max_B;
 
-		shape_A->project_range(axis,*transform_A,min_A,max_A);
-		shape_B->project_range(axis,*transform_B,min_B,max_B);
+		if (castA)
+			shape_A->project_range_cast(motion_A,axis,*transform_A,min_A,max_A);
+		else
+			shape_A->project_range(axis,*transform_A,min_A,max_A);
+
+		if (castB)
+			shape_B->project_range_cast(motion_B,axis,*transform_B,min_B,max_B);
+		else
+			shape_B->project_range(axis,*transform_B,min_B,max_B);
 
 		min_B -= ( max_A - min_A ) * 0.5;
 		max_B += ( max_A - min_A ) * 0.5;
@@ -427,21 +457,28 @@ public:
 			return; //only collide, no callback
 		static const int max_supports=2;
 
-
 		Vector2 supports_A[max_supports];
 		int support_count_A;
-		shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A);
-		for(int i=0;i<support_count_A;i++) {
-			supports_A[i] = transform_A->xform(supports_A[i]);
+		if (castA) {
+			shape_A->get_supports_transformed_cast(motion_A,-best_axis,*transform_A,supports_A,support_count_A);
+		} else {
+			shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A);
+			for(int i=0;i<support_count_A;i++) {
+				supports_A[i] = transform_A->xform(supports_A[i]);
+			}
 		}
 
 
 
 		Vector2 supports_B[max_supports];
 		int support_count_B;
-		shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B);
-		for(int i=0;i<support_count_B;i++) {
-			supports_B[i] = transform_B->xform(supports_B[i]);
+		if (castB) {
+			shape_B->get_supports_transformed_cast(motion_B,best_axis,*transform_B,supports_B,support_count_B);
+		} else {
+			shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B);
+			for(int i=0;i<support_count_B;i++) {
+				supports_B[i] = transform_B->xform(supports_B[i]);
+			}
 		}
 /*
 
@@ -480,14 +517,15 @@ public:
 
 	}
 
-	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2()) {
 		best_depth=1e15;
 		shape_A=p_shape_A;
 		shape_B=p_shape_B;
 		transform_A=&p_transform_a;
 		transform_B=&p_transform_b;
-		transform_inv_A=&p_transform_inv_a;
-		transform_inv_B=&p_transform_inv_b;
+		motion_A=p_motion_A;
+		motion_B=p_motion_B;
+
 		callback=p_collector;
 #ifdef DEBUG_ENABLED
 		best_axis_count=0;
@@ -503,68 +541,92 @@ public:
 /****** SAT TESTS *******/
 
 
-typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Matrix32&,const Shape2DSW*,const Matrix32&,const Matrix32&,_CollectorCallback2D *p_collector);
+#define TEST_POINT(m_a,m_b) \
+	 (	(!separator.test_axis(((m_a)-(m_b)).normalized())) ||\
+		(castA && !separator.test_axis(((m_a)+p_motion_a-(m_b)).normalized())) ||\
+		(castB && !separator.test_axis(((m_a)-((m_b)+p_motion_b)).normalized())) ||\
+		(castA && castB && !separator.test_axis(((m_a)+p_motion_a-((m_b)+p_motion_b)).normalized())) )
+
+
+typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&);
 
-static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+
+template<bool castA, bool castB>
+static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,segment_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB> separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
+	//this collision is kind of pointless
+
 
-	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+	if (!separator.test_previous_axis())
 		return;
-	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_B->get_normal()).normalized()))
+
+	if (!separator.test_cast())
+		return;
+
+	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
+		return;
+	if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b)))
 		return;
 
 	separator.generate_contacts();
 
 }
 
-static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
+	//segment normal
 	if (!separator.test_axis(
 				(p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent()
 				))
 		return;
 
-//	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
-//		return;
-	if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-p_transform_b.get_origin()).normalized()))
+	//endpoint a vs circle
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.get_origin()))
 		return;
-	if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-p_transform_b.get_origin()).normalized()))
+	//endpoint b vs circle
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.get_origin()))
 		return;
 
 
 	separator.generate_contacts();
-
-
 }
 
-static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB> separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
-	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+	if (!separator.test_cast())
+		return;
+
+	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
 		return;
 
 	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
@@ -577,50 +639,58 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_
 
 }
 
-static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
-	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+	if (!separator.test_cast())
+		return;
+
+	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
 		return;
 
 	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
 		return;
 
-	if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
 		return;
-	if (!separator.test_axis((p_transform_a.xform(segment_A->get_a())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
 		return;
-	if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
 		return;
-	if (!separator.test_axis((p_transform_a.xform(segment_A->get_b())-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
 		return;
 
 	separator.generate_contacts();
 }
 
-static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW> separator(segment_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
-	if (!separator.test_axis(p_transform_inv_a.basis_xform_inv(segment_A->get_normal()).normalized()))
+	if (!separator.test_cast())
+		return;
+
+	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
 		return;
 
 	for(int i=0;i<convex_B->get_point_count();i++) {
 
-		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
 			return;
 	}
 
@@ -631,35 +701,44 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3
 
 /////////
 
-static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
 	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,circle_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
-	if (!separator.test_axis((p_transform_a.get_origin()-p_transform_b.get_origin()).normalized()))
+	if (!separator.test_cast())
 		return;
 
+	if (TEST_POINT(p_transform_a.get_origin(),p_transform_b.get_origin()))
+		return;
+
+
 	separator.generate_contacts();
 
 }
 
-static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
 	const Vector2 &sphere=p_transform_a.elements[2];
 	const Vector2 *axis=&p_transform_b.elements[0];
 	const Vector2& half_extents = rectangle_B->get_half_extents();
@@ -670,65 +749,120 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t
 	if (!separator.test_axis(axis[1].normalized()))
 		return;
 
-	Vector2 local_v = p_transform_inv_b.xform(p_transform_a.get_origin());
+	{
+		Vector2 local_v = p_transform_b.affine_inverse().xform(p_transform_a.get_origin());
 
-	Vector2 he(
-		(local_v.x<0) ? -half_extents.x : half_extents.x,
-		(local_v.y<0) ? -half_extents.y : half_extents.y
-	);
+		Vector2 he(
+			(local_v.x<0) ? -half_extents.x : half_extents.x,
+			(local_v.y<0) ? -half_extents.y : half_extents.y
+		);
 
 
-	if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized()))
-		return;
+		if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized()))
+			return;
+	}
+
+	if (castA) {
+
+		Vector2 sphereofs = sphere + p_motion_a;
+		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
+
+		Vector2 he(
+			(local_v.x<0) ? -half_extents.x : half_extents.x,
+			(local_v.y<0) ? -half_extents.y : half_extents.y
+		);
+
+
+		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
+			return;
+	}
+
+	if (castB) {
+
+		Vector2 sphereofs = sphere - p_motion_b;
+		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
+
+		Vector2 he(
+			(local_v.x<0) ? -half_extents.x : half_extents.x,
+			(local_v.y<0) ? -half_extents.y : half_extents.y
+		);
+
+
+		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
+			return;
+	}
+
+	if (castA && castB) {
+
+		Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
+		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
+
+		Vector2 he(
+			(local_v.x<0) ? -half_extents.x : half_extents.x,
+			(local_v.y<0) ? -half_extents.y : half_extents.y
+		);
+
+
+		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
+			return;
+	}
 
 	separator.generate_contacts();
 }
 
-static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
 	//capsule axis
 	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
 		return;
 
 	//capsule endpoints
-	if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)).normalized()))
+	if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
 		return;
-	if (!separator.test_axis((p_transform_a.get_origin()-(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)).normalized()))
+	if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
 		return;
 
-
 	separator.generate_contacts();
 
 
 }
 
-static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW> separator(circle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
+
 	//poly faces and poly points vs circle
 	for(int i=0;i<convex_B->get_point_count();i++) {
 
-		if (!separator.test_axis( (p_transform_b.xform(convex_B->get_point(i))-p_transform_a.get_origin()).normalized() ))
+		if (TEST_POINT( p_transform_a.get_origin(),p_transform_b.xform(convex_B->get_point(i)) ))
 			return;
 
-		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
 			return;
 	}
 
@@ -738,17 +872,21 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32
 
 /////////
 
-static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,rectangle_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
 	//box faces A
 	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
 		return;
@@ -766,17 +904,21 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32&
 	separator.generate_contacts();
 }
 
-static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
 	//box faces
 	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
 		return;
@@ -791,21 +933,77 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
 
 	//box endpoints to capsule circles
 
+	Matrix32 boxinv = p_transform_a.affine_inverse();
+
 	for(int i=0;i<2;i++) {
 
-		Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
+		{
+			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
 
-		const Vector2& half_extents = rectangle_A->get_half_extents();
-		Vector2 local_v = p_transform_inv_a.xform(capsule_endpoint);
+			const Vector2& half_extents = rectangle_A->get_half_extents();
+			Vector2 local_v = boxinv.xform(capsule_endpoint);
 
-		Vector2 he(
-			(local_v.x<0) ? -half_extents.x : half_extents.x,
-			(local_v.y<0) ? -half_extents.y : half_extents.y
-		);
+			Vector2 he(
+				(local_v.x<0) ? -half_extents.x : half_extents.x,
+				(local_v.y<0) ? -half_extents.y : half_extents.y
+			);
 
+			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
+				return;
+		}
+
+
+		if (castA) {
+			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
+			capsule_endpoint-=p_motion_a;
+
+
+			const Vector2& half_extents = rectangle_A->get_half_extents();
+			Vector2 local_v = boxinv.xform(capsule_endpoint);
+
+			Vector2 he(
+				(local_v.x<0) ? -half_extents.x : half_extents.x,
+				(local_v.y<0) ? -half_extents.y : half_extents.y
+			);
+
+			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
+				return;
+		}
+
+		if (castB) {
+			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
+			capsule_endpoint+=p_motion_b;
 
-		if (!separator.test_axis(p_transform_a.xform(he).normalized()))
-			return;
+
+			const Vector2& half_extents = rectangle_A->get_half_extents();
+			Vector2 local_v = boxinv.xform(capsule_endpoint);
+
+			Vector2 he(
+				(local_v.x<0) ? -half_extents.x : half_extents.x,
+				(local_v.y<0) ? -half_extents.y : half_extents.y
+			);
+
+			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
+				return;
+		}
+
+		if (castA && castB) {
+			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
+			capsule_endpoint-=p_motion_a;
+			capsule_endpoint+=p_motion_b;
+
+
+			const Vector2& half_extents = rectangle_A->get_half_extents();
+			Vector2 local_v = boxinv.xform(capsule_endpoint);
+
+			Vector2 he(
+				(local_v.x<0) ? -half_extents.x : half_extents.x,
+				(local_v.y<0) ? -half_extents.y : half_extents.y
+			);
+
+			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
+				return;
+		}
 
 	}
 
@@ -813,16 +1011,20 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
 	separator.generate_contacts();
 }
 
-static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW> separator(rectangle_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
 	//box faces
 	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
 		return;
@@ -833,7 +1035,7 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri
 	//convex faces
 	for(int i=0;i<convex_B->get_point_count();i++) {
 
-		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
 			return;
 	}
 
@@ -844,17 +1046,21 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri
 
 /////////
 
-static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,capsule_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
 	//capsule axis
 
 	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
@@ -873,7 +1079,7 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
 
 			Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5);
 
-			if (!separator.test_axis( (capsule_endpoint_A-capsule_endpoint_B).normalized() ))
+			if (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) )
 				return;
 
 		}
@@ -883,17 +1089,21 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
 
 }
 
-static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW> separator(capsule_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
 	//capsule axis
 
 	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
@@ -909,12 +1119,12 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3
 
 			Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5);
 
-			if (!separator.test_axis( (cpoint - capsule_endpoint_A).normalized() ))
+			if (TEST_POINT(capsule_endpoint_A,cpoint ))
 				return;
 
 		}
 
-		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
 			return;
 	}
 
@@ -925,26 +1135,31 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3
 /////////
 
 
-static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Matrix32& p_transform_inv_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,const Matrix32& p_transform_inv_b,_CollectorCallback2D *p_collector) {
+template<bool castA, bool castB>
+static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
 
 
 	const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW> separator(convex_A,p_transform_a,p_transform_inv_a,convex_B,p_transform_b,p_transform_inv_b,p_collector);
+	SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
+	if (!separator.test_cast())
+		return;
+
+
 	for(int i=0;i<convex_A->get_point_count();i++) {
 
-		if (!separator.test_axis( p_transform_inv_a.basis_xform_inv(convex_A->get_segment_normal(i)).normalized() ))
+		if (!separator.test_axis( convex_A->get_xformed_segment_normal(p_transform_a,i)))
 			return;
 	}
 
 	for(int i=0;i<convex_B->get_point_count();i++) {
 
-		if (!separator.test_axis( p_transform_inv_b.basis_xform_inv(convex_B->get_segment_normal(i)).normalized() ))
+		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
 			return;
 	}
 
@@ -955,7 +1170,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const
 
 ////////
 
-bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Matrix32& p_transform_inv_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B, const Matrix32& p_transform_inv_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) {
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) {
 
 	Physics2DServer::ShapeType type_A=p_shape_A->get_type();
 
@@ -971,31 +1186,118 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_
 
 
 	static const CollisionFunc collision_table[5][5]={
-		{_collision_segment_segment,
-		 _collision_segment_circle,
-		 _collision_segment_rectangle,
-		 _collision_segment_capsule,
-		 _collision_segment_convex_polygon},
+		{_collision_segment_segment<false,false>,
+		 _collision_segment_circle<false,false>,
+		 _collision_segment_rectangle<false,false>,
+		 _collision_segment_capsule<false,false>,
+		 _collision_segment_convex_polygon<false,false>},
 		{0,
-		 _collision_circle_circle,
-		 _collision_circle_rectangle,
-		 _collision_circle_capsule,
-		 _collision_circle_convex_polygon},
+		 _collision_circle_circle<false,false>,
+		 _collision_circle_rectangle<false,false>,
+		 _collision_circle_capsule<false,false>,
+		 _collision_circle_convex_polygon<false,false>},
 		{0,
 		 0,
-		 _collision_rectangle_rectangle,
-		 _collision_rectangle_capsule,
-		 _collision_rectangle_convex_polygon},
+		 _collision_rectangle_rectangle<false,false>,
+		 _collision_rectangle_capsule<false,false>,
+		 _collision_rectangle_convex_polygon<false,false>},
 		{0,
 		 0,
 		 0,
-		 _collision_capsule_capsule,
-		 _collision_capsule_convex_polygon},
+		 _collision_capsule_capsule<false,false>,
+		 _collision_capsule_convex_polygon<false,false>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon}
+		 _collision_convex_polygon_convex_polygon<false,false>}
+
+	};
+
+	static const CollisionFunc collision_table_castA[5][5]={
+		{_collision_segment_segment<true,false>,
+		 _collision_segment_circle<true,false>,
+		 _collision_segment_rectangle<true,false>,
+		 _collision_segment_capsule<true,false>,
+		 _collision_segment_convex_polygon<true,false>},
+		{0,
+		 _collision_circle_circle<true,false>,
+		 _collision_circle_rectangle<true,false>,
+		 _collision_circle_capsule<true,false>,
+		 _collision_circle_convex_polygon<true,false>},
+		{0,
+		 0,
+		 _collision_rectangle_rectangle<true,false>,
+		 _collision_rectangle_capsule<true,false>,
+		 _collision_rectangle_convex_polygon<true,false>},
+		{0,
+		 0,
+		 0,
+		 _collision_capsule_capsule<true,false>,
+		 _collision_capsule_convex_polygon<true,false>},
+		{0,
+		 0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon<true,false>}
+
+	};
+
+	static const CollisionFunc collision_table_castB[5][5]={
+		{_collision_segment_segment<false,true>,
+		 _collision_segment_circle<false,true>,
+		 _collision_segment_rectangle<false,true>,
+		 _collision_segment_capsule<false,true>,
+		 _collision_segment_convex_polygon<false,true>},
+		{0,
+		 _collision_circle_circle<false,true>,
+		 _collision_circle_rectangle<false,true>,
+		 _collision_circle_capsule<false,true>,
+		 _collision_circle_convex_polygon<false,true>},
+		{0,
+		 0,
+		 _collision_rectangle_rectangle<false,true>,
+		 _collision_rectangle_capsule<false,true>,
+		 _collision_rectangle_convex_polygon<false,true>},
+		{0,
+		 0,
+		 0,
+		 _collision_capsule_capsule<false,true>,
+		 _collision_capsule_convex_polygon<false,true>},
+		{0,
+		 0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon<false,true>}
+
+	};
+
+	static const CollisionFunc collision_table_castA_castB[5][5]={
+		{_collision_segment_segment<true,true>,
+		 _collision_segment_circle<true,true>,
+		 _collision_segment_rectangle<true,true>,
+		 _collision_segment_capsule<true,true>,
+		 _collision_segment_convex_polygon<true,true>},
+		{0,
+		 _collision_circle_circle<true,true>,
+		 _collision_circle_rectangle<true,true>,
+		 _collision_circle_capsule<true,true>,
+		 _collision_circle_convex_polygon<true,true>},
+		{0,
+		 0,
+		 _collision_rectangle_rectangle<true,true>,
+		 _collision_rectangle_capsule<true,true>,
+		 _collision_rectangle_convex_polygon<true,true>},
+		{0,
+		 0,
+		 0,
+		 _collision_capsule_capsule<true,true>,
+		 _collision_capsule_convex_polygon<true,true>},
+		{0,
+		 0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon<true,true>}
 
 	};
 
@@ -1010,23 +1312,34 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_
 	const Shape2DSW *B=p_shape_B;
 	const Matrix32 *transform_A=&p_transform_A;
 	const Matrix32 *transform_B=&p_transform_B;
-	const Matrix32 *transform_inv_A=&p_transform_inv_A;
-	const Matrix32 *transform_inv_B=&p_transform_inv_B;
+	const Vector2 *motion_A=&p_motion_A;
+	const Vector2 *motion_B=&p_motion_B;
 
 	if (type_A > type_B) {
 		SWAP(A,B);
 		SWAP(transform_A,transform_B);
-		SWAP(transform_inv_A,transform_inv_B);
 		SWAP(type_A,type_B);
+		SWAP(motion_A,motion_B);
 		callback.swap = !callback.swap;
 	}
 
 
-	CollisionFunc collision_func = collision_table[type_A-2][type_B-2];
-	ERR_FAIL_COND_V(!collision_func,false);
+	CollisionFunc collision_func;
+	if (*motion_A==Vector2() && *motion_B==Vector2()) {
+		collision_func = collision_table[type_A-2][type_B-2];
+	} else if (*motion_A!=Vector2() &&  *motion_B==Vector2()) {
+		collision_func = collision_table_castA[type_A-2][type_B-2];
+	} else if (*motion_A==Vector2() && *motion_B!=Vector2()) {
+		collision_func = collision_table_castB[type_A-2][type_B-2];
+	} else {
+		collision_func = collision_table_castA_castB[type_A-2][type_B-2];
+	}
+
 
 
-	collision_func(A,*transform_A,*transform_inv_A,B,*transform_B,*transform_inv_B,&callback);
+	ERR_FAIL_COND_V(!collision_func,false);
+
+	collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B);
 
 	return callback.collided;
 

servers/physics_2d/collision_solver_2d_sat.h

@@ -32,6 +32,6 @@
 #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);
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_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

@@ -34,7 +34,7 @@
 //#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) {
+bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
 
 
 	const LineShape2DSW *line = static_cast<const LineShape2DSW*>(p_shape_A);
@@ -49,7 +49,7 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Mat
 	Vector2 supports[2];
 	int support_count;
 
-	p_shape_B->get_supports(p_transform_inv_B.basis_xform(-n).normalized(),supports,support_count);
+	p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(),supports,support_count);
 
 	bool found=false;
 
@@ -77,7 +77,7 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Mat
 	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) {
+bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
 
 
 
@@ -89,8 +89,9 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix3
 	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);
+	Matrix32 invb = p_transform_B.affine_inverse();
+	from = invb.xform(from);
+	to = invb.xform(to);
 
 	Vector2 p,n;
 	if (!p_shape_B->intersect_segment(from,to,p,n)) {
@@ -145,10 +146,10 @@ bool CollisionSolver2DSW::solve_ray(const Shape2DSW *p_shape_A,const Matrix32& p
 struct _ConcaveCollisionInfo2D {
 
 	const Matrix32 *transform_A;
-	const Matrix32 *transform_inv_A;
 	const Shape2DSW *shape_A;
 	const Matrix32 *transform_B;
-	const Matrix32 *transform_inv_B;
+	Vector2 motion_A;
+	Vector2 motion_B;
 	CollisionSolver2DSW::CallbackResult result_callback;
 	void *userdata;
 	bool swap_result;
@@ -168,7 +169,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
 	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 );
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis );
 	if (!collided)
 		return;
 
@@ -178,17 +179,16 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
 
 }
 
-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) {
+bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_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.motion_A=p_motion_A;
 	cinfo.result_callback=p_result_callback;
 	cinfo.userdata=p_userdata;
 	cinfo.swap_result=p_swap_result;
@@ -227,7 +227,8 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
 }
 
 
-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) {
+bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) {
+
 
 
 
@@ -253,9 +254,9 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32
 		}
 
 		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);
+			return solve_static_line(p_shape_B,p_transform_B,p_shape_A,p_transform_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);
+			return solve_static_line(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
 		}
 
 	/*} else if (type_A==Physics2DServer::SHAPE_RAY) {
@@ -278,9 +279,9 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32
 
 
 		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);
+			return solve_raycast(p_shape_B,p_transform_B,p_shape_A,p_transform_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);
+			return solve_raycast(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,sep_axis);
 		}
 
 
@@ -291,16 +292,16 @@ bool CollisionSolver2DSW::solve_static(const Shape2DSW *p_shape_A,const Matrix32
 			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);
+			return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_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);
+			return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_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 collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis);
 	}
 
 

servers/physics_2d/collision_solver_2d_sw.h

@@ -35,16 +35,16 @@ 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 bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_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);
+	static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_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 Shape2DSW *p_shape_B,const Matrix32& p_transform_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);
+	static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL);
 
 
 };

servers/physics_2d/physics_2d_server_sw.cpp

@@ -29,6 +29,7 @@
 #include "physics_2d_server_sw.h"
 #include "broad_phase_2d_basic.h"
 #include "broad_phase_2d_hash_grid.h"
+#include "collision_solver_2d_sw.h"
 
 RID Physics2DServerSW::shape_create(ShapeType p_shape) {
 
@@ -81,6 +82,9 @@ RID Physics2DServerSW::shape_create(ShapeType p_shape) {
 	return id;
 };
 
+
+
+
 void Physics2DServerSW::shape_set_data(RID p_shape, const Variant& p_data) {
 
 	Shape2DSW *shape = shape_owner.get(p_shape);
@@ -126,6 +130,63 @@ real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const {
 }
 
 
+void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
+
+	CollCbkData *cbk=(CollCbkData *)p_userdata;
+
+	if (cbk->amount == cbk->max) {
+		//find least deep
+		float min_depth=1e20;
+		int min_depth_idx=0;
+		for(int i=0;i<cbk->amount;i++) {
+
+			float d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]);
+			if (d<min_depth) {
+				min_depth=d;
+				min_depth_idx=i;
+			}
+
+		}
+
+		float d = p_point_A.distance_squared_to(p_point_B);
+		if (d<min_depth)
+			return;
+		cbk->ptr[min_depth_idx*2+0]=p_point_A;
+		cbk->ptr[min_depth_idx*2+1]=p_point_B;
+
+
+	} else {
+
+		cbk->ptr[cbk->amount*2+0]=p_point_A;
+		cbk->ptr[cbk->amount*2+1]=p_point_B;
+	}
+}
+
+bool Physics2DServerSW::shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count) {
+
+
+	Shape2DSW *shape_A = shape_owner.get(p_shape_A);
+	ERR_FAIL_COND_V(!shape_A,false);
+	Shape2DSW *shape_B = shape_owner.get(p_shape_B);
+	ERR_FAIL_COND_V(!shape_B,false);
+
+	if (p_result_max==0) {
+
+		return CollisionSolver2DSW::solve(shape_A,p_xform_A,p_motion_A,shape_B,p_xform_B,p_motion_B,NULL,NULL);
+	}
+
+	CollCbkData cbk;
+	cbk.max=p_result_max;
+	cbk.amount=0;
+	cbk.ptr=r_results;
+
+	bool res= CollisionSolver2DSW::solve(shape_A,p_xform_A,p_motion_A,shape_B,p_xform_B,p_motion_B,_shape_col_cbk,&cbk);
+	r_result_count=cbk.amount;
+	return res;
+
+}
+
+
 RID Physics2DServerSW::space_create() {
 
 	Space2DSW *space = memnew( Space2DSW );
@@ -442,7 +503,7 @@ void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
 	body->set_mode(p_mode);
 };
 
-Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body, BodyMode p_mode) const {
+Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body,BODY_MODE_STATIC);
@@ -550,23 +611,25 @@ bool Physics2DServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx
 }
 
 
-void Physics2DServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) {
+void Physics2DServerSW::body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-
-	body->set_continuous_collision_detection(p_enable);
+	body->set_continuous_collision_detection_mode(p_mode);
 
 }
 
-bool Physics2DServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const {
+Physics2DServerSW::CCDMode Physics2DServerSW::body_get_continuous_collision_detection_mode(RID p_body) const{
 
-	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
+	const Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,CCD_MODE_DISABLED);
+
+	return body->get_continuous_collision_detection_mode();
 
-	return body->is_continuous_collision_detection_enabled();
 }
 
+
+
 void Physics2DServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) {
 
 	Body2DSW *body = body_owner.get(p_body);
@@ -617,13 +680,6 @@ float Physics2DServerSW::body_get_param(RID p_body, BodyParameter p_param) const
 };
 
 
-void Physics2DServerSW::body_static_simulate_motion(RID p_body,const Matrix32& p_new_transform) {
-
-	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND(!body);
-	body->simulate_motion(p_new_transform,last_step);
-
-};
 
 void Physics2DServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) {
 
@@ -775,6 +831,16 @@ void Physics2DServerSW::body_set_force_integration_callback(RID p_body,Object *p
 
 }
 
+bool Physics2DServerSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,false);
+	ERR_FAIL_INDEX_V(p_body_shape,body->get_shape_count(),false);
+
+	return shape_collide(body->get_shape(p_body_shape)->get_self(),body->get_transform() * body->get_shape_transform(p_body_shape),Vector2(),p_shape,p_shape_xform,p_motion,r_results,p_result_max,r_result_count);
+
+}
+
 
 /* JOINT API */
 

servers/physics_2d/physics_2d_server_sw.h

@@ -58,6 +58,16 @@ friend class Physics2DDirectSpaceStateSW;
 	mutable RID_Owner<Body2DSW> body_owner;
 	mutable RID_Owner<Joint2DSW> joint_owner;
 
+	struct CollCbkData {
+
+		int max;
+		int amount;
+		Vector2 *ptr;
+	};
+
+	static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
+
+
 //	void _clear_query(Query2DSW *p_query);
 public:
 
@@ -69,6 +79,8 @@ public:
 	virtual Variant shape_get_data(RID p_shape) const;
 	virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
 
+	virtual bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count);
+
 	/* SPACE API */
 
 	virtual RID space_create();
@@ -124,7 +136,7 @@ public:
 	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 BodyMode body_get_mode(RID p_body) 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);
@@ -143,8 +155,8 @@ public:
 	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_continuous_collision_detection_mode(RID p_body,CCDMode p_mode);
+	virtual CCDMode body_get_continuous_collision_detection_mode(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;
@@ -152,8 +164,6 @@ public:
 	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;
@@ -181,6 +191,7 @@ public:
 	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());
+	virtual bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count);
 
 	/* JOINT API */
 

servers/physics_2d/shape_2d_sw.cpp

@@ -26,1060 +26,1059 @@
 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
-#include "shape_2d_sw.h"
-#include "geometry.h"
-#include "sort.h"
-
-#define _SEGMENT_IS_VALID_SUPPORT_TRESHOLD 0.99998
-
-
-void Shape2DSW::configure(const Rect2& p_aabb) {
-	aabb=p_aabb;
-	configured=true;
-	for (Map<ShapeOwner2DSW*,int>::Element *E=owners.front();E;E=E->next()) {
-		ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key();
-		co->_shape_changed();
-	}
-}
-
-
-Vector2 Shape2DSW::get_support(const Vector2& p_normal) const {
-
-	Vector2 res[2];
-	int amnt;
-	get_supports(p_normal,res,amnt);
-	return res[0];
-}
-
-void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
-
-	Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
-	if (E) {
-		E->get()++;
-	} else {
-		owners[p_owner]=1;
-	}
-}
-
-void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){
-
-	Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
-	ERR_FAIL_COND(!E);
-	E->get()--;
-	if (E->get()==0) {
-		owners.erase(E);
-	}
-
-}
-
-bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{
-
-	return owners.has(p_owner);
-
-}
-
-const Map<ShapeOwner2DSW*,int>& Shape2DSW::get_owners() const{
-	return owners;
-}
-
-
-Shape2DSW::Shape2DSW() {
-
-	custom_bias=0;
-	configured=false;
-}
-
-
-Shape2DSW::~Shape2DSW() {
-
-	ERR_FAIL_COND(owners.size());
-}
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	r_amount=0;
-}
-
-bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-	Vector2 segment= p_begin - p_end;
-	real_t den=normal.dot( segment );
-
-	//printf("den is %i\n",den);
-	if (Math::abs(den)<=CMP_EPSILON) {
-
-		return false;
-	}
-
-	real_t dist=(normal.dot( p_begin ) - d) / den;
-	//printf("dist is %i\n",dist);
-
-	if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) {
-
-		return false;
-	}
-
-	r_point = p_begin + segment * -dist;
-	r_normal=normal;
-
-	return true;
-}
-
-real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const {
-
-	return 0;
-}
-
-void LineShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY);
-	Array arr = p_data;
-	ERR_FAIL_COND(arr.size()!=2);
-	normal=arr[0];
-	d=arr[1];
-	configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2)));
-
-}
-
-Variant LineShape2DSW::get_data() const {
-
-	Array arr;
-	arr.resize(2);
-	arr[0]=normal;
-	arr[1]=d;
-	return arr;
-}
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-
-	r_amount=1;
-
-	if (p_normal.y>0)
-		*r_supports=Vector2(0,length);
-	else
-		*r_supports=Vector2();
-
-}
-
-bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-	return false; //rays can't be intersected
-
-}
-
-real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const {
-
-	return 0; //rays are mass-less
-}
-
-void RayShape2DSW::set_data(const Variant& p_data) {
-
-	length=p_data;
-	configure(Rect2(0,0,0.001,length));
-}
-
-Variant RayShape2DSW::get_data() const {
-
-	return length;
-}
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)  {
-		r_supports[0]=a;
-		r_supports[1]=b;
-		r_amount=2;
-		return;
-
-	}
-
-	float dp=p_normal.dot(b-a);
-	if (dp>0)
-		*r_supports=b;
-	else
-		*r_supports=a;
-	r_amount=1;
-
-}
-
-bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-	if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point))
-		return false;
-
-	Vector2 d = p_end-p_begin;
-	if (n.dot(p_begin) > n.dot(a)) {
-		r_normal=n;
-	} else {
-		r_normal=-n;
-	}
-
-	return true;
-}
-
-real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const {
-
-	real_t l = b.distance_to(a);
-	Vector2 ofs = (a+b)*0.5;
-
-	return p_mass*(l*l/12.0f + ofs.length_squared());
-}
-
-void SegmentShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2);
-
-	Rect2 r = p_data;
-	a=r.pos;
-	b=r.size;
-	n=(b-a).tangent();
-
-	Rect2 aabb;
-	aabb.pos=a;
-	aabb.expand_to(b);
-	if (aabb.size.x==0)
-		aabb.size.x=0.001;
-	if (aabb.size.y==0)
-		aabb.size.y=0.001;
-	configure(aabb);
-}
-
-Variant SegmentShape2DSW::get_data() const {
-
-	Rect2 r;
-	r.pos=a;
-	r.size=b;
-	return r;
-}
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	r_amount=1;
-	*r_supports=p_normal*radius;
-
-}
-
-bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-
-	Vector2 line_vec = p_end - p_begin;
-
-	real_t a, b, c;
-
-	a = line_vec.dot(line_vec);
-	b = 2 * p_begin.dot(line_vec);
-	c = p_begin.dot(p_begin) - radius * radius;
-
-	real_t sqrtterm = b*b - 4*a*c;
-
-	if(sqrtterm < 0)
-		return false;
-	sqrtterm = Math::sqrt(sqrtterm);
-	real_t res = ( -b - sqrtterm ) / (2 * a);
-
-	if (res <0 || res >1+CMP_EPSILON) {
-		return false;
-	}
-
-	r_point=p_begin+line_vec*res;
-	r_normal=r_point.normalized();
-	return true;
-}
-
-real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const {
-
-	return radius*radius;
-}
-
-void CircleShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(!p_data.is_num());
-	radius=p_data;
-	configure(Rect2(-radius,-radius,radius*2,radius*2));
-}
-
-Variant CircleShape2DSW::get_data() const {
-
-	return radius;
-}
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	for(int i=0;i<2;i++) {
-
-		Vector2 ag;
-		ag[i]=1.0;
-		float dp = ag.dot(p_normal);
-		if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
-			continue;
-
-		float sgn = dp>0 ? 1.0 : -1.0;
-
-		r_amount=2;
-
-		r_supports[0][i]=half_extents[i]*sgn;
-		r_supports[0][i^1]=half_extents[i^1];
-
-		r_supports[1][i]=half_extents[i]*sgn;
-		r_supports[1][i^1]=-half_extents[i^1];
-
-		return;
-
-
-	}
-
-	/* USE POINT */
-
-	r_amount=1;
-	r_supports[0]=Vector2(
-		(p_normal.x<0) ? -half_extents.x : half_extents.x,
-		(p_normal.y<0) ? -half_extents.y : half_extents.y
-	);
-
-}
-
-bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-
-	return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
-}
-
-real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const {
-
-	Vector2 he2=half_extents*2;
-	return p_mass*he2.dot(he2)/12.0f;
-}
-
-void RectangleShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2);
-
-	half_extents=p_data;
-	configure(Rect2(-half_extents,half_extents*2.0));
-}
-
-Variant RectangleShape2DSW::get_data() const {
-
-	return half_extents;
-}
-
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	Vector2 n=p_normal;
-
-	float d = n.y;
-
-	if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
-
-		// make it flat
-		n.y=0.0;
-		n.normalize();
-		n*=radius;
-
-		r_amount=2;
-		r_supports[0]=n;
-		r_supports[0].y+=height*0.5;
-		r_supports[1]=n;
-		r_supports[1].y-=height*0.5;
-
-	} else {
-
-		float h = (d > 0) ? height : -height;
-
-		n*=radius;
-		n.y += h*0.5;
-		r_amount=1;
-		*r_supports=n;
-
-	}
-}
-
-bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-
-	float d = 1e10;
-	Vector2 n = (p_end-p_begin).normalized();
-	bool collided=false;
-
-	//try spheres
-	for(int i=0;i<2;i++) {
-
-		Vector2 begin = p_begin;
-		Vector2 end = p_end;
-		float ofs = (i==0)?-height*0.5:height*0.5;
-		begin.y+=ofs;
-		end.y+=ofs;
-
-		Vector2 line_vec = end - begin;
-
-		real_t a, b, c;
-
-		a = line_vec.dot(line_vec);
-		b = 2 * begin.dot(line_vec);
-		c = begin.dot(begin) - radius * radius;
-
-		real_t sqrtterm = b*b - 4*a*c;
-
-		if(sqrtterm < 0)
-			continue;
-
-		sqrtterm = Math::sqrt(sqrtterm);
-		real_t res = ( -b - sqrtterm ) / (2 * a);
-
-		if (res <0 || res >1+CMP_EPSILON) {
-			continue;
-		}
-
-		Vector2 point = begin+line_vec*res;
-		Vector2 pointf(point.x,point.y-ofs);
-		real_t pd = n.dot(pointf);
-		if (pd<d) {
-			r_point=pointf;
-			r_normal=point.normalized();
-			d=pd;
-			collided=true;
-		}
-	}
-
-
-	Vector2 rpos,rnorm;
-	if (Rect2( Point2(-radius,-height*0.5), Size2(radius*2.0,height) ).intersects_segment(p_begin,p_end,&rpos,&rnorm)) {
-
-		real_t pd = n.dot(rpos);
-		if (pd<d) {
-			r_point=rpos;
-			r_normal=rnorm;
-			d=pd;
-			collided=true;
-		}
-	}
-
-	//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
-	return collided; //todo
-}
-
-real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass) const {
-
-	Vector2 he2(radius*2,height+radius*2);
-	return p_mass*he2.dot(he2)/12.0f;
-}
-
-void CapsuleShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2);
-
-	if (p_data.get_type()==Variant::ARRAY) {
-		Array arr=p_data;
-		ERR_FAIL_COND(arr.size()!=2);
-		height=arr[0];
-		radius=arr[1];
-	} else {
-
-		Point2 p = p_data;
-		radius=p.x;
-		height=p.y;
-	}
-
-	Point2 he(radius,height*0.5+radius);
-	configure(Rect2(-he,he*2));
-
-}
-
-Variant CapsuleShape2DSW::get_data() const {
-
-	return Point2(height,radius);
-}
-
-
-
-/*********************************************************/
-/*********************************************************/
-/*********************************************************/
-
-
-
-void ConvexPolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	int support_idx=-1;
-	real_t d=-1e10;
-
-	for(int i=0;i<point_count;i++) {
-
-		//test point
-		real_t ld = p_normal.dot(points[i].pos);
-		if (ld>d) {
-			support_idx=i;
-			d=ld;
-		}
-
-		//test segment
-		if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
-
-			r_amount=2;
-			r_supports[0]=points[i].pos;
-			r_supports[1]=points[(i+1)%point_count].pos;
-			return;
-		}
-	}
-
-	ERR_FAIL_COND(support_idx==-1);
-
-	r_amount=1;
-	r_supports[0]=points[support_idx].pos;
-
-}
-
-bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-	Vector2 n = (p_end-p_begin).normalized();
-	real_t d=1e10;
-	bool inters=false;
-
-	for(int i=0;i<point_count;i++) {
-
-		//hmm crap.. no can do..
-		//if (d.dot(points[i].normal)>=0)
-		//	continue;
-
-
-		Vector2 res;
-
-		if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res))
-			continue;
-
-		float nd = n.dot(res);
-		if (nd<d) {
-
-			d=nd;
-			r_point=res;
-			r_normal=points[i].normal;
-			inters=true;
-		}
-
-	}
-
-
-	if (inters) {
-
-		if (n.dot(r_normal)>0)
-			r_normal=-r_normal;
-	}
-
-	//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
-	return inters; //todo
-}
-
-real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const {
-
-	Rect2 aabb;
-	aabb.pos=points[0].pos;
-	for(int i=0;i<point_count;i++) {
-
-		aabb.expand_to(points[i].pos);
-	}
-
-	return p_mass*aabb.size.dot(aabb.size)/12.0f;
-}
-
-void ConvexPolygonShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
-
-
-	if (points)
-		memdelete_arr(points);
-	points=NULL;
-	point_count=0;
-
-	if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
-		DVector<Vector2> arr=p_data;
-		ERR_FAIL_COND(arr.size()==0);
-		point_count=arr.size();
-		points = memnew_arr(Point,point_count);
-		DVector<Vector2>::Read r = arr.read();
-
-		for(int i=0;i<point_count;i++) {
-			points[i].pos=r[i];
-		}
-
-		for(int i=0;i<point_count;i++) {
-
-			Vector2 p = points[i].pos;
-			Vector2 pn = points[(i+1)%point_count].pos;
-			points[i].normal=(pn-p).tangent().normalized();
-		}
-	} else {
-
-		DVector<real_t> dvr = p_data;
-		point_count=dvr.size()/4;
-		ERR_FAIL_COND(point_count==0);
-
-		points = memnew_arr(Point,point_count);
-		DVector<real_t>::Read r = dvr.read();
-
-		for(int i=0;i<point_count;i++) {
-
-			int idx=i<<2;
-			points[i].pos.x=r[idx+0];
-			points[i].pos.y=r[idx+1];
-			points[i].normal.x=r[idx+2];
-			points[i].normal.y=r[idx+3];
-
-		}
-	}
-
-
-	ERR_FAIL_COND(point_count==0);
-	Rect2 aabb;
-	aabb.pos=points[0].pos;
-	for(int i=1;i<point_count;i++)
-		aabb.expand_to(points[i].pos);
-
-	configure(aabb);
-}
-
-Variant ConvexPolygonShape2DSW::get_data() const {
-
-	DVector<Vector2> dvr;
-
-	dvr.resize(point_count);
-
-	for(int i=0;i<point_count;i++) {	
-		dvr.set(i,points[i].pos);
-	}
-
-	return dvr;
-}
-
-
-ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {
-
-	points=NULL;
-	point_count=0;
-
-}
-
-ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW(){
-
-	if (points)
-		memdelete_arr(points);
-
-}
-
-//////////////////////////////////////////////////
-
-
-void ConcavePolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	real_t d=-1e10;
-	int idx=-1;
-	for(int i=0;i<points.size();i++) {
-
-		real_t ld = p_normal.dot(points[i]);
-		if (ld>d) {
-			d=ld;
-			idx=i;
-		}
-	}
-
-
-	r_amount=1;
-	ERR_FAIL_COND(idx==-1);
-	*r_supports=points[idx];
-
-}
-
-bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{
-
-	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
-
-	enum {
-		TEST_AABB_BIT=0,
-		VISIT_LEFT_BIT=1,
-		VISIT_RIGHT_BIT=2,
-		VISIT_DONE_BIT=3,
-		VISITED_BIT_SHIFT=29,
-		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
-		VISITED_BIT_MASK=~NODE_IDX_MASK,
-
-
-	};
-
-	Vector2 n = (p_end-p_begin).normalized();
-	real_t d=1e10;
-	bool inters=false;
-
-	//for(int i=0;i<bvh_depth;i++)
-	//	stack[i]=0;
-
-	int level=0;
-
-	const Segment *segmentptr=&segments[0];
-	const Vector2 *pointptr=&points[0];
-	const BVH *bvhptr = &bvh[0];
-	int pos=bvh.size()-1;
-
-
-	stack[0]=0;
-	while(true) {
-
-		uint32_t node = stack[level]&NODE_IDX_MASK;
-		const BVH &b = bvhptr[ node ];
-		bool done=false;
-
-		switch(stack[level]>>VISITED_BIT_SHIFT) {
-			case TEST_AABB_BIT: {
-
-
-				bool valid = b.aabb.intersects_segment(p_begin,p_end);
-				if (!valid) {
-
-					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
-				} else {
-
-					if (b.left<0) {
-
-						const Segment &s=segmentptr[ b.right ];
-						Vector2 a = pointptr[ s.points[0] ];
-						Vector2 b = pointptr[ s.points[1] ];
-
-
-						Vector2 res;
-
-						if (Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&res)) {
-
-							float nd = n.dot(res);
-							if (nd<d) {
-
-								d=nd;
-								r_point=res;
-								r_normal=(b-a).tangent().normalized();
-								inters=true;
-							}
-
-						}
-
-						stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
-					} else {
-
-						stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
-					}
-				}
-
-			} continue;
-			case VISIT_LEFT_BIT: {
-
-				stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
-				stack[level+1]=b.left|TEST_AABB_BIT;
-				level++;
-
-			} continue;
-			case VISIT_RIGHT_BIT: {
-
-				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-				stack[level+1]=b.right|TEST_AABB_BIT;
-				level++;
-			} continue;
-			case VISIT_DONE_BIT: {
-
-				if (level==0) {
-					done=true;
-					break;
-				} else
-					level--;
-
-			} continue;
-		}
-
-
-		if (done)
-			break;
-	}
-
-
-	if (inters) {
-
-		if (n.dot(r_normal)>0)
-			r_normal=-r_normal;
-	}
-
-	return inters;
-
-}
-
-
-
-int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) {
-
-	if (p_len==1) {
-
-		bvh_depth=MAX(p_depth,bvh_depth);
-		bvh.push_back(*p_bvh);
-		return bvh.size()-1;
-	}
-
-	//else sort best
-
-	Rect2 global_aabb=p_bvh[0].aabb;
-	for(int i=1;i<p_len;i++) {
-		global_aabb=global_aabb.merge(p_bvh[i].aabb);
-	}
-
-	if (global_aabb.size.x > global_aabb.size.y) {
-
-		SortArray<BVH,BVH_CompareX> sort;
-		sort.sort(p_bvh,p_len);
-
-	} else {
-
-		SortArray<BVH,BVH_CompareY> sort;
-		sort.sort(p_bvh,p_len);
-
-	}
-
-	int median = p_len/2;
-
-
-	BVH node;
-	node.aabb=global_aabb;
-	int node_idx = bvh.size();
-	bvh.push_back(node);
-
-	int l = _generate_bvh(p_bvh,median,p_depth+1);
-	int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1);
-	bvh[node_idx].left=l;
-	bvh[node_idx].right=r;
-
-	return node_idx;
-
-}
-
-void ConcavePolygonShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
-
-	segments.clear();;
-	points.clear();;
-	bvh.clear();;
-	bvh_depth=1;
-
-	Rect2 aabb;
-
-	if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
-
-		DVector<Vector2> p2arr = p_data;
-		int len = p2arr.size();
-		DVector<Vector2>::Read arr = p2arr.read();
-
-
-		Map<Point2,int> pointmap;
-		for(int i=0;i<len;i+=2) {
-
-			Point2 p1 =arr[i];
-			Point2 p2 =arr[i+1];
-			int idx_p1,idx_p2;
-			if (p1==p2)
-				continue; //don't want it
-
-			if (pointmap.has(p1)) {
-				idx_p1=pointmap[p1];
-			} else {
-				idx_p1=pointmap.size();
-				pointmap[p1]=idx_p1;
-			}
-
-			if (pointmap.has(p2)) {
-				idx_p2=pointmap[p2];
-			} else {
-				idx_p2=pointmap.size();
-				pointmap[p2]=idx_p2;
-			}
-
-			Segment s;
-			s.points[0]=idx_p1;
-			s.points[1]=idx_p2;
-			segments.push_back(s);
-		}
-
-		points.resize(pointmap.size());
-		aabb.pos=pointmap.front()->key();
-		for(Map<Point2,int>::Element *E=pointmap.front();E;E=E->next()) {
-
-			aabb.expand_to(E->key());
-			points[E->get()]=E->key();
-		}
-
-		Vector<BVH> main_vbh;
-		main_vbh.resize(segments.size());
-		for(int i=0;i<main_vbh.size();i++) {
-
-
-			main_vbh[i].aabb.pos=points[segments[i].points[0]];
-			main_vbh[i].aabb.expand_to(points[segments[i].points[1]]);
-			main_vbh[i].left=-1;
-			main_vbh[i].right=i;
-		}
-
-		_generate_bvh(&main_vbh[0],main_vbh.size(),1);
-
-
-	} else {
-		//dictionary with arrays
-
-	}
-
-
-	configure(aabb);
-}
-Variant ConcavePolygonShape2DSW::get_data() const {
-
-
-	DVector<Vector2> rsegments;
-	int len = segments.size();
-	rsegments.resize(len*2);
-	DVector<Vector2>::Write w = rsegments.write();
-	for(int i=0;i<len;i++) {
-
-		w[(i<<1)+0]=points[segments[i].points[0]];
-		w[(i<<1)+1]=points[segments[i].points[1]];
-	}
-
-	w=DVector<Vector2>::Write();
-
-	return rsegments;
-}
-
-void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const {
-
-	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
-
-	enum {
-		TEST_AABB_BIT=0,
-		VISIT_LEFT_BIT=1,
-		VISIT_RIGHT_BIT=2,
-		VISIT_DONE_BIT=3,
-		VISITED_BIT_SHIFT=29,
-		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
-		VISITED_BIT_MASK=~NODE_IDX_MASK,
-
-
-	};
-
-	//for(int i=0;i<bvh_depth;i++)
-	//	stack[i]=0;
-
-
-	int level=0;
-
-	const Segment *segmentptr=&segments[0];
-	const Vector2 *pointptr=&points[0];
-	const BVH *bvhptr = &bvh[0];
-	int pos=bvh.size()-1;
-
-
-	stack[0]=0;
-	while(true) {
-
-		uint32_t node = stack[level]&NODE_IDX_MASK;
-		const BVH &b = bvhptr[ node ];
-
-		switch(stack[level]>>VISITED_BIT_SHIFT) {
-			case TEST_AABB_BIT: {
-
-
-				bool valid = p_local_aabb.intersects(b.aabb);
-				if (!valid) {
-
-					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
-				} else {
-
-					if (b.left<0) {
-
-						const Segment &s=segmentptr[ b.right ];
-						Vector2 a = pointptr[ s.points[0] ];
-						Vector2 b = pointptr[ s.points[1] ];
-
-						SegmentShape2DSW ss(a,b,(b-a).tangent().normalized());
-
-						p_callback(p_userdata,&ss);
-						stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-
-					} else {
-
-						stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
-					}
-				}
-
-			} continue;
-			case VISIT_LEFT_BIT: {
-
-				stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
-				stack[level+1]=b.left|TEST_AABB_BIT;
-				level++;
-
-			} continue;
-			case VISIT_RIGHT_BIT: {
-
-				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-				stack[level+1]=b.right|TEST_AABB_BIT;
-				level++;
-			} continue;
-			case VISIT_DONE_BIT: {
-
-				if (level==0)
-					return;
-				else
-					level--;
-
-			} continue;
-		}
-	}
-
-}
-
-
+#include "shape_2d_sw.h"
+#include "geometry.h"
+#include "sort.h"
+
+
+
+void Shape2DSW::configure(const Rect2& p_aabb) {
+	aabb=p_aabb;
+	configured=true;
+	for (Map<ShapeOwner2DSW*,int>::Element *E=owners.front();E;E=E->next()) {
+		ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key();
+		co->_shape_changed();
+	}
+}
+
+
+Vector2 Shape2DSW::get_support(const Vector2& p_normal) const {
+
+	Vector2 res[2];
+	int amnt;
+	get_supports(p_normal,res,amnt);
+	return res[0];
+}
+
+void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
+
+	Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
+	if (E) {
+		E->get()++;
+	} else {
+		owners[p_owner]=1;
+	}
+}
+
+void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){
+
+	Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
+	ERR_FAIL_COND(!E);
+	E->get()--;
+	if (E->get()==0) {
+		owners.erase(E);
+	}
+
+}
+
+bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{
+
+	return owners.has(p_owner);
+
+}
+
+const Map<ShapeOwner2DSW*,int>& Shape2DSW::get_owners() const{
+	return owners;
+}
+
+
+Shape2DSW::Shape2DSW() {
+
+	custom_bias=0;
+	configured=false;
+}
+
+
+Shape2DSW::~Shape2DSW() {
+
+	ERR_FAIL_COND(owners.size());
+}
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+	r_amount=0;
+}
+
+bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+	Vector2 segment= p_begin - p_end;
+	real_t den=normal.dot( segment );
+
+	//printf("den is %i\n",den);
+	if (Math::abs(den)<=CMP_EPSILON) {
+
+		return false;
+	}
+
+	real_t dist=(normal.dot( p_begin ) - d) / den;
+	//printf("dist is %i\n",dist);
+
+	if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) {
+
+		return false;
+	}
+
+	r_point = p_begin + segment * -dist;
+	r_normal=normal;
+
+	return true;
+}
+
+real_t LineShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+	return 0;
+}
+
+void LineShape2DSW::set_data(const Variant& p_data) {
+
+	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY);
+	Array arr = p_data;
+	ERR_FAIL_COND(arr.size()!=2);
+	normal=arr[0];
+	d=arr[1];
+	configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2)));
+
+}
+
+Variant LineShape2DSW::get_data() const {
+
+	Array arr;
+	arr.resize(2);
+	arr[0]=normal;
+	arr[1]=d;
+	return arr;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+
+	r_amount=1;
+
+	if (p_normal.y>0)
+		*r_supports=Vector2(0,length);
+	else
+		*r_supports=Vector2();
+
+}
+
+bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+	return false; //rays can't be intersected
+
+}
+
+real_t RayShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+	return 0; //rays are mass-less
+}
+
+void RayShape2DSW::set_data(const Variant& p_data) {
+
+	length=p_data;
+	configure(Rect2(0,0,0.001,length));
+}
+
+Variant RayShape2DSW::get_data() const {
+
+	return length;
+}
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+	if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)  {
+		r_supports[0]=a;
+		r_supports[1]=b;
+		r_amount=2;
+		return;
+
+	}
+
+	float dp=p_normal.dot(b-a);
+	if (dp>0)
+		*r_supports=b;
+	else
+		*r_supports=a;
+	r_amount=1;
+
+}
+
+bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+	if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point))
+		return false;
+
+	Vector2 d = p_end-p_begin;
+	if (n.dot(p_begin) > n.dot(a)) {
+		r_normal=n;
+	} else {
+		r_normal=-n;
+	}
+
+	return true;
+}
+
+real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+	real_t l = b.distance_to(a);
+	Vector2 ofs = (a+b)*0.5;
+
+	return p_mass*(l*l/12.0f + ofs.length_squared());
+}
+
+void SegmentShape2DSW::set_data(const Variant& p_data) {
+
+	ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2);
+
+	Rect2 r = p_data;
+	a=r.pos;
+	b=r.size;
+	n=(b-a).tangent();
+
+	Rect2 aabb;
+	aabb.pos=a;
+	aabb.expand_to(b);
+	if (aabb.size.x==0)
+		aabb.size.x=0.001;
+	if (aabb.size.y==0)
+		aabb.size.y=0.001;
+	configure(aabb);
+}
+
+Variant SegmentShape2DSW::get_data() const {
+
+	Rect2 r;
+	r.pos=a;
+	r.size=b;
+	return r;
+}
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+	r_amount=1;
+	*r_supports=p_normal*radius;
+
+}
+
+bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+
+	Vector2 line_vec = p_end - p_begin;
+
+	real_t a, b, c;
+
+	a = line_vec.dot(line_vec);
+	b = 2 * p_begin.dot(line_vec);
+	c = p_begin.dot(p_begin) - radius * radius;
+
+	real_t sqrtterm = b*b - 4*a*c;
+
+	if(sqrtterm < 0)
+		return false;
+	sqrtterm = Math::sqrt(sqrtterm);
+	real_t res = ( -b - sqrtterm ) / (2 * a);
+
+	if (res <0 || res >1+CMP_EPSILON) {
+		return false;
+	}
+
+	r_point=p_begin+line_vec*res;
+	r_normal=r_point.normalized();
+	return true;
+}
+
+real_t CircleShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+	return radius*radius;
+}
+
+void CircleShape2DSW::set_data(const Variant& p_data) {
+
+	ERR_FAIL_COND(!p_data.is_num());
+	radius=p_data;
+	configure(Rect2(-radius,-radius,radius*2,radius*2));
+}
+
+Variant CircleShape2DSW::get_data() const {
+
+	return radius;
+}
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+	for(int i=0;i<2;i++) {
+
+		Vector2 ag;
+		ag[i]=1.0;
+		float dp = ag.dot(p_normal);
+		if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
+			continue;
+
+		float sgn = dp>0 ? 1.0 : -1.0;
+
+		r_amount=2;
+
+		r_supports[0][i]=half_extents[i]*sgn;
+		r_supports[0][i^1]=half_extents[i^1];
+
+		r_supports[1][i]=half_extents[i]*sgn;
+		r_supports[1][i^1]=-half_extents[i^1];
+
+		return;
+
+
+	}
+
+	/* USE POINT */
+
+	r_amount=1;
+	r_supports[0]=Vector2(
+		(p_normal.x<0) ? -half_extents.x : half_extents.x,
+		(p_normal.y<0) ? -half_extents.y : half_extents.y
+	);
+
+}
+
+bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+
+	return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
+}
+
+real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+	Vector2 he2=half_extents*2;
+	return p_mass*he2.dot(he2)/12.0f;
+}
+
+void RectangleShape2DSW::set_data(const Variant& p_data) {
+
+	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2);
+
+	half_extents=p_data;
+	configure(Rect2(-half_extents,half_extents*2.0));
+}
+
+Variant RectangleShape2DSW::get_data() const {
+
+	return half_extents;
+}
+
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+	Vector2 n=p_normal;
+
+	float d = n.y;
+
+	if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
+
+		// make it flat
+		n.y=0.0;
+		n.normalize();
+		n*=radius;
+
+		r_amount=2;
+		r_supports[0]=n;
+		r_supports[0].y+=height*0.5;
+		r_supports[1]=n;
+		r_supports[1].y-=height*0.5;
+
+	} else {
+
+		float h = (d > 0) ? height : -height;
+
+		n*=radius;
+		n.y += h*0.5;
+		r_amount=1;
+		*r_supports=n;
+
+	}
+}
+
+bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+
+	float d = 1e10;
+	Vector2 n = (p_end-p_begin).normalized();
+	bool collided=false;
+
+	//try spheres
+	for(int i=0;i<2;i++) {
+
+		Vector2 begin = p_begin;
+		Vector2 end = p_end;
+		float ofs = (i==0)?-height*0.5:height*0.5;
+		begin.y+=ofs;
+		end.y+=ofs;
+
+		Vector2 line_vec = end - begin;
+
+		real_t a, b, c;
+
+		a = line_vec.dot(line_vec);
+		b = 2 * begin.dot(line_vec);
+		c = begin.dot(begin) - radius * radius;
+
+		real_t sqrtterm = b*b - 4*a*c;
+
+		if(sqrtterm < 0)
+			continue;
+
+		sqrtterm = Math::sqrt(sqrtterm);
+		real_t res = ( -b - sqrtterm ) / (2 * a);
+
+		if (res <0 || res >1+CMP_EPSILON) {
+			continue;
+		}
+
+		Vector2 point = begin+line_vec*res;
+		Vector2 pointf(point.x,point.y-ofs);
+		real_t pd = n.dot(pointf);
+		if (pd<d) {
+			r_point=pointf;
+			r_normal=point.normalized();
+			d=pd;
+			collided=true;
+		}
+	}
+
+
+	Vector2 rpos,rnorm;
+	if (Rect2( Point2(-radius,-height*0.5), Size2(radius*2.0,height) ).intersects_segment(p_begin,p_end,&rpos,&rnorm)) {
+
+		real_t pd = n.dot(rpos);
+		if (pd<d) {
+			r_point=rpos;
+			r_normal=rnorm;
+			d=pd;
+			collided=true;
+		}
+	}
+
+	//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
+	return collided; //todo
+}
+
+real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+	Vector2 he2(radius*2,height+radius*2);
+	return p_mass*he2.dot(he2)/12.0f;
+}
+
+void CapsuleShape2DSW::set_data(const Variant& p_data) {
+
+	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2);
+
+	if (p_data.get_type()==Variant::ARRAY) {
+		Array arr=p_data;
+		ERR_FAIL_COND(arr.size()!=2);
+		height=arr[0];
+		radius=arr[1];
+	} else {
+
+		Point2 p = p_data;
+		radius=p.x;
+		height=p.y;
+	}
+
+	Point2 he(radius,height*0.5+radius);
+	configure(Rect2(-he,he*2));
+
+}
+
+Variant CapsuleShape2DSW::get_data() const {
+
+	return Point2(height,radius);
+}
+
+
+
+/*********************************************************/
+/*********************************************************/
+/*********************************************************/
+
+
+
+void ConvexPolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+	int support_idx=-1;
+	real_t d=-1e10;
+
+	for(int i=0;i<point_count;i++) {
+
+		//test point
+		real_t ld = p_normal.dot(points[i].pos);
+		if (ld>d) {
+			support_idx=i;
+			d=ld;
+		}
+
+		//test segment
+		if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
+
+			r_amount=2;
+			r_supports[0]=points[i].pos;
+			r_supports[1]=points[(i+1)%point_count].pos;
+			return;
+		}
+	}
+
+	ERR_FAIL_COND(support_idx==-1);
+
+	r_amount=1;
+	r_supports[0]=points[support_idx].pos;
+
+}
+
+bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+
+	Vector2 n = (p_end-p_begin).normalized();
+	real_t d=1e10;
+	bool inters=false;
+
+	for(int i=0;i<point_count;i++) {
+
+		//hmm crap.. no can do..
+		//if (d.dot(points[i].normal)>=0)
+		//	continue;
+
+
+		Vector2 res;
+
+		if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res))
+			continue;
+
+		float nd = n.dot(res);
+		if (nd<d) {
+
+			d=nd;
+			r_point=res;
+			r_normal=points[i].normal;
+			inters=true;
+		}
+
+	}
+
+
+	if (inters) {
+
+		if (n.dot(r_normal)>0)
+			r_normal=-r_normal;
+	}
+
+	//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
+	return inters; //todo
+}
+
+real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass) const {
+
+	Rect2 aabb;
+	aabb.pos=points[0].pos;
+	for(int i=0;i<point_count;i++) {
+
+		aabb.expand_to(points[i].pos);
+	}
+
+	return p_mass*aabb.size.dot(aabb.size)/12.0f;
+}
+
+void ConvexPolygonShape2DSW::set_data(const Variant& p_data) {
+
+	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
+
+
+	if (points)
+		memdelete_arr(points);
+	points=NULL;
+	point_count=0;
+
+	if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
+		DVector<Vector2> arr=p_data;
+		ERR_FAIL_COND(arr.size()==0);
+		point_count=arr.size();
+		points = memnew_arr(Point,point_count);
+		DVector<Vector2>::Read r = arr.read();
+
+		for(int i=0;i<point_count;i++) {
+			points[i].pos=r[i];
+		}
+
+		for(int i=0;i<point_count;i++) {
+
+			Vector2 p = points[i].pos;
+			Vector2 pn = points[(i+1)%point_count].pos;
+			points[i].normal=(pn-p).tangent().normalized();
+		}
+	} else {
+
+		DVector<real_t> dvr = p_data;
+		point_count=dvr.size()/4;
+		ERR_FAIL_COND(point_count==0);
+
+		points = memnew_arr(Point,point_count);
+		DVector<real_t>::Read r = dvr.read();
+
+		for(int i=0;i<point_count;i++) {
+
+			int idx=i<<2;
+			points[i].pos.x=r[idx+0];
+			points[i].pos.y=r[idx+1];
+			points[i].normal.x=r[idx+2];
+			points[i].normal.y=r[idx+3];
+
+		}
+	}
+
+
+	ERR_FAIL_COND(point_count==0);
+	Rect2 aabb;
+	aabb.pos=points[0].pos;
+	for(int i=1;i<point_count;i++)
+		aabb.expand_to(points[i].pos);
+
+	configure(aabb);
+}
+
+Variant ConvexPolygonShape2DSW::get_data() const {
+
+	DVector<Vector2> dvr;
+
+	dvr.resize(point_count);
+
+	for(int i=0;i<point_count;i++) {	
+		dvr.set(i,points[i].pos);
+	}
+
+	return dvr;
+}
+
+
+ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {
+
+	points=NULL;
+	point_count=0;
+
+}
+
+ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW(){
+
+	if (points)
+		memdelete_arr(points);
+
+}
+
+//////////////////////////////////////////////////
+
+
+void ConcavePolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
+
+	real_t d=-1e10;
+	int idx=-1;
+	for(int i=0;i<points.size();i++) {
+
+		real_t ld = p_normal.dot(points[i]);
+		if (ld>d) {
+			d=ld;
+			idx=i;
+		}
+	}
+
+
+	r_amount=1;
+	ERR_FAIL_COND(idx==-1);
+	*r_supports=points[idx];
+
+}
+
+bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{
+
+	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
+
+	enum {
+		TEST_AABB_BIT=0,
+		VISIT_LEFT_BIT=1,
+		VISIT_RIGHT_BIT=2,
+		VISIT_DONE_BIT=3,
+		VISITED_BIT_SHIFT=29,
+		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
+		VISITED_BIT_MASK=~NODE_IDX_MASK,
+
+
+	};
+
+	Vector2 n = (p_end-p_begin).normalized();
+	real_t d=1e10;
+	bool inters=false;
+
+	//for(int i=0;i<bvh_depth;i++)
+	//	stack[i]=0;
+
+	int level=0;
+
+	const Segment *segmentptr=&segments[0];
+	const Vector2 *pointptr=&points[0];
+	const BVH *bvhptr = &bvh[0];
+	int pos=bvh.size()-1;
+
+
+	stack[0]=0;
+	while(true) {
+
+		uint32_t node = stack[level]&NODE_IDX_MASK;
+		const BVH &b = bvhptr[ node ];
+		bool done=false;
+
+		switch(stack[level]>>VISITED_BIT_SHIFT) {
+			case TEST_AABB_BIT: {
+
+
+				bool valid = b.aabb.intersects_segment(p_begin,p_end);
+				if (!valid) {
+
+					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+
+				} else {
+
+					if (b.left<0) {
+
+						const Segment &s=segmentptr[ b.right ];
+						Vector2 a = pointptr[ s.points[0] ];
+						Vector2 b = pointptr[ s.points[1] ];
+
+
+						Vector2 res;
+
+						if (Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&res)) {
+
+							float nd = n.dot(res);
+							if (nd<d) {
+
+								d=nd;
+								r_point=res;
+								r_normal=(b-a).tangent().normalized();
+								inters=true;
+							}
+
+						}
+
+						stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+
+					} else {
+
+						stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
+					}
+				}
+
+			} continue;
+			case VISIT_LEFT_BIT: {
+
+				stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
+				stack[level+1]=b.left|TEST_AABB_BIT;
+				level++;
+
+			} continue;
+			case VISIT_RIGHT_BIT: {
+
+				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+				stack[level+1]=b.right|TEST_AABB_BIT;
+				level++;
+			} continue;
+			case VISIT_DONE_BIT: {
+
+				if (level==0) {
+					done=true;
+					break;
+				} else
+					level--;
+
+			} continue;
+		}
+
+
+		if (done)
+			break;
+	}
+
+
+	if (inters) {
+
+		if (n.dot(r_normal)>0)
+			r_normal=-r_normal;
+	}
+
+	return inters;
+
+}
+
+
+
+int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) {
+
+	if (p_len==1) {
+
+		bvh_depth=MAX(p_depth,bvh_depth);
+		bvh.push_back(*p_bvh);
+		return bvh.size()-1;
+	}
+
+	//else sort best
+
+	Rect2 global_aabb=p_bvh[0].aabb;
+	for(int i=1;i<p_len;i++) {
+		global_aabb=global_aabb.merge(p_bvh[i].aabb);
+	}
+
+	if (global_aabb.size.x > global_aabb.size.y) {
+
+		SortArray<BVH,BVH_CompareX> sort;
+		sort.sort(p_bvh,p_len);
+
+	} else {
+
+		SortArray<BVH,BVH_CompareY> sort;
+		sort.sort(p_bvh,p_len);
+
+	}
+
+	int median = p_len/2;
+
+
+	BVH node;
+	node.aabb=global_aabb;
+	int node_idx = bvh.size();
+	bvh.push_back(node);
+
+	int l = _generate_bvh(p_bvh,median,p_depth+1);
+	int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1);
+	bvh[node_idx].left=l;
+	bvh[node_idx].right=r;
+
+	return node_idx;
+
+}
+
+void ConcavePolygonShape2DSW::set_data(const Variant& p_data) {
+
+	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
+
+	segments.clear();;
+	points.clear();;
+	bvh.clear();;
+	bvh_depth=1;
+
+	Rect2 aabb;
+
+	if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
+
+		DVector<Vector2> p2arr = p_data;
+		int len = p2arr.size();
+		DVector<Vector2>::Read arr = p2arr.read();
+
+
+		Map<Point2,int> pointmap;
+		for(int i=0;i<len;i+=2) {
+
+			Point2 p1 =arr[i];
+			Point2 p2 =arr[i+1];
+			int idx_p1,idx_p2;
+			if (p1==p2)
+				continue; //don't want it
+
+			if (pointmap.has(p1)) {
+				idx_p1=pointmap[p1];
+			} else {
+				idx_p1=pointmap.size();
+				pointmap[p1]=idx_p1;
+			}
+
+			if (pointmap.has(p2)) {
+				idx_p2=pointmap[p2];
+			} else {
+				idx_p2=pointmap.size();
+				pointmap[p2]=idx_p2;
+			}
+
+			Segment s;
+			s.points[0]=idx_p1;
+			s.points[1]=idx_p2;
+			segments.push_back(s);
+		}
+
+		points.resize(pointmap.size());
+		aabb.pos=pointmap.front()->key();
+		for(Map<Point2,int>::Element *E=pointmap.front();E;E=E->next()) {
+
+			aabb.expand_to(E->key());
+			points[E->get()]=E->key();
+		}
+
+		Vector<BVH> main_vbh;
+		main_vbh.resize(segments.size());
+		for(int i=0;i<main_vbh.size();i++) {
+
+
+			main_vbh[i].aabb.pos=points[segments[i].points[0]];
+			main_vbh[i].aabb.expand_to(points[segments[i].points[1]]);
+			main_vbh[i].left=-1;
+			main_vbh[i].right=i;
+		}
+
+		_generate_bvh(&main_vbh[0],main_vbh.size(),1);
+
+
+	} else {
+		//dictionary with arrays
+
+	}
+
+
+	configure(aabb);
+}
+Variant ConcavePolygonShape2DSW::get_data() const {
+
+
+	DVector<Vector2> rsegments;
+	int len = segments.size();
+	rsegments.resize(len*2);
+	DVector<Vector2>::Write w = rsegments.write();
+	for(int i=0;i<len;i++) {
+
+		w[(i<<1)+0]=points[segments[i].points[0]];
+		w[(i<<1)+1]=points[segments[i].points[1]];
+	}
+
+	w=DVector<Vector2>::Write();
+
+	return rsegments;
+}
+
+void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const {
+
+	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
+
+	enum {
+		TEST_AABB_BIT=0,
+		VISIT_LEFT_BIT=1,
+		VISIT_RIGHT_BIT=2,
+		VISIT_DONE_BIT=3,
+		VISITED_BIT_SHIFT=29,
+		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
+		VISITED_BIT_MASK=~NODE_IDX_MASK,
+
+
+	};
+
+	//for(int i=0;i<bvh_depth;i++)
+	//	stack[i]=0;
+
+
+	int level=0;
+
+	const Segment *segmentptr=&segments[0];
+	const Vector2 *pointptr=&points[0];
+	const BVH *bvhptr = &bvh[0];
+	int pos=bvh.size()-1;
+
+
+	stack[0]=0;
+	while(true) {
+
+		uint32_t node = stack[level]&NODE_IDX_MASK;
+		const BVH &b = bvhptr[ node ];
+
+		switch(stack[level]>>VISITED_BIT_SHIFT) {
+			case TEST_AABB_BIT: {
+
+
+				bool valid = p_local_aabb.intersects(b.aabb);
+				if (!valid) {
+
+					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+
+				} else {
+
+					if (b.left<0) {
+
+						const Segment &s=segmentptr[ b.right ];
+						Vector2 a = pointptr[ s.points[0] ];
+						Vector2 b = pointptr[ s.points[1] ];
+
+						SegmentShape2DSW ss(a,b,(b-a).tangent().normalized());
+
+						p_callback(p_userdata,&ss);
+						stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+
+					} else {
+
+						stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
+					}
+				}
+
+			} continue;
+			case VISIT_LEFT_BIT: {
+
+				stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
+				stack[level+1]=b.left|TEST_AABB_BIT;
+				level++;
+
+			} continue;
+			case VISIT_RIGHT_BIT: {
+
+				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+				stack[level+1]=b.right|TEST_AABB_BIT;
+				level++;
+			} continue;
+			case VISIT_DONE_BIT: {
+
+				if (level==0)
+					return;
+				else
+					level--;
+
+			} continue;
+		}
+	}
+
+}
+
+

servers/physics_2d/shape_2d_sw.h

@@ -26,417 +26,516 @@
 /* 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
+#ifndef SHAPE_2D_2DSW_H
+#define SHAPE_2D_2DSW_H
+
+#include "servers/physics_2d_server.h"
+#define _SEGMENT_IS_VALID_SUPPORT_TRESHOLD 0.99998
+
+/*
+
+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 void project_range_castv(const Vector2& p_cast, 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;
+
+
+	_FORCE_INLINE_ void get_supports_transformed_cast(const Vector2& p_cast,const Vector2& p_normal,const Matrix32& p_xform,Vector2 *r_supports,int & r_amount) const {
+
+		get_supports(p_xform.basis_xform_inv(p_normal).normalized(),r_supports,r_amount);
+		for(int i=0;i<r_amount;i++)
+			r_supports[i]=p_xform.xform(r_supports[i]);
+
+		if (r_amount==1) {
+
+			if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
+				//make line because they are parallel
+				r_amount=2;
+				r_supports[1]=r_supports[0]+p_cast;
+			} else if (p_cast.dot(p_normal)>0) {
+				//normal points towards cast, add cast
+				r_supports[0]+=p_cast;
+			}
+
+		} else {
+
+			if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
+				//optimize line and make it larger because they are parallel
+				if ((r_supports[1]-r_supports[0]).dot(p_cast)>0) {
+					//larger towards 1
+					r_supports[1]+=p_cast;
+				} else {
+					//larger towards 0
+					r_supports[0]+=p_cast;
+				}
+			} else if (p_cast.dot(p_normal)>0) {
+				//normal points towards cast, add cast
+				r_supports[0]+=p_cast;
+				r_supports[1]+=p_cast;
+			}
+
+		}
+	}
+
+	Shape2DSW();
+	virtual ~Shape2DSW();
+};
+
+//let the optimizer do the magic
+#define DEFAULT_PROJECT_RANGE_CAST \
+virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {\
+	project_range_cast(p_cast,p_normal,p_transform,r_min,r_max);\
+}\
+_FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {\
+\
+	real_t mina,maxa;\
+	real_t minb,maxb;\
+	Matrix32 ofsb=p_transform;\
+	ofsb.elements[2]+=p_cast;\
+	project_range(p_normal,p_transform,mina,maxa);\
+	project_range(p_normal,ofsb,minb,maxb);	\
+	r_min=MIN(mina,minb);\
+	r_max=MAX(maxa,maxb);\
+}
+
+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;
+	}
+
+	virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+		project_range_cast(p_cast,p_normal,p_transform,r_min,r_max);
+	}
+
+	_FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, 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);
+		}
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+
+	_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; }
+
+	_FORCE_INLINE_ Vector2 get_xformed_normal(const Matrix32& p_xform) const {
+
+		return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent();
+	}
+	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);
+		}
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+	_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;
+	}
+
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+};
+
+
+
+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;
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+};
+
+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 );
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+};
+
+
+
+
+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; }
+	_FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Matrix32& p_xform, int p_idx) const {
+
+		Vector2 a = points[p_idx].pos;
+		p_idx++;
+		Vector2 b = points[p_idx==point_count?0:p_idx].pos;
+		return (p_xform.xform(b)-p_xform.xform(a)).normalized().tangent();
+	}
+
+	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;
+
+		}
+
+	}
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+	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 project_range(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;
+
+
+	DEFAULT_PROJECT_RANGE_CAST
+
+};
+
+
+#endif // SHAPE_2D_2DSW_H

servers/physics_2d/space_2d_sw.cpp

@@ -26,407 +26,597 @@
 /* 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 );
-}
-
-
-
+#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,const Vector2& p_motion,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(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),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;
+
+}
+
+
+struct MotionCallbackRayCastData {
+
+	Vector2 best_contact;
+	Vector2 best_normal;
+	float best_len;
+	Matrix32 b_xform_inv;
+	Matrix32 b_xform;
+	Vector2 motion;
+	Shape2DSW * shape_B;
+
+};
+
+static void _motion_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
+
+
+	MotionCallbackRayCastData *rd=(MotionCallbackRayCastData*)p_userdata;
+
+	Vector2 contact_normal = (p_point_B-p_point_A).normalized();
+
+	Vector2 from=p_point_A-(rd->motion*1.01);
+	Vector2 p,n;
+
+	if (contact_normal.dot(rd->motion.normalized())<CMP_EPSILON) {
+		//safe to assume it was a perpendicular collision
+		n=contact_normal;
+		p=p_point_B;
+	} else {
+		//entered in a different angle
+		Vector2 to = p_point_A+rd->motion; //avoid precission issues
+
+
+		bool res = rd->shape_B->intersect_segment(rd->b_xform_inv.xform(from),rd->b_xform_inv.xform(to),p,n);
+
+
+		if (!res) {
+			print_line("lolwut failed");
+			return;
+		}
+
+		p = rd->b_xform.xform(p);
+
+		n = rd->b_xform_inv.basis_xform_inv(n).normalized();
+	}
+
+	float len = p.distance_to(from);
+
+	if (len<rd->best_len) {
+		rd->best_contact=p;
+		rd->best_normal=n;
+		rd->best_len=len;
+	}
+}
+
+bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+	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());
+	aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	bool collided=false;
+	r_result.travel=1;
+
+	MotionCallbackRayCastData best_normal;
+	best_normal.best_len=1e20;
+	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; //ignore excluded
+
+
+		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+
+		Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+		//test initial overlap, does it collide if going all the way?
+		if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) {
+
+			continue;
+		}
+
+
+		//test initial overlap
+		if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) {
+
+			r_result.collider_id=col_obj->get_instance_id();
+			r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL;
+			r_result.shape=shape_idx;
+			r_result.rid=col_obj->get_self();
+			r_result.travel=0;
+			r_result.point=Vector2();
+			r_result.normal=Vector2();
+			return true;
+		}
+
+#if 0
+		Vector2 mnormal=p_motion.normalized();
+		Matrix32 col_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+		ShapeSW *col_shape = col_obj->get_shape(shape_idx);
+
+		real_t min,max;
+		col_shape->project_rangev(mnormal,col_shape_xform,min,max);
+		real_t width = max-min;
+
+		int a;
+		Vector2 s[2];
+		col_shape->get_supports(col_shape_xform.basis_xform(mnormal).normalized(),s,a);
+		Vector2 from = col_shape_xform.xform(s[0]);
+		Vector2 to = from + p_motion;
+
+		Matrix32 from_inv = col_shape_xform.affine_inverse();
+
+		Vector2 local_from = from_inv.xform(from-mnormal*width*0.1); //start from a little inside the bounding box
+		Vector2 local_to = from_inv.xform(to);
+
+		Vector2 rpos,rnorm;
+		if (!col_shape->intersect_segment(local_from,local_to,rpos,rnorm))
+			return false;
+
+		//ray hit something
+
+
+		Vector2 hitpos = p_xform_B.xform(rpos);
+#endif
+
+		//just do kinematic solving
+		float low=0;
+		float hi=1;
+		Vector2 mnormal=p_motion.normalized();
+
+		for(int i=0;i<8;i++) { //steps should be customizable..
+
+			Matrix32 xfa = p_xform;
+			float ofs = (low+hi)*0.5;
+
+			Vector2 sep=mnormal; //important optimization for this to work fast enough
+			bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep);
+
+			if (collided) {
+
+				hi=ofs;
+			} else {
+
+				low=ofs;
+			}
+		}
+
+
+		best_normal.shape_B=col_obj->get_shape(shape_idx);
+		best_normal.motion=p_motion*hi;
+		best_normal.b_xform=col_obj_xform;
+		best_normal.b_xform_inv=col_obj_xform.affine_inverse();
+
+		bool sc = CollisionSolver2DSW::solve(shape,p_xform,p_motion*hi,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_motion_cbk_result,&best_normal);
+		print_line("CLD: "+itos(sc));
+
+
+		if (collided && low>=r_result.travel)
+			continue;
+
+		collided=true;
+		r_result.travel=low;
+
+		r_result.collider_id=col_obj->get_instance_id();
+		r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL;
+		r_result.shape=shape_idx;
+		r_result.rid=col_obj->get_self();
+
+	}
+
+	if (collided) {
+		ERR_FAIL_COND_V(best_normal.best_normal==Vector2(),false);
+		r_result.normal=best_normal.best_normal;
+		r_result.point=best_normal.best_contact;
+	}
+
+	return collided;
+
+
+}
+
+
+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

@@ -26,135 +26,136 @@
 /* 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
+#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,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+	bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, 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

@@ -49,7 +49,7 @@ void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2
 			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)
+			if (b->get_island_step()==_step || b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC)
 				continue; //no go
 			_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
 		}
@@ -87,8 +87,10 @@ void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) {
 	Body2DSW *b = p_island;
 	while(b) {
 
-		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
+		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) {
+			b=b->get_island_next();
 			continue; //ignore for static
+		}
 
 		if (!b->sleep_test(p_delta))
 			can_sleep=false;
@@ -101,8 +103,10 @@ void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) {
 	b = p_island;
 	while(b) {
 
-		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC)
+		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) {
+			b=b->get_island_next();
 			continue; //ignore for static
+		}
 
 		bool active = b->is_active();
 
@@ -210,8 +214,9 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
 	b = body_list->first();
 	while(b) {
 
+		const SelfList<Body2DSW>*n=b->next();
 		b->self()->integrate_velocities(p_delta);
-		b=b->next();
+		b=n;  // in case it shuts itself down
 	}
 
 	/* SLEEP / WAKE UP ISLANDS */

servers/physics_2d_server.cpp

@@ -122,7 +122,7 @@ Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const V
 	if (!res)
 		return Variant();
 
-	Dictionary d;
+	Dictionary d(true);
 	d["position"]=inters.position;
 	d["normal"]=inters.normal;
 	d["collider_id"]=inters.collider_id;
@@ -145,7 +145,7 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma
 
 	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);
+	int rc = intersect_shape(p_shape,p_xform,Vector2(),res,p_result_max,exclude,p_user_mask);
 
 	if (rc==0)
 		return Variant();
@@ -160,6 +160,34 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma
 }
 
 
+Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+
+
+	Set<RID> exclude;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+
+
+	MotionCastCollision mcc;
+
+	bool result = cast_motion(p_shape,p_xform,p_motion,mcc,exclude,p_user_mask);
+
+	if (!result)
+		return Variant();
+
+	Dictionary d(true);
+	d["point"]=mcc.point;
+	d["normal"]=mcc.normal;
+	d["rid"]=mcc.rid;
+	d["collider_id"]=mcc.collider_id;
+	d["collider"]=mcc.collider;
+	d["shape"]=mcc.shape;
+
+	return d;
+
+
+}
 
 
 
@@ -175,6 +203,7 @@ 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));
+	ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
 
 }
 
@@ -297,8 +326,8 @@ void Physics2DServer::_bind_methods() {
 	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_continuous_collision_detection_mode","body","mode"),&Physics2DServer::body_set_continuous_collision_detection_mode);
+	ObjectTypeDB::bind_method(_MD("body_get_continuous_collision_detection_mode","body"),&Physics2DServer::body_get_continuous_collision_detection_mode);
 
 
 	//ObjectTypeDB::bind_method(_MD("body_set_user_flags","flags""),&Physics2DServer::body_set_shape,DEFVAL(Matrix32));
@@ -307,8 +336,6 @@ void Physics2DServer::_bind_methods() {
 	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);
 
@@ -371,7 +398,7 @@ void Physics2DServer::_bind_methods() {
 	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
 
 	BIND_CONSTANT( BODY_MODE_STATIC );
-	BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE );
+	BIND_CONSTANT( BODY_MODE_KINEMATIC );
 	BIND_CONSTANT( BODY_MODE_RIGID );
 	BIND_CONSTANT( BODY_MODE_CHARACTER );
 
@@ -394,6 +421,10 @@ void Physics2DServer::_bind_methods() {
 	BIND_CONSTANT( DAMPED_STRING_STIFFNESS );
 	BIND_CONSTANT( DAMPED_STRING_DAMPING );
 
+	BIND_CONSTANT( CCD_MODE_DISABLED );
+	BIND_CONSTANT( CCD_MODE_CAST_RAY );
+	BIND_CONSTANT( CCD_MODE_CAST_SHAPE );
+
 //	BIND_CONSTANT( TYPE_BODY );
 //	BIND_CONSTANT( TYPE_AREA );
 

servers/physics_2d_server.h

@@ -90,6 +90,7 @@ class Physics2DDirectSpaceState : public 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);
+	Variant _cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
 
 
 protected:
@@ -118,7 +119,26 @@ public:
 
 	};
 
-	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;
+	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+
+
+	struct MotionCastCollision {
+
+		float travel; //0 to 1, if 0 then it's blocked
+		Vector2 point;
+		Vector2 normal;
+		RID rid;
+		ObjectID collider_id;
+		Object *collider;
+		int shape;
+
+	};
+
+	virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+
+
+
 
 	Physics2DDirectSpaceState();
 };
@@ -179,6 +199,8 @@ public:
 	virtual Variant shape_get_data(RID p_shape) const=0;
 	virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0;
 
+	//these work well, but should be used from the main thread only
+	virtual bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count)=0;
 
 	/* SPACE API */
 
@@ -265,10 +287,10 @@ public:
 
 	enum BodyMode {
 		BODY_MODE_STATIC,
-		BODY_MODE_STATIC_ACTIVE,
+		BODY_MODE_KINEMATIC,
 		BODY_MODE_RIGID,
-		//BODY_MODE_SOFT
 		BODY_MODE_CHARACTER
+		//BODY_MODE_SOFT ??
 	};
 
 	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0;
@@ -277,7 +299,7 @@ public:
 	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 BodyMode body_get_mode(RID p_body) 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;
@@ -296,8 +318,14 @@ public:
 	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;
+	enum CCDMode {
+		CCD_MODE_DISABLED,
+		CCD_MODE_CAST_RAY,
+		CCD_MODE_CAST_SHAPE,
+	};
+
+	virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode)=0;
+	virtual CCDMode body_get_continuous_collision_detection_mode(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;
@@ -313,8 +341,6 @@ public:
 	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 {
@@ -355,6 +381,8 @@ public:
 
 	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
 
+	virtual bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count)=0;
+
 	/* JOINT API */
 
 	enum JointType {
@@ -417,6 +445,7 @@ VARIANT_ENUM_CAST( Physics2DServer::AreaSpaceOverrideMode );
 VARIANT_ENUM_CAST( Physics2DServer::BodyMode );
 VARIANT_ENUM_CAST( Physics2DServer::BodyParameter );
 VARIANT_ENUM_CAST( Physics2DServer::BodyState );
+VARIANT_ENUM_CAST( Physics2DServer::CCDMode );
 VARIANT_ENUM_CAST( Physics2DServer::JointParam );
 VARIANT_ENUM_CAST( Physics2DServer::JointType );
 VARIANT_ENUM_CAST( Physics2DServer::DampedStringParam );

servers/physics_server.cpp

@@ -374,7 +374,7 @@ void PhysicsServer::_bind_methods() {
 	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
 
 	BIND_CONSTANT( BODY_MODE_STATIC );
-	BIND_CONSTANT( BODY_MODE_STATIC_ACTIVE );
+	BIND_CONSTANT( BODY_MODE_KINEMATIC );
 	BIND_CONSTANT( BODY_MODE_RIGID );
 	BIND_CONSTANT( BODY_MODE_CHARACTER );
 

servers/physics_server.h

@@ -268,7 +268,7 @@ public:
 
 	enum BodyMode {
 		BODY_MODE_STATIC,
-		BODY_MODE_STATIC_ACTIVE,
+		BODY_MODE_KINEMATIC,
 		BODY_MODE_RIGID,
 		//BODY_MODE_SOFT
 		BODY_MODE_CHARACTER

tools/editor/io_plugins/editor_texture_import_plugin.cpp

@@ -150,6 +150,11 @@ void EditorImportTextureOptions::_notification(int p_what) {
 	}
 }
 
+void EditorImportTextureOptions::show_2d_notice() {
+
+	notice_for_2d->show();
+}
+
 EditorImportTextureOptions::EditorImportTextureOptions() {
 
 
@@ -206,6 +211,14 @@ EditorImportTextureOptions::EditorImportTextureOptions() {
 
 
 	add_margin_child("Texture Options",flags,true);
+
+	notice_for_2d = memnew( Label );
+	notice_for_2d->set_text("NOTICE: You are not forced to import textures for 2D projects. Just copy your .jpg or .png files to your project, and change export options later. Atlases can be generated on export too.");
+	notice_for_2d->set_custom_minimum_size(Size2(0,50));
+	notice_for_2d->set_autowrap(true);
+	add_child(notice_for_2d);
+	notice_for_2d->hide();
+
 }
 
 ///////////////////////////////////////////////////////////
@@ -530,6 +543,7 @@ EditorTextureImportDialog::EditorTextureImportDialog(EditorTextureImportPlugin*
 		texture_options->set_flags(EditorTextureImportPlugin::IMAGE_FLAG_FIX_BORDER_ALPHA|EditorTextureImportPlugin::IMAGE_FLAG_NO_MIPMAPS|EditorTextureImportPlugin::IMAGE_FLAG_FILTER);
 		texture_options->set_quality(0.7);
 		texture_options->set_format(EditorTextureImportPlugin::IMAGE_FORMAT_COMPRESS_DISK_LOSSY);
+		texture_options->show_2d_notice();
 		set_title("Import Textures for Atlas (2D)");
 
 	} else if (p_2d) {
@@ -537,6 +551,7 @@ EditorTextureImportDialog::EditorTextureImportDialog(EditorTextureImportPlugin*
 		texture_options->set_flags(EditorTextureImportPlugin::IMAGE_FLAG_NO_MIPMAPS|EditorTextureImportPlugin::IMAGE_FLAG_FIX_BORDER_ALPHA|EditorTextureImportPlugin::IMAGE_FLAG_FILTER);
 		texture_options->set_quality(0.7);
 		texture_options->set_format(EditorTextureImportPlugin::IMAGE_FORMAT_COMPRESS_DISK_LOSSY);
+		texture_options->show_2d_notice();
 		set_title("Import Textures for 2D");
 	} else {
 

tools/editor/io_plugins/editor_texture_import_plugin.h

@@ -116,6 +116,7 @@ class EditorImportTextureOptions : public VBoxContainer {
 	HSlider *quality;
 	Tree *flags;
 	Vector<TreeItem*> items;
+	Label *notice_for_2d;
 
 	bool updating;
 
@@ -140,6 +141,8 @@ public:
 	void set_quality(float p_quality);
 	float get_quality() const;
 
+	void show_2d_notice();
+
 	EditorImportTextureOptions();
 
 

tools/editor/plugins/tile_set_editor_plugin.cpp

@@ -73,7 +73,7 @@ void TileSetEditor::_import_scene(Node *scene, Ref<TileSet> p_library, bool p_me
 
 	       p_library->tile_set_texture(id,texture);
 	       if (mi->is_centered()) {
-		       p_library->tile_set_offset(id,texture->get_size()/2);
+		       p_library->tile_set_texture_offset(id,texture->get_size()/2);
 	       }
 	       if (mi->is_region()) {
 		       p_library->tile_set_region(id,mi->get_region_rect());