Add static body conveyer belt (#25)

- Fixes #11
- Fixes #2

README.md

@@ -34,11 +34,9 @@ A 2d [rapier](https://github.com/dimforge/rapier) physics server for [Godot Engi
 
 # Limitations
 
-- Static Body Constant Speed for CharacterBody2D and RigidBody2D missing.
-- Spring Joint missing.
-- Shape skewing missing.
-- Shape Cast Margin is missing.
-- Warmstart missing (affects boxes stackability)
+- SeparationRay2D missing.
+- DampedSpringJoint2D missing.
+- Shape skew missing.
 
 # Supported Platforms
 

src/bodies/rapier_body_2d.cpp

@@ -178,6 +178,16 @@ void RapierBody2D::on_update_active() {
 	if (!direct_state_query_list.in_list()) {
 		get_space()->body_add_to_state_query_list(&direct_state_query_list);
 	}
+	if (mode >= PhysicsServer2D::BODY_MODE_RIGID) {
+		if (to_add_angular_velocity != 0.0) {
+			set_angular_velocity(to_add_angular_velocity);
+			to_add_angular_velocity = 0.0;
+		}
+		if (to_add_linear_velocity != Vector2()) {
+			set_linear_velocity(to_add_linear_velocity);
+			to_add_linear_velocity = Vector2();
+		}
+	}
 }
 
 void RapierBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) {
@@ -535,19 +545,19 @@ void RapierBody2D::set_space(RapierSpace2D *p_space) {
 				}
 
 				_mass_properties_changed();
-				if (!linear_velocity.is_zero_approx()) {
+				if (linear_velocity != Vector2()) {
 					set_linear_velocity(linear_velocity);
 				}
 				if (angular_velocity != 0.0) {
 					set_angular_velocity(angular_velocity);
 				}
-				if (!constant_force.is_zero_approx()) {
+				if (constant_force != Vector2()) {
 					set_constant_force(constant_force);
 				}
 				if (constant_torque != 0.0) {
 					set_constant_torque(constant_torque);
 				}
-				if (!impulse.is_zero_approx()) {
+				if (impulse != Vector2()) {
 					apply_impulse(impulse);
 				}
 				if (torque != 0.0) {

src/bodies/rapier_body_2d.h

@@ -54,6 +54,10 @@ class RapierBody2D : public RapierCollisionObject2D {
 	real_t angular_velocity = 0.0;
 	real_t constant_torque = 0.0;
 
+	// Add this after body intersected with static body
+	real_t to_add_angular_velocity = 0.0;
+	Vector2 to_add_linear_velocity;
+
 	bool sleep = false;
 
 	void _mass_properties_changed();
@@ -121,6 +125,8 @@ class RapierBody2D : public RapierCollisionObject2D {
 	virtual void _init_collider(rapier2d::Handle collider_handle) const override;
 
 public:
+	_FORCE_INLINE_ void to_add_static_constant_linear_velocity(const Vector2 &linear_velocity) { to_add_linear_velocity = linear_velocity; }
+	_FORCE_INLINE_ void to_add_static_constant_angular_velocity(const real_t &angular_velocity) { to_add_angular_velocity = angular_velocity; }
 	void set_linear_velocity(const Vector2 &linear_velocity);
 	Vector2 get_linear_velocity() const;
 	Vector2 get_static_linear_velocity() const;

src/servers/rapier_body_utils_2d.cpp

@@ -333,8 +333,11 @@ bool RapierBodyUtils2D::body_motion_collide(const RapierSpace2D &p_space, Rapier
 			}
 		}
 	}
-
 	if (best_collision_body) {
+		// conveyer belt
+		if (best_collision_body->get_static_linear_velocity() != Vector2()) {
+			p_result->travel += best_collision_body->get_static_linear_velocity() * p_space.get_last_step();
+		}
 		if (p_result) {
 			p_result->collider = best_collision_body->get_rid();
 			p_result->collider_id = best_collision_body->get_instance_id();

src/spaces/rapier_space_2d.cpp

@@ -145,19 +145,18 @@ rapier2d::OneWayDirection RapierSpace2D::collision_modify_contacts_callback(rapi
 		if (collision_object_1->get_type() == RapierCollisionObject2D::TYPE_BODY && collision_object_2->get_type() == RapierCollisionObject2D::TYPE_BODY) {
 			RapierBody2D *body1 = static_cast<RapierBody2D *>(collision_object_1);
 			RapierBody2D *body2 = static_cast<RapierBody2D *>(collision_object_2);
-			if (!body1->get_static_linear_velocity().is_zero_approx()) {
-				//body2->apply_central_impulse(body1->get_static_linear_velocity());
-				//body2->apply_central_impulse(Vector2(100,0));
+			if (body1->get_static_linear_velocity() != Vector2()) {
+				body2->to_add_static_constant_linear_velocity(body1->get_static_linear_velocity());
 			}
-			if (!body2->get_static_linear_velocity().is_zero_approx()) {
-				//body1->apply_central_impulse(body2->get_static_linear_velocity());
+			if (body2->get_static_linear_velocity() != Vector2()) {
+				body1->to_add_static_constant_linear_velocity(body2->get_static_linear_velocity());
+			}
+			if (body1->get_static_angular_velocity() != 0.0) {
+				body2->to_add_static_constant_angular_velocity(body1->get_static_angular_velocity());
+			}
+			if (body2->get_static_angular_velocity() != 0.0) {
+				body1->to_add_static_constant_angular_velocity(body2->get_static_angular_velocity());
 			}
-			//body2->set_angular_velocity(body2->get_angular_velocity() + body1->get_static_angular_velocity());
-			//if (body2->is_static()) {
-			// TODO figure out when to set this.
-			//body1->set_linear_velocity(body1->get_linear_velocity() + body2->get_static_linear_velocity());
-			//body1->set_angular_velocity(body1->get_angular_velocity() + body2->get_static_angular_velocity());
-			//}
 		}
 	}
 
@@ -671,6 +670,9 @@ RapierSpace2D::~RapierSpace2D() {
 }
 
 bool RapierSpace2D::test_body_motion(RapierBody2D *p_body, const Transform2D &p_from, const Vector2 &p_motion, double p_margin, bool p_collide_separation_ray, bool p_recovery_as_collision, PhysicsServer2DExtensionMotionResult *r_result) const {
+	if (r_result) {
+		r_result->travel = Vector2();
+	}
 	Transform2D body_transform = p_from; // Because body_transform needs to be modified during recovery
 	// Step 1: recover motion.
 	// Expand the body colliders by the margin (grow) and check if now it collides with a collider,
@@ -703,12 +705,12 @@ bool RapierSpace2D::test_body_motion(RapierBody2D *p_body, const Transform2D &p_
 
 	if (r_result) {
 		if (collided) {
-			r_result->travel = recover_motion + p_motion * best_safe;
+			r_result->travel += recover_motion + p_motion * best_safe;
 			r_result->remainder = p_motion - p_motion * best_safe;
 			r_result->collision_safe_fraction = best_safe;
 			r_result->collision_unsafe_fraction = best_unsafe;
 		} else {
-			r_result->travel = recover_motion + p_motion;
+			r_result->travel += recover_motion + p_motion;
 			r_result->remainder = Vector2();
 			r_result->collision_depth = 0.0f;
 			r_result->collision_safe_fraction = 1.0f;