flushing transform from physics state to rigid body immeditely after …

…physics update

main/main.cpp

@@ -2092,6 +2092,7 @@ bool Main::iteration() {
 		message_queue->flush();
 
 		PhysicsServer::get_singleton()->step(frame_slice);
+		PhysicsServer::get_singleton()->flush_transforms();
 
 		Physics2DServer::get_singleton()->end_sync();
 		Physics2DServer::get_singleton()->step(frame_slice);

modules/bullet/bullet_physics_server.h

@@ -40,6 +40,8 @@
 #include "soft_body_bullet.h"
 #include "space_bullet.h"
 
+#include <functional>
+
 /**
 	@author AndreaCatania
 */
@@ -246,6 +248,7 @@ class BulletPhysicsServer : public PhysicsServer {
 	virtual void body_set_omit_force_integration(RID p_body, bool p_omit);
 	virtual bool body_is_omitting_force_integration(RID p_body) const;
 
+	virtual void body_set_flush_transform_callback(RID p_body, Object *p_receiver, std::function<void(PhysicsDirectBodyState*)> callback) {}
 	virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant());
 
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable);
@@ -399,6 +402,8 @@ class BulletPhysicsServer : public PhysicsServer {
 	virtual void sync();
 	virtual void flush_queries();
 	virtual void finish();
+	virtual void flush_transforms() {};
+
 
 	virtual bool is_flushing_queries() const { return false; }
 

scene/3d/physics_body.cpp

@@ -38,6 +38,8 @@
 #include "core/rid.h"
 #include "scene/scene_string_names.h"
 
+#include <functional>
+
 #ifdef TOOLS_ENABLED
 #include "editor/plugins/spatial_editor_plugin.h"
 #endif
@@ -450,9 +452,9 @@ void RigidBody::_direct_state_changed(Object *p_state) {
 #endif
 
 	set_ignore_transform_notification(true);
-	set_global_transform(state->get_transform());
-	linear_velocity = state->get_linear_velocity();
-	angular_velocity = state->get_angular_velocity();
+//	set_global_transform(state->get_transform());
+//	linear_velocity = state->get_linear_velocity();
+//	angular_velocity = state->get_angular_velocity();
 	inverse_inertia_tensor = state->get_inverse_inertia_tensor();
 	if (sleeping != state->is_sleeping()) {
 		sleeping = state->is_sleeping();
@@ -550,6 +552,16 @@ void RigidBody::_direct_state_changed(Object *p_state) {
 	state = NULL;
 }
 
+void RigidBody::flush_transform(PhysicsDirectBodyState *p_state)
+{
+	set_ignore_transform_notification(true);
+	set_global_transform(p_state->get_transform());
+	linear_velocity = p_state->get_linear_velocity();
+	angular_velocity = p_state->get_angular_velocity();	
+	set_ignore_transform_notification(false);
+	//OS::get_singleton()->print("_flush_transform %f %f %f", state->get_transform().origin.x, state->get_transform().origin.y, state->get_transform().origin.z);
+}
+
 void RigidBody::_notification(int p_what) {
 
 #ifdef TOOLS_ENABLED
@@ -1074,6 +1086,10 @@ RigidBody::RigidBody() :
 	can_sleep = true;
 
 	PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), this, "_direct_state_changed");
+
+    // bind to a pointer to member function
+    auto flush_callback = [this](PhysicsDirectBodyState* state) { this->flush_transform(state); };
+	PhysicsServer::get_singleton()->body_set_flush_transform_callback(get_rid(), this, flush_callback);
 }
 
 RigidBody::~RigidBody() {

scene/3d/physics_body.h

@@ -190,6 +190,7 @@ class RigidBody : public PhysicsBody {
 
 	void _body_inout(int p_status, ObjectID p_instance, int p_body_shape, int p_local_shape);
 	virtual void _direct_state_changed(Object *p_state);
+	virtual void flush_transform(PhysicsDirectBodyState *p_state);
 
 	void _notification(int p_what);
 	static void _bind_methods();

servers/physics/body_sw.cpp

@@ -30,6 +30,7 @@
 
 #include "body_sw.h"
 #include "area_sw.h"
+#include "core/os/os.h"
 #include "space_sw.h"
 
 void BodySW::_update_inertia() {
@@ -389,10 +390,15 @@ void BodySW::set_space(SpaceSW *p_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);
+
+		if (flush_transform_list.in_list())
+			get_space()->body_remove_from_flush_transform_list(&flush_transform_list);
 	}
 
 	_set_space(p_space);
@@ -581,6 +587,9 @@ void BodySW::integrate_velocities(real_t p_step) {
 	if (fi_callback)
 		get_space()->body_add_to_state_query_list(&direct_state_query_list);
 
+	if (flush_callback)
+		get_space()->body_add_to_flush_transform_list(&flush_transform_list);
+
 	//apply axis lock linear
 	for (int i = 0; i < 3; i++) {
 		if (is_axis_locked((PhysicsServer::BodyAxis)(1 << i))) {
@@ -720,6 +729,23 @@ void BodySW::call_queries() {
 	}
 }
 
+void BodySW::flush_transforms() {
+	if (flush_callback == nullptr)
+		return;
+
+	PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;
+	dbs->body = this;
+
+	//Variant v = dbs;
+
+	Object *obj = ObjectDB::get_instance(flush_callback->id);
+	if (!obj) {
+		set_flush_transform_callback(0, nullptr);
+	} else {
+		flush_callback->callback(dbs);
+	}
+}
+
 bool BodySW::sleep_test(real_t p_step) {
 
 	if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC)
@@ -758,6 +784,21 @@ void BodySW::set_force_integration_callback(ObjectID p_id, const StringName &p_m
 	}
 }
 
+void BodySW::set_flush_transform_callback(ObjectID p_id, std::function<void(PhysicsDirectBodyState *)> callback) {
+
+	if (flush_callback) {
+
+		memdelete(flush_callback);
+		flush_callback = NULL;
+	}
+
+	if (p_id != 0) {
+		flush_callback = memnew(FlushTransformCallback);
+		flush_callback->id = p_id;
+		flush_callback->callback = callback;
+	}
+}
+
 void BodySW::set_kinematic_margin(real_t p_margin) {
 	kinematic_safe_margin = p_margin;
 }
@@ -767,7 +808,8 @@ BodySW::BodySW() :
 		locked_axis(0),
 		active_list(this),
 		inertia_update_list(this),
-		direct_state_query_list(this) {
+		direct_state_query_list(this),
+		flush_transform_list(this) {
 
 	mode = PhysicsServer::BODY_MODE_RIGID;
 	active = true;
@@ -798,12 +840,16 @@ BodySW::BodySW() :
 	continuous_cd = false;
 	can_sleep = true;
 	fi_callback = NULL;
+	flush_callback = nullptr;
 }
 
 BodySW::~BodySW() {
 
 	if (fi_callback)
 		memdelete(fi_callback);
+
+	if (flush_callback)
+		memdelete(flush_callback);
 }
 
 PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton = NULL;

servers/physics/body_sw.h

@@ -82,6 +82,7 @@ class BodySW : public CollisionObjectSW {
 	SelfList<BodySW> active_list;
 	SelfList<BodySW> inertia_update_list;
 	SelfList<BodySW> direct_state_query_list;
+	SelfList<BodySW> flush_transform_list;
 
 	VSet<RID> exceptions;
 	bool omit_force_integration;
@@ -136,7 +137,14 @@ class BodySW : public CollisionObjectSW {
 		Variant udata;
 	};
 
+	struct FlushTransformCallback
+	{
+		ObjectID id;
+		std::function<void(PhysicsDirectBodyState*)> callback;
+	};
+
 	ForceIntegrationCallback *fi_callback;
+	FlushTransformCallback *flush_callback;
 
 	uint64_t island_step;
 	BodySW *island_next;
@@ -150,6 +158,7 @@ class BodySW : public CollisionObjectSW {
 
 public:
 	void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
+	void set_flush_transform_callback(ObjectID p_id, std::function<void(PhysicsDirectBodyState*)> callback);
 
 	void set_kinematic_margin(real_t p_margin);
 	_FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; }
@@ -331,6 +340,7 @@ class BodySW : public CollisionObjectSW {
 
 	//void simulate_motion(const Transform& p_xform,real_t p_step);
 	void call_queries();
+	void flush_transforms();
 	void wakeup_neighbours();
 
 	bool sleep_test(real_t p_step);

servers/physics/physics_server_sw.cpp

@@ -937,6 +937,13 @@ void PhysicsServerSW::body_set_force_integration_callback(RID p_body, Object *p_
 	body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(0), p_method, p_udata);
 }
 
+void PhysicsServerSW::body_set_flush_transform_callback(RID p_body, Object *p_receiver, std::function<void(PhysicsDirectBodyState*)> callback) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_flush_transform_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(0), callback);
+}
+
 void PhysicsServerSW::body_set_ray_pickable(RID p_body, bool p_enable) {
 
 	BodySW *body = body_owner.get(p_body);
@@ -1503,6 +1510,18 @@ void PhysicsServerSW::flush_queries() {
 #endif
 };
 
+void PhysicsServerSW::flush_transforms() 
+{
+	if (!active)
+		return;
+
+	for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) 
+	{
+		SpaceSW *space = (SpaceSW *)E->get();
+		space->flush_transforms();
+	}
+};
+
 void PhysicsServerSW::finish() {
 
 	memdelete(stepper);

servers/physics/physics_server_sw.h

@@ -37,6 +37,8 @@
 #include "space_sw.h"
 #include "step_sw.h"
 
+#include <functional>
+
 class PhysicsServerSW : public PhysicsServer {
 
 	GDCLASS(PhysicsServerSW, PhysicsServer);
@@ -232,6 +234,7 @@ class PhysicsServerSW : public PhysicsServer {
 	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 void body_set_flush_transform_callback(RID p_body, Object *p_receiver, std::function<void(PhysicsDirectBodyState*)> callback);
 
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable);
 	virtual bool body_is_ray_pickable(RID p_body) const;
@@ -368,6 +371,7 @@ class PhysicsServerSW : public PhysicsServer {
 	virtual void step(real_t p_step);
 	virtual void sync();
 	virtual void flush_queries();
+	virtual void flush_transforms();
 	virtual void finish();
 
 	virtual bool is_flushing_queries() const { return flushing_queries; }

servers/physics/space_sw.cpp

@@ -1096,11 +1096,22 @@ void SpaceSW::body_add_to_state_query_list(SelfList<BodySW> *p_body) {
 
 	state_query_list.add(p_body);
 }
+
 void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW> *p_body) {
 
 	state_query_list.remove(p_body);
 }
 
+void SpaceSW::body_add_to_flush_transform_list(SelfList<BodySW> *p_body) 
+{
+	flush_transform_list.add(p_body);
+}
+
+void SpaceSW::body_remove_from_flush_transform_list(SelfList<BodySW> *p_body) 
+{
+	flush_transform_list.remove(p_body);
+}
+
 void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW> *p_area) {
 
 	monitor_query_list.add(p_area);
@@ -1125,6 +1136,16 @@ const SelfList<AreaSW>::List &SpaceSW::get_moved_area_list() const {
 	return area_moved_list;
 }
 
+void SpaceSW::flush_transforms() 
+{
+	while (flush_transform_list.first()) 
+	{
+		BodySW *b = flush_transform_list.first()->self();
+		flush_transform_list.remove(flush_transform_list.first());
+		b->flush_transforms();
+	}
+}
+
 void SpaceSW::call_queries() {
 
 	while (state_query_list.first()) {

servers/physics/space_sw.h

@@ -82,6 +82,7 @@ class SpaceSW : public RID_Data {
 	SelfList<BodySW>::List active_list;
 	SelfList<BodySW>::List inertia_update_list;
 	SelfList<BodySW>::List state_query_list;
+	SelfList<BodySW>::List flush_transform_list;
 	SelfList<AreaSW>::List monitor_query_list;
 	SelfList<AreaSW>::List area_moved_list;
 
@@ -141,6 +142,8 @@ class SpaceSW : public RID_Data {
 
 	void body_add_to_state_query_list(SelfList<BodySW> *p_body);
 	void body_remove_from_state_query_list(SelfList<BodySW> *p_body);
+	void body_add_to_flush_transform_list(SelfList<BodySW> *p_body); 
+	void body_remove_from_flush_transform_list(SelfList<BodySW> *p_body);
 
 	void area_add_to_monitor_query_list(SelfList<AreaSW> *p_area);
 	void area_remove_from_monitor_query_list(SelfList<AreaSW> *p_area);
@@ -165,6 +168,7 @@ class SpaceSW : public RID_Data {
 
 	void update();
 	void setup();
+	void flush_transforms();
 	void call_queries();
 
 	bool is_locked() const;

servers/physics_2d/physics_2d_server_sw.h

@@ -243,6 +243,7 @@ class Physics2DServerSW : public Physics2DServer {
 	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
 	virtual int body_get_max_contacts_reported(RID p_body) const;
 
+	virtual void body_flush_transform_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) {}
 	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 Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count);
 
@@ -283,6 +284,7 @@ class Physics2DServerSW : public Physics2DServer {
 	virtual void flush_queries();
 	virtual void end_sync();
 	virtual void finish();
+	virtual void flush_transforms() {}
 
 	virtual bool is_flushing_queries() const { return flushing_queries; }