diff --git a/doc/classes/PhysicsServer2D.xml b/doc/classes/PhysicsServer2D.xml index 1df2fd0158c..6cb3678feca 100644 --- a/doc/classes/PhysicsServer2D.xml +++ b/doc/classes/PhysicsServer2D.xml @@ -489,6 +489,9 @@ Sets the function used to calculate physics for an object, if that object allows it (see [method body_set_omit_force_integration]). + The force integration function takes 2 arguments: + [code]state:[/code] [PhysicsDirectBodyState2D] used to retrieve and modify the body's state. + [code]userdata:[/code] Optional user data, if it was passed when calling [code]body_set_force_integration_callback[/code]. diff --git a/doc/classes/PhysicsServer3D.xml b/doc/classes/PhysicsServer3D.xml index 33eb0de698b..fa4ec55783d 100644 --- a/doc/classes/PhysicsServer3D.xml +++ b/doc/classes/PhysicsServer3D.xml @@ -478,6 +478,9 @@ Sets the function used to calculate physics for an object, if that object allows it (see [method body_set_omit_force_integration]). + The force integration function takes 2 arguments: + [code]state:[/code] [PhysicsDirectBodyState3D] used to retrieve and modify the body's state. + [code]userdata:[/code] Optional user data, if it was passed when calling [code]body_set_force_integration_callback[/code]. diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index a9d4877cbbe..aa82530690b 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -259,15 +259,17 @@ bool StaticBody2D::is_sync_to_physics_enabled() const { return sync_to_physics; } -void StaticBody2D::_direct_state_changed(Object *p_state) { +void StaticBody2D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state) { + StaticBody2D *body = (StaticBody2D *)p_instance; + body->_body_state_changed(p_state); +} + +void StaticBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) { if (!sync_to_physics) { return; } - PhysicsDirectBodyState2D *state = Object::cast_to(p_state); - ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState2D object as argument"); - - last_valid_transform = state->get_transform(); + last_valid_transform = p_state->get_transform(); set_notify_local_transform(false); set_global_transform(last_valid_transform); set_notify_local_transform(true); @@ -379,11 +381,11 @@ void StaticBody2D::_update_kinematic_motion() { #endif if (kinematic_motion && sync_to_physics) { - PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &StaticBody2D::_direct_state_changed)); + PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), this, _body_state_changed_callback); set_only_update_transform_changes(true); set_notify_local_transform(true); } else { - PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable()); + PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr); set_only_update_transform_changes(false); set_notify_local_transform(false); } @@ -511,26 +513,26 @@ struct _RigidBody2DInOut { int local_shape = 0; }; -void RigidBody2D::_direct_state_changed(Object *p_state) { -#ifdef DEBUG_ENABLED - state = Object::cast_to(p_state); - ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState2D object as argument"); -#else - state = (PhysicsDirectBodyState2D *)p_state; //trust it -#endif +void RigidBody2D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state) { + RigidBody2D *body = (RigidBody2D *)p_instance; + body->_body_state_changed(p_state); +} +void RigidBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) { set_block_transform_notify(true); // don't want notify (would feedback loop) if (mode != MODE_KINEMATIC) { - set_global_transform(state->get_transform()); + set_global_transform(p_state->get_transform()); } - linear_velocity = state->get_linear_velocity(); - angular_velocity = state->get_angular_velocity(); - if (sleeping != state->is_sleeping()) { - sleeping = state->is_sleeping(); + + linear_velocity = p_state->get_linear_velocity(); + angular_velocity = p_state->get_angular_velocity(); + + if (sleeping != p_state->is_sleeping()) { + sleeping = p_state->is_sleeping(); emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed); } - GDVIRTUAL_CALL(_integrate_forces, state); + GDVIRTUAL_CALL(_integrate_forces, p_state); set_block_transform_notify(false); // want it back @@ -546,20 +548,18 @@ void RigidBody2D::_direct_state_changed(Object *p_state) { } } - _RigidBody2DInOut *toadd = (_RigidBody2DInOut *)alloca(state->get_contact_count() * sizeof(_RigidBody2DInOut)); + _RigidBody2DInOut *toadd = (_RigidBody2DInOut *)alloca(p_state->get_contact_count() * sizeof(_RigidBody2DInOut)); int toadd_count = 0; //state->get_contact_count(); RigidBody2D_RemoveAction *toremove = (RigidBody2D_RemoveAction *)alloca(rc * sizeof(RigidBody2D_RemoveAction)); int toremove_count = 0; //put the ones to add - for (int i = 0; i < state->get_contact_count(); i++) { - RID rid = state->get_contact_collider(i); - ObjectID obj = state->get_contact_collider_id(i); - int local_shape = state->get_contact_local_shape(i); - int shape = state->get_contact_collider_shape(i); - - //bool found=false; + for (int i = 0; i < p_state->get_contact_count(); i++) { + RID rid = p_state->get_contact_collider(i); + ObjectID obj = p_state->get_contact_collider_id(i); + int local_shape = p_state->get_contact_local_shape(i); + int shape = p_state->get_contact_collider_shape(i); Map::Element *E = contact_monitor->body_map.find(obj); if (!E) { @@ -612,8 +612,6 @@ void RigidBody2D::_direct_state_changed(Object *p_state) { contact_monitor->locked = false; } - - state = nullptr; } void RigidBody2D::set_mode(Mode p_mode) { @@ -709,25 +707,15 @@ real_t RigidBody2D::get_angular_damp() const { } void RigidBody2D::set_axis_velocity(const Vector2 &p_axis) { - Vector2 v = state ? state->get_linear_velocity() : linear_velocity; Vector2 axis = p_axis.normalized(); - v -= axis * axis.dot(v); - v += p_axis; - if (state) { - set_linear_velocity(v); - } else { - PhysicsServer2D::get_singleton()->body_set_axis_velocity(get_rid(), p_axis); - linear_velocity = v; - } + linear_velocity -= axis * axis.dot(linear_velocity); + linear_velocity += p_axis; + PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity); } void RigidBody2D::set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; - if (state) { - state->set_linear_velocity(linear_velocity); - } else { - PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity); - } + PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity); } Vector2 RigidBody2D::get_linear_velocity() const { @@ -736,11 +724,7 @@ Vector2 RigidBody2D::get_linear_velocity() const { void RigidBody2D::set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; - if (state) { - state->set_angular_velocity(angular_velocity); - } else { - PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity); - } + PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity); } real_t RigidBody2D::get_angular_velocity() const { @@ -1019,7 +1003,7 @@ void RigidBody2D::_bind_methods() { RigidBody2D::RigidBody2D() : PhysicsBody2D(PhysicsServer2D::BODY_MODE_DYNAMIC) { - PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &RigidBody2D::_direct_state_changed)); + PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), this, _body_state_changed_callback); } RigidBody2D::~RigidBody2D() { @@ -1052,7 +1036,7 @@ bool CharacterBody2D::move_and_slide() { if ((on_floor || on_wall) && platform_rid.is_valid()) { bool excluded = (moving_platform_ignore_layers & platform_layer) != 0; if (!excluded) { - // This approach makes sure there is less delay between the actual body velocity and the one we saved. + //this approach makes sure there is less delay between the actual body velocity and the one we saved PhysicsDirectBodyState2D *bs = PhysicsServer2D::get_singleton()->body_get_direct_state(platform_rid); if (bs) { Transform2D gt = get_global_transform(); diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index a9993179536..ef550241349 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -73,7 +73,8 @@ class StaticBody2D : public PhysicsBody2D { Transform2D last_valid_transform; - void _direct_state_changed(Object *p_state); + static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state); + void _body_state_changed(PhysicsDirectBodyState2D *p_state); protected: void _notification(int p_what); @@ -124,7 +125,6 @@ public: private: bool can_sleep = true; - PhysicsDirectBodyState2D *state = nullptr; Mode mode = MODE_DYNAMIC; real_t mass = 1.0; @@ -183,7 +183,9 @@ private: void _body_exit_tree(ObjectID p_id); void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape); - void _direct_state_changed(Object *p_state); + + static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state); + void _body_state_changed(PhysicsDirectBodyState2D *p_state); protected: void _notification(int p_what); diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp index d34331e1d8f..e8fb884cf2a 100644 --- a/scene/3d/physics_body_3d.cpp +++ b/scene/3d/physics_body_3d.cpp @@ -283,18 +283,20 @@ bool StaticBody3D::is_sync_to_physics_enabled() const { return sync_to_physics; } -void StaticBody3D::_direct_state_changed(Object *p_state) { - PhysicsDirectBodyState3D *state = Object::cast_to(p_state); - ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument"); +void StaticBody3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) { + StaticBody3D *body = (StaticBody3D *)p_instance; + body->_body_state_changed(p_state); +} - linear_velocity = state->get_linear_velocity(); - angular_velocity = state->get_angular_velocity(); +void StaticBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { + linear_velocity = p_state->get_linear_velocity(); + angular_velocity = p_state->get_angular_velocity(); if (!sync_to_physics) { return; } - last_valid_transform = state->get_transform(); + last_valid_transform = p_state->get_transform(); set_notify_local_transform(false); set_global_transform(last_valid_transform); set_notify_local_transform(true); @@ -458,13 +460,13 @@ void StaticBody3D::_update_kinematic_motion() { bool needs_physics_process = false; if (kinematic_motion) { - PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &StaticBody3D::_direct_state_changed)); + PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &StaticBody3D::_body_state_changed_callback); if (!constant_angular_velocity.is_equal_approx(Vector3()) || !constant_linear_velocity.is_equal_approx(Vector3())) { needs_physics_process = true; } } else { - PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable()); + PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr); } set_physics_process_internal(needs_physics_process); @@ -582,25 +584,26 @@ struct _RigidBodyInOut { int local_shape = 0; }; -void RigidBody3D::_direct_state_changed(Object *p_state) { -#ifdef DEBUG_ENABLED - state = Object::cast_to(p_state); - ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument"); -#else - state = (PhysicsDirectBodyState3D *)p_state; //trust it -#endif +void RigidBody3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) { + RigidBody3D *body = (RigidBody3D *)p_instance; + body->_body_state_changed(p_state); +} +void RigidBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { set_ignore_transform_notification(true); - 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(); + set_global_transform(p_state->get_transform()); + + linear_velocity = p_state->get_linear_velocity(); + angular_velocity = p_state->get_angular_velocity(); + + inverse_inertia_tensor = p_state->get_inverse_inertia_tensor(); + + if (sleeping != p_state->is_sleeping()) { + sleeping = p_state->is_sleeping(); emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed); } - GDVIRTUAL_CALL(_integrate_forces, state); + GDVIRTUAL_CALL(_integrate_forces, p_state); set_ignore_transform_notification(false); _on_transform_changed(); @@ -617,18 +620,18 @@ void RigidBody3D::_direct_state_changed(Object *p_state) { } } - _RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(state->get_contact_count() * sizeof(_RigidBodyInOut)); + _RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(p_state->get_contact_count() * sizeof(_RigidBodyInOut)); int toadd_count = 0; //state->get_contact_count(); RigidBody3D_RemoveAction *toremove = (RigidBody3D_RemoveAction *)alloca(rc * sizeof(RigidBody3D_RemoveAction)); int toremove_count = 0; //put the ones to add - for (int i = 0; i < state->get_contact_count(); i++) { - RID rid = state->get_contact_collider(i); - ObjectID obj = state->get_contact_collider_id(i); - int local_shape = state->get_contact_local_shape(i); - int shape = state->get_contact_collider_shape(i); + for (int i = 0; i < p_state->get_contact_count(); i++) { + RID rid = p_state->get_contact_collider(i); + ObjectID obj = p_state->get_contact_collider_id(i); + int local_shape = p_state->get_contact_local_shape(i); + int shape = p_state->get_contact_collider_shape(i); //bool found=false; @@ -683,8 +686,6 @@ void RigidBody3D::_direct_state_changed(Object *p_state) { contact_monitor->locked = false; } - - state = nullptr; } void RigidBody3D::_notification(int p_what) { @@ -787,25 +788,15 @@ real_t RigidBody3D::get_angular_damp() const { } void RigidBody3D::set_axis_velocity(const Vector3 &p_axis) { - Vector3 v = state ? state->get_linear_velocity() : linear_velocity; Vector3 axis = p_axis.normalized(); - v -= axis * axis.dot(v); - v += p_axis; - if (state) { - set_linear_velocity(v); - } else { - PhysicsServer3D::get_singleton()->body_set_axis_velocity(get_rid(), p_axis); - linear_velocity = v; - } + linear_velocity -= axis * axis.dot(linear_velocity); + linear_velocity += p_axis; + PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity); } void RigidBody3D::set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; - if (state) { - state->set_linear_velocity(linear_velocity); - } else { - PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity); - } + PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity); } Vector3 RigidBody3D::get_linear_velocity() const { @@ -814,11 +805,7 @@ Vector3 RigidBody3D::get_linear_velocity() const { void RigidBody3D::set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; - if (state) { - state->set_angular_velocity(angular_velocity); - } else { - PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity); - } + PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity); } Vector3 RigidBody3D::get_angular_velocity() const { @@ -1055,7 +1042,7 @@ void RigidBody3D::_bind_methods() { RigidBody3D::RigidBody3D() : PhysicsBody3D(PhysicsServer3D::BODY_MODE_DYNAMIC) { - PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &RigidBody3D::_direct_state_changed)); + PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &RigidBody3D::_body_state_changed_callback); } RigidBody3D::~RigidBody3D() { @@ -2249,23 +2236,19 @@ void PhysicalBone3D::_notification(int p_what) { } } -void PhysicalBone3D::_direct_state_changed(Object *p_state) { +void PhysicalBone3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) { + PhysicalBone3D *bone = (PhysicalBone3D *)p_instance; + bone->_body_state_changed(p_state); +} + +void PhysicalBone3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { if (!simulate_physics || !_internal_simulate_physics) { return; } - /// Update bone transform + /// Update bone transform. - PhysicsDirectBodyState3D *state; - -#ifdef DEBUG_ENABLED - state = Object::cast_to(p_state); - ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument"); -#else - state = (PhysicsDirectBodyState3D *)p_state; //trust it -#endif - - Transform3D global_transform(state->get_transform()); + Transform3D global_transform(p_state->get_transform()); set_ignore_transform_notification(true); set_global_transform(global_transform); @@ -2727,7 +2710,7 @@ void PhysicalBone3D::_start_physics_simulation() { set_body_mode(PhysicsServer3D::BODY_MODE_DYNAMIC); PhysicsServer3D::get_singleton()->body_set_collision_layer(get_rid(), get_collision_layer()); PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), get_collision_mask()); - PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &PhysicalBone3D::_direct_state_changed)); + PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &PhysicalBone3D::_body_state_changed_callback); set_as_top_level(true); _internal_simulate_physics = true; } @@ -2746,7 +2729,7 @@ void PhysicalBone3D::_stop_physics_simulation() { PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), 0); } if (_internal_simulate_physics) { - PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable()); + PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr); parent_skeleton->set_bone_global_pose_override(bone_id, Transform3D(), 0.0, false); set_as_top_level(false); _internal_simulate_physics = false; diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h index 26b9a390471..0733d57f510 100644 --- a/scene/3d/physics_body_3d.h +++ b/scene/3d/physics_body_3d.h @@ -86,7 +86,8 @@ class StaticBody3D : public PhysicsBody3D { Transform3D last_valid_transform; - void _direct_state_changed(Object *p_state); + static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state); + void _body_state_changed(PhysicsDirectBodyState3D *p_state); protected: void _notification(int p_what); @@ -136,7 +137,6 @@ public: protected: bool can_sleep = true; - PhysicsDirectBodyState3D *state = nullptr; Mode mode = MODE_DYNAMIC; real_t mass = 1.0; @@ -197,7 +197,8 @@ protected: void _body_exit_tree(ObjectID p_id); void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape); - virtual void _direct_state_changed(Object *p_state); + static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state); + virtual void _body_state_changed(PhysicsDirectBodyState3D *p_state); void _notification(int p_what); static void _bind_methods(); @@ -525,7 +526,8 @@ protected: bool _get(const StringName &p_name, Variant &r_ret) const; void _get_property_list(List *p_list) const; void _notification(int p_what); - void _direct_state_changed(Object *p_state); + static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state); + void _body_state_changed(PhysicsDirectBodyState3D *p_state); static void _bind_methods(); diff --git a/scene/3d/vehicle_body_3d.cpp b/scene/3d/vehicle_body_3d.cpp index 92c0e099470..daeea818910 100644 --- a/scene/3d/vehicle_body_3d.cpp +++ b/scene/3d/vehicle_body_3d.cpp @@ -802,24 +802,21 @@ void VehicleBody3D::_update_friction(PhysicsDirectBodyState3D *s) { } } -void VehicleBody3D::_direct_state_changed(Object *p_state) { - RigidBody3D::_direct_state_changed(p_state); +void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { + RigidBody3D::_body_state_changed(p_state); - state = Object::cast_to(p_state); - ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument"); - - real_t step = state->get_step(); + real_t step = p_state->get_step(); for (int i = 0; i < wheels.size(); i++) { - _update_wheel(i, state); + _update_wheel(i, p_state); } for (int i = 0; i < wheels.size(); i++) { - _ray_cast(i, state); - wheels[i]->set_transform(state->get_transform().inverse() * wheels[i]->m_worldTransform); + _ray_cast(i, p_state); + wheels[i]->set_transform(p_state->get_transform().inverse() * wheels[i]->m_worldTransform); } - _update_suspension(state); + _update_suspension(p_state); for (int i = 0; i < wheels.size(); i++) { //apply suspension force @@ -831,20 +828,20 @@ void VehicleBody3D::_direct_state_changed(Object *p_state) { suspensionForce = wheel.m_maxSuspensionForce; } Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; - Vector3 relative_position = wheel.m_raycastInfo.m_contactPointWS - state->get_transform().origin; + Vector3 relative_position = wheel.m_raycastInfo.m_contactPointWS - p_state->get_transform().origin; - state->apply_impulse(impulse, relative_position); + p_state->apply_impulse(impulse, relative_position); } - _update_friction(state); + _update_friction(p_state); for (int i = 0; i < wheels.size(); i++) { VehicleWheel3D &wheel = *wheels[i]; - Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - state->get_transform().origin; - Vector3 vel = state->get_linear_velocity() + (state->get_angular_velocity()).cross(relpos); // * mPos); + Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - p_state->get_transform().origin; + Vector3 vel = p_state->get_linear_velocity() + (p_state->get_angular_velocity()).cross(relpos); // * mPos); if (wheel.m_raycastInfo.m_isInContact) { - const Transform3D &chassisWorldTransform = state->get_transform(); + const Transform3D &chassisWorldTransform = p_state->get_transform(); Vector3 fwd( chassisWorldTransform.basis[0][Vector3::AXIS_Z], @@ -864,8 +861,6 @@ void VehicleBody3D::_direct_state_changed(Object *p_state) { wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact } - - state = nullptr; } void VehicleBody3D::set_engine_force(real_t p_engine_force) { @@ -926,7 +921,5 @@ void VehicleBody3D::_bind_methods() { VehicleBody3D::VehicleBody3D() { exclude.insert(get_rid()); - //PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &VehicleBody3D::_direct_state_changed)); - set_mass(40); } diff --git a/scene/3d/vehicle_body_3d.h b/scene/3d/vehicle_body_3d.h index 2c10205ea3f..f29c3d89b75 100644 --- a/scene/3d/vehicle_body_3d.h +++ b/scene/3d/vehicle_body_3d.h @@ -192,7 +192,8 @@ class VehicleBody3D : public RigidBody3D { static void _bind_methods(); - void _direct_state_changed(Object *p_state) override; + static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state); + virtual void _body_state_changed(PhysicsDirectBodyState3D *p_state) override; public: void set_engine_force(real_t p_engine_force); diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index 7aa2f9b7ded..77adb6a7daf 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -29,8 +29,9 @@ /*************************************************************************/ #include "body_2d_sw.h" + #include "area_2d_sw.h" -#include "physics_server_2d_sw.h" +#include "body_direct_state_2d_sw.h" #include "space_2d_sw.h" void Body2DSW::_update_inertia() { @@ -502,7 +503,7 @@ void Body2DSW::integrate_velocities(real_t p_step) { return; } - if (fi_callback) { + if (fi_callback_data || body_state_callback) { get_space()->body_add_to_state_query_list(&direct_state_query_list); } @@ -554,27 +555,27 @@ void Body2DSW::wakeup_neighbours() { } void Body2DSW::call_queries() { - if (fi_callback) { - PhysicsDirectBodyState2DSW *dbs = PhysicsDirectBodyState2DSW::singleton; - dbs->body = this; - - Variant v = dbs; - const Variant *vp[2] = { &v, &fi_callback->callback_udata }; - - Object *obj = fi_callback->callable.get_object(); - if (!obj) { + if (fi_callback_data) { + if (!fi_callback_data->callable.get_object()) { set_force_integration_callback(Callable()); } else { + Variant direct_state_variant = get_direct_state(); + const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; + Callable::CallError ce; Variant rv; - if (fi_callback->callback_udata.get_type() != Variant::NIL) { - fi_callback->callable.call(vp, 2, rv, ce); + if (fi_callback_data->udata.get_type() != Variant::NIL) { + fi_callback_data->callable.call(vp, 2, rv, ce); } else { - fi_callback->callable.call(vp, 1, rv, ce); + fi_callback_data->callable.call(vp, 1, rv, ce); } } } + + if (body_state_callback) { + (body_state_callback)(body_state_callback_instance, get_direct_state()); + } } bool Body2DSW::sleep_test(real_t p_step) { @@ -594,19 +595,32 @@ bool Body2DSW::sleep_test(real_t p_step) { } } -void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { - if (fi_callback) { - memdelete(fi_callback); - fi_callback = nullptr; - } +void Body2DSW::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) { + body_state_callback_instance = p_instance; + body_state_callback = p_callback; +} +void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { if (p_callable.get_object()) { - fi_callback = memnew(ForceIntegrationCallback); - fi_callback->callable = p_callable; - fi_callback->callback_udata = p_udata; + if (!fi_callback_data) { + fi_callback_data = memnew(ForceIntegrationCallbackData); + } + fi_callback_data->callable = p_callable; + fi_callback_data->udata = p_udata; + } else if (fi_callback_data) { + memdelete(fi_callback_data); + fi_callback_data = nullptr; } } +PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() { + if (!direct_state) { + direct_state = memnew(PhysicsDirectBodyState2DSW); + direct_state->body = this; + } + return direct_state; +} + Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), @@ -639,33 +653,13 @@ Body2DSW::Body2DSW() : still_time = 0; continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; can_sleep = true; - fi_callback = nullptr; } Body2DSW::~Body2DSW() { - if (fi_callback) { - memdelete(fi_callback); + if (fi_callback_data) { + memdelete(fi_callback_data); + } + if (direct_state) { + memdelete(direct_state); } } - -PhysicsDirectBodyState2DSW *PhysicsDirectBodyState2DSW::singleton = nullptr; - -PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() { - return body->get_space()->get_direct_state(); -} - -Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant()); - - if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) { - return Variant(); - } - Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider); - - int sidx = body->contacts[p_contact_idx].collider_shape; - if (sidx < 0 || sidx >= other->get_shape_count()) { - return Variant(); - } - - return other->get_shape_metadata(sidx); -} diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index 74bef433dc5..9b615a581fa 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -38,6 +38,7 @@ #include "core/templates/vset.h" class Constraint2DSW; +class PhysicsDirectBodyState2DSW; class Body2DSW : public CollisionObject2DSW { PhysicsServer2D::BodyMode mode; @@ -116,12 +117,17 @@ class Body2DSW : public CollisionObject2DSW { Vector contacts; //no contacts by default int contact_count; - struct ForceIntegrationCallback { + void *body_state_callback_instance = nullptr; + PhysicsServer2D::BodyStateCallback body_state_callback = nullptr; + + struct ForceIntegrationCallbackData { Callable callable; - Variant callback_udata; + Variant udata; }; - ForceIntegrationCallback *fi_callback; + ForceIntegrationCallbackData *fi_callback_data = nullptr; + + PhysicsDirectBodyState2DSW *direct_state = nullptr; uint64_t island_step; @@ -130,8 +136,11 @@ class Body2DSW : public CollisionObject2DSW { friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose public: + void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + PhysicsDirectBodyState2DSW *get_direct_state(); + _FORCE_INLINE_ void add_area(Area2DSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { @@ -332,87 +341,4 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; } -class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { - GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); - -public: - static PhysicsDirectBodyState2DSW *singleton; - Body2DSW *body; - real_t step; - - virtual Vector2 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area - - virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass - virtual real_t get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space - - virtual void set_linear_velocity(const Vector2 &p_velocity) override { body->set_linear_velocity(p_velocity); } - virtual Vector2 get_linear_velocity() const override { return body->get_linear_velocity(); } - - virtual void set_angular_velocity(real_t p_velocity) override { body->set_angular_velocity(p_velocity); } - virtual real_t get_angular_velocity() const override { return body->get_angular_velocity(); } - - virtual void set_transform(const Transform2D &p_transform) override { body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform2D get_transform() const override { return body->get_transform(); } - - virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override { return body->get_velocity_in_local_point(p_position); } - - virtual void add_central_force(const Vector2 &p_force) override { body->add_central_force(p_force); } - virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override { body->add_force(p_force, p_position); } - virtual void add_torque(real_t p_torque) override { body->add_torque(p_torque); } - virtual void apply_central_impulse(const Vector2 &p_impulse) override { body->apply_central_impulse(p_impulse); } - virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override { body->apply_impulse(p_impulse, p_position); } - virtual void apply_torque_impulse(real_t p_torque) override { body->apply_torque_impulse(p_torque); } - - virtual void set_sleep_state(bool p_enable) override { body->set_active(!p_enable); } - virtual bool is_sleeping() const override { return !body->is_active(); } - - virtual int get_contact_count() const override { return body->contact_count; } - - virtual Vector2 get_contact_local_position(int p_contact_idx) const override { - 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 override { - 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 override { - 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 override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); - return body->contacts[p_contact_idx].collider; - } - virtual Vector2 get_contact_collider_position(int p_contact_idx) const override { - 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 override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); - return body->contacts[p_contact_idx].collider_instance_id; - } - virtual int get_contact_collider_shape(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_shape; - } - virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const override; - - virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); - return body->contacts[p_contact_idx].collider_velocity_at_pos; - } - - virtual PhysicsDirectSpaceState2D *get_space_state() override; - - virtual real_t get_step() const override { return step; } - PhysicsDirectBodyState2DSW() { - singleton = this; - body = nullptr; - } -}; - #endif // BODY_2D_SW_H diff --git a/servers/physics_2d/body_direct_state_2d_sw.cpp b/servers/physics_2d/body_direct_state_2d_sw.cpp new file mode 100644 index 00000000000..1f68cca843e --- /dev/null +++ b/servers/physics_2d/body_direct_state_2d_sw.cpp @@ -0,0 +1,182 @@ +/*************************************************************************/ +/* body_direct_state_2d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "body_direct_state_2d_sw.h" + +#include "body_2d_sw.h" +#include "physics_server_2d_sw.h" +#include "space_2d_sw.h" + +Vector2 PhysicsDirectBodyState2DSW::get_total_gravity() const { + return body->gravity; +} + +real_t PhysicsDirectBodyState2DSW::get_total_angular_damp() const { + return body->area_angular_damp; +} + +real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const { + return body->area_linear_damp; +} + +real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const { + return body->get_inv_mass(); +} + +real_t PhysicsDirectBodyState2DSW::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +void PhysicsDirectBodyState2DSW::set_linear_velocity(const Vector2 &p_velocity) { + body->set_linear_velocity(p_velocity); +} + +Vector2 PhysicsDirectBodyState2DSW::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void PhysicsDirectBodyState2DSW::set_angular_velocity(real_t p_velocity) { + body->set_angular_velocity(p_velocity); +} + +real_t PhysicsDirectBodyState2DSW::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void PhysicsDirectBodyState2DSW::set_transform(const Transform2D &p_transform) { + body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform2D PhysicsDirectBodyState2DSW::get_transform() const { + return body->get_transform(); +} + +Vector2 PhysicsDirectBodyState2DSW::get_velocity_at_local_position(const Vector2 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void PhysicsDirectBodyState2DSW::add_central_force(const Vector2 &p_force) { + body->add_central_force(p_force); +} + +void PhysicsDirectBodyState2DSW::add_force(const Vector2 &p_force, const Vector2 &p_position) { + body->add_force(p_force, p_position); +} + +void PhysicsDirectBodyState2DSW::add_torque(real_t p_torque) { + body->add_torque(p_torque); +} + +void PhysicsDirectBodyState2DSW::apply_central_impulse(const Vector2 &p_impulse) { + body->apply_central_impulse(p_impulse); +} + +void PhysicsDirectBodyState2DSW::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { + body->apply_impulse(p_impulse, p_position); +} + +void PhysicsDirectBodyState2DSW::apply_torque_impulse(real_t p_torque) { + body->apply_torque_impulse(p_torque); +} + +void PhysicsDirectBodyState2DSW::set_sleep_state(bool p_enable) { + body->set_active(!p_enable); +} + +bool PhysicsDirectBodyState2DSW::is_sleeping() const { + return !body->is_active(); +} + +int PhysicsDirectBodyState2DSW::get_contact_count() const { + return body->contact_count; +} + +Vector2 PhysicsDirectBodyState2DSW::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector2 PhysicsDirectBodyState2DSW::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; +} + +int PhysicsDirectBodyState2DSW::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; +} + +RID PhysicsDirectBodyState2DSW::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; +} +Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID PhysicsDirectBodyState2DSW::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int PhysicsDirectBodyState2DSW::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; +} + +Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_velocity_at_position(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; +} + +Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant()); + + if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) { + return Variant(); + } + Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider); + + int sidx = body->contacts[p_contact_idx].collider_shape; + if (sidx < 0 || sidx >= other->get_shape_count()) { + return Variant(); + } + + return other->get_shape_metadata(sidx); +} + +PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t PhysicsDirectBodyState2DSW::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/servers/physics_2d/body_direct_state_2d_sw.h b/servers/physics_2d/body_direct_state_2d_sw.h new file mode 100644 index 00000000000..aef9186086f --- /dev/null +++ b/servers/physics_2d/body_direct_state_2d_sw.h @@ -0,0 +1,91 @@ +/*************************************************************************/ +/* body_direct_state_2d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef BODY_DIRECT_STATE_2D_SW_H +#define BODY_DIRECT_STATE_2D_SW_H + +#include "servers/physics_server_2d.h" + +class Body2DSW; + +class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { + GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); + +public: + Body2DSW *body = nullptr; + + virtual Vector2 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual real_t get_inverse_mass() const override; + virtual real_t get_inverse_inertia() const override; + + virtual void set_linear_velocity(const Vector2 &p_velocity) override; + virtual Vector2 get_linear_velocity() const override; + + virtual void set_angular_velocity(real_t p_velocity) override; + virtual real_t get_angular_velocity() const override; + + virtual void set_transform(const Transform2D &p_transform) override; + virtual Transform2D get_transform() const override; + + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override; + + virtual void add_central_force(const Vector2 &p_force) override; + virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void add_torque(real_t p_torque) override; + virtual void apply_central_impulse(const Vector2 &p_impulse) override; + virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + virtual void apply_torque_impulse(real_t p_torque) override; + + virtual void set_sleep_state(bool p_enable) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector2 get_contact_local_position(int p_contact_idx) const override; + virtual Vector2 get_contact_local_normal(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const override; + + virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState2D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // BODY_2D_SW_H diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp index 88c097453e6..0529a93358a 100644 --- a/servers/physics_2d/physics_server_2d_sw.cpp +++ b/servers/physics_2d/physics_server_2d_sw.cpp @@ -30,6 +30,7 @@ #include "physics_server_2d_sw.h" +#include "body_direct_state_2d_sw.h" #include "broad_phase_2d_bvh.h" #include "collision_solver_2d_sw.h" #include "core/config/project_settings.h" @@ -919,6 +920,12 @@ int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } +void PhysicsServer2DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { + Body2DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + body->set_state_sync_callback(p_instance, p_callback); +} + void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -953,17 +960,13 @@ bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from, PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) { ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - if (!body_owner.owns(p_body)) { - return nullptr; - } - Body2DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, nullptr); + ERR_FAIL_COND_V(!body->get_space(), nullptr); ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - direct_state->body = body; - return direct_state; + return body->get_direct_state(); } /* JOINT API */ @@ -1227,10 +1230,8 @@ void PhysicsServer2DSW::set_collision_iterations(int p_iterations) { void PhysicsServer2DSW::init() { doing_sync = false; - last_step = 0.001; iterations = 8; // 8? stepper = memnew(Step2DSW); - direct_state = memnew(PhysicsDirectBodyState2DSW); }; void PhysicsServer2DSW::step(real_t p_step) { @@ -1240,8 +1241,6 @@ void PhysicsServer2DSW::step(real_t p_step) { _update_shapes(); - last_step = p_step; - PhysicsDirectBodyState2DSW::singleton->step = p_step; island_count = 0; active_objects = 0; collision_pairs = 0; @@ -1313,7 +1312,6 @@ void PhysicsServer2DSW::end_sync() { void PhysicsServer2DSW::finish() { memdelete(stepper); - memdelete(direct_state); }; void PhysicsServer2DSW::_update_shapes() { diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h index 3610f43f93c..c0994fa9a75 100644 --- a/servers/physics_2d/physics_server_2d_sw.h +++ b/servers/physics_2d/physics_server_2d_sw.h @@ -46,7 +46,6 @@ class PhysicsServer2DSW : public PhysicsServer2D { bool active; int iterations; bool doing_sync; - real_t last_step; int island_count; int active_objects; @@ -59,8 +58,6 @@ class PhysicsServer2DSW : public PhysicsServer2D { Step2DSW *stepper; Set active_spaces; - PhysicsDirectBodyState2DSW *direct_state; - mutable RID_PtrOwner shape_owner; mutable RID_PtrOwner space_owner; mutable RID_PtrOwner area_owner; @@ -241,7 +238,9 @@ public: virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; virtual int body_get_max_contacts_reported(RID p_body) const override; + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; + 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) override; virtual void body_set_pickable(RID p_body, bool p_pickable) override; diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h index b93178919da..a0a7332ef33 100644 --- a/servers/physics_2d/physics_server_2d_wrap_mt.h +++ b/servers/physics_2d/physics_server_2d_wrap_mt.h @@ -244,6 +244,7 @@ public: FUNC2(body_set_omit_force_integration, RID, bool); FUNC1RC(bool, body_is_omitting_force_integration, RID); + FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); 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) override { diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index f04f3ab5835..d2aa71715ec 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -634,7 +634,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction Vector2 lv = b->get_linear_velocity(); //compute displacement from linear velocity - Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; + Vector2 motion = lv * last_step; real_t motion_len = motion.length(); motion.normalize(); cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); @@ -916,7 +916,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction Vector2 lv = b->get_linear_velocity(); //compute displacement from linear velocity - Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; + Vector2 motion = lv * last_step; real_t motion_len = motion.length(); motion.normalize(); rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index 3be36852b0c..061508af8a0 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -117,6 +117,8 @@ private: bool locked; + real_t last_step = 0.001; + int island_count; int active_objects; int collision_pairs; @@ -172,6 +174,9 @@ public: void lock(); void unlock(); + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value); real_t get_param(PhysicsServer2D::SpaceParameter p_param) const; diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 8b30160cc1b..0306ec5050e 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -129,6 +129,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + p_space->set_last_step(p_delta); + iterations = p_iterations; delta = p_delta; diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index 0c4079332d6..556f1f85c6a 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -29,7 +29,9 @@ /*************************************************************************/ #include "body_3d_sw.h" + #include "area_3d_sw.h" +#include "body_direct_state_3d_sw.h" #include "space_3d_sw.h" void Body3DSW::_update_inertia() { @@ -543,7 +545,7 @@ void Body3DSW::integrate_velocities(real_t p_step) { return; } - if (fi_callback) { + if (fi_callback_data || body_state_callback) { get_space()->body_add_to_state_query_list(&direct_state_query_list); } @@ -600,11 +602,6 @@ void Body3DSW::integrate_velocities(real_t p_step) { _set_inv_transform(get_transform().inverse()); _update_transform_dependant(); - - /* - if (fi_callback) { - get_space()->body_add_to_state_query_list(&direct_state_query_list); - */ } /* @@ -662,24 +659,23 @@ void Body3DSW::wakeup_neighbours() { } void Body3DSW::call_queries() { - if (fi_callback) { - PhysicsDirectBodyState3DSW *dbs = PhysicsDirectBodyState3DSW::singleton; - dbs->body = this; - - Variant v = dbs; - - Object *obj = fi_callback->callable.get_object(); - if (!obj) { + if (fi_callback_data) { + if (!fi_callback_data->callable.get_object()) { set_force_integration_callback(Callable()); } else { - const Variant *vp[2] = { &v, &fi_callback->udata }; + Variant direct_state_variant = get_direct_state(); + const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; Callable::CallError ce; - int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2; + int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2; Variant rv; - fi_callback->callable.call(vp, argc, rv, ce); + fi_callback_data->callable.call(vp, argc, rv, ce); } } + + if (body_state_callback_instance) { + (body_state_callback)(body_state_callback_instance, get_direct_state()); + } } bool Body3DSW::sleep_test(real_t p_step) { @@ -699,22 +695,34 @@ bool Body3DSW::sleep_test(real_t p_step) { } } -void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { - if (fi_callback) { - memdelete(fi_callback); - fi_callback = nullptr; - } +void Body3DSW::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) { + body_state_callback_instance = p_instance; + body_state_callback = p_callback; +} +void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { if (p_callable.get_object()) { - fi_callback = memnew(ForceIntegrationCallback); - fi_callback->callable = p_callable; - fi_callback->udata = p_udata; + if (!fi_callback_data) { + fi_callback_data = memnew(ForceIntegrationCallbackData); + } + fi_callback_data->callable = p_callable; + fi_callback_data->udata = p_udata; + } else if (fi_callback_data) { + memdelete(fi_callback_data); + fi_callback_data = nullptr; } } +PhysicsDirectBodyState3DSW *Body3DSW::get_direct_state() { + if (!direct_state) { + direct_state = memnew(PhysicsDirectBodyState3DSW); + direct_state->body = this; + } + return direct_state; +} + Body3DSW::Body3DSW() : CollisionObject3DSW(TYPE_BODY), - active_list(this), inertia_update_list(this), direct_state_query_list(this) { @@ -742,17 +750,13 @@ Body3DSW::Body3DSW() : still_time = 0; continuous_cd = false; can_sleep = true; - fi_callback = nullptr; } Body3DSW::~Body3DSW() { - if (fi_callback) { - memdelete(fi_callback); + if (fi_callback_data) { + memdelete(fi_callback_data); + } + if (direct_state) { + memdelete(direct_state); } } - -PhysicsDirectBodyState3DSW *PhysicsDirectBodyState3DSW::singleton = nullptr; - -PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() { - return body->get_space()->get_direct_state(); -} diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index efb114a3254..0acf3348265 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -28,14 +28,15 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ -#ifndef BODY_SW_H -#define BODY_SW_H +#ifndef BODY_3D_SW_H +#define BODY_3D_SW_H #include "area_3d_sw.h" #include "collision_object_3d_sw.h" #include "core/templates/vset.h" class Constraint3DSW; +class PhysicsDirectBodyState3DSW; class Body3DSW : public CollisionObject3DSW { PhysicsServer3D::BodyMode mode; @@ -113,12 +114,17 @@ class Body3DSW : public CollisionObject3DSW { Vector contacts; //no contacts by default int contact_count; - struct ForceIntegrationCallback { + void *body_state_callback_instance = nullptr; + PhysicsServer3D::BodyStateCallback body_state_callback = nullptr; + + struct ForceIntegrationCallbackData { Callable callable; Variant udata; }; - ForceIntegrationCallback *fi_callback; + ForceIntegrationCallbackData *fi_callback_data = nullptr; + + PhysicsDirectBodyState3DSW *direct_state = nullptr; uint64_t island_step; @@ -129,8 +135,11 @@ class Body3DSW : public CollisionObject3DSW { friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose public: + void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + PhysicsDirectBodyState3DSW *get_direct_state(); + _FORCE_INLINE_ void add_area(Area3DSW *p_area) { int index = areas.find(AreaCMP(p_area)); if (index > -1) { @@ -349,96 +358,4 @@ void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_no c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; } -class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D { - GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D); - -public: - static PhysicsDirectBodyState3DSW *singleton; - Body3DSW *body; - real_t step; - - virtual Vector3 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area - virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area - virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area - - virtual Vector3 get_center_of_mass() const override { return body->get_center_of_mass(); } - virtual Basis get_principal_inertia_axes() const override { return body->get_principal_inertia_axes(); } - - virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass - virtual Vector3 get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space - virtual Basis get_inverse_inertia_tensor() const override { return body->get_inv_inertia_tensor(); } // get density of this body space - - virtual void set_linear_velocity(const Vector3 &p_velocity) override { body->set_linear_velocity(p_velocity); } - virtual Vector3 get_linear_velocity() const override { return body->get_linear_velocity(); } - - virtual void set_angular_velocity(const Vector3 &p_velocity) override { body->set_angular_velocity(p_velocity); } - virtual Vector3 get_angular_velocity() const override { return body->get_angular_velocity(); } - - virtual void set_transform(const Transform3D &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } - virtual Transform3D get_transform() const override { return body->get_transform(); } - - virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override { return body->get_velocity_in_local_point(p_position); } - - virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); } - virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override { - body->add_force(p_force, p_position); - } - virtual void add_torque(const Vector3 &p_torque) override { body->add_torque(p_torque); } - virtual void apply_central_impulse(const Vector3 &p_impulse) override { body->apply_central_impulse(p_impulse); } - virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override { - body->apply_impulse(p_impulse, p_position); - } - virtual void apply_torque_impulse(const Vector3 &p_impulse) override { body->apply_torque_impulse(p_impulse); } - - virtual void set_sleep_state(bool p_sleep) override { body->set_active(!p_sleep); } - virtual bool is_sleeping() const override { return !body->is_active(); } - - virtual int get_contact_count() const override { return body->contact_count; } - - virtual Vector3 get_contact_local_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].local_pos; - } - virtual Vector3 get_contact_local_normal(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].local_normal; - } - virtual real_t get_contact_impulse(int p_contact_idx) const override { - return 0.0f; // Only implemented for bullet - } - virtual int get_contact_local_shape(int p_contact_idx) const override { - 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 override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); - return body->contacts[p_contact_idx].collider; - } - virtual Vector3 get_contact_collider_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].collider_pos; - } - virtual ObjectID get_contact_collider_id(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); - return body->contacts[p_contact_idx].collider_instance_id; - } - virtual int get_contact_collider_shape(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); - return body->contacts[p_contact_idx].collider_shape; - } - virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override { - ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); - return body->contacts[p_contact_idx].collider_velocity_at_pos; - } - - virtual PhysicsDirectSpaceState3D *get_space_state() override; - - virtual real_t get_step() const override { return step; } - PhysicsDirectBodyState3DSW() { - singleton = this; - body = nullptr; - } -}; - -#endif // BODY__SW_H +#endif // BODY_3D_SW_H diff --git a/servers/physics_3d/body_direct_state_3d_sw.cpp b/servers/physics_3d/body_direct_state_3d_sw.cpp new file mode 100644 index 00000000000..d197dd288dd --- /dev/null +++ b/servers/physics_3d/body_direct_state_3d_sw.cpp @@ -0,0 +1,182 @@ +/*************************************************************************/ +/* body_direct_state_3d_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "body_direct_state_3d_sw.h" + +#include "body_3d_sw.h" +#include "space_3d_sw.h" + +Vector3 PhysicsDirectBodyState3DSW::get_total_gravity() const { + return body->gravity; +} + +real_t PhysicsDirectBodyState3DSW::get_total_angular_damp() const { + return body->area_angular_damp; +} + +real_t PhysicsDirectBodyState3DSW::get_total_linear_damp() const { + return body->area_linear_damp; +} + +Vector3 PhysicsDirectBodyState3DSW::get_center_of_mass() const { + return body->get_center_of_mass(); +} + +Basis PhysicsDirectBodyState3DSW::get_principal_inertia_axes() const { + return body->get_principal_inertia_axes(); +} + +real_t PhysicsDirectBodyState3DSW::get_inverse_mass() const { + return body->get_inv_mass(); +} + +Vector3 PhysicsDirectBodyState3DSW::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +Basis PhysicsDirectBodyState3DSW::get_inverse_inertia_tensor() const { + return body->get_inv_inertia_tensor(); +} + +void PhysicsDirectBodyState3DSW::set_linear_velocity(const Vector3 &p_velocity) { + body->set_linear_velocity(p_velocity); +} + +Vector3 PhysicsDirectBodyState3DSW::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void PhysicsDirectBodyState3DSW::set_angular_velocity(const Vector3 &p_velocity) { + body->set_angular_velocity(p_velocity); +} + +Vector3 PhysicsDirectBodyState3DSW::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void PhysicsDirectBodyState3DSW::set_transform(const Transform3D &p_transform) { + body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform3D PhysicsDirectBodyState3DSW::get_transform() const { + return body->get_transform(); +} + +Vector3 PhysicsDirectBodyState3DSW::get_velocity_at_local_position(const Vector3 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void PhysicsDirectBodyState3DSW::add_central_force(const Vector3 &p_force) { + body->add_central_force(p_force); +} + +void PhysicsDirectBodyState3DSW::add_force(const Vector3 &p_force, const Vector3 &p_position) { + body->add_force(p_force, p_position); +} + +void PhysicsDirectBodyState3DSW::add_torque(const Vector3 &p_torque) { + body->add_torque(p_torque); +} + +void PhysicsDirectBodyState3DSW::apply_central_impulse(const Vector3 &p_impulse) { + body->apply_central_impulse(p_impulse); +} + +void PhysicsDirectBodyState3DSW::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { + body->apply_impulse(p_impulse, p_position); +} + +void PhysicsDirectBodyState3DSW::apply_torque_impulse(const Vector3 &p_impulse) { + body->apply_torque_impulse(p_impulse); +} + +void PhysicsDirectBodyState3DSW::set_sleep_state(bool p_sleep) { + body->set_active(!p_sleep); +} + +bool PhysicsDirectBodyState3DSW::is_sleeping() const { + return !body->is_active(); +} + +int PhysicsDirectBodyState3DSW::get_contact_count() const { + return body->contact_count; +} + +Vector3 PhysicsDirectBodyState3DSW::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector3 PhysicsDirectBodyState3DSW::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_normal; +} + +real_t PhysicsDirectBodyState3DSW::get_contact_impulse(int p_contact_idx) const { + return 0.0f; // Only implemented for bullet +} + +int PhysicsDirectBodyState3DSW::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; +} + +RID PhysicsDirectBodyState3DSW::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; +} + +Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID PhysicsDirectBodyState3DSW::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int PhysicsDirectBodyState3DSW::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; +} + +Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; +} + +PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t PhysicsDirectBodyState3DSW::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/servers/physics_3d/body_direct_state_3d_sw.h b/servers/physics_3d/body_direct_state_3d_sw.h new file mode 100644 index 00000000000..51323767151 --- /dev/null +++ b/servers/physics_3d/body_direct_state_3d_sw.h @@ -0,0 +1,94 @@ +/*************************************************************************/ +/* body_direct_state_3d_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef BODY_DIRECT_STATE_3D_SW_H +#define BODY_DIRECT_STATE_3D_SW_H + +#include "servers/physics_server_3d.h" + +class Body3DSW; + +class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D { + GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D); + +public: + Body3DSW *body = nullptr; + + virtual Vector3 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual Vector3 get_center_of_mass() const override; + virtual Basis get_principal_inertia_axes() const override; + + virtual real_t get_inverse_mass() const override; + virtual Vector3 get_inverse_inertia() const override; + virtual Basis get_inverse_inertia_tensor() const override; + + virtual void set_linear_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_linear_velocity() const override; + + virtual void set_angular_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_angular_velocity() const override; + + virtual void set_transform(const Transform3D &p_transform) override; + virtual Transform3D get_transform() const override; + + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override; + + virtual void add_central_force(const Vector3 &p_force) override; + virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void add_torque(const Vector3 &p_torque) override; + virtual void apply_central_impulse(const Vector3 &p_impulse) override; + virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; + virtual void apply_torque_impulse(const Vector3 &p_impulse) override; + + virtual void set_sleep_state(bool p_sleep) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector3 get_contact_local_position(int p_contact_idx) const override; + virtual Vector3 get_contact_local_normal(int p_contact_idx) const override; + virtual real_t get_contact_impulse(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState3D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // BODY_DIRECT_STATE_3D_SW_H diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index fbb374bd74f..e52a3bec2d8 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -30,6 +30,7 @@ #include "physics_server_3d_sw.h" +#include "body_direct_state_3d_sw.h" #include "broad_phase_3d_bvh.h" #include "core/debugger/engine_debugger.h" #include "core/os/os.h" @@ -836,6 +837,12 @@ int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const { return body->get_max_contacts_reported(); } +void PhysicsServer3DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) { + Body3DSW *body = body_owner.getornull(p_body); + ERR_FAIL_COND(!body); + body->set_state_sync_callback(p_instance, p_callback); +} + void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND(!body); @@ -863,11 +870,12 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, nullptr); + ERR_FAIL_NULL_V(body, nullptr); + + ERR_FAIL_NULL_V(body->get_space(), nullptr); ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); - direct_state->body = body; - return direct_state; + return body->get_direct_state(); } /* SOFT BODY */ @@ -1572,10 +1580,8 @@ void PhysicsServer3DSW::set_collision_iterations(int p_iterations) { }; void PhysicsServer3DSW::init() { - last_step = 0.001; iterations = 8; // 8? stepper = memnew(Step3DSW); - direct_state = memnew(PhysicsDirectBodyState3DSW); }; void PhysicsServer3DSW::step(real_t p_step) { @@ -1587,9 +1593,6 @@ void PhysicsServer3DSW::step(real_t p_step) { _update_shapes(); - last_step = p_step; - PhysicsDirectBodyState3DSW::singleton->step = p_step; - island_count = 0; active_objects = 0; collision_pairs = 0; @@ -1665,7 +1668,6 @@ void PhysicsServer3DSW::end_sync() { void PhysicsServer3DSW::finish() { memdelete(stepper); - memdelete(direct_state); }; int PhysicsServer3DSW::get_process_info(ProcessInfo p_info) { diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index 6e59a77e891..758cd0aa2f6 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -44,7 +44,6 @@ class PhysicsServer3DSW : public PhysicsServer3D { friend class PhysicsDirectSpaceState3DSW; bool active; int iterations; - real_t last_step; int island_count; int active_objects; @@ -57,8 +56,6 @@ class PhysicsServer3DSW : public PhysicsServer3D { Step3DSW *stepper; Set active_spaces; - PhysicsDirectBodyState3DSW *direct_state; - mutable RID_PtrOwner shape_owner; mutable RID_PtrOwner space_owner; mutable RID_PtrOwner area_owner; @@ -237,6 +234,7 @@ public: virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; virtual int body_get_max_contacts_reported(RID p_body) const override; + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override; virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h index 75174628bf2..c336747db8c 100644 --- a/servers/physics_3d/physics_server_3d_wrap_mt.h +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -245,6 +245,7 @@ public: FUNC2(body_set_omit_force_integration, RID, bool); FUNC1RC(bool, body_is_omitting_force_integration, RID); + FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback); FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &); FUNC2(body_set_ray_pickable, RID, bool); diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h index 9b5b4de0699..70faf0c1da1 100644 --- a/servers/physics_3d/space_3d_sw.h +++ b/servers/physics_3d/space_3d_sw.h @@ -112,6 +112,8 @@ private: bool locked; + real_t last_step = 0.001; + int island_count; int active_objects; int collision_pairs; @@ -174,6 +176,9 @@ public: void lock(); void unlock(); + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + void set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value); real_t get_param(PhysicsServer3D::SpaceParameter p_param) const; diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index ba18bac6112..d0604b0aa0e 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -185,6 +185,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + p_space->set_last_step(p_delta); + iterations = p_iterations; delta = p_delta; diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h index 4c559dd7bdf..deec3654f21 100644 --- a/servers/physics_server_2d.h +++ b/servers/physics_server_2d.h @@ -455,6 +455,10 @@ public: virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0; virtual bool body_is_omitting_force_integration(RID p_body) const = 0; + // Callback for C++ use only. + typedef void (*BodyStateCallback)(void *p_instance, PhysicsDirectBodyState2D *p_state); + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) = 0; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) = 0; 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) = 0; diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h index 5201e764980..a1c89c3ff29 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -469,6 +469,10 @@ public: virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0; virtual bool body_is_omitting_force_integration(RID p_body) const = 0; + // Callback for C++ use only. + typedef void (*BodyStateCallback)(void *p_instance, PhysicsDirectBodyState3D *p_state); + virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) = 0; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) = 0; virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0;