Fix crash (#91)

* fix scaling issue

* fix crash

* fix crash for raycast also

.gitignore

@@ -1,5 +1,6 @@
 # Binaries
 *.os
+*.os.tmp
 *.o
 *.dblite
 

src/bodies/box2d_body_2d.cpp

@@ -166,6 +166,7 @@ void Box2DBody2D::on_marked_active() {
 }
 
 void Box2DBody2D::on_update_active() {
+	ERR_FAIL_COND(!get_space());
 	if (!marked_active) {
 		set_active(false);
 		return;
@@ -481,6 +482,7 @@ void Box2DBody2D::_init_material(box2d::Material &mat) const {
 }
 
 void Box2DBody2D::_init_collider(box2d::FixtureHandle collider_handle) const {
+	ERR_FAIL_COND(!get_space());
 	b2World *space_handle = get_space()->get_handle();
 	ERR_FAIL_COND(!box2d::is_handle_valid(space_handle));
 
@@ -546,21 +548,35 @@ void Box2DBody2D::set_space(Box2DSpace2D *p_space) {
 				_mass_properties_changed();
 				if (linear_velocity != Vector2()) {
 					set_linear_velocity(linear_velocity);
+					linear_velocity = Vector2();
 				}
 				if (angular_velocity != 0.0) {
 					set_angular_velocity(angular_velocity);
+					angular_velocity = 0.0;
 				}
 				if (constant_force != Vector2()) {
 					set_constant_force(constant_force);
+					constant_force = Vector2();
 				}
 				if (constant_torque != 0.0) {
 					set_constant_torque(constant_torque);
+					constant_torque = 0.0;
 				}
 				if (impulse != Vector2()) {
 					apply_impulse(impulse);
+					impulse = Vector2();
 				}
 				if (torque != 0.0) {
 					apply_torque_impulse(torque);
+					torque = 0.0;
+				}
+				if (force != Vector2()) {
+					apply_force(force);
+					force = Vector2();
+				}
+				if (torque_force != 0.0) {
+					apply_torque(torque_force);
+					torque_force = 0.0;
 				}
 				b2World *space_handle = get_space()->get_handle();
 				box2d::Material mat;
@@ -688,9 +704,7 @@ void Box2DBody2D::apply_torque_impulse(real_t p_torque) {
 }
 
 void Box2DBody2D::apply_central_force(const Vector2 &p_force) {
-	// Note: using last delta assuming constant physics time
-	real_t last_delta = get_space()->get_last_step();
-	impulse += p_force * last_delta;
+	force += p_force;
 	if (!get_space()) {
 		return;
 	}
@@ -702,17 +716,17 @@ void Box2DBody2D::apply_central_force(const Vector2 &p_force) {
 
 	b2World *space_handle = get_space()->get_handle();
 	ERR_FAIL_COND(!box2d::is_handle_valid(space_handle));
+	// Note: using last delta assuming constant physics time
+	real_t last_delta = get_space()->get_last_step();
 
 	ERR_FAIL_COND(!box2d::is_handle_valid(body_handle));
 	b2Vec2 force = { p_force.x * last_delta, p_force.y * last_delta };
 	box2d::body_apply_impulse(space_handle, body_handle, force);
 }
 
 void Box2DBody2D::apply_force(const Vector2 &p_force, const Vector2 &p_position) {
-	// Note: using last delta assuming constant physics time
-	real_t last_delta = get_space()->get_last_step();
-	impulse += p_force * last_delta;
-	torque += p_position.cross(p_force) * last_delta;
+	force += p_force;
+	torque_force += p_position.cross(p_force);
 	if (!get_space()) {
 		return;
 	}
@@ -725,16 +739,16 @@ void Box2DBody2D::apply_force(const Vector2 &p_force, const Vector2 &p_position)
 	b2World *space_handle = get_space()->get_handle();
 	ERR_FAIL_COND(!box2d::is_handle_valid(space_handle));
 	ERR_FAIL_COND(!box2d::is_handle_valid(body_handle));
+	// Note: using last delta assuming constant physics time
+	real_t last_delta = get_space()->get_last_step();
 	b2Vec2 force = { p_force.x * last_delta, p_force.y * last_delta };
 	Vector2 point_centered = get_transform().get_origin() + p_position + get_center_of_mass();
 	b2Vec2 pos = { point_centered.x, point_centered.y };
 	box2d::body_apply_impulse_at_point(space_handle, body_handle, force, pos);
 }
 
 void Box2DBody2D::apply_torque(real_t p_torque) {
-	// Note: using last delta assuming constant physics time
-	real_t last_delta = get_space()->get_last_step();
-	torque += p_torque * last_delta;
+	torque_force += p_torque;
 	if (!get_space()) {
 		return;
 	}
@@ -748,6 +762,8 @@ void Box2DBody2D::apply_torque(real_t p_torque) {
 	ERR_FAIL_COND(!box2d::is_handle_valid(space_handle));
 
 	ERR_FAIL_COND(!box2d::is_handle_valid(body_handle));
+	// Note: using last delta assuming constant physics time
+	real_t last_delta = get_space()->get_last_step();
 	box2d::body_apply_torque_impulse(space_handle, body_handle, p_torque * last_delta);
 }
 
@@ -896,15 +912,12 @@ void Box2DBody2D::wakeup() {
 
 void Box2DBody2D::force_sleep() {
 	sleep = true;
-	if (!get_space()) {
+	if (!get_space() || !can_sleep) {
 		return;
 	}
 
-	ERR_FAIL_COND(!can_sleep);
-
 	b2World *space_handle = get_space()->get_handle();
 	ERR_FAIL_COND(!box2d::is_handle_valid(space_handle));
-
 	ERR_FAIL_COND(!box2d::is_handle_valid(body_handle));
 	box2d::body_force_sleep(space_handle, body_handle);
 }

src/bodies/box2d_body_2d.h

@@ -50,7 +50,9 @@ class Box2DBody2D : public Box2DCollisionObject2D {
 	Vector2 constant_force;
 	Vector2 linear_velocity;
 	Vector2 impulse;
+	Vector2 force;
 	real_t torque = 0.0;
+	real_t torque_force = 0.0;
 	real_t angular_velocity = 0.0;
 	real_t constant_torque = 0.0;
 

src/box2d-wrapper/box2d_wrapper.cpp

@@ -701,12 +701,13 @@ class RayCastQueryCallback : public b2RayCastCallback {
 			return -1;
 		}
 		if (!handle_excluded_callback(world, fixture, fixture->GetUserData(), handle_excluded_info)) {
-			hit_info_array[count++] = RayHitInfo{
+			hit_info_array[0] = RayHitInfo{
 				point,
 				normal,
 				fixture,
 				fixture->GetUserData()
 			};
+			count = 1;
 		}
 		return 1;
 	}
@@ -1178,7 +1179,7 @@ b2World *box2d::world_create(const WorldSettings *settings) {
 }
 
 void box2d::world_destroy(b2World *world_handle) {
-	memfree(world_handle);
+	memdelete(world_handle);
 }
 
 size_t box2d::world_get_active_objects_count(b2World *world_handle) {

src/spaces/box2d_direct_space_state_2d.cpp

@@ -15,7 +15,11 @@ int Box2DDirectSpaceState2D::_intersect_point(const Vector2 &position, uint64_t
 
 	uint32_t result_count = box2d::intersect_point(space->handle, box2d_pos, collide_with_bodies, collide_with_areas, hit_info_array, p_result_max, Box2DSpace2D::_is_handle_excluded_callback, &query_excluded_info);
 	ERR_FAIL_COND_V(result_count > (uint32_t)p_result_max, 0);
-
+	result_count = MIN(result_count, p_result_max);
+	if (!r_results) {
+		memfree(hit_info_array);
+		return result_count;
+	}
 	for (uint32_t i = 0; i < result_count; i++) {
 		box2d::PointHitInfo &hit_info = hit_info_array[i];
 		PhysicsServer2DExtensionShapeResult &result = r_results[i];
@@ -33,12 +37,14 @@ int Box2DDirectSpaceState2D::_intersect_point(const Vector2 &position, uint64_t
 			result.collider = Box2DSpace2D::_get_object_instance_hack(result.collider_id);
 		}
 	}
+	memfree(hit_info_array);
 
 	return result_count;
 }
 
 bool Box2DDirectSpaceState2D::_intersect_ray(const Vector2 &from, const Vector2 &to, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, bool hit_from_inside, PhysicsServer2DExtensionRayResult *r_result) {
 	ERR_FAIL_COND_V(space->locked, false);
+	ERR_FAIL_COND_V(!space, false);
 	ERR_FAIL_COND_V(!box2d::is_handle_valid(space->handle), false);
 
 	// Raycast Info
@@ -67,20 +73,22 @@ bool Box2DDirectSpaceState2D::_intersect_ray(const Vector2 &from, const Vector2
 			&query_excluded_info);
 
 	if (collide) {
-		r_result->position = Vector2(hit_info.position.x, hit_info.position.y);
-		r_result->normal = Vector2(hit_info.normal.x, hit_info.normal.y);
-
-		ERR_FAIL_COND_V(!box2d::is_user_data_valid(hit_info.user_data), false);
-		uint32_t shape_index = 0;
-		Box2DCollisionObject2D *collision_object_2d = Box2DCollisionObject2D::get_collider_user_data(hit_info.user_data, shape_index);
-		ERR_FAIL_NULL_V(collision_object_2d, false);
-
-		r_result->shape = shape_index;
-		r_result->collider_id = collision_object_2d->get_instance_id();
-		r_result->rid = collision_object_2d->get_rid();
-
-		if (r_result->collider_id.is_valid()) {
-			r_result->collider = Box2DSpace2D::_get_object_instance_hack(r_result->collider_id);
+		if (r_result != nullptr) {
+			r_result->position = Vector2(hit_info.position.x, hit_info.position.y);
+			r_result->normal = Vector2(hit_info.normal.x, hit_info.normal.y);
+
+			ERR_FAIL_COND_V(!box2d::is_user_data_valid(hit_info.user_data), false);
+			uint32_t shape_index = 0;
+			Box2DCollisionObject2D *collision_object_2d = Box2DCollisionObject2D::get_collider_user_data(hit_info.user_data, shape_index);
+			ERR_FAIL_NULL_V(collision_object_2d, false);
+
+			r_result->shape = shape_index;
+			r_result->collider_id = collision_object_2d->get_instance_id();
+			r_result->rid = collision_object_2d->get_rid();
+
+			if (r_result->collider_id.is_valid()) {
+				r_result->collider = Box2DSpace2D::_get_object_instance_hack(r_result->collider_id);
+			}
 		}
 
 		return true;
@@ -101,8 +109,12 @@ bool Box2DDirectSpaceState2D::_cast_motion(const RID &shape_rid, const Transform
 	box2d::QueryExcludedInfo query_excluded_info = box2d::default_query_excluded_info();
 	query_excluded_info.query_collision_layer_mask = collision_mask;
 	real_t hit = box2d::shape_casting(space->handle, box2d_motion, shape_info, collide_with_bodies, collide_with_areas, Box2DSpace2D::_is_handle_excluded_callback, &query_excluded_info, margin).toi;
-	*p_closest_safe = hit;
-	*p_closest_unsafe = hit;
+	if (p_closest_safe != nullptr) {
+		*p_closest_safe = hit;
+	}
+	if (p_closest_unsafe != nullptr) {
+		*p_closest_unsafe = hit;
+	}
 	return true;
 }
 
@@ -130,13 +142,14 @@ bool Box2DDirectSpaceState2D::_collide_shape(const RID &shape_rid, const Transfo
 		}
 		(*result_count)++;
 		query_excluded_info.query_exclude[query_excluded_info.query_exclude_size++] = result.collider;
-
-		results_out[array_idx++] = Vector2(result.witness1.x, result.witness1.y);
-		results_out[array_idx++] = Vector2(result.witness2.x, result.witness2.y);
+		if (results_out != nullptr) {
+			results_out[array_idx++] = Vector2(result.witness1.x, result.witness1.y);
+			results_out[array_idx++] = Vector2(result.witness2.x, result.witness2.y);
+		}
 
 		cpt++;
 	} while (cpt < max_results);
-
+	memfree(query_excluded_info.query_exclude);
 	return array_idx > 0;
 }
 
@@ -181,6 +194,7 @@ int Box2DDirectSpaceState2D::_intersect_shape(const RID &shape_rid, const Transf
 
 	} while (cpt < p_result_max);
 
+	memfree(query_excluded_info.query_exclude);
 	return cpt;
 }