diff options
51 files changed, 1145 insertions, 544 deletions
diff --git a/core/input/input_map.cpp b/core/input/input_map.cpp index aab4e6593c..7421909650 100644 --- a/core/input/input_map.cpp +++ b/core/input/input_map.cpp @@ -474,6 +474,7 @@ const OrderedHashMap<String, List<Ref<InputEvent>>> &InputMap::get_builtins() { inputs = List<Ref<InputEvent>>(); inputs.push_back(InputEventKey::create_reference(KEY_TAB)); + inputs.push_back(InputEventKey::create_reference(KEY_ENTER)); default_builtin_cache.insert("ui_text_completion_accept", inputs); // Newlines diff --git a/doc/classes/EditorPlugin.xml b/doc/classes/EditorPlugin.xml index 61f1761249..f6f51df7c0 100644 --- a/doc/classes/EditorPlugin.xml +++ b/doc/classes/EditorPlugin.xml @@ -157,6 +157,16 @@ Registers a custom translation parser plugin for extracting translatable strings from custom files. </description> </method> + <method name="add_undo_redo_inspector_hook_callback"> + <return type="void"> + </return> + <argument index="0" name="callable" type="Callable"> + </argument> + <description> + Hooks a callback into the undo/redo action creation when a property is modified in the inspector. This allows, for example, to save other properties that may be lost when a given property is modified. + The callback should have 4 arguments: [Object] [code]undo_redo[/code], [Object] [code]modified_object[/code], [String] [code]property[/code] and [Variant] [code]new_value[/code]. They are, respectively, the [UndoRedo] object used by the inspector, the currently modified object, the name of the modified property and the new value the property is about to take. + </description> + </method> <method name="apply_changes" qualifiers="virtual"> <return type="void"> </return> @@ -622,6 +632,15 @@ Removes a registered custom translation parser plugin. </description> </method> + <method name="remove_undo_redo_inspector_hook_callback"> + <return type="void"> + </return> + <argument index="0" name="callable" type="Callable"> + </argument> + <description> + Removes a callback previsously added by [method add_undo_redo_inspector_hook_callback]. + </description> + </method> <method name="save_external_data" qualifiers="virtual"> <return type="void"> </return> diff --git a/doc/classes/ScrollContainer.xml b/doc/classes/ScrollContainer.xml index 9c5634f43a..40e29ac74b 100644 --- a/doc/classes/ScrollContainer.xml +++ b/doc/classes/ScrollContainer.xml @@ -39,12 +39,18 @@ <member name="scroll_horizontal_enabled" type="bool" setter="set_enable_h_scroll" getter="is_h_scroll_enabled" default="true"> If [code]true[/code], enables horizontal scrolling. </member> + <member name="scroll_horizontal_visible" type="bool" setter="set_h_scroll_visible" getter="is_h_scroll_visible" default="true"> + If [code]false[/code], hides the horizontal scrollbar. + </member> <member name="scroll_vertical" type="int" setter="set_v_scroll" getter="get_v_scroll" default="0"> The current vertical scroll value. </member> <member name="scroll_vertical_enabled" type="bool" setter="set_enable_v_scroll" getter="is_v_scroll_enabled" default="true"> If [code]true[/code], enables vertical scrolling. </member> + <member name="scroll_vertical_visible" type="bool" setter="set_v_scroll_visible" getter="is_v_scroll_visible" default="true"> + If [code]false[/code], hides the vertical scrollbar. + </member> </members> <signals> <signal name="scroll_ended"> diff --git a/editor/editor_about.cpp b/editor/editor_about.cpp index d962658484..7527514dca 100644 --- a/editor/editor_about.cpp +++ b/editor/editor_about.cpp @@ -37,6 +37,9 @@ #include "core/version.h" #include "core/version_hash.gen.h" +// The metadata key used to store and retrieve the version text to copy to the clipboard. +static const String META_TEXT_TO_COPY = "text_to_copy"; + void EditorAbout::_theme_changed() { const Ref<Font> font = get_theme_font("source", "EditorFonts"); const int font_size = get_theme_font_size("source_size", "EditorFonts"); @@ -63,7 +66,12 @@ void EditorAbout::_license_tree_selected() { _tpl_text->set_text(selected->get_metadata(0)); } +void EditorAbout::_version_button_pressed() { + DisplayServer::get_singleton()->clipboard_set(version_btn->get_meta(META_TEXT_TO_COPY)); +} + void EditorAbout::_bind_methods() { + ClassDB::bind_method(D_METHOD("_version_button_pressed"), &EditorAbout::_version_button_pressed); } TextureRect *EditorAbout::get_logo() const { @@ -124,17 +132,32 @@ EditorAbout::EditorAbout() { _logo = memnew(TextureRect); hbc->add_child(_logo); + VBoxContainer *version_info_vbc = memnew(VBoxContainer); + + // Add a dummy control node for spacing. + Control *v_spacer = memnew(Control); + version_info_vbc->add_child(v_spacer); + + version_btn = memnew(LinkButton); String hash = String(VERSION_HASH); if (hash.length() != 0) { hash = "." + hash.left(9); } + version_btn->set_text(VERSION_FULL_NAME + hash); + // Set the text to copy in metadata as it slightly differs from the button's text. + version_btn->set_meta(META_TEXT_TO_COPY, "v" VERSION_FULL_BUILD + hash); + version_btn->set_underline_mode(LinkButton::UNDERLINE_MODE_ON_HOVER); + version_btn->set_tooltip(TTR("Click to copy.")); + version_btn->connect("pressed", callable_mp(this, &EditorAbout::_version_button_pressed)); + version_info_vbc->add_child(version_btn); Label *about_text = memnew(Label); about_text->set_v_size_flags(Control::SIZE_SHRINK_CENTER); - about_text->set_text(VERSION_FULL_NAME + hash + - String::utf8("\n\xc2\xa9 2007-2021 Juan Linietsky, Ariel Manzur.\n\xc2\xa9 2014-2021 ") + + about_text->set_text(String::utf8("\xc2\xa9 2007-2021 Juan Linietsky, Ariel Manzur.\n\xc2\xa9 2014-2021 ") + TTR("Godot Engine contributors") + "\n"); - hbc->add_child(about_text); + version_info_vbc->add_child(about_text); + + hbc->add_child(version_info_vbc); TabContainer *tc = memnew(TabContainer); tc->set_custom_minimum_size(Size2(950, 400) * EDSCALE); diff --git a/editor/editor_about.h b/editor/editor_about.h index 2823220a8a..b76a2ada34 100644 --- a/editor/editor_about.h +++ b/editor/editor_about.h @@ -34,6 +34,7 @@ #include "scene/gui/control.h" #include "scene/gui/dialogs.h" #include "scene/gui/item_list.h" +#include "scene/gui/link_button.h" #include "scene/gui/rich_text_label.h" #include "scene/gui/scroll_container.h" #include "scene/gui/separator.h" @@ -53,8 +54,10 @@ class EditorAbout : public AcceptDialog { private: void _license_tree_selected(); + void _version_button_pressed(); ScrollContainer *_populate_list(const String &p_name, const List<String> &p_sections, const char *const *const p_src[], const int p_flag_single_column = 0); + LinkButton *version_btn; Tree *_tpl_tree; RichTextLabel *_license_text; RichTextLabel *_tpl_text; diff --git a/editor/editor_data.cpp b/editor/editor_data.cpp index fa4703d425..6405af3876 100644 --- a/editor/editor_data.cpp +++ b/editor/editor_data.cpp @@ -426,6 +426,18 @@ UndoRedo &EditorData::get_undo_redo() { return undo_redo; } +void EditorData::add_undo_redo_inspector_hook_callback(Callable p_callable) { + undo_redo_callbacks.push_back(p_callable); +} + +void EditorData::remove_undo_redo_inspector_hook_callback(Callable p_callable) { + undo_redo_callbacks.erase(p_callable); +} + +const Vector<Callable> EditorData::get_undo_redo_inspector_hook_callback() { + return undo_redo_callbacks; +} + void EditorData::remove_editor_plugin(EditorPlugin *p_plugin) { p_plugin->undo_redo = nullptr; editor_plugins.erase(p_plugin); diff --git a/editor/editor_data.h b/editor/editor_data.h index dbe729d9d9..2ece94d6a3 100644 --- a/editor/editor_data.h +++ b/editor/editor_data.h @@ -132,6 +132,7 @@ private: List<PropertyData> clipboard; UndoRedo undo_redo; + Vector<Callable> undo_redo_callbacks; void _cleanup_history(); @@ -166,6 +167,9 @@ public: EditorPlugin *get_editor_plugin(int p_idx); UndoRedo &get_undo_redo(); + void add_undo_redo_inspector_hook_callback(Callable p_callable); // Callbacks shoud have 4 args: (Object* undo_redo, Object *modified_object, String property, Varian new_value) + void remove_undo_redo_inspector_hook_callback(Callable p_callable); + const Vector<Callable> get_undo_redo_inspector_hook_callback(); void save_editor_global_states(); void restore_editor_global_states(); diff --git a/editor/editor_inspector.cpp b/editor/editor_inspector.cpp index 738b2f9f82..5bb3c8b4d0 100644 --- a/editor/editor_inspector.cpp +++ b/editor/editor_inspector.cpp @@ -2266,6 +2266,22 @@ void EditorInspector::_edit_set(const String &p_name, const Variant &p_value, bo undo_redo->add_do_property(object, p_name, p_value); undo_redo->add_undo_property(object, p_name, object->get(p_name)); + Variant v_undo_redo = (Object *)undo_redo; + Variant v_object = object; + Variant v_name = p_name; + for (int i = 0; i < EditorNode::get_singleton()->get_editor_data().get_undo_redo_inspector_hook_callback().size(); i++) { + const Callable &callback = EditorNode::get_singleton()->get_editor_data().get_undo_redo_inspector_hook_callback()[i]; + + const Variant *p_arguments[] = { &v_undo_redo, &v_object, &v_name, &p_value }; + Variant return_value; + Callable::CallError call_error; + + callback.call(p_arguments, 4, return_value, call_error); + if (call_error.error != Callable::CallError::CALL_OK) { + ERR_PRINT("Invalid UndoRedo callback."); + } + } + if (p_refresh_all) { undo_redo->add_do_method(this, "_edit_request_change", object, ""); undo_redo->add_undo_method(this, "_edit_request_change", object, ""); diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp index 6137617564..b5bb35a6e7 100644 --- a/editor/editor_node.cpp +++ b/editor/editor_node.cpp @@ -46,11 +46,13 @@ #include "core/string/print_string.h" #include "core/string/translation.h" #include "core/version.h" +#include "core/version_hash.gen.h" #include "main/main.h" #include "scene/gui/center_container.h" #include "scene/gui/control.h" #include "scene/gui/dialogs.h" #include "scene/gui/file_dialog.h" +#include "scene/gui/link_button.h" #include "scene/gui/menu_button.h" #include "scene/gui/panel.h" #include "scene/gui/panel_container.h" @@ -184,6 +186,9 @@ EditorNode *EditorNode::singleton = nullptr; +// The metadata key used to store and retrieve the version text to copy to the clipboard. +static const String META_TEXT_TO_COPY = "text_to_copy"; + void EditorNode::disambiguate_filenames(const Vector<String> p_full_paths, Vector<String> &r_filenames) { // Keep track of a list of "index sets," i.e. sets of indices // within disambiguated_scene_names which contain the same name. @@ -989,6 +994,10 @@ void EditorNode::_reload_project_settings() { void EditorNode::_vp_resized() { } +void EditorNode::_version_button_pressed() { + DisplayServer::get_singleton()->clipboard_set(version_btn->get_meta(META_TEXT_TO_COPY)); +} + void EditorNode::_node_renamed() { if (get_inspector()) { get_inspector()->update_tree(); @@ -5559,6 +5568,8 @@ void EditorNode::_bind_methods() { ClassDB::bind_method("_screenshot", &EditorNode::_screenshot); ClassDB::bind_method("_save_screenshot", &EditorNode::_save_screenshot); + ClassDB::bind_method("_version_button_pressed", &EditorNode::_version_button_pressed); + ADD_SIGNAL(MethodInfo("play_pressed")); ADD_SIGNAL(MethodInfo("pause_pressed")); ADD_SIGNAL(MethodInfo("stop_pressed")); @@ -6617,11 +6628,31 @@ EditorNode::EditorNode() { bottom_panel_hb_editors->set_h_size_flags(Control::SIZE_EXPAND_FILL); bottom_panel_hb->add_child(bottom_panel_hb_editors); - version_label = memnew(Label); - version_label->set_text(VERSION_FULL_CONFIG); + VBoxContainer *version_info_vbc = memnew(VBoxContainer); + bottom_panel_hb->add_child(version_info_vbc); + + // Add a dummy control node for vertical spacing. + Control *v_spacer = memnew(Control); + version_info_vbc->add_child(v_spacer); + + version_btn = memnew(LinkButton); + version_btn->set_text(VERSION_FULL_CONFIG); + String hash = String(VERSION_HASH); + if (hash.length() != 0) { + hash = "." + hash.left(9); + } + // Set the text to copy in metadata as it slightly differs from the button's text. + version_btn->set_meta(META_TEXT_TO_COPY, "v" VERSION_FULL_BUILD + hash); // Fade out the version label to be less prominent, but still readable - version_label->set_self_modulate(Color(1, 1, 1, 0.6)); - bottom_panel_hb->add_child(version_label); + version_btn->set_self_modulate(Color(1, 1, 1, 0.65)); + version_btn->set_underline_mode(LinkButton::UNDERLINE_MODE_ON_HOVER); + version_btn->set_tooltip(TTR("Click to copy.")); + version_btn->connect("pressed", callable_mp(this, &EditorNode::_version_button_pressed)); + version_info_vbc->add_child(version_btn); + + // Add a dummy control node for horizontal spacing. + Control *h_spacer = memnew(Control); + bottom_panel_hb->add_child(h_spacer); bottom_panel_raise = memnew(Button); bottom_panel_raise->set_flat(true); diff --git a/editor/editor_node.h b/editor/editor_node.h index 7e16936f5d..d06851cb4f 100644 --- a/editor/editor_node.h +++ b/editor/editor_node.h @@ -40,6 +40,7 @@ #include "editor/inspector_dock.h" #include "editor/property_editor.h" #include "editor/scene_tree_dock.h" +#include "scene/gui/link_button.h" typedef void (*EditorNodeInitCallback)(); typedef void (*EditorPluginInitializeCallback)(); @@ -424,7 +425,7 @@ private: HBoxContainer *bottom_panel_hb; HBoxContainer *bottom_panel_hb_editors; VBoxContainer *bottom_panel_vb; - Label *version_label; + LinkButton *version_btn; Button *bottom_panel_raise; Tree *disk_changed_list; @@ -477,6 +478,7 @@ private: void _close_messages(); void _show_messages(); void _vp_resized(); + void _version_button_pressed(); int _save_external_resources(); diff --git a/editor/editor_plugin.cpp b/editor/editor_plugin.cpp index eabcbacd9a..6b96cb0f5c 100644 --- a/editor/editor_plugin.cpp +++ b/editor/editor_plugin.cpp @@ -703,6 +703,14 @@ bool EditorPlugin::get_remove_list(List<Node *> *p_list) { void EditorPlugin::restore_global_state() {} void EditorPlugin::save_global_state() {} +void EditorPlugin::add_undo_redo_inspector_hook_callback(Callable p_callable) { + EditorNode::get_singleton()->get_editor_data().add_undo_redo_inspector_hook_callback(p_callable); +} + +void EditorPlugin::remove_undo_redo_inspector_hook_callback(Callable p_callable) { + EditorNode::get_singleton()->get_editor_data().remove_undo_redo_inspector_hook_callback(p_callable); +} + void EditorPlugin::add_translation_parser_plugin(const Ref<EditorTranslationParserPlugin> &p_parser) { EditorTranslationParser::get_singleton()->add_parser(p_parser, EditorTranslationParser::CUSTOM); } @@ -862,6 +870,8 @@ void EditorPlugin::_bind_methods() { ClassDB::bind_method(D_METHOD("hide_bottom_panel"), &EditorPlugin::hide_bottom_panel); ClassDB::bind_method(D_METHOD("get_undo_redo"), &EditorPlugin::_get_undo_redo); + ClassDB::bind_method(D_METHOD("add_undo_redo_inspector_hook_callback", "callable"), &EditorPlugin::add_undo_redo_inspector_hook_callback); + ClassDB::bind_method(D_METHOD("remove_undo_redo_inspector_hook_callback", "callable"), &EditorPlugin::remove_undo_redo_inspector_hook_callback); ClassDB::bind_method(D_METHOD("queue_save_layout"), &EditorPlugin::queue_save_layout); ClassDB::bind_method(D_METHOD("add_translation_parser_plugin", "parser"), &EditorPlugin::add_translation_parser_plugin); ClassDB::bind_method(D_METHOD("remove_translation_parser_plugin", "parser"), &EditorPlugin::remove_translation_parser_plugin); diff --git a/editor/editor_plugin.h b/editor/editor_plugin.h index 67b163eabf..37412e5ebe 100644 --- a/editor/editor_plugin.h +++ b/editor/editor_plugin.h @@ -225,6 +225,9 @@ public: EditorInterface *get_editor_interface(); ScriptCreateDialog *get_script_create_dialog(); + void add_undo_redo_inspector_hook_callback(Callable p_callable); + void remove_undo_redo_inspector_hook_callback(Callable p_callable); + int update_overlays() const; void queue_save_layout(); diff --git a/editor/project_manager.cpp b/editor/project_manager.cpp index e51e8ee82e..780dbb644e 100644 --- a/editor/project_manager.cpp +++ b/editor/project_manager.cpp @@ -2371,6 +2371,7 @@ void ProjectManager::_on_search_term_changed(const String &p_term) { void ProjectManager::_bind_methods() { ClassDB::bind_method("_unhandled_key_input", &ProjectManager::_unhandled_key_input); ClassDB::bind_method("_update_project_buttons", &ProjectManager::_update_project_buttons); + ClassDB::bind_method("_version_button_pressed", &ProjectManager::_version_button_pressed); } void ProjectManager::_open_asset_library() { @@ -2378,6 +2379,10 @@ void ProjectManager::_open_asset_library() { tabs->set_current_tab(1); } +void ProjectManager::_version_button_pressed() { + DisplayServer::get_singleton()->clipboard_set(version_btn->get_text()); +} + ProjectManager::ProjectManager() { // load settings if (!EditorSettings::get_singleton()) { @@ -2601,15 +2606,30 @@ ProjectManager::ProjectManager() { settings_hb->set_h_grow_direction(Control::GROW_DIRECTION_BEGIN); settings_hb->set_anchors_and_offsets_preset(Control::PRESET_TOP_RIGHT); - Label *version_label = memnew(Label); + // A VBoxContainer that contains a dummy Control node to adjust the LinkButton's vertical position. + VBoxContainer *spacer_vb = memnew(VBoxContainer); + settings_hb->add_child(spacer_vb); + + Control *v_spacer = memnew(Control); + spacer_vb->add_child(v_spacer); + + version_btn = memnew(LinkButton); String hash = String(VERSION_HASH); if (hash.length() != 0) { hash = "." + hash.left(9); } - version_label->set_text("v" VERSION_FULL_BUILD "" + hash); - version_label->set_self_modulate(Color(1, 1, 1, 0.6)); - version_label->set_align(Label::ALIGN_CENTER); - settings_hb->add_child(version_label); + version_btn->set_text("v" VERSION_FULL_BUILD + hash); + // Fade the version label to be less prominent, but still readable. + version_btn->set_self_modulate(Color(1, 1, 1, 0.6)); + version_btn->set_underline_mode(LinkButton::UNDERLINE_MODE_ON_HOVER); + version_btn->set_tooltip(TTR("Click to copy.")); + version_btn->connect("pressed", callable_mp(this, &ProjectManager::_version_button_pressed)); + spacer_vb->add_child(version_btn); + + // Add a small horizontal spacer between the version and language buttons + // to distinguish them. + Control *h_spacer = memnew(Control); + settings_hb->add_child(h_spacer); language_btn = memnew(OptionButton); language_btn->set_flat(true); diff --git a/editor/project_manager.h b/editor/project_manager.h index a66b7c4ab6..0fc69bd313 100644 --- a/editor/project_manager.h +++ b/editor/project_manager.h @@ -89,6 +89,7 @@ class ProjectManager : public Control { ProjectDialog *npdialog; OptionButton *language_btn; + LinkButton *version_btn; void _open_asset_library(); void _scan_projects(); @@ -123,6 +124,7 @@ class ProjectManager : public Control { void _unhandled_key_input(const Ref<InputEvent> &p_ev); void _files_dropped(PackedStringArray p_files, int p_screen); + void _version_button_pressed(); void _on_order_option_changed(int p_idx); void _on_tab_changed(int p_tab); void _on_search_term_changed(const String &p_term); diff --git a/scene/2d/tile_map.cpp b/scene/2d/tile_map.cpp index 4565543ec3..532a795b7c 100644 --- a/scene/2d/tile_map.cpp +++ b/scene/2d/tile_map.cpp @@ -1526,6 +1526,12 @@ Vector2 TileMap::map_to_world(const Vector2 &p_pos, bool p_ignore_ofs) const { Vector2 TileMap::world_to_map(const Vector2 &p_pos) const { Vector2 ret = get_cell_transform().affine_inverse().xform(p_pos); + // Account for precision errors on the border (GH-23250). + // 0.00005 is 5*CMP_EPSILON, results would start being unpredictable if + // cell size is > 15,000, but we can hardly have more precision anyway with + // floating point. + ret += Vector2(0.00005, 0.00005); + switch (half_offset) { case HALF_OFFSET_X: { if (int(floor(ret.y)) & 1) { @@ -1552,11 +1558,6 @@ Vector2 TileMap::world_to_map(const Vector2 &p_pos) const { } } - // Account for precision errors on the border (GH-23250). - // 0.00005 is 5*CMP_EPSILON, results would start being unpredictable if - // cell size is > 15,000, but we can hardly have more precision anyway with - // floating point. - ret += Vector2(0.00005, 0.00005); return ret.floor(); } diff --git a/scene/gui/scroll_container.cpp b/scene/gui/scroll_container.cpp index 73c6371658..67dcf458b0 100644 --- a/scene/gui/scroll_container.cpp +++ b/scene/gui/scroll_container.cpp @@ -299,7 +299,7 @@ void ScrollContainer::_update_dimensions() { child_max_size.x = MAX(child_max_size.x, minsize.x); child_max_size.y = MAX(child_max_size.y, minsize.y); - Rect2 r = Rect2(-scroll, minsize); + Rect2 r = Rect2(-Size2(get_h_scroll(), get_v_scroll()), minsize); if (!scroll_h || (!h_scroll->is_visible_in_tree() && c->get_h_size_flags() & SIZE_EXPAND)) { r.position.x = 0; if (c->get_h_size_flags() & SIZE_EXPAND) { @@ -434,40 +434,16 @@ void ScrollContainer::update_scrollbars() { Size2 min = child_max_size; - bool hide_scroll_v = !scroll_v || min.height <= size.height; - bool hide_scroll_h = !scroll_h || min.width <= size.width; - - v_scroll->set_max(min.height); - if (hide_scroll_v) { - v_scroll->set_page(size.height); - v_scroll->hide(); - scroll.y = 0; - } else { - v_scroll->show(); - if (hide_scroll_h) { - v_scroll->set_page(size.height); - } else { - v_scroll->set_page(size.height - hmin.height); - } - - scroll.y = v_scroll->get_value(); - } + bool hide_scroll_h = !scroll_h || min.width <= size.width || !h_scroll_visible; + bool hide_scroll_v = !scroll_v || min.height <= size.height || !v_scroll_visible; h_scroll->set_max(min.width); - if (hide_scroll_h) { - h_scroll->set_page(size.width); - h_scroll->hide(); - scroll.x = 0; - } else { - h_scroll->show(); - if (hide_scroll_v) { - h_scroll->set_page(size.width); - } else { - h_scroll->set_page(size.width - vmin.width); - } + h_scroll->set_page(size.width - (hide_scroll_v ? 0 : vmin.width)); + h_scroll->set_visible(!hide_scroll_h); - scroll.x = h_scroll->get_value(); - } + v_scroll->set_max(min.height); + v_scroll->set_page(size.height - (hide_scroll_h ? 0 : hmin.height)); + v_scroll->set_visible(!hide_scroll_v); // Avoid scrollbar overlapping. h_scroll->set_anchor_and_offset(SIDE_RIGHT, ANCHOR_END, hide_scroll_v ? 0 : -vmin.width); @@ -475,13 +451,28 @@ void ScrollContainer::update_scrollbars() { } void ScrollContainer::_scroll_moved(float) { - scroll.x = h_scroll->get_value(); - scroll.y = v_scroll->get_value(); queue_sort(); - update(); }; +void ScrollContainer::set_h_scroll(int p_pos) { + h_scroll->set_value(p_pos); + _cancel_drag(); +} + +int ScrollContainer::get_h_scroll() const { + return h_scroll->get_value(); +} + +void ScrollContainer::set_v_scroll(int p_pos) { + v_scroll->set_value(p_pos); + _cancel_drag(); +} + +int ScrollContainer::get_v_scroll() const { + return v_scroll->get_value(); +} + void ScrollContainer::set_enable_h_scroll(bool p_enable) { if (scroll_h == p_enable) { return; @@ -510,22 +501,30 @@ bool ScrollContainer::is_v_scroll_enabled() const { return scroll_v; } -int ScrollContainer::get_v_scroll() const { - return v_scroll->get_value(); +void ScrollContainer::set_h_scroll_visible(bool p_visible) { + if (h_scroll_visible == p_visible) { + return; + } + + h_scroll_visible = p_visible; + update_scrollbars(); } -void ScrollContainer::set_v_scroll(int p_pos) { - v_scroll->set_value(p_pos); - _cancel_drag(); +bool ScrollContainer::is_h_scroll_visible() const { + return h_scroll_visible; } -int ScrollContainer::get_h_scroll() const { - return h_scroll->get_value(); +void ScrollContainer::set_v_scroll_visible(bool p_visible) { + if (v_scroll_visible == p_visible) { + return; + } + + v_scroll_visible = p_visible; + update_scrollbars(); } -void ScrollContainer::set_h_scroll(int p_pos) { - h_scroll->set_value(p_pos); - _cancel_drag(); +bool ScrollContainer::is_v_scroll_visible() const { + return v_scroll_visible; } int ScrollContainer::get_deadzone() const { @@ -581,17 +580,29 @@ VScrollBar *ScrollContainer::get_v_scrollbar() { void ScrollContainer::_bind_methods() { ClassDB::bind_method(D_METHOD("_gui_input"), &ScrollContainer::_gui_input); - ClassDB::bind_method(D_METHOD("set_enable_h_scroll", "enable"), &ScrollContainer::set_enable_h_scroll); - ClassDB::bind_method(D_METHOD("is_h_scroll_enabled"), &ScrollContainer::is_h_scroll_enabled); - ClassDB::bind_method(D_METHOD("set_enable_v_scroll", "enable"), &ScrollContainer::set_enable_v_scroll); - ClassDB::bind_method(D_METHOD("is_v_scroll_enabled"), &ScrollContainer::is_v_scroll_enabled); ClassDB::bind_method(D_METHOD("_update_scrollbar_position"), &ScrollContainer::_update_scrollbar_position); + ClassDB::bind_method(D_METHOD("set_h_scroll", "value"), &ScrollContainer::set_h_scroll); ClassDB::bind_method(D_METHOD("get_h_scroll"), &ScrollContainer::get_h_scroll); + ClassDB::bind_method(D_METHOD("set_v_scroll", "value"), &ScrollContainer::set_v_scroll); ClassDB::bind_method(D_METHOD("get_v_scroll"), &ScrollContainer::get_v_scroll); + + ClassDB::bind_method(D_METHOD("set_enable_h_scroll", "enable"), &ScrollContainer::set_enable_h_scroll); + ClassDB::bind_method(D_METHOD("is_h_scroll_enabled"), &ScrollContainer::is_h_scroll_enabled); + + ClassDB::bind_method(D_METHOD("set_enable_v_scroll", "enable"), &ScrollContainer::set_enable_v_scroll); + ClassDB::bind_method(D_METHOD("is_v_scroll_enabled"), &ScrollContainer::is_v_scroll_enabled); + + ClassDB::bind_method(D_METHOD("set_h_scroll_visible", "visible"), &ScrollContainer::set_h_scroll_visible); + ClassDB::bind_method(D_METHOD("is_h_scroll_visible"), &ScrollContainer::is_h_scroll_visible); + + ClassDB::bind_method(D_METHOD("set_v_scroll_visible", "visible"), &ScrollContainer::set_v_scroll_visible); + ClassDB::bind_method(D_METHOD("is_v_scroll_visible"), &ScrollContainer::is_v_scroll_visible); + ClassDB::bind_method(D_METHOD("set_deadzone", "deadzone"), &ScrollContainer::set_deadzone); ClassDB::bind_method(D_METHOD("get_deadzone"), &ScrollContainer::get_deadzone); + ClassDB::bind_method(D_METHOD("set_follow_focus", "enabled"), &ScrollContainer::set_follow_focus); ClassDB::bind_method(D_METHOD("is_following_focus"), &ScrollContainer::is_following_focus); @@ -604,10 +615,12 @@ void ScrollContainer::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::BOOL, "follow_focus"), "set_follow_focus", "is_following_focus"); ADD_GROUP("Scroll", "scroll_"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "scroll_horizontal_enabled"), "set_enable_h_scroll", "is_h_scroll_enabled"); ADD_PROPERTY(PropertyInfo(Variant::INT, "scroll_horizontal"), "set_h_scroll", "get_h_scroll"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "scroll_vertical_enabled"), "set_enable_v_scroll", "is_v_scroll_enabled"); ADD_PROPERTY(PropertyInfo(Variant::INT, "scroll_vertical"), "set_v_scroll", "get_v_scroll"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "scroll_horizontal_enabled"), "set_enable_h_scroll", "is_h_scroll_enabled"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "scroll_vertical_enabled"), "set_enable_v_scroll", "is_v_scroll_enabled"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "scroll_horizontal_visible"), "set_h_scroll_visible", "is_h_scroll_visible"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "scroll_vertical_visible"), "set_v_scroll_visible", "is_v_scroll_visible"); ADD_PROPERTY(PropertyInfo(Variant::INT, "scroll_deadzone"), "set_deadzone", "get_deadzone"); GLOBAL_DEF("gui/common/default_scroll_deadzone", 0); diff --git a/scene/gui/scroll_container.h b/scene/gui/scroll_container.h index e7d73bab0a..f61df70b85 100644 --- a/scene/gui/scroll_container.h +++ b/scene/gui/scroll_container.h @@ -42,7 +42,6 @@ class ScrollContainer : public Container { VScrollBar *v_scroll; Size2 child_max_size; - Size2 scroll; void update_scrollbars(); @@ -50,16 +49,17 @@ class ScrollContainer : public Container { Vector2 drag_accum; Vector2 drag_from; Vector2 last_drag_accum; - float last_drag_time = 0.0; - float time_since_motion = 0.0; + float time_since_motion = 0.0f; bool drag_touching = false; bool drag_touching_deaccel = false; - bool click_handled = false; bool beyond_deadzone = false; bool scroll_h = true; bool scroll_v = true; + bool h_scroll_visible = true; + bool v_scroll_visible = true; + int deadzone = 0; bool follow_focus = false; @@ -80,11 +80,11 @@ protected: void _ensure_focused_visible(Control *p_node); public: - int get_v_scroll() const; - void set_v_scroll(int p_pos); - - int get_h_scroll() const; void set_h_scroll(int p_pos); + int get_h_scroll() const; + + void set_v_scroll(int p_pos); + int get_v_scroll() const; void set_enable_h_scroll(bool p_enable); bool is_h_scroll_enabled() const; @@ -92,6 +92,12 @@ public: void set_enable_v_scroll(bool p_enable); bool is_v_scroll_enabled() const; + void set_h_scroll_visible(bool p_visible); + bool is_h_scroll_visible() const; + + void set_v_scroll_visible(bool p_visible); + bool is_v_scroll_visible() const; + int get_deadzone() const; void set_deadzone(int p_deadzone); diff --git a/scene/gui/tree.cpp b/scene/gui/tree.cpp index 73fd9dbcd7..5fbf1bdb29 100644 --- a/scene/gui/tree.cpp +++ b/scene/gui/tree.cpp @@ -1388,7 +1388,7 @@ int Tree::draw_item(const Point2i &p_pos, const Point2 &p_draw_ofs, const Size2 } } - if ((select_mode == SELECT_ROW && selected_item == p_item) || p_item->cells[i].selected) { + if ((select_mode == SELECT_ROW && selected_item == p_item) || p_item->cells[i].selected || !p_item->has_meta("__focus_rect")) { Rect2i r(cell_rect.position, cell_rect.size); p_item->set_meta("__focus_rect", Rect2(r.position, r.size)); diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp index 90f556cf1b..f861e3064c 100644 --- a/scene/main/viewport.cpp +++ b/scene/main/viewport.cpp @@ -1840,6 +1840,7 @@ void Viewport::_gui_input_event(Ref<InputEvent> p_event) { Control *over = nullptr; Point2 mpos = mb->get_position(); + gui.last_mouse_pos = mpos; if (mb->is_pressed()) { Size2 pos = mpos; if (gui.mouse_focus_mask) { diff --git a/scene/resources/physics_material.cpp b/scene/resources/physics_material.cpp index d65b0c8927..31df35aa51 100644 --- a/scene/resources/physics_material.cpp +++ b/scene/resources/physics_material.cpp @@ -43,9 +43,9 @@ void PhysicsMaterial::_bind_methods() { ClassDB::bind_method(D_METHOD("set_absorbent", "absorbent"), &PhysicsMaterial::set_absorbent); ClassDB::bind_method(D_METHOD("is_absorbent"), &PhysicsMaterial::is_absorbent); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "friction", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_friction", "get_friction"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "friction", PROPERTY_HINT_RANGE, "0,1,0.01,or_greater"), "set_friction", "get_friction"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "rough"), "set_rough", "is_rough"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "bounce", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_bounce", "get_bounce"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "bounce", PROPERTY_HINT_RANGE, "0,1,0.01,or_greater"), "set_bounce", "get_bounce"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "absorbent"), "set_absorbent", "is_absorbent"); } diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index 21ad57e344..eb9fc0e800 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -40,31 +40,48 @@ bool AreaPair2DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->add_area(area); - } - if (area->has_monitor_callback()) { - area->add_body_to_query(body, body_shape, area_shape); - } - - } else { - if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; } colliding = result; } - return false; //never do any post solving + return process_collision; +} + +bool AreaPair2DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. } void AreaPair2DSW::solve(real_t p_step) { + // Nothing to do. } AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { @@ -72,7 +89,6 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, area = p_area; body_shape = p_body_shape; area_shape = p_area_shape; - colliding = false; body->add_constraint(this, 0); area->add_constraint(this); if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair @@ -103,33 +119,48 @@ bool Area2Pair2DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - area_b->add_area_to_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } - - } else { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision = true; + } else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision = true; } colliding = result; } - return false; //never do any post solving + return process_collision; +} + +bool Area2Pair2DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + area_b->add_area_to_query(area_a, shape_a, shape_b); + } + + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } + } else { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + return false; // Never do any post solving. } void Area2Pair2DSW::solve(real_t p_step) { + // Nothing to do. } Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { @@ -137,7 +168,6 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area area_b = p_area_b; shape_a = p_shape_a; shape_b = p_shape_b; - colliding = false; area_a->add_constraint(this); area_b->add_constraint(this); } diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h index 4015aad5d1..4632a307d9 100644 --- a/servers/physics_2d/area_pair_2d_sw.h +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -36,30 +36,34 @@ #include "constraint_2d_sw.h" class AreaPair2DSW : public Constraint2DSW { - Body2DSW *body; - Area2DSW *area; - int body_shape; - int area_shape; - bool colliding; + Body2DSW *body = nullptr; + Area2DSW *area = nullptr; + int body_shape = 0; + int area_shape = 0; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); ~AreaPair2DSW(); }; class Area2Pair2DSW : public Constraint2DSW { - Area2DSW *area_a; - Area2DSW *area_b; - int shape_a; - int shape_b; - bool colliding; + Area2DSW *area_a = nullptr; + Area2DSW *area_b = nullptr; + int shape_a = 0; + int shape_b = 0; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); ~Area2Pair2DSW(); diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index 9306fea70c..f78a487e27 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -104,31 +104,17 @@ void Body2DSW::set_active(bool p_active) { } active = p_active; - if (!p_active) { - if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } - } else { + + if (active) { if (mode == PhysicsServer2D::BODY_MODE_STATIC) { - return; //static bodies can't become active - } - if (get_space()) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { get_space()->body_add_to_active_list(&active_list); } - - //still_time=0; - } - /* - if (!space) - return; - - for(int i=0;i<get_shape_count();i++) { - Shape &s=shapes[i]; - if (s.bpid>0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); } -*/ } void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, real_t p_value) { @@ -370,13 +356,6 @@ void Body2DSW::set_space(Space2DSW *p_space) { if (active) { get_space()->body_add_to_active_list(&active_list); } - /* - _update_queries(); - if (is_active()) { - active=false; - set_active(true); - } - */ } first_integration = false; diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index da70ac7d4b..ff31825b83 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -87,10 +87,13 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto int least_deep = -1; real_t min_depth = 1e10; + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + for (int i = 0; i <= contact_count; i++) { Contact &c = (i == contact_count) ? contact : contacts[i]; - Vector2 global_A = A->get_transform().basis_xform(c.local_A); - Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); @@ -124,6 +127,9 @@ void BodyPair2DSW::_validate_contacts() { real_t max_separation = space->get_contact_max_separation(); real_t max_separation2 = max_separation * max_separation; + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; @@ -134,8 +140,8 @@ void BodyPair2DSW::_validate_contacts() { } else { c.reused = false; - Vector2 global_A = A->get_transform().basis_xform(c.local_A); - Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; Vector2 axis = global_A - global_B; real_t depth = axis.dot(c.normal); @@ -220,14 +226,16 @@ real_t combine_friction(Body2DSW *A, Body2DSW *B) { } bool BodyPair2DSW::setup(real_t p_step) { - //cannot collide + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; } - bool report_contacts_only = false; - if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { + report_contacts_only = false; + if (!dynamic_A && !dynamic_B) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { report_contacts_only = true; } else { @@ -246,12 +254,12 @@ bool BodyPair2DSW::setup(real_t p_step) { _validate_contacts(); - Vector2 offset_A = A->get_transform().get_origin(); + const Vector2 &offset_A = A->get_transform().get_origin(); Transform2D xform_Au = A->get_transform().untranslated(); Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); Transform2D xform_Bu = B->get_transform(); - xform_Bu.elements[2] -= A->get_transform().get_origin(); + xform_Bu.elements[2] -= offset_A; Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); Shape2DSW *shape_A_ptr = A->get_shape(shape_A); @@ -272,13 +280,13 @@ bool BodyPair2DSW::setup(real_t p_step) { if (!collided) { //test ccd (currently just a raycast) - if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_A) { if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { collided = true; } } - if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && dynamic_B) { if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { collided = true; } @@ -338,9 +346,21 @@ bool BodyPair2DSW::setup(real_t p_step) { } } + return true; +} + +bool BodyPair2DSW::pre_solve(real_t p_step) { + if (!collided || oneway_disabled) { + return false; + } + real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = 0.3; + + Shape2DSW *shape_A_ptr = A->get_shape(shape_A); + Shape2DSW *shape_B_ptr = B->get_shape(shape_B); + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias() == 0) { bias = shape_B_ptr->get_custom_bias(); @@ -351,21 +371,23 @@ bool BodyPair2DSW::setup(real_t p_step) { } } - cc = 0; - real_t inv_dt = 1.0 / p_step; bool do_process = false; + const Vector2 &offset_A = A->get_transform().get_origin(); + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; - c.active = false; - Vector2 global_A = xform_Au.xform(c.local_A); - Vector2 global_B = xform_Bu.xform(c.local_B); + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; - real_t depth = c.normal.dot(global_A - global_B); + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); if (depth <= 0 || !c.reused) { continue; @@ -396,8 +418,6 @@ bool BodyPair2DSW::setup(real_t p_step) { continue; } - c.active = true; - // Precompute normal mass, tangent mass, and bias. real_t rnA = c.rA.dot(c.normal); real_t rnB = c.rB.dot(c.normal); @@ -421,8 +441,12 @@ bool BodyPair2DSW::setup(real_t p_step) { // Apply normal + friction impulse Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; - A->apply_impulse(-P, c.rA); - B->apply_impulse(P, c.rB); + if (dynamic_A) { + A->apply_impulse(-P, c.rA); + } + if (dynamic_B) { + B->apply_impulse(P, c.rB); + } } #endif @@ -434,6 +458,7 @@ bool BodyPair2DSW::setup(real_t p_step) { c.bounce = c.bounce * dv.dot(c.normal); } + c.active = true; do_process = true; } @@ -441,13 +466,12 @@ bool BodyPair2DSW::setup(real_t p_step) { } void BodyPair2DSW::solve(real_t p_step) { - if (!collided) { + if (!collided || oneway_disabled) { return; } for (int i = 0; i < contact_count; ++i) { Contact &c = contacts[i]; - cc++; if (!c.active) { continue; @@ -474,8 +498,12 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(-jb, c.rA); - B->apply_bias_impulse(jb, c.rB); + if (dynamic_A) { + A->apply_bias_impulse(-jb, c.rA); + } + if (dynamic_B) { + B->apply_bias_impulse(jb, c.rB); + } real_t jn = -(c.bounce + vn) * c.mass_normal; real_t jnOld = c.acc_normal_impulse; @@ -490,8 +518,12 @@ void BodyPair2DSW::solve(real_t p_step) { Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); - A->apply_impulse(-j, c.rA); - B->apply_impulse(j, c.rB); + if (dynamic_A) { + A->apply_impulse(-j, c.rA); + } + if (dynamic_B) { + B->apply_impulse(j, c.rB); + } } } @@ -504,9 +536,6 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh space = A->get_space(); A->add_constraint(this, 0); B->add_constraint(this, 1); - contact_count = 0; - collided = false; - oneway_disabled = false; } BodyPair2DSW::~BodyPair2DSW() { diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index 31ab9b9017..4b42b44c92 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -44,13 +44,16 @@ class BodyPair2DSW : public Constraint2DSW { Body2DSW *B; }; - Body2DSW *_arr[2]; + Body2DSW *_arr[2] = { nullptr, nullptr }; }; - int shape_A; - int shape_B; + int shape_A = 0; + int shape_B = 0; - Space2DSW *space; + bool dynamic_A = false; + bool dynamic_B = false; + + Space2DSW *space = nullptr; struct Contact { Vector2 position; @@ -73,10 +76,10 @@ class BodyPair2DSW : public Constraint2DSW { Vector2 sep_axis; Contact contacts[MAX_CONTACTS]; - int contact_count; - bool collided; - bool oneway_disabled; - int cc; + int contact_count = 0; + bool collided = false; + bool oneway_disabled = false; + bool report_contacts_only = false; bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false); void _validate_contacts(); @@ -84,8 +87,9 @@ class BodyPair2DSW : public Constraint2DSW { _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B); ~BodyPair2DSW(); diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 2db3961f41..c395e59694 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -143,8 +143,8 @@ public: return shapes[p_index].metadata; } - _FORCE_INLINE_ Transform2D get_transform() const { return transform; } - _FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ Space2DSW *get_space() const { return space; } void set_shape_as_disabled(int p_idx, bool p_disabled); diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index b724deb48e..5e8dfbe570 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -63,6 +63,7 @@ public: _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0; virtual ~Constraint2DSW() {} diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp index 20d4b9aa1a..eaec582f9b 100644 --- a/servers/physics_2d/joints_2d_sw.cpp +++ b/servers/physics_2d/joints_2d_sw.cpp @@ -97,7 +97,10 @@ normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vecto } bool PinJoint2DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -148,12 +151,6 @@ bool PinJoint2DSW::setup(real_t p_step) { bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); - // apply accumulated impulse - A->apply_impulse(-P, rA); - if (B) { - B->apply_impulse(P, rB); - } - return true; } @@ -161,6 +158,18 @@ inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { return Vector2(p_other * p_vec.y, -p_other * p_vec.x); } +bool PinJoint2DSW::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-P, rA); + } + if (B && dynamic_B) { + B->apply_impulse(P, rB); + } + + return true; +} + void PinJoint2DSW::solve(real_t p_step) { // compute relative velocity Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity()); @@ -174,8 +183,10 @@ void PinJoint2DSW::solve(real_t p_step) { Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); - A->apply_impulse(-impulse, rA); - if (B) { + if (dynamic_A) { + A->apply_impulse(-impulse, rA); + } + if (B && dynamic_B) { B->apply_impulse(impulse, rB); } @@ -262,14 +273,19 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { } bool GrooveJoint2DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } + Space2DSW *space = A->get_space(); + ERR_FAIL_COND_V(!space, false); + // calculate endpoints in worldspace Vector2 ta = A->get_transform().xform(A_groove_1); Vector2 tb = A->get_transform().xform(A_groove_2); - Space2DSW *space = A->get_space(); // calculate axis Vector2 n = -(tb - ta).orthogonal().normalized(); @@ -308,14 +324,22 @@ bool GrooveJoint2DSW::setup(real_t p_step) { real_t _b = get_bias(); gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).clamped(get_max_bias()); - // apply accumulated impulse - A->apply_impulse(-jn_acc, rA); - B->apply_impulse(jn_acc, rB); - correct = true; return true; } +bool GrooveJoint2DSW::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-jn_acc, rA); + } + if (dynamic_B) { + B->apply_impulse(jn_acc, rB); + } + + return true; +} + void GrooveJoint2DSW::solve(real_t p_step) { // compute impulse Vector2 vr = relative_velocity(A, B, rA, rB); @@ -328,8 +352,12 @@ void GrooveJoint2DSW::solve(real_t p_step) { j = jn_acc - jOld; - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } } GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) : @@ -351,7 +379,10 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_ ////////////////////////////////////////////// bool DampedSpringJoint2DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer2D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -373,12 +404,21 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) { target_vrn = 0.0f; v_coef = 1.0f - Math::exp(-damping * (p_step)*k); - // apply spring force + // Calculate spring force. real_t f_spring = (rest_length - dist) * stiffness; - Vector2 j = n * f_spring * (p_step); + j = n * f_spring * (p_step); - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + return true; +} + +bool DampedSpringJoint2DSW::pre_solve(real_t p_step) { + // Apply spring force. + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } return true; } @@ -393,8 +433,12 @@ void DampedSpringJoint2DSW::solve(real_t p_step) { target_vrn = vrn + v_damp; Vector2 j = n * v_damp * n_mass; - A->apply_impulse(-j, rA); - B->apply_impulse(j, rB); + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } } void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h index 628de972ae..ccc5c585a0 100644 --- a/servers/physics_2d/joints_2d_sw.h +++ b/servers/physics_2d/joints_2d_sw.h @@ -39,6 +39,10 @@ class Joint2DSW : public Constraint2DSW { real_t bias; real_t max_bias; +protected: + bool dynamic_A = false; + bool dynamic_B = false; + public: _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } _FORCE_INLINE_ real_t get_max_force() const { return max_force; } @@ -49,8 +53,9 @@ public: _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } - virtual bool setup(real_t p_step) { return false; } - virtual void solve(real_t p_step) {} + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return false; } + virtual void solve(real_t p_step) override {} void copy_settings_from(Joint2DSW *p_joint); @@ -90,10 +95,11 @@ class PinJoint2DSW : public Joint2DSW { real_t softness; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_PIN; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); real_t get_param(PhysicsServer2D::PinJointParam p_param) const; @@ -126,10 +132,11 @@ class GrooveJoint2DSW : public Joint2DSW { bool correct; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_GROOVE; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b); }; @@ -153,15 +160,17 @@ class DampedSpringJoint2DSW : public Joint2DSW { Vector2 rA, rB; Vector2 n; + Vector2 j; real_t n_mass; real_t target_vrn; real_t v_coef; public: - virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 406d750776..b8cb4cddc5 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -29,45 +29,59 @@ /*************************************************************************/ #include "step_2d_sw.h" + #include "core/os/os.h" #define BODY_ISLAND_COUNT_RESERVE 128 #define BODY_ISLAND_SIZE_RESERVE 512 #define ISLAND_COUNT_RESERVE 128 #define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) { p_body->set_island_step(_step); - p_body_island.push_back(p_body); - // Faster with reversed iterations. - for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().back(); E; E = E->prev()) { - Constraint2DSW *c = (Constraint2DSW *)E->get().first; - if (c->get_island_step() == _step) { - continue; //already processed + if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (const List<Pair<Constraint2DSW *, int>>::Element *E = p_body->get_constraint_list().front(); E; E = E->next()) { + Constraint2DSW *constraint = (Constraint2DSW *)E->get().first; + if (constraint->get_island_step() == _step) { + continue; // Already processed. } - c->set_island_step(_step); - p_constraint_island.push_back(c); + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + all_constraints.push_back(constraint); - for (int i = 0; i < c->get_body_count(); i++) { + for (int i = 0; i < constraint->get_body_count(); i++) { if (i == E->get().second) { continue; } - Body2DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - continue; //no go + Body2DSW *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. } - _populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island); + _populate_island(other_body, p_body_island, p_constraint_island); } } } -void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta) { +void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + Constraint2DSW *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const { uint32_t constraint_count = p_constraint_island.size(); uint32_t valid_constraint_count = 0; for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { Constraint2DSW *constraint = p_constraint_island[constraint_index]; - if (p_constraint_island[constraint_index]->setup(p_delta)) { + if (p_constraint_island[constraint_index]->pre_solve(delta)) { // Keep this constraint for solving. p_constraint_island[valid_constraint_count++] = constraint; } @@ -75,27 +89,25 @@ void Step2DSW::_setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, p_constraint_island.resize(valid_constraint_count); } -void Step2DSW::_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta) { - for (int i = 0; i < p_iterations; i++) { - uint32_t constraint_count = p_constraint_island.size(); +void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const { + const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index]; + + for (int i = 0; i < iterations; i++) { + uint32_t constraint_count = constraint_island.size(); for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - p_constraint_island[constraint_index]->solve(p_delta); + constraint_island[constraint_index]->solve(delta); } } } -void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta) { +void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const { bool can_sleep = true; uint32_t body_count = p_body_island.size(); for (uint32_t body_index = 0; body_index < body_count; ++body_index) { Body2DSW *body = p_body_island[body_index]; - if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - continue; // Ignore for static. - } - - if (!body->sleep_test(p_delta)) { + if (!body->sleep_test(delta)) { can_sleep = false; } } @@ -104,10 +116,6 @@ void Step2DSW::_check_suspend(const LocalVector<Body2DSW *> &p_body_island, real for (uint32_t body_index = 0; body_index < body_count; ++body_index) { Body2DSW *body = p_body_island[body_index]; - if (body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { - continue; // Ignore for static. - } - bool active = body->is_active(); if (active == can_sleep) { @@ -121,6 +129,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + iterations = p_iterations; + delta = p_delta; + const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list(); /* INTEGRATE FORCES */ @@ -145,12 +156,39 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* GENERATE CONSTRAINT ISLANDS */ + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + Constraint2DSW *constraint = E->get(); + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ b = body_list->first(); uint32_t body_island_count = 0; - uint32_t island_count = 0; while (b) { Body2DSW *body = b->self(); @@ -174,7 +212,9 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { _populate_island(body, body_island, constraint_island); - body_islands.push_back(body_island); + if (body_island.is_empty()) { + --body_island_count; + } if (constraint_island.is_empty()) { --island_count; @@ -185,37 +225,16 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { p_space->set_island_count((int)island_count); - const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list(); - - while (aml.first()) { - for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint2DSW *c = E->get(); - if (c->get_island_step() == _step) { - continue; - } - c->set_island_step(_step); - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); - } - LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.push_back(c); - } - p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here - } - { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); profile_begtime = profile_endtime; } - /* SETUP CONSTRAINT ISLANDS */ + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - _setup_island(constraint_islands[island_index], p_delta); - } + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -223,10 +242,21 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* SOLVE CONSTRAINT ISLANDS */ + /* PRE-SOLVE CONSTRAINT ISLANDS */ + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - _solve_island(constraint_islands[island_index], p_iterations, p_delta); + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // Warning: _solve_island modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + if (island_count > 1) { + work_pool.do_work(island_count, this, &Step2DSW::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); } { //profile @@ -247,7 +277,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { /* SLEEP / WAKE UP ISLANDS */ for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { - _check_suspend(body_islands[island_index], p_delta); + _check_suspend(body_islands[island_index]); } { //profile @@ -256,6 +286,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { //profile_begtime=profile_endtime; } + all_constraints.clear(); + p_space->update(); p_space->unlock(); _step++; @@ -266,4 +298,11 @@ Step2DSW::Step2DSW() { body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); + + work_pool.init(); +} + +Step2DSW::~Step2DSW() { + work_pool.finish(); } diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h index 5af4a36f52..c51fd73a79 100644 --- a/servers/physics_2d/step_2d_sw.h +++ b/servers/physics_2d/step_2d_sw.h @@ -34,21 +34,30 @@ #include "space_2d_sw.h" #include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" class Step2DSW { uint64_t _step; + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + LocalVector<LocalVector<Body2DSW *>> body_islands; LocalVector<LocalVector<Constraint2DSW *>> constraint_islands; + LocalVector<Constraint2DSW *> all_constraints; void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island); - void _setup_island(LocalVector<Constraint2DSW *> &p_constraint_island, real_t p_delta); - void _solve_island(LocalVector<Constraint2DSW *> &p_constraint_island, int p_iterations, real_t p_delta); - void _check_suspend(const LocalVector<Body2DSW *> &p_body_island, real_t p_delta); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; + void _check_suspend(LocalVector<Body2DSW *> &p_body_island) const; public: void step(Space2DSW *p_space, real_t p_delta, int p_iterations); Step2DSW(); + ~Step2DSW(); }; #endif // STEP_2D_SW_H diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp index 4de5f1ba47..2073df7dee 100644 --- a/servers/physics_3d/area_pair_3d_sw.cpp +++ b/servers/physics_3d/area_pair_3d_sw.cpp @@ -40,31 +40,48 @@ bool AreaPair3DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->add_area(area); - } - if (area->has_monitor_callback()) { - area->add_body_to_query(body, body_shape, area_shape); - } - - } else { - if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { - body->remove_area(area); - } - if (area->has_monitor_callback()) { - area->remove_body_from_query(body, body_shape, area_shape); - } + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + process_collision = true; + } else if (area->has_monitor_callback()) { + process_collision = true; } colliding = result; } - return false; //never do any post solving + return process_collision; +} + +bool AreaPair3DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. } void AreaPair3DSW::solve(real_t p_step) { + // Nothing to do. } AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) { @@ -72,7 +89,6 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, area = p_area; body_shape = p_body_shape; area_shape = p_area_shape; - colliding = false; body->add_constraint(this, 0); area->add_constraint(this); if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { @@ -103,33 +119,48 @@ bool Area2Pair3DSW::setup(real_t p_step) { result = true; } + process_collision = false; if (result != colliding) { - if (result) { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - area_b->add_area_to_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->add_area_to_query(area_b, shape_b, shape_a); - } - - } else { - if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { - area_b->remove_area_from_query(area_a, shape_a, shape_b); - } - - if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { - area_a->remove_area_from_query(area_b, shape_b, shape_a); - } + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + process_collision = true; + } else if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + process_collision = true; } colliding = result; } - return false; //never do any post solving + return process_collision; +} + +bool Area2Pair3DSW::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + area_b->add_area_to_query(area_a, shape_a, shape_b); + } + + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } + } else { + if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + + if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + return false; // Never do any post solving. } void Area2Pair3DSW::solve(real_t p_step) { + // Nothing to do. } Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) { @@ -137,7 +168,6 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area area_b = p_area_b; shape_a = p_shape_a; shape_b = p_shape_b; - colliding = false; area_a->add_constraint(this); area_b->add_constraint(this); } diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h index fbdaa25cbb..596d893082 100644 --- a/servers/physics_3d/area_pair_3d_sw.h +++ b/servers/physics_3d/area_pair_3d_sw.h @@ -40,11 +40,13 @@ class AreaPair3DSW : public Constraint3DSW { Area3DSW *area; int body_shape; int area_shape; - bool colliding; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape); ~AreaPair3DSW(); @@ -55,11 +57,13 @@ class Area2Pair3DSW : public Constraint3DSW { Area3DSW *area_b; int shape_a; int shape_b; - bool colliding; + bool colliding = false; + bool process_collision = false; public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b); ~Area2Pair3DSW(); diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp index d54345821d..4357c474e4 100644 --- a/servers/physics_3d/body_3d_sw.cpp +++ b/servers/physics_3d/body_3d_sw.cpp @@ -145,31 +145,17 @@ void Body3DSW::set_active(bool p_active) { } active = p_active; - if (!p_active) { - if (get_space()) { - get_space()->body_remove_from_active_list(&active_list); - } - } else { + + if (active) { if (mode == PhysicsServer3D::BODY_MODE_STATIC) { - return; //static bodies can't become active - } - if (get_space()) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { get_space()->body_add_to_active_list(&active_list); } - - //still_time=0; + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); } - /* - if (!space) - return; - - for(int i=0;i<get_shape_count();i++) { - Shape &s=shapes[i]; - if (s.bpid>0) { - get_space()->get_broadphase()->set_active(s.bpid,active); - } - } -*/ } void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) { @@ -392,13 +378,6 @@ void Body3DSW::set_space(Space3DSW *p_space) { if (active) { get_space()->body_add_to_active_list(&active_list); } - /* - _update_queries(); - if (is_active()) { - active=false; - set_active(true); - } - */ } first_integration = true; diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 28c854466f..cdb3da665e 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -212,14 +212,16 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) { } bool BodyPair3DSW::setup(real_t p_step) { - //cannot collide + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { collided = false; return false; } - bool report_contacts_only = false; - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + report_contacts_only = false; + if (!dynamic_A && !dynamic_B) { if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { report_contacts_only = true; } else { @@ -237,7 +239,7 @@ bool BodyPair3DSW::setup(real_t p_step) { validate_contacts(); - Vector3 offset_A = A->get_transform().get_origin(); + const Vector3 &offset_A = A->get_transform().get_origin(); Transform xform_Au = Transform(A->get_transform().basis, Vector3()); Transform xform_A = xform_Au * A->get_shape_transform(shape_A); @@ -248,27 +250,37 @@ bool BodyPair3DSW::setup(real_t p_step) { Shape3DSW *shape_A_ptr = A->get_shape(shape_A); Shape3DSW *shape_B_ptr = B->get_shape(shape_B); - bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - this->collided = collided; + collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); if (!collided) { //test ccd (currently just a raycast) - if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (A->is_continuous_collision_detection_enabled() && dynamic_A && !dynamic_B) { _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); } - if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (B->is_continuous_collision_detection_enabled() && dynamic_B && !dynamic_A) { _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); } return false; } + return true; +} + +bool BodyPair3DSW::pre_solve(real_t p_step) { + if (!collided) { + return false; + } + real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = (real_t)0.3; + Shape3DSW *shape_A_ptr = A->get_shape(shape_A); + Shape3DSW *shape_B_ptr = B->get_shape(shape_B); + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias() == 0) { bias = shape_B_ptr->get_custom_bias(); @@ -283,22 +295,26 @@ bool BodyPair3DSW::setup(real_t p_step) { bool do_process = false; + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; c.active = false; - Vector3 global_A = xform_Au.xform(c.local_A); - Vector3 global_B = xform_Bu.xform(c.local_B); + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; - real_t depth = c.normal.dot(global_A - global_B); + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); if (depth <= 0) { continue; } #ifdef DEBUG_ENABLED - if (space->is_debugging_contacts()) { + const Vector3 &offset_A = A->get_transform().get_origin(); space->add_debug_contact(global_A + offset_A); space->add_debug_contact(global_B + offset_A); } @@ -338,8 +354,12 @@ bool BodyPair3DSW::setup(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); - B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); + } c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; @@ -361,6 +381,8 @@ void BodyPair3DSW::solve(real_t p_step) { return; } + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + for (int i = 0; i < contact_count; i++) { Contact &c = contacts[i]; if (!c.active) { @@ -384,8 +406,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); - B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); + if (dynamic_A) { + A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); + } + if (dynamic_B) { + B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); + } crbA = A->get_biased_angular_velocity().cross(c.rA); crbB = B->get_biased_angular_velocity().cross(c.rB); @@ -400,8 +426,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); - B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + if (dynamic_A) { + A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); + } + if (dynamic_B) { + B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + } } c.active = true; @@ -421,8 +451,12 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - A->apply_impulse(-j, c.rA + A->get_center_of_mass()); - B->apply_impulse(j, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-j, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(j, c.rB + B->get_center_of_mass()); + } c.active = true; } @@ -464,8 +498,12 @@ void BodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); - B->apply_impulse(jt, c.rB + B->get_center_of_mass()); + if (dynamic_A) { + A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); + } + if (dynamic_B) { + B->apply_impulse(jt, c.rB + B->get_center_of_mass()); + } c.active = true; } @@ -481,8 +519,6 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh space = A->get_space(); A->add_constraint(this, 0); B->add_constraint(this, 1); - contact_count = 0; - collided = false; } BodyPair3DSW::~BodyPair3DSW() { @@ -564,6 +600,8 @@ void BodySoftBodyPair3DSW::validate_contacts() { } bool BodySoftBodyPair3DSW::setup(real_t p_step) { + body_dynamic = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + if (!body->test_collision_mask(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { collided = false; return false; @@ -585,12 +623,22 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { Shape3DSW *shape_A_ptr = body->get_shape(body_shape); Shape3DSW *shape_B_ptr = soft_body->get_shape(0); - bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); - this->collided = collided; + collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); + + return collided; +} + +bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { + if (!collided) { + return false; + } real_t max_penetration = space->get_contact_max_allowed_penetration(); real_t bias = (real_t)0.3; + + Shape3DSW *shape_A_ptr = body->get_shape(body_shape); + if (shape_A_ptr->get_custom_bias()) { bias = shape_A_ptr->get_custom_bias(); } @@ -599,6 +647,8 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { bool do_process = false; + const Transform &transform_A = body->get_transform(); + uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; @@ -609,10 +659,10 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { continue; } - Vector3 global_A = xform_Au.xform(c.local_A); + Vector3 global_A = transform_A.xform(c.local_A); Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; - - real_t depth = c.normal.dot(global_A - global_B); + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); if (depth <= 0) { continue; @@ -629,7 +679,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { } #endif - c.rA = global_A - xform_Au.origin - body->get_center_of_mass(); + c.rA = global_A - transform_A.origin - body->get_center_of_mass(); c.rB = global_B; if (body->can_report_contacts()) { @@ -637,7 +687,7 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA); } - if (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { + if (body_dynamic) { body->set_active(true); } @@ -651,7 +701,9 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) { c.depth = depth; Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; - body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); + if (body_dynamic) { + body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, j_vec); c.acc_bias_impulse = 0; c.acc_bias_impulse_center_of_mass = 0; @@ -675,6 +727,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { return; } + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + uint32_t contact_count = contacts.size(); for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { Contact &c = contacts[contact_index]; @@ -697,7 +751,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), MAX_BIAS_ROTATION / p_step); + if (body_dynamic) { + body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av); + } soft_body->apply_node_bias_impulse(c.index_B, jb); crbA = body->get_biased_angular_velocity().cross(c.rA); @@ -712,7 +768,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); - body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); + if (body_dynamic) { + body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); + } soft_body->apply_node_bias_impulse(c.index_B, jb_com); } @@ -732,7 +790,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); - body->apply_impulse(-j, c.rA + body->get_center_of_mass()); + if (body_dynamic) { + body->apply_impulse(-j, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, j); c.active = true; @@ -773,7 +833,9 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { jt = c.acc_tangent_impulse - jtOld; - body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); + if (body_dynamic) { + body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); + } soft_body->apply_node_impulse(c.index_B, jt); c.active = true; @@ -781,7 +843,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) { } } -BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) { +BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) : + BodyContact3DSW(&body, 1) { body = p_A; soft_body = p_B; body_shape = p_shape_A; diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h index 74dddfa6aa..3f425ba2d7 100644 --- a/servers/physics_3d/body_pair_3d_sw.h +++ b/servers/physics_3d/body_pair_3d_sw.h @@ -57,9 +57,9 @@ protected: }; Vector3 sep_axis; - bool collided; + bool collided = false; - Space3DSW *space; + Space3DSW *space = nullptr; BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : Constraint3DSW(p_body_ptr, p_body_count) { @@ -77,16 +77,21 @@ class BodyPair3DSW : public BodyContact3DSW { Body3DSW *B; }; - Body3DSW *_arr[2]; + Body3DSW *_arr[2] = { nullptr, nullptr }; }; - int shape_A; - int shape_B; + int shape_A = 0; + int shape_B = 0; + + bool dynamic_A = false; + bool dynamic_B = false; + + bool report_contacts_only = false; Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection Contact contacts[MAX_CONTACTS]; - int contact_count; + int contact_count = 0; static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); @@ -96,18 +101,21 @@ class BodyPair3DSW : public BodyContact3DSW { bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B); public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B); ~BodyPair3DSW(); }; class BodySoftBodyPair3DSW : public BodyContact3DSW { - Body3DSW *body; - SoftBody3DSW *soft_body; + Body3DSW *body = nullptr; + SoftBody3DSW *soft_body = nullptr; - int body_shape; + int body_shape = 0; + + bool body_dynamic = false; LocalVector<Contact> contacts; @@ -118,8 +126,12 @@ class BodySoftBodyPair3DSW : public BodyContact3DSW { void validate_contacts(); public: - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const override { return soft_body; } + virtual int get_soft_body_count() const override { return 1; } BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B); ~BodySoftBodyPair3DSW(); diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/constraint_3d_sw.h index 16a31e167d..7b44726ef5 100644 --- a/servers/physics_3d/constraint_3d_sw.h +++ b/servers/physics_3d/constraint_3d_sw.h @@ -31,7 +31,8 @@ #ifndef CONSTRAINT_SW_H #define CONSTRAINT_SW_H -#include "body_3d_sw.h" +class Body3DSW; +class SoftBody3DSW; class Constraint3DSW { Body3DSW **_body_ptr; @@ -61,6 +62,9 @@ public: _FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ int get_body_count() const { return _body_count; } + virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const { return nullptr; } + virtual int get_soft_body_count() const { return 0; } + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ int get_priority() const { return priority; } @@ -68,6 +72,7 @@ public: _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0; virtual ~Constraint3DSW() {} diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp index 167f797bfe..e9efddf165 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp @@ -109,7 +109,10 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans } bool ConeTwistJoint3DSW::setup(real_t p_timestep) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -265,8 +268,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } } } @@ -287,8 +294,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { Vector3 impulse = m_swingAxis * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } // solve twist limit @@ -303,8 +314,12 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) { Vector3 impulse = m_twistAxis * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } } } diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h index 4e4d4e7f0c..b871ea50db 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -102,10 +102,10 @@ public: bool m_solveSwingLimit; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &rbAFrame, const Transform &rbBFrame); diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp index a86e8b4e76..7c504764a7 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp @@ -82,7 +82,7 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) { real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( real_t timeStep, Vector3 &axis, real_t jacDiagABInv, - Body3DSW *body0, Body3DSW *body1) { + Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) { if (!needApplyTorques()) { return 0.0f; } @@ -138,8 +138,10 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( Vector3 motorImp = clippedMotorImpulse * axis; - body0->apply_torque_impulse(motorImp); - if (body1) { + if (p_body0_dynamic) { + body0->apply_torque_impulse(motorImp); + } + if (body1 && p_body1_dynamic) { body1->apply_torque_impulse(-motorImp); } @@ -154,6 +156,7 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( real_t jacDiagABInv, Body3DSW *body1, const Vector3 &pointInA, Body3DSW *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, int limit_index, const Vector3 &axis_normal_on_a, const Vector3 &anchorPos) { @@ -205,8 +208,12 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis( normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1->apply_impulse(impulse_vector, rel_pos1); - body2->apply_impulse(-impulse_vector, rel_pos2); + if (p_body1_dynamic) { + body1->apply_impulse(impulse_vector, rel_pos1); + } + if (p_body2_dynamic) { + body2->apply_impulse(-impulse_vector, rel_pos2); + } return normalImpulse; } @@ -303,7 +310,10 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) { } bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -384,6 +394,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { jacDiagABInv, A, pointInA, B, pointInB, + dynamic_A, dynamic_B, i, linear_axis, m_AnchorPos); } } @@ -398,7 +409,7 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) { angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); - m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B); + m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B); } } } diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h index d61a033231..8af76cefc2 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -120,7 +120,7 @@ public: int testLimitValue(real_t test_value); //! apply the correction impulses for two bodies - real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1); + real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic); }; class G6DOFTranslationalLimitMotor3DSW { @@ -166,6 +166,7 @@ public: real_t jacDiagABInv, Body3DSW *body1, const Vector3 &pointInA, Body3DSW *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, int limit_index, const Vector3 &axis_normal_on_a, const Vector3 &anchorPos); @@ -234,10 +235,10 @@ protected: public: Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } - virtual bool setup(real_t p_timestep); - virtual void solve(real_t p_timestep); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; //! Calcs global transform of the offsets /*! diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp index 90b82f4680..bb8858c28a 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.cpp @@ -155,7 +155,10 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo } bool HingeJoint3DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -279,8 +282,12 @@ void HingeJoint3DSW::solve(real_t p_step) { real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } } } @@ -322,8 +329,12 @@ void HingeJoint3DSW::solve(real_t p_step) { angularError *= (real_t(1.) / denom2) * relaxation; } - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } // solve limit if (m_solveLimit) { @@ -337,8 +348,12 @@ void HingeJoint3DSW::solve(real_t p_step) { impulseMag = m_accLimitImpulse - temp; Vector3 impulse = axisA * impulseMag * m_limitSign; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } } } @@ -359,8 +374,12 @@ void HingeJoint3DSW::solve(real_t p_step) { clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; Vector3 motorImp = clippedMotorImpulse * axisA; - A->apply_torque_impulse(motorImp + angularLimit); - B->apply_torque_impulse(-motorImp - angularLimit); + if (dynamic_A) { + A->apply_torque_impulse(motorImp + angularLimit); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp - angularLimit); + } } } } diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index b6117aa0bc..2100f5de44 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -96,10 +96,10 @@ class HingeJoint3DSW : public Joint3DSW { real_t m_appliedImpulse; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; real_t get_hinge_angle(); diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/pin_joint_3d_sw.cpp index 75d87992d1..8eb84d1c2f 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/pin_joint_3d_sw.cpp @@ -50,7 +50,10 @@ subject to the following restrictions: #include "pin_joint_3d_sw.h" bool PinJoint3DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -123,8 +126,12 @@ void PinJoint3DSW::solve(real_t p_step) { m_appliedImpulse += impulse; Vector3 impulse_vector = normal * impulse; - A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); - B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } normal[i] = 0; } diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index 1875983527..3d91452850 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -74,10 +74,10 @@ class PinJoint3DSW : public Joint3DSW { Vector3 m_pivotInB; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; } - virtual bool setup(real_t p_step); - virtual void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value); real_t get_param(PhysicsServer3D::PinJointParam p_param) const; diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/slider_joint_3d_sw.cpp index 2e1ee8e770..8bd1951311 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp +++ b/servers/physics_3d/joints/slider_joint_3d_sw.cpp @@ -127,7 +127,10 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform & //----------------------------------------------------------------------------- bool SliderJoint3DSW::setup(real_t p_step) { - if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { return false; } @@ -200,8 +203,12 @@ void SliderJoint3DSW::solve(real_t p_step) { // calcutate and apply impulse real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; Vector3 impulse_vector = normal * normalImpulse; - A->apply_impulse(impulse_vector, m_relPosA); - B->apply_impulse(-impulse_vector, m_relPosB); + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } if (m_poweredLinMotor && (!i)) { // apply linear motor if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { real_t desiredMotorVel = m_targetLinMotorVelocity; @@ -221,8 +228,12 @@ void SliderJoint3DSW::solve(real_t p_step) { m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; - A->apply_impulse(impulse_vector, m_relPosA); - B->apply_impulse(-impulse_vector, m_relPosB); + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } } } } @@ -256,8 +267,12 @@ void SliderJoint3DSW::solve(real_t p_step) { angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; } // apply impulse - A->apply_torque_impulse(-velrelOrthog + angularError); - B->apply_torque_impulse(velrelOrthog - angularError); + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } real_t impulseMag; //solve angular limits if (m_solveAngLim) { @@ -268,8 +283,12 @@ void SliderJoint3DSW::solve(real_t p_step) { impulseMag *= m_kAngle * m_softnessDirAng; } Vector3 impulse = axisA * impulseMag; - A->apply_torque_impulse(impulse); - B->apply_torque_impulse(-impulse); + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } //apply angular motor if (m_poweredAngMotor) { if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { @@ -294,8 +313,12 @@ void SliderJoint3DSW::solve(real_t p_step) { m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse Vector3 motorImp = angImpulse * axisA; - A->apply_torque_impulse(motorImp); - B->apply_torque_impulse(-motorImp); + if (dynamic_A) { + A->apply_torque_impulse(motorImp); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp); + } } } } // SliderJointSW::solveConstraint() diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index f52f6ace27..ef5891d0f9 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -240,10 +240,10 @@ public: void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value); real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; - bool setup(real_t p_step); - void solve(real_t p_step); + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_SLIDER; } + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; } }; #endif // SLIDER_JOINT_SW_H diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h index 225a71aca9..e2514674ea 100644 --- a/servers/physics_3d/joints_3d_sw.h +++ b/servers/physics_3d/joints_3d_sw.h @@ -35,9 +35,14 @@ #include "constraint_3d_sw.h" class Joint3DSW : public Constraint3DSW { +protected: + bool dynamic_A = false; + bool dynamic_B = false; + public: - virtual bool setup(real_t p_step) { return false; } - virtual void solve(real_t p_step) {} + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return true; } + virtual void solve(real_t p_step) override {} void copy_settings_from(Joint3DSW *p_joint) { set_self(p_joint->get_self()); diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h index 6c0451ff45..98e554218b 100644 --- a/servers/physics_3d/soft_body_3d_sw.h +++ b/servers/physics_3d/soft_body_3d_sw.h @@ -106,6 +106,8 @@ class SoftBody3DSW : public CollisionObject3DSW { VSet<RID> exceptions; + uint64_t island_step = 0; + public: SoftBody3DSW(); @@ -124,6 +126,9 @@ public: _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; } + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + virtual void set_space(Space3DSW *p_space); void set_mesh(const Ref<Mesh> &p_mesh); diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp index 06f3227eab..ba18bac611 100644 --- a/servers/physics_3d/step_3d_sw.cpp +++ b/servers/physics_3d/step_3d_sw.cpp @@ -37,39 +37,90 @@ #define BODY_ISLAND_SIZE_RESERVE 512 #define ISLAND_COUNT_RESERVE 128 #define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { p_body->set_island_step(_step); - p_body_island.push_back(p_body); - // Faster with reversed iterations. - for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().back(); E; E = E->prev()) { - Constraint3DSW *c = (Constraint3DSW *)E->key(); - if (c->get_island_step() == _step) { - continue; //already processed + if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { + // Only dynamic bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { + Constraint3DSW *constraint = (Constraint3DSW *)E->key(); + if (constraint->get_island_step() == _step) { + continue; // Already processed. } - c->set_island_step(_step); - p_constraint_island.push_back(c); + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); - for (int i = 0; i < c->get_body_count(); i++) { + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { if (i == E->get()) { continue; } - Body3DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - continue; //no go + Body3DSW *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(other_body, p_body_island, p_constraint_island); + } + + // Find connected soft bodies. + for (int i = 0; i < constraint->get_soft_body_count(); i++) { + SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i); + if (soft_body->get_island_step() == _step) { + continue; // Already processed. } - _populate_island(c->get_body_ptr()[i], p_body_island, p_constraint_island); + _populate_island_soft_body(soft_body, p_body_island, p_constraint_island); } } } -void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta) { +void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { + p_soft_body->set_island_step(_step); + + for (Set<Constraint3DSW *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { + Constraint3DSW *constraint = (Constraint3DSW *)E->get(); + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { + Body3DSW *body = constraint->get_body_ptr()[i]; + if (body->get_island_step() == _step) { + continue; // Already processed. + } + if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(body, p_body_island, p_constraint_island); + } + } +} + +void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { + Constraint3DSW *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const { uint32_t constraint_count = p_constraint_island.size(); uint32_t valid_constraint_count = 0; for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { Constraint3DSW *constraint = p_constraint_island[constraint_index]; - if (p_constraint_island[constraint_index]->setup(p_delta)) { + if (p_constraint_island[constraint_index]->pre_solve(delta)) { // Keep this constraint for solving. p_constraint_island[valid_constraint_count++] = constraint; } @@ -77,15 +128,17 @@ void Step3DSW::_setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, p_constraint_island.resize(valid_constraint_count); } -void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta) { +void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) { + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index]; + int current_priority = 1; - uint32_t constraint_count = p_constraint_island.size(); + uint32_t constraint_count = constraint_island.size(); while (constraint_count > 0) { - for (int i = 0; i < p_iterations; i++) { + for (int i = 0; i < iterations; i++) { // Go through all iterations. for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - p_constraint_island[constraint_index]->solve(p_delta); + constraint_island[constraint_index]->solve(delta); } } @@ -93,28 +146,24 @@ void Step3DSW::_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, uint32_t priority_constraint_count = 0; ++current_priority; for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { - Constraint3DSW *constraint = p_constraint_island[constraint_index]; + Constraint3DSW *constraint = constraint_island[constraint_index]; if (constraint->get_priority() >= current_priority) { // Keep this constraint for the next iteration. - p_constraint_island[priority_constraint_count++] = constraint; + constraint_island[priority_constraint_count++] = constraint; } } constraint_count = priority_constraint_count; } } -void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta) { +void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const { bool can_sleep = true; uint32_t body_count = p_body_island.size(); for (uint32_t body_index = 0; body_index < body_count; ++body_index) { Body3DSW *body = p_body_island[body_index]; - if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - continue; // Ignore for static. - } - - if (!body->sleep_test(p_delta)) { + if (!body->sleep_test(delta)) { can_sleep = false; } } @@ -123,10 +172,6 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island, real for (uint32_t body_index = 0; body_index < body_count; ++body_index) { Body3DSW *body = p_body_island[body_index]; - if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC || body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { - continue; // Ignore for static. - } - bool active = body->is_active(); if (active == can_sleep) { @@ -140,6 +185,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { p_space->setup(); //update inertias, etc + iterations = p_iterations; + delta = p_delta; + const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list(); const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list(); @@ -175,12 +223,39 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* GENERATE CONSTRAINT ISLANDS */ + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { + Constraint3DSW *constraint = E->get(); + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ b = body_list->first(); uint32_t body_island_count = 0; - uint32_t island_count = 0; while (b) { Body3DSW *body = b->self(); @@ -204,7 +279,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { _populate_island(body, body_island, constraint_island); - body_islands.push_back(body_island); + if (body_island.is_empty()) { + --body_island_count; + } if (constraint_island.is_empty()) { --island_count; @@ -213,58 +290,54 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { b = b->next(); } - p_space->set_island_count((int)island_count); + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */ - const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list(); + sb = soft_body_list->first(); + while (sb) { + SoftBody3DSW *soft_body = sb->self(); - while (aml.first()) { - for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *c = E->get(); - if (c->get_island_step() == _step) { - continue; + if (soft_body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); } - c->set_island_step(_step); + LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + ++island_count; if (constraint_islands.size() < island_count) { constraint_islands.resize(island_count); } LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; constraint_island.clear(); - constraint_island.push_back(c); - } - p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here - } + constraint_island.reserve(ISLAND_SIZE_RESERVE); - sb = soft_body_list->first(); - while (sb) { - for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) { - Constraint3DSW *c = E->get(); - if (c->get_island_step() == _step) { - continue; + _populate_island_soft_body(soft_body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; } - c->set_island_step(_step); - ++island_count; - if (constraint_islands.size() < island_count) { - constraint_islands.resize(island_count); + + if (constraint_island.is_empty()) { + --island_count; } - LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; - constraint_island.clear(); - constraint_island.push_back(c); } sb = sb->next(); } + p_space->set_island_count((int)island_count); + { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); profile_begtime = profile_endtime; } - /* SETUP CONSTRAINT ISLANDS */ + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ - for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - _setup_island(constraint_islands[island_index], p_delta); - } + uint32_t total_contraint_count = all_constraints.size(); + work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr); { //profile profile_endtime = OS::get_singleton()->get_ticks_usec(); @@ -272,12 +345,21 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } - /* SOLVE CONSTRAINT ISLANDS */ + /* PRE-SOLVE CONSTRAINT ISLANDS */ + // Warning: This doesn't run on threads, because it involves thread-unsafe processing. for (uint32_t island_index = 0; island_index < island_count; ++island_index) { - // Warning: _solve_island modifies the constraint islands for optimization purpose, - // their content is not reliable after these calls and shouldn't be used anymore. - _solve_island(constraint_islands[island_index], p_iterations, p_delta); + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // Warning: _solve_island modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + if (island_count > 1) { + work_pool.do_work(island_count, this, &Step3DSW::_solve_island, nullptr); + } else if (island_count > 0) { + _solve_island(0); } { //profile @@ -298,7 +380,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { /* SLEEP / WAKE UP ISLANDS */ for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { - _check_suspend(body_islands[island_index], p_delta); + _check_suspend(body_islands[island_index]); } /* UPDATE SOFT BODY CONSTRAINTS */ @@ -315,6 +397,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { profile_begtime = profile_endtime; } + all_constraints.clear(); + p_space->update(); p_space->unlock(); _step++; @@ -325,4 +409,11 @@ Step3DSW::Step3DSW() { body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); + + work_pool.init(); +} + +Step3DSW::~Step3DSW() { + work_pool.finish(); } diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/step_3d_sw.h index f406c35c3a..9c60004b24 100644 --- a/servers/physics_3d/step_3d_sw.h +++ b/servers/physics_3d/step_3d_sw.h @@ -34,21 +34,31 @@ #include "space_3d_sw.h" #include "core/templates/local_vector.h" +#include "core/templates/thread_work_pool.h" class Step3DSW { uint64_t _step; + int iterations = 0; + real_t delta = 0.0; + + ThreadWorkPool work_pool; + LocalVector<LocalVector<Body3DSW *>> body_islands; LocalVector<LocalVector<Constraint3DSW *>> constraint_islands; + LocalVector<Constraint3DSW *> all_constraints; void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); - void _setup_island(LocalVector<Constraint3DSW *> &p_constraint_island, real_t p_delta); - void _solve_island(LocalVector<Constraint3DSW *> &p_constraint_island, int p_iterations, real_t p_delta); - void _check_suspend(const LocalVector<Body3DSW *> &p_body_island, real_t p_delta); + void _populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); + void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr); + void _check_suspend(const LocalVector<Body3DSW *> &p_body_island) const; public: void step(Space3DSW *p_space, real_t p_delta, int p_iterations); Step3DSW(); + ~Step3DSW(); }; #endif // STEP__SW_H |