1 files changed, 504 insertions, 0 deletions

diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 0644147762..9a1b977bda 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -556,7 +556,511 @@ Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {
 
 
 
+bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) {
 
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+	Rect2 body_aabb;
+
+	for(int i=0;i<p_body->get_shape_count();i++) {
+
+		if (i==0)
+			body_aabb=p_body->get_shape_aabb(i);
+		else
+			body_aabb=body_aabb.merge(p_body->get_shape_aabb(i));
+	}
+
+	body_aabb=body_aabb.grow(p_margin);
+
+	{
+		//add motion
+
+		Rect2 motion_aabb=body_aabb;
+		motion_aabb.pos+=p_motion;
+		body_aabb=body_aabb.merge(motion_aabb);
+	}
+
+
+	int amount = broadphase->cull_aabb(body_aabb,intersection_query_results,INTERSECTION_QUERY_MAX,intersection_query_subindex_results);
+
+	for(int i=0;i<amount;i++) {
+
+		bool keep=true;
+
+		if (intersection_query_results[i]==p_body)
+			keep=false;
+		else if (intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
+			keep=false;
+		else if ((static_cast<Body2DSW*>(intersection_query_results[i])->get_layer_mask()&p_body->get_layer_mask())==0)
+			keep=false;
+		else if (static_cast<Body2DSW*>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self()))
+			keep=false;
+		else if (static_cast<Body2DSW*>(intersection_query_results[i])->is_shape_set_as_trigger(intersection_query_subindex_results[i]))
+			keep=false;
+
+		if (!keep) {
+
+			if (i<amount-1) {
+				SWAP(intersection_query_results[i],intersection_query_results[amount-1]);
+				SWAP(intersection_query_subindex_results[i],intersection_query_subindex_results[amount-1]);
+
+			}
+
+			amount--;
+			i--;
+
+		}
+	}
+
+	Matrix32 body_transform = p_body->get_transform();
+
+	{
+		//STEP 1, FREE BODY IF STUCK
+
+		const int max_results = 32;
+		int recover_attempts=4;
+		Vector2 sr[max_results*2];
+
+		do {
+
+			Physics2DServerSW::CollCbkData cbk;
+			cbk.max=max_results;
+			cbk.amount=0;
+			cbk.ptr=sr;
+
+
+			CollisionSolver2DSW::CallbackResult cbkres=NULL;
+
+			Physics2DServerSW::CollCbkData *cbkptr=NULL;
+			cbkptr=&cbk;
+			cbkres=Physics2DServerSW::_shape_col_cbk;
+
+			bool collided=false;
+
+
+			for(int j=0;j<p_body->get_shape_count();j++) {
+				if (p_body->is_shape_set_as_trigger(j))
+					continue;
+
+				Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
+				Shape2DSW *body_shape = p_body->get_shape(j);
+				for(int i=0;i<amount;i++) {
+
+					const CollisionObject2DSW *col_obj=intersection_query_results[i];
+					int shape_idx=intersection_query_subindex_results[i];
+
+					if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+
+						const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+						cbk.valid_dir=body->get_one_way_collision_direction();
+						cbk.valid_depth=body->get_one_way_collision_max_depth();
+					} else {
+						cbk.valid_dir=Vector2();
+						cbk.valid_depth=0;
+					}
+
+					if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
+						collided=cbk.amount>0;
+					}
+				}
+			}
+
+
+			if (!collided)
+				break;
+
+			Vector2 recover_motion;
+
+			for(int i=0;i<cbk.amount;i++) {
+
+				Vector2 a = sr[i*2+0];
+				Vector2 b = sr[i*2+1];
+
+			//	float d = a.distance_to(b);
+
+				//if (d<margin)
+				///	continue;
+				recover_motion+=(b-a)*0.4;
+			}
+
+			if (recover_motion==Vector2()) {
+				collided=false;
+				break;
+			}
+
+			body_transform.elements[2]+=recover_motion;
+
+			recover_attempts--;
+
+		} while (recover_attempts);
+	}
+
+
+
+	float safe = 1.0;
+	float unsafe = 1.0;
+	int best_shape=-1;
+
+	{
+		// STEP 2 ATTEMPT MOTION
+
+
+
+		for(int j=0;j<p_body->get_shape_count();j++) {
+
+			if (p_body->is_shape_set_as_trigger(j))
+				continue;
+
+			Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
+			Shape2DSW *body_shape = p_body->get_shape(j);
+
+			bool stuck=false;
+
+			float best_safe=1;
+			float best_unsafe=1;
+
+			for(int i=0;i<amount;i++) {
+
+				const CollisionObject2DSW *col_obj=intersection_query_results[i];
+				int shape_idx=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(body_shape,body_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
+					continue;
+				}
+
+
+				//test initial overlap
+				if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
+
+					if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+						//if one way collision direction ignore initial overlap
+						const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+						if (body->get_one_way_collision_direction()!=Vector2()) {
+							continue;
+						}
+					}
+
+					stuck=true;
+					break;
+				}
+
+
+				//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(body_shape,body_shape_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,0);
+
+					if (collided) {
+
+						hi=ofs;
+					} else {
+
+						low=ofs;
+					}
+				}
+
+				if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+
+					const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+					if (body->get_one_way_collision_direction()!=Vector2()) {
+
+						Vector2 cd[2];
+						Physics2DServerSW::CollCbkData cbk;
+						cbk.max=1;
+						cbk.amount=0;
+						cbk.ptr=cd;
+						cbk.valid_dir=body->get_one_way_collision_direction();
+						cbk.valid_depth=body->get_one_way_collision_max_depth();
+
+						Vector2 sep=mnormal; //important optimization for this to work fast enough
+						bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*(hi+contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,0);
+						if (!collided || cbk.amount==0) {
+							continue;
+						}
+
+					}
+				}
+
+
+				if (low<best_safe) {
+					best_safe=low;
+					best_unsafe=hi;
+				}
+			}
+
+			if (stuck) {
+
+				safe=0;
+				unsafe=0;
+				best_shape=j; //sadly it's the best
+				break;
+			}
+			if (best_safe==1.0) {
+				continue;
+			}
+			if (best_safe < safe) {
+
+				safe=best_safe;
+				unsafe=best_unsafe;
+				best_shape=j;
+			}
+		}
+	}
+
+	bool collided=false;
+	if (safe>=1) {
+		//not collided
+		collided=false;
+		if (r_result) {
+
+			r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
+			r_result->remainder=Vector2();
+		}
+
+	} else {
+
+		//it collided, let's get the rest info in unsafe advance
+		Matrix32 ugt = body_transform;
+		ugt.elements[2]+=p_motion*unsafe;
+
+		_RestCallbackData2D rcd;
+		rcd.best_len=0;
+		rcd.best_object=NULL;
+		rcd.best_shape=0;
+
+		Matrix32 body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
+		Shape2DSW *body_shape = p_body->get_shape(best_shape);
+
+
+		for(int i=0;i<amount;i++) {
+
+
+			const CollisionObject2DSW *col_obj=intersection_query_results[i];
+			int shape_idx=intersection_query_subindex_results[i];
+
+			if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+
+				const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+				rcd.valid_dir=body->get_one_way_collision_direction();
+				rcd.valid_depth=body->get_one_way_collision_max_depth();
+			} else {
+				rcd.valid_dir=Vector2();
+				rcd.valid_depth=0;
+			}
+
+
+			rcd.object=col_obj;
+			rcd.shape=shape_idx;
+			bool sc = CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin);
+			if (!sc)
+				continue;
+
+		}
+
+		if (rcd.best_len!=0) {
+
+			if (r_result) {
+				r_result->collider=rcd.best_object->get_self();
+				r_result->collider_id=rcd.best_object->get_instance_id();
+				r_result->collider_shape=rcd.best_shape;
+				r_result->collision_normal=rcd.best_normal;
+				r_result->collision_point=rcd.best_contact;
+				r_result->collider_metadata=rcd.best_object->get_shape_metadata(rcd.best_shape);
+
+				const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
+				Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin();
+				r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
+
+				r_result->motion=safe*p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
+				r_result->remainder=p_motion - safe * p_motion;
+			}
+
+			collided=true;
+		} else {
+			if (r_result) {
+
+				r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
+				r_result->remainder=Vector2();
+			}
+
+			collided=false;
+
+		}
+	}
+
+	return collided;
+
+
+#if 0
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+
+	colliding=false;
+	ERR_FAIL_COND_V(!is_inside_tree(),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;
+	Vector2 sr[max_shapes*2];
+	int res_shapes;
+
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+
+	//recover first
+	int recover_attempts=4;
+
+	bool collided=false;
+	uint32_t mask=0;
+	if (collide_static)
+		mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
+	if (collide_kinematic)
+		mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
+	if (collide_rigid)
+		mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
+	if (collide_character)
+		mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
+
+//	print_line("motion: "+p_motion+" margin: "+rtos(margin));
+
+	//print_line("margin: "+rtos(margin));
+	do {
+
+		//motion recover
+		for(int i=0;i<get_shape_count();i++) {
+
+			if (is_shape_set_as_trigger(i))
+				continue;
+			if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask))
+				collided=true;
+
+		}
+
+		if (!collided)
+			break;
+
+		Vector2 recover_motion;
+
+		for(int i=0;i<res_shapes;i++) {
+
+			Vector2 a = sr[i*2+0];
+			Vector2 b = sr[i*2+1];
+
+			float d = a.distance_to(b);
+
+			//if (d<margin)
+			///	continue;
+			recover_motion+=(b-a)*0.4;
+		}
+
+		if (recover_motion==Vector2()) {
+			collided=false;
+			break;
+		}
+
+		Matrix32 gt = get_global_transform();
+		gt.elements[2]+=recover_motion;
+		set_global_transform(gt);
+
+		recover_attempts--;
+
+	} while (recover_attempts);
+
+
+	//move second
+	float safe = 1.0;
+	float unsafe = 1.0;
+	int best_shape=-1;
+
+	for(int i=0;i<get_shape_count();i++) {
+
+		if (is_shape_set_as_trigger(i))
+			continue;
+
+		float lsafe,lunsafe;
+		bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,get_layer_mask(),mask);
+		//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
+		if (!valid) {
+
+			safe=0;
+			unsafe=0;
+			best_shape=i; //sadly it's the best
+			break;
+		}
+		if (lsafe==1.0) {
+			continue;
+		}
+		if (lsafe < safe) {
+
+			safe=lsafe;
+			unsafe=lunsafe;
+			best_shape=i;
+		}
+	}
+
+
+	//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
+
+	if (safe>=1) {
+		//not collided
+		colliding=false;
+	} else {
+
+		//it collided, let's get the rest info in unsafe advance
+		Matrix32 ugt = get_global_transform();
+		ugt.elements[2]+=p_motion*unsafe;
+		Physics2DDirectSpaceState::ShapeRestInfo rest_info;
+		bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,get_layer_mask(),mask);
+		if (!c2) {
+			//should not happen, but floating point precision is so weird..
+
+			colliding=false;
+		} else {
+
+
+			//print_line("Travel: "+rtos(travel));
+			colliding=true;
+			collision=rest_info.point;
+			normal=rest_info.normal;
+			collider=rest_info.collider_id;
+			collider_vel=rest_info.linear_velocity;
+			collider_shape=rest_info.shape;
+			collider_metadata=rest_info.metadata;
+		}
+
+	}
+
+	Vector2 motion=p_motion*safe;
+	Matrix32 gt = get_global_transform();
+	gt.elements[2]+=motion;
+	set_global_transform(gt);
+
+	return p_motion-motion;
+
+#endif
+	return false;
+}