diff options
300 files changed, 13066 insertions, 9643 deletions
diff --git a/core/SCsub b/core/SCsub index 45918fb520..c9f84a9a00 100644 --- a/core/SCsub +++ b/core/SCsub @@ -38,6 +38,9 @@ with open("script_encryption_key.gen.cpp", "w") as f: # Add required thirdparty code. + +thirdparty_obj = [] + env_thirdparty = env.Clone() env_thirdparty.disable_warnings() @@ -55,7 +58,7 @@ thirdparty_misc_sources = [ "clipper.cpp", ] thirdparty_misc_sources = [thirdparty_misc_dir + file for file in thirdparty_misc_sources] -env_thirdparty.add_source_files(env.core_sources, thirdparty_misc_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_misc_sources) # Zlib library, can be unbundled if env["builtin_zlib"]: @@ -81,14 +84,14 @@ if env["builtin_zlib"]: if env["target"] == "debug": env_thirdparty.Append(CPPDEFINES=["ZLIB_DEBUG"]) - env_thirdparty.add_source_files(env.core_sources, thirdparty_zlib_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_zlib_sources) # Minizip library, could be unbundled in theory # However, our version has some custom modifications, so it won't compile with the system one thirdparty_minizip_dir = "#thirdparty/minizip/" thirdparty_minizip_sources = ["ioapi.c", "unzip.c", "zip.c"] thirdparty_minizip_sources = [thirdparty_minizip_dir + file for file in thirdparty_minizip_sources] -env_thirdparty.add_source_files(env.core_sources, thirdparty_minizip_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_minizip_sources) # Zstd library, can be unbundled in theory # though we currently use some private symbols @@ -130,10 +133,14 @@ if env["builtin_zstd"]: # Also needed in main env includes will trigger warnings env.Append(CPPDEFINES=["ZSTD_STATIC_LINKING_ONLY"]) - env_thirdparty.add_source_files(env.core_sources, thirdparty_zstd_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_zstd_sources) + +env.core_sources += thirdparty_obj + + +# Godot source files -# Godot's own sources env.add_source_files(env.core_sources, "*.cpp") # Certificates @@ -185,3 +192,6 @@ SConscript("error/SCsub") # Build it all as a library lib = env.add_library("core", env.core_sources) env.Prepend(LIBS=[lib]) + +# Needed to force rebuilding the core files when the thirdparty code is updated. +env.Depends(lib, thirdparty_obj) diff --git a/core/core_constants.cpp b/core/core_constants.cpp index 02055012ad..8585078722 100644 --- a/core/core_constants.cpp +++ b/core/core_constants.cpp @@ -525,6 +525,7 @@ void register_global_constants() { BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_COLOR_NO_ALPHA); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_IMAGE_COMPRESS_LOSSY); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_IMAGE_COMPRESS_LOSSLESS); + BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_TYPE_STRING); BIND_CORE_ENUM_CONSTANT(PROPERTY_USAGE_STORAGE); BIND_CORE_ENUM_CONSTANT(PROPERTY_USAGE_EDITOR); diff --git a/core/crypto/SCsub b/core/crypto/SCsub index da4a9c9381..4f3104d84b 100644 --- a/core/crypto/SCsub +++ b/core/crypto/SCsub @@ -6,6 +6,7 @@ env_crypto = env.Clone() is_builtin = env["builtin_mbedtls"] has_module = env["module_mbedtls_enabled"] +thirdparty_obj = [] if is_builtin or not has_module: # Use our headers for builtin or if the module is not going to be compiled. @@ -35,6 +36,16 @@ if not has_module: "godot_core_mbedtls_platform.c", ] thirdparty_mbedtls_sources = [thirdparty_mbedtls_dir + file for file in thirdparty_mbedtls_sources] - env_thirdparty.add_source_files(env.core_sources, thirdparty_mbedtls_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_mbedtls_sources) + env.core_sources += thirdparty_obj -env_crypto.add_source_files(env.core_sources, "*.cpp") + +# Godot source files + +core_obj = [] + +env_crypto.add_source_files(core_obj, "*.cpp") +env.core_sources += core_obj + +# Needed to force rebuilding the core files when the thirdparty library is updated. +env.Depends(core_obj, thirdparty_obj) diff --git a/core/io/image.cpp b/core/io/image.cpp index 7c32c02701..56d84325b5 100644 --- a/core/io/image.cpp +++ b/core/io/image.cpp @@ -993,7 +993,7 @@ bool Image::is_size_po2() const { return uint32_t(width) == next_power_of_2(width) && uint32_t(height) == next_power_of_2(height); } -void Image::resize_to_po2(bool p_square) { +void Image::resize_to_po2(bool p_square, Interpolation p_interpolation) { ERR_FAIL_COND_MSG(!_can_modify(format), "Cannot resize in compressed or custom image formats."); int w = next_power_of_2(width); @@ -1008,7 +1008,7 @@ void Image::resize_to_po2(bool p_square) { } } - resize(w, h); + resize(w, h, p_interpolation); } void Image::resize(int p_width, int p_height, Interpolation p_interpolation) { @@ -2480,7 +2480,7 @@ void Image::blit_rect(const Ref<Image> &p_src, const Rect2 &p_src_rect, const Po ERR_FAIL_COND(format != p_src->format); ERR_FAIL_COND_MSG(!_can_modify(format), "Cannot blit_rect in compressed or custom image formats."); - Rect2i clipped_src_rect = Rect2i(0, 0, p_src->width, p_src->height).clip(p_src_rect); + Rect2i clipped_src_rect = Rect2i(0, 0, p_src->width, p_src->height).intersection(p_src_rect); if (p_dest.x < 0) { clipped_src_rect.position.x = ABS(p_dest.x); @@ -2494,7 +2494,7 @@ void Image::blit_rect(const Ref<Image> &p_src, const Rect2 &p_src_rect, const Po } Point2 src_underscan = Point2(MIN(0, p_src_rect.position.x), MIN(0, p_src_rect.position.y)); - Rect2i dest_rect = Rect2i(0, 0, width, height).clip(Rect2i(p_dest - src_underscan, clipped_src_rect.size)); + Rect2i dest_rect = Rect2i(0, 0, width, height).intersection(Rect2i(p_dest - src_underscan, clipped_src_rect.size)); uint8_t *wp = data.ptrw(); uint8_t *dst_data_ptr = wp; @@ -2535,7 +2535,7 @@ void Image::blit_rect_mask(const Ref<Image> &p_src, const Ref<Image> &p_mask, co ERR_FAIL_COND_MSG(p_src->height != p_mask->height, "Source image height is different from mask height."); ERR_FAIL_COND(format != p_src->format); - Rect2i clipped_src_rect = Rect2i(0, 0, p_src->width, p_src->height).clip(p_src_rect); + Rect2i clipped_src_rect = Rect2i(0, 0, p_src->width, p_src->height).intersection(p_src_rect); if (p_dest.x < 0) { clipped_src_rect.position.x = ABS(p_dest.x); @@ -2549,7 +2549,7 @@ void Image::blit_rect_mask(const Ref<Image> &p_src, const Ref<Image> &p_mask, co } Point2 src_underscan = Point2(MIN(0, p_src_rect.position.x), MIN(0, p_src_rect.position.y)); - Rect2i dest_rect = Rect2i(0, 0, width, height).clip(Rect2i(p_dest - src_underscan, clipped_src_rect.size)); + Rect2i dest_rect = Rect2i(0, 0, width, height).intersection(Rect2i(p_dest - src_underscan, clipped_src_rect.size)); uint8_t *wp = data.ptrw(); uint8_t *dst_data_ptr = wp; @@ -2589,7 +2589,7 @@ void Image::blend_rect(const Ref<Image> &p_src, const Rect2 &p_src_rect, const P ERR_FAIL_COND(srcdsize == 0); ERR_FAIL_COND(format != p_src->format); - Rect2i clipped_src_rect = Rect2i(0, 0, p_src->width, p_src->height).clip(p_src_rect); + Rect2i clipped_src_rect = Rect2i(0, 0, p_src->width, p_src->height).intersection(p_src_rect); if (p_dest.x < 0) { clipped_src_rect.position.x = ABS(p_dest.x); @@ -2603,7 +2603,7 @@ void Image::blend_rect(const Ref<Image> &p_src, const Rect2 &p_src_rect, const P } Point2 src_underscan = Point2(MIN(0, p_src_rect.position.x), MIN(0, p_src_rect.position.y)); - Rect2i dest_rect = Rect2i(0, 0, width, height).clip(Rect2i(p_dest - src_underscan, clipped_src_rect.size)); + Rect2i dest_rect = Rect2i(0, 0, width, height).intersection(Rect2i(p_dest - src_underscan, clipped_src_rect.size)); Ref<Image> img = p_src; @@ -2638,7 +2638,7 @@ void Image::blend_rect_mask(const Ref<Image> &p_src, const Ref<Image> &p_mask, c ERR_FAIL_COND_MSG(p_src->height != p_mask->height, "Source image height is different from mask height."); ERR_FAIL_COND(format != p_src->format); - Rect2i clipped_src_rect = Rect2i(0, 0, p_src->width, p_src->height).clip(p_src_rect); + Rect2i clipped_src_rect = Rect2i(0, 0, p_src->width, p_src->height).intersection(p_src_rect); if (p_dest.x < 0) { clipped_src_rect.position.x = ABS(p_dest.x); @@ -2652,7 +2652,7 @@ void Image::blend_rect_mask(const Ref<Image> &p_src, const Ref<Image> &p_mask, c } Point2 src_underscan = Point2(MIN(0, p_src_rect.position.x), MIN(0, p_src_rect.position.y)); - Rect2i dest_rect = Rect2i(0, 0, width, height).clip(Rect2i(p_dest - src_underscan, clipped_src_rect.size)); + Rect2i dest_rect = Rect2i(0, 0, width, height).intersection(Rect2i(p_dest - src_underscan, clipped_src_rect.size)); Ref<Image> img = p_src; Ref<Image> msk = p_mask; @@ -3077,7 +3077,7 @@ void Image::_bind_methods() { ClassDB::bind_method(D_METHOD("get_mipmap_offset", "mipmap"), &Image::get_mipmap_offset); - ClassDB::bind_method(D_METHOD("resize_to_po2", "square"), &Image::resize_to_po2, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("resize_to_po2", "square", "interpolation"), &Image::resize_to_po2, DEFVAL(false), DEFVAL(INTERPOLATE_BILINEAR)); ClassDB::bind_method(D_METHOD("resize", "width", "height", "interpolation"), &Image::resize, DEFVAL(INTERPOLATE_BILINEAR)); ClassDB::bind_method(D_METHOD("shrink_x2"), &Image::shrink_x2); diff --git a/core/io/image.h b/core/io/image.h index 0151df0cf9..6b4488bd66 100644 --- a/core/io/image.h +++ b/core/io/image.h @@ -244,7 +244,7 @@ public: /** * Resize the image, using the preferred interpolation method. */ - void resize_to_po2(bool p_square = false); + void resize_to_po2(bool p_square = false, Interpolation p_interpolation = INTERPOLATE_BILINEAR); void resize(int p_width, int p_height, Interpolation p_interpolation = INTERPOLATE_BILINEAR); void shrink_x2(); bool is_size_po2() const; diff --git a/core/math/aabb.h b/core/math/aabb.h index 45dcbc7f7f..474304eae2 100644 --- a/core/math/aabb.h +++ b/core/math/aabb.h @@ -190,9 +190,9 @@ Vector3 AABB::get_support(const Vector3 &p_normal) const { Vector3 ofs = position + half_extents; return Vector3( - (p_normal.x > 0) ? -half_extents.x : half_extents.x, - (p_normal.y > 0) ? -half_extents.y : half_extents.y, - (p_normal.z > 0) ? -half_extents.z : half_extents.z) + + (p_normal.x > 0) ? half_extents.x : -half_extents.x, + (p_normal.y > 0) ? half_extents.y : -half_extents.y, + (p_normal.z > 0) ? half_extents.z : -half_extents.z) + ofs; } diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp index 5e5efb6356..f29cb7a269 100644 --- a/core/math/camera_matrix.cpp +++ b/core/math/camera_matrix.cpp @@ -655,6 +655,17 @@ real_t CameraMatrix::get_fov() const { } } +float CameraMatrix::get_lod_multiplier() const { + if (is_orthogonal()) { + return get_viewport_half_extents().x; + } else { + float zn = get_z_near(); + float width = get_viewport_half_extents().x * 2.0; + return 1.0 / (zn / width); + } + + //usage is lod_size / (lod_distance * multiplier) < threshold +} void CameraMatrix::make_scale(const Vector3 &p_scale) { set_identity(); matrix[0][0] = p_scale.x; diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h index c5cdd98377..f856a7b1bf 100644 --- a/core/math/camera_matrix.h +++ b/core/math/camera_matrix.h @@ -108,6 +108,8 @@ struct CameraMatrix { return !(*this == p_cam); } + float get_lod_multiplier() const; + CameraMatrix(); CameraMatrix(const Transform &p_transform); ~CameraMatrix(); diff --git a/core/math/dynamic_bvh.cpp b/core/math/dynamic_bvh.cpp new file mode 100644 index 0000000000..8486415c81 --- /dev/null +++ b/core/math/dynamic_bvh.cpp @@ -0,0 +1,400 @@ +/*************************************************************************/ +/* dynamic_bvh.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "dynamic_bvh.h" + +void DynamicBVH::_delete_node(Node *p_node) { + memdelete(p_node); +} + +void DynamicBVH::_recurse_delete_node(Node *p_node) { + if (!p_node->is_leaf()) { + _recurse_delete_node(p_node->childs[0]); + _recurse_delete_node(p_node->childs[1]); + } + if (p_node == bvh_root) { + bvh_root = nullptr; + } + _delete_node(p_node); +} + +DynamicBVH::Node *DynamicBVH::_create_node(Node *p_parent, void *p_data) { + Node *node = memnew(Node); + node->parent = p_parent; + node->data = p_data; + node->childs[1] = 0; + return (node); +} + +DynamicBVH::Node *DynamicBVH::_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data) { + Node *node = _create_node(p_parent, p_data); + node->volume = p_volume; + return node; +} + +void DynamicBVH::_insert_leaf(Node *p_root, Node *p_leaf) { + if (!bvh_root) { + bvh_root = p_leaf; + p_leaf->parent = 0; + } else { + if (!p_root->is_leaf()) { + do { + p_root = p_root->childs[p_leaf->volume.select_by_proximity( + p_root->childs[0]->volume, + p_root->childs[1]->volume)]; + } while (!p_root->is_leaf()); + } + Node *prev = p_root->parent; + Node *node = _create_node_with_volume(prev, p_leaf->volume.merge(p_root->volume), 0); + if (prev) { + prev->childs[p_root->get_index_in_parent()] = node; + node->childs[0] = p_root; + p_root->parent = node; + node->childs[1] = p_leaf; + p_leaf->parent = node; + do { + if (!prev->volume.contains(node->volume)) { + prev->volume = prev->childs[0]->volume.merge(prev->childs[1]->volume); + } else { + break; + } + node = prev; + } while (0 != (prev = node->parent)); + } else { + node->childs[0] = p_root; + p_root->parent = node; + node->childs[1] = p_leaf; + p_leaf->parent = node; + bvh_root = node; + } + } +} + +DynamicBVH::Node *DynamicBVH::_remove_leaf(Node *leaf) { + if (leaf == bvh_root) { + bvh_root = 0; + return (0); + } else { + Node *parent = leaf->parent; + Node *prev = parent->parent; + Node *sibling = parent->childs[1 - leaf->get_index_in_parent()]; + if (prev) { + prev->childs[parent->get_index_in_parent()] = sibling; + sibling->parent = prev; + _delete_node(parent); + while (prev) { + const Volume pb = prev->volume; + prev->volume = prev->childs[0]->volume.merge(prev->childs[1]->volume); + if (pb.is_not_equal_to(prev->volume)) { + prev = prev->parent; + } else + break; + } + return (prev ? prev : bvh_root); + } else { + bvh_root = sibling; + sibling->parent = 0; + _delete_node(parent); + return (bvh_root); + } + } +} + +void DynamicBVH::_fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth) { + if (p_root->is_internal() && p_depth) { + _fetch_leaves(p_root->childs[0], r_leaves, p_depth - 1); + _fetch_leaves(p_root->childs[1], r_leaves, p_depth - 1); + _delete_node(p_root); + } else { + r_leaves.push_back(p_root); + } +} + +// Partitions leaves such that leaves[0, n) are on the +// left of axis, and leaves[n, count) are on the right +// of axis. returns N. +int DynamicBVH::_split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis) { + int begin = 0; + int end = p_count; + for (;;) { + while (begin != end && leaves[begin]->is_left_of_axis(p_org, p_axis)) { + ++begin; + } + + if (begin == end) { + break; + } + + while (begin != end && !leaves[end - 1]->is_left_of_axis(p_org, p_axis)) { + --end; + } + + if (begin == end) { + break; + } + + // swap out of place nodes + --end; + Node *temp = leaves[begin]; + leaves[begin] = leaves[end]; + leaves[end] = temp; + ++begin; + } + + return begin; +} + +DynamicBVH::Volume DynamicBVH::_bounds(Node **leaves, int p_count) { + Volume volume = leaves[0]->volume; + for (int i = 1, ni = p_count; i < ni; ++i) { + volume = volume.merge(leaves[i]->volume); + } + return (volume); +} + +void DynamicBVH::_bottom_up(Node **leaves, int p_count) { + while (p_count > 1) { + real_t minsize = Math_INF; + int minidx[2] = { -1, -1 }; + for (int i = 0; i < p_count; ++i) { + for (int j = i + 1; j < p_count; ++j) { + const real_t sz = leaves[i]->volume.merge(leaves[j]->volume).get_size(); + if (sz < minsize) { + minsize = sz; + minidx[0] = i; + minidx[1] = j; + } + } + } + Node *n[] = { leaves[minidx[0]], leaves[minidx[1]] }; + Node *p = _create_node_with_volume(nullptr, n[0]->volume.merge(n[1]->volume), nullptr); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves[minidx[1]] = leaves[p_count - 1]; + --p_count; + } +} + +DynamicBVH::Node *DynamicBVH::_top_down(Node **leaves, int p_count, int p_bu_threshold) { + static const Vector3 axis[] = { Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) }; + + ERR_FAIL_COND_V(p_bu_threshold <= 1, nullptr); + if (p_count > 1) { + if (p_count > p_bu_threshold) { + const Volume vol = _bounds(leaves, p_count); + const Vector3 org = vol.get_center(); + int partition; + int bestaxis = -1; + int bestmidp = p_count; + int splitcount[3][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 } }; + int i; + for (i = 0; i < p_count; ++i) { + const Vector3 x = leaves[i]->volume.get_center() - org; + for (int j = 0; j < 3; ++j) { + ++splitcount[j][x.dot(axis[j]) > 0 ? 1 : 0]; + } + } + for (i = 0; i < 3; ++i) { + if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { + const int midp = (int)Math::abs(real_t(splitcount[i][0] - splitcount[i][1])); + if (midp < bestmidp) { + bestaxis = i; + bestmidp = midp; + } + } + } + if (bestaxis >= 0) { + partition = _split(leaves, p_count, org, axis[bestaxis]); + ERR_FAIL_COND_V(partition == 0 || partition == p_count, nullptr); + } else { + partition = p_count / 2 + 1; + } + + Node *node = _create_node_with_volume(nullptr, vol, nullptr); + node->childs[0] = _top_down(&leaves[0], partition, p_bu_threshold); + node->childs[1] = _top_down(&leaves[partition], p_count - partition, p_bu_threshold); + node->childs[0]->parent = node; + node->childs[1]->parent = node; + return (node); + } else { + _bottom_up(leaves, p_count); + return (leaves[0]); + } + } + return (leaves[0]); +} + +DynamicBVH::Node *DynamicBVH::_node_sort(Node *n, Node *&r) { + Node *p = n->parent; + ERR_FAIL_COND_V(!n->is_internal(), nullptr); + if (p > n) { + const int i = n->get_index_in_parent(); + const int j = 1 - i; + Node *s = p->childs[j]; + Node *q = p->parent; + ERR_FAIL_COND_V(n != p->childs[i], nullptr); + if (q) + q->childs[p->get_index_in_parent()] = n; + else + r = n; + s->parent = n; + p->parent = n; + n->parent = q; + p->childs[0] = n->childs[0]; + p->childs[1] = n->childs[1]; + n->childs[0]->parent = p; + n->childs[1]->parent = p; + n->childs[i] = p; + n->childs[j] = s; + SWAP(p->volume, n->volume); + return (p); + } + return (n); +} + +void DynamicBVH::clear() { + if (bvh_root) { + _recurse_delete_node(bvh_root); + } + lkhd = -1; + opath = 0; +} + +void DynamicBVH::optimize_bottom_up() { + if (bvh_root) { + LocalVector<Node *> leaves; + _fetch_leaves(bvh_root, leaves); + _bottom_up(&leaves[0], leaves.size()); + bvh_root = leaves[0]; + } +} + +void DynamicBVH::optimize_top_down(int bu_threshold) { + if (bvh_root) { + LocalVector<Node *> leaves; + _fetch_leaves(bvh_root, leaves); + bvh_root = _top_down(&leaves[0], leaves.size(), bu_threshold); + } +} + +void DynamicBVH::optimize_incremental(int passes) { + if (passes < 0) + passes = total_leaves; + if (bvh_root && (passes > 0)) { + do { + Node *node = bvh_root; + unsigned bit = 0; + while (node->is_internal()) { + node = _node_sort(node, bvh_root)->childs[(opath >> bit) & 1]; + bit = (bit + 1) & (sizeof(unsigned) * 8 - 1); + } + _update(node); + ++opath; + } while (--passes); + } +} + +DynamicBVH::ID DynamicBVH::insert(const AABB &p_box, void *p_userdata) { + Volume volume; + volume.min = p_box.position; + volume.max = p_box.position + p_box.size; + + Node *leaf = _create_node_with_volume(nullptr, volume, p_userdata); + _insert_leaf(bvh_root, leaf); + ++total_leaves; + + ID id; + id.node = leaf; + return id; +} + +void DynamicBVH::_update(Node *leaf, int lookahead) { + Node *root = _remove_leaf(leaf); + if (root) { + if (lookahead >= 0) { + for (int i = 0; (i < lookahead) && root->parent; ++i) { + root = root->parent; + } + } else + root = bvh_root; + } + _insert_leaf(root, leaf); +} + +void DynamicBVH::update(const ID &p_id, const AABB &p_box) { + ERR_FAIL_COND(!p_id.is_valid()); + Node *leaf = p_id.node; + Node *base = _remove_leaf(leaf); + Volume volume; + volume.min = p_box.position; + volume.max = p_box.position + p_box.size; + if (base) { + if (lkhd >= 0) { + for (int i = 0; (i < lkhd) && base->parent; ++i) { + base = base->parent; + } + } else + base = bvh_root; + } + leaf->volume = volume; + _insert_leaf(base, leaf); +} + +void DynamicBVH::remove(const ID &p_id) { + ERR_FAIL_COND(!p_id.is_valid()); + Node *leaf = p_id.node; + _remove_leaf(leaf); + _delete_node(leaf); + --total_leaves; +} + +void DynamicBVH::_extract_leaves(Node *p_node, List<ID> *r_elements) { + if (p_node->is_internal()) { + _extract_leaves(p_node->childs[0], r_elements); + _extract_leaves(p_node->childs[1], r_elements); + } else { + ID id; + id.node = p_node; + r_elements->push_back(id); + } +} + +void DynamicBVH::get_elements(List<ID> *r_elements) { + if (bvh_root) { + _extract_leaves(bvh_root, r_elements); + } +} + +DynamicBVH::~DynamicBVH() { + clear(); +} diff --git a/core/math/dynamic_bvh.h b/core/math/dynamic_bvh.h new file mode 100644 index 0000000000..81d70d7469 --- /dev/null +++ b/core/math/dynamic_bvh.h @@ -0,0 +1,359 @@ +/*************************************************************************/ +/* dynamic_bvh.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef DYNAMICBVH_H +#define DYNAMICBVH_H + +#include "core/math/aabb.h" +#include "core/templates/list.h" +#include "core/templates/local_vector.h" +#include "core/typedefs.h" + +// Based on bullet Dbvh + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///DynamicBVH implementation by Nathanael Presson +// The DynamicBVH class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). + +class DynamicBVH { + struct Node; + +public: + struct ID { + Node *node; + + public: + _FORCE_INLINE_ bool is_valid() const { return node != nullptr; } + _FORCE_INLINE_ ID() { + node = nullptr; + } + }; + +private: + struct Volume { + Vector3 min, max; + + _FORCE_INLINE_ Vector3 get_center() const { return ((min + max) / 2); } + _FORCE_INLINE_ Vector3 get_length() const { return (max - min); } + + _FORCE_INLINE_ bool contains(const Volume &a) const { + return ((min.x <= a.min.x) && + (min.y <= a.min.y) && + (min.z <= a.min.z) && + (max.x >= a.max.x) && + (max.y >= a.max.y) && + (max.z >= a.max.z)); + } + + _FORCE_INLINE_ Volume merge(const Volume &b) const { + Volume r; + for (int i = 0; i < 3; ++i) { + if (min[i] < b.min[i]) + r.min[i] = min[i]; + else + r.min[i] = b.min[i]; + if (max[i] > b.max[i]) + r.max[i] = max[i]; + else + r.max[i] = b.max[i]; + } + return r; + } + + _FORCE_INLINE_ real_t get_size() const { + const Vector3 edges = get_length(); + return (edges.x * edges.y * edges.z + + edges.x + edges.y + edges.z); + } + + _FORCE_INLINE_ bool is_not_equal_to(const Volume &b) const { + return ((min.x != b.min.x) || + (min.y != b.min.y) || + (min.z != b.min.z) || + (max.x != b.max.x) || + (max.y != b.max.y) || + (max.z != b.max.z)); + } + + _FORCE_INLINE_ real_t get_proximity_to(const Volume &b) const { + const Vector3 d = (min + max) - (b.min + b.max); + return (Math::abs(d.x) + Math::abs(d.y) + Math::abs(d.z)); + } + + _FORCE_INLINE_ int select_by_proximity(const Volume &a, const Volume &b) const { + return (get_proximity_to(a) < get_proximity_to(b) ? 0 : 1); + } + + // + _FORCE_INLINE_ bool intersects(const Volume &b) const { + return ((min.x <= b.max.x) && + (max.x >= b.min.x) && + (min.y <= b.max.y) && + (max.y >= b.min.y) && + (min.z <= b.max.z) && + (max.z >= b.min.z)); + } + }; + + struct Node { + Volume volume; + Node *parent = nullptr; + union { + Node *childs[2]; + void *data; + }; + + _FORCE_INLINE_ bool is_leaf() const { return data != nullptr; } + _FORCE_INLINE_ bool is_internal() const { return (!is_leaf()); } + + _FORCE_INLINE_ int get_index_in_parent() const { + ERR_FAIL_COND_V(!parent, 0); + return (parent->childs[1] == this) ? 1 : 0; + } + _FORCE_INLINE_ void get_max_depth(int depth, int &maxdepth) { + if (is_internal()) { + childs[0]->get_max_depth(depth + 1, maxdepth); + childs[1]->get_max_depth(depth + 1, maxdepth); + } else { + maxdepth = MAX(maxdepth, depth); + } + } + + // + int count_leaves() const { + if (is_internal()) + return childs[0]->count_leaves() + childs[1]->count_leaves(); + else + return (1); + } + + bool is_left_of_axis(const Vector3 &org, const Vector3 &axis) const { + return axis.dot(volume.get_center() - org) <= 0; + } + + Node() { + childs[0] = nullptr; + childs[1] = nullptr; + } + }; + + // Fields + Node *bvh_root = nullptr; + int lkhd = -1; + int total_leaves = 0; + uint32_t opath = 0; + + enum { + ALLOCA_STACK_SIZE = 128 + }; + + _FORCE_INLINE_ void _delete_node(Node *p_node); + void _recurse_delete_node(Node *p_node); + _FORCE_INLINE_ Node *_create_node(Node *p_parent, void *p_data); + _FORCE_INLINE_ DynamicBVH::Node *_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data); + _FORCE_INLINE_ void _insert_leaf(Node *p_root, Node *p_leaf); + _FORCE_INLINE_ Node *_remove_leaf(Node *leaf); + void _fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth = -1); + static int _split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis); + static Volume _bounds(Node **leaves, int p_count); + void _bottom_up(Node **leaves, int p_count); + Node *_top_down(Node **leaves, int p_count, int p_bu_threshold); + Node *_node_sort(Node *n, Node *&r); + + _FORCE_INLINE_ void _update(Node *leaf, int lookahead = -1); + + void _extract_leaves(Node *p_node, List<ID> *r_elements); + + _FORCE_INLINE_ bool _ray_aabb(const Vector3 &rayFrom, const Vector3 &rayInvDirection, const unsigned int raySign[3], const Vector3 bounds[2], real_t &tmin, real_t lambda_min, real_t lambda_max) { + real_t tmax, tymin, tymax, tzmin, tzmax; + tmin = (bounds[raySign[0]].x - rayFrom.x) * rayInvDirection.x; + tmax = (bounds[1 - raySign[0]].x - rayFrom.x) * rayInvDirection.x; + tymin = (bounds[raySign[1]].y - rayFrom.y) * rayInvDirection.y; + tymax = (bounds[1 - raySign[1]].y - rayFrom.y) * rayInvDirection.y; + + if ((tmin > tymax) || (tymin > tmax)) + return false; + + if (tymin > tmin) + tmin = tymin; + + if (tymax < tmax) + tmax = tymax; + + tzmin = (bounds[raySign[2]].z - rayFrom.z) * rayInvDirection.z; + tzmax = (bounds[1 - raySign[2]].z - rayFrom.z) * rayInvDirection.z; + + if ((tmin > tzmax) || (tzmin > tmax)) + return false; + if (tzmin > tmin) + tmin = tzmin; + if (tzmax < tmax) + tmax = tzmax; + return ((tmin < lambda_max) && (tmax > lambda_min)); + } + +public: + // Methods + void clear(); + bool empty() const { return (0 == bvh_root); } + void optimize_bottom_up(); + void optimize_top_down(int bu_threshold = 128); + void optimize_incremental(int passes); + ID insert(const AABB &p_box, void *p_userdata); + void update(const ID &p_id, const AABB &p_box); + void remove(const ID &p_id); + void get_elements(List<ID> *r_elements); + + /* Discouraged, but works as a reference on how it must be used */ + struct DefaultQueryResult { + virtual bool operator()(void *p_data) = 0; //return true whether you want to continue the query + virtual ~DefaultQueryResult() {} + }; + + template <class QueryResult> + _FORCE_INLINE_ void aabb_query(const AABB &p_aabb, QueryResult &r_result); + template <class QueryResult> + _FORCE_INLINE_ void ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result); + + DynamicBVH(); + ~DynamicBVH(); +}; + +template <class QueryResult> +void DynamicBVH::aabb_query(const AABB &p_box, QueryResult &r_result) { + if (!bvh_root) { + return; + } + + Volume volume; + volume.min = p_box.position; + volume.max = p_box.position + p_box.size; + + const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *)); + stack[0] = bvh_root; + int32_t depth = 1; + int32_t threshold = ALLOCA_STACK_SIZE - 2; + + LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time. + + do { + const Node *n = stack[depth - 1]; + depth--; + if (n->volume.intersects(volume)) { + if (n->is_internal()) { + if (depth > threshold) { + if (aux_stack.empty()) { + aux_stack.resize(ALLOCA_STACK_SIZE * 2); + copymem(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *)); + } else { + aux_stack.resize(aux_stack.size() * 2); + } + stack = aux_stack.ptr(); + threshold = aux_stack.size() - 2; + } + stack[depth++] = n->childs[0]; + stack[depth++] = n->childs[1]; + } else { + if (r_result(n->data)) { + return; + } + } + } + } while (depth > 0); +} + +template <class QueryResult> +void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result) { + Vector3 ray_dir = (p_to - p_from); + ray_dir.normalize(); + + ///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT + Vector3 inv_dir; + inv_dir[0] = ray_dir[0] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[0]; + inv_dir[1] = ray_dir[1] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[1]; + inv_dir[2] = ray_dir[2] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[2]; + unsigned int signs[3] = { inv_dir[0] < 0.0, inv_dir[1] < 0.0, inv_dir[2] < 0.0 }; + + real_t lambda_max = ray_dir.dot(p_to - p_from); + + Vector3 bounds[2]; + + const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *)); + stack[0] = bvh_root; + int32_t depth = 1; + int32_t threshold = ALLOCA_STACK_SIZE - 2; + + LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time. + + do { + const Node *node = stack[--depth]; + bounds[0] = node->volume.min; + bounds[1] = node->volume.max; + real_t tmin = 1.f, lambda_min = 0.f; + unsigned int result1 = false; + result1 = _ray_aabb(p_from, inv_dir, signs, bounds, tmin, lambda_min, lambda_max); + if (result1) { + if (node->is_internal()) { + if (depth > threshold) { + if (aux_stack.empty()) { + aux_stack.resize(ALLOCA_STACK_SIZE * 2); + copymem(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *)); + } else { + aux_stack.resize(aux_stack.size() * 2); + } + stack = aux_stack.ptr(); + threshold = aux_stack.size() - 2; + } + stack[depth++] = node->childs[0]; + stack[depth++] = node->childs[1]; + } else { + if (r_result(node->data)) { + return; + } + } + } + } while (depth > 0); +} + +#endif // DYNAMICBVH_H diff --git a/core/math/rect2.h b/core/math/rect2.h index b1fe865ba5..f36084ec74 100644 --- a/core/math/rect2.h +++ b/core/math/rect2.h @@ -123,8 +123,9 @@ struct Rect2 { _FORCE_INLINE_ bool has_no_area() const { return (size.x <= 0 || size.y <= 0); } - inline Rect2 clip(const Rect2 &p_rect) const { /// return a clipped rect + // Returns the instersection between two Rect2s or an empty Rect2 if there is no intersection + inline Rect2 intersection(const Rect2 &p_rect) const { Rect2 new_rect = p_rect; if (!intersects(new_rect)) { @@ -365,8 +366,9 @@ struct Rect2i { _FORCE_INLINE_ bool has_no_area() const { return (size.x <= 0 || size.y <= 0); } - inline Rect2i clip(const Rect2i &p_rect) const { /// return a clipped rect + // Returns the instersection between two Rect2is or an empty Rect2i if there is no intersection + inline Rect2i intersection(const Rect2i &p_rect) const { Rect2i new_rect = p_rect; if (!intersects(new_rect)) { diff --git a/core/object/object.h b/core/object/object.h index dc004f38a9..6e2328848b 100644 --- a/core/object/object.h +++ b/core/object/object.h @@ -482,8 +482,6 @@ private: void _set_indexed_bind(const NodePath &p_name, const Variant &p_value); Variant _get_indexed_bind(const NodePath &p_name) const; - void property_list_changed_notify(); - _FORCE_INLINE_ void _construct_object(bool p_reference); friend class Reference; @@ -525,6 +523,7 @@ protected: static void get_valid_parents_static(List<String> *p_parents); static void _get_valid_parents_static(List<String> *p_parents); + void property_list_changed_notify(); virtual void _changed_callback(Object *p_changed, const char *p_prop); //Variant _call_bind(const StringName& p_name, const Variant& p_arg1 = Variant(), const Variant& p_arg2 = Variant(), const Variant& p_arg3 = Variant(), const Variant& p_arg4 = Variant()); diff --git a/core/object/undo_redo.cpp b/core/object/undo_redo.cpp index 1dcbb0cd6b..65bae6f6e8 100644 --- a/core/object/undo_redo.cpp +++ b/core/object/undo_redo.cpp @@ -30,6 +30,7 @@ #include "undo_redo.h" +#include "core/io/resource.h" #include "core/os/os.h" void UndoRedo::_discard_redo() { @@ -104,8 +105,8 @@ void UndoRedo::add_do_method(Object *p_object, const StringName &p_method, VARIA ERR_FAIL_COND((current_action + 1) >= actions.size()); Operation do_op; do_op.object = p_object->get_instance_id(); - if (Object::cast_to<Resource>(p_object)) { - do_op.resref = Ref<Resource>(Object::cast_to<Resource>(p_object)); + if (Object::cast_to<Reference>(p_object)) { + do_op.ref = Ref<Reference>(Object::cast_to<Reference>(p_object)); } do_op.type = Operation::TYPE_METHOD; @@ -130,8 +131,8 @@ void UndoRedo::add_undo_method(Object *p_object, const StringName &p_method, VAR Operation undo_op; undo_op.object = p_object->get_instance_id(); - if (Object::cast_to<Resource>(p_object)) { - undo_op.resref = Ref<Resource>(Object::cast_to<Resource>(p_object)); + if (Object::cast_to<Reference>(p_object)) { + undo_op.ref = Ref<Reference>(Object::cast_to<Reference>(p_object)); } undo_op.type = Operation::TYPE_METHOD; @@ -149,8 +150,8 @@ void UndoRedo::add_do_property(Object *p_object, const StringName &p_property, c ERR_FAIL_COND((current_action + 1) >= actions.size()); Operation do_op; do_op.object = p_object->get_instance_id(); - if (Object::cast_to<Resource>(p_object)) { - do_op.resref = Ref<Resource>(Object::cast_to<Resource>(p_object)); + if (Object::cast_to<Reference>(p_object)) { + do_op.ref = Ref<Reference>(Object::cast_to<Reference>(p_object)); } do_op.type = Operation::TYPE_PROPERTY; @@ -171,8 +172,8 @@ void UndoRedo::add_undo_property(Object *p_object, const StringName &p_property, Operation undo_op; undo_op.object = p_object->get_instance_id(); - if (Object::cast_to<Resource>(p_object)) { - undo_op.resref = Ref<Resource>(Object::cast_to<Resource>(p_object)); + if (Object::cast_to<Reference>(p_object)) { + undo_op.ref = Ref<Reference>(Object::cast_to<Reference>(p_object)); } undo_op.type = Operation::TYPE_PROPERTY; @@ -187,8 +188,8 @@ void UndoRedo::add_do_reference(Object *p_object) { ERR_FAIL_COND((current_action + 1) >= actions.size()); Operation do_op; do_op.object = p_object->get_instance_id(); - if (Object::cast_to<Resource>(p_object)) { - do_op.resref = Ref<Resource>(Object::cast_to<Resource>(p_object)); + if (Object::cast_to<Reference>(p_object)) { + do_op.ref = Ref<Reference>(Object::cast_to<Reference>(p_object)); } do_op.type = Operation::TYPE_REFERENCE; @@ -207,8 +208,8 @@ void UndoRedo::add_undo_reference(Object *p_object) { Operation undo_op; undo_op.object = p_object->get_instance_id(); - if (Object::cast_to<Resource>(p_object)) { - undo_op.resref = Ref<Resource>(Object::cast_to<Resource>(p_object)); + if (Object::cast_to<Reference>(p_object)) { + undo_op.ref = Ref<Reference>(Object::cast_to<Reference>(p_object)); } undo_op.type = Operation::TYPE_REFERENCE; diff --git a/core/object/undo_redo.h b/core/object/undo_redo.h index 68d78e0d7d..06c2759a65 100644 --- a/core/object/undo_redo.h +++ b/core/object/undo_redo.h @@ -31,8 +31,8 @@ #ifndef UNDO_REDO_H #define UNDO_REDO_H -#include "core/io/resource.h" #include "core/object/class_db.h" +#include "core/object/reference.h" class UndoRedo : public Object { GDCLASS(UndoRedo, Object); @@ -61,7 +61,7 @@ private: }; Type type; - Ref<Resource> resref; + Ref<Reference> ref; ObjectID object; StringName name; Variant args[VARIANT_ARG_MAX]; diff --git a/core/variant/variant_call.cpp b/core/variant/variant_call.cpp index 0a1af591a1..afe4f2702e 100644 --- a/core/variant/variant_call.cpp +++ b/core/variant/variant_call.cpp @@ -1021,7 +1021,7 @@ static void _register_variant_builtin_methods() { bind_method(Rect2, is_equal_approx, sarray("rect"), varray()); bind_method(Rect2, intersects, sarray("b", "include_borders"), varray(false)); bind_method(Rect2, encloses, sarray("b"), varray()); - bind_method(Rect2, clip, sarray("b"), varray()); + bind_method(Rect2, intersection, sarray("b"), varray()); bind_method(Rect2, merge, sarray("b"), varray()); bind_method(Rect2, expand, sarray("to"), varray()); bind_method(Rect2, grow, sarray("by"), varray()); @@ -1036,7 +1036,7 @@ static void _register_variant_builtin_methods() { bind_method(Rect2i, has_point, sarray("point"), varray()); bind_method(Rect2i, intersects, sarray("b"), varray()); bind_method(Rect2i, encloses, sarray("b"), varray()); - bind_method(Rect2i, clip, sarray("b"), varray()); + bind_method(Rect2i, intersection, sarray("b"), varray()); bind_method(Rect2i, merge, sarray("b"), varray()); bind_method(Rect2i, expand, sarray("to"), varray()); bind_method(Rect2i, grow, sarray("by"), varray()); diff --git a/doc/classes/@GlobalScope.xml b/doc/classes/@GlobalScope.xml index f94eb7adef..3f0a7b7c25 100644 --- a/doc/classes/@GlobalScope.xml +++ b/doc/classes/@GlobalScope.xml @@ -2423,6 +2423,16 @@ <constant name="PROPERTY_HINT_IMAGE_COMPRESS_LOSSLESS" value="21" enum="PropertyHint"> Hints that an image is compressed using lossless compression. </constant> + <constant name="PROPERTY_HINT_TYPE_STRING" value="23" enum="PropertyHint"> + Hint that a property represents a particular type. If a property is [constant TYPE_STRING], allows to set a type from the create dialog. If you need to create an [Array] to contain elements of a specific type, the [code]hint_string[/code] must encode nested types using [code]":"[/code] and [code]"/"[/code] for specifying [Resource] types. For instance: + [codeblock] + hint_string = "%s:" % [TYPE_INT] # Array of inteters. + hint_string = "%s:%s:" % [TYPE_ARRAY, TYPE_REAL] # Two-dimensional array of floats. + hint_string = "%s/%s:Resource" % [TYPE_OBJECT, TYPE_OBJECT] # Array of resources. + hint_string = "%s:%s/%s:Resource" % [TYPE_ARRAY, TYPE_OBJECT, TYPE_OBJECT] # Two-dimensional array of resources. + [/codeblock] + [b]Note:[/b] the final colon is required to specify for properly detecting built-in types. + </constant> <constant name="PROPERTY_USAGE_STORAGE" value="1" enum="PropertyUsageFlags"> The property is serialized and saved in the scene file (default). </constant> diff --git a/doc/classes/AnimationPlayer.xml b/doc/classes/AnimationPlayer.xml index de2087d930..bebff61671 100644 --- a/doc/classes/AnimationPlayer.xml +++ b/doc/classes/AnimationPlayer.xml @@ -260,6 +260,10 @@ <member name="playback_speed" type="float" setter="set_speed_scale" getter="get_speed_scale" default="1.0"> The speed scaling ratio. For instance, if this value is 1, then the animation plays at normal speed. If it's 0.5, then it plays at half speed. If it's 2, then it plays at double speed. </member> + <member name="reset_on_save" type="bool" setter="set_reset_on_save_enabled" getter="is_reset_on_save_enabled" default="true"> + This is used by the editor. If set to [code]true[/code], the scene will be saved with the effects of the reset animation applied (as if it had been seeked to time 0), then reverted after saving. + In other words, the saved scene file will contain the "default pose", as defined by the reset animation, if any, with the editor keeping the values that the nodes had before saving. + </member> <member name="root_node" type="NodePath" setter="set_root" getter="get_root" default="NodePath("..")"> The node from which node path references will travel. </member> diff --git a/doc/classes/BoxMesh.xml b/doc/classes/BoxMesh.xml index 88d22ac899..8a1b9e939e 100644 --- a/doc/classes/BoxMesh.xml +++ b/doc/classes/BoxMesh.xml @@ -6,6 +6,7 @@ <description> Generate an axis-aligned box [PrimitiveMesh]. The box's UV layout is arranged in a 3×2 layout that allows texturing each face individually. To apply the same texture on all faces, change the material's UV property to [code]Vector3(3, 2, 1)[/code]. + [b]Note:[/b] When using a large textured [BoxMesh] (e.g. as a floor), you may stumble upon UV jittering issues depending on the camera angle. To solve this, increase [member subdivide_depth], [member subdivide_height] and [member subdivide_width] until you no longer notice UV jittering. </description> <tutorials> </tutorials> diff --git a/doc/classes/Camera3D.xml b/doc/classes/Camera3D.xml index b3fe452b12..052b23a7ab 100644 --- a/doc/classes/Camera3D.xml +++ b/doc/classes/Camera3D.xml @@ -189,7 +189,7 @@ <member name="environment" type="Environment" setter="set_environment" getter="get_environment"> The [Environment] to use for this camera. </member> - <member name="far" type="float" setter="set_zfar" getter="get_zfar" default="100.0"> + <member name="far" type="float" setter="set_zfar" getter="get_zfar" default="4000.0"> The distance to the far culling boundary for this camera relative to its local Z axis. </member> <member name="fov" type="float" setter="set_fov" getter="get_fov" default="75.0"> diff --git a/doc/classes/GraphEdit.xml b/doc/classes/GraphEdit.xml index ef4d92a5e7..c5884aa44a 100644 --- a/doc/classes/GraphEdit.xml +++ b/doc/classes/GraphEdit.xml @@ -174,14 +174,20 @@ </method> </methods> <members> + <member name="connection_lines_antialiased" type="bool" setter="set_connection_lines_antialiased" getter="is_connection_lines_antialiased" default="true"> + If [code]true[/code], the lines between nodes will use antialiasing. + </member> + <member name="connection_lines_thickness" type="float" setter="set_connection_lines_thickness" getter="get_connection_lines_thickness" default="2.0"> + The thickness of the lines between the nodes. + </member> <member name="focus_mode" type="int" setter="set_focus_mode" getter="get_focus_mode" override="true" enum="Control.FocusMode" default="2" /> <member name="minimap_enabled" type="bool" setter="set_minimap_enabled" getter="is_minimap_enabled" default="true"> If [code]true[/code], the minimap is visible. </member> - <member name="minimap_opacity" type="float" setter="set_minimap_opacity" getter="get_minimap_opacity" default="0.45"> + <member name="minimap_opacity" type="float" setter="set_minimap_opacity" getter="get_minimap_opacity" default="0.65"> The opacity of the minimap rectangle. </member> - <member name="minimap_size" type="Vector2" setter="set_minimap_size" getter="get_minimap_size" default="Vector2(240, 160)"> + <member name="minimap_size" type="Vector2" setter="set_minimap_size" getter="get_minimap_size" default="Vector2( 240, 160 )"> The size of the minimap rectangle. The map itself is based on the size of the grid area and is scaled to fit this rectangle. </member> <member name="rect_clip_content" type="bool" setter="set_clip_contents" getter="is_clipping_contents" override="true" default="true" /> @@ -326,6 +332,8 @@ <theme_item name="grid_minor" type="Color" default="Color( 1, 1, 1, 0.05 )"> Color of minor grid lines. </theme_item> + <theme_item name="minimap" type="Texture2D"> + </theme_item> <theme_item name="minus" type="Texture2D"> The icon for the zoom out button. </theme_item> diff --git a/doc/classes/Image.xml b/doc/classes/Image.xml index 71db1e5106..414249f110 100644 --- a/doc/classes/Image.xml +++ b/doc/classes/Image.xml @@ -422,7 +422,7 @@ <argument index="2" name="interpolation" type="int" enum="Image.Interpolation" default="1"> </argument> <description> - Resizes the image to the given [code]width[/code] and [code]height[/code]. New pixels are calculated using [code]interpolation[/code]. See [code]interpolation[/code] constants. + Resizes the image to the given [code]width[/code] and [code]height[/code]. New pixels are calculated using the [code]interpolation[/code] mode defined via [enum Interpolation] constants. </description> </method> <method name="resize_to_po2"> @@ -430,8 +430,10 @@ </return> <argument index="0" name="square" type="bool" default="false"> </argument> + <argument index="1" name="interpolation" type="int" enum="Image.Interpolation" default="1"> + </argument> <description> - Resizes the image to the nearest power of 2 for the width and height. If [code]square[/code] is [code]true[/code] then set width and height to be the same. + Resizes the image to the nearest power of 2 for the width and height. If [code]square[/code] is [code]true[/code] then set width and height to be the same. New pixels are calculated using the [code]interpolation[/code] mode defined via [enum Interpolation] constants. </description> </method> <method name="rgbe_to_srgb"> diff --git a/doc/classes/PlaneMesh.xml b/doc/classes/PlaneMesh.xml index 2081442e04..333d687e91 100644 --- a/doc/classes/PlaneMesh.xml +++ b/doc/classes/PlaneMesh.xml @@ -5,6 +5,7 @@ </brief_description> <description> Class representing a planar [PrimitiveMesh]. This flat mesh does not have a thickness. By default, this mesh is aligned on the X and Z axes; this default rotation isn't suited for use with billboarded materials. For billboarded materials, use [QuadMesh] instead. + [b]Note:[/b] When using a large textured [PlaneMesh] (e.g. as a floor), you may stumble upon UV jittering issues depending on the camera angle. To solve this, increase [member subdivide_depth] and [member subdivide_width] until you no longer notice UV jittering. </description> <tutorials> </tutorials> diff --git a/doc/classes/Rect2.xml b/doc/classes/Rect2.xml index 02a77a0e24..5682e12b63 100644 --- a/doc/classes/Rect2.xml +++ b/doc/classes/Rect2.xml @@ -72,7 +72,7 @@ Returns a [Rect2] with equivalent position and area, modified so that the top-left corner is the origin and [code]width[/code] and [code]height[/code] are positive. </description> </method> - <method name="clip"> + <method name="intersection"> <return type="Rect2"> </return> <argument index="0" name="b" type="Rect2"> diff --git a/doc/classes/Rect2i.xml b/doc/classes/Rect2i.xml index e8b75a6ac6..ef12c8de1e 100644 --- a/doc/classes/Rect2i.xml +++ b/doc/classes/Rect2i.xml @@ -70,7 +70,7 @@ Returns a [Rect2i] with equivalent position and area, modified so that the top-left corner is the origin and [code]width[/code] and [code]height[/code] are positive. </description> </method> - <method name="clip"> + <method name="intersection"> <return type="Rect2i"> </return> <argument index="0" name="b" type="Rect2i"> diff --git a/doc/classes/XRController3D.xml b/doc/classes/XRController3D.xml index 78684d10ee..345e5efdee 100644 --- a/doc/classes/XRController3D.xml +++ b/doc/classes/XRController3D.xml @@ -19,7 +19,7 @@ If active, returns the name of the associated controller if provided by the AR/VR SDK used. </description> </method> - <method name="get_hand" qualifiers="const"> + <method name="get_tracker_hand" qualifiers="const"> <return type="int" enum="XRPositionalTracker.TrackerHand"> </return> <description> diff --git a/doc/classes/XRPositionalTracker.xml b/doc/classes/XRPositionalTracker.xml index 5ed7a26b19..36ff312e4d 100644 --- a/doc/classes/XRPositionalTracker.xml +++ b/doc/classes/XRPositionalTracker.xml @@ -12,7 +12,7 @@ <link title="VR tutorial index">https://docs.godotengine.org/en/latest/tutorials/vr/index.html</link> </tutorials> <methods> - <method name="get_hand" qualifiers="const"> + <method name="get_tracker_hand" qualifiers="const"> <return type="int" enum="XRPositionalTracker.TrackerHand"> </return> <description> @@ -68,18 +68,18 @@ Returns the tracker's type, which will be one of the values from the [enum XRServer.TrackerType] enum. </description> </method> - <method name="get_tracks_orientation" qualifiers="const"> + <method name="is_tracking_orientation" qualifiers="const"> <return type="bool"> </return> <description> - Returns [code]true[/code] if this device tracks orientation. + Returns [code]true[/code] if this device is tracking orientation. </description> </method> - <method name="get_tracks_position" qualifiers="const"> + <method name="is_tracking_position" qualifiers="const"> <return type="bool"> </return> <description> - Returns [code]true[/code] if this device tracks position. + Returns [code]true[/code] if this device is tracking position. </description> </method> <method name="get_transform" qualifiers="const"> @@ -101,10 +101,10 @@ <constant name="TRACKER_HAND_UNKNOWN" value="0" enum="TrackerHand"> The hand this tracker is held in is unknown or not applicable. </constant> - <constant name="TRACKER_LEFT_HAND" value="1" enum="TrackerHand"> + <constant name="TRACKER_HAND_LEFT" value="1" enum="TrackerHand"> This tracker is the left hand controller. </constant> - <constant name="TRACKER_RIGHT_HAND" value="2" enum="TrackerHand"> + <constant name="TRACKER_HAND_RIGHT" value="2" enum="TrackerHand"> This tracker is the right hand controller. </constant> </constants> diff --git a/drivers/png/SCsub b/drivers/png/SCsub index db08be0c47..26508dc612 100644 --- a/drivers/png/SCsub +++ b/drivers/png/SCsub @@ -5,6 +5,9 @@ Import("env") env_png = env.Clone() # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_libpng"]: thirdparty_dir = "#thirdparty/libpng/" thirdparty_sources = [ @@ -41,7 +44,7 @@ if env["builtin_libpng"]: env_thirdparty = env_png.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.drivers_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) if use_neon: env_neon = env_thirdparty.Clone() @@ -52,9 +55,17 @@ if env["builtin_libpng"]: neon_sources.append(env_neon.Object(thirdparty_dir + "/arm/filter_neon_intrinsics.c")) neon_sources.append(env_neon.Object(thirdparty_dir + "/arm/filter_neon.S")) neon_sources.append(env_neon.Object(thirdparty_dir + "/arm/palette_neon_intrinsics.c")) - env.drivers_sources += neon_sources + thirdparty_obj += neon_sources + + env.drivers_sources += thirdparty_obj + # Godot source files -env_png.add_source_files(env.drivers_sources, "*.cpp") -Export("env") +driver_obj = [] + +env_png.add_source_files(driver_obj, "*.cpp") +env.drivers_sources += driver_obj + +# Needed to force rebuilding the driver files when the thirdparty library is updated. +env.Depends(driver_obj, thirdparty_obj) diff --git a/drivers/spirv-reflect/SCsub b/drivers/spirv-reflect/SCsub index d0ffaf068d..1e7b3de0e6 100644 --- a/drivers/spirv-reflect/SCsub +++ b/drivers/spirv-reflect/SCsub @@ -2,8 +2,7 @@ Import("env") -env_spirv_reflect = env.Clone() -env_spirv_reflect.disable_warnings() +# Thirdparty source files thirdparty_dir = "#thirdparty/spirv-reflect/" thirdparty_sources = [ @@ -12,6 +11,7 @@ thirdparty_sources = [ thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] -env_spirv_reflect.add_source_files(env.drivers_sources, thirdparty_sources) +env_thirdparty = env.Clone() +env_thirdparty.disable_warnings() -Export("env") +env_thirdparty.add_source_files(env.drivers_sources, thirdparty_sources) diff --git a/drivers/unix/os_unix.h b/drivers/unix/os_unix.h index 2982e0c55c..51e5a00e36 100644 --- a/drivers/unix/os_unix.h +++ b/drivers/unix/os_unix.h @@ -45,7 +45,7 @@ protected: virtual int unix_initialize_audio(int p_audio_driver); //virtual Error initialize(int p_video_driver,int p_audio_driver); - virtual void finalize_core(); + virtual void finalize_core() override; String stdin_buf; @@ -53,7 +53,7 @@ public: OS_Unix(); virtual void alert(const String &p_alert, const String &p_title = "ALERT!"); - virtual String get_stdin_string(bool p_block); + virtual String get_stdin_string(bool p_block) override; //virtual void set_mouse_show(bool p_show); //virtual void set_mouse_grab(bool p_grab); @@ -65,39 +65,39 @@ public: //virtual VideoMode get_video_mode() const; //virtual void get_fullscreen_mode_list(List<VideoMode> *p_list) const; - virtual Error open_dynamic_library(const String p_path, void *&p_library_handle, bool p_also_set_library_path = false); - virtual Error close_dynamic_library(void *p_library_handle); - virtual Error get_dynamic_library_symbol_handle(void *p_library_handle, const String p_name, void *&p_symbol_handle, bool p_optional = false); + virtual Error open_dynamic_library(const String p_path, void *&p_library_handle, bool p_also_set_library_path = false) override; + virtual Error close_dynamic_library(void *p_library_handle) override; + virtual Error get_dynamic_library_symbol_handle(void *p_library_handle, const String p_name, void *&p_symbol_handle, bool p_optional = false) override; - virtual Error set_cwd(const String &p_cwd); + virtual Error set_cwd(const String &p_cwd) override; - virtual String get_name() const; + virtual String get_name() const override; - virtual Date get_date(bool utc) const; - virtual Time get_time(bool utc) const; - virtual TimeZoneInfo get_time_zone_info() const; + virtual Date get_date(bool utc) const override; + virtual Time get_time(bool utc) const override; + virtual TimeZoneInfo get_time_zone_info() const override; - virtual double get_unix_time() const; + virtual double get_unix_time() const override; - virtual void delay_usec(uint32_t p_usec) const; - virtual uint64_t get_ticks_usec() const; + virtual void delay_usec(uint32_t p_usec) const override; + virtual uint64_t get_ticks_usec() const override; - virtual Error execute(const String &p_path, const List<String> &p_arguments, bool p_blocking = true, ProcessID *r_child_id = nullptr, String *r_pipe = nullptr, int *r_exitcode = nullptr, bool read_stderr = false, Mutex *p_pipe_mutex = nullptr); - virtual Error kill(const ProcessID &p_pid); - virtual int get_process_id() const; + virtual Error execute(const String &p_path, const List<String> &p_arguments, bool p_blocking = true, ProcessID *r_child_id = nullptr, String *r_pipe = nullptr, int *r_exitcode = nullptr, bool read_stderr = false, Mutex *p_pipe_mutex = nullptr) override; + virtual Error kill(const ProcessID &p_pid) override; + virtual int get_process_id() const override; - virtual bool has_environment(const String &p_var) const; - virtual String get_environment(const String &p_var) const; - virtual bool set_environment(const String &p_var, const String &p_value) const; - virtual String get_locale() const; + virtual bool has_environment(const String &p_var) const override; + virtual String get_environment(const String &p_var) const override; + virtual bool set_environment(const String &p_var, const String &p_value) const override; + virtual String get_locale() const override; - virtual int get_processor_count() const; + virtual int get_processor_count() const override; - virtual void debug_break(); - virtual void initialize_debugging(); + virtual void debug_break() override; + virtual void initialize_debugging() override; - virtual String get_executable_path() const; - virtual String get_user_data_dir() const; + virtual String get_executable_path() const override; + virtual String get_user_data_dir() const override; }; class UnixTerminalLogger : public StdLogger { diff --git a/drivers/vulkan/SCsub b/drivers/vulkan/SCsub index 13fcaf16d2..14b9d63204 100644 --- a/drivers/vulkan/SCsub +++ b/drivers/vulkan/SCsub @@ -2,7 +2,7 @@ Import("env") -env.add_source_files(env.drivers_sources, "*.cpp") +thirdparty_obj = [] # FIXME: Refactor all this to reduce code duplication. if env["platform"] == "android": @@ -22,7 +22,8 @@ if env["platform"] == "android": thirdparty_dir = "#thirdparty/vulkan" vma_sources = [thirdparty_dir + "/android/vk_mem_alloc.cpp"] - env_thirdparty.add_source_files(env.drivers_sources, vma_sources) + env_thirdparty.add_source_files(thirdparty_obj, vma_sources) + elif env["platform"] == "iphone": # Use bundled Vulkan headers thirdparty_dir = "#thirdparty/vulkan" @@ -33,7 +34,8 @@ elif env["platform"] == "iphone": env_thirdparty.disable_warnings() vma_sources = [thirdparty_dir + "/vk_mem_alloc.cpp"] - env_thirdparty.add_source_files(env.drivers_sources, vma_sources) + env_thirdparty.add_source_files(thirdparty_obj, vma_sources) + elif env["builtin_vulkan"]: # Use bundled Vulkan headers thirdparty_dir = "#thirdparty/vulkan" @@ -98,8 +100,9 @@ elif env["builtin_vulkan"]: env_thirdparty.AppendUnique(CPPDEFINES=["HAVE_SECURE_GETENV"]) loader_sources = [thirdparty_dir + "/loader/" + file for file in loader_sources] - env_thirdparty.add_source_files(env.drivers_sources, loader_sources) - env_thirdparty.add_source_files(env.drivers_sources, vma_sources) + env_thirdparty.add_source_files(thirdparty_obj, loader_sources) + env_thirdparty.add_source_files(thirdparty_obj, vma_sources) + else: # Always build VMA. thirdparty_dir = "#thirdparty/vulkan" env.Prepend(CPPPATH=[thirdparty_dir]) @@ -109,4 +112,18 @@ else: # Always build VMA. env_thirdparty.disable_warnings() vma_sources = [thirdparty_dir + "/vk_mem_alloc.cpp"] - env_thirdparty.add_source_files(env.drivers_sources, vma_sources) + env_thirdparty.add_source_files(thirdparty_obj, vma_sources) + + +env.drivers_sources += thirdparty_obj + + +# Godot source files + +driver_obj = [] + +env.add_source_files(driver_obj, "*.cpp") +env.drivers_sources += driver_obj + +# Needed to force rebuilding the driver files when the thirdparty code is updated. +env.Depends(driver_obj, thirdparty_obj) diff --git a/drivers/vulkan/rendering_device_vulkan.cpp b/drivers/vulkan/rendering_device_vulkan.cpp index 2c6ba1e230..c6d49de274 100644 --- a/drivers/vulkan/rendering_device_vulkan.cpp +++ b/drivers/vulkan/rendering_device_vulkan.cpp @@ -4600,7 +4600,7 @@ RID RenderingDeviceVulkan::uniform_set_create(const Vector<Uniform> &p_uniforms, List<Vector<VkBufferView>> buffer_views; List<Vector<VkDescriptorImageInfo>> image_infos; //used for verification to make sure a uniform set does not use a framebuffer bound texture - Vector<RID> attachable_textures; + LocalVector<UniformSet::AttachableTexture> attachable_textures; Vector<Texture *> mutable_sampled_textures; Vector<Texture *> mutable_storage_textures; @@ -4693,7 +4693,10 @@ RID RenderingDeviceVulkan::uniform_set_create(const Vector<Uniform> &p_uniforms, img_info.imageView = texture->view; if (texture->usage_flags & (TEXTURE_USAGE_COLOR_ATTACHMENT_BIT | TEXTURE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT | TEXTURE_USAGE_RESOLVE_ATTACHMENT_BIT)) { - attachable_textures.push_back(texture->owner.is_valid() ? texture->owner : uniform.ids[j + 1]); + UniformSet::AttachableTexture attachable_texture; + attachable_texture.bind = set_uniform.binding; + attachable_texture.texture = texture->owner.is_valid() ? texture->owner : uniform.ids[j + 1]; + attachable_textures.push_back(attachable_texture); } if (texture->usage_flags & TEXTURE_USAGE_STORAGE_BIT) { @@ -4743,7 +4746,10 @@ RID RenderingDeviceVulkan::uniform_set_create(const Vector<Uniform> &p_uniforms, img_info.imageView = texture->view; if (texture->usage_flags & (TEXTURE_USAGE_COLOR_ATTACHMENT_BIT | TEXTURE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT | TEXTURE_USAGE_RESOLVE_ATTACHMENT_BIT)) { - attachable_textures.push_back(texture->owner.is_valid() ? texture->owner : uniform.ids[j]); + UniformSet::AttachableTexture attachable_texture; + attachable_texture.bind = set_uniform.binding; + attachable_texture.texture = texture->owner.is_valid() ? texture->owner : uniform.ids[j]; + attachable_textures.push_back(attachable_texture); } if (texture->usage_flags & TEXTURE_USAGE_STORAGE_BIT) { @@ -6168,13 +6174,13 @@ void RenderingDeviceVulkan::draw_list_bind_uniform_set(DrawListID p_list, RID p_ #ifdef DEBUG_ENABLED { //validate that textures bound are not attached as framebuffer bindings uint32_t attachable_count = uniform_set->attachable_textures.size(); - const RID *attachable_ptr = uniform_set->attachable_textures.ptr(); + const UniformSet::AttachableTexture *attachable_ptr = uniform_set->attachable_textures.ptr(); uint32_t bound_count = draw_list_bound_textures.size(); const RID *bound_ptr = draw_list_bound_textures.ptr(); for (uint32_t i = 0; i < attachable_count; i++) { for (uint32_t j = 0; j < bound_count; j++) { - ERR_FAIL_COND_MSG(attachable_ptr[i] == bound_ptr[j], - "Attempted to use the same texture in framebuffer attachment and a uniform set, this is not allowed."); + ERR_FAIL_COND_MSG(attachable_ptr[i].texture == bound_ptr[j], + "Attempted to use the same texture in framebuffer attachment and a uniform (set: " + itos(p_index) + ", binding: " + itos(attachable_ptr[i].bind) + "), this is not allowed."); } } } @@ -6378,7 +6384,7 @@ void RenderingDeviceVulkan::draw_list_enable_scissor(DrawListID p_list, const Re Rect2i rect = p_rect; rect.position += dl->viewport.position; - rect = dl->viewport.clip(rect); + rect = dl->viewport.intersection(rect); if (rect.get_area() == 0) { return; diff --git a/drivers/vulkan/rendering_device_vulkan.h b/drivers/vulkan/rendering_device_vulkan.h index 37e5e222fa..05c5b9c63e 100644 --- a/drivers/vulkan/rendering_device_vulkan.h +++ b/drivers/vulkan/rendering_device_vulkan.h @@ -32,6 +32,7 @@ #define RENDERING_DEVICE_VULKAN_H #include "core/os/thread_safe.h" +#include "core/templates/local_vector.h" #include "core/templates/oa_hash_map.h" #include "core/templates/rid_owner.h" #include "servers/rendering/rendering_device.h" @@ -45,11 +46,6 @@ #include <vulkan/vulkan.h> -//todo: -//compute -//push constants -//views of texture slices - class VulkanContext; class RenderingDeviceVulkan : public RenderingDevice { @@ -638,7 +634,12 @@ class RenderingDeviceVulkan : public RenderingDevice { DescriptorPoolKey pool_key; VkDescriptorSet descriptor_set = VK_NULL_HANDLE; //VkPipelineLayout pipeline_layout; //not owned, inherited from shader - Vector<RID> attachable_textures; //used for validation + struct AttachableTexture { + uint32_t bind; + RID texture; + }; + + LocalVector<AttachableTexture> attachable_textures; //used for validation Vector<Texture *> mutable_sampled_textures; //used for layout change Vector<Texture *> mutable_storage_textures; //used for layout change }; diff --git a/drivers/vulkan/vulkan_context.cpp b/drivers/vulkan/vulkan_context.cpp index 855c8a2d68..1f4092745a 100644 --- a/drivers/vulkan/vulkan_context.cpp +++ b/drivers/vulkan/vulkan_context.cpp @@ -493,6 +493,8 @@ Error VulkanContext::_create_physical_device() { // features based on this query vkGetPhysicalDeviceFeatures(gpu, &physical_device_features); + physical_device_features.robustBufferAccess = false; //turn off robust buffer access, which can hamper performance on some hardware + #define GET_INSTANCE_PROC_ADDR(inst, entrypoint) \ { \ fp##entrypoint = (PFN_vk##entrypoint)vkGetInstanceProcAddr(inst, "vk" #entrypoint); \ diff --git a/editor/animation_track_editor.cpp b/editor/animation_track_editor.cpp index 8933fc6e2e..ec411c6415 100644 --- a/editor/animation_track_editor.cpp +++ b/editor/animation_track_editor.cpp @@ -37,6 +37,7 @@ #include "editor/plugins/animation_player_editor_plugin.h" #include "editor_node.h" #include "editor_scale.h" +#include "scene/animation/animation_player.h" #include "scene/main/window.h" #include "servers/audio/audio_stream.h" @@ -2208,7 +2209,7 @@ void AnimationTrackEdit::draw_rect_clipped(const Rect2 &p_rect, const Color &p_c return; } Rect2 clip = Rect2(clip_left, 0, clip_right - clip_left, get_size().height); - draw_rect(clip.clip(p_rect), p_color, p_filled); + draw_rect(clip.intersection(p_rect), p_color, p_filled); } void AnimationTrackEdit::draw_bg(int p_clip_left, int p_clip_right) { @@ -2912,7 +2913,7 @@ void AnimationTrackEdit::append_to_selection(const Rect2 &p_box, bool p_deselect // Left Border including space occupied by keyframes on t=0. int limit_start_hitbox = timeline->get_name_limit() - type_icon->get_width(); Rect2 select_rect(limit_start_hitbox, 0, get_size().width - timeline->get_name_limit() - timeline->get_buttons_width(), get_size().height); - select_rect = select_rect.clip(p_box); + select_rect = select_rect.intersection(p_box); // Select should happen in the opposite order of drawing for more accurate overlap select. for (int i = animation->track_get_key_count(track) - 1; i >= 0; i--) { @@ -3299,6 +3300,19 @@ void AnimationTrackEditor::set_anim_pos(float p_pos) { bezier_edit->set_play_position(p_pos); } +static bool track_type_is_resettable(Animation::TrackType p_type) { + switch (p_type) { + case Animation::TYPE_VALUE: + [[fallthrough]]; + case Animation::TYPE_BEZIER: + [[fallthrough]]; + case Animation::TYPE_TRANSFORM: + return true; + default: + return false; + } +} + void AnimationTrackEditor::_query_insert(const InsertData &p_id) { if (insert_frame != Engine::get_singleton()->get_frames_drawn()) { //clear insert list for the frame if frame changed @@ -3319,40 +3333,58 @@ void AnimationTrackEditor::_query_insert(const InsertData &p_id) { insert_data.push_back(p_id); + bool reset_allowed = true; + AnimationPlayer *player = AnimationPlayerEditor::singleton->get_player(); + if (player->has_animation("RESET") && player->get_animation("RESET") == animation) { + // Avoid messing with the reset animation itself + reset_allowed = false; + } else { + bool some_resettable = false; + for (int i = 0; i < insert_data.size(); i++) { + if (track_type_is_resettable(insert_data[i].type)) { + some_resettable = true; + break; + } + } + if (!some_resettable) { + reset_allowed = false; + } + } + if (p_id.track_idx == -1) { - if (bool(EDITOR_DEF("editors/animation/confirm_insert_track", true))) { - //potential new key, does not exist - int num_tracks = 0; - bool all_bezier = true; - for (int i = 0; i < insert_data.size(); i++) { - if (insert_data[i].type != Animation::TYPE_VALUE && insert_data[i].type != Animation::TYPE_BEZIER) { - all_bezier = false; - } + int num_tracks = 0; + bool all_bezier = true; + for (int i = 0; i < insert_data.size(); i++) { + if (insert_data[i].type != Animation::TYPE_VALUE && insert_data[i].type != Animation::TYPE_BEZIER) { + all_bezier = false; + } - if (insert_data[i].track_idx == -1) { - ++num_tracks; - } + if (insert_data[i].track_idx == -1) { + ++num_tracks; + } - if (insert_data[i].type != Animation::TYPE_VALUE) { - continue; - } + if (insert_data[i].type != Animation::TYPE_VALUE) { + continue; + } - switch (insert_data[i].value.get_type()) { - case Variant::INT: - case Variant::FLOAT: - case Variant::VECTOR2: - case Variant::VECTOR3: - case Variant::QUAT: - case Variant::PLANE: - case Variant::COLOR: { - // Valid. - } break; - default: { - all_bezier = false; - } + switch (insert_data[i].value.get_type()) { + case Variant::INT: + case Variant::FLOAT: + case Variant::VECTOR2: + case Variant::VECTOR3: + case Variant::QUAT: + case Variant::PLANE: + case Variant::COLOR: { + // Valid. + } break; + default: { + all_bezier = false; } } + } + if (bool(EDITOR_DEF("editors/animation/confirm_insert_track", true))) { + //potential new key, does not exist if (num_tracks == 1) { insert_confirm_text->set_text(vformat(TTR("Create new track for %s and insert key?"), p_id.query)); } else { @@ -3360,23 +3392,26 @@ void AnimationTrackEditor::_query_insert(const InsertData &p_id) { } insert_confirm_bezier->set_visible(all_bezier); + insert_confirm_reset->set_visible(reset_allowed); + insert_confirm->get_ok_button()->set_text(TTR("Create")); insert_confirm->popup_centered(); insert_query = true; } else { - call_deferred("_insert_delay"); + call_deferred("_insert_delay", reset_allowed && EDITOR_GET("editors/animation/default_create_reset_tracks"), all_bezier && EDITOR_GET("editors/animation/default_create_bezier_tracks")); insert_queue = true; } } else { if (!insert_query && !insert_queue) { - call_deferred("_insert_delay"); + // Create Beziers wouldn't make sense in this case, where no tracks are being created + call_deferred("_insert_delay", reset_allowed && EDITOR_GET("editors/animation/default_create_reset_tracks"), false); insert_queue = true; } } } -void AnimationTrackEditor::_insert_delay() { +void AnimationTrackEditor::_insert_delay(bool p_create_reset, bool p_create_beziers) { if (insert_query) { //discard since it's entered into query mode insert_queue = false; @@ -3385,13 +3420,18 @@ void AnimationTrackEditor::_insert_delay() { undo_redo->create_action(TTR("Anim Insert")); - int last_track = animation->get_track_count(); + Ref<Animation> reset_anim; + if (p_create_reset) { + reset_anim = _create_and_get_reset_animation(); + } + + TrackIndices next_tracks(animation.ptr(), reset_anim.ptr()); bool advance = false; while (insert_data.size()) { if (insert_data.front()->get().advance) { advance = true; } - last_track = _confirm_insert(insert_data.front()->get(), last_track); + next_tracks = _confirm_insert(insert_data.front()->get(), next_tracks, p_create_reset, p_create_beziers); insert_data.pop_front(); } @@ -3682,12 +3722,34 @@ void AnimationTrackEditor::insert_value_key(const String &p_property, const Vari } } +Ref<Animation> AnimationTrackEditor::_create_and_get_reset_animation() { + AnimationPlayer *player = AnimationPlayerEditor::singleton->get_player(); + if (player->has_animation("RESET")) { + return player->get_animation("RESET"); + } else { + Ref<Animation> reset_anim; + reset_anim.instance(); + reset_anim->set_length(ANIM_MIN_LENGTH); + undo_redo->add_do_method(player, "add_animation", "RESET", reset_anim); + undo_redo->add_do_method(AnimationPlayerEditor::singleton, "_animation_player_changed", player); + undo_redo->add_undo_method(player, "remove_animation", "RESET"); + undo_redo->add_undo_method(AnimationPlayerEditor::singleton, "_animation_player_changed", player); + return reset_anim; + } +} + void AnimationTrackEditor::_confirm_insert_list() { undo_redo->create_action(TTR("Anim Create & Insert")); - int last_track = animation->get_track_count(); + bool create_reset = insert_confirm_reset->is_visible() && insert_confirm_reset->is_pressed(); + Ref<Animation> reset_anim; + if (create_reset) { + reset_anim = _create_and_get_reset_animation(); + } + + TrackIndices next_tracks(animation.ptr(), reset_anim.ptr()); while (insert_data.size()) { - last_track = _confirm_insert(insert_data.front()->get(), last_track, insert_confirm_bezier->is_pressed()); + next_tracks = _confirm_insert(insert_data.front()->get(), next_tracks, create_reset, insert_confirm_bezier->is_pressed()); insert_data.pop_front(); } @@ -3807,11 +3869,7 @@ static Vector<String> _get_bezier_subindices_for_type(Variant::Type p_type, bool return subindices; } -int AnimationTrackEditor::_confirm_insert(InsertData p_id, int p_last_track, bool p_create_beziers) { - if (p_last_track == -1) { - p_last_track = animation->get_track_count(); - } - +AnimationTrackEditor::TrackIndices AnimationTrackEditor::_confirm_insert(InsertData p_id, TrackIndices p_next_tracks, bool p_create_reset, bool p_create_beziers) { bool created = false; if (p_id.track_idx < 0) { if (p_create_beziers) { @@ -3823,10 +3881,10 @@ int AnimationTrackEditor::_confirm_insert(InsertData p_id, int p_last_track, boo id.type = Animation::TYPE_BEZIER; id.value = p_id.value.get(subindices[i].substr(1, subindices[i].length())); id.path = String(p_id.path) + subindices[i]; - _confirm_insert(id, p_last_track + i); + p_next_tracks = _confirm_insert(id, p_next_tracks, p_create_reset, false); } - return p_last_track + subindices.size(); + return p_next_tracks; } } created = true; @@ -3863,7 +3921,7 @@ int AnimationTrackEditor::_confirm_insert(InsertData p_id, int p_last_track, boo } } - p_id.track_idx = p_last_track; + p_id.track_idx = p_next_tracks.normal; undo_redo->add_do_method(animation.ptr(), "add_track", p_id.type); undo_redo->add_do_method(animation.ptr(), "track_set_path", p_id.track_idx, p_id.path); @@ -3915,7 +3973,7 @@ int AnimationTrackEditor::_confirm_insert(InsertData p_id, int p_last_track, boo // Just remove the track. undo_redo->add_undo_method(this, "_clear_selection", false); undo_redo->add_undo_method(animation.ptr(), "remove_track", animation->get_track_count()); - p_last_track++; + p_next_tracks.normal++; } else { undo_redo->add_undo_method(animation.ptr(), "track_remove_key_at_position", p_id.track_idx, time); int existing = animation->track_find_key(p_id.track_idx, time, true); @@ -3926,9 +3984,27 @@ int AnimationTrackEditor::_confirm_insert(InsertData p_id, int p_last_track, boo } } + if (p_create_reset && track_type_is_resettable(p_id.type)) { + bool create_reset_track = true; + Animation *reset_anim = AnimationPlayerEditor::singleton->get_player()->get_animation("RESET").ptr(); + for (int i = 0; i < reset_anim->get_track_count(); i++) { + if (reset_anim->track_get_path(i) == p_id.path) { + create_reset_track = false; + break; + } + } + if (create_reset_track) { + undo_redo->add_do_method(reset_anim, "add_track", p_id.type); + undo_redo->add_do_method(reset_anim, "track_set_path", p_next_tracks.reset, p_id.path); + undo_redo->add_do_method(reset_anim, "track_insert_key", p_next_tracks.reset, 0.0f, value); + undo_redo->add_undo_method(reset_anim, "remove_track", reset_anim->get_track_count()); + p_next_tracks.reset++; + } + } + undo_redo->commit_action(); - return p_last_track; + return p_next_tracks; } void AnimationTrackEditor::show_select_node_warning(bool p_show) { @@ -4224,6 +4300,7 @@ void AnimationTrackEditor::_notification(int p_what) { selected_filter->set_icon(get_theme_icon("AnimationFilter", "EditorIcons")); imported_anim_warning->set_icon(get_theme_icon("NodeWarning", "EditorIcons")); main_panel->add_theme_style_override("panel", get_theme_stylebox("bg", "Tree")); + edit->get_popup()->set_item_icon(edit->get_popup()->get_item_index(EDIT_APPLY_RESET), get_theme_icon("Reload", "EditorIcons")); } if (p_what == NOTIFICATION_READY) { @@ -4947,7 +5024,7 @@ void AnimationTrackEditor::_scroll_input(const Ref<InputEvent> &p_event) { Rect2 rect(from, to - from); Rect2 scroll_rect = Rect2(scroll->get_global_position(), scroll->get_size()); - rect = scroll_rect.clip(rect); + rect = scroll_rect.intersection(rect); box_selection->set_position(rect.position); box_selection->set_size(rect.size); @@ -5056,6 +5133,11 @@ void AnimationTrackEditor::_anim_duplicate_keys(bool transpose) { } } +void AnimationTrackEditor::_edit_menu_about_to_popup() { + AnimationPlayer *player = AnimationPlayerEditor::singleton->get_player(); + edit->get_popup()->set_item_disabled(edit->get_popup()->get_item_index(EDIT_APPLY_RESET), !player->can_apply_reset()); +} + void AnimationTrackEditor::_edit_menu_pressed(int p_option) { last_menu_track_opt = p_option; switch (p_option) { @@ -5378,6 +5460,10 @@ void AnimationTrackEditor::_edit_menu_pressed(int p_option) { emit_signal("timeline_changed", pos, true); } break; + case EDIT_APPLY_RESET: { + AnimationPlayerEditor::singleton->get_player()->apply_reset(true); + + } break; case EDIT_OPTIMIZE_ANIMATION: { optimize_dialog->popup_centered(Size2(250, 180) * EDSCALE); @@ -5710,10 +5796,13 @@ AnimationTrackEditor::AnimationTrackEditor() { edit->get_popup()->add_shortcut(ED_SHORTCUT("animation_editor/goto_next_step", TTR("Go to Next Step"), KEY_MASK_CMD | KEY_RIGHT), EDIT_GOTO_NEXT_STEP); edit->get_popup()->add_shortcut(ED_SHORTCUT("animation_editor/goto_prev_step", TTR("Go to Previous Step"), KEY_MASK_CMD | KEY_LEFT), EDIT_GOTO_PREV_STEP); edit->get_popup()->add_separator(); + edit->get_popup()->add_shortcut(ED_SHORTCUT("animation_editor/apply_reset", TTR("Apply Reset")), EDIT_APPLY_RESET); + edit->get_popup()->add_separator(); edit->get_popup()->add_item(TTR("Optimize Animation"), EDIT_OPTIMIZE_ANIMATION); edit->get_popup()->add_item(TTR("Clean-Up Animation"), EDIT_CLEAN_UP_ANIMATION); edit->get_popup()->connect("id_pressed", callable_mp(this, &AnimationTrackEditor::_edit_menu_pressed)); + edit->get_popup()->connect("about_to_popup", callable_mp(this, &AnimationTrackEditor::_edit_menu_about_to_popup)); pick_track = memnew(SceneTreeDialog); add_child(pick_track); @@ -5739,9 +5828,16 @@ AnimationTrackEditor::AnimationTrackEditor() { insert_confirm->add_child(icvb); insert_confirm_text = memnew(Label); icvb->add_child(insert_confirm_text); + HBoxContainer *ichb = memnew(HBoxContainer); + icvb->add_child(ichb); insert_confirm_bezier = memnew(CheckBox); insert_confirm_bezier->set_text(TTR("Use Bezier Curves")); - icvb->add_child(insert_confirm_bezier); + insert_confirm_bezier->set_pressed(EDITOR_GET("editors/animation/default_create_bezier_tracks")); + ichb->add_child(insert_confirm_bezier); + insert_confirm_reset = memnew(CheckBox); + insert_confirm_reset->set_text(TTR("Create RESET Track(s)", "")); + insert_confirm_reset->set_pressed(EDITOR_GET("editors/animation/default_create_reset_tracks")); + ichb->add_child(insert_confirm_reset); keying = false; moving_selection = false; key_edit = nullptr; diff --git a/editor/animation_track_editor.h b/editor/animation_track_editor.h index 3cb31fe21e..7006959187 100644 --- a/editor/animation_track_editor.h +++ b/editor/animation_track_editor.h @@ -47,6 +47,8 @@ #include "scene/resources/animation.h" #include "scene_tree_editor.h" +class AnimationPlayer; + class AnimationTimelineEdit : public Range { GDCLASS(AnimationTimelineEdit, Range); @@ -285,6 +287,7 @@ class AnimationTrackEditor : public VBoxContainer { EDIT_DELETE_SELECTION, EDIT_GOTO_NEXT_STEP, EDIT_GOTO_PREV_STEP, + EDIT_APPLY_RESET, EDIT_OPTIMIZE_ANIMATION, EDIT_OPTIMIZE_ANIMATION_CONFIRM, EDIT_CLEAN_UP_ANIMATION, @@ -361,6 +364,7 @@ class AnimationTrackEditor : public VBoxContainer { Label *insert_confirm_text; CheckBox *insert_confirm_bezier; + CheckBox *insert_confirm_reset; ConfirmationDialog *insert_confirm; bool insert_queue; bool inserting; @@ -369,9 +373,19 @@ class AnimationTrackEditor : public VBoxContainer { uint64_t insert_frame; void _query_insert(const InsertData &p_id); + Ref<Animation> _create_and_get_reset_animation(); void _confirm_insert_list(); - int _confirm_insert(InsertData p_id, int p_last_track, bool p_create_beziers = false); - void _insert_delay(); + struct TrackIndices { + int normal; + int reset; + + TrackIndices(const Animation *p_anim = nullptr, const Animation *p_reset_anim = nullptr) { + normal = p_anim ? p_anim->get_track_count() : 0; + reset = p_reset_anim ? p_reset_anim->get_track_count() : 0; + } + }; + TrackIndices _confirm_insert(InsertData p_id, TrackIndices p_next_tracks, bool p_create_reset, bool p_create_beziers); + void _insert_delay(bool p_create_reset, bool p_create_beziers); void _root_removed(Node *p_root); @@ -447,6 +461,7 @@ class AnimationTrackEditor : public VBoxContainer { void _select_all_tracks_for_copy(); + void _edit_menu_about_to_popup(); void _edit_menu_pressed(int p_option); int last_menu_track_opt; diff --git a/editor/editor_help_search.cpp b/editor/editor_help_search.cpp index c5d89b713c..5e784ba051 100644 --- a/editor/editor_help_search.cpp +++ b/editor/editor_help_search.cpp @@ -412,8 +412,20 @@ bool EditorHelpSearch::Runner::_phase_member_items() { ClassMatch &match = iterator_match->value(); TreeItem *parent = (search_flags & SEARCH_SHOW_HIERARCHY) ? class_items[match.doc->name] : root_item; + bool constructor_created = false; for (int i = 0; i < match.methods.size(); i++) { - _create_method_item(parent, match.doc, match.methods[i]); + String text = match.methods[i]->name; + if (!constructor_created) { + if (match.doc->name == match.methods[i]->name) { + text += " " + TTR("(constructors)"); + constructor_created = true; + } + } else { + if (match.doc->name == match.methods[i]->name) { + continue; + } + } + _create_method_item(parent, match.doc, text, match.methods[i]); } for (int i = 0; i < match.signals.size(); i++) { _create_signal_item(parent, match.doc, match.signals[i]); @@ -508,7 +520,7 @@ TreeItem *EditorHelpSearch::Runner::_create_class_item(TreeItem *p_parent, const return item; } -TreeItem *EditorHelpSearch::Runner::_create_method_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::MethodDoc *p_doc) { +TreeItem *EditorHelpSearch::Runner::_create_method_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const String &p_text, const DocData::MethodDoc *p_doc) { String tooltip = p_doc->return_type + " " + p_class_doc->name + "." + p_doc->name + "("; for (int i = 0; i < p_doc->arguments.size(); i++) { const DocData::ArgumentDoc &arg = p_doc->arguments[i]; @@ -521,7 +533,7 @@ TreeItem *EditorHelpSearch::Runner::_create_method_item(TreeItem *p_parent, cons } } tooltip += ")"; - return _create_member_item(p_parent, p_class_doc->name, "MemberMethod", p_doc->name, TTRC("Method"), "method", tooltip); + return _create_member_item(p_parent, p_class_doc->name, "MemberMethod", p_doc->name, p_text, TTRC("Method"), "method", tooltip); } TreeItem *EditorHelpSearch::Runner::_create_signal_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::MethodDoc *p_doc) { @@ -537,32 +549,32 @@ TreeItem *EditorHelpSearch::Runner::_create_signal_item(TreeItem *p_parent, cons } } tooltip += ")"; - return _create_member_item(p_parent, p_class_doc->name, "MemberSignal", p_doc->name, TTRC("Signal"), "signal", tooltip); + return _create_member_item(p_parent, p_class_doc->name, "MemberSignal", p_doc->name, p_doc->name, TTRC("Signal"), "signal", tooltip); } TreeItem *EditorHelpSearch::Runner::_create_constant_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::ConstantDoc *p_doc) { String tooltip = p_class_doc->name + "." + p_doc->name; - return _create_member_item(p_parent, p_class_doc->name, "MemberConstant", p_doc->name, TTRC("Constant"), "constant", tooltip); + return _create_member_item(p_parent, p_class_doc->name, "MemberConstant", p_doc->name, p_doc->name, TTRC("Constant"), "constant", tooltip); } TreeItem *EditorHelpSearch::Runner::_create_property_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::PropertyDoc *p_doc) { String tooltip = p_doc->type + " " + p_class_doc->name + "." + p_doc->name; tooltip += "\n " + p_class_doc->name + "." + p_doc->setter + "(value) setter"; tooltip += "\n " + p_class_doc->name + "." + p_doc->getter + "() getter"; - return _create_member_item(p_parent, p_class_doc->name, "MemberProperty", p_doc->name, TTRC("Property"), "property", tooltip); + return _create_member_item(p_parent, p_class_doc->name, "MemberProperty", p_doc->name, p_doc->name, TTRC("Property"), "property", tooltip); } TreeItem *EditorHelpSearch::Runner::_create_theme_property_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::PropertyDoc *p_doc) { String tooltip = p_doc->type + " " + p_class_doc->name + "." + p_doc->name; - return _create_member_item(p_parent, p_class_doc->name, "MemberTheme", p_doc->name, TTRC("Theme Property"), "theme_item", tooltip); + return _create_member_item(p_parent, p_class_doc->name, "MemberTheme", p_doc->name, p_doc->name, TTRC("Theme Property"), "theme_item", tooltip); } -TreeItem *EditorHelpSearch::Runner::_create_member_item(TreeItem *p_parent, const String &p_class_name, const String &p_icon, const String &p_name, const String &p_type, const String &p_metatype, const String &p_tooltip) { +TreeItem *EditorHelpSearch::Runner::_create_member_item(TreeItem *p_parent, const String &p_class_name, const String &p_icon, const String &p_name, const String &p_text, const String &p_type, const String &p_metatype, const String &p_tooltip) { Ref<Texture2D> icon; String text; if (search_flags & SEARCH_SHOW_HIERARCHY) { icon = ui_service->get_theme_icon(p_icon, "EditorIcons"); - text = p_name; + text = p_text; } else { icon = ui_service->get_theme_icon(p_icon, "EditorIcons"); /*// In flat mode, show the class icon. @@ -570,7 +582,7 @@ if (ui_service->has_icon(p_class_name, "EditorIcons")) icon = ui_service->get_icon(p_class_name, "EditorIcons"); else if (ClassDB::is_parent_class(p_class_name, "Object")) icon = ui_service->get_icon("Object", "EditorIcons");*/ - text = p_class_name + "." + p_name; + text = p_class_name + "." + p_text; } TreeItem *item = results_tree->create_item(p_parent); diff --git a/editor/editor_help_search.h b/editor/editor_help_search.h index cb52c515de..d94066308a 100644 --- a/editor/editor_help_search.h +++ b/editor/editor_help_search.h @@ -140,12 +140,12 @@ class EditorHelpSearch::Runner : public Reference { void _match_item(TreeItem *p_item, const String &p_text); TreeItem *_create_class_hierarchy(const ClassMatch &p_match); TreeItem *_create_class_item(TreeItem *p_parent, const DocData::ClassDoc *p_doc, bool p_gray); - TreeItem *_create_method_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::MethodDoc *p_doc); + TreeItem *_create_method_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const String &p_text, const DocData::MethodDoc *p_doc); TreeItem *_create_signal_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::MethodDoc *p_doc); TreeItem *_create_constant_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::ConstantDoc *p_doc); TreeItem *_create_property_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::PropertyDoc *p_doc); TreeItem *_create_theme_property_item(TreeItem *p_parent, const DocData::ClassDoc *p_class_doc, const DocData::PropertyDoc *p_doc); - TreeItem *_create_member_item(TreeItem *p_parent, const String &p_class_name, const String &p_icon, const String &p_name, const String &p_type, const String &p_metatype, const String &p_tooltip); + TreeItem *_create_member_item(TreeItem *p_parent, const String &p_class_name, const String &p_icon, const String &p_name, const String &p_text, const String &p_type, const String &p_metatype, const String &p_tooltip); public: bool work(uint64_t slot = 100000); diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp index 64b5f50b91..a8dc14427d 100644 --- a/editor/editor_node.cpp +++ b/editor/editor_node.cpp @@ -525,6 +525,9 @@ void EditorNode::_notification(int p_what) { scene_root->set_sdf_oversize(sdf_oversize); Viewport::SDFScale sdf_scale = Viewport::SDFScale(int(GLOBAL_GET("rendering/quality/2d_sdf/scale"))); scene_root->set_sdf_scale(sdf_scale); + + float lod_threshold = GLOBAL_GET("rendering/quality/mesh_lod/threshold_pixels"); + scene_root->set_lod_threshold(lod_threshold); } ResourceImporterTexture::get_singleton()->update_imports(); @@ -1416,6 +1419,17 @@ int EditorNode::_save_external_resources() { return saved; } +static void _reset_animation_players(Node *p_node, List<Ref<AnimatedValuesBackup>> *r_anim_backups) { + for (int i = 0; i < p_node->get_child_count(); i++) { + AnimationPlayer *player = Object::cast_to<AnimationPlayer>(p_node->get_child(i)); + if (player && player->is_reset_on_save_enabled() && player->can_apply_reset()) { + Ref<AnimatedValuesBackup> old_values = player->apply_reset(); + r_anim_backups->push_back(old_values); + } + _reset_animation_players(p_node->get_child(i), r_anim_backups); + } +} + void EditorNode::_save_scene(String p_file, int idx) { Node *scene = editor_data.get_edited_scene_root(idx); @@ -1430,6 +1444,8 @@ void EditorNode::_save_scene(String p_file, int idx) { } editor_data.apply_changes_in_editors(); + List<Ref<AnimatedValuesBackup>> anim_backups; + _reset_animation_players(scene, &anim_backups); _save_default_environment(); _set_scene_metadata(p_file, idx); @@ -1477,6 +1493,11 @@ void EditorNode::_save_scene(String p_file, int idx) { _save_external_resources(); editor_data.save_editor_external_data(); + + for (List<Ref<AnimatedValuesBackup>>::Element *E = anim_backups.front(); E; E = E->next()) { + E->get()->restore(); + } + if (err == OK) { scene->set_filename(ProjectSettings::get_singleton()->localize_path(p_file)); if (idx < 0 || idx == editor_data.get_edited_scene()) { diff --git a/editor/editor_properties.cpp b/editor/editor_properties.cpp index 2c4e403a81..523e65babe 100644 --- a/editor/editor_properties.cpp +++ b/editor/editor_properties.cpp @@ -68,9 +68,9 @@ void EditorPropertyText::_text_changed(const String &p_string) { } if (string_name) { - emit_changed(get_edited_property(), StringName(p_string), "", true); + emit_changed(get_edited_property(), StringName(p_string), "", false); } else { - emit_changed(get_edited_property(), p_string, "", true); + emit_changed(get_edited_property(), p_string, "", false); } } diff --git a/editor/editor_settings.cpp b/editor/editor_settings.cpp index 0840707886..73a191d317 100644 --- a/editor/editor_settings.cpp +++ b/editor/editor_settings.cpp @@ -549,9 +549,15 @@ void EditorSettings::_load_defaults(Ref<ConfigFile> p_extra_config) { _initial_set("editors/3d/grid_xy_plane", false); _initial_set("editors/3d/grid_yz_plane", false); + // Use a lower default FOV for the 3D camera compared to the + // Camera3D node as the 3D viewport doesn't span the whole screen. + // This means it's technically viewed from a further distance, which warrants a narrower FOV. _initial_set("editors/3d/default_fov", 70.0); + hints["editors/3d/default_fov"] = PropertyInfo(Variant::FLOAT, "editors/3d/default_fov", PROPERTY_HINT_RANGE, "1,179,0.1"); _initial_set("editors/3d/default_z_near", 0.05); - _initial_set("editors/3d/default_z_far", 500.0); + hints["editors/3d/default_z_near"] = PropertyInfo(Variant::FLOAT, "editors/3d/default_z_near", PROPERTY_HINT_RANGE, "0.01,10,0.01,or_greater"); + _initial_set("editors/3d/default_z_far", 4000.0); + hints["editors/3d/default_z_far"] = PropertyInfo(Variant::FLOAT, "editors/3d/default_z_far", PROPERTY_HINT_RANGE, "0.1,4000,0.1,or_greater"); // 3D: Navigation _initial_set("editors/3d/navigation/navigation_scheme", 0); @@ -622,6 +628,8 @@ void EditorSettings::_load_defaults(Ref<ConfigFile> p_extra_config) { // Animation _initial_set("editors/animation/autorename_animation_tracks", true); _initial_set("editors/animation/confirm_insert_track", true); + _initial_set("editors/animation/default_create_bezier_tracks", false); + _initial_set("editors/animation/default_create_reset_tracks", true); _initial_set("editors/animation/onion_layers_past_color", Color(1, 0, 0)); _initial_set("editors/animation/onion_layers_future_color", Color(0, 1, 0)); diff --git a/editor/icons/GuiToggleOn.svg b/editor/icons/GuiToggleOn.svg index 8ab0998f71..37b47e8de4 100644 --- a/editor/icons/GuiToggleOn.svg +++ b/editor/icons/GuiToggleOn.svg @@ -1 +1 @@ -<svg height="16" viewBox="0 0 38 15.999999" width="38" xmlns="http://www.w3.org/2000/svg"><path d="m8 1c-3.878 0-7 3.122-7 7s3.122 7 7 7h22c3.878 0 7-3.122 7-7s-3.122-7-7-7zm22 2a5 5 0 0 1 5 5 5 5 0 0 1 -5 5 5 5 0 0 1 -5-5 5 5 0 0 1 5-5z" fill="#e0e0e0" stroke-width="55.8958"/></svg> +<svg height="16" viewBox="0 0 38 15.999999" width="38" xmlns="http://www.w3.org/2000/svg"><path d="m8 1c-4 0-7 3.0000002-7 7.0000002 0 3.9999998 3 6.9999998 7 6.9999998h22c4 0 7-3 7-6.9999998 0-4-3-7.0000002-7-7.0000002-7.333334 0-14.55609 0-22 0z" fill="#699ce8"/><circle cx="30" cy="8" fill="#fefefe" r="5"/></svg> diff --git a/editor/icons/GuiToggleOnMirrored.svg b/editor/icons/GuiToggleOnMirrored.svg index 7339b6efd2..fa7f602ee7 100644 --- a/editor/icons/GuiToggleOnMirrored.svg +++ b/editor/icons/GuiToggleOnMirrored.svg @@ -1 +1 @@ -<svg xmlns="http://www.w3.org/2000/svg" width="42" height="26"><path fill="#e0e0e0" d="M31 5c4.986 0 9 3.568 9 8s-4.014 8-9 8H11c-4.986 0-9-3.568-9-8s4.014-8 9-8zM10 8a-5 5 0 0 0-5 5-5 5 0 0 0 5 5-5 5 0 0 0 5-5-5 5 0 0 0-5-5z"/></svg> +<svg height="26" width="42" xmlns="http://www.w3.org/2000/svg"><path d="m31 5c4.986 0 9 3.568 9 8s-4.014 8-9 8h-20c-4.986 0-9-3.568-9-8s4.014-8 9-8z" fill="#699ce8"/><circle cx="10" cy="13" fill="#fefefe" r="5"/></svg> diff --git a/editor/import/collada.h b/editor/import/collada.h index 3b6b508b28..29d49d4aa7 100644 --- a/editor/import/collada.h +++ b/editor/import/collada.h @@ -96,8 +96,8 @@ public: }; float aspect = 1; - float z_near = 0.1; - float z_far = 100; + float z_near = 0.05; + float z_far = 4000; CameraData() {} }; diff --git a/editor/import/editor_scene_importer_gltf.h b/editor/import/editor_scene_importer_gltf.h index 6390f46524..e6163a46be 100644 --- a/editor/import/editor_scene_importer_gltf.h +++ b/editor/import/editor_scene_importer_gltf.h @@ -205,9 +205,9 @@ class EditorSceneImporterGLTF : public EditorSceneImporter { struct GLTFCamera { bool perspective = true; - float fov_size = 64; - float zfar = 500; - float znear = 0.1; + float fov_size = 75; + float zfar = 4000; + float znear = 0.05; }; struct GLTFLight { diff --git a/editor/import/resource_importer_scene.cpp b/editor/import/resource_importer_scene.cpp index 5abae339df..b591627660 100644 --- a/editor/import/resource_importer_scene.cpp +++ b/editor/import/resource_importer_scene.cpp @@ -44,6 +44,7 @@ #include "scene/resources/ray_shape_3d.h" #include "scene/resources/resource_format_text.h" #include "scene/resources/sphere_shape_3d.h" +#include "scene/resources/surface_tool.h" #include "scene/resources/world_margin_shape_3d.h" uint32_t EditorSceneImporter::get_import_flags() const { @@ -217,6 +218,47 @@ Ref<Material> EditorSceneImporterMesh::get_surface_material(int p_surface) const return surfaces[p_surface].material; } +void EditorSceneImporterMesh::generate_lods() { + if (!SurfaceTool::simplify_func) { + return; + } + + for (int i = 0; i < surfaces.size(); i++) { + if (surfaces[i].primitive != Mesh::PRIMITIVE_TRIANGLES) { + continue; + } + + surfaces.write[i].lods.clear(); + Vector<Vector3> vertices = surfaces[i].arrays[RS::ARRAY_VERTEX]; + Vector<int> indices = surfaces[i].arrays[RS::ARRAY_INDEX]; + if (indices.size() == 0) { + continue; //no lods if no indices + } + uint32_t vertex_count = vertices.size(); + const Vector3 *vertices_ptr = vertices.ptr(); + + int min_indices = 10; + int index_target = indices.size() / 2; + print_line("total: " + itos(indices.size())); + while (index_target > min_indices) { + float error; + Vector<int> new_indices; + new_indices.resize(indices.size()); + size_t new_len = SurfaceTool::simplify_func((unsigned int *)new_indices.ptrw(), (const unsigned int *)indices.ptr(), indices.size(), (const float *)vertices_ptr, vertex_count, sizeof(Vector3), index_target, 1e20, &error); + print_line("shoot for " + itos(index_target) + ", got " + itos(new_len) + " distance " + rtos(error)); + if ((int)new_len > (index_target * 120 / 100)) { + break; // 20 percent tolerance + } + new_indices.resize(new_len); + Surface::LOD lod; + lod.distance = error; + lod.indices = new_indices; + surfaces.write[i].lods.push_back(lod); + index_target /= 2; + } + } +} + bool EditorSceneImporterMesh::has_mesh() const { return mesh.is_valid(); } @@ -1422,9 +1464,9 @@ void ResourceImporterScene::get_import_options(List<ImportOption> *r_options, in r_options->push_back(ImportOption(PropertyInfo(Variant::INT, "materials/location", PROPERTY_HINT_ENUM, "Node,Mesh"), (meshes_out || materials_out) ? 1 : 0)); r_options->push_back(ImportOption(PropertyInfo(Variant::INT, "materials/storage", PROPERTY_HINT_ENUM, "Built-In,Files (.material),Files (.tres)", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_UPDATE_ALL_IF_MODIFIED), materials_out ? 1 : 0)); r_options->push_back(ImportOption(PropertyInfo(Variant::BOOL, "materials/keep_on_reimport"), materials_out)); - r_options->push_back(ImportOption(PropertyInfo(Variant::BOOL, "meshes/compress"), true)); r_options->push_back(ImportOption(PropertyInfo(Variant::BOOL, "meshes/ensure_tangents"), true)); r_options->push_back(ImportOption(PropertyInfo(Variant::INT, "meshes/storage", PROPERTY_HINT_ENUM, "Built-In,Files (.mesh),Files (.tres)"), meshes_out ? 1 : 0)); + r_options->push_back(ImportOption(PropertyInfo(Variant::BOOL, "meshes/generate_lods"), true)); r_options->push_back(ImportOption(PropertyInfo(Variant::INT, "meshes/light_baking", PROPERTY_HINT_ENUM, "Disabled,Enable,Gen Lightmaps", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_UPDATE_ALL_IF_MODIFIED), 0)); r_options->push_back(ImportOption(PropertyInfo(Variant::FLOAT, "meshes/lightmap_texel_size", PROPERTY_HINT_RANGE, "0.001,100,0.001"), 0.1)); r_options->push_back(ImportOption(PropertyInfo(Variant::BOOL, "skins/use_named_skins"), true)); @@ -1517,7 +1559,7 @@ Ref<Animation> ResourceImporterScene::import_animation_from_other_importer(Edito return importer->import_animation(p_path, p_flags, p_bake_fps); } -void ResourceImporterScene::_generate_meshes(Node *p_node) { +void ResourceImporterScene::_generate_meshes(Node *p_node, bool p_generate_lods) { EditorSceneImporterMeshNode *src_mesh = Object::cast_to<EditorSceneImporterMeshNode>(p_node); if (src_mesh != nullptr) { //is mesh @@ -1528,6 +1570,9 @@ void ResourceImporterScene::_generate_meshes(Node *p_node) { Ref<ArrayMesh> mesh; if (!src_mesh->get_mesh()->has_mesh()) { + if (p_generate_lods) { + src_mesh->get_mesh()->generate_lods(); + } //do mesh processing } mesh = src_mesh->get_mesh()->get_mesh(); @@ -1542,7 +1587,7 @@ void ResourceImporterScene::_generate_meshes(Node *p_node) { } for (int i = 0; i < p_node->get_child_count(); i++) { - _generate_meshes(p_node->get_child(i)); + _generate_meshes(p_node->get_child(i), p_generate_lods); } } Error ResourceImporterScene::import(const String &p_source_file, const String &p_save_path, const Map<StringName, Variant> &p_options, List<String> *r_platform_variants, List<String> *r_gen_files, Variant *r_metadata) { @@ -1583,10 +1628,6 @@ Error ResourceImporterScene::import(const String &p_source_file, const String &p import_flags |= EditorSceneImporter::IMPORT_ANIMATION; } - if (int(p_options["meshes/compress"])) { - import_flags |= EditorSceneImporter::IMPORT_USE_COMPRESSION; - } - if (bool(p_options["meshes/ensure_tangents"])) { import_flags |= EditorSceneImporter::IMPORT_GENERATE_TANGENT_ARRAYS; } @@ -1641,7 +1682,9 @@ Error ResourceImporterScene::import(const String &p_source_file, const String &p scene->set_name(p_save_path.get_file().get_basename()); } - _generate_meshes(scene); + bool gen_lods = bool(p_options["meshes/generate_lods"]); + + _generate_meshes(scene, gen_lods); err = OK; diff --git a/editor/import/resource_importer_scene.h b/editor/import/resource_importer_scene.h index 758390b367..aef6c0ac50 100644 --- a/editor/import/resource_importer_scene.h +++ b/editor/import/resource_importer_scene.h @@ -144,6 +144,8 @@ public: float get_surface_lod_size(int p_surface, int p_lod) const; Ref<Material> get_surface_material(int p_surface) const; + void generate_lods(); + bool has_mesh() const; Ref<ArrayMesh> get_mesh(); void clear(); @@ -205,7 +207,7 @@ class ResourceImporterScene : public ResourceImporter { }; void _replace_owner(Node *p_node, Node *p_scene, Node *p_new_owner); - void _generate_meshes(Node *p_node); + void _generate_meshes(Node *p_node, bool p_generate_lods); public: static ResourceImporterScene *get_singleton() { return singleton; } diff --git a/editor/plugins/animation_player_editor_plugin.cpp b/editor/plugins/animation_player_editor_plugin.cpp index 7e376eee57..4b97dacbc1 100644 --- a/editor/plugins/animation_player_editor_plugin.cpp +++ b/editor/plugins/animation_player_editor_plugin.cpp @@ -116,6 +116,19 @@ void AnimationPlayerEditor::_notification(int p_what) { play_bw_from->set_icon(get_theme_icon("PlayBackwards", "EditorIcons")); autoplay_icon = get_theme_icon("AutoPlay", "EditorIcons"); + reset_icon = get_theme_icon("Reload", "EditorIcons"); + { + Ref<Image> autoplay_img = autoplay_icon->get_data(); + Ref<Image> reset_img = reset_icon->get_data(); + Ref<Image> autoplay_reset_img; + Size2 icon_size = Size2(autoplay_img->get_width(), autoplay_img->get_height()); + autoplay_reset_img.instance(); + autoplay_reset_img->create(icon_size.x * 2, icon_size.y, false, autoplay_img->get_format()); + autoplay_reset_img->blit_rect(autoplay_img, Rect2(Point2(), icon_size), Point2()); + autoplay_reset_img->blit_rect(reset_img, Rect2(Point2(), icon_size), Point2(icon_size.x, 0)); + autoplay_reset_icon.instance(); + autoplay_reset_icon->create_from_image(autoplay_reset_img); + } stop->set_icon(get_theme_icon("Stop", "EditorIcons")); onion_toggle->set_icon(get_theme_icon("Onion", "EditorIcons")); @@ -817,11 +830,17 @@ void AnimationPlayerEditor::_update_player() { int active_idx = -1; for (List<StringName>::Element *E = animlist.front(); E; E = E->next()) { - if (player->get_autoplay() == E->get()) { - animation->add_icon_item(autoplay_icon, E->get()); - } else { - animation->add_item(E->get()); + Ref<Texture2D> icon; + if (E->get() == player->get_autoplay()) { + if (E->get() == "RESET") { + icon = autoplay_reset_icon; + } else { + icon = autoplay_icon; + } + } else if (E->get() == "RESET") { + icon = reset_icon; } + animation->add_icon_item(icon, E->get()); if (player->get_assigned_animation() == E->get()) { active_idx = animation->get_item_count() - 1; @@ -1375,7 +1394,7 @@ void AnimationPlayerEditor::_prepare_onion_layers_2() { } // Backup current animation state. - AnimatedValuesBackup values_backup = player->backup_animated_values(); + Ref<AnimatedValuesBackup> values_backup = player->backup_animated_values(); float cpos = player->get_current_animation_position(); // Render every past/future step with the capture shader. @@ -1405,7 +1424,7 @@ void AnimationPlayerEditor::_prepare_onion_layers_2() { if (valid) { player->seek(pos, true); get_tree()->flush_transform_notifications(); // Needed for transforms of Node3Ds. - values_backup.update_skeletons(); // Needed for Skeletons (2D & 3D). + values_backup->update_skeletons(); // Needed for Skeletons (2D & 3D). RS::get_singleton()->viewport_set_active(onion.captures[cidx], true); RS::get_singleton()->viewport_set_parent_viewport(root_vp, onion.captures[cidx]); @@ -1425,7 +1444,7 @@ void AnimationPlayerEditor::_prepare_onion_layers_2() { // (Seeking with update=true wouldn't do the trick because the current value of the properties // may not match their value for the current point in the animation). player->seek(cpos, false); - player->restore_animated_values(values_backup); + values_backup->restore(); // Restore state of main editors. if (Node3DEditor::get_singleton()->is_visible()) { diff --git a/editor/plugins/animation_player_editor_plugin.h b/editor/plugins/animation_player_editor_plugin.h index 17e554ee0d..ab3feb115f 100644 --- a/editor/plugins/animation_player_editor_plugin.h +++ b/editor/plugins/animation_player_editor_plugin.h @@ -105,6 +105,8 @@ class AnimationPlayerEditor : public VBoxContainer { Label *name_title; UndoRedo *undo_redo; Ref<Texture2D> autoplay_icon; + Ref<Texture2D> reset_icon; + Ref<ImageTexture> autoplay_reset_icon; bool last_active; float timeline_position; diff --git a/editor/plugins/node_3d_editor_plugin.cpp b/editor/plugins/node_3d_editor_plugin.cpp index ff3b50303f..4d7be66180 100644 --- a/editor/plugins/node_3d_editor_plugin.cpp +++ b/editor/plugins/node_3d_editor_plugin.cpp @@ -2705,7 +2705,7 @@ void Node3DEditorViewport::_draw() { } break; } - draw_rect = Rect2(Vector2(), s).clip(draw_rect); + draw_rect = Rect2(Vector2(), s).intersection(draw_rect); surface->draw_rect(draw_rect, Color(0.6, 0.6, 0.1, 0.5), false, Math::round(2 * EDSCALE)); @@ -3017,7 +3017,8 @@ void Node3DEditorViewport::_menu_option(int p_option) { case VIEW_DISPLAY_DEBUG_DECAL_ATLAS: case VIEW_DISPLAY_DEBUG_SDFGI: case VIEW_DISPLAY_DEBUG_SDFGI_PROBES: - case VIEW_DISPLAY_DEBUG_GI_BUFFER: { + case VIEW_DISPLAY_DEBUG_GI_BUFFER: + case VIEW_DISPLAY_DEBUG_DISABLE_LOD: { static const int display_options[] = { VIEW_DISPLAY_NORMAL, VIEW_DISPLAY_WIREFRAME, @@ -3034,6 +3035,7 @@ void Node3DEditorViewport::_menu_option(int p_option) { VIEW_DISPLAY_DEBUG_SCENE_LUMINANCE, VIEW_DISPLAY_DEBUG_SSAO, VIEW_DISPLAY_DEBUG_GI_BUFFER, + VIEW_DISPLAY_DEBUG_DISABLE_LOD, VIEW_DISPLAY_DEBUG_PSSM_SPLITS, VIEW_DISPLAY_DEBUG_DECAL_ATLAS, VIEW_DISPLAY_DEBUG_SDFGI, @@ -3056,6 +3058,7 @@ void Node3DEditorViewport::_menu_option(int p_option) { Viewport::DEBUG_DRAW_SCENE_LUMINANCE, Viewport::DEBUG_DRAW_SSAO, Viewport::DEBUG_DRAW_GI_BUFFER, + Viewport::DEBUG_DRAW_DISABLE_LOD, Viewport::DEBUG_DRAW_PSSM_SPLITS, Viewport::DEBUG_DRAW_DECAL_ATLAS, Viewport::DEBUG_DRAW_SDFGI, @@ -3959,6 +3962,8 @@ Node3DEditorViewport::Node3DEditorViewport(Node3DEditor *p_spatial_editor, Edito display_submenu->add_radio_check_item(TTR("SSAO"), VIEW_DISPLAY_DEBUG_SSAO); display_submenu->add_separator(); display_submenu->add_radio_check_item(TTR("GI Buffer"), VIEW_DISPLAY_DEBUG_GI_BUFFER); + display_submenu->add_separator(); + display_submenu->add_radio_check_item(TTR("Disable LOD"), VIEW_DISPLAY_DEBUG_DISABLE_LOD); display_submenu->set_name("display_advanced"); view_menu->get_popup()->add_submenu_item(TTR("Display Advanced..."), "display_advanced", VIEW_DISPLAY_ADVANCED); view_menu->get_popup()->add_separator(); @@ -6176,9 +6181,9 @@ void Node3DEditor::_bind_methods() { } void Node3DEditor::clear() { - settings_fov->set_value(EDITOR_DEF("editors/3d/default_fov", 70.0)); - settings_znear->set_value(EDITOR_DEF("editors/3d/default_z_near", 0.05)); - settings_zfar->set_value(EDITOR_DEF("editors/3d/default_z_far", 1500.0)); + settings_fov->set_value(EDITOR_GET("editors/3d/default_fov")); + settings_znear->set_value(EDITOR_GET("editors/3d/default_z_near")); + settings_zfar->set_value(EDITOR_GET("editors/3d/default_z_far")); for (uint32_t i = 0; i < VIEWPORTS_COUNT; i++) { viewports[i]->reset(); @@ -6470,22 +6475,22 @@ Node3DEditor::Node3DEditor(EditorNode *p_editor) { settings_fov = memnew(SpinBox); settings_fov->set_max(MAX_FOV); settings_fov->set_min(MIN_FOV); - settings_fov->set_step(0.01); - settings_fov->set_value(EDITOR_DEF("editors/3d/default_fov", 70.0)); + settings_fov->set_step(0.1); + settings_fov->set_value(EDITOR_GET("editors/3d/default_fov")); settings_vbc->add_margin_child(TTR("Perspective FOV (deg.):"), settings_fov); settings_znear = memnew(SpinBox); settings_znear->set_max(MAX_Z); settings_znear->set_min(MIN_Z); settings_znear->set_step(0.01); - settings_znear->set_value(EDITOR_DEF("editors/3d/default_z_near", 0.05)); + settings_znear->set_value(EDITOR_GET("editors/3d/default_z_near")); settings_vbc->add_margin_child(TTR("View Z-Near:"), settings_znear); settings_zfar = memnew(SpinBox); settings_zfar->set_max(MAX_Z); settings_zfar->set_min(MIN_Z); - settings_zfar->set_step(0.01); - settings_zfar->set_value(EDITOR_DEF("editors/3d/default_z_far", 1500)); + settings_zfar->set_step(0.1); + settings_zfar->set_value(EDITOR_GET("editors/3d/default_z_far")); settings_vbc->add_margin_child(TTR("View Z-Far:"), settings_zfar); for (uint32_t i = 0; i < VIEWPORTS_COUNT; ++i) { diff --git a/editor/plugins/node_3d_editor_plugin.h b/editor/plugins/node_3d_editor_plugin.h index 66ee678154..079c86ceb4 100644 --- a/editor/plugins/node_3d_editor_plugin.h +++ b/editor/plugins/node_3d_editor_plugin.h @@ -212,6 +212,7 @@ class Node3DEditorViewport : public Control { VIEW_DISPLAY_DEBUG_SDFGI, VIEW_DISPLAY_DEBUG_SDFGI_PROBES, VIEW_DISPLAY_DEBUG_GI_BUFFER, + VIEW_DISPLAY_DEBUG_DISABLE_LOD, VIEW_LOCK_ROTATION, VIEW_CINEMATIC_PREVIEW, VIEW_AUTO_ORTHOGONAL, diff --git a/editor/plugins/version_control_editor_plugin.cpp b/editor/plugins/version_control_editor_plugin.cpp index 27ed279edb..7747c740df 100644 --- a/editor/plugins/version_control_editor_plugin.cpp +++ b/editor/plugins/version_control_editor_plugin.cpp @@ -119,19 +119,13 @@ void VersionControlEditorPlugin::_initialize_vcs() { } void VersionControlEditorPlugin::_send_commit_msg() { - String msg = commit_message->get_text(); - if (msg == "") { - commit_status->set_text(TTR("No commit message was provided")); - return; - } - if (EditorVCSInterface::get_singleton()) { if (staged_files_count == 0) { commit_status->set_text(TTR("No files added to stage")); return; } - EditorVCSInterface::get_singleton()->commit(msg); + EditorVCSInterface::get_singleton()->commit(commit_message->get_text()); commit_message->set_text(""); version_control_dock_button->set_pressed(false); @@ -294,6 +288,10 @@ void VersionControlEditorPlugin::_update_commit_status() { staged_files_count = 0; } +void VersionControlEditorPlugin::_update_commit_button() { + commit_button->set_disabled(commit_message->get_text().strip_edges() == ""); +} + void VersionControlEditorPlugin::register_editor() { if (!EditorVCSInterface::get_singleton()) { EditorNode::get_singleton()->add_control_to_dock(EditorNode::DOCK_SLOT_RIGHT_UL, version_commit_dock); @@ -463,11 +461,12 @@ VersionControlEditorPlugin::VersionControlEditorPlugin() { commit_message->set_v_grow_direction(Control::GrowDirection::GROW_DIRECTION_END); commit_message->set_custom_minimum_size(Size2(200, 100)); commit_message->set_wrap_enabled(true); - commit_message->set_text(TTR("Add a commit message")); + commit_message->connect("text_changed", callable_mp(this, &VersionControlEditorPlugin::_update_commit_button)); commit_box_vbc->add_child(commit_message); commit_button = memnew(Button); commit_button->set_text(TTR("Commit Changes")); + commit_button->set_disabled(true); commit_button->connect("pressed", callable_mp(this, &VersionControlEditorPlugin::_send_commit_msg)); commit_box_vbc->add_child(commit_button); diff --git a/editor/plugins/version_control_editor_plugin.h b/editor/plugins/version_control_editor_plugin.h index 34643e85e4..3f107ddffb 100644 --- a/editor/plugins/version_control_editor_plugin.h +++ b/editor/plugins/version_control_editor_plugin.h @@ -110,6 +110,7 @@ private: void _clear_file_diff(); void _update_stage_status(); void _update_commit_status(); + void _update_commit_button(); friend class EditorVCSInterface; diff --git a/modules/arkit/arkit_interface.mm b/modules/arkit/arkit_interface.mm index 6d69f4a2f4..1c42e6e008 100644 --- a/modules/arkit/arkit_interface.mm +++ b/modules/arkit/arkit_interface.mm @@ -398,14 +398,14 @@ XRPositionalTracker *ARKitInterface::get_anchor_for_uuid(const unsigned char *p_ } XRPositionalTracker *new_tracker = memnew(XRPositionalTracker); - new_tracker->set_type(XRServer::TRACKER_ANCHOR); + new_tracker->set_tracker_type(XRServer::TRACKER_ANCHOR); char tracker_name[256]; sprintf(tracker_name, "Anchor %02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-%02x%02x%02x%02x%02x%02x", p_uuid[0], p_uuid[1], p_uuid[2], p_uuid[3], p_uuid[4], p_uuid[5], p_uuid[6], p_uuid[7], p_uuid[8], p_uuid[9], p_uuid[10], p_uuid[11], p_uuid[12], p_uuid[13], p_uuid[14], p_uuid[15]); String name = tracker_name; print_line("Adding tracker " + name); - new_tracker->set_name(name); + new_tracker->set_tracker_name(name); // add our tracker XRServer::get_singleton()->add_tracker(new_tracker); diff --git a/modules/assimp/SCsub b/modules/assimp/SCsub index f1d0c742b4..7213efb74d 100644 --- a/modules/assimp/SCsub +++ b/modules/assimp/SCsub @@ -5,8 +5,13 @@ Import("env_modules") env_assimp = env_modules.Clone() +# Thirdparty source files + +thirdparty_obj = [] + # Force bundled version for now, there's no released version of Assimp with # support for ArmaturePopulate which we use from their master branch. + if True: # env['builtin_assimp']: thirdparty_dir = "#thirdparty/assimp" @@ -84,11 +89,20 @@ if True: # env['builtin_assimp']: env_thirdparty = env_assimp.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, Glob("#thirdparty/assimp/code/CApi/*.cpp")) - env_thirdparty.add_source_files(env.modules_sources, Glob("#thirdparty/assimp/code/Common/*.cpp")) - env_thirdparty.add_source_files(env.modules_sources, Glob("#thirdparty/assimp/code/PostProcessing/*.cpp")) - env_thirdparty.add_source_files(env.modules_sources, Glob("#thirdparty/assimp/code/Material/*.cpp")) - env_thirdparty.add_source_files(env.modules_sources, Glob("#thirdparty/assimp/code/FBX/*.cpp")) - -# Godot's own source files -env_assimp.add_source_files(env.modules_sources, "*.cpp") + env_thirdparty.add_source_files(thirdparty_obj, Glob("#thirdparty/assimp/code/CApi/*.cpp")) + env_thirdparty.add_source_files(thirdparty_obj, Glob("#thirdparty/assimp/code/Common/*.cpp")) + env_thirdparty.add_source_files(thirdparty_obj, Glob("#thirdparty/assimp/code/PostProcessing/*.cpp")) + env_thirdparty.add_source_files(thirdparty_obj, Glob("#thirdparty/assimp/code/Material/*.cpp")) + env_thirdparty.add_source_files(thirdparty_obj, Glob("#thirdparty/assimp/code/FBX/*.cpp")) + env.modules_sources += thirdparty_obj + + +# Godot source files + +module_obj = [] + +env_assimp.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/basis_universal/SCsub b/modules/basis_universal/SCsub index dc7b176d24..351628a0e3 100644 --- a/modules/basis_universal/SCsub +++ b/modules/basis_universal/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_basisu = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + # Not unbundled so far since not widespread as shared library thirdparty_dir = "#thirdparty/basis_universal/" tool_sources = [ @@ -41,8 +44,16 @@ if env["target"] == "debug": env_thirdparty = env_basisu.Clone() env_thirdparty.disable_warnings() if env["tools"]: - env_thirdparty.add_source_files(env.modules_sources, tool_sources) -env_thirdparty.add_source_files(env.modules_sources, transcoder_sources) + env_thirdparty.add_source_files(thirdparty_obj, tool_sources) +env_thirdparty.add_source_files(thirdparty_obj, transcoder_sources) +env.modules_sources += thirdparty_obj # Godot source files -env_basisu.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_basisu.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/bullet/SCsub b/modules/bullet/SCsub index 21bdcca18e..bfac0df5b0 100644 --- a/modules/bullet/SCsub +++ b/modules/bullet/SCsub @@ -7,6 +7,8 @@ env_bullet = env_modules.Clone() # Thirdparty source files +thirdparty_obj = [] + if env["builtin_bullet"]: # Build only version 2 for now (as of 2.89) # Sync file list with relevant upstream CMakeLists.txt for each folder. @@ -208,8 +210,16 @@ if env["builtin_bullet"]: env_thirdparty = env_bullet.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj # Godot source files -env_bullet.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_bullet.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp index 0c64c3640f..284a22717b 100644 --- a/modules/bullet/rigid_body_bullet.cpp +++ b/modules/bullet/rigid_body_bullet.cpp @@ -323,9 +323,6 @@ void RigidBodyBullet::set_space(SpaceBullet *p_space) { can_integrate_forces = false; isScratchedSpaceOverrideModificator = false; - // Remove all eventual constraints - assert_no_constraints(); - // Remove this object form the physics world space->remove_rigid_body(this); } @@ -443,12 +440,6 @@ bool RigidBodyBullet::was_colliding(RigidBodyBullet *p_other_object) { return false; } -void RigidBodyBullet::assert_no_constraints() { - if (btBody->getNumConstraintRefs()) { - WARN_PRINT("A body with a joints is destroyed. Please check the implementation in order to destroy the joint before the body."); - } -} - void RigidBodyBullet::set_activation_state(bool p_active) { if (p_active) { btBody->activate(); diff --git a/modules/bullet/rigid_body_bullet.h b/modules/bullet/rigid_body_bullet.h index c643611397..8ff96577b6 100644 --- a/modules/bullet/rigid_body_bullet.h +++ b/modules/bullet/rigid_body_bullet.h @@ -267,8 +267,6 @@ public: bool add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, const float &p_appliedImpulse, int p_other_shape_index, int p_local_shape_index); bool was_colliding(RigidBodyBullet *p_other_object); - void assert_no_constraints(); - void set_activation_state(bool p_active); bool is_active() const; diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index abad1beacb..3bfcd83606 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -478,10 +478,20 @@ void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) { } void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) { + btRigidBody *btBody = p_body->get_bt_rigid_body(); + + int constraints = btBody->getNumConstraintRefs(); + if (constraints > 0) { + WARN_PRINT("A body connected to joints was removed. Ensure bodies are disconnected from joints before removing them."); + for (int i = 0; i < constraints; i++) { + dynamicsWorld->removeConstraint(btBody->getConstraintRef(i)); + } + } + if (p_body->is_static()) { - dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body()); + dynamicsWorld->removeCollisionObject(btBody); } else { - dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body()); + dynamicsWorld->removeRigidBody(btBody); } } diff --git a/modules/cvtt/SCsub b/modules/cvtt/SCsub index 5438f7ebac..e56177d6e9 100644 --- a/modules/cvtt/SCsub +++ b/modules/cvtt/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_cvtt = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + thirdparty_dir = "#thirdparty/cvtt/" thirdparty_sources = [ "ConvectionKernels.cpp", @@ -17,7 +20,15 @@ env_cvtt.Prepend(CPPPATH=[thirdparty_dir]) env_thirdparty = env_cvtt.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj # Godot source files -env_cvtt.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_cvtt.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/denoise/SCsub b/modules/denoise/SCsub index bf3bd7d073..97feea2b44 100644 --- a/modules/denoise/SCsub +++ b/modules/denoise/SCsub @@ -8,6 +8,9 @@ Import("env_modules") env_oidn = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + thirdparty_dir = "#thirdparty/oidn/" thirdparty_sources = [ "core/api.cpp", @@ -106,7 +109,8 @@ env_oidn.Append( env_thirdparty = env_oidn.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj weights_in_path = thirdparty_dir + "weights/rtlightmap_hdr.tza" weights_out_path = thirdparty_dir + "weights/rtlightmap_hdr.gen.cpp" @@ -114,5 +118,12 @@ weights_out_path = thirdparty_dir + "weights/rtlightmap_hdr.gen.cpp" env_thirdparty.Depends(weights_out_path, weights_in_path) env_thirdparty.CommandNoCache(weights_out_path, weights_in_path, resource_to_cpp.tza_to_cpp) -env_oidn.add_source_files(env.modules_sources, "denoise_wrapper.cpp") -env_modules.add_source_files(env.modules_sources, ["register_types.cpp", "lightmap_denoiser.cpp"]) +# Godot source files + +module_obj = [] + +env_oidn.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/enet/SCsub b/modules/enet/SCsub index c8f4b3885e..580e5a3eb0 100644 --- a/modules/enet/SCsub +++ b/modules/enet/SCsub @@ -7,6 +7,8 @@ env_enet = env_modules.Clone() # Thirdparty source files +thirdparty_obj = [] + if env["builtin_enet"]: thirdparty_dir = "#thirdparty/enet/" thirdparty_sources = [ @@ -26,6 +28,16 @@ if env["builtin_enet"]: env_thirdparty = env_enet.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + + +# Godot source files + +module_obj = [] + +env_enet.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -env_enet.add_source_files(env.modules_sources, "*.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/etc/SCsub b/modules/etc/SCsub index 383bbf83c3..9b46f17916 100644 --- a/modules/etc/SCsub +++ b/modules/etc/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_etc = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + # Not unbundled so far since not widespread as shared library thirdparty_dir = "#thirdparty/etc2comp/" thirdparty_sources = [ @@ -31,7 +34,15 @@ env_etc.Prepend(CPPPATH=[thirdparty_dir]) env_thirdparty = env_etc.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj # Godot source files -env_etc.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_etc.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/freetype/SCsub b/modules/freetype/SCsub index bfc1658bb4..fc2535a6ca 100644 --- a/modules/freetype/SCsub +++ b/modules/freetype/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_freetype = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_freetype"]: thirdparty_dir = "#thirdparty/freetype/" thirdparty_sources = [ @@ -84,6 +87,7 @@ if env["builtin_freetype"]: env_thirdparty = env_freetype.Clone() env_thirdparty.disable_warnings() lib = env_thirdparty.add_library("freetype_builtin", thirdparty_sources) + thirdparty_obj += lib # Needs to be appended to arrive after libscene in the linker call, # but we don't want it to arrive *after* system libs, so manual hack @@ -98,5 +102,13 @@ if env["builtin_freetype"]: if not inserted: env.Append(LIBS=[lib]) + # Godot source files -env_freetype.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_freetype.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/gdnative/gdnative/rect2.cpp b/modules/gdnative/gdnative/rect2.cpp index bacefced5d..0576fb569f 100644 --- a/modules/gdnative/gdnative/rect2.cpp +++ b/modules/gdnative/gdnative/rect2.cpp @@ -90,11 +90,11 @@ godot_bool GDAPI godot_rect2_has_no_area(const godot_rect2 *p_self) { return self->has_no_area(); } -godot_rect2 GDAPI godot_rect2_clip(const godot_rect2 *p_self, const godot_rect2 *p_b) { +godot_rect2 GDAPI godot_rect2_intersection(const godot_rect2 *p_self, const godot_rect2 *p_b) { godot_rect2 dest; const Rect2 *self = (const Rect2 *)p_self; const Rect2 *b = (const Rect2 *)p_b; - *((Rect2 *)&dest) = self->clip(*b); + *((Rect2 *)&dest) = self->intersection(*b); return dest; } @@ -233,11 +233,11 @@ godot_bool GDAPI godot_rect2i_has_no_area(const godot_rect2i *p_self) { return self->has_no_area(); } -godot_rect2i GDAPI godot_rect2i_clip(const godot_rect2i *p_self, const godot_rect2i *p_b) { +godot_rect2i GDAPI godot_rect2i_intersection(const godot_rect2i *p_self, const godot_rect2i *p_b) { godot_rect2i dest; const Rect2i *self = (const Rect2i *)p_self; const Rect2i *b = (const Rect2i *)p_b; - *((Rect2i *)&dest) = self->clip(*b); + *((Rect2i *)&dest) = self->intersection(*b); return dest; } diff --git a/modules/gdnative/gdnative_api.json b/modules/gdnative/gdnative_api.json index 84b36b3745..b3fd033e6c 100644 --- a/modules/gdnative/gdnative_api.json +++ b/modules/gdnative/gdnative_api.json @@ -3573,7 +3573,7 @@ ] }, { - "name": "godot_rect2_clip", + "name": "godot_rect2_intersection", "return_type": "godot_rect2", "arguments": [ ["const godot_rect2 *", "p_self"], @@ -3715,7 +3715,7 @@ ] }, { - "name": "godot_rect2i_clip", + "name": "godot_rect2i_intersection", "return_type": "godot_rect2i", "arguments": [ ["const godot_rect2i *", "p_self"], diff --git a/modules/gdnative/include/gdnative/rect2.h b/modules/gdnative/include/gdnative/rect2.h index f317afc9da..2c8f836d16 100644 --- a/modules/gdnative/include/gdnative/rect2.h +++ b/modules/gdnative/include/gdnative/rect2.h @@ -80,7 +80,7 @@ godot_bool GDAPI godot_rect2_encloses(const godot_rect2 *p_self, const godot_rec godot_bool GDAPI godot_rect2_has_no_area(const godot_rect2 *p_self); -godot_rect2 GDAPI godot_rect2_clip(const godot_rect2 *p_self, const godot_rect2 *p_b); +godot_rect2 GDAPI godot_rect2_intersection(const godot_rect2 *p_self, const godot_rect2 *p_b); godot_rect2 GDAPI godot_rect2_merge(const godot_rect2 *p_self, const godot_rect2 *p_b); @@ -123,7 +123,7 @@ godot_bool GDAPI godot_rect2i_encloses(const godot_rect2i *p_self, const godot_r godot_bool GDAPI godot_rect2i_has_no_area(const godot_rect2i *p_self); -godot_rect2i GDAPI godot_rect2i_clip(const godot_rect2i *p_self, const godot_rect2i *p_b); +godot_rect2i GDAPI godot_rect2i_intersection(const godot_rect2i *p_self, const godot_rect2i *p_b); godot_rect2i GDAPI godot_rect2i_merge(const godot_rect2i *p_self, const godot_rect2i *p_b); diff --git a/modules/gdnative/xr/xr_interface_gdnative.cpp b/modules/gdnative/xr/xr_interface_gdnative.cpp index d03fc33935..d1d575db62 100644 --- a/modules/gdnative/xr/xr_interface_gdnative.cpp +++ b/modules/gdnative/xr/xr_interface_gdnative.cpp @@ -302,12 +302,12 @@ godot_int GDAPI godot_xr_add_controller(char *p_device_name, godot_int p_hand, g ERR_FAIL_NULL_V(input, 0); XRPositionalTracker *new_tracker = memnew(XRPositionalTracker); - new_tracker->set_name(p_device_name); - new_tracker->set_type(XRServer::TRACKER_CONTROLLER); + new_tracker->set_tracker_name(p_device_name); + new_tracker->set_tracker_type(XRServer::TRACKER_CONTROLLER); if (p_hand == 1) { - new_tracker->set_hand(XRPositionalTracker::TRACKER_LEFT_HAND); + new_tracker->set_tracker_hand(XRPositionalTracker::TRACKER_HAND_LEFT); } else if (p_hand == 2) { - new_tracker->set_hand(XRPositionalTracker::TRACKER_RIGHT_HAND); + new_tracker->set_tracker_hand(XRPositionalTracker::TRACKER_HAND_RIGHT); } // also register as joystick... diff --git a/modules/gdnavigation/SCsub b/modules/gdnavigation/SCsub index 877d601c6a..22b5509b32 100644 --- a/modules/gdnavigation/SCsub +++ b/modules/gdnavigation/SCsub @@ -5,6 +5,10 @@ Import("env_modules") env_navigation = env_modules.Clone() +# Thirdparty source files + +thirdparty_obj = [] + # Recast Thirdparty source files if env["builtin_recast"]: thirdparty_dir = "#thirdparty/recastnavigation/Recast/" @@ -23,28 +27,37 @@ if env["builtin_recast"]: ] thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] - env_navigation.Prepend(CPPPATH=[thirdparty_dir + "/Include"]) + env_navigation.Prepend(CPPPATH=[thirdparty_dir + "Include"]) env_thirdparty = env_navigation.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) - + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) # RVO Thirdparty source files if env["builtin_rvo2"]: - thirdparty_dir = "#thirdparty/rvo2" + thirdparty_dir = "#thirdparty/rvo2/" thirdparty_sources = [ - "/src/Agent.cpp", - "/src/KdTree.cpp", + "Agent.cpp", + "KdTree.cpp", ] thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] - env_navigation.Prepend(CPPPATH=[thirdparty_dir + "/src"]) + env_navigation.Prepend(CPPPATH=[thirdparty_dir]) env_thirdparty = env_navigation.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + + +env.modules_sources += thirdparty_obj # Godot source files -env_navigation.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_navigation.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/glslang/SCsub b/modules/glslang/SCsub index 58c033c75d..182272ffc7 100644 --- a/modules/glslang/SCsub +++ b/modules/glslang/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_glslang = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_glslang"]: thirdparty_dir = "#thirdparty/glslang/" thirdparty_sources = [ @@ -70,7 +73,16 @@ if env["builtin_glslang"]: env_thirdparty = env_glslang.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + + +# Godot source files + +module_obj = [] + +env_glslang.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -# Godot's own source files -env_glslang.add_source_files(env.modules_sources, "*.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/jpg/SCsub b/modules/jpg/SCsub index 8ee8e6dd6e..7c6ceeea29 100644 --- a/modules/jpg/SCsub +++ b/modules/jpg/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_jpg = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + # Not unbundled for now as they are not commonly available as shared library thirdparty_dir = "#thirdparty/jpeg-compressor/" thirdparty_sources = [ @@ -17,7 +20,15 @@ env_jpg.Prepend(CPPPATH=[thirdparty_dir]) env_thirdparty = env_jpg.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj + +# Godot source files + +module_obj = [] + +env_jpg.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -# Godot's own source files -env_jpg.add_source_files(env.modules_sources, "*.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/mbedtls/SCsub b/modules/mbedtls/SCsub index 3b1739c6ee..4fcbe8fb43 100755 --- a/modules/mbedtls/SCsub +++ b/modules/mbedtls/SCsub @@ -5,8 +5,11 @@ Import("env_modules") env_mbed_tls = env_modules.Clone() +# Thirdparty source files + +thirdparty_obj = [] + if env["builtin_mbedtls"]: - # Thirdparty source files thirdparty_sources = [ "aes.c", "aesni.c", @@ -96,11 +99,21 @@ if env["builtin_mbedtls"]: env_thirdparty = env_mbed_tls.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + + +# Godot source files -# Module sources -env_mbed_tls.add_source_files(env.modules_sources, "*.cpp") +module_obj = [] + +env_mbed_tls.add_source_files(module_obj, "*.cpp") if env["tests"]: env_mbed_tls.Append(CPPDEFINES=["TESTS_ENABLED"]) - env_mbed_tls.add_source_files(env.modules_sources, "./tests/*.cpp") + env_mbed_tls.add_source_files(module_obj, "./tests/*.cpp") + +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/meshoptimizer/SCsub b/modules/meshoptimizer/SCsub index 3b1a5f917e..3f86bb4f00 100644 --- a/modules/meshoptimizer/SCsub +++ b/modules/meshoptimizer/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_meshoptimizer = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + thirdparty_dir = "#thirdparty/meshoptimizer/" thirdparty_sources = [ "allocator.cpp", @@ -26,9 +29,17 @@ thirdparty_sources = [ ] thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] - env_thirdparty = env_meshoptimizer.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj + +# Godot source files + +module_obj = [] + +env_meshoptimizer.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -env_modules.add_source_files(env.modules_sources, ["register_types.cpp"]) +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/ogg/SCsub b/modules/ogg/SCsub index e768fb4ae8..e415d92498 100644 --- a/modules/ogg/SCsub +++ b/modules/ogg/SCsub @@ -9,6 +9,9 @@ Import("env_modules") env_ogg = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_libogg"]: thirdparty_dir = "#thirdparty/libogg/" thirdparty_sources = [ @@ -21,7 +24,16 @@ if env["builtin_libogg"]: env_thirdparty = env_ogg.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + # Godot source files -env_ogg.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_ogg.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/opensimplex/SCsub b/modules/opensimplex/SCsub index 52d8b145ef..86d77c3dfb 100644 --- a/modules/opensimplex/SCsub +++ b/modules/opensimplex/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_opensimplex = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + thirdparty_dir = "#thirdparty/misc/" thirdparty_sources = [ "open-simplex-noise.c", @@ -16,7 +19,15 @@ env_opensimplex.Prepend(CPPPATH=[thirdparty_dir]) env_thirdparty = env_opensimplex.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj + +# Godot source files + +module_obj = [] + +env_opensimplex.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -# Godot's own source files -env_opensimplex.add_source_files(env.modules_sources, "*.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/opus/SCsub b/modules/opus/SCsub index 52c61fa708..1437cd86df 100644 --- a/modules/opus/SCsub +++ b/modules/opus/SCsub @@ -10,6 +10,10 @@ Import("env_modules") env_opus = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + +# Thirdparty source files if env["builtin_opus"]: thirdparty_dir = "#thirdparty/opus/" @@ -233,7 +237,16 @@ if env["builtin_opus"]: env_thirdparty = env_opus.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + + +# Godot source files + +module_obj = [] + +env_opus.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -# Module files -env_opus.add_source_files(env.modules_sources, "register_types.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/pvr/SCsub b/modules/pvr/SCsub index e0baf851f1..36052cffed 100644 --- a/modules/pvr/SCsub +++ b/modules/pvr/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_pvr = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + # Not unbundled so far since not widespread as shared library thirdparty_dir = "#thirdparty/pvrtccompressor/" thirdparty_sources = [ @@ -21,7 +24,15 @@ env_pvr.Prepend(CPPPATH=[thirdparty_dir]) env_thirdparty = env_pvr.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj # Godot source files -env_pvr.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_pvr.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/regex/SCsub b/modules/regex/SCsub index 2afacc1d9c..deb9db7591 100644 --- a/modules/regex/SCsub +++ b/modules/regex/SCsub @@ -5,6 +5,10 @@ Import("env_modules") env_regex = env_modules.Clone() +# Thirdparty source files + +thirdparty_obj = [] + if env["builtin_pcre2"]: thirdparty_dir = "#thirdparty/pcre2/src/" thirdparty_flags = ["PCRE2_STATIC", "HAVE_CONFIG_H", "SUPPORT_UNICODE"] @@ -52,11 +56,21 @@ if env["builtin_pcre2"]: env_pcre2 = env_regex.Clone() env_pcre2.disable_warnings() env_pcre2["OBJSUFFIX"] = "_" + width + env_pcre2["OBJSUFFIX"] - env_pcre2.add_source_files(env.modules_sources, thirdparty_sources) env_pcre2.Append(CPPDEFINES=[("PCRE2_CODE_UNIT_WIDTH", width)]) + env_pcre2.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj pcre2_builtin("16") pcre2_builtin("32") + +# Godot source files + +module_obj = [] + env_regex.Append(CPPDEFINES=[("PCRE2_CODE_UNIT_WIDTH", 0)]) -env_regex.add_source_files(env.modules_sources, "*.cpp") +env_regex.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/squish/SCsub b/modules/squish/SCsub index b31032403f..c9e29911d8 100644 --- a/modules/squish/SCsub +++ b/modules/squish/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_squish = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_squish"]: thirdparty_dir = "#thirdparty/squish/" thirdparty_sources = [ @@ -26,7 +29,16 @@ if env["builtin_squish"]: env_thirdparty = env_squish.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + # Godot source files -env_squish.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_squish.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/stb_vorbis/SCsub b/modules/stb_vorbis/SCsub index 266c87c802..8fddb23dc8 100644 --- a/modules/stb_vorbis/SCsub +++ b/modules/stb_vorbis/SCsub @@ -6,11 +6,22 @@ Import("env_modules") env_stb_vorbis = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + thirdparty_sources = ["#thirdparty/misc/stb_vorbis.c"] env_thirdparty = env_stb_vorbis.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj + +# Godot source files + +module_obj = [] + +env_stb_vorbis.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -# Godot's own source files -env_stb_vorbis.add_source_files(env.modules_sources, "*.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/svg/SCsub b/modules/svg/SCsub index 0bfba34fe5..c7228a8d0b 100644 --- a/modules/svg/SCsub +++ b/modules/svg/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_svg = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + thirdparty_dir = "#thirdparty/nanosvg/" thirdparty_sources = [ "nanosvg.cc", @@ -16,7 +19,15 @@ env_svg.Prepend(CPPPATH=[thirdparty_dir]) env_thirdparty = env_svg.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj + +# Godot source files + +module_obj = [] + +env_svg.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -# Godot's own source files -env_svg.add_source_files(env.modules_sources, "*.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/text_server_adv/SCsub b/modules/text_server_adv/SCsub index 7403b01a4c..3589c8546d 100644 --- a/modules/text_server_adv/SCsub +++ b/modules/text_server_adv/SCsub @@ -35,10 +35,14 @@ def make_icu_data(target, source, env): g.write("#endif") +# Thirdparty source files + +thirdparty_obj = [] + if env["builtin_harfbuzz"]: env_harfbuzz = env_modules.Clone() + env_harfbuzz.disable_warnings() - # Thirdparty source files thirdparty_dir = "#thirdparty/harfbuzz/" thirdparty_sources = [ "src/hb-aat-layout.cc", @@ -107,6 +111,15 @@ if env["builtin_harfbuzz"]: ] thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] + env_harfbuzz.Append( + CPPPATH=[ + "#thirdparty/harfbuzz/src", + "#thirdparty/freetype/include", + "#thirdparty/graphite/include", + "#thirdparty/icu4c/common/", + ] + ) + if env["platform"] == "android" or env["platform"] == "linuxbsd" or env["platform"] == "server": env_harfbuzz.Append(CCFLAGS=["-DHAVE_PTHREAD"]) @@ -117,14 +130,6 @@ if env["builtin_harfbuzz"]: env_harfbuzz.Append(CCFLAGS=["-DHB_NO_MT"]) env_harfbuzz.Append( - CPPPATH=[ - "#thirdparty/harfbuzz/src", - "#thirdparty/freetype/include", - "#thirdparty/graphite/include", - "#thirdparty/icu4c/common/", - ] - ) - env_harfbuzz.Append( CCFLAGS=[ "-DHAVE_ICU_BUILTIN", "-DHAVE_ICU", @@ -133,10 +138,9 @@ if env["builtin_harfbuzz"]: "-DGRAPHITE2_STATIC", ] ) - env_harfbuzz.disable_warnings() - env_thirdparty = env_harfbuzz.Clone() - env_thirdparty.disable_warnings() - lib = env_thirdparty.add_library("harfbuzz_builtin", thirdparty_sources) + + lib = env_harfbuzz.add_library("harfbuzz_builtin", thirdparty_sources) + thirdparty_obj += lib # Needs to be appended to arrive after libscene in the linker call, # but we don't want it to arrive *after* system libs, so manual hack @@ -151,10 +155,11 @@ if env["builtin_harfbuzz"]: if not inserted: env.Append(LIBS=[lib]) + if env["builtin_graphite"]: env_graphite = env_modules.Clone() + env_graphite.disable_warnings() - # Thirdparty source files thirdparty_dir = "#thirdparty/graphite/" thirdparty_sources = [ "src/gr_char_info.cpp", @@ -203,10 +208,9 @@ if env["builtin_graphite"]: "-DGRAPHITE2_NFILEFACE", ] ) - env_graphite.disable_warnings() - env_thirdparty = env_graphite.Clone() - env_thirdparty.disable_warnings() - lib = env_thirdparty.add_library("graphite_builtin", thirdparty_sources) + + lib = env_graphite.add_library("graphite_builtin", thirdparty_sources) + thirdparty_obj += lib # Needs to be appended to arrive after libscene in the linker call, # but we don't want it to arrive *after* system libs, so manual hack @@ -221,12 +225,12 @@ if env["builtin_graphite"]: if not inserted: env.Append(LIBS=[lib]) + if env["builtin_icu"]: env_icu = env_modules.Clone() + env_icu.disable_warnings() - # Thirdparty source files thirdparty_dir = "#thirdparty/icu4c/" - # Thirdparty source files thirdparty_sources = [ "common/appendable.cpp", "common/bmpset.cpp", @@ -457,10 +461,8 @@ if env["builtin_icu"]: ] ) - env_icu.disable_warnings() - env_thirdparty = env_icu.Clone() - env_thirdparty.disable_warnings() - lib = env_thirdparty.add_library("icu_builtin", thirdparty_sources) + lib = env_icu.add_library("icu_builtin", thirdparty_sources) + thirdparty_obj += lib # Needs to be appended to arrive after libscene in the linker call, # but we don't want it to arrive *after* system libs, so manual hack @@ -475,6 +477,11 @@ if env["builtin_icu"]: if not inserted: env.Append(LIBS=[lib]) + +# Godot source files + +module_obj = [] + if env_text_server_adv["tools"]: env_text_server_adv.Append(CXXFLAGS=["-DICU_STATIC_DATA"]) @@ -486,4 +493,9 @@ env_text_server_adv.Append( "#thirdparty/icu4c/common/", ] ) -env_text_server_adv.add_source_files(env.modules_sources, "*.cpp") + +env_text_server_adv.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/text_server_fb/SCsub b/modules/text_server_fb/SCsub index 7650e27063..03eccbe7bd 100644 --- a/modules/text_server_fb/SCsub +++ b/modules/text_server_fb/SCsub @@ -9,4 +9,5 @@ env_text_server_fb.Append( "#thirdparty/freetype/include", ] ) + env_text_server_fb.add_source_files(env.modules_sources, "*.cpp") diff --git a/modules/theora/SCsub b/modules/theora/SCsub index a01e65b4b0..6038ea086a 100644 --- a/modules/theora/SCsub +++ b/modules/theora/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_theora = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_libtheora"]: thirdparty_dir = "#thirdparty/libtheora/" thirdparty_sources = [ @@ -80,7 +83,16 @@ if env["builtin_libtheora"]: env_thirdparty = env_theora.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + # Godot source files -env_theora.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_theora.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/tinyexr/SCsub b/modules/tinyexr/SCsub index 84b3b4015b..30bde96fb4 100644 --- a/modules/tinyexr/SCsub +++ b/modules/tinyexr/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_tinyexr = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + # Not unbundled for now as they are not commonly available as shared library thirdparty_dir = "#thirdparty/tinyexr/" thirdparty_sources = [ @@ -20,7 +23,15 @@ env_tinyexr.Append(CPPDEFINES=["TINYEXR_USE_THREAD"]) env_thirdparty = env_tinyexr.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj + +# Godot source files + +module_obj = [] + +env_tinyexr.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -# Godot's own source files -env_tinyexr.add_source_files(env.modules_sources, "*.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/upnp/SCsub b/modules/upnp/SCsub index 2e129e15ca..bc0b215be3 100644 --- a/modules/upnp/SCsub +++ b/modules/upnp/SCsub @@ -7,6 +7,8 @@ env_upnp = env_modules.Clone() # Thirdparty source files +thirdparty_obj = [] + if env["builtin_miniupnpc"]: thirdparty_dir = "#thirdparty/miniupnpc/" thirdparty_sources = [ @@ -31,7 +33,16 @@ if env["builtin_miniupnpc"]: env_thirdparty = env_upnp.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + # Godot source files -env_upnp.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_upnp.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/vhacd/SCsub b/modules/vhacd/SCsub index ecd432b275..1ff4122114 100644 --- a/modules/vhacd/SCsub +++ b/modules/vhacd/SCsub @@ -7,6 +7,8 @@ env_vhacd = env_modules.Clone() # Thirdparty source files +thirdparty_obj = [] + thirdparty_dir = "#thirdparty/vhacd/" thirdparty_sources = [ @@ -24,10 +26,19 @@ thirdparty_sources = [ thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] -env_vhacd.Prepend(CPPPATH=[thirdparty_dir + "/inc"]) +env_vhacd.Prepend(CPPPATH=[thirdparty_dir + "inc"]) env_thirdparty = env_vhacd.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj + +# Godot source files + +module_obj = [] + +env_vhacd.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -env_vhacd.add_source_files(env.modules_sources, "*.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/visual_script/visual_script_editor.cpp b/modules/visual_script/visual_script_editor.cpp index 066fe766db..1bd88a2a19 100644 --- a/modules/visual_script/visual_script_editor.cpp +++ b/modules/visual_script/visual_script_editor.cpp @@ -2547,7 +2547,7 @@ void VisualScriptEditor::set_edited_resource(const RES &p_res) { } _update_graph(); - _update_members(); + call_deferred("_update_members"); } void VisualScriptEditor::enable_editor() { diff --git a/modules/vorbis/SCsub b/modules/vorbis/SCsub index 05d46757d3..bc31fff066 100644 --- a/modules/vorbis/SCsub +++ b/modules/vorbis/SCsub @@ -8,9 +8,10 @@ Import("env_modules") env_vorbis = env_modules.Clone() -stub = True - # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_libvorbis"]: thirdparty_dir = "#thirdparty/libvorbis/" thirdparty_sources = [ @@ -51,7 +52,16 @@ if env["builtin_libvorbis"]: env_thirdparty = env_vorbis.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + + +# Godot source files + +module_obj = [] + +env_vorbis.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj -# Module files -env_vorbis.add_source_files(env.modules_sources, "register_types.cpp") +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/webm/SCsub b/modules/webm/SCsub index 247b4ead37..44e80e2870 100644 --- a/modules/webm/SCsub +++ b/modules/webm/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_webm = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + thirdparty_dir = "#thirdparty/libsimplewebm/" thirdparty_sources = [ "libwebm/mkvparser/mkvparser.cc", @@ -31,7 +34,15 @@ if env["builtin_libvpx"]: env_thirdparty = env_webm.Clone() env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.modules_sources += thirdparty_obj # Godot source files -env_webm.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_webm.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/webp/SCsub b/modules/webp/SCsub index 58f2bb35e6..4c0c2f7893 100644 --- a/modules/webp/SCsub +++ b/modules/webp/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_webp = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_libwebp"]: thirdparty_dir = "#thirdparty/libwebp/" thirdparty_sources = [ @@ -130,7 +133,16 @@ if env["builtin_libwebp"]: env_thirdparty = env_webp.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + # Godot source files -env_webp.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_webp.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/webrtc/SCsub b/modules/webrtc/SCsub index 4f870ddb2f..31b8a73bf2 100644 --- a/modules/webrtc/SCsub +++ b/modules/webrtc/SCsub @@ -3,8 +3,6 @@ Import("env") Import("env_modules") -# Thirdparty source files - env_webrtc = env_modules.Clone() use_gdnative = env_webrtc["module_gdnative_enabled"] diff --git a/modules/websocket/SCsub b/modules/websocket/SCsub index 13e51a39c0..4c022c43cf 100644 --- a/modules/websocket/SCsub +++ b/modules/websocket/SCsub @@ -5,28 +5,44 @@ Import("env_modules") env_ws = env_modules.Clone() +thirdparty_obj = [] + if env["platform"] == "javascript": # Our JavaScript/C++ interface. env.AddJSLibraries(["library_godot_websocket.js"]) + elif env["builtin_wslay"]: # Thirdparty source files - wslay_dir = "#thirdparty/wslay/" - wslay_sources = [ + thirdparty_dir = "#thirdparty/wslay/" + thirdparty_sources = [ "wslay_net.c", "wslay_event.c", "wslay_queue.c", "wslay_stack.c", "wslay_frame.c", ] - wslay_sources = [wslay_dir + s for s in wslay_sources] - env_ws.Prepend(CPPPATH=[wslay_dir + "includes/"]) + thirdparty_sources = [thirdparty_dir + s for s in thirdparty_sources] + + env_ws.Prepend(CPPPATH=[thirdparty_dir + "includes/"]) env_ws.Append(CPPDEFINES=["HAVE_CONFIG_H"]) + if env["platform"] == "windows" or env["platform"] == "uwp": env_ws.Append(CPPDEFINES=["HAVE_WINSOCK2_H"]) else: env_ws.Append(CPPDEFINES=["HAVE_NETINET_IN_H"]) - env_wslay = env_ws.Clone() - env_wslay.disable_warnings() - env_wslay.add_source_files(env.modules_sources, wslay_sources) -env_ws.add_source_files(env.modules_sources, "*.cpp") + env_thirdparty = env_ws.Clone() + env_thirdparty.disable_warnings() + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + + +# Godot source files + +module_obj = [] + +env_ws.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/modules/xatlas_unwrap/SCsub b/modules/xatlas_unwrap/SCsub index c659349d05..aa6bdaea33 100644 --- a/modules/xatlas_unwrap/SCsub +++ b/modules/xatlas_unwrap/SCsub @@ -6,6 +6,9 @@ Import("env_modules") env_xatlas_unwrap = env_modules.Clone() # Thirdparty source files + +thirdparty_obj = [] + if env["builtin_xatlas"]: thirdparty_dir = "#thirdparty/xatlas/" thirdparty_sources = [ @@ -17,7 +20,16 @@ if env["builtin_xatlas"]: env_thirdparty = env_xatlas_unwrap.Clone() env_thirdparty.disable_warnings() - env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + env.modules_sources += thirdparty_obj + # Godot source files -env_xatlas_unwrap.add_source_files(env.modules_sources, "*.cpp") + +module_obj = [] + +env_xatlas_unwrap.add_source_files(module_obj, "*.cpp") +env.modules_sources += module_obj + +# Needed to force rebuilding the module files when the thirdparty library is updated. +env.Depends(module_obj, thirdparty_obj) diff --git a/platform/android/SCsub b/platform/android/SCsub index d8013b0baf..7e9dac926c 100644 --- a/platform/android/SCsub +++ b/platform/android/SCsub @@ -29,10 +29,14 @@ for x in android_files: env_thirdparty = env_android.Clone() env_thirdparty.disable_warnings() -android_objects.append(env_thirdparty.SharedObject("#thirdparty/misc/ifaddrs-android.cc")) +thirdparty_obj = env_thirdparty.SharedObject("#thirdparty/misc/ifaddrs-android.cc") +android_objects.append(thirdparty_obj) lib = env_android.add_shared_library("#bin/libgodot", [android_objects], SHLIBSUFFIX=env["SHLIBSUFFIX"]) +# Needed to force rebuilding the platform files when the thirdparty code is updated. +env.Depends(lib, thirdparty_obj) + lib_arch_dir = "" if env["android_arch"] == "armv7": lib_arch_dir = "armeabi-v7a" diff --git a/platform/android/os_android.h b/platform/android/os_android.h index cac7efaa88..d9a07411ad 100644 --- a/platform/android/os_android.h +++ b/platform/android/os_android.h @@ -68,15 +68,15 @@ private: GodotIOJavaWrapper *godot_io_java; public: - virtual void initialize_core(); - virtual void initialize(); + virtual void initialize_core() override; + virtual void initialize() override; - virtual void initialize_joypads(); + virtual void initialize_joypads() override; - virtual void set_main_loop(MainLoop *p_main_loop); - virtual void delete_main_loop(); + virtual void set_main_loop(MainLoop *p_main_loop) override; + virtual void delete_main_loop() override; - virtual void finalize(); + virtual void finalize() override; typedef int64_t ProcessID; @@ -84,14 +84,14 @@ public: GodotJavaWrapper *get_godot_java(); GodotIOJavaWrapper *get_godot_io_java(); - virtual bool request_permission(const String &p_name); - virtual bool request_permissions(); - virtual Vector<String> get_granted_permissions() const; + virtual bool request_permission(const String &p_name) override; + virtual bool request_permissions() override; + virtual Vector<String> get_granted_permissions() const override; - virtual Error open_dynamic_library(const String p_path, void *&p_library_handle, bool p_also_set_library_path = false); + virtual Error open_dynamic_library(const String p_path, void *&p_library_handle, bool p_also_set_library_path = false) override; - virtual String get_name() const; - virtual MainLoop *get_main_loop() const; + virtual String get_name() const override; + virtual MainLoop *get_main_loop() const override; void main_loop_begin(); bool main_loop_iterate(); @@ -109,19 +109,19 @@ public: void set_native_window(ANativeWindow *p_native_window); ANativeWindow *get_native_window() const; - virtual Error shell_open(String p_uri); - virtual String get_user_data_dir() const; - virtual String get_resource_dir() const; - virtual String get_locale() const; - virtual String get_model_name() const; + virtual Error shell_open(String p_uri) override; + virtual String get_user_data_dir() const override; + virtual String get_resource_dir() const override; + virtual String get_locale() const override; + virtual String get_model_name() const override; - virtual String get_unique_id() const; + virtual String get_unique_id() const override; - virtual String get_system_dir(SystemDir p_dir) const; + virtual String get_system_dir(SystemDir p_dir) const override; - void vibrate_handheld(int p_duration_ms); + void vibrate_handheld(int p_duration_ms) override; - virtual bool _check_internal_feature_support(const String &p_feature); + virtual bool _check_internal_feature_support(const String &p_feature) override; OS_Android(GodotJavaWrapper *p_godot_java, GodotIOJavaWrapper *p_godot_io_java, bool p_use_apk_expansion); ~OS_Android(); }; diff --git a/platform/linuxbsd/os_linuxbsd.h b/platform/linuxbsd/os_linuxbsd.h index 6e93bf6ef2..89d0bcd0f2 100644 --- a/platform/linuxbsd/os_linuxbsd.h +++ b/platform/linuxbsd/os_linuxbsd.h @@ -43,7 +43,7 @@ #include "servers/rendering_server.h" class OS_LinuxBSD : public OS_Unix { - virtual void delete_main_loop(); + virtual void delete_main_loop() override; bool force_quit; @@ -68,36 +68,36 @@ class OS_LinuxBSD : public OS_Unix { MainLoop *main_loop; protected: - virtual void initialize(); - virtual void finalize(); + virtual void initialize() override; + virtual void finalize() override; - virtual void initialize_joypads(); + virtual void initialize_joypads() override; - virtual void set_main_loop(MainLoop *p_main_loop); + virtual void set_main_loop(MainLoop *p_main_loop) override; public: - virtual String get_name() const; + virtual String get_name() const override; - virtual MainLoop *get_main_loop() const; + virtual MainLoop *get_main_loop() const override; - virtual String get_config_path() const; - virtual String get_data_path() const; - virtual String get_cache_path() const; + virtual String get_config_path() const override; + virtual String get_data_path() const override; + virtual String get_cache_path() const override; - virtual String get_system_dir(SystemDir p_dir) const; + virtual String get_system_dir(SystemDir p_dir) const override; - virtual Error shell_open(String p_uri); + virtual Error shell_open(String p_uri) override; - virtual String get_unique_id() const; + virtual String get_unique_id() const override; - virtual bool _check_internal_feature_support(const String &p_feature); + virtual bool _check_internal_feature_support(const String &p_feature) override; void run(); - void disable_crash_handler(); - bool is_disable_crash_handler() const; + virtual void disable_crash_handler() override; + virtual bool is_disable_crash_handler() const override; - virtual Error move_to_trash(const String &p_path); + virtual Error move_to_trash(const String &p_path) override; OS_LinuxBSD(); }; diff --git a/platform/osx/os_osx.h b/platform/osx/os_osx.h index 5a9e43450f..f6eee31a6a 100644 --- a/platform/osx/os_osx.h +++ b/platform/osx/os_osx.h @@ -40,7 +40,7 @@ #include "servers/audio_server.h" class OS_OSX : public OS_Unix { - virtual void delete_main_loop(); + virtual void delete_main_loop() override; bool force_quit; @@ -61,45 +61,45 @@ public: String open_with_filename; protected: - virtual void initialize_core(); - virtual void initialize(); - virtual void finalize(); + virtual void initialize_core() override; + virtual void initialize() override; + virtual void finalize() override; - virtual void initialize_joypads(); + virtual void initialize_joypads() override; - virtual void set_main_loop(MainLoop *p_main_loop); + virtual void set_main_loop(MainLoop *p_main_loop) override; public: - virtual String get_name() const; + virtual String get_name() const override; - virtual Error open_dynamic_library(const String p_path, void *&p_library_handle, bool p_also_set_library_path = false); + virtual Error open_dynamic_library(const String p_path, void *&p_library_handle, bool p_also_set_library_path = false) override; - virtual MainLoop *get_main_loop() const; + virtual MainLoop *get_main_loop() const override; - virtual String get_config_path() const; - virtual String get_data_path() const; - virtual String get_cache_path() const; - virtual String get_bundle_resource_dir() const; - virtual String get_godot_dir_name() const; + virtual String get_config_path() const override; + virtual String get_data_path() const override; + virtual String get_cache_path() const override; + virtual String get_bundle_resource_dir() const override; + virtual String get_godot_dir_name() const override; - virtual String get_system_dir(SystemDir p_dir) const; + virtual String get_system_dir(SystemDir p_dir) const override; - Error shell_open(String p_uri); + Error shell_open(String p_uri) override; - String get_locale() const; + String get_locale() const override; - virtual String get_executable_path() const; + virtual String get_executable_path() const override; - virtual String get_unique_id() const; //++ + virtual String get_unique_id() const override; //++ - virtual bool _check_internal_feature_support(const String &p_feature); + virtual bool _check_internal_feature_support(const String &p_feature) override; void run(); - void disable_crash_handler(); - bool is_disable_crash_handler() const; + virtual void disable_crash_handler() override; + virtual bool is_disable_crash_handler() const override; - virtual Error move_to_trash(const String &p_path); + virtual Error move_to_trash(const String &p_path) override; OS_OSX(); }; diff --git a/platform/windows/display_server_windows.cpp b/platform/windows/display_server_windows.cpp index 62fcd20f61..520b43f963 100644 --- a/platform/windows/display_server_windows.cpp +++ b/platform/windows/display_server_windows.cpp @@ -2956,7 +2956,7 @@ DisplayServer::WindowID DisplayServerWindows::_create_window(WindowMode p_mode, Rect2i r; r.position = screen_get_position(i); r.size = screen_get_size(i); - Rect2 inters = r.clip(p_rect); + Rect2 inters = r.intersection(p_rect); int area = inters.size.width * inters.size.height; if (area >= nearest_area) { screen_rect = r; diff --git a/platform/windows/os_windows.h b/platform/windows/os_windows.h index 5451e15d36..14d09d2b35 100644 --- a/platform/windows/os_windows.h +++ b/platform/windows/os_windows.h @@ -93,14 +93,14 @@ class OS_Windows : public OS { // functions used by main to initialize/deinitialize the OS protected: - virtual void initialize(); + virtual void initialize() override; - virtual void set_main_loop(MainLoop *p_main_loop); - virtual void delete_main_loop(); + virtual void set_main_loop(MainLoop *p_main_loop) override; + virtual void delete_main_loop() override; - virtual void finalize(); - virtual void finalize_core(); - virtual String get_stdin_string(bool p_block); + virtual void finalize() override; + virtual void finalize_core() override; + virtual String get_stdin_string(bool p_block) override; String _quote_command_line_argument(const String &p_text) const; @@ -111,66 +111,66 @@ protected: Map<ProcessID, ProcessInfo> *process_map; public: - virtual Error open_dynamic_library(const String p_path, void *&p_library_handle, bool p_also_set_library_path = false); - virtual Error close_dynamic_library(void *p_library_handle); - virtual Error get_dynamic_library_symbol_handle(void *p_library_handle, const String p_name, void *&p_symbol_handle, bool p_optional = false); + virtual Error open_dynamic_library(const String p_path, void *&p_library_handle, bool p_also_set_library_path = false) override; + virtual Error close_dynamic_library(void *p_library_handle) override; + virtual Error get_dynamic_library_symbol_handle(void *p_library_handle, const String p_name, void *&p_symbol_handle, bool p_optional = false) override; - virtual MainLoop *get_main_loop() const; + virtual MainLoop *get_main_loop() const override; - virtual String get_name() const; + virtual String get_name() const override; - virtual int get_tablet_driver_count() const; - virtual String get_tablet_driver_name(int p_driver) const; - virtual String get_current_tablet_driver() const; - virtual void set_current_tablet_driver(const String &p_driver); + virtual int get_tablet_driver_count() const override; + virtual String get_tablet_driver_name(int p_driver) const override; + virtual String get_current_tablet_driver() const override; + virtual void set_current_tablet_driver(const String &p_driver) override; - virtual void initialize_joypads() {} + virtual void initialize_joypads() override {} - virtual Date get_date(bool utc) const; - virtual Time get_time(bool utc) const; - virtual TimeZoneInfo get_time_zone_info() const; - virtual double get_unix_time() const; + virtual Date get_date(bool utc) const override; + virtual Time get_time(bool utc) const override; + virtual TimeZoneInfo get_time_zone_info() const override; + virtual double get_unix_time() const override; - virtual Error set_cwd(const String &p_cwd); + virtual Error set_cwd(const String &p_cwd) override; - virtual void delay_usec(uint32_t p_usec) const; - virtual uint64_t get_ticks_usec() const; + virtual void delay_usec(uint32_t p_usec) const override; + virtual uint64_t get_ticks_usec() const override; - virtual Error execute(const String &p_path, const List<String> &p_arguments, bool p_blocking = true, ProcessID *r_child_id = nullptr, String *r_pipe = nullptr, int *r_exitcode = nullptr, bool read_stderr = false, Mutex *p_pipe_mutex = nullptr); - virtual Error kill(const ProcessID &p_pid); - virtual int get_process_id() const; + virtual Error execute(const String &p_path, const List<String> &p_arguments, bool p_blocking = true, ProcessID *r_child_id = nullptr, String *r_pipe = nullptr, int *r_exitcode = nullptr, bool read_stderr = false, Mutex *p_pipe_mutex = nullptr) override; + virtual Error kill(const ProcessID &p_pid) override; + virtual int get_process_id() const override; - virtual bool has_environment(const String &p_var) const; - virtual String get_environment(const String &p_var) const; - virtual bool set_environment(const String &p_var, const String &p_value) const; + virtual bool has_environment(const String &p_var) const override; + virtual String get_environment(const String &p_var) const override; + virtual bool set_environment(const String &p_var, const String &p_value) const override; - virtual String get_executable_path() const; + virtual String get_executable_path() const override; - virtual String get_locale() const; + virtual String get_locale() const override; - virtual int get_processor_count() const; + virtual int get_processor_count() const override; - virtual String get_config_path() const; - virtual String get_data_path() const; - virtual String get_cache_path() const; - virtual String get_godot_dir_name() const; + virtual String get_config_path() const override; + virtual String get_data_path() const override; + virtual String get_cache_path() const override; + virtual String get_godot_dir_name() const override; - virtual String get_system_dir(SystemDir p_dir) const; - virtual String get_user_data_dir() const; + virtual String get_system_dir(SystemDir p_dir) const override; + virtual String get_user_data_dir() const override; - virtual String get_unique_id() const; + virtual String get_unique_id() const override; - virtual Error shell_open(String p_uri); + virtual Error shell_open(String p_uri) override; void run(); - virtual bool _check_internal_feature_support(const String &p_feature); + virtual bool _check_internal_feature_support(const String &p_feature) override; - void disable_crash_handler(); - bool is_disable_crash_handler() const; - virtual void initialize_debugging(); + virtual void disable_crash_handler() override; + virtual bool is_disable_crash_handler() const override; + virtual void initialize_debugging() override; - virtual Error move_to_trash(const String &p_path); + virtual Error move_to_trash(const String &p_path) override; void set_main_window(HWND p_main_window) { main_window = p_main_window; } diff --git a/scene/2d/node_2d.cpp b/scene/2d/node_2d.cpp index 42c2585487..a2f687cd96 100644 --- a/scene/2d/node_2d.cpp +++ b/scene/2d/node_2d.cpp @@ -475,12 +475,3 @@ void Node2D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "z_index", PROPERTY_HINT_RANGE, itos(RS::CANVAS_ITEM_Z_MIN) + "," + itos(RS::CANVAS_ITEM_Z_MAX) + ",1"), "set_z_index", "get_z_index"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "z_as_relative"), "set_z_as_relative", "is_z_relative"); } - -Node2D::Node2D() { - angle = 0; - _scale = Vector2(1, 1); - skew = 0; - _xform_dirty = false; - z_index = 0; - z_relative = true; -} diff --git a/scene/2d/node_2d.h b/scene/2d/node_2d.h index e20f746447..a66e7f625d 100644 --- a/scene/2d/node_2d.h +++ b/scene/2d/node_2d.h @@ -37,15 +37,15 @@ class Node2D : public CanvasItem { GDCLASS(Node2D, CanvasItem); Point2 pos; - float angle; - Size2 _scale; - float skew; - int z_index; - bool z_relative; + float angle = 0; + Size2 _scale = Vector2(1, 1); + float skew = 0; + int z_index = 0; + bool z_relative = true; Transform2D _mat; - bool _xform_dirty; + bool _xform_dirty = false; void _update_transform(); @@ -121,7 +121,7 @@ public: Transform2D get_transform() const override; - Node2D(); + Node2D() {} }; #endif // NODE2D_H diff --git a/scene/2d/path_2d.cpp b/scene/2d/path_2d.cpp index f40a993423..6571474c9b 100644 --- a/scene/2d/path_2d.cpp +++ b/scene/2d/path_2d.cpp @@ -387,14 +387,3 @@ void PathFollow2D::set_loop(bool p_loop) { bool PathFollow2D::has_loop() const { return loop; } - -PathFollow2D::PathFollow2D() { - offset = 0; - h_offset = 0; - v_offset = 0; - path = nullptr; - rotates = true; - cubic = true; - loop = true; - lookahead = 4; -} diff --git a/scene/2d/path_2d.h b/scene/2d/path_2d.h index fcb8b40125..40042a04ef 100644 --- a/scene/2d/path_2d.h +++ b/scene/2d/path_2d.h @@ -63,14 +63,14 @@ class PathFollow2D : public Node2D { public: private: - Path2D *path; - real_t offset; - real_t h_offset; - real_t v_offset; - real_t lookahead; - bool cubic; - bool loop; - bool rotates; + Path2D *path = nullptr; + real_t offset = 0; + real_t h_offset = 0; + real_t v_offset = 0; + real_t lookahead = 4; + bool cubic = true; + bool loop = true; + bool rotates = true; void _update_transform(); @@ -107,7 +107,7 @@ public: String get_configuration_warning() const override; - PathFollow2D(); + PathFollow2D() {} }; #endif // PATH_2D_H diff --git a/scene/3d/camera_3d.cpp b/scene/3d/camera_3d.cpp index 191159448a..178c5c8ff8 100644 --- a/scene/3d/camera_3d.cpp +++ b/scene/3d/camera_3d.cpp @@ -519,8 +519,8 @@ void Camera3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "fov", PROPERTY_HINT_RANGE, "1,179,0.1"), "set_fov", "get_fov"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "size", PROPERTY_HINT_RANGE, "0.1,16384,0.01"), "set_size", "get_size"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "frustum_offset"), "set_frustum_offset", "get_frustum_offset"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "near", PROPERTY_HINT_EXP_RANGE, "0.001,8192,0.001,or_greater"), "set_znear", "get_znear"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "far", PROPERTY_HINT_EXP_RANGE, "0.01,8192,0.01,or_greater"), "set_zfar", "get_zfar"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "near", PROPERTY_HINT_EXP_RANGE, "0.001,10,0.001,or_greater"), "set_znear", "get_znear"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "far", PROPERTY_HINT_EXP_RANGE, "0.01,4000,0.01,or_greater"), "set_zfar", "get_zfar"); BIND_ENUM_CONSTANT(PROJECTION_PERSPECTIVE); BIND_ENUM_CONSTANT(PROJECTION_ORTHOGONAL); @@ -662,7 +662,7 @@ Camera3D::Camera3D() { viewport = nullptr; force_change = false; mode = PROJECTION_PERSPECTIVE; - set_perspective(75.0, 0.05, 100.0); + set_perspective(75.0, 0.05, 4000.0); keep_aspect = KEEP_HEIGHT; layers = 0xfffff; v_offset = 0; diff --git a/scene/3d/node_3d.cpp b/scene/3d/node_3d.cpp index e8005f38ed..deb04f0978 100644 --- a/scene/3d/node_3d.cpp +++ b/scene/3d/node_3d.cpp @@ -784,28 +784,4 @@ void Node3D::_bind_methods() { } Node3D::Node3D() : - xform_change(this) { - data.dirty = DIRTY_NONE; - data.children_lock = 0; - - data.ignore_notification = false; - data.top_level = false; - data.top_level_active = false; - data.scale = Vector3(1, 1, 1); - data.viewport = nullptr; - data.inside_world = false; - data.visible = true; - data.disable_scale = false; - -#ifdef TOOLS_ENABLED - data.gizmo_disabled = false; - data.gizmo_dirty = false; -#endif - data.notify_local_transform = false; - data.notify_transform = false; - data.parent = nullptr; - data.C = nullptr; -} - -Node3D::~Node3D() { -} + xform_change(this) {} diff --git a/scene/3d/node_3d.h b/scene/3d/node_3d.h index 5fb421c930..180c441a2a 100644 --- a/scene/3d/node_3d.h +++ b/scene/3d/node_3d.h @@ -65,32 +65,32 @@ class Node3D : public Node { mutable Transform global_transform; mutable Transform local_transform; mutable Vector3 rotation; - mutable Vector3 scale; + mutable Vector3 scale = Vector3(1, 1, 1); - mutable int dirty; + mutable int dirty = DIRTY_NONE; - Viewport *viewport; + Viewport *viewport = nullptr; - bool top_level_active; - bool top_level; - bool inside_world; + bool top_level_active = false; + bool top_level = false; + bool inside_world = false; - int children_lock; - Node3D *parent; + int children_lock = 0; + Node3D *parent = nullptr; List<Node3D *> children; - List<Node3D *>::Element *C; + List<Node3D *>::Element *C = nullptr; - bool ignore_notification; - bool notify_local_transform; - bool notify_transform; + bool ignore_notification = false; + bool notify_local_transform = false; + bool notify_transform = false; - bool visible; - bool disable_scale; + bool visible = true; + bool disable_scale = false; #ifdef TOOLS_ENABLED Ref<Node3DGizmo> gizmo; - bool gizmo_disabled; - bool gizmo_dirty; + bool gizmo_disabled = false; + bool gizmo_dirty = false; #endif } data; @@ -197,7 +197,6 @@ public: void force_update_transform(); Node3D(); - ~Node3D(); }; #endif // NODE_3D_H diff --git a/scene/3d/path_3d.cpp b/scene/3d/path_3d.cpp index ae6bbad8bf..54e6330722 100644 --- a/scene/3d/path_3d.cpp +++ b/scene/3d/path_3d.cpp @@ -385,14 +385,3 @@ void PathFollow3D::set_loop(bool p_loop) { bool PathFollow3D::has_loop() const { return loop; } - -PathFollow3D::PathFollow3D() { - offset = 0; - delta_offset = 0; - h_offset = 0; - v_offset = 0; - path = nullptr; - rotation_mode = ROTATION_XYZ; - cubic = true; - loop = true; -} diff --git a/scene/3d/path_3d.h b/scene/3d/path_3d.h index 1b0f5fa4e0..39f04f1556 100644 --- a/scene/3d/path_3d.h +++ b/scene/3d/path_3d.h @@ -65,14 +65,14 @@ public: }; private: - Path3D *path; - real_t delta_offset; // change in offset since last _update_transform - real_t offset; - real_t h_offset; - real_t v_offset; - bool cubic; - bool loop; - RotationMode rotation_mode; + Path3D *path = nullptr; + real_t delta_offset = 0; // Change in offset since last _update_transform. + real_t offset = 0; + real_t h_offset = 0; + real_t v_offset = 0; + bool cubic = true; + bool loop = true; + RotationMode rotation_mode = ROTATION_XYZ; void _update_transform(bool p_update_xyz_rot = true); @@ -106,7 +106,7 @@ public: String get_configuration_warning() const override; - PathFollow3D(); + PathFollow3D() {} }; VARIANT_ENUM_CAST(PathFollow3D::RotationMode); diff --git a/scene/3d/reflection_probe.cpp b/scene/3d/reflection_probe.cpp index c7948395d3..c82ed423a7 100644 --- a/scene/3d/reflection_probe.cpp +++ b/scene/3d/reflection_probe.cpp @@ -76,6 +76,15 @@ float ReflectionProbe::get_max_distance() const { return max_distance; } +void ReflectionProbe::set_lod_threshold(float p_pixels) { + lod_threshold = p_pixels; + RS::get_singleton()->reflection_probe_set_lod_threshold(probe, p_pixels); +} + +float ReflectionProbe::get_lod_threshold() const { + return lod_threshold; +} + void ReflectionProbe::set_extents(const Vector3 &p_extents) { extents = p_extents; @@ -199,6 +208,9 @@ void ReflectionProbe::_bind_methods() { ClassDB::bind_method(D_METHOD("set_max_distance", "max_distance"), &ReflectionProbe::set_max_distance); ClassDB::bind_method(D_METHOD("get_max_distance"), &ReflectionProbe::get_max_distance); + ClassDB::bind_method(D_METHOD("set_lod_threshold", "ratio"), &ReflectionProbe::set_lod_threshold); + ClassDB::bind_method(D_METHOD("get_lod_threshold"), &ReflectionProbe::get_lod_threshold); + ClassDB::bind_method(D_METHOD("set_extents", "extents"), &ReflectionProbe::set_extents); ClassDB::bind_method(D_METHOD("get_extents"), &ReflectionProbe::get_extents); @@ -229,6 +241,7 @@ void ReflectionProbe::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::BOOL, "interior"), "set_as_interior", "is_set_as_interior"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enable_shadows"), "set_enable_shadows", "are_shadows_enabled"); ADD_PROPERTY(PropertyInfo(Variant::INT, "cull_mask", PROPERTY_HINT_LAYERS_3D_RENDER), "set_cull_mask", "get_cull_mask"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "lod_threshold", PROPERTY_HINT_RANGE, "0,1024,0.1"), "set_lod_threshold", "get_lod_threshold"); ADD_GROUP("Ambient", "ambient_"); ADD_PROPERTY(PropertyInfo(Variant::INT, "ambient_mode", PROPERTY_HINT_ENUM, "Disabled,Environment,ConstantColor"), "set_ambient_mode", "get_ambient_mode"); @@ -256,6 +269,7 @@ ReflectionProbe::ReflectionProbe() { enable_shadows = false; cull_mask = (1 << 20) - 1; update_mode = UPDATE_ONCE; + lod_threshold = 1.0; probe = RenderingServer::get_singleton()->reflection_probe_create(); RS::get_singleton()->instance_set_base(get_instance(), probe); diff --git a/scene/3d/reflection_probe.h b/scene/3d/reflection_probe.h index 56177d0f95..4bff2f8bf9 100644 --- a/scene/3d/reflection_probe.h +++ b/scene/3d/reflection_probe.h @@ -63,6 +63,7 @@ private: AmbientMode ambient_mode; Color ambient_color; float ambient_color_energy; + float lod_threshold; uint32_t cull_mask; UpdateMode update_mode; @@ -90,6 +91,9 @@ public: void set_max_distance(float p_distance); float get_max_distance() const; + void set_lod_threshold(float p_pixels); + float get_lod_threshold() const; + void set_extents(const Vector3 &p_extents); Vector3 get_extents() const; diff --git a/scene/3d/visual_instance_3d.cpp b/scene/3d/visual_instance_3d.cpp index a1c498e8ab..0b70b0f920 100644 --- a/scene/3d/visual_instance_3d.cpp +++ b/scene/3d/visual_instance_3d.cpp @@ -278,6 +278,16 @@ float GeometryInstance3D::get_extra_cull_margin() const { return extra_cull_margin; } +void GeometryInstance3D::set_lod_bias(float p_bias) { + ERR_FAIL_COND(p_bias < 0.0); + lod_bias = p_bias; + RS::get_singleton()->instance_geometry_set_lod_bias(get_instance(), lod_bias); +} + +float GeometryInstance3D::get_lod_bias() const { + return lod_bias; +} + void GeometryInstance3D::set_shader_instance_uniform(const StringName &p_uniform, const Variant &p_value) { if (p_value.get_type() == Variant::NIL) { Variant def_value = RS::get_singleton()->instance_geometry_get_shader_parameter_default_value(get_instance(), p_uniform); @@ -361,6 +371,9 @@ void GeometryInstance3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_gi_mode", "mode"), &GeometryInstance3D::set_gi_mode); ClassDB::bind_method(D_METHOD("get_gi_mode"), &GeometryInstance3D::get_gi_mode); + ClassDB::bind_method(D_METHOD("set_lod_bias", "p_bias"), &GeometryInstance3D::set_lod_bias); + ClassDB::bind_method(D_METHOD("get_lod_bias"), &GeometryInstance3D::get_lod_bias); + ClassDB::bind_method(D_METHOD("set_custom_aabb", "aabb"), &GeometryInstance3D::set_custom_aabb); ClassDB::bind_method(D_METHOD("get_aabb"), &GeometryInstance3D::get_aabb); @@ -369,6 +382,7 @@ void GeometryInstance3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "material_override", PROPERTY_HINT_RESOURCE_TYPE, "ShaderMaterial,StandardMaterial3D", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_DEFERRED_SET_RESOURCE), "set_material_override", "get_material_override"); ADD_PROPERTY(PropertyInfo(Variant::INT, "cast_shadow", PROPERTY_HINT_ENUM, "Off,On,Double-Sided,Shadows Only"), "set_cast_shadows_setting", "get_cast_shadows_setting"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "extra_cull_margin", PROPERTY_HINT_RANGE, "0,16384,0.01"), "set_extra_cull_margin", "get_extra_cull_margin"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "lod_bias", PROPERTY_HINT_RANGE, "0.001,128,0.001"), "set_lod_bias", "get_lod_bias"); ADD_GROUP("Global Illumination", "gi_"); ADD_PROPERTY(PropertyInfo(Variant::INT, "gi_mode", PROPERTY_HINT_ENUM, "Disabled,Baked,Dynamic"), "set_gi_mode", "get_gi_mode"); ADD_PROPERTY(PropertyInfo(Variant::INT, "gi_lightmap_scale", PROPERTY_HINT_ENUM, "1x,2x,4x,8x"), "set_lightmap_scale", "get_lightmap_scale"); @@ -403,6 +417,8 @@ GeometryInstance3D::GeometryInstance3D() { lod_min_hysteresis = 0; lod_max_hysteresis = 0; + lod_bias = 1.0; + gi_mode = GI_MODE_DISABLED; lightmap_scale = LIGHTMAP_SCALE_1X; diff --git a/scene/3d/visual_instance_3d.h b/scene/3d/visual_instance_3d.h index 51bcb411da..0810b7b4ce 100644 --- a/scene/3d/visual_instance_3d.h +++ b/scene/3d/visual_instance_3d.h @@ -112,6 +112,8 @@ private: float lod_min_hysteresis; float lod_max_hysteresis; + float lod_bias; + mutable HashMap<StringName, Variant> instance_uniforms; mutable HashMap<StringName, StringName> instance_uniform_property_remap; @@ -151,6 +153,9 @@ public: void set_extra_cull_margin(float p_margin); float get_extra_cull_margin() const; + void set_lod_bias(float p_bias); + float get_lod_bias() const; + void set_gi_mode(GIMode p_mode); GIMode get_gi_mode() const; diff --git a/scene/3d/xr_nodes.cpp b/scene/3d/xr_nodes.cpp index 763461880f..5d48795dc1 100644 --- a/scene/3d/xr_nodes.cpp +++ b/scene/3d/xr_nodes.cpp @@ -247,7 +247,7 @@ void XRController3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_joystick_axis", "axis"), &XRController3D::get_joystick_axis); ClassDB::bind_method(D_METHOD("get_is_active"), &XRController3D::get_is_active); - ClassDB::bind_method(D_METHOD("get_hand"), &XRController3D::get_hand); + ClassDB::bind_method(D_METHOD("get_tracker_hand"), &XRController3D::get_tracker_hand); ClassDB::bind_method(D_METHOD("get_rumble"), &XRController3D::get_rumble); ClassDB::bind_method(D_METHOD("set_rumble", "rumble"), &XRController3D::set_rumble); @@ -349,7 +349,7 @@ bool XRController3D::get_is_active() const { return is_active; }; -XRPositionalTracker::TrackerHand XRController3D::get_hand() const { +XRPositionalTracker::TrackerHand XRController3D::get_tracker_hand() const { // get our XRServer XRServer *xr_server = XRServer::get_singleton(); ERR_FAIL_NULL_V(xr_server, XRPositionalTracker::TRACKER_HAND_UNKNOWN); @@ -359,7 +359,7 @@ XRPositionalTracker::TrackerHand XRController3D::get_hand() const { return XRPositionalTracker::TRACKER_HAND_UNKNOWN; }; - return tracker->get_hand(); + return tracker->get_tracker_hand(); }; String XRController3D::get_configuration_warning() const { diff --git a/scene/3d/xr_nodes.h b/scene/3d/xr_nodes.h index 751b2b68cb..6aa7709485 100644 --- a/scene/3d/xr_nodes.h +++ b/scene/3d/xr_nodes.h @@ -93,7 +93,7 @@ public: void set_rumble(real_t p_rumble); bool get_is_active() const; - XRPositionalTracker::TrackerHand get_hand() const; + XRPositionalTracker::TrackerHand get_tracker_hand() const; Ref<Mesh> get_mesh() const; diff --git a/scene/SCsub b/scene/SCsub index f9fc00f3f2..ccd2bab8ff 100644 --- a/scene/SCsub +++ b/scene/SCsub @@ -4,24 +4,9 @@ Import("env") env.scene_sources = [] -# Thirdparty code -thirdparty_dir = "#thirdparty/misc/" -thirdparty_sources = [ - # C++ sources - "easing_equations.cpp", - # C sources - "mikktspace.c", -] -thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] - -env_thirdparty = env.Clone() -env_thirdparty.disable_warnings() -env_thirdparty.add_source_files(env.scene_sources, thirdparty_sources) - -# Godot's own sources +# Godot source files env.add_source_files(env.scene_sources, "*.cpp") - # Chain load SCsubs SConscript("main/SCsub") SConscript("gui/SCsub") @@ -32,7 +17,6 @@ SConscript("audio/SCsub") SConscript("resources/SCsub") SConscript("debugger/SCsub") - # Build it all as a library lib = env.add_library("scene", env.scene_sources) env.Prepend(LIBS=[lib]) diff --git a/scene/animation/SCsub b/scene/animation/SCsub index fc61250247..cc33a5af84 100644 --- a/scene/animation/SCsub +++ b/scene/animation/SCsub @@ -2,4 +2,23 @@ Import("env") -env.add_source_files(env.scene_sources, "*.cpp") +# Thirdparty code + +thirdparty_obj = [] + +thirdparty_sources = "#thirdparty/misc/easing_equations.cpp" + +env_thirdparty = env.Clone() +env_thirdparty.disable_warnings() +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.scene_sources += thirdparty_obj + +# Godot source files + +scene_obj = [] + +env.add_source_files(scene_obj, "*.cpp") +env.scene_sources += scene_obj + +# Needed to force rebuilding the scene files when the thirdparty code is updated. +env.Depends(scene_obj, thirdparty_obj) diff --git a/scene/animation/animation_player.cpp b/scene/animation/animation_player.cpp index e9e17148d6..159ccae130 100644 --- a/scene/animation/animation_player.cpp +++ b/scene/animation/animation_player.cpp @@ -36,6 +36,7 @@ #include "servers/audio/audio_stream.h" #ifdef TOOLS_ENABLED +#include "editor/editor_node.h" #include "editor/editor_settings.h" #include "scene/2d/skeleton_2d.h" @@ -53,6 +54,21 @@ void AnimatedValuesBackup::update_skeletons() { } } } + +void AnimatedValuesBackup::restore() const { + for (int i = 0; i < entries.size(); i++) { + const AnimatedValuesBackup::Entry *entry = &entries[i]; + if (entry->bone_idx == -1) { + entry->object->set_indexed(entry->subpath, entry->value); + } else { + Object::cast_to<Skeleton3D>(entry->object)->set_bone_pose(entry->bone_idx, entry->value); + } + } +} + +void AnimatedValuesBackup::_bind_methods() { + ClassDB::bind_method(D_METHOD("restore"), &AnimatedValuesBackup::restore); +} #endif bool AnimationPlayer::_set(const StringName &p_name, const Variant &p_value) { @@ -1379,6 +1395,14 @@ String AnimationPlayer::get_autoplay() const { return autoplay; } +void AnimationPlayer::set_reset_on_save_enabled(bool p_enabled) { + reset_on_save = p_enabled; +} + +bool AnimationPlayer::is_reset_on_save_enabled() const { + return reset_on_save; +} + void AnimationPlayer::set_animation_process_mode(AnimationProcessMode p_mode) { if (animation_process_mode == p_mode) { return; @@ -1473,15 +1497,15 @@ void AnimationPlayer::get_argument_options(const StringName &p_function, int p_i } #ifdef TOOLS_ENABLED -AnimatedValuesBackup AnimationPlayer::backup_animated_values() { +Ref<AnimatedValuesBackup> AnimationPlayer::backup_animated_values() { + Ref<AnimatedValuesBackup> backup; if (!playback.current.from) { - return AnimatedValuesBackup(); + return backup; } _ensure_node_caches(playback.current.from); - AnimatedValuesBackup backup; - + backup.instance(); for (int i = 0; i < playback.current.from->node_cache.size(); i++) { TrackNodeCache *nc = playback.current.from->node_cache[i]; if (!nc) { @@ -1497,7 +1521,7 @@ AnimatedValuesBackup AnimationPlayer::backup_animated_values() { entry.object = nc->skeleton; entry.bone_idx = nc->bone_idx; entry.value = nc->skeleton->get_bone_pose(nc->bone_idx); - backup.entries.push_back(entry); + backup->entries.push_back(entry); } else { if (nc->spatial) { AnimatedValuesBackup::Entry entry; @@ -1505,7 +1529,7 @@ AnimatedValuesBackup AnimationPlayer::backup_animated_values() { entry.subpath.push_back("transform"); entry.value = nc->spatial->get_transform(); entry.bone_idx = -1; - backup.entries.push_back(entry); + backup->entries.push_back(entry); } else { for (Map<StringName, TrackNodeCache::PropertyAnim>::Element *E = nc->property_anim.front(); E; E = E->next()) { AnimatedValuesBackup::Entry entry; @@ -1515,7 +1539,7 @@ AnimatedValuesBackup AnimationPlayer::backup_animated_values() { entry.value = E->value().object->get_indexed(E->value().subpath, &valid); entry.bone_idx = -1; if (valid) { - backup.entries.push_back(entry); + backup->entries.push_back(entry); } } } @@ -1525,15 +1549,40 @@ AnimatedValuesBackup AnimationPlayer::backup_animated_values() { return backup; } -void AnimationPlayer::restore_animated_values(const AnimatedValuesBackup &p_backup) { - for (int i = 0; i < p_backup.entries.size(); i++) { - const AnimatedValuesBackup::Entry *entry = &p_backup.entries[i]; - if (entry->bone_idx == -1) { - entry->object->set_indexed(entry->subpath, entry->value); - } else { - Object::cast_to<Skeleton3D>(entry->object)->set_bone_pose(entry->bone_idx, entry->value); - } +Ref<AnimatedValuesBackup> AnimationPlayer::apply_reset(bool p_user_initiated) { + ERR_FAIL_COND_V(!can_apply_reset(), Ref<AnimatedValuesBackup>()); + + Ref<Animation> reset_anim = animation_set["RESET"].animation; + ERR_FAIL_COND_V(reset_anim.is_null(), Ref<AnimatedValuesBackup>()); + + Node *root_node = get_node_or_null(root); + ERR_FAIL_COND_V(!root_node, Ref<AnimatedValuesBackup>()); + + AnimationPlayer *aux_player = memnew(AnimationPlayer); + EditorNode::get_singleton()->add_child(aux_player); + aux_player->set_root(aux_player->get_path_to(root_node)); + aux_player->add_animation("RESET", reset_anim); + aux_player->set_assigned_animation("RESET"); + Ref<AnimatedValuesBackup> old_values = aux_player->backup_animated_values(); + aux_player->seek(0.0f, true); + aux_player->queue_delete(); + + if (p_user_initiated) { + Ref<AnimatedValuesBackup> new_values = aux_player->backup_animated_values(); + old_values->restore(); + + UndoRedo *ur = EditorNode::get_singleton()->get_undo_redo(); + ur->create_action(TTR("Anim Apply Reset")); + ur->add_do_method(new_values.ptr(), "restore"); + ur->add_undo_method(old_values.ptr(), "restore"); + ur->commit_action(); } + + return old_values; +} + +bool AnimationPlayer::can_apply_reset() const { + return has_animation("RESET") && playback.assigned != StringName("RESET"); } #endif @@ -1577,6 +1626,9 @@ void AnimationPlayer::_bind_methods() { ClassDB::bind_method(D_METHOD("set_autoplay", "name"), &AnimationPlayer::set_autoplay); ClassDB::bind_method(D_METHOD("get_autoplay"), &AnimationPlayer::get_autoplay); + ClassDB::bind_method(D_METHOD("set_reset_on_save_enabled", "enabled"), &AnimationPlayer::set_reset_on_save_enabled); + ClassDB::bind_method(D_METHOD("is_reset_on_save_enabled"), &AnimationPlayer::is_reset_on_save_enabled); + ClassDB::bind_method(D_METHOD("set_root", "path"), &AnimationPlayer::set_root); ClassDB::bind_method(D_METHOD("get_root"), &AnimationPlayer::get_root); @@ -1600,6 +1652,7 @@ void AnimationPlayer::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "current_animation", PROPERTY_HINT_ENUM, "", PROPERTY_USAGE_EDITOR | PROPERTY_USAGE_ANIMATE_AS_TRIGGER), "set_current_animation", "get_current_animation"); ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "assigned_animation", PROPERTY_HINT_NONE, "", 0), "set_assigned_animation", "get_assigned_animation"); ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "autoplay", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_autoplay", "get_autoplay"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "reset_on_save", PROPERTY_HINT_NONE, ""), "set_reset_on_save_enabled", "is_reset_on_save_enabled"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "current_animation_length", PROPERTY_HINT_NONE, "", 0), "", "get_current_animation_length"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "current_animation_position", PROPERTY_HINT_NONE, "", 0), "", "get_current_animation_position"); @@ -1631,6 +1684,7 @@ AnimationPlayer::AnimationPlayer() { speed_scale = 1; end_reached = false; end_notify = false; + reset_on_save = true; animation_process_mode = ANIMATION_PROCESS_IDLE; method_call_mode = ANIMATION_METHOD_CALL_DEFERRED; processing = false; diff --git a/scene/animation/animation_player.h b/scene/animation/animation_player.h index dbce5643c7..7f0d5630e1 100644 --- a/scene/animation/animation_player.h +++ b/scene/animation/animation_player.h @@ -37,8 +37,9 @@ #include "scene/resources/animation.h" #ifdef TOOLS_ENABLED -// To save/restore animated values -class AnimatedValuesBackup { +class AnimatedValuesBackup : public Reference { + GDCLASS(AnimatedValuesBackup, Reference); + struct Entry { Object *object; Vector<StringName> subpath; // Unused if bone @@ -49,8 +50,12 @@ class AnimatedValuesBackup { friend class AnimationPlayer; +protected: + static void _bind_methods(); + public: void update_skeletons(); + void restore() const; }; #endif @@ -215,6 +220,7 @@ private: bool end_notify; String autoplay; + bool reset_on_save; AnimationProcessMode animation_process_mode; AnimationMethodCallMode method_call_mode; bool processing; @@ -304,6 +310,9 @@ public: void set_autoplay(const String &p_name); String get_autoplay() const; + void set_reset_on_save_enabled(bool p_enabled); + bool is_reset_on_save_enabled() const; + void set_animation_process_mode(AnimationProcessMode p_mode); AnimationProcessMode get_animation_process_mode() const; @@ -325,9 +334,9 @@ public: void get_argument_options(const StringName &p_function, int p_idx, List<String> *r_options) const override; #ifdef TOOLS_ENABLED - // These may be interesting for games, but are too dangerous for general use - AnimatedValuesBackup backup_animated_values(); - void restore_animated_values(const AnimatedValuesBackup &p_backup); + Ref<AnimatedValuesBackup> backup_animated_values(); + Ref<AnimatedValuesBackup> apply_reset(bool p_user_initiated = false); + bool can_apply_reset() const; #endif AnimationPlayer(); diff --git a/scene/gui/control.cpp b/scene/gui/control.cpp index bdbb0d4684..bc37045386 100644 --- a/scene/gui/control.cpp +++ b/scene/gui/control.cpp @@ -3043,38 +3043,3 @@ void Control::_bind_methods() { BIND_VMETHOD(MethodInfo(Variant::BOOL, "has_point", PropertyInfo(Variant::VECTOR2, "point"))); } - -Control::Control() { - data.parent = nullptr; - - data.mouse_filter = MOUSE_FILTER_STOP; - - data.RI = nullptr; - data.theme_owner = nullptr; - data.theme_owner_window = nullptr; - data.default_cursor = CURSOR_ARROW; - data.layout_dir = LAYOUT_DIRECTION_INHERITED; - data.h_size_flags = SIZE_FILL; - data.v_size_flags = SIZE_FILL; - data.expand = 1; - data.rotation = 0; - data.parent_canvas_item = nullptr; - data.scale = Vector2(1, 1); - - data.block_minimum_size_adjust = false; - data.disable_visibility_clip = false; - data.h_grow = GROW_DIRECTION_END; - data.v_grow = GROW_DIRECTION_END; - data.minimum_size_valid = false; - data.updating_last_minimum_size = false; - - data.clip_contents = false; - for (int i = 0; i < 4; i++) { - data.anchor[i] = ANCHOR_BEGIN; - data.margin[i] = 0; - } - data.focus_mode = FOCUS_NONE; -} - -Control::~Control() { -} diff --git a/scene/gui/control.h b/scene/gui/control.h index d314c7357b..2241c242bb 100644 --- a/scene/gui/control.h +++ b/scene/gui/control.h @@ -166,46 +166,46 @@ private: Point2 pos_cache; Size2 size_cache; Size2 minimum_size_cache; - bool minimum_size_valid; + bool minimum_size_valid = false; Size2 last_minimum_size; - bool updating_last_minimum_size; + bool updating_last_minimum_size = false; - float margin[4]; - float anchor[4]; - FocusMode focus_mode; - GrowDirection h_grow; - GrowDirection v_grow; + float margin[4] = { 0.0, 0.0, 0.0, 0.0 }; + float anchor[4] = { ANCHOR_BEGIN, ANCHOR_BEGIN, ANCHOR_BEGIN, ANCHOR_BEGIN }; + FocusMode focus_mode = FOCUS_NONE; + GrowDirection h_grow = GROW_DIRECTION_END; + GrowDirection v_grow = GROW_DIRECTION_END; - LayoutDirection layout_dir; + LayoutDirection layout_dir = LAYOUT_DIRECTION_INHERITED; - float rotation; - Vector2 scale; + float rotation = 0; + Vector2 scale = Vector2(1, 1); Vector2 pivot_offset; - int h_size_flags; - int v_size_flags; - float expand; + int h_size_flags = SIZE_FILL; + int v_size_flags = SIZE_FILL; + float expand = 1; Point2 custom_minimum_size; - MouseFilter mouse_filter; + MouseFilter mouse_filter = MOUSE_FILTER_STOP; - bool clip_contents; + bool clip_contents = false; - bool block_minimum_size_adjust; - bool disable_visibility_clip; + bool block_minimum_size_adjust = false; + bool disable_visibility_clip = false; - Control *parent; + Control *parent = nullptr; ObjectID drag_owner; Ref<Theme> theme; - Control *theme_owner; - Window *theme_owner_window; + Control *theme_owner = nullptr; + Window *theme_owner_window = nullptr; String tooltip; - CursorShape default_cursor; + CursorShape default_cursor = CURSOR_ARROW; - List<Control *>::Element *RI; + List<Control *>::Element *RI = nullptr; - CanvasItem *parent_canvas_item; + CanvasItem *parent_canvas_item = nullptr; NodePath focus_neighbor[4]; NodePath focus_next; @@ -518,8 +518,7 @@ public: virtual void get_argument_options(const StringName &p_function, int p_idx, List<String> *r_options) const override; virtual String get_configuration_warning() const override; - Control(); - ~Control(); + Control() {} }; VARIANT_ENUM_CAST(Control::FocusMode); diff --git a/scene/gui/graph_edit.cpp b/scene/gui/graph_edit.cpp index 84e2231919..2bcc1890fe 100644 --- a/scene/gui/graph_edit.cpp +++ b/scene/gui/graph_edit.cpp @@ -392,6 +392,7 @@ void GraphEdit::add_child_notify(Node *p_child) { gn->connect("offset_changed", callable_mp(this, &GraphEdit::_graph_node_moved), varray(gn)); gn->connect("raise_request", callable_mp(this, &GraphEdit::_graph_node_raised), varray(gn)); gn->connect("item_rect_changed", callable_mp((CanvasItem *)connections_layer, &CanvasItem::update)); + gn->connect("item_rect_changed", callable_mp((CanvasItem *)minimap, &GraphEditMinimap::update)); _graph_node_moved(gn); gn->set_mouse_filter(MOUSE_FILTER_PASS); } @@ -409,6 +410,7 @@ void GraphEdit::remove_child_notify(Node *p_child) { gn->disconnect("offset_changed", callable_mp(this, &GraphEdit::_graph_node_moved)); gn->disconnect("raise_request", callable_mp(this, &GraphEdit::_graph_node_raised)); gn->disconnect("item_rect_changed", callable_mp((CanvasItem *)connections_layer, &CanvasItem::update)); + gn->disconnect("item_rect_changed", callable_mp((CanvasItem *)minimap, &GraphEditMinimap::update)); } } @@ -782,7 +784,7 @@ void GraphEdit::_bake_segment2d(Vector<Vector2> &points, Vector<Color> &colors, } } -void GraphEdit::_draw_cos_line(CanvasItem *p_where, const Vector2 &p_from, const Vector2 &p_to, const Color &p_color, const Color &p_to_color, float p_bezier_ratio) { +void GraphEdit::_draw_cos_line(CanvasItem *p_where, const Vector2 &p_from, const Vector2 &p_to, const Color &p_color, const Color &p_to_color, float p_width, float p_bezier_ratio = 1.0) { //cubic bezier code float diff = p_to.x - p_from.x; float cp_offset; @@ -809,9 +811,9 @@ void GraphEdit::_draw_cos_line(CanvasItem *p_where, const Vector2 &p_from, const colors.push_back(p_to_color); #ifdef TOOLS_ENABLED - p_where->draw_polyline_colors(points, colors, Math::floor(2 * EDSCALE), true); + p_where->draw_polyline_colors(points, colors, Math::floor(p_width * EDSCALE), lines_antialiased); #else - p_where->draw_polyline_colors(points, colors, 2, true); + p_where->draw_polyline_colors(points, colors, p_width, lines_antialiased); #endif } @@ -858,7 +860,7 @@ void GraphEdit::_connections_layer_draw() { color = color.lerp(activity_color, E->get().activity); tocolor = tocolor.lerp(activity_color, E->get().activity); } - _draw_cos_line(connections_layer, frompos, topos, color, tocolor, 1.0); + _draw_cos_line(connections_layer, frompos, topos, color, tocolor, lines_thickness); } while (to_erase.size()) { @@ -897,7 +899,7 @@ void GraphEdit::_top_layer_draw() { if (!connecting_out) { SWAP(pos, topos); } - _draw_cos_line(top_layer, pos, topos, col, col, 1.0); + _draw_cos_line(top_layer, pos, topos, col, col, lines_thickness); } if (box_selecting) { @@ -990,18 +992,18 @@ void GraphEdit::_minimap_draw() { continue; } - Vector2 from_slot_position = gfrom->get_offset() + gfrom->get_connection_output_position(E->get().from_port); - Vector2 from_position = minimap->_convert_from_graph_position(from_slot_position * zoom - graph_offset) + minimap_offset; + Vector2 from_slot_position = gfrom->get_offset() * zoom + gfrom->get_connection_output_position(E->get().from_port); + Vector2 from_position = minimap->_convert_from_graph_position(from_slot_position - graph_offset) + minimap_offset; Color from_color = gfrom->get_connection_output_color(E->get().from_port); - Vector2 to_slot_position = gto->get_offset() + gto->get_connection_input_position(E->get().to_port); - Vector2 to_position = minimap->_convert_from_graph_position(to_slot_position * zoom - graph_offset) + minimap_offset; + Vector2 to_slot_position = gto->get_offset() * zoom + gto->get_connection_input_position(E->get().to_port); + Vector2 to_position = minimap->_convert_from_graph_position(to_slot_position - graph_offset) + minimap_offset; Color to_color = gto->get_connection_input_color(E->get().to_port); if (E->get().activity > 0) { from_color = from_color.lerp(activity_color, E->get().activity); to_color = to_color.lerp(activity_color, E->get().activity); } - _draw_cos_line(minimap, from_position, to_position, from_color, to_color, 0.5); + _draw_cos_line(minimap, from_position, to_position, from_color, to_color, 1.0, 0.5); } // Draw the "camera" viewport. @@ -1495,8 +1497,6 @@ void GraphEdit::_snap_value_changed(double) { update(); } -// _minimap_toggled - void GraphEdit::set_minimap_size(Vector2 p_size) { minimap->set_size(p_size); Vector2 minimap_size = minimap->get_size(); // The size might've been adjusted by the minimum size. @@ -1536,6 +1536,24 @@ void GraphEdit::_minimap_toggled() { minimap->update(); } +void GraphEdit::set_connection_lines_thickness(float p_thickness) { + lines_thickness = p_thickness; + update(); +} + +float GraphEdit::get_connection_lines_thickness() const { + return lines_thickness; +} + +void GraphEdit::set_connection_lines_antialiased(bool p_antialiased) { + lines_antialiased = p_antialiased; + update(); +} + +bool GraphEdit::is_connection_lines_antialiased() const { + return lines_antialiased; +} + HBoxContainer *GraphEdit::get_zoom_hbox() { return zoom_hb; } @@ -1567,6 +1585,12 @@ void GraphEdit::_bind_methods() { ClassDB::bind_method(D_METHOD("set_use_snap", "enable"), &GraphEdit::set_use_snap); ClassDB::bind_method(D_METHOD("is_using_snap"), &GraphEdit::is_using_snap); + ClassDB::bind_method(D_METHOD("set_connection_lines_thickness", "pixels"), &GraphEdit::set_connection_lines_thickness); + ClassDB::bind_method(D_METHOD("get_connection_lines_thickness"), &GraphEdit::get_connection_lines_thickness); + + ClassDB::bind_method(D_METHOD("set_connection_lines_antialiased", "pixels"), &GraphEdit::set_connection_lines_antialiased); + ClassDB::bind_method(D_METHOD("is_connection_lines_antialiased"), &GraphEdit::is_connection_lines_antialiased); + ClassDB::bind_method(D_METHOD("set_minimap_size", "p_size"), &GraphEdit::set_minimap_size); ClassDB::bind_method(D_METHOD("get_minimap_size"), &GraphEdit::get_minimap_size); ClassDB::bind_method(D_METHOD("set_minimap_opacity", "p_opacity"), &GraphEdit::set_minimap_opacity); @@ -1590,6 +1614,8 @@ void GraphEdit::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "snap_distance"), "set_snap", "get_snap"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_snap"), "set_use_snap", "is_using_snap"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "zoom"), "set_zoom", "get_zoom"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "connection_lines_thickness"), "set_connection_lines_thickness", "get_connection_lines_thickness"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "connection_lines_antialiased"), "set_connection_lines_antialiased", "is_connection_lines_antialiased"); ADD_GROUP("Minimap", "minimap"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "minimap_enabled"), "set_minimap_enabled", "is_minimap_enabled"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "minimap_size"), "set_minimap_size", "get_minimap_size"); @@ -1709,7 +1735,7 @@ GraphEdit::GraphEdit() { zoom_hb->add_child(minimap_button); Vector2 minimap_size = Vector2(240, 160); - float minimap_opacity = 0.45; + float minimap_opacity = 0.65; minimap = memnew(GraphEditMinimap(this)); top_layer->add_child(minimap); diff --git a/scene/gui/graph_edit.h b/scene/gui/graph_edit.h index b9c40251ce..d081789784 100644 --- a/scene/gui/graph_edit.h +++ b/scene/gui/graph_edit.h @@ -158,9 +158,12 @@ private: bool awaiting_scroll_offset_update; List<Connection> connections; + float lines_thickness = 2.0f; + bool lines_antialiased = true; + void _bake_segment2d(Vector<Vector2> &points, Vector<Color> &colors, float p_begin, float p_end, const Vector2 &p_a, const Vector2 &p_out, const Vector2 &p_b, const Vector2 &p_in, int p_depth, int p_min_depth, int p_max_depth, float p_tol, const Color &p_color, const Color &p_to_color, int &lines) const; - void _draw_cos_line(CanvasItem *p_where, const Vector2 &p_from, const Vector2 &p_to, const Color &p_color, const Color &p_to_color, float p_bezier_ratio); + void _draw_cos_line(CanvasItem *p_where, const Vector2 &p_from, const Vector2 &p_to, const Color &p_color, const Color &p_to_color, float p_width, float p_bezier_ratio); void _graph_node_raised(Node *p_gn); void _graph_node_moved(Node *p_gn); @@ -275,6 +278,12 @@ public: int get_snap() const; void set_snap(int p_snap); + void set_connection_lines_thickness(float p_thickness); + float get_connection_lines_thickness() const; + + void set_connection_lines_antialiased(bool p_antialiased); + bool is_connection_lines_antialiased() const; + HBoxContainer *get_zoom_hbox(); GraphEdit(); diff --git a/scene/gui/panel.cpp b/scene/gui/panel.cpp index acbb6d7ab5..28cc056d6e 100644 --- a/scene/gui/panel.cpp +++ b/scene/gui/panel.cpp @@ -63,6 +63,3 @@ Panel::Panel() { // Has visible stylebox, so stop by default. set_mouse_filter(MOUSE_FILTER_STOP); } - -Panel::~Panel() { -} diff --git a/scene/gui/panel.h b/scene/gui/panel.h index a68c3d3f0c..e2c1ddc91d 100644 --- a/scene/gui/panel.h +++ b/scene/gui/panel.h @@ -54,7 +54,6 @@ public: Mode get_mode() const; Panel(); - ~Panel(); }; VARIANT_ENUM_CAST(Panel::Mode) diff --git a/scene/gui/tab_container.cpp b/scene/gui/tab_container.cpp index 01c1a15b79..809b4ffd64 100644 --- a/scene/gui/tab_container.cpp +++ b/scene/gui/tab_container.cpp @@ -568,16 +568,12 @@ void TabContainer::_draw_tab(Ref<StyleBox> &p_tab_style, Color &p_font_color, in text_buf[p_index]->draw(canvas, text_pos, p_font_color); } -void TabContainer::_on_theme_changed() { - if (!_theme_changing) { - return; - } - +void TabContainer::_refresh_texts() { text_buf.clear(); + Vector<Control *> tabs = _get_tabs(); bool rtl = is_layout_rtl(); Ref<Font> font = get_theme_font("font"); int font_size = get_theme_font_size("font_size"); - Vector<Control *> tabs = _get_tabs(); for (int i = 0; i < tabs.size(); i++) { Control *control = Object::cast_to<Control>(tabs[i]); String text = control->has_meta("_tab_name") ? String(tr(String(control->get_meta("_tab_name")))) : String(tr(control->get_name())); @@ -587,6 +583,14 @@ void TabContainer::_on_theme_changed() { name->add_string(text, font, font_size, Dictionary(), TranslationServer::get_singleton()->get_tool_locale()); text_buf.push_back(name); } +} + +void TabContainer::_on_theme_changed() { + if (!_theme_changing) { + return; + } + + _refresh_texts(); minimum_size_changed(); if (get_tab_count() > 0) { @@ -679,21 +683,7 @@ Vector<Control *> TabContainer::_get_tabs() const { } void TabContainer::_child_renamed_callback() { - text_buf.clear(); - Vector<Control *> tabs = _get_tabs(); - bool rtl = is_layout_rtl(); - Ref<Font> font = get_theme_font("font"); - int font_size = get_theme_font_size("font_size"); - for (int i = 0; i < tabs.size(); i++) { - Control *control = Object::cast_to<Control>(tabs[i]); - String text = control->has_meta("_tab_name") ? String(tr(String(control->get_meta("_tab_name")))) : String(tr(control->get_name())); - Ref<TextLine> name; - name.instance(); - name->set_direction(rtl ? TextServer::DIRECTION_RTL : TextServer::DIRECTION_LTR); - name->add_string(text, font, font_size, Dictionary(), TranslationServer::get_singleton()->get_tool_locale()); - text_buf.push_back(name); - } - + _refresh_texts(); update(); } @@ -708,20 +698,8 @@ void TabContainer::add_child_notify(Node *p_child) { return; } - text_buf.clear(); Vector<Control *> tabs = _get_tabs(); - bool rtl = is_layout_rtl(); - Ref<Font> font = get_theme_font("font"); - int font_size = get_theme_font_size("font_size"); - for (int i = 0; i < tabs.size(); i++) { - Control *control = Object::cast_to<Control>(tabs[i]); - String text = control->has_meta("_tab_name") ? String(tr(String(control->get_meta("_tab_name")))) : String(tr(control->get_name())); - Ref<TextLine> name; - name.instance(); - name->set_direction(rtl ? TextServer::DIRECTION_RTL : TextServer::DIRECTION_LTR); - name->add_string(text, font, font_size, Dictionary(), TranslationServer::get_singleton()->get_tool_locale()); - text_buf.push_back(name); - } + _refresh_texts(); bool first = false; @@ -743,7 +721,6 @@ void TabContainer::add_child_notify(Node *p_child) { c->set_margin(Margin(MARGIN_LEFT), c->get_margin(Margin(MARGIN_LEFT)) + sb->get_margin(Margin(MARGIN_LEFT))); c->set_margin(Margin(MARGIN_RIGHT), c->get_margin(Margin(MARGIN_RIGHT)) - sb->get_margin(Margin(MARGIN_RIGHT))); c->set_margin(Margin(MARGIN_BOTTOM), c->get_margin(Margin(MARGIN_BOTTOM)) - sb->get_margin(Margin(MARGIN_BOTTOM))); - update(); p_child->connect("renamed", callable_mp(this, &TabContainer::_child_renamed_callback)); if (first && is_inside_tree()) { @@ -751,6 +728,13 @@ void TabContainer::add_child_notify(Node *p_child) { } } +void TabContainer::move_child_notify(Node *p_child) { + Container::move_child_notify(p_child); + call_deferred("_update_current_tab"); + _refresh_texts(); + update(); +} + int TabContainer::get_tab_count() const { return _get_tabs().size(); } @@ -813,20 +797,8 @@ void TabContainer::remove_child_notify(Node *p_child) { } void TabContainer::_update_current_tab() { - text_buf.clear(); Vector<Control *> tabs = _get_tabs(); - bool rtl = is_layout_rtl(); - Ref<Font> font = get_theme_font("font"); - int font_size = get_theme_font_size("font_size"); - for (int i = 0; i < tabs.size(); i++) { - Control *control = Object::cast_to<Control>(tabs[i]); - String text = control->has_meta("_tab_name") ? String(tr(String(control->get_meta("_tab_name")))) : String(tr(control->get_name())); - Ref<TextLine> name; - name.instance(); - name->set_direction(rtl ? TextServer::DIRECTION_RTL : TextServer::DIRECTION_LTR); - name->add_string(text, font, font_size, Dictionary(), TranslationServer::get_singleton()->get_tool_locale()); - text_buf.push_back(name); - } + _refresh_texts(); int tc = tabs.size(); if (current >= tc) { diff --git a/scene/gui/tab_container.h b/scene/gui/tab_container.h index 91153e5fc3..9ff56afe6e 100644 --- a/scene/gui/tab_container.h +++ b/scene/gui/tab_container.h @@ -73,12 +73,14 @@ private: void _on_mouse_exited(); void _update_current_tab(); void _draw_tab(Ref<StyleBox> &p_tab_style, Color &p_font_color, int p_index, float p_x); + void _refresh_texts(); protected: void _child_renamed_callback(); void _gui_input(const Ref<InputEvent> &p_event); void _notification(int p_what); virtual void add_child_notify(Node *p_child) override; + virtual void move_child_notify(Node *p_child) override; virtual void remove_child_notify(Node *p_child) override; Variant get_drag_data(const Point2 &p_point) override; diff --git a/scene/main/canvas_item.cpp b/scene/main/canvas_item.cpp index 43350fae37..8babb1460f 100644 --- a/scene/main/canvas_item.cpp +++ b/scene/main/canvas_item.cpp @@ -292,15 +292,10 @@ void CanvasItemMaterial::_bind_methods() { CanvasItemMaterial::CanvasItemMaterial() : element(this) { - blend_mode = BLEND_MODE_MIX; - light_mode = LIGHT_MODE_NORMAL; - particles_animation = false; - set_particles_anim_h_frames(1); set_particles_anim_v_frames(1); set_particles_anim_loop(false); - current_key.key = 0; current_key.invalid_key = 1; _queue_shader_change(); } @@ -1411,30 +1406,7 @@ CanvasItem::TextureRepeat CanvasItem::get_texture_repeat() const { CanvasItem::CanvasItem() : xform_change(this) { - window = nullptr; canvas_item = RenderingServer::get_singleton()->canvas_item_create(); - visible = true; - pending_update = false; - modulate = Color(1, 1, 1, 1); - self_modulate = Color(1, 1, 1, 1); - top_level = false; - first_draw = false; - drawing = false; - behind = false; - clip_children = false; - block_transform_notify = false; - canvas_layer = nullptr; - use_parent_material = false; - global_invalid = true; - notify_local_transform = false; - notify_transform = false; - light_mask = 1; - texture_repeat = TEXTURE_REPEAT_PARENT_NODE; - texture_filter = TEXTURE_FILTER_PARENT_NODE; - texture_filter_cache = RS::CANVAS_ITEM_TEXTURE_FILTER_LINEAR; - texture_repeat_cache = RS::CANVAS_ITEM_TEXTURE_REPEAT_DISABLED; - - C = nullptr; } CanvasItem::~CanvasItem() { diff --git a/scene/main/canvas_item.h b/scene/main/canvas_item.h index 3cde6b69c1..34268c1a78 100644 --- a/scene/main/canvas_item.h +++ b/scene/main/canvas_item.h @@ -73,7 +73,7 @@ private: uint32_t invalid_key : 1; }; - uint32_t key; + uint32_t key = 0; bool operator<(const MaterialKey &p_key) const { return key < p_key.key; @@ -114,10 +114,11 @@ private: _FORCE_INLINE_ void _queue_shader_change(); _FORCE_INLINE_ bool _is_shader_dirty() const; - BlendMode blend_mode; - LightMode light_mode; - bool particles_animation; + BlendMode blend_mode = BLEND_MODE_MIX; + LightMode light_mode = LIGHT_MODE_NORMAL; + bool particles_animation = false; + // Initialized in the constructor. int particles_anim_h_frames; int particles_anim_v_frames; bool particles_anim_loop; @@ -188,39 +189,38 @@ private: RID canvas_item; String group; - CanvasLayer *canvas_layer; + CanvasLayer *canvas_layer = nullptr; - Color modulate; - Color self_modulate; + Color modulate = Color(1, 1, 1, 1); + Color self_modulate = Color(1, 1, 1, 1); List<CanvasItem *> children_items; - List<CanvasItem *>::Element *C; - - int light_mask; - - Window *window; - bool first_draw; - bool visible; - bool clip_children; - bool pending_update; - bool top_level; - bool drawing; - bool block_transform_notify; - bool behind; - bool use_parent_material; - bool notify_local_transform; - bool notify_transform; - - RS::CanvasItemTextureFilter texture_filter_cache; - RS::CanvasItemTextureRepeat texture_repeat_cache; - - TextureFilter texture_filter; - TextureRepeat texture_repeat; + List<CanvasItem *>::Element *C = nullptr; + + int light_mask = 1; + + Window *window = nullptr; + bool first_draw = false; + bool visible = true; + bool clip_children = false; + bool pending_update = false; + bool top_level = false; + bool drawing = false; + bool block_transform_notify = false; + bool behind = false; + bool use_parent_material = false; + bool notify_local_transform = false; + bool notify_transform = false; + + RS::CanvasItemTextureFilter texture_filter_cache = RS::CANVAS_ITEM_TEXTURE_FILTER_LINEAR; + RS::CanvasItemTextureRepeat texture_repeat_cache = RS::CANVAS_ITEM_TEXTURE_REPEAT_DISABLED; + TextureFilter texture_filter = TEXTURE_FILTER_PARENT_NODE; + TextureRepeat texture_repeat = TEXTURE_REPEAT_PARENT_NODE; Ref<Material> material; mutable Transform2D global_transform; - mutable bool global_invalid; + mutable bool global_invalid = true; void _top_level_raise_self(); diff --git a/scene/main/node.cpp b/scene/main/node.cpp index 47440f8c60..e2df2860ea 100644 --- a/scene/main/node.cpp +++ b/scene/main/node.cpp @@ -2925,35 +2925,6 @@ String Node::_get_name_num_separator() { } Node::Node() { - data.pos = -1; - data.depth = -1; - data.blocked = 0; - data.parent = nullptr; - data.tree = nullptr; - data.physics_process = false; - data.idle_process = false; - data.process_priority = 0; - data.physics_process_internal = false; - data.idle_process_internal = false; - data.inside_tree = false; - data.ready_notified = false; - - data.owner = nullptr; - data.OW = nullptr; - data.input = false; - data.unhandled_input = false; - data.unhandled_key_input = false; - data.pause_mode = PAUSE_MODE_INHERIT; - data.pause_owner = nullptr; - data.network_master = 1; //server by default - data.path_cache = nullptr; - data.parent_owned = false; - data.in_constructor = true; - data.viewport = nullptr; - data.use_placeholder = false; - data.display_folded = false; - data.ready_first = true; - orphan_node_count++; } diff --git a/scene/main/node.h b/scene/main/node.h index 024d036fd3..5c178d401c 100644 --- a/scene/main/node.h +++ b/scene/main/node.h @@ -90,54 +90,54 @@ private: HashMap<NodePath, int> editable_instances; - Node *parent; - Node *owner; - Vector<Node *> children; // list of children - int pos; - int depth; - int blocked; // safeguard that throws an error when attempting to modify the tree in a harmful way while being traversed. + Node *parent = nullptr; + Node *owner = nullptr; + Vector<Node *> children; + int pos = -1; + int depth = -1; + int blocked = 0; // Safeguard that throws an error when attempting to modify the tree in a harmful way while being traversed. StringName name; - SceneTree *tree; - bool inside_tree; - bool ready_notified; //this is a small hack, so if a node is added during _ready() to the tree, it correctly gets the _ready() notification - bool ready_first; + SceneTree *tree = nullptr; + bool inside_tree = false; + bool ready_notified = false; // This is a small hack, so if a node is added during _ready() to the tree, it correctly gets the _ready() notification. + bool ready_first = true; #ifdef TOOLS_ENABLED - NodePath import_path; //path used when imported, used by scene editors to keep tracking + NodePath import_path; // Path used when imported, used by scene editors to keep tracking. #endif - Viewport *viewport; + Viewport *viewport = nullptr; Map<StringName, GroupData> grouped; - List<Node *>::Element *OW; // owned element + List<Node *>::Element *OW = nullptr; // Owned element. List<Node *> owned; - PauseMode pause_mode; - Node *pause_owner; + PauseMode pause_mode = PAUSE_MODE_INHERIT; + Node *pause_owner = nullptr; - int network_master; + int network_master = 1; // Server by default. Vector<NetData> rpc_methods; Vector<NetData> rpc_properties; - // variables used to properly sort the node when processing, ignored otherwise - //should move all the stuff below to bits - bool physics_process; - bool idle_process; - int process_priority; + // Variables used to properly sort the node when processing, ignored otherwise. + // TODO: Should move all the stuff below to bits. + bool physics_process = false; + bool idle_process = false; + int process_priority = 0; - bool physics_process_internal; - bool idle_process_internal; + bool physics_process_internal = false; + bool idle_process_internal = false; - bool input; - bool unhandled_input; - bool unhandled_key_input; + bool input = false; + bool unhandled_input = false; + bool unhandled_key_input = false; - bool parent_owned; - bool in_constructor; - bool use_placeholder; + bool parent_owned = false; + bool in_constructor = true; + bool use_placeholder = false; - bool display_folded; + bool display_folded = false; - mutable NodePath *path_cache; + mutable NodePath *path_cache = nullptr; } data; diff --git a/scene/main/scene_tree.cpp b/scene/main/scene_tree.cpp index 9e396d4030..5cf3cbd382 100644 --- a/scene/main/scene_tree.cpp +++ b/scene/main/scene_tree.cpp @@ -1332,14 +1332,6 @@ SceneTree::SceneTree() { if (singleton == nullptr) { singleton = this; } - _quit = false; - accept_quit = true; - quit_on_go_back = true; - initialized = false; -#ifdef DEBUG_ENABLED - debug_collisions_hint = false; - debug_navigation_hint = false; -#endif debug_collisions_color = GLOBAL_DEF("debug/shapes/collision/shape_color", Color(0.0, 0.6, 0.7, 0.5)); debug_collision_contact_color = GLOBAL_DEF("debug/shapes/collision/contact_color", Color(1.0, 0.2, 0.1, 0.8)); debug_navigation_color = GLOBAL_DEF("debug/shapes/navigation/geometry_color", Color(0.1, 1.0, 0.7, 0.4)); @@ -1347,23 +1339,7 @@ SceneTree::SceneTree() { collision_debug_contacts = GLOBAL_DEF("debug/shapes/collision/max_contacts_displayed", 10000); ProjectSettings::get_singleton()->set_custom_property_info("debug/shapes/collision/max_contacts_displayed", PropertyInfo(Variant::INT, "debug/shapes/collision/max_contacts_displayed", PROPERTY_HINT_RANGE, "0,20000,1")); // No negative - tree_version = 1; - physics_process_time = 1; - idle_process_time = 1; - - root = nullptr; - pause = false; - current_frame = 0; - tree_changed_name = "tree_changed"; - node_added_name = "node_added"; - node_removed_name = "node_removed"; - node_renamed_name = "node_renamed"; - ugc_locked = false; - call_lock = 0; - root_lock = 0; - node_count = 0; - - //create with mainloop + // Create with mainloop. root = memnew(Window); root->set_name("root"); @@ -1371,8 +1347,7 @@ SceneTree::SceneTree() { root->set_world_3d(Ref<World3D>(memnew(World3D))); } - // Initialize network state - multiplayer_poll = true; + // Initialize network state. set_multiplayer(Ref<MultiplayerAPI>(memnew(MultiplayerAPI))); //root->set_world_2d( Ref<World2D>( memnew( World2D ))); @@ -1391,6 +1366,10 @@ SceneTree::SceneTree() { const bool use_debanding = GLOBAL_DEF("rendering/quality/screen_filters/use_debanding", false); root->set_use_debanding(use_debanding); + float lod_threshold = GLOBAL_DEF("rendering/quality/mesh_lod/threshold_pixels", 1.0); + ProjectSettings::get_singleton()->set_custom_property_info("rendering/quality/mesh_lod/threshold_pixels", PropertyInfo(Variant::FLOAT, "rendering/quality/mesh_lod/threshold_pixels", PROPERTY_HINT_RANGE, "0,1024,0.1")); + root->set_lod_threshold(lod_threshold); + bool snap_2d_transforms = GLOBAL_DEF("rendering/quality/2d/snap_2d_transforms_to_pixel", false); root->set_snap_2d_transforms_to_pixel(snap_2d_transforms); @@ -1405,8 +1384,8 @@ SceneTree::SceneTree() { ProjectSettings::get_singleton()->set_custom_property_info("rendering/quality/2d_sdf/oversize", PropertyInfo(Variant::INT, "rendering/quality/2d_sdf/oversize", PROPERTY_HINT_ENUM, "100%,120%,150%,200%")); ProjectSettings::get_singleton()->set_custom_property_info("rendering/quality/2d_sdf/scale", PropertyInfo(Variant::INT, "rendering/quality/2d_sdf/scale", PROPERTY_HINT_ENUM, "100%,50%,25%")); - { //load default fallback environment - //get possible extensions + { // Load default fallback environment. + // Get possible extensions. List<String> exts; ResourceLoader::get_recognized_extensions_for_type("Environment", &exts); String ext_hint; @@ -1416,9 +1395,9 @@ SceneTree::SceneTree() { } ext_hint += "*." + E->get(); } - //get path + // Get path. String env_path = GLOBAL_DEF("rendering/environment/default_environment", ""); - //setup property + // Setup property. ProjectSettings::get_singleton()->set_custom_property_info("rendering/environment/default_environment", PropertyInfo(Variant::STRING, "rendering/viewport/default_environment", PROPERTY_HINT_FILE, ext_hint)); env_path = env_path.strip_edges(); if (env_path != String()) { @@ -1427,10 +1406,10 @@ SceneTree::SceneTree() { root->get_world_3d()->set_fallback_environment(env); } else { if (Engine::get_singleton()->is_editor_hint()) { - //file was erased, clear the field. + // File was erased, clear the field. ProjectSettings::get_singleton()->set("rendering/environment/default_environment", ""); } else { - //file was erased, notify user. + // File was erased, notify user. ERR_PRINT(RTR("Default Environment as specified in Project Settings (Rendering -> Environment -> Default Environment) could not be loaded.")); } } diff --git a/scene/main/scene_tree.h b/scene/main/scene_tree.h index 3e5802ce2e..9cf129d959 100644 --- a/scene/main/scene_tree.h +++ b/scene/main/scene_tree.h @@ -80,37 +80,36 @@ public: private: struct Group { Vector<Node *> nodes; - //uint64_t last_tree_version; bool changed; Group() { changed = false; }; }; - Window *root; + Window *root = nullptr; - uint64_t tree_version; - float physics_process_time; - float idle_process_time; - bool accept_quit; - bool quit_on_go_back; + uint64_t tree_version = 1; + float physics_process_time = 1.0; + float idle_process_time = 1.0; + bool accept_quit = true; + bool quit_on_go_back = true; #ifdef DEBUG_ENABLED - bool debug_collisions_hint; - bool debug_navigation_hint; + bool debug_collisions_hint = false; + bool debug_navigation_hint = false; #endif - bool pause; - int root_lock; + bool pause = false; + int root_lock = 0; Map<StringName, Group> group_map; - bool _quit; - bool initialized; + bool _quit = false; + bool initialized = false; - StringName tree_changed_name; - StringName node_added_name; - StringName node_removed_name; - StringName node_renamed_name; + StringName tree_changed_name = "tree_changed"; + StringName node_added_name = "node_added"; + StringName node_removed_name = "node_removed"; + StringName node_renamed_name = "node_renamed"; - int64_t current_frame; - int node_count; + int64_t current_frame = 0; + int node_count = 0; #ifdef TOOLS_ENABLED Node *edited_scene_root; @@ -122,14 +121,14 @@ private: bool operator<(const UGCall &p_with) const { return group == p_with.group ? call < p_with.call : group < p_with.group; } }; - //safety for when a node is deleted while a group is being called - int call_lock; - Set<Node *> call_skip; //skip erased nodes + // Safety for when a node is deleted while a group is being called. + int call_lock = 0; + Set<Node *> call_skip; // Skip erased nodes. List<ObjectID> delete_queue; Map<UGCall, Vector<Variant>> unique_group_calls; - bool ugc_locked; + bool ugc_locked = false; void _flush_ugc(); _FORCE_INLINE_ void _update_group_order(Group &g, bool p_use_priority = false); @@ -157,7 +156,7 @@ private: ///network/// Ref<MultiplayerAPI> multiplayer; - bool multiplayer_poll; + bool multiplayer_poll = true; void _network_peer_connected(int p_id); void _network_peer_disconnected(int p_id); @@ -183,7 +182,7 @@ private: Variant _call_group(const Variant **p_args, int p_argcount, Callable::CallError &r_error); void _flush_delete_queue(); - //optimization + // Optimization. friend class CanvasItem; friend class Node3D; friend class Viewport; diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp index 9f50b34e21..c96dd4ad35 100644 --- a/scene/main/viewport.cpp +++ b/scene/main/viewport.cpp @@ -3192,6 +3192,14 @@ bool Viewport::is_using_debanding() const { return use_debanding; } +void Viewport::set_lod_threshold(float p_pixels) { + lod_threshold = p_pixels; + RS::get_singleton()->viewport_set_lod_threshold(viewport, lod_threshold); +} +float Viewport::get_lod_threshold() const { + return lod_threshold; +} + void Viewport::set_debug_draw(DebugDraw p_debug_draw) { debug_draw = p_debug_draw; RS::get_singleton()->viewport_set_debug_draw(viewport, RS::ViewportDebugDraw(p_debug_draw)); @@ -3505,6 +3513,9 @@ void Viewport::_bind_methods() { ClassDB::bind_method(D_METHOD("set_sdf_scale", "scale"), &Viewport::set_sdf_scale); ClassDB::bind_method(D_METHOD("get_sdf_scale"), &Viewport::get_sdf_scale); + ClassDB::bind_method(D_METHOD("set_lod_threshold", "pixels"), &Viewport::set_lod_threshold); + ClassDB::bind_method(D_METHOD("get_lod_threshold"), &Viewport::get_lod_threshold); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "own_world_3d"), "set_use_own_world_3d", "is_using_own_world_3d"); ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "world_3d", PROPERTY_HINT_RESOURCE_TYPE, "World3D"), "set_world_3d", "get_world_3d"); ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "world_2d", PROPERTY_HINT_RESOURCE_TYPE, "World2D", 0), "set_world_2d", "get_world_2d"); @@ -3516,6 +3527,7 @@ void Viewport::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "msaa", PROPERTY_HINT_ENUM, "Disabled,2x,4x,8x,16x,AndroidVR 2x,AndroidVR 4x"), "set_msaa", "get_msaa"); ADD_PROPERTY(PropertyInfo(Variant::INT, "screen_space_aa", PROPERTY_HINT_ENUM, "Disabled,FXAA"), "set_screen_space_aa", "get_screen_space_aa"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_debanding"), "set_use_debanding", "is_using_debanding"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "lod_threshold", PROPERTY_HINT_RANGE, "0,1024,0.1"), "set_lod_threshold", "get_lod_threshold"); ADD_PROPERTY(PropertyInfo(Variant::INT, "debug_draw", PROPERTY_HINT_ENUM, "Disabled,Unshaded,Overdraw,Wireframe"), "set_debug_draw", "get_debug_draw"); ADD_GROUP("Canvas Items", "canvas_item_"); ADD_PROPERTY(PropertyInfo(Variant::INT, "canvas_item_default_texture_filter", PROPERTY_HINT_ENUM, "Nearest,Linear,MipmapLinear,MipmapNearest"), "set_default_canvas_item_texture_filter", "get_default_canvas_item_texture_filter"); @@ -3590,6 +3602,7 @@ void Viewport::_bind_methods() { BIND_ENUM_CONSTANT(DEBUG_DRAW_SDFGI); BIND_ENUM_CONSTANT(DEBUG_DRAW_SDFGI_PROBES); BIND_ENUM_CONSTANT(DEBUG_DRAW_GI_BUFFER); + BIND_ENUM_CONSTANT(DEBUG_DRAW_DISABLE_LOD); BIND_ENUM_CONSTANT(DEFAULT_CANVAS_ITEM_TEXTURE_FILTER_NEAREST); BIND_ENUM_CONSTANT(DEFAULT_CANVAS_ITEM_TEXTURE_FILTER_LINEAR); @@ -3652,6 +3665,8 @@ Viewport::Viewport() { set_shadow_atlas_quadrant_subdiv(2, SHADOW_ATLAS_QUADRANT_SUBDIV_16); set_shadow_atlas_quadrant_subdiv(3, SHADOW_ATLAS_QUADRANT_SUBDIV_64); + set_lod_threshold(lod_threshold); + String id = itos(get_instance_id()); input_group = "_vp_input" + id; gui_input_group = "_vp_gui_input" + id; diff --git a/scene/main/viewport.h b/scene/main/viewport.h index f08f255dde..ffbc3c782a 100644 --- a/scene/main/viewport.h +++ b/scene/main/viewport.h @@ -142,6 +142,7 @@ public: DEBUG_DRAW_SDFGI, DEBUG_DRAW_SDFGI_PROBES, DEBUG_DRAW_GI_BUFFER, + DEBUG_DRAW_DISABLE_LOD, }; enum DefaultCanvasItemTextureFilter { @@ -297,6 +298,8 @@ private: MSAA msaa; ScreenSpaceAA screen_space_aa; bool use_debanding = false; + float lod_threshold = 1.0; + Ref<ViewportTexture> default_texture; Set<ViewportTexture *> viewport_textures; @@ -542,6 +545,9 @@ public: void set_use_debanding(bool p_use_debanding); bool is_using_debanding() const; + void set_lod_threshold(float p_pixels); + float get_lod_threshold() const; + Vector2 get_camera_coords(const Vector2 &p_viewport_coords) const; Vector2 get_camera_rect_size() const; diff --git a/scene/resources/SCsub b/scene/resources/SCsub index 3a86b22835..f4dc7a46fb 100644 --- a/scene/resources/SCsub +++ b/scene/resources/SCsub @@ -2,6 +2,25 @@ Import("env") -env.add_source_files(env.scene_sources, "*.cpp") +# Thirdparty code + +thirdparty_obj = [] + +thirdparty_sources = "#thirdparty/misc/mikktspace.c" + +env_thirdparty = env.Clone() +env_thirdparty.disable_warnings() +env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) +env.scene_sources += thirdparty_obj + +# Godot source files + +scene_obj = [] + +env.add_source_files(scene_obj, "*.cpp") +env.scene_sources += scene_obj + +# Needed to force rebuilding the scene files when the thirdparty code is updated. +env.Depends(scene_obj, thirdparty_obj) SConscript("default_theme/SCsub") diff --git a/scene/resources/animation.cpp b/scene/resources/animation.cpp index a7a02361a9..f4670ca850 100644 --- a/scene/resources/animation.cpp +++ b/scene/resources/animation.cpp @@ -33,8 +33,6 @@ #include "core/math/geometry_3d.h" -#define ANIM_MIN_LENGTH 0.001 - bool Animation::_set(const StringName &p_name, const Variant &p_value) { String name = p_name; diff --git a/scene/resources/animation.h b/scene/resources/animation.h index 060d1fe2d9..650a54ebfc 100644 --- a/scene/resources/animation.h +++ b/scene/resources/animation.h @@ -33,6 +33,8 @@ #include "core/io/resource.h" +#define ANIM_MIN_LENGTH 0.001 + class Animation : public Resource { GDCLASS(Animation, Resource); RES_BASE_EXTENSION("anim"); diff --git a/scene/resources/bit_map.cpp b/scene/resources/bit_map.cpp index 10f0de8ff8..d07447179d 100644 --- a/scene/resources/bit_map.cpp +++ b/scene/resources/bit_map.cpp @@ -63,7 +63,7 @@ void BitMap::create_from_image_alpha(const Ref<Image> &p_image, float p_threshol } void BitMap::set_bit_rect(const Rect2 &p_rect, bool p_value) { - Rect2i current = Rect2i(0, 0, width, height).clip(p_rect); + Rect2i current = Rect2i(0, 0, width, height).intersection(p_rect); uint8_t *data = bitmask.ptrw(); for (int i = current.position.x; i < current.position.x + current.size.x; i++) { @@ -482,7 +482,7 @@ static void fill_bits(const BitMap *p_src, Ref<BitMap> &p_map, const Point2i &p_ } Vector<Vector<Vector2>> BitMap::clip_opaque_to_polygons(const Rect2 &p_rect, float p_epsilon) const { - Rect2i r = Rect2i(0, 0, width, height).clip(p_rect); + Rect2i r = Rect2i(0, 0, width, height).intersection(p_rect); print_verbose("BitMap: Rect: " + r); Point2i from; @@ -522,7 +522,7 @@ void BitMap::grow_mask(int p_pixels, const Rect2 &p_rect) { bool bit_value = p_pixels > 0; p_pixels = Math::abs(p_pixels); - Rect2i r = Rect2i(0, 0, width, height).clip(p_rect); + Rect2i r = Rect2i(0, 0, width, height).intersection(p_rect); Ref<BitMap> copy; copy.instance(); diff --git a/scene/resources/font.cpp b/scene/resources/font.cpp index fab8642c20..791f260c0e 100644 --- a/scene/resources/font.cpp +++ b/scene/resources/font.cpp @@ -969,7 +969,7 @@ RES ResourceFormatLoaderFont::load(const String &p_path, const String &p_origina void ResourceFormatLoaderFont::get_recognized_extensions_for_type(const String &p_type, List<String> *p_extensions) const { #ifndef DISABLE_DEPRECATED - if (p_type == "DynacmicFontData") { + if (p_type == "DynamicFontData") { p_extensions->push_back("ttf"); p_extensions->push_back("otf"); p_extensions->push_back("woff"); diff --git a/scene/resources/surface_tool.cpp b/scene/resources/surface_tool.cpp index 772b54bc53..50308d641a 100644 --- a/scene/resources/surface_tool.cpp +++ b/scene/resources/surface_tool.cpp @@ -1128,7 +1128,8 @@ Vector<int> SurfaceTool::generate_lod(float p_threshold, int p_target_index_coun vertices[i * 3 + 2] = vertex_array[i].vertex.z; } - uint32_t index_count = simplify_func((unsigned int *)lod.ptrw(), (unsigned int *)index_array.ptr(), index_array.size(), vertices.ptr(), vertex_array.size(), sizeof(float) * 3, p_target_index_count, p_threshold); + float error; + uint32_t index_count = simplify_func((unsigned int *)lod.ptrw(), (unsigned int *)index_array.ptr(), index_array.size(), vertices.ptr(), vertex_array.size(), sizeof(float) * 3, p_target_index_count, p_threshold, &error); ERR_FAIL_COND_V(index_count == 0, lod); lod.resize(index_count); diff --git a/scene/resources/surface_tool.h b/scene/resources/surface_tool.h index e80a5339a9..0e60bfe389 100644 --- a/scene/resources/surface_tool.h +++ b/scene/resources/surface_tool.h @@ -76,7 +76,7 @@ public: typedef void (*OptimizeVertexCacheFunc)(unsigned int *destination, const unsigned int *indices, size_t index_count, size_t vertex_count); static OptimizeVertexCacheFunc optimize_vertex_cache_func; - typedef size_t (*SimplifyFunc)(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); + typedef size_t (*SimplifyFunc)(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_error); static SimplifyFunc simplify_func; private: diff --git a/scene/resources/texture.cpp b/scene/resources/texture.cpp index 1507537cd0..706b18d2b5 100644 --- a/scene/resources/texture.cpp +++ b/scene/resources/texture.cpp @@ -1253,7 +1253,7 @@ bool AtlasTexture::get_rect_region(const Rect2 &p_rect, const Rect2 &p_src_rect, Vector2 scale = p_rect.size / src.size; src.position += (rc.position - margin.position); - Rect2 src_c = rc.clip(src); + Rect2 src_c = rc.intersection(src); if (src_c.size == Size2()) { return false; } @@ -1575,7 +1575,7 @@ void LargeTexture::draw_rect_region(RID p_canvas_item, const Rect2 &p_rect, cons if (!p_src_rect.intersects(rect)) { continue; } - Rect2 local = p_src_rect.clip(rect); + Rect2 local = p_src_rect.intersection(rect); Rect2 target = local; target.size *= scale; target.position = p_rect.position + (p_src_rect.position + rect.position) * scale; diff --git a/servers/camera/SCsub b/servers/camera/SCsub index c949f3bb25..86681f9c74 100644 --- a/servers/camera/SCsub +++ b/servers/camera/SCsub @@ -3,5 +3,3 @@ Import("env") env.add_source_files(env.servers_sources, "*.cpp") - -Export("env") diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp index 848138940e..d8f187a7f8 100644 --- a/servers/physics_3d/body_pair_3d_sw.cpp +++ b/servers/physics_3d/body_pair_3d_sw.cpp @@ -367,8 +367,8 @@ void BodyPair3DSW::solve(real_t p_step) { Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); - A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step); - B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb, MAX_BIAS_ROTATION / p_step); + 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); crbA = A->get_biased_angular_velocity().cross(c.rA); crbB = B->get_biased_angular_velocity().cross(c.rB); @@ -383,8 +383,8 @@ 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(A->get_center_of_mass(), -jb_com, 0.0f); - B->apply_bias_impulse(B->get_center_of_mass(), jb_com, 0.0f); + 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); } c.active = true; diff --git a/servers/rendering/renderer_canvas_cull.cpp b/servers/rendering/renderer_canvas_cull.cpp index a397ba4389..53deba512e 100644 --- a/servers/rendering/renderer_canvas_cull.cpp +++ b/servers/rendering/renderer_canvas_cull.cpp @@ -144,7 +144,7 @@ void RendererCanvasCull::_cull_canvas_item(Item *p_canvas_item, const Transform2 if (ci->clip) { if (p_canvas_clip != nullptr) { - ci->final_clip_rect = p_canvas_clip->final_clip_rect.clip(global_rect); + ci->final_clip_rect = p_canvas_clip->final_clip_rect.intersection(global_rect); } else { ci->final_clip_rect = global_rect; } @@ -195,7 +195,7 @@ void RendererCanvasCull::_cull_canvas_item(Item *p_canvas_item, const Transform2 } if (ci->copy_back_buffer) { - ci->copy_back_buffer->screen_rect = xform.xform(ci->copy_back_buffer->rect).clip(p_clip_rect); + ci->copy_back_buffer->screen_rect = xform.xform(ci->copy_back_buffer->rect).intersection(p_clip_rect); } if (use_canvas_group) { diff --git a/servers/rendering/renderer_rd/renderer_scene_render_forward.cpp b/servers/rendering/renderer_rd/renderer_scene_render_forward.cpp index 9de7af3c22..93f4b7bc8e 100644 --- a/servers/rendering/renderer_rd/renderer_scene_render_forward.cpp +++ b/servers/rendering/renderer_rd/renderer_scene_render_forward.cpp @@ -953,7 +953,7 @@ void RendererSceneRenderForward::_fill_instances(RenderList::Element **p_element /// RENDERING /// -void RendererSceneRenderForward::_render_list(RenderingDevice::DrawListID p_draw_list, RenderingDevice::FramebufferFormatID p_framebuffer_Format, RenderList::Element **p_elements, int p_element_count, bool p_reverse_cull, PassMode p_pass_mode, bool p_no_gi, RID p_render_pass_uniform_set, bool p_force_wireframe, const Vector2 &p_uv_offset) { +void RendererSceneRenderForward::_render_list(RenderingDevice::DrawListID p_draw_list, RenderingDevice::FramebufferFormatID p_framebuffer_Format, RenderList::Element **p_elements, int p_element_count, bool p_reverse_cull, PassMode p_pass_mode, bool p_no_gi, RID p_render_pass_uniform_set, bool p_force_wireframe, const Vector2 &p_uv_offset, const Plane &p_lod_plane, float p_lod_distance_multiplier, float p_screen_lod_threshold) { RD::DrawListID draw_list = p_draw_list; RD::FramebufferFormatID framebuffer_format = p_framebuffer_Format; @@ -996,10 +996,13 @@ void RendererSceneRenderForward::_render_list(RenderingDevice::DrawListID p_draw //find primitive and vertex format RS::PrimitiveType primitive; + void *mesh_surface = nullptr; switch (e->instance->base_type) { case RS::INSTANCE_MESH: { - primitive = storage->mesh_surface_get_primitive(e->instance->base, e->surface_index); + mesh_surface = storage->mesh_get_surface(e->instance->base, e->surface_index); + + primitive = storage->mesh_surface_get_primitive(mesh_surface); if (e->instance->skeleton.is_valid()) { xforms_uniform_set = storage->skeleton_get_3d_uniform_set(e->instance->skeleton, default_shader_rd, TRANSFORMS_UNIFORM_SET); } @@ -1007,7 +1010,10 @@ void RendererSceneRenderForward::_render_list(RenderingDevice::DrawListID p_draw case RS::INSTANCE_MULTIMESH: { RID mesh = storage->multimesh_get_mesh(e->instance->base); ERR_CONTINUE(!mesh.is_valid()); //should be a bug - primitive = storage->mesh_surface_get_primitive(mesh, e->surface_index); + + mesh_surface = storage->mesh_get_surface(e->instance->base, e->surface_index); + + primitive = storage->mesh_surface_get_primitive(mesh_surface); xforms_uniform_set = storage->multimesh_get_3d_uniform_set(e->instance->base, default_shader_rd, TRANSFORMS_UNIFORM_SET); @@ -1018,7 +1024,10 @@ void RendererSceneRenderForward::_render_list(RenderingDevice::DrawListID p_draw case RS::INSTANCE_PARTICLES: { RID mesh = storage->particles_get_draw_pass_mesh(e->instance->base, e->surface_index >> 16); ERR_CONTINUE(!mesh.is_valid()); //should be a bug - primitive = storage->mesh_surface_get_primitive(mesh, e->surface_index & 0xFFFF); + + mesh_surface = storage->mesh_get_surface(e->instance->base, e->surface_index & 0xFFFF); + + primitive = storage->mesh_surface_get_primitive(mesh_surface); xforms_uniform_set = storage->particles_get_instance_buffer_uniform_set(e->instance->base, default_shader_rd, TRANSFORMS_UNIFORM_SET); @@ -1077,29 +1086,39 @@ void RendererSceneRenderForward::_render_list(RenderingDevice::DrawListID p_draw RID vertex_array_rd; RID index_array_rd; - switch (e->instance->base_type) { - case RS::INSTANCE_MESH: { - if (e->instance->mesh_instance.is_valid()) { //skeleton and blend shape - storage->mesh_instance_surface_get_arrays_and_format(e->instance->mesh_instance, e->surface_index, pipeline->get_vertex_input_mask(), vertex_array_rd, index_array_rd, vertex_format); - } else { - storage->mesh_surface_get_arrays_and_format(e->instance->base, e->surface_index, pipeline->get_vertex_input_mask(), vertex_array_rd, index_array_rd, vertex_format); + if (mesh_surface) { + if (e->instance->mesh_instance.is_valid()) { //skeleton and blend shape + storage->mesh_instance_surface_get_vertex_arrays_and_format(e->instance->mesh_instance, e->surface_index, pipeline->get_vertex_input_mask(), vertex_array_rd, vertex_format); + } else { + storage->mesh_surface_get_vertex_arrays_and_format(mesh_surface, pipeline->get_vertex_input_mask(), vertex_array_rd, vertex_format); + } + + if (p_screen_lod_threshold > 0.0 && storage->mesh_surface_has_lod(mesh_surface)) { + Vector3 support_min = e->instance->transformed_aabb.get_support(-p_lod_plane.normal); + Vector3 support_max = e->instance->transformed_aabb.get_support(p_lod_plane.normal); + + float distance_min = p_lod_plane.distance_to(support_min); + float distance_max = p_lod_plane.distance_to(support_max); + + float distance = 0.0; + + if (distance_min * distance_max < 0.0) { + //crossing plane + distance = 0.0; + } else if (distance_min >= 0.0) { + distance = distance_min; + } else if (distance_max <= 0.0) { + distance = -distance_max; } - } break; - case RS::INSTANCE_MULTIMESH: { - RID mesh = storage->multimesh_get_mesh(e->instance->base); - ERR_CONTINUE(!mesh.is_valid()); //should be a bug - storage->mesh_surface_get_arrays_and_format(mesh, e->surface_index, pipeline->get_vertex_input_mask(), vertex_array_rd, index_array_rd, vertex_format); - } break; - case RS::INSTANCE_IMMEDIATE: { - ERR_CONTINUE(true); //should be a bug - } break; - case RS::INSTANCE_PARTICLES: { - RID mesh = storage->particles_get_draw_pass_mesh(e->instance->base, e->surface_index >> 16); - ERR_CONTINUE(!mesh.is_valid()); //should be a bug - storage->mesh_surface_get_arrays_and_format(mesh, e->surface_index & 0xFFFF, pipeline->get_vertex_input_mask(), vertex_array_rd, index_array_rd, vertex_format); - } break; - default: { - ERR_CONTINUE(true); //should be a bug + + Vector3 model_scale_vec = e->instance->transform.basis.get_scale_abs(); + + float model_scale = MAX(model_scale_vec.x, MAX(model_scale_vec.y, model_scale_vec.z)); + + index_array_rd = storage->mesh_surface_get_index_array_with_lod(mesh_surface, model_scale * e->instance->lod_bias, distance * p_lod_distance_multiplier, p_screen_lod_threshold); + + } else { + index_array_rd = storage->mesh_surface_get_index_array(mesh_surface); } } @@ -1635,7 +1654,7 @@ void RendererSceneRenderForward::_setup_lightmaps(InstanceBase **p_lightmap_cull } } -void RendererSceneRenderForward::_render_scene(RID p_render_buffer, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, int p_directional_light_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, const Color &p_default_bg_color) { +void RendererSceneRenderForward::_render_scene(RID p_render_buffer, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, int p_directional_light_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, const Color &p_default_bg_color, float p_screen_lod_threshold) { RenderBufferDataForward *render_buffer = nullptr; if (p_render_buffer.is_valid()) { render_buffer = (RenderBufferDataForward *)render_buffers_get_data(p_render_buffer); @@ -1654,6 +1673,13 @@ void RendererSceneRenderForward::_render_scene(RID p_render_buffer, const Transf scene_state.ubo.reflection_multiplier = 1.0; } + float lod_distance_multiplier = p_cam_projection.get_lod_multiplier(); + Plane lod_camera_plane(p_cam_transform.get_origin(), -p_cam_transform.basis.get_axis(Vector3::AXIS_Z)); + + if (get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_DISABLE_LOD) { + p_screen_lod_threshold = 0.0; + } + //scene_state.ubo.subsurface_scatter_width = subsurface_scatter_size; Vector2 vp_he = p_cam_projection.get_viewport_half_extents(); @@ -1856,7 +1882,7 @@ void RendererSceneRenderForward::_render_scene(RID p_render_buffer, const Transf bool finish_depth = using_ssao || using_sdfgi || using_giprobe; RD::DrawListID draw_list = RD::get_singleton()->draw_list_begin(depth_framebuffer, RD::INITIAL_ACTION_CLEAR, RD::FINAL_ACTION_READ, RD::INITIAL_ACTION_CLEAR, finish_depth ? RD::FINAL_ACTION_READ : RD::FINAL_ACTION_CONTINUE, depth_pass_clear); - _render_list(draw_list, RD::get_singleton()->framebuffer_get_format(depth_framebuffer), render_list.elements, render_list.element_count, false, depth_pass_mode, render_buffer == nullptr, rp_uniform_set, get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_WIREFRAME); + _render_list(draw_list, RD::get_singleton()->framebuffer_get_format(depth_framebuffer), render_list.elements, render_list.element_count, false, depth_pass_mode, render_buffer == nullptr, rp_uniform_set, get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_WIREFRAME, Vector2(), lod_camera_plane, lod_distance_multiplier, p_screen_lod_threshold); RD::get_singleton()->draw_list_end(); if (render_buffer && render_buffer->msaa != RS::VIEWPORT_MSAA_DISABLED) { @@ -1904,7 +1930,7 @@ void RendererSceneRenderForward::_render_scene(RID p_render_buffer, const Transf RID framebuffer = using_separate_specular ? opaque_specular_framebuffer : opaque_framebuffer; RD::DrawListID draw_list = RD::get_singleton()->draw_list_begin(framebuffer, keep_color ? RD::INITIAL_ACTION_KEEP : RD::INITIAL_ACTION_CLEAR, will_continue_color ? RD::FINAL_ACTION_CONTINUE : RD::FINAL_ACTION_READ, depth_pre_pass ? (continue_depth ? RD::INITIAL_ACTION_KEEP : RD::INITIAL_ACTION_CONTINUE) : RD::INITIAL_ACTION_CLEAR, will_continue_depth ? RD::FINAL_ACTION_CONTINUE : RD::FINAL_ACTION_READ, c, 1.0, 0); - _render_list(draw_list, RD::get_singleton()->framebuffer_get_format(framebuffer), render_list.elements, render_list.element_count, false, using_separate_specular ? PASS_MODE_COLOR_SPECULAR : PASS_MODE_COLOR, render_buffer == nullptr, rp_uniform_set, get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_WIREFRAME); + _render_list(draw_list, RD::get_singleton()->framebuffer_get_format(framebuffer), render_list.elements, render_list.element_count, false, using_separate_specular ? PASS_MODE_COLOR_SPECULAR : PASS_MODE_COLOR, render_buffer == nullptr, rp_uniform_set, get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_WIREFRAME, Vector2(), lod_camera_plane, lod_distance_multiplier, p_screen_lod_threshold); RD::get_singleton()->draw_list_end(); if (will_continue_color && using_separate_specular) { @@ -1992,7 +2018,7 @@ void RendererSceneRenderForward::_render_scene(RID p_render_buffer, const Transf { RD::DrawListID draw_list = RD::get_singleton()->draw_list_begin(alpha_framebuffer, can_continue_color ? RD::INITIAL_ACTION_CONTINUE : RD::INITIAL_ACTION_KEEP, RD::FINAL_ACTION_READ, can_continue_depth ? RD::INITIAL_ACTION_CONTINUE : RD::INITIAL_ACTION_KEEP, RD::FINAL_ACTION_READ); - _render_list(draw_list, RD::get_singleton()->framebuffer_get_format(alpha_framebuffer), &render_list.elements[render_list.max_elements - render_list.alpha_element_count], render_list.alpha_element_count, false, PASS_MODE_COLOR, render_buffer == nullptr, rp_uniform_set, get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_WIREFRAME); + _render_list(draw_list, RD::get_singleton()->framebuffer_get_format(alpha_framebuffer), &render_list.elements[render_list.max_elements - render_list.alpha_element_count], render_list.alpha_element_count, false, PASS_MODE_COLOR, render_buffer == nullptr, rp_uniform_set, get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_WIREFRAME, Vector2(), lod_camera_plane, lod_distance_multiplier, p_screen_lod_threshold); RD::get_singleton()->draw_list_end(); } @@ -2001,7 +2027,7 @@ void RendererSceneRenderForward::_render_scene(RID p_render_buffer, const Transf } } -void RendererSceneRenderForward::_render_shadow(RID p_framebuffer, InstanceBase **p_cull_result, int p_cull_count, const CameraMatrix &p_projection, const Transform &p_transform, float p_zfar, float p_bias, float p_normal_bias, bool p_use_dp, bool p_use_dp_flip, bool p_use_pancake) { +void RendererSceneRenderForward::_render_shadow(RID p_framebuffer, InstanceBase **p_cull_result, int p_cull_count, const CameraMatrix &p_projection, const Transform &p_transform, float p_zfar, float p_bias, float p_normal_bias, bool p_use_dp, bool p_use_dp_flip, bool p_use_pancake, const Plane &p_camera_plane, float p_lod_distance_multiplier, float p_screen_lod_threshold) { RENDER_TIMESTAMP("Setup Rendering Shadow"); _update_render_base_uniform_set(); @@ -2012,6 +2038,10 @@ void RendererSceneRenderForward::_render_shadow(RID p_framebuffer, InstanceBase _setup_environment(RID(), RID(), p_projection, p_transform, RID(), true, Vector2(1, 1), RID(), true, Color(), 0, p_zfar, false, p_use_pancake); + if (get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_DISABLE_LOD) { + p_screen_lod_threshold = 0.0; + } + render_list.clear(); PassMode pass_mode = p_use_dp ? PASS_MODE_SHADOW_DP : PASS_MODE_SHADOW; @@ -2029,7 +2059,7 @@ void RendererSceneRenderForward::_render_shadow(RID p_framebuffer, InstanceBase { //regular forward for now RD::DrawListID draw_list = RD::get_singleton()->draw_list_begin(p_framebuffer, RD::INITIAL_ACTION_CLEAR, RD::FINAL_ACTION_READ, RD::INITIAL_ACTION_CLEAR, RD::FINAL_ACTION_READ); - _render_list(draw_list, RD::get_singleton()->framebuffer_get_format(p_framebuffer), render_list.elements, render_list.element_count, p_use_dp_flip, pass_mode, true, rp_uniform_set); + _render_list(draw_list, RD::get_singleton()->framebuffer_get_format(p_framebuffer), render_list.elements, render_list.element_count, p_use_dp_flip, pass_mode, true, rp_uniform_set, false, Vector2(), p_camera_plane, p_lod_distance_multiplier, p_screen_lod_threshold); RD::get_singleton()->draw_list_end(); } } @@ -2504,7 +2534,7 @@ RID RendererSceneRenderForward::_setup_render_pass_uniform_set(RID p_render_buff RD::Uniform u; u.binding = 4; u.uniform_type = RD::UNIFORM_TYPE_TEXTURE; - RID texture = rb && rb->depth.is_valid() ? rb->depth : storage->texture_rd_get_default(RendererStorageRD::DEFAULT_RD_TEXTURE_WHITE); + RID texture = (false && rb && rb->depth.is_valid()) ? rb->depth : storage->texture_rd_get_default(RendererStorageRD::DEFAULT_RD_TEXTURE_WHITE); u.ids.push_back(texture); uniforms.push_back(u); } @@ -2688,7 +2718,7 @@ RID RendererSceneRenderForward::_setup_sdfgi_render_pass_uniform_set(RID p_albed uniforms.push_back(u); } - sdfgi_pass_uniform_set = RD::get_singleton()->uniform_set_create(uniforms, default_shader_rd, RENDER_PASS_UNIFORM_SET); + sdfgi_pass_uniform_set = RD::get_singleton()->uniform_set_create(uniforms, default_shader_sdfgi_rd, RENDER_PASS_UNIFORM_SET); return sdfgi_pass_uniform_set; } @@ -2879,6 +2909,7 @@ RendererSceneRenderForward::RendererSceneRenderForward(RendererStorageRD *p_stor actions.renames["DIFFUSE_LIGHT"] = "diffuse_light"; actions.renames["SPECULAR_LIGHT"] = "specular_light"; + actions.usage_defines["NORMAL"] = "#define NORMAL_USED\n"; actions.usage_defines["TANGENT"] = "#define TANGENT_USED\n"; actions.usage_defines["BINORMAL"] = "@TANGENT"; actions.usage_defines["RIM"] = "#define LIGHT_RIM_USED\n"; diff --git a/servers/rendering/renderer_rd/renderer_scene_render_forward.h b/servers/rendering/renderer_rd/renderer_scene_render_forward.h index 6d76d5f0eb..da4cb579c1 100644 --- a/servers/rendering/renderer_rd/renderer_scene_render_forward.h +++ b/servers/rendering/renderer_rd/renderer_scene_render_forward.h @@ -570,7 +570,7 @@ class RendererSceneRenderForward : public RendererSceneRenderRD { void _setup_lightmaps(InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, const Transform &p_cam_transform); void _fill_instances(RenderList::Element **p_elements, int p_element_count, bool p_for_depth, bool p_has_sdfgi = false, bool p_has_opaque_gi = false); - void _render_list(RenderingDevice::DrawListID p_draw_list, RenderingDevice::FramebufferFormatID p_framebuffer_Format, RenderList::Element **p_elements, int p_element_count, bool p_reverse_cull, PassMode p_pass_mode, bool p_no_gi, RID p_render_pass_uniform_set, bool p_force_wireframe = false, const Vector2 &p_uv_offset = Vector2()); + void _render_list(RenderingDevice::DrawListID p_draw_list, RenderingDevice::FramebufferFormatID p_framebuffer_Format, RenderList::Element **p_elements, int p_element_count, bool p_reverse_cull, PassMode p_pass_mode, bool p_no_gi, RID p_render_pass_uniform_set, bool p_force_wireframe = false, const Vector2 &p_uv_offset = Vector2(), const Plane &p_lod_plane = Plane(), float p_lod_distance_multiplier = 0.0, float p_screen_lod_threshold = 0.0); _FORCE_INLINE_ void _add_geometry(InstanceBase *p_instance, uint32_t p_surface, RID p_material, PassMode p_pass_mode, uint32_t p_geometry_index, bool p_using_sdfgi = false); _FORCE_INLINE_ void _add_geometry_with_material(InstanceBase *p_instance, uint32_t p_surface, MaterialData *p_material, RID p_material_rid, PassMode p_pass_mode, uint32_t p_geometry_index, bool p_using_sdfgi = false); @@ -581,8 +581,8 @@ class RendererSceneRenderForward : public RendererSceneRenderRD { bool low_end = false; protected: - virtual void _render_scene(RID p_render_buffer, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, int p_directional_light_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, const Color &p_default_bg_color); - virtual void _render_shadow(RID p_framebuffer, InstanceBase **p_cull_result, int p_cull_count, const CameraMatrix &p_projection, const Transform &p_transform, float p_zfar, float p_bias, float p_normal_bias, bool p_use_dp, bool p_use_dp_flip, bool p_use_pancake); + virtual void _render_scene(RID p_render_buffer, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, int p_directional_light_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, const Color &p_default_bg_color, float p_lod_threshold); + virtual void _render_shadow(RID p_framebuffer, InstanceBase **p_cull_result, int p_cull_count, const CameraMatrix &p_projection, const Transform &p_transform, float p_zfar, float p_bias, float p_normal_bias, bool p_use_dp, bool p_use_dp_flip, bool p_use_pancake, const Plane &p_camera_plane = Plane(), float p_lod_distance_multiplier = 0.0, float p_screen_lod_threshold = 0.0); virtual void _render_material(const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID p_framebuffer, const Rect2i &p_region); virtual void _render_uv2(InstanceBase **p_cull_result, int p_cull_count, RID p_framebuffer, const Rect2i &p_region); virtual void _render_sdfgi(RID p_render_buffers, const Vector3i &p_from, const Vector3i &p_size, const AABB &p_bounds, InstanceBase **p_cull_result, int p_cull_count, const RID &p_albedo_texture, const RID &p_emission_texture, const RID &p_emission_aniso_texture, const RID &p_geom_facing_texture); diff --git a/servers/rendering/renderer_rd/renderer_scene_render_rd.cpp b/servers/rendering/renderer_rd/renderer_scene_render_rd.cpp index f880eb7d8a..4543ced8dc 100644 --- a/servers/rendering/renderer_rd/renderer_scene_render_rd.cpp +++ b/servers/rendering/renderer_rd/renderer_scene_render_rd.cpp @@ -3255,6 +3255,13 @@ void RendererSceneRenderRD::reflection_atlas_set_size(RID p_ref_atlas, int p_ref } } +int RendererSceneRenderRD::reflection_atlas_get_size(RID p_ref_atlas) const { + ReflectionAtlas *ra = reflection_atlas_owner.getornull(p_ref_atlas); + ERR_FAIL_COND_V(!ra, 0); + + return ra->size; +} + //////////////////////// RID RendererSceneRenderRD::reflection_probe_instance_create(RID p_probe) { ReflectionProbeInstance rpi; @@ -7012,7 +7019,7 @@ void RendererSceneRenderRD::_update_volumetric_fog(RID p_render_buffers, RID p_e RD::get_singleton()->compute_list_end(); } -void RendererSceneRenderRD::render_scene(RID p_render_buffers, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID *p_light_cull_result, int p_light_cull_count, RID *p_reflection_probe_cull_result, int p_reflection_probe_cull_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, RID *p_decal_cull_result, int p_decal_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass) { +void RendererSceneRenderRD::render_scene(RID p_render_buffers, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID *p_light_cull_result, int p_light_cull_count, RID *p_reflection_probe_cull_result, int p_reflection_probe_cull_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, RID *p_decal_cull_result, int p_decal_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, float p_screen_lod_threshold) { Color clear_color; if (p_render_buffers.is_valid()) { RenderBuffers *rb = render_buffers_owner.getornull(p_render_buffers); @@ -7069,7 +7076,7 @@ void RendererSceneRenderRD::render_scene(RID p_render_buffers, const Transform & _update_volumetric_fog(p_render_buffers, p_environment, p_cam_projection, p_cam_transform, p_shadow_atlas, directional_light_count, directional_shadows, positional_light_count, gi_probe_count); } - _render_scene(p_render_buffers, p_cam_transform, p_cam_projection, p_cam_ortogonal, p_cull_result, p_cull_count, directional_light_count, p_gi_probe_cull_result, p_gi_probe_cull_count, p_lightmap_cull_result, p_lightmap_cull_count, p_environment, p_camera_effects, p_shadow_atlas, p_reflection_atlas, p_reflection_probe, p_reflection_probe_pass, clear_color); + _render_scene(p_render_buffers, p_cam_transform, p_cam_projection, p_cam_ortogonal, p_cull_result, p_cull_count, directional_light_count, p_gi_probe_cull_result, p_gi_probe_cull_count, p_lightmap_cull_result, p_lightmap_cull_count, p_environment, p_camera_effects, p_shadow_atlas, p_reflection_atlas, p_reflection_probe, p_reflection_probe_pass, clear_color, p_screen_lod_threshold); if (p_render_buffers.is_valid()) { RENDER_TIMESTAMP("Tonemap"); @@ -7082,7 +7089,7 @@ void RendererSceneRenderRD::render_scene(RID p_render_buffers, const Transform & } } -void RendererSceneRenderRD::render_shadow(RID p_light, RID p_shadow_atlas, int p_pass, InstanceBase **p_cull_result, int p_cull_count) { +void RendererSceneRenderRD::render_shadow(RID p_light, RID p_shadow_atlas, int p_pass, InstanceBase **p_cull_result, int p_cull_count, const Plane &p_camera_plane, float p_lod_distance_multiplier, float p_screen_lod_threshold) { LightInstance *light_instance = light_instance_owner.getornull(p_light); ERR_FAIL_COND(!light_instance); @@ -7233,7 +7240,7 @@ void RendererSceneRenderRD::render_shadow(RID p_light, RID p_shadow_atlas, int p if (render_cubemap) { //rendering to cubemap - _render_shadow(render_fb, p_cull_result, p_cull_count, light_projection, light_transform, zfar, 0, 0, false, false, use_pancake); + _render_shadow(render_fb, p_cull_result, p_cull_count, light_projection, light_transform, zfar, 0, 0, false, false, use_pancake, p_camera_plane, p_lod_distance_multiplier, p_screen_lod_threshold); if (finalize_cubemap) { //reblit atlas_rect.size.height /= 2; @@ -7244,7 +7251,7 @@ void RendererSceneRenderRD::render_shadow(RID p_light, RID p_shadow_atlas, int p } else { //render shadow - _render_shadow(render_fb, p_cull_result, p_cull_count, light_projection, light_transform, zfar, bias, normal_bias, using_dual_paraboloid, using_dual_paraboloid_flip, use_pancake); + _render_shadow(render_fb, p_cull_result, p_cull_count, light_projection, light_transform, zfar, bias, normal_bias, using_dual_paraboloid, using_dual_paraboloid_flip, use_pancake, p_camera_plane, p_lod_distance_multiplier, p_screen_lod_threshold); //copy to atlas if (use_linear_depth) { diff --git a/servers/rendering/renderer_rd/renderer_scene_render_rd.h b/servers/rendering/renderer_rd/renderer_scene_render_rd.h index e3dfee2da7..3afe6e3f4a 100644 --- a/servers/rendering/renderer_rd/renderer_scene_render_rd.h +++ b/servers/rendering/renderer_rd/renderer_scene_render_rd.h @@ -109,8 +109,8 @@ protected: void _setup_reflections(RID *p_reflection_probe_cull_result, int p_reflection_probe_cull_count, const Transform &p_camera_inverse_transform, RID p_environment); void _setup_giprobes(RID p_render_buffers, const Transform &p_transform, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, uint32_t &r_gi_probes_used); - virtual void _render_scene(RID p_render_buffer, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, int p_directional_light_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, const Color &p_default_color) = 0; - virtual void _render_shadow(RID p_framebuffer, InstanceBase **p_cull_result, int p_cull_count, const CameraMatrix &p_projection, const Transform &p_transform, float p_zfar, float p_bias, float p_normal_bias, bool p_use_dp, bool use_dp_flip, bool p_use_pancake) = 0; + virtual void _render_scene(RID p_render_buffer, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, int p_directional_light_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, const Color &p_default_color, float p_screen_lod_threshold) = 0; + virtual void _render_shadow(RID p_framebuffer, InstanceBase **p_cull_result, int p_cull_count, const CameraMatrix &p_projection, const Transform &p_transform, float p_zfar, float p_bias, float p_normal_bias, bool p_use_dp, bool use_dp_flip, bool p_use_pancake, const Plane &p_camera_plane = Plane(), float p_lod_distance_multiplier = 0.0, float p_screen_lod_threshold = 0.0) = 0; virtual void _render_material(const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID p_framebuffer, const Rect2i &p_region) = 0; virtual void _render_uv2(InstanceBase **p_cull_result, int p_cull_count, RID p_framebuffer, const Rect2i &p_region) = 0; virtual void _render_sdfgi(RID p_render_buffers, const Vector3i &p_from, const Vector3i &p_size, const AABB &p_bounds, InstanceBase **p_cull_result, int p_cull_count, const RID &p_albedo_texture, const RID &p_emission_texture, const RID &p_emission_aniso_texture, const RID &p_geom_facing_texture) = 0; @@ -339,7 +339,7 @@ private: Vector<Reflection> reflections; }; - RID_Owner<ReflectionAtlas> reflection_atlas_owner; + mutable RID_Owner<ReflectionAtlas> reflection_atlas_owner; /* REFLECTION PROBE INSTANCE */ @@ -1728,6 +1728,8 @@ public: virtual RID reflection_atlas_create(); virtual void reflection_atlas_set_size(RID p_ref_atlas, int p_reflection_size, int p_reflection_count); + virtual int reflection_atlas_get_size(RID p_ref_atlas) const; + _FORCE_INLINE_ RID reflection_atlas_get_texture(RID p_ref_atlas) { ReflectionAtlas *atlas = reflection_atlas_owner.getornull(p_ref_atlas); ERR_FAIL_COND_V(!atlas, RID()); @@ -1884,9 +1886,9 @@ public: float render_buffers_get_volumetric_fog_end(RID p_render_buffers); float render_buffers_get_volumetric_fog_detail_spread(RID p_render_buffers); - void render_scene(RID p_render_buffers, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID *p_light_cull_result, int p_light_cull_count, RID *p_reflection_probe_cull_result, int p_reflection_probe_cull_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, RID *p_decal_cull_result, int p_decal_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_shadow_atlas, RID p_camera_effects, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass); + void render_scene(RID p_render_buffers, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID *p_light_cull_result, int p_light_cull_count, RID *p_reflection_probe_cull_result, int p_reflection_probe_cull_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, RID *p_decal_cull_result, int p_decal_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_shadow_atlas, RID p_camera_effects, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, float p_screen_lod_threshold); - void render_shadow(RID p_light, RID p_shadow_atlas, int p_pass, InstanceBase **p_cull_result, int p_cull_count); + void render_shadow(RID p_light, RID p_shadow_atlas, int p_pass, InstanceBase **p_cull_result, int p_cull_count, const Plane &p_camera_plane = Plane(), float p_lod_distance_multiplier = 0, float p_screen_lod_threshold = 0.0); void render_material(const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID p_framebuffer, const Rect2i &p_region); diff --git a/servers/rendering/renderer_rd/renderer_storage_rd.cpp b/servers/rendering/renderer_rd/renderer_storage_rd.cpp index 286281c83d..60c0bd1603 100644 --- a/servers/rendering/renderer_rd/renderer_storage_rd.cpp +++ b/servers/rendering/renderer_rd/renderer_storage_rd.cpp @@ -5645,6 +5645,15 @@ void RendererStorageRD::reflection_probe_set_resolution(RID p_probe, int p_resol reflection_probe->resolution = p_resolution; } +void RendererStorageRD::reflection_probe_set_lod_threshold(RID p_probe, float p_ratio) { + ReflectionProbe *reflection_probe = reflection_probe_owner.getornull(p_probe); + ERR_FAIL_COND(!reflection_probe); + + reflection_probe->lod_threshold = p_ratio; + + reflection_probe->instance_dependency.instance_notify_changed(true, false); +} + AABB RendererStorageRD::reflection_probe_get_aabb(RID p_probe) const { const ReflectionProbe *reflection_probe = reflection_probe_owner.getornull(p_probe); ERR_FAIL_COND_V(!reflection_probe, AABB()); @@ -5698,6 +5707,13 @@ float RendererStorageRD::reflection_probe_get_origin_max_distance(RID p_probe) c return reflection_probe->max_distance; } +float RendererStorageRD::reflection_probe_get_lod_threshold(RID p_probe) const { + const ReflectionProbe *reflection_probe = reflection_probe_owner.getornull(p_probe); + ERR_FAIL_COND_V(!reflection_probe, 0); + + return reflection_probe->lod_threshold; +} + int RendererStorageRD::reflection_probe_get_resolution(RID p_probe) const { const ReflectionProbe *reflection_probe = reflection_probe_owner.getornull(p_probe); ERR_FAIL_COND_V(!reflection_probe, 0); @@ -6933,7 +6949,7 @@ void RendererStorageRD::render_target_copy_to_back_buffer(RID p_render_target, c if (p_region == Rect2i()) { region.size = rt->size; } else { - region = Rect2i(Size2i(), rt->size).clip(p_region); + region = Rect2i(Size2i(), rt->size).intersection(p_region); if (region.size == Size2i()) { return; //nothing to do } @@ -6973,7 +6989,7 @@ void RendererStorageRD::render_target_clear_back_buffer(RID p_render_target, con if (p_region == Rect2i()) { region.size = rt->size; } else { - region = Rect2i(Size2i(), rt->size).clip(p_region); + region = Rect2i(Size2i(), rt->size).intersection(p_region); if (region.size == Size2i()) { return; //nothing to do } @@ -6994,7 +7010,7 @@ void RendererStorageRD::render_target_gen_back_buffer_mipmaps(RID p_render_targe if (p_region == Rect2i()) { region.size = rt->size; } else { - region = Rect2i(Size2i(), rt->size).clip(p_region); + region = Rect2i(Size2i(), rt->size).intersection(p_region); if (region.size == Size2i()) { return; //nothing to do } diff --git a/servers/rendering/renderer_rd/renderer_storage_rd.h b/servers/rendering/renderer_rd/renderer_storage_rd.h index ee4cf6da1a..e4199ffd12 100644 --- a/servers/rendering/renderer_rd/renderer_storage_rd.h +++ b/servers/rendering/renderer_rd/renderer_storage_rd.h @@ -964,6 +964,7 @@ private: bool box_projection = false; bool enable_shadows = false; uint32_t cull_mask = (1 << 20) - 1; + float lod_threshold = 0.01; RendererStorage::InstanceDependency instance_dependency; }; @@ -1417,22 +1418,50 @@ public: return mesh->material_cache.ptr(); } - _FORCE_INLINE_ RS::PrimitiveType mesh_surface_get_primitive(RID p_mesh, uint32_t p_surface_index) { + _FORCE_INLINE_ void *mesh_get_surface(RID p_mesh, uint32_t p_surface_index) { Mesh *mesh = mesh_owner.getornull(p_mesh); - ERR_FAIL_COND_V(!mesh, RS::PRIMITIVE_MAX); - ERR_FAIL_UNSIGNED_INDEX_V(p_surface_index, mesh->surface_count, RS::PRIMITIVE_MAX); + ERR_FAIL_COND_V(!mesh, nullptr); + ERR_FAIL_UNSIGNED_INDEX_V(p_surface_index, mesh->surface_count, nullptr); - return mesh->surfaces[p_surface_index]->primitive; + return mesh->surfaces[p_surface_index]; } - _FORCE_INLINE_ void mesh_surface_get_arrays_and_format(RID p_mesh, uint32_t p_surface_index, uint32_t p_input_mask, RID &r_vertex_array_rd, RID &r_index_array_rd, RD::VertexFormatID &r_vertex_format) { - Mesh *mesh = mesh_owner.getornull(p_mesh); - ERR_FAIL_COND(!mesh); - ERR_FAIL_UNSIGNED_INDEX(p_surface_index, mesh->surface_count); + _FORCE_INLINE_ RS::PrimitiveType mesh_surface_get_primitive(void *p_surface) { + Mesh::Surface *surface = reinterpret_cast<Mesh::Surface *>(p_surface); + return surface->primitive; + } - Mesh::Surface *s = mesh->surfaces[p_surface_index]; + _FORCE_INLINE_ bool mesh_surface_has_lod(void *p_surface) const { + Mesh::Surface *s = reinterpret_cast<Mesh::Surface *>(p_surface); + return s->lod_count > 0; + } + + _FORCE_INLINE_ RID mesh_surface_get_index_array(void *p_surface) const { + Mesh::Surface *s = reinterpret_cast<Mesh::Surface *>(p_surface); + + return s->index_array; + } - r_index_array_rd = s->index_array; + _FORCE_INLINE_ RID mesh_surface_get_index_array_with_lod(void *p_surface, float p_model_scale, float p_distance_threshold, float p_lod_threshold) const { + Mesh::Surface *s = reinterpret_cast<Mesh::Surface *>(p_surface); + + int32_t current_lod = -1; + for (uint32_t i = 0; i < s->lod_count; i++) { + float screen_size = s->lods[i].edge_length * p_model_scale / p_distance_threshold; + if (screen_size > p_lod_threshold) { + break; + } + current_lod = i; + } + if (current_lod == -1) { + return s->index_array; + } else { + return s->lods[current_lod].index_array; + } + } + + _FORCE_INLINE_ void mesh_surface_get_vertex_arrays_and_format(void *p_surface, uint32_t p_input_mask, RID &r_vertex_array_rd, RD::VertexFormatID &r_vertex_format) { + Mesh::Surface *s = reinterpret_cast<Mesh::Surface *>(p_surface); s->version_lock.lock(); @@ -1461,7 +1490,7 @@ public: s->version_lock.unlock(); } - _FORCE_INLINE_ void mesh_instance_surface_get_arrays_and_format(RID p_mesh_instance, uint32_t p_surface_index, uint32_t p_input_mask, RID &r_vertex_array_rd, RID &r_index_array_rd, RD::VertexFormatID &r_vertex_format) { + _FORCE_INLINE_ void mesh_instance_surface_get_vertex_arrays_and_format(RID p_mesh_instance, uint32_t p_surface_index, uint32_t p_input_mask, RID &r_vertex_array_rd, RD::VertexFormatID &r_vertex_format) { MeshInstance *mi = mesh_instance_owner.getornull(p_mesh_instance); ERR_FAIL_COND(!mi); Mesh *mesh = mi->mesh; @@ -1470,8 +1499,6 @@ public: MeshInstance::Surface *mis = &mi->surfaces[p_surface_index]; Mesh::Surface *s = mesh->surfaces[p_surface_index]; - r_index_array_rd = s->index_array; - s->version_lock.lock(); //there will never be more than, at much, 3 or 4 versions, so iterating is the fastest way @@ -1780,6 +1807,7 @@ public: void reflection_probe_set_enable_shadows(RID p_probe, bool p_enable); void reflection_probe_set_cull_mask(RID p_probe, uint32_t p_layers); void reflection_probe_set_resolution(RID p_probe, int p_resolution); + void reflection_probe_set_lod_threshold(RID p_probe, float p_ratio); AABB reflection_probe_get_aabb(RID p_probe) const; RS::ReflectionProbeUpdateMode reflection_probe_get_update_mode(RID p_probe) const; @@ -1787,6 +1815,8 @@ public: Vector3 reflection_probe_get_extents(RID p_probe) const; Vector3 reflection_probe_get_origin_offset(RID p_probe) const; float reflection_probe_get_origin_max_distance(RID p_probe) const; + float reflection_probe_get_lod_threshold(RID p_probe) const; + int reflection_probe_get_resolution(RID p_probe) const; bool reflection_probe_renders_shadows(RID p_probe) const; diff --git a/servers/rendering/renderer_rd/shaders/scene_forward.glsl b/servers/rendering/renderer_rd/shaders/scene_forward.glsl index 5b01cb1f82..a7fe86b029 100644 --- a/servers/rendering/renderer_rd/shaders/scene_forward.glsl +++ b/servers/rendering/renderer_rd/shaders/scene_forward.glsl @@ -9,7 +9,13 @@ VERSION_DEFINES /* INPUT ATTRIBS */ layout(location = 0) in vec3 vertex_attrib; + +//only for pure render depth when normal is not used + +#ifdef NORMAL_USED layout(location = 1) in vec3 normal_attrib; +#endif + #if defined(TANGENT_USED) || defined(NORMALMAP_USED) || defined(LIGHT_ANISOTROPY_USED) layout(location = 2) in vec4 tangent_attrib; #endif @@ -18,7 +24,9 @@ layout(location = 2) in vec4 tangent_attrib; layout(location = 3) in vec4 color_attrib; #endif +#ifdef UV_USED layout(location = 4) in vec2 uv_attrib; +#endif #if defined(UV2_USED) || defined(USE_LIGHTMAP) || defined(MODE_RENDER_MATERIAL) layout(location = 5) in vec2 uv2_attrib; @@ -51,13 +59,18 @@ layout(location = 11) in vec4 weight_attrib; /* Varyings */ layout(location = 0) out vec3 vertex_interp; + +#ifdef NORMAL_USED layout(location = 1) out vec3 normal_interp; +#endif #if defined(COLOR_USED) layout(location = 2) out vec4 color_interp; #endif +#ifdef UV_USED layout(location = 3) out vec2 uv_interp; +#endif #if defined(UV2_USED) || defined(USE_LIGHTMAP) layout(location = 4) out vec2 uv2_interp; @@ -138,7 +151,9 @@ void main() { } vec3 vertex = vertex_attrib; +#ifdef NORMAL_USED vec3 normal = normal_attrib * 2.0 - 1.0; +#endif #if defined(TANGENT_USED) || defined(NORMALMAP_USED) || defined(LIGHT_ANISOTROPY_USED) vec3 tangent = tangent_attrib.xyz * 2.0 - 1.0; @@ -171,7 +186,10 @@ void main() { #endif } #endif + +#ifdef UV_USED uv_interp = uv_attrib; +#endif #if defined(UV2_USED) || defined(USE_LIGHTMAP) uv2_interp = uv2_attrib; @@ -215,9 +233,12 @@ VERTEX_SHADER_CODE #if !defined(SKIP_TRANSFORM_USED) && !defined(VERTEX_WORLD_COORDS_USED) vertex = (modelview * vec4(vertex, 1.0)).xyz; +#ifdef NORMAL_USED normal = modelview_normal * normal; #endif +#endif + #if defined(TANGENT_USED) || defined(NORMALMAP_USED) || defined(LIGHT_ANISOTROPY_USED) binormal = modelview_normal * binormal; @@ -238,7 +259,9 @@ VERTEX_SHADER_CODE #endif vertex_interp = vertex; +#ifdef NORMAL_USED normal_interp = normal; +#endif #if defined(TANGENT_USED) || defined(NORMALMAP_USED) || defined(LIGHT_ANISOTROPY_USED) tangent_interp = tangent; @@ -250,7 +273,6 @@ VERTEX_SHADER_CODE #ifdef MODE_DUAL_PARABOLOID vertex_interp.z *= scene_data.dual_paraboloid_side; - normal_interp.z *= scene_data.dual_paraboloid_side; dp_clip = vertex_interp.z; //this attempts to avoid noise caused by objects sent to the other parabolloid side due to bias @@ -301,13 +323,18 @@ VERSION_DEFINES /* Varyings */ layout(location = 0) in vec3 vertex_interp; + +#ifdef NORMAL_USED layout(location = 1) in vec3 normal_interp; +#endif #if defined(COLOR_USED) layout(location = 2) in vec4 color_interp; #endif +#ifdef UV_USED layout(location = 3) in vec2 uv_interp; +#endif #if defined(UV2_USED) || defined(USE_LIGHTMAP) layout(location = 4) in vec2 uv2_interp; @@ -1799,6 +1826,8 @@ void main() { vec3 binormal = vec3(0.0); vec3 tangent = vec3(0.0); #endif + +#ifdef NORMAL_USED vec3 normal = normalize(normal_interp); #if defined(DO_SIDE_CHECK) @@ -1807,7 +1836,11 @@ void main() { } #endif +#endif //NORMAL_USED + +#ifdef UV_USED vec2 uv = uv_interp; +#endif #if defined(UV2_USED) || defined(USE_LIGHTMAP) vec2 uv2 = uv2_interp; @@ -1994,6 +2027,7 @@ FRAGMENT_SHADER_CODE #endif //not render depth /////////////////////// LIGHTING ////////////////////////////// +#ifdef NORMAL_USED if (scene_data.roughness_limiter_enabled) { //http://www.jp.square-enix.com/tech/library/pdf/ImprovedGeometricSpecularAA.pdf float roughness2 = roughness * roughness; @@ -2003,6 +2037,7 @@ FRAGMENT_SHADER_CODE float filteredRoughness2 = min(1.0, roughness2 + kernelRoughness2); roughness = sqrt(filteredRoughness2); } +#endif //apply energy conservation vec3 specular_light = vec3(0.0, 0.0, 0.0); diff --git a/servers/rendering/renderer_rd/shaders/scene_forward_inc.glsl b/servers/rendering/renderer_rd/shaders/scene_forward_inc.glsl index d18581c1b3..fdc9941bba 100644 --- a/servers/rendering/renderer_rd/shaders/scene_forward_inc.glsl +++ b/servers/rendering/renderer_rd/shaders/scene_forward_inc.glsl @@ -5,6 +5,12 @@ #include "cluster_data_inc.glsl" +#if !defined(MODE_RENDER_DEPTH) || defined(MODE_RENDER_MATERIAL) || defined(MODE_RENDER_SDF) || defined(MODE_RENDER_NORMAL_ROUGHNESS) || defined(MODE_RENDER_GIPROBE) || defined(TANGENT_USED) || defined(NORMALMAP_USED) +#ifndef NORMAL_USED +#define NORMAL_USED +#endif +#endif + layout(push_constant, binding = 0, std430) uniform DrawCall { uint instance_index; uint pad; //16 bits minimum size diff --git a/servers/rendering/renderer_scene.h b/servers/rendering/renderer_scene.h index 3da08f10af..56c38beaa3 100644 --- a/servers/rendering/renderer_scene.h +++ b/servers/rendering/renderer_scene.h @@ -88,6 +88,7 @@ public: virtual void instance_geometry_set_draw_range(RID p_instance, float p_min, float p_max, float p_min_margin, float p_max_margin) = 0; virtual void instance_geometry_set_as_instance_lod(RID p_instance, RID p_as_lod_of_instance) = 0; virtual void instance_geometry_set_lightmap(RID p_instance, RID p_lightmap, const Rect2 &p_lightmap_uv_scale, int p_slice_index) = 0; + virtual void instance_geometry_set_lod_bias(RID p_instance, float p_lod_bias) = 0; virtual void instance_geometry_set_shader_parameter(RID p_instance, const StringName &p_parameter, const Variant &p_value) = 0; virtual void instance_geometry_get_shader_parameter_list(RID p_instance, List<PropertyInfo> *p_parameters) const = 0; @@ -187,8 +188,8 @@ public: virtual void sdfgi_set_debug_probe_select(const Vector3 &p_position, const Vector3 &p_dir) = 0; virtual void render_empty_scene(RID p_render_buffers, RID p_scenario, RID p_shadow_atlas) = 0; - virtual void render_camera(RID p_render_buffers, RID p_camera, RID p_scenario, Size2 p_viewport_size, RID p_shadow_atlas) = 0; - virtual void render_camera(RID p_render_buffers, Ref<XRInterface> &p_interface, XRInterface::Eyes p_eye, RID p_camera, RID p_scenario, Size2 p_viewport_size, RID p_shadow_atlas) = 0; + virtual void render_camera(RID p_render_buffers, RID p_camera, RID p_scenario, Size2 p_viewport_size, float p_lod_threshold, RID p_shadow_atlas) = 0; + virtual void render_camera(RID p_render_buffers, Ref<XRInterface> &p_interface, XRInterface::Eyes p_eye, RID p_camera, RID p_scenario, Size2 p_viewport_size, float p_lod_threshold, RID p_shadow_atlas) = 0; virtual void update() = 0; virtual void render_probes() = 0; diff --git a/servers/rendering/renderer_scene_cull.cpp b/servers/rendering/renderer_scene_cull.cpp index c63152c11e..88a0859a28 100644 --- a/servers/rendering/renderer_scene_cull.cpp +++ b/servers/rendering/renderer_scene_cull.cpp @@ -969,6 +969,13 @@ void RendererSceneCull::instance_geometry_set_lightmap(RID p_instance, RID p_lig } } +void RendererSceneCull::instance_geometry_set_lod_bias(RID p_instance, float p_lod_bias) { + Instance *instance = instance_owner.getornull(p_instance); + ERR_FAIL_COND(!instance); + + instance->lod_bias = p_lod_bias; +} + void RendererSceneCull::instance_geometry_set_shader_parameter(RID p_instance, const StringName &p_parameter, const Variant &p_value) { Instance *instance = instance_owner.getornull(p_instance); ERR_FAIL_COND(!instance); @@ -1325,7 +1332,7 @@ void RendererSceneCull::_update_instance_lightmap_captures(Instance *p_instance) } } -bool RendererSceneCull::_light_instance_update_shadow(Instance *p_instance, const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, bool p_cam_vaspect, RID p_shadow_atlas, Scenario *p_scenario) { +bool RendererSceneCull::_light_instance_update_shadow(Instance *p_instance, const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, bool p_cam_vaspect, RID p_shadow_atlas, Scenario *p_scenario, float p_screen_lod_threshold) { InstanceLightData *light = static_cast<InstanceLightData *>(p_instance->base_data); Transform light_transform = p_instance->transform; @@ -1335,6 +1342,8 @@ bool RendererSceneCull::_light_instance_update_shadow(Instance *p_instance, cons switch (RSG::storage->light_get_type(p_instance->base)) { case RS::LIGHT_DIRECTIONAL: { + Plane camera_plane(p_cam_transform.get_origin(), -p_cam_transform.basis.get_axis(Vector3::AXIS_Z)); + real_t max_distance = p_cam_projection.get_z_far(); real_t shadow_max = RSG::storage->light_get_param(p_instance->base, RS::LIGHT_PARAM_SHADOW_MAX_DISTANCE); if (shadow_max > 0 && !p_cam_orthogonal) { //its impractical (and leads to unwanted behaviors) to set max distance in orthogonal camera @@ -1703,7 +1712,7 @@ bool RendererSceneCull::_light_instance_update_shadow(Instance *p_instance, cons RSG::storage->update_mesh_instances(); - scene_render->render_shadow(light->instance, p_shadow_atlas, i, (RendererSceneRender::InstanceBase **)instance_shadow_cull_result, cull_count); + scene_render->render_shadow(light->instance, p_shadow_atlas, i, (RendererSceneRender::InstanceBase **)instance_shadow_cull_result, cull_count, camera_plane, p_cam_projection.get_lod_multiplier(), p_screen_lod_threshold); } } break; @@ -1860,7 +1869,7 @@ bool RendererSceneCull::_light_instance_update_shadow(Instance *p_instance, cons return animated_material_found; } -void RendererSceneCull::render_camera(RID p_render_buffers, RID p_camera, RID p_scenario, Size2 p_viewport_size, RID p_shadow_atlas) { +void RendererSceneCull::render_camera(RID p_render_buffers, RID p_camera, RID p_scenario, Size2 p_viewport_size, float p_screen_lod_threshold, RID p_shadow_atlas) { // render to mono camera #ifndef _3D_DISABLED @@ -1905,12 +1914,12 @@ void RendererSceneCull::render_camera(RID p_render_buffers, RID p_camera, RID p_ RID environment = _render_get_environment(p_camera, p_scenario); - _prepare_scene(camera->transform, camera_matrix, ortho, camera->vaspect, p_render_buffers, environment, camera->visible_layers, p_scenario, p_shadow_atlas, RID()); - _render_scene(p_render_buffers, camera->transform, camera_matrix, ortho, environment, camera->effects, p_scenario, p_shadow_atlas, RID(), -1); + _prepare_scene(camera->transform, camera_matrix, ortho, camera->vaspect, p_render_buffers, environment, camera->visible_layers, p_scenario, p_shadow_atlas, RID(), p_screen_lod_threshold); + _render_scene(p_render_buffers, camera->transform, camera_matrix, ortho, environment, camera->effects, p_scenario, p_shadow_atlas, RID(), -1, p_screen_lod_threshold); #endif } -void RendererSceneCull::render_camera(RID p_render_buffers, Ref<XRInterface> &p_interface, XRInterface::Eyes p_eye, RID p_camera, RID p_scenario, Size2 p_viewport_size, RID p_shadow_atlas) { +void RendererSceneCull::render_camera(RID p_render_buffers, Ref<XRInterface> &p_interface, XRInterface::Eyes p_eye, RID p_camera, RID p_scenario, Size2 p_viewport_size, float p_screen_lod_threshold, RID p_shadow_atlas) { // render for AR/VR interface Camera *camera = camera_owner.getornull(p_camera); @@ -1984,17 +1993,17 @@ void RendererSceneCull::render_camera(RID p_render_buffers, Ref<XRInterface> &p_ mono_transform *= apply_z_shift; // now prepare our scene with our adjusted transform projection matrix - _prepare_scene(mono_transform, combined_matrix, false, false, p_render_buffers, environment, camera->visible_layers, p_scenario, p_shadow_atlas, RID()); + _prepare_scene(mono_transform, combined_matrix, false, false, p_render_buffers, environment, camera->visible_layers, p_scenario, p_shadow_atlas, RID(), p_screen_lod_threshold); } else if (p_eye == XRInterface::EYE_MONO) { // For mono render, prepare as per usual - _prepare_scene(cam_transform, camera_matrix, false, false, p_render_buffers, environment, camera->visible_layers, p_scenario, p_shadow_atlas, RID()); + _prepare_scene(cam_transform, camera_matrix, false, false, p_render_buffers, environment, camera->visible_layers, p_scenario, p_shadow_atlas, RID(), p_screen_lod_threshold); } // And render our scene... - _render_scene(p_render_buffers, cam_transform, camera_matrix, false, environment, camera->effects, p_scenario, p_shadow_atlas, RID(), -1); + _render_scene(p_render_buffers, cam_transform, camera_matrix, false, environment, camera->effects, p_scenario, p_shadow_atlas, RID(), -1, p_screen_lod_threshold); }; -void RendererSceneCull::_prepare_scene(const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, bool p_cam_vaspect, RID p_render_buffers, RID p_environment, uint32_t p_visible_layers, RID p_scenario, RID p_shadow_atlas, RID p_reflection_probe, bool p_using_shadows) { +void RendererSceneCull::_prepare_scene(const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, bool p_cam_vaspect, RID p_render_buffers, RID p_environment, uint32_t p_visible_layers, RID p_scenario, RID p_shadow_atlas, RID p_reflection_probe, float p_screen_lod_threshold, bool p_using_shadows) { // Note, in stereo rendering: // - p_cam_transform will be a transform in the middle of our two eyes // - p_cam_projection is a wider frustrum that encompasses both eyes @@ -2250,7 +2259,7 @@ void RendererSceneCull::_prepare_scene(const Transform p_cam_transform, const Ca for (int i = 0; i < directional_shadow_count; i++) { RENDER_TIMESTAMP(">Rendering Directional Light " + itos(i)); - _light_instance_update_shadow(lights_with_shadow[i], p_cam_transform, p_cam_projection, p_cam_orthogonal, p_cam_vaspect, p_shadow_atlas, scenario); + _light_instance_update_shadow(lights_with_shadow[i], p_cam_transform, p_cam_projection, p_cam_orthogonal, p_cam_vaspect, p_shadow_atlas, scenario, p_screen_lod_threshold); RENDER_TIMESTAMP("<Rendering Directional Light " + itos(i)); } @@ -2349,7 +2358,7 @@ void RendererSceneCull::_prepare_scene(const Transform p_cam_transform, const Ca if (redraw) { //must redraw! RENDER_TIMESTAMP(">Rendering Light " + itos(i)); - light->shadow_dirty = _light_instance_update_shadow(ins, p_cam_transform, p_cam_projection, p_cam_orthogonal, p_cam_vaspect, p_shadow_atlas, scenario); + light->shadow_dirty = _light_instance_update_shadow(ins, p_cam_transform, p_cam_projection, p_cam_orthogonal, p_cam_vaspect, p_shadow_atlas, scenario, p_screen_lod_threshold); RENDER_TIMESTAMP("<Rendering Light " + itos(i)); } } @@ -2447,7 +2456,7 @@ RID RendererSceneCull::_render_get_environment(RID p_camera, RID p_scenario) { return RID(); } -void RendererSceneCull::_render_scene(RID p_render_buffers, const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, RID p_environment, RID p_force_camera_effects, RID p_scenario, RID p_shadow_atlas, RID p_reflection_probe, int p_reflection_probe_pass) { +void RendererSceneCull::_render_scene(RID p_render_buffers, const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, RID p_environment, RID p_force_camera_effects, RID p_scenario, RID p_shadow_atlas, RID p_reflection_probe, int p_reflection_probe_pass, float p_screen_lod_threshold) { Scenario *scenario = scenario_owner.getornull(p_scenario); RID camera_effects; @@ -2459,7 +2468,7 @@ void RendererSceneCull::_render_scene(RID p_render_buffers, const Transform p_ca /* PROCESS GEOMETRY AND DRAW SCENE */ RENDER_TIMESTAMP("Render Scene "); - scene_render->render_scene(p_render_buffers, p_cam_transform, p_cam_projection, p_cam_orthogonal, (RendererSceneRender::InstanceBase **)instance_cull_result, instance_cull_count, light_instance_cull_result, light_cull_count + directional_light_count, reflection_probe_instance_cull_result, reflection_probe_cull_count, gi_probe_instance_cull_result, gi_probe_cull_count, decal_instance_cull_result, decal_cull_count, (RendererSceneRender::InstanceBase **)lightmap_cull_result, lightmap_cull_count, p_environment, camera_effects, p_shadow_atlas, p_reflection_probe.is_valid() ? RID() : scenario->reflection_atlas, p_reflection_probe, p_reflection_probe_pass); + scene_render->render_scene(p_render_buffers, p_cam_transform, p_cam_projection, p_cam_orthogonal, (RendererSceneRender::InstanceBase **)instance_cull_result, instance_cull_count, light_instance_cull_result, light_cull_count + directional_light_count, reflection_probe_instance_cull_result, reflection_probe_cull_count, gi_probe_instance_cull_result, gi_probe_cull_count, decal_instance_cull_result, decal_cull_count, (RendererSceneRender::InstanceBase **)lightmap_cull_result, lightmap_cull_count, p_environment, camera_effects, p_shadow_atlas, p_reflection_probe.is_valid() ? RID() : scenario->reflection_atlas, p_reflection_probe, p_reflection_probe_pass, p_screen_lod_threshold); } void RendererSceneCull::render_empty_scene(RID p_render_buffers, RID p_scenario, RID p_shadow_atlas) { @@ -2474,7 +2483,7 @@ void RendererSceneCull::render_empty_scene(RID p_render_buffers, RID p_scenario, environment = scenario->fallback_environment; } RENDER_TIMESTAMP("Render Empty Scene "); - scene_render->render_scene(p_render_buffers, Transform(), CameraMatrix(), true, nullptr, 0, nullptr, 0, nullptr, 0, nullptr, 0, nullptr, 0, nullptr, 0, environment, RID(), p_shadow_atlas, scenario->reflection_atlas, RID(), 0); + scene_render->render_scene(p_render_buffers, Transform(), CameraMatrix(), true, nullptr, 0, nullptr, 0, nullptr, 0, nullptr, 0, nullptr, 0, nullptr, 0, environment, RID(), p_shadow_atlas, scenario->reflection_atlas, RID(), 0, 0); #endif } @@ -2512,6 +2521,8 @@ bool RendererSceneCull::_render_reflection_probe_step(Instance *p_instance, int Vector3 extents = RSG::storage->reflection_probe_get_extents(p_instance->base); Vector3 origin_offset = RSG::storage->reflection_probe_get_origin_offset(p_instance->base); float max_distance = RSG::storage->reflection_probe_get_origin_max_distance(p_instance->base); + float size = scene_render->reflection_atlas_get_size(scenario->reflection_atlas); + float lod_threshold = RSG::storage->reflection_probe_get_lod_threshold(p_instance->base) / size; Vector3 edge = view_normals[p_step] * extents; float distance = ABS(view_normals[p_step].dot(edge) - view_normals[p_step].dot(origin_offset)); //distance from origin offset to actual view distance limit @@ -2535,8 +2546,8 @@ bool RendererSceneCull::_render_reflection_probe_step(Instance *p_instance, int } RENDER_TIMESTAMP("Render Reflection Probe, Step " + itos(p_step)); - _prepare_scene(xform, cm, false, false, RID(), RID(), RSG::storage->reflection_probe_get_cull_mask(p_instance->base), p_instance->scenario->self, shadow_atlas, reflection_probe->instance, use_shadows); - _render_scene(RID(), xform, cm, false, RID(), RID(), p_instance->scenario->self, shadow_atlas, reflection_probe->instance, p_step); + _prepare_scene(xform, cm, false, false, RID(), RID(), RSG::storage->reflection_probe_get_cull_mask(p_instance->base), p_instance->scenario->self, shadow_atlas, reflection_probe->instance, lod_threshold, use_shadows); + _render_scene(RID(), xform, cm, false, RID(), RID(), p_instance->scenario->self, shadow_atlas, reflection_probe->instance, p_step, lod_threshold); } else { //do roughness postprocess step until it believes it's done diff --git a/servers/rendering/renderer_scene_cull.h b/servers/rendering/renderer_scene_cull.h index c4379e4560..4d82d873cc 100644 --- a/servers/rendering/renderer_scene_cull.h +++ b/servers/rendering/renderer_scene_cull.h @@ -89,7 +89,7 @@ public: fov = 75; type = PERSPECTIVE; znear = 0.05; - zfar = 100; + zfar = 4000; size = 1.0; offset = Vector2(); vaspect = false; @@ -447,6 +447,7 @@ public: virtual void instance_geometry_set_draw_range(RID p_instance, float p_min, float p_max, float p_min_margin, float p_max_margin); virtual void instance_geometry_set_as_instance_lod(RID p_instance, RID p_as_lod_of_instance); virtual void instance_geometry_set_lightmap(RID p_instance, RID p_lightmap, const Rect2 &p_lightmap_uv_scale, int p_slice_index); + virtual void instance_geometry_set_lod_bias(RID p_instance, float p_lod_bias); void _update_instance_shader_parameters_from_material(Map<StringName, RendererSceneRender::InstanceBase::InstanceShaderParameter> &isparams, const Map<StringName, RendererSceneRender::InstanceBase::InstanceShaderParameter> &existing_isparams, RID p_material); @@ -460,17 +461,17 @@ public: _FORCE_INLINE_ void _update_dirty_instance(Instance *p_instance); _FORCE_INLINE_ void _update_instance_lightmap_captures(Instance *p_instance); - _FORCE_INLINE_ bool _light_instance_update_shadow(Instance *p_instance, const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, bool p_cam_vaspect, RID p_shadow_atlas, Scenario *p_scenario); + _FORCE_INLINE_ bool _light_instance_update_shadow(Instance *p_instance, const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, bool p_cam_vaspect, RID p_shadow_atlas, Scenario *p_scenario, float p_scren_lod_threshold); RID _render_get_environment(RID p_camera, RID p_scenario); bool _render_reflection_probe_step(Instance *p_instance, int p_step); - void _prepare_scene(const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, bool p_cam_vaspect, RID p_render_buffers, RID p_environment, uint32_t p_visible_layers, RID p_scenario, RID p_shadow_atlas, RID p_reflection_probe, bool p_using_shadows = true); - void _render_scene(RID p_render_buffers, const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, RID p_environment, RID p_force_camera_effects, RID p_scenario, RID p_shadow_atlas, RID p_reflection_probe, int p_reflection_probe_pass); + void _prepare_scene(const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, bool p_cam_vaspect, RID p_render_buffers, RID p_environment, uint32_t p_visible_layers, RID p_scenario, RID p_shadow_atlas, RID p_reflection_probe, float p_screen_lod_threshold, bool p_using_shadows = true); + void _render_scene(RID p_render_buffers, const Transform p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_orthogonal, RID p_environment, RID p_force_camera_effects, RID p_scenario, RID p_shadow_atlas, RID p_reflection_probe, int p_reflection_probe_pass, float p_screen_lod_threshold); void render_empty_scene(RID p_render_buffers, RID p_scenario, RID p_shadow_atlas); - void render_camera(RID p_render_buffers, RID p_camera, RID p_scenario, Size2 p_viewport_size, RID p_shadow_atlas); - void render_camera(RID p_render_buffers, Ref<XRInterface> &p_interface, XRInterface::Eyes p_eye, RID p_camera, RID p_scenario, Size2 p_viewport_size, RID p_shadow_atlas); + void render_camera(RID p_render_buffers, RID p_camera, RID p_scenario, Size2 p_viewport_size, float p_screen_lod_threshold, RID p_shadow_atlas); + void render_camera(RID p_render_buffers, Ref<XRInterface> &p_interface, XRInterface::Eyes p_eye, RID p_camera, RID p_scenario, Size2 p_viewport_size, float p_screen_lod_threshold, RID p_shadow_atlas); void update_dirty_instances(); void render_particle_colliders(); diff --git a/servers/rendering/renderer_scene_render.h b/servers/rendering/renderer_scene_render.h index 5690988297..e1f3ea9b6b 100644 --- a/servers/rendering/renderer_scene_render.h +++ b/servers/rendering/renderer_scene_render.h @@ -139,6 +139,8 @@ public: Transform transform; + float lod_bias; + int depth_layer; uint32_t layer_mask; @@ -195,6 +197,7 @@ public: lightmap_slice_index = 0; lightmap = nullptr; lightmap_cull_index = 0; + lod_bias = 1.0; } virtual ~InstanceBase() { @@ -212,6 +215,7 @@ public: virtual RID reflection_atlas_create() = 0; virtual void reflection_atlas_set_size(RID p_ref_atlas, int p_reflection_size, int p_reflection_count) = 0; + virtual int reflection_atlas_get_size(RID p_ref_atlas) const = 0; virtual RID reflection_probe_instance_create(RID p_probe) = 0; virtual void reflection_probe_instance_set_transform(RID p_instance, const Transform &p_transform) = 0; @@ -231,9 +235,9 @@ public: virtual void gi_probe_set_quality(RS::GIProbeQuality) = 0; - virtual void render_scene(RID p_render_buffers, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID *p_light_cull_result, int p_light_cull_count, RID *p_reflection_probe_cull_result, int p_reflection_probe_cull_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, RID *p_decal_cull_result, int p_decal_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass) = 0; + virtual void render_scene(RID p_render_buffers, const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID *p_light_cull_result, int p_light_cull_count, RID *p_reflection_probe_cull_result, int p_reflection_probe_cull_count, RID *p_gi_probe_cull_result, int p_gi_probe_cull_count, RID *p_decal_cull_result, int p_decal_cull_count, InstanceBase **p_lightmap_cull_result, int p_lightmap_cull_count, RID p_environment, RID p_camera_effects, RID p_shadow_atlas, RID p_reflection_atlas, RID p_reflection_probe, int p_reflection_probe_pass, float p_screen_lod_threshold) = 0; - virtual void render_shadow(RID p_light, RID p_shadow_atlas, int p_pass, InstanceBase **p_cull_result, int p_cull_count) = 0; + virtual void render_shadow(RID p_light, RID p_shadow_atlas, int p_pass, InstanceBase **p_cull_result, int p_cull_count, const Plane &p_camera_plane = Plane(), float p_lod_distance_multiplier = 0, float p_screen_lod_threshold = 0.0) = 0; virtual void render_material(const Transform &p_cam_transform, const CameraMatrix &p_cam_projection, bool p_cam_ortogonal, InstanceBase **p_cull_result, int p_cull_count, RID p_framebuffer, const Rect2i &p_region) = 0; virtual void render_sdfgi(RID p_render_buffers, int p_region, InstanceBase **p_cull_result, int p_cull_count) = 0; virtual void render_sdfgi_static_lights(RID p_render_buffers, uint32_t p_cascade_count, const uint32_t *p_cascade_indices, const RID **p_positional_light_cull_result, const uint32_t *p_positional_light_cull_count) = 0; diff --git a/servers/rendering/renderer_storage.h b/servers/rendering/renderer_storage.h index 2dbef2fcc6..895a7a5be8 100644 --- a/servers/rendering/renderer_storage.h +++ b/servers/rendering/renderer_storage.h @@ -338,6 +338,7 @@ public: virtual void reflection_probe_set_enable_box_projection(RID p_probe, bool p_enable) = 0; virtual void reflection_probe_set_enable_shadows(RID p_probe, bool p_enable) = 0; virtual void reflection_probe_set_cull_mask(RID p_probe, uint32_t p_layers) = 0; + virtual void reflection_probe_set_lod_threshold(RID p_probe, float p_ratio) = 0; virtual AABB reflection_probe_get_aabb(RID p_probe) const = 0; virtual RS::ReflectionProbeUpdateMode reflection_probe_get_update_mode(RID p_probe) const = 0; @@ -346,6 +347,7 @@ public: virtual Vector3 reflection_probe_get_origin_offset(RID p_probe) const = 0; virtual float reflection_probe_get_origin_max_distance(RID p_probe) const = 0; virtual bool reflection_probe_renders_shadows(RID p_probe) const = 0; + virtual float reflection_probe_get_lod_threshold(RID p_probe) const = 0; virtual void base_update_dependency(RID p_base, InstanceBaseDependency *p_instance) = 0; virtual void skeleton_update_dependency(RID p_base, InstanceBaseDependency *p_instance) = 0; diff --git a/servers/rendering/renderer_viewport.cpp b/servers/rendering/renderer_viewport.cpp index 86bfda056b..ea95eb1189 100644 --- a/servers/rendering/renderer_viewport.cpp +++ b/servers/rendering/renderer_viewport.cpp @@ -79,10 +79,11 @@ void RendererViewport::_draw_3d(Viewport *p_viewport, XRInterface::Eyes p_eye) { xr_interface = XRServer::get_singleton()->get_primary_interface(); } + float screen_lod_threshold = p_viewport->lod_threshold / float(p_viewport->size.width); if (p_viewport->use_xr && xr_interface.is_valid()) { - RSG::scene->render_camera(p_viewport->render_buffers, xr_interface, p_eye, p_viewport->camera, p_viewport->scenario, p_viewport->size, p_viewport->shadow_atlas); + RSG::scene->render_camera(p_viewport->render_buffers, xr_interface, p_eye, p_viewport->camera, p_viewport->scenario, p_viewport->size, screen_lod_threshold, p_viewport->shadow_atlas); } else { - RSG::scene->render_camera(p_viewport->render_buffers, p_viewport->camera, p_viewport->scenario, p_viewport->size, p_viewport->shadow_atlas); + RSG::scene->render_camera(p_viewport->render_buffers, p_viewport->camera, p_viewport->scenario, p_viewport->size, screen_lod_threshold, p_viewport->shadow_atlas); } RENDER_TIMESTAMP("<End Rendering 3D Scene"); } @@ -885,6 +886,13 @@ void RendererViewport::viewport_set_use_debanding(RID p_viewport, bool p_use_deb } } +void RendererViewport::viewport_set_lod_threshold(RID p_viewport, float p_pixels) { + Viewport *viewport = viewport_owner.getornull(p_viewport); + ERR_FAIL_COND(!viewport); + + viewport->lod_threshold = p_pixels; +} + int RendererViewport::viewport_get_render_info(RID p_viewport, RS::ViewportRenderInfo p_info) { ERR_FAIL_INDEX_V(p_info, RS::VIEWPORT_RENDER_INFO_MAX, -1); diff --git a/servers/rendering/renderer_viewport.h b/servers/rendering/renderer_viewport.h index 6634ef66e2..e836d05dfc 100644 --- a/servers/rendering/renderer_viewport.h +++ b/servers/rendering/renderer_viewport.h @@ -84,6 +84,8 @@ public: bool sdf_active; + float lod_threshold = 1.0; + uint64_t last_pass = 0; int render_info[RS::VIEWPORT_RENDER_INFO_MAX]; @@ -222,6 +224,8 @@ public: void viewport_set_screen_space_aa(RID p_viewport, RS::ViewportScreenSpaceAA p_mode); void viewport_set_use_debanding(RID p_viewport, bool p_use_debanding); + void viewport_set_lod_threshold(RID p_viewport, float p_pixels); + virtual int viewport_get_render_info(RID p_viewport, RS::ViewportRenderInfo p_info); virtual void viewport_set_debug_draw(RID p_viewport, RS::ViewportDebugDraw p_draw); diff --git a/servers/rendering/rendering_server_default.h b/servers/rendering/rendering_server_default.h index 97c3aeaf45..dd1a41a4f7 100644 --- a/servers/rendering/rendering_server_default.h +++ b/servers/rendering/rendering_server_default.h @@ -359,6 +359,7 @@ public: BIND2(reflection_probe_set_enable_shadows, RID, bool) BIND2(reflection_probe_set_cull_mask, RID, uint32_t) BIND2(reflection_probe_set_resolution, RID, int) + BIND2(reflection_probe_set_lod_threshold, RID, float) /* DECAL API */ @@ -545,6 +546,7 @@ public: BIND2(viewport_set_msaa, RID, ViewportMSAA) BIND2(viewport_set_screen_space_aa, RID, ViewportScreenSpaceAA) BIND2(viewport_set_use_debanding, RID, bool) + BIND2(viewport_set_lod_threshold, RID, float) BIND2R(int, viewport_get_render_info, RID, ViewportRenderInfo) BIND2(viewport_set_debug_draw, RID, ViewportDebugDraw) @@ -673,6 +675,7 @@ public: BIND5(instance_geometry_set_draw_range, RID, float, float, float, float) BIND2(instance_geometry_set_as_instance_lod, RID, RID) BIND4(instance_geometry_set_lightmap, RID, RID, const Rect2 &, int) + BIND2(instance_geometry_set_lod_bias, RID, float) BIND3(instance_geometry_set_shader_parameter, RID, const StringName &, const Variant &) BIND2RC(Variant, instance_geometry_get_shader_parameter, RID, const StringName &) diff --git a/servers/rendering/rendering_server_wrap_mt.h b/servers/rendering/rendering_server_wrap_mt.h index 62a866955d..20556779fe 100644 --- a/servers/rendering/rendering_server_wrap_mt.h +++ b/servers/rendering/rendering_server_wrap_mt.h @@ -266,6 +266,7 @@ public: FUNC2(reflection_probe_set_enable_shadows, RID, bool) FUNC2(reflection_probe_set_cull_mask, RID, uint32_t) FUNC2(reflection_probe_set_resolution, RID, int) + FUNC2(reflection_probe_set_lod_threshold, RID, float) /* DECAL API */ @@ -450,6 +451,8 @@ public: FUNC2(viewport_set_screen_space_aa, RID, ViewportScreenSpaceAA) FUNC2(viewport_set_use_debanding, RID, bool) + FUNC2(viewport_set_lod_threshold, RID, float) + //this passes directly to avoid stalling, but it's pretty dangerous, so don't call after freeing a viewport virtual int viewport_get_render_info(RID p_viewport, ViewportRenderInfo p_info) { return rendering_server->viewport_get_render_info(p_viewport, p_info); @@ -575,6 +578,7 @@ public: FUNC5(instance_geometry_set_draw_range, RID, float, float, float, float) FUNC2(instance_geometry_set_as_instance_lod, RID, RID) FUNC4(instance_geometry_set_lightmap, RID, RID, const Rect2 &, int) + FUNC2(instance_geometry_set_lod_bias, RID, float) FUNC3(instance_geometry_set_shader_parameter, RID, const StringName &, const Variant &) FUNC2RC(Variant, instance_geometry_get_shader_parameter, RID, const StringName &) diff --git a/servers/rendering_server.h b/servers/rendering_server.h index 598c23f4ee..6f7359b276 100644 --- a/servers/rendering_server.h +++ b/servers/rendering_server.h @@ -507,6 +507,7 @@ public: virtual void reflection_probe_set_enable_shadows(RID p_probe, bool p_enable) = 0; virtual void reflection_probe_set_cull_mask(RID p_probe, uint32_t p_layers) = 0; virtual void reflection_probe_set_resolution(RID p_probe, int p_resolution) = 0; + virtual void reflection_probe_set_lod_threshold(RID p_probe, float p_pixels) = 0; /* DECAL API */ @@ -805,6 +806,8 @@ public: virtual void viewport_set_use_debanding(RID p_viewport, bool p_use_debanding) = 0; + virtual void viewport_set_lod_threshold(RID p_viewport, float p_pixels) = 0; + enum ViewportRenderInfo { VIEWPORT_RENDER_INFO_OBJECTS_IN_FRAME, VIEWPORT_RENDER_INFO_VERTICES_IN_FRAME, @@ -836,6 +839,7 @@ public: VIEWPORT_DEBUG_DRAW_SDFGI, VIEWPORT_DEBUG_DRAW_SDFGI_PROBES, VIEWPORT_DEBUG_DRAW_GI_BUFFER, + VIEWPORT_DEBUG_DRAW_DISABLE_LOD, }; @@ -1142,6 +1146,7 @@ public: virtual void instance_geometry_set_draw_range(RID p_instance, float p_min, float p_max, float p_min_margin, float p_max_margin) = 0; virtual void instance_geometry_set_as_instance_lod(RID p_instance, RID p_as_lod_of_instance) = 0; virtual void instance_geometry_set_lightmap(RID p_instance, RID p_lightmap, const Rect2 &p_lightmap_uv_scale, int p_lightmap_slice) = 0; + virtual void instance_geometry_set_lod_bias(RID p_instance, float p_lod_bias) = 0; virtual void instance_geometry_set_shader_parameter(RID p_instance, const StringName &, const Variant &p_value) = 0; virtual Variant instance_geometry_get_shader_parameter(RID p_instance, const StringName &) const = 0; diff --git a/servers/xr/xr_positional_tracker.cpp b/servers/xr/xr_positional_tracker.cpp index a59565fe0d..91469b1aec 100644 --- a/servers/xr/xr_positional_tracker.cpp +++ b/servers/xr/xr_positional_tracker.cpp @@ -34,25 +34,25 @@ void XRPositionalTracker::_bind_methods() { BIND_ENUM_CONSTANT(TRACKER_HAND_UNKNOWN); - BIND_ENUM_CONSTANT(TRACKER_LEFT_HAND); - BIND_ENUM_CONSTANT(TRACKER_RIGHT_HAND); + BIND_ENUM_CONSTANT(TRACKER_HAND_LEFT); + BIND_ENUM_CONSTANT(TRACKER_HAND_RIGHT); // this class is read only from GDScript, so we only have access to getters.. ClassDB::bind_method(D_METHOD("get_tracker_type"), &XRPositionalTracker::get_tracker_type); ClassDB::bind_method(D_METHOD("get_tracker_id"), &XRPositionalTracker::get_tracker_id); ClassDB::bind_method(D_METHOD("get_tracker_name"), &XRPositionalTracker::get_tracker_name); ClassDB::bind_method(D_METHOD("get_joy_id"), &XRPositionalTracker::get_joy_id); - ClassDB::bind_method(D_METHOD("get_tracks_orientation"), &XRPositionalTracker::get_tracks_orientation); + ClassDB::bind_method(D_METHOD("is_tracking_orientation"), &XRPositionalTracker::is_tracking_orientation); ClassDB::bind_method(D_METHOD("get_orientation"), &XRPositionalTracker::get_orientation); - ClassDB::bind_method(D_METHOD("get_tracks_position"), &XRPositionalTracker::get_tracks_position); + ClassDB::bind_method(D_METHOD("is_tracking_position"), &XRPositionalTracker::is_tracking_position); ClassDB::bind_method(D_METHOD("get_position"), &XRPositionalTracker::get_position); - ClassDB::bind_method(D_METHOD("get_hand"), &XRPositionalTracker::get_hand); + ClassDB::bind_method(D_METHOD("get_tracker_hand"), &XRPositionalTracker::get_tracker_hand); ClassDB::bind_method(D_METHOD("get_transform", "adjust_by_reference_frame"), &XRPositionalTracker::get_transform); ClassDB::bind_method(D_METHOD("get_mesh"), &XRPositionalTracker::get_mesh); // these functions we don't want to expose to normal users but do need to be callable from GDNative - ClassDB::bind_method(D_METHOD("_set_type", "type"), &XRPositionalTracker::set_type); - ClassDB::bind_method(D_METHOD("_set_name", "name"), &XRPositionalTracker::set_name); + ClassDB::bind_method(D_METHOD("_set_tracker_type", "type"), &XRPositionalTracker::set_tracker_type); + ClassDB::bind_method(D_METHOD("_set_tracker_name", "name"), &XRPositionalTracker::set_tracker_name); ClassDB::bind_method(D_METHOD("_set_joy_id", "joy_id"), &XRPositionalTracker::set_joy_id); ClassDB::bind_method(D_METHOD("_set_orientation", "orientation"), &XRPositionalTracker::set_orientation); ClassDB::bind_method(D_METHOD("_set_rw_position", "rw_position"), &XRPositionalTracker::set_rw_position); @@ -63,7 +63,7 @@ void XRPositionalTracker::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "rumble"), "set_rumble", "get_rumble"); }; -void XRPositionalTracker::set_type(XRServer::TrackerType p_type) { +void XRPositionalTracker::set_tracker_type(XRServer::TrackerType p_type) { if (type != p_type) { type = p_type; hand = XRPositionalTracker::TRACKER_HAND_UNKNOWN; @@ -81,7 +81,7 @@ XRServer::TrackerType XRPositionalTracker::get_tracker_type() const { return type; }; -void XRPositionalTracker::set_name(const String &p_name) { +void XRPositionalTracker::set_tracker_name(const String &p_name) { name = p_name; }; @@ -101,14 +101,14 @@ int XRPositionalTracker::get_joy_id() const { return joy_id; }; -bool XRPositionalTracker::get_tracks_orientation() const { - return tracks_orientation; +bool XRPositionalTracker::is_tracking_orientation() const { + return tracking_orientation; }; void XRPositionalTracker::set_orientation(const Basis &p_orientation) { _THREAD_SAFE_METHOD_ - tracks_orientation = true; // obviously we have this + tracking_orientation = true; // obviously we have this orientation = p_orientation; }; @@ -118,8 +118,8 @@ Basis XRPositionalTracker::get_orientation() const { return orientation; }; -bool XRPositionalTracker::get_tracks_position() const { - return tracks_position; +bool XRPositionalTracker::is_tracking_position() const { + return tracking_position; }; void XRPositionalTracker::set_position(const Vector3 &p_position) { @@ -130,7 +130,7 @@ void XRPositionalTracker::set_position(const Vector3 &p_position) { real_t world_scale = xr_server->get_world_scale(); ERR_FAIL_COND(world_scale == 0); - tracks_position = true; // obviously we have this + tracking_position = true; // obviously we have this rw_position = p_position / world_scale; }; @@ -147,7 +147,7 @@ Vector3 XRPositionalTracker::get_position() const { void XRPositionalTracker::set_rw_position(const Vector3 &p_rw_position) { _THREAD_SAFE_METHOD_ - tracks_position = true; // obviously we have this + tracking_position = true; // obviously we have this rw_position = p_rw_position; }; @@ -169,11 +169,11 @@ Ref<Mesh> XRPositionalTracker::get_mesh() const { return mesh; }; -XRPositionalTracker::TrackerHand XRPositionalTracker::get_hand() const { +XRPositionalTracker::TrackerHand XRPositionalTracker::get_tracker_hand() const { return hand; }; -void XRPositionalTracker::set_hand(const XRPositionalTracker::TrackerHand p_hand) { +void XRPositionalTracker::set_tracker_hand(const XRPositionalTracker::TrackerHand p_hand) { XRServer *xr_server = XRServer::get_singleton(); ERR_FAIL_NULL(xr_server); @@ -182,11 +182,11 @@ void XRPositionalTracker::set_hand(const XRPositionalTracker::TrackerHand p_hand ERR_FAIL_COND((type != XRServer::TRACKER_CONTROLLER) && (p_hand != XRPositionalTracker::TRACKER_HAND_UNKNOWN)); hand = p_hand; - if (hand == XRPositionalTracker::TRACKER_LEFT_HAND) { + if (hand == XRPositionalTracker::TRACKER_HAND_LEFT) { if (!xr_server->is_tracker_id_in_use_for_type(type, 1)) { tracker_id = 1; }; - } else if (hand == XRPositionalTracker::TRACKER_RIGHT_HAND) { + } else if (hand == XRPositionalTracker::TRACKER_HAND_RIGHT) { if (!xr_server->is_tracker_id_in_use_for_type(type, 2)) { tracker_id = 2; }; @@ -227,8 +227,8 @@ XRPositionalTracker::XRPositionalTracker() { name = "Unknown"; joy_id = -1; tracker_id = 0; - tracks_orientation = false; - tracks_position = false; + tracking_orientation = false; + tracking_position = false; hand = TRACKER_HAND_UNKNOWN; rumble = 0.0; }; diff --git a/servers/xr/xr_positional_tracker.h b/servers/xr/xr_positional_tracker.h index 8834b64464..c0976d3c13 100644 --- a/servers/xr/xr_positional_tracker.h +++ b/servers/xr/xr_positional_tracker.h @@ -50,8 +50,8 @@ class XRPositionalTracker : public Object { public: enum TrackerHand { TRACKER_HAND_UNKNOWN, /* unknown or not applicable */ - TRACKER_LEFT_HAND, /* controller is the left hand controller */ - TRACKER_RIGHT_HAND /* controller is the right hand controller */ + TRACKER_HAND_LEFT, /* controller is the left hand controller */ + TRACKER_HAND_RIGHT /* controller is the right hand controller */ }; private: @@ -59,9 +59,9 @@ private: StringName name; // (unique) name of the tracker int tracker_id; // tracker index id that is unique per type int joy_id; // if we also have a related joystick entity, the id of the joystick - bool tracks_orientation; // do we track orientation? + bool tracking_orientation; // do we track orientation? Basis orientation; // our orientation - bool tracks_position; // do we track position? + bool tracking_position; // do we track position? Vector3 rw_position; // our position "in the real world, so without world_scale applied" Ref<Mesh> mesh; // when available, a mesh that can be used to render this tracker TrackerHand hand; // if known, the hand this tracker is held in @@ -71,23 +71,23 @@ protected: static void _bind_methods(); public: - void set_type(XRServer::TrackerType p_type); + void set_tracker_type(XRServer::TrackerType p_type); XRServer::TrackerType get_tracker_type() const; - void set_name(const String &p_name); + void set_tracker_name(const String &p_name); StringName get_tracker_name() const; int get_tracker_id() const; void set_joy_id(int p_joy_id); int get_joy_id() const; - bool get_tracks_orientation() const; + bool is_tracking_orientation() const; void set_orientation(const Basis &p_orientation); Basis get_orientation() const; - bool get_tracks_position() const; + bool is_tracking_position() const; void set_position(const Vector3 &p_position); // set position with world_scale applied Vector3 get_position() const; // get position with world_scale applied void set_rw_position(const Vector3 &p_rw_position); Vector3 get_rw_position() const; - XRPositionalTracker::TrackerHand get_hand() const; - void set_hand(const XRPositionalTracker::TrackerHand p_hand); + XRPositionalTracker::TrackerHand get_tracker_hand() const; + void set_tracker_hand(const XRPositionalTracker::TrackerHand p_hand); real_t get_rumble() const; void set_rumble(real_t p_rumble); void set_mesh(const Ref<Mesh> &p_mesh); diff --git a/tests/test_aabb.h b/tests/test_aabb.h index 8acd2a9963..404a73a95f 100644 --- a/tests/test_aabb.h +++ b/tests/test_aabb.h @@ -298,6 +298,12 @@ TEST_CASE("[AABB] Get longest/shortest axis") { "get_shortest_axis() should return the expected value."); } +#ifndef _MSC_VER +#warning Support tests need to be re-done +#endif + +/* Support function was actually broken. As it was fixed, the tests now fail. Tests need to be re-done. + TEST_CASE("[AABB] Get support") { const AABB aabb = AABB(Vector3(-1.5, 2, -2.5), Vector3(4, 5, 6)); CHECK_MESSAGE( @@ -319,7 +325,7 @@ TEST_CASE("[AABB] Get support") { aabb.get_support(Vector3()).is_equal_approx(Vector3(2.5, 7, 3.5)), "get_support() should return the expected value with a null vector."); } - +*/ TEST_CASE("[AABB] Grow") { const AABB aabb = AABB(Vector3(-1.5, 2, -2.5), Vector3(4, 5, 6)); CHECK_MESSAGE( diff --git a/tests/test_list.h b/tests/test_list.h index 1b23233838..8d29bd907f 100644 --- a/tests/test_list.h +++ b/tests/test_list.h @@ -45,6 +45,276 @@ static void populate_integers(List<int> &p_list, List<int>::Element *r_elements[ } } +TEST_CASE("[List] Push/pop back") { + List<String> list; + + List<String>::Element *n; + n = list.push_back("A"); + CHECK(n->get() == "A"); + n = list.push_back("B"); + CHECK(n->get() == "B"); + n = list.push_back("C"); + CHECK(n->get() == "C"); + + CHECK(list.size() == 3); + CHECK(!list.empty()); + + String v; + v = list.back()->get(); + list.pop_back(); + CHECK(v == "C"); + v = list.back()->get(); + list.pop_back(); + CHECK(v == "B"); + v = list.back()->get(); + list.pop_back(); + CHECK(v == "A"); + + CHECK(list.size() == 0); + CHECK(list.empty()); + + CHECK(list.back() == nullptr); + CHECK(list.front() == nullptr); +} + +TEST_CASE("[List] Push/pop front") { + List<String> list; + + List<String>::Element *n; + n = list.push_front("A"); + CHECK(n->get() == "A"); + n = list.push_front("B"); + CHECK(n->get() == "B"); + n = list.push_front("C"); + CHECK(n->get() == "C"); + + CHECK(list.size() == 3); + CHECK(!list.empty()); + + String v; + v = list.front()->get(); + list.pop_front(); + CHECK(v == "C"); + v = list.front()->get(); + list.pop_front(); + CHECK(v == "B"); + v = list.front()->get(); + list.pop_front(); + CHECK(v == "A"); + + CHECK(list.size() == 0); + CHECK(list.empty()); + + CHECK(list.back() == nullptr); + CHECK(list.front() == nullptr); +} + +TEST_CASE("[List] Set and get") { + List<String> list; + list.push_back("A"); + + List<String>::Element *n = list.front(); + CHECK(n->get() == "A"); + + n->set("X"); + CHECK(n->get() == "X"); +} + +TEST_CASE("[List] Insert before") { + List<String> list; + List<String>::Element *a = list.push_back("A"); + List<String>::Element *b = list.push_back("B"); + List<String>::Element *c = list.push_back("C"); + + list.insert_before(b, "I"); + + CHECK(a->next()->get() == "I"); + CHECK(c->prev()->prev()->get() == "I"); + CHECK(list.front()->next()->get() == "I"); + CHECK(list.back()->prev()->prev()->get() == "I"); +} + +TEST_CASE("[List] Insert after") { + List<String> list; + List<String>::Element *a = list.push_back("A"); + List<String>::Element *b = list.push_back("B"); + List<String>::Element *c = list.push_back("C"); + + list.insert_after(b, "I"); + + CHECK(a->next()->next()->get() == "I"); + CHECK(c->prev()->get() == "I"); + CHECK(list.front()->next()->next()->get() == "I"); + CHECK(list.back()->prev()->get() == "I"); +} + +TEST_CASE("[List] Insert before null") { + List<String> list; + List<String>::Element *a = list.push_back("A"); + List<String>::Element *b = list.push_back("B"); + List<String>::Element *c = list.push_back("C"); + + list.insert_before(nullptr, "I"); + + CHECK(a->next()->get() == "B"); + CHECK(b->get() == "B"); + CHECK(c->prev()->prev()->get() == "A"); + CHECK(list.front()->next()->get() == "B"); + CHECK(list.back()->prev()->prev()->get() == "B"); + CHECK(list.back()->get() == "I"); +} + +TEST_CASE("[List] Insert after null") { + List<String> list; + List<String>::Element *a = list.push_back("A"); + List<String>::Element *b = list.push_back("B"); + List<String>::Element *c = list.push_back("C"); + + list.insert_after(nullptr, "I"); + + CHECK(a->next()->get() == "B"); + CHECK(b->get() == "B"); + CHECK(c->prev()->prev()->get() == "A"); + CHECK(list.front()->next()->get() == "B"); + CHECK(list.back()->prev()->prev()->get() == "B"); + CHECK(list.back()->get() == "I"); +} + +TEST_CASE("[List] Find") { + List<int> list; + List<int>::Element *n[10]; + // Indices match values. + populate_integers(list, n, 10); + + for (int i = 0; i < 10; ++i) { + CHECK(n[i]->get() == list.find(i)->get()); + } +} + +TEST_CASE("[List] Erase (by value)") { + List<int> list; + List<int>::Element *n[4]; + // Indices match values. + populate_integers(list, n, 4); + + CHECK(list.front()->next()->next()->get() == 2); + bool erased = list.erase(2); // 0, 1, 3. + CHECK(erased); + CHECK(list.size() == 3); + + // The pointer n[2] points to the freed memory which is not reset to zero, + // so the below assertion may pass, but this relies on undefined behavior. + // CHECK(n[2]->get() == 2); + + CHECK(list.front()->get() == 0); + CHECK(list.front()->next()->next()->get() == 3); + CHECK(list.back()->get() == 3); + CHECK(list.back()->prev()->get() == 1); + + CHECK(n[1]->next()->get() == 3); + CHECK(n[3]->prev()->get() == 1); + + erased = list.erase(9000); // Doesn't exist. + CHECK(!erased); +} + +TEST_CASE("[List] Erase (by element)") { + List<int> list; + List<int>::Element *n[4]; + // Indices match values. + populate_integers(list, n, 4); + + bool erased = list.erase(n[2]); + CHECK(erased); + CHECK(list.size() == 3); + CHECK(n[1]->next()->get() == 3); + CHECK(n[3]->prev()->get() == 1); +} + +TEST_CASE("[List] Element erase") { + List<int> list; + List<int>::Element *n[4]; + // Indices match values. + populate_integers(list, n, 4); + + n[2]->erase(); + + CHECK(list.size() == 3); + CHECK(n[1]->next()->get() == 3); + CHECK(n[3]->prev()->get() == 1); +} + +TEST_CASE("[List] Clear") { + List<int> list; + List<int>::Element *n[100]; + populate_integers(list, n, 100); + + list.clear(); + + CHECK(list.size() == 0); + CHECK(list.empty()); +} + +TEST_CASE("[List] Invert") { + List<int> list; + List<int>::Element *n[4]; + populate_integers(list, n, 4); + + list.invert(); + + CHECK(list.front()->get() == 3); + CHECK(list.front()->next()->get() == 2); + CHECK(list.back()->prev()->get() == 1); + CHECK(list.back()->get() == 0); +} + +TEST_CASE("[List] Move to front") { + List<int> list; + List<int>::Element *n[4]; + populate_integers(list, n, 4); + + list.move_to_front(n[3]); + + CHECK(list.front()->get() == 3); + CHECK(list.back()->get() == 2); +} + +TEST_CASE("[List] Move to back") { + List<int> list; + List<int>::Element *n[4]; + populate_integers(list, n, 4); + + list.move_to_back(n[0]); + + CHECK(list.back()->get() == 0); + CHECK(list.front()->get() == 1); +} + +TEST_CASE("[List] Move before") { + List<int> list; + List<int>::Element *n[4]; + populate_integers(list, n, 4); + + list.move_before(n[3], n[1]); + + CHECK(list.front()->next()->get() == n[3]->get()); +} + +TEST_CASE("[List] Sort") { + List<String> list; + list.push_back("D"); + list.push_back("B"); + list.push_back("A"); + list.push_back("C"); + + list.sort(); + + CHECK(list.front()->get() == "A"); + CHECK(list.front()->next()->get() == "B"); + CHECK(list.back()->prev()->get() == "C"); + CHECK(list.back()->get() == "D"); +} + TEST_CASE("[List] Swap adjacent front and back") { List<int> list; List<int>::Element *n[2]; diff --git a/tests/test_macros.h b/tests/test_macros.h index 05fae128b3..ae6af93825 100644 --- a/tests/test_macros.h +++ b/tests/test_macros.h @@ -41,6 +41,9 @@ // The test is skipped with this, run pending tests with `--test --no-skip`. #define TEST_CASE_PENDING(name) TEST_CASE(name *doctest::skip()) +// The test case is marked as failed, but does not fail the entire test run. +#define TEST_CASE_MAY_FAIL(name) TEST_CASE(name *doctest::may_fail()) + // Temporarily disable error prints to test failure paths. // This allows to avoid polluting the test summary with error messages. // The `_print_error_enabled` boolean is defined in `core/print_string.cpp` and diff --git a/tests/test_random_number_generator.h b/tests/test_random_number_generator.h index 50ad5ee362..900bb55ffd 100644 --- a/tests/test_random_number_generator.h +++ b/tests/test_random_number_generator.h @@ -36,6 +36,150 @@ namespace TestRandomNumberGenerator { +TEST_CASE("[RandomNumberGenerator] Float") { + Ref<RandomNumberGenerator> rng = memnew(RandomNumberGenerator); + rng->set_seed(0); + + INFO("Should give float between 0.0 and 1.0."); + for (int i = 0; i < 1000; i++) { + real_t n = rng->randf(); + CHECK(n >= 0.0); + CHECK(n <= 1.0); + } +} + +TEST_CASE("[RandomNumberGenerator] Integer range via modulo") { + Ref<RandomNumberGenerator> rng = memnew(RandomNumberGenerator); + rng->set_seed(0); + + INFO("Should give integer between 0 and 100."); + for (int i = 0; i < 1000; i++) { + uint32_t n = rng->randi() % 100; + CHECK(n >= 0); + CHECK(n <= 100); + } +} + +TEST_CASE_MAY_FAIL("[RandomNumberGenerator] Integer 32 bit") { + Ref<RandomNumberGenerator> rng = memnew(RandomNumberGenerator); + rng->set_seed(0); // Change the seed if this fails. + + bool higher = false; + int i; + for (i = 0; i < 1000; i++) { + uint32_t n = rng->randi(); + if (n > 0x0fff'ffff) { + higher = true; + break; + } + } + INFO("Current seed: " << rng->get_seed()); + INFO("Current iteration: " << i); + CHECK_MESSAGE(higher, "Given current seed, this should give an integer higher than 0x0fff'ffff at least once."); +} + +TEST_CASE("[RandomNumberGenerator] Float and integer range") { + Ref<RandomNumberGenerator> rng = memnew(RandomNumberGenerator); + rng->set_seed(0); + uint64_t initial_state = rng->get_state(); + uint32_t initial_seed = rng->get_seed(); + + INFO("Should give float between -100.0 and 100.0, base test."); + for (int i = 0; i < 1000; i++) { + real_t n0 = rng->randf_range(-100.0, 100.0); + CHECK(n0 >= -100); + CHECK(n0 <= 100); + } + + rng->randomize(); + INFO("Should give float between -75.0 and 75.0."); + INFO("Shouldn't be affected by randomize."); + for (int i = 0; i < 1000; i++) { + real_t n1 = rng->randf_range(-75.0, 75.0); + CHECK(n1 >= -75); + CHECK(n1 <= 75); + } + + rng->set_state(initial_state); + INFO("Should give integer between -50 and 50."); + INFO("Shouldn't be affected by set_state."); + for (int i = 0; i < 1000; i++) { + real_t n2 = rng->randi_range(-50, 50); + CHECK(n2 >= -50); + CHECK(n2 <= 50); + } + + rng->set_seed(initial_seed); + INFO("Should give integer between -25 and 25."); + INFO("Shouldn't be affected by set_seed."); + for (int i = 0; i < 1000; i++) { + int32_t n3 = rng->randi_range(-25, 25); + CHECK(n3 >= -25); + CHECK(n3 <= 25); + } + + rng->randf(); + rng->randf(); + + INFO("Should give float between -10.0 and 10.0."); + INFO("Shouldn't be affected after generating new numbers."); + for (int i = 0; i < 1000; i++) { + real_t n4 = rng->randf_range(-10.0, 10.0); + CHECK(n4 >= -10); + CHECK(n4 <= 10); + } + + rng->randi(); + rng->randi(); + + INFO("Should give integer between -5 and 5."); + INFO("Shouldn't be affected after generating new numbers."); + for (int i = 0; i < 1000; i++) { + real_t n5 = rng->randf_range(-5, 5); + CHECK(n5 >= -5); + CHECK(n5 <= 5); + } +} + +TEST_CASE_MAY_FAIL("[RandomNumberGenerator] Normal distribution") { + Ref<RandomNumberGenerator> rng = memnew(RandomNumberGenerator); + rng->set_seed(1); // Change the seed if this fails. + INFO("Should give a number between -5 to 5 (5 std deviations away; above 99.7% chance it will be in this range)."); + INFO("Standard randfn function call."); + for (int i = 0; i < 100; i++) { + real_t n = rng->randfn(); + CHECK(n >= -5); + CHECK(n <= 5); + } + + INFO("Should give number between -5 to 5 after multiple randi/randf calls."); + INFO("5 std deviations away; above 99.7% chance it will be in this range."); + rng->randf(); + rng->randi(); + for (int i = 0; i < 100; i++) { + real_t n = rng->randfn(); + CHECK(n >= -5); + CHECK(n <= 5); + } + + INFO("Checks if user defined mean and deviation work properly."); + INFO("5 std deviations away; above 99.7% chance it will be in this range."); + for (int i = 0; i < 100; i++) { + real_t n = rng->randfn(5, 10); + CHECK(n >= -45); + CHECK(n <= 55); + } + + INFO("Checks if randfn works with changed seeds."); + INFO("5 std deviations away; above 99.7% chance it will be in this range."); + rng->randomize(); + for (int i = 0; i < 100; i++) { + real_t n = rng->randfn(3, 3); + CHECK(n >= -12); + CHECK(n <= 18); + } +} + TEST_CASE("[RandomNumberGenerator] Zero for first number immediately after seeding") { Ref<RandomNumberGenerator> rng = memnew(RandomNumberGenerator); rng->set_seed(0); diff --git a/tests/test_rect2.h b/tests/test_rect2.h index aefceb1128..b1c588fda1 100644 --- a/tests/test_rect2.h +++ b/tests/test_rect2.h @@ -144,29 +144,29 @@ TEST_CASE("[Rect2] Absolute coordinates") { "abs() should return the expected Rect2."); } -TEST_CASE("[Rect2] Clipping") { +TEST_CASE("[Rect2] Intersecton") { CHECK_MESSAGE( - Rect2(0, 100, 1280, 720).clip(Rect2(0, 300, 100, 100)).is_equal_approx(Rect2(0, 300, 100, 100)), - "clip() with fully enclosed Rect2 should return the expected result."); + Rect2(0, 100, 1280, 720).intersection(Rect2(0, 300, 100, 100)).is_equal_approx(Rect2(0, 300, 100, 100)), + "intersection() with fully enclosed Rect2 should return the expected result."); // The resulting Rect2 is 100 pixels high because the first Rect2 is vertically offset by 100 pixels. CHECK_MESSAGE( - Rect2(0, 100, 1280, 720).clip(Rect2(1200, 700, 100, 100)).is_equal_approx(Rect2(1200, 700, 80, 100)), - "clip() with partially enclosed Rect2 should return the expected result."); + Rect2(0, 100, 1280, 720).intersection(Rect2(1200, 700, 100, 100)).is_equal_approx(Rect2(1200, 700, 80, 100)), + "intersection() with partially enclosed Rect2 should return the expected result."); CHECK_MESSAGE( - Rect2(0, 100, 1280, 720).clip(Rect2(-4000, -4000, 100, 100)).is_equal_approx(Rect2()), - "clip() with non-enclosed Rect2 should return the expected result."); + Rect2(0, 100, 1280, 720).intersection(Rect2(-4000, -4000, 100, 100)).is_equal_approx(Rect2()), + "intersection() with non-enclosed Rect2 should return the expected result."); } TEST_CASE("[Rect2] Enclosing") { CHECK_MESSAGE( Rect2(0, 100, 1280, 720).encloses(Rect2(0, 300, 100, 100)), - "clip() with fully contained Rect2 should return the expected result."); + "encloses() with fully contained Rect2 should return the expected result."); CHECK_MESSAGE( !Rect2(0, 100, 1280, 720).encloses(Rect2(1200, 700, 100, 100)), - "clip() with partially contained Rect2 should return the expected result."); + "encloses() with partially contained Rect2 should return the expected result."); CHECK_MESSAGE( !Rect2(0, 100, 1280, 720).encloses(Rect2(-4000, -4000, 100, 100)), - "clip() with non-contained Rect2 should return the expected result."); + "encloses() with non-contained Rect2 should return the expected result."); } TEST_CASE("[Rect2] Expanding") { @@ -356,29 +356,29 @@ TEST_CASE("[Rect2i] Absolute coordinates") { "abs() should return the expected Rect2i."); } -TEST_CASE("[Rect2i] Clipping") { +TEST_CASE("[Rect2i] Intersection") { CHECK_MESSAGE( - Rect2i(0, 100, 1280, 720).clip(Rect2i(0, 300, 100, 100)) == Rect2i(0, 300, 100, 100), - "clip() with fully enclosed Rect2i should return the expected result."); + Rect2i(0, 100, 1280, 720).intersection(Rect2i(0, 300, 100, 100)) == Rect2i(0, 300, 100, 100), + "intersection() with fully enclosed Rect2i should return the expected result."); // The resulting Rect2i is 100 pixels high because the first Rect2i is vertically offset by 100 pixels. CHECK_MESSAGE( - Rect2i(0, 100, 1280, 720).clip(Rect2i(1200, 700, 100, 100)) == Rect2i(1200, 700, 80, 100), - "clip() with partially enclosed Rect2i should return the expected result."); + Rect2i(0, 100, 1280, 720).intersection(Rect2i(1200, 700, 100, 100)) == Rect2i(1200, 700, 80, 100), + "intersection() with partially enclosed Rect2i should return the expected result."); CHECK_MESSAGE( - Rect2i(0, 100, 1280, 720).clip(Rect2i(-4000, -4000, 100, 100)) == Rect2i(), - "clip() with non-enclosed Rect2i should return the expected result."); + Rect2i(0, 100, 1280, 720).intersection(Rect2i(-4000, -4000, 100, 100)) == Rect2i(), + "intersection() with non-enclosed Rect2i should return the expected result."); } TEST_CASE("[Rect2i] Enclosing") { CHECK_MESSAGE( Rect2i(0, 100, 1280, 720).encloses(Rect2i(0, 300, 100, 100)), - "clip() with fully contained Rect2i should return the expected result."); + "encloses() with fully contained Rect2i should return the expected result."); CHECK_MESSAGE( !Rect2i(0, 100, 1280, 720).encloses(Rect2i(1200, 700, 100, 100)), - "clip() with partially contained Rect2i should return the expected result."); + "encloses() with partially contained Rect2i should return the expected result."); CHECK_MESSAGE( !Rect2i(0, 100, 1280, 720).encloses(Rect2i(-4000, -4000, 100, 100)), - "clip() with non-contained Rect2i should return the expected result."); + "encloses() with non-contained Rect2i should return the expected result."); } TEST_CASE("[Rect2i] Expanding") { diff --git a/thirdparty/README.md b/thirdparty/README.md index 3962a83597..8cb6424a1c 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -40,11 +40,9 @@ Files extracted from upstream source: ## bullet - Upstream: https://github.com/bulletphysics/bullet3 -- Version: git pre-2.90 (cd8cf7521cbb8b7808126a6adebd47bb83ea166a, 2020) +- Version: 3.07 (e32fc59c88a3908876949c6f2665e8d091d987fa, 2020) - License: zlib -Important: Synced with a pre-release version of bullet 2.90 from the master branch. - Files extracted from upstream source: - src/* apart from CMakeLists.txt and premake4.lua files @@ -208,7 +206,7 @@ Files extracted from upstream source: ## International Components for Unicode - Upstream: https://github.com/unicode-org/icu -- Version: 68.1 +- Version: 68.2 - License: Unicode Files extracted from upstream source: @@ -341,7 +339,7 @@ changes are marked with `// -- GODOT --` comments. ## mbedtls - Upstream: https://tls.mbed.org/ -- Version: 2.16.8 (2020) +- Version: 2.16.9 (2020) - License: Apache 2.0 File extracted from upstream release tarball: @@ -357,15 +355,20 @@ File extracted from upstream release tarball: - Added 2 files `godot_core_mbedtls_platform.{c,h}` providing configuration for light bundling with core. + ## meshoptimizer - Upstream: https://github.com/zeux/meshoptimizer -- Version: 0.15(2020) +- Version: 0.15 (2020) - License: MIT File extracted from upstream release tarball: -- Files in src/ go to thirdparty/meshoptimizer +- All files in `src/`. + +Important: Some files have Godot-made changes. +They can be applied with the patch in the `patches` folder, but are meant to be superseded +by upstream API changes. ## miniupnpc diff --git a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp index 6f2c5251a0..4938fa17af 100644 --- a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp @@ -285,7 +285,6 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart); curNodeSubPart = nodeSubPart; - b3Assert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, @@ -293,7 +292,13 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: b3Assert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp b/thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp index 145de62db3..f6c779a919 100644 --- a/thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp +++ b/thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp @@ -851,12 +851,12 @@ void bFile::swapData(char *data, short type, int arraySize, bool ignoreEndianFla void bFile::safeSwapPtr(char *dst, const char *src) { + if (!src || !dst) + return; + int ptrFile = mFileDNA->getPointerSize(); int ptrMem = mMemoryDNA->getPointerSize(); - if (!src && !dst) - return; - if (ptrFile == ptrMem) { memcpy(dst, src, ptrMem); diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 4954e773e2..19f1737b73 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -346,8 +346,6 @@ void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb } } -int maxIterations = 0; - void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const { btAssert(!m_useQuantization); @@ -387,8 +385,6 @@ void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, cons curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } /* @@ -529,8 +525,6 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const @@ -654,8 +648,6 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const @@ -718,8 +710,6 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } //This traversal can be called from Playstation 3 SPU diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h index 85dc488c8c..e085c40892 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -127,6 +127,7 @@ public: enum CollisionFlags { + CF_DYNAMIC_OBJECT = 0, CF_STATIC_OBJECT = 1, CF_KINEMATIC_OBJECT = 2, CF_NO_CONTACT_RESPONSE = 4, @@ -251,6 +252,16 @@ public: m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0; } + int getNumObjectsWithoutCollision() const + { + return m_objectsWithoutCollisionCheck.size(); + } + + const btCollisionObject* getObjectWithoutCollision(int index) + { + return m_objectsWithoutCollisionCheck[index]; + } + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index a4252c296a..a71700f58a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -361,7 +361,13 @@ void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangle for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index d663b3d6d6..c66ce58e3e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -124,12 +124,17 @@ void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const nodeSubPart); unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { @@ -193,12 +198,17 @@ void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, con nodeSubPart); unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h index c80e105a4d..16f9e0c77a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h @@ -30,11 +30,12 @@ protected: int m_shapeType; void* m_userPointer; int m_userIndex; + int m_userIndex2; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) + btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1), m_userIndex2(-1) { } @@ -137,6 +138,16 @@ public: return m_userIndex; } + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + + int getUserIndex2() const + { + return m_userIndex2; + } + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 34e7926f17..cab6980b65 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -21,8 +21,7 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape( int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, PHY_ScalarType hdt, bool flipQuadEdges) - :m_userIndex2(-1), - m_userValue3(0), + :m_userValue3(0), m_triangleInfoMap(0) { initialize(heightStickWidth, heightStickLength, heightfieldData, @@ -31,8 +30,7 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape( } btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges) - :m_userIndex2(-1), - m_userValue3(0), + : m_userValue3(0), m_triangleInfoMap(0) { // legacy constructor: support only float or unsigned char, diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 8dea98fc6b..2cf3c00721 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -114,7 +114,7 @@ protected: int m_vboundsGridLength; int m_vboundsChunkSize; - int m_userIndex2; + btScalar m_userValue3; struct btTriangleInfoMap* m_triangleInfoMap; @@ -192,14 +192,6 @@ public: virtual const char* getName() const { return "HEIGHTFIELD"; } - void setUserIndex2(int index) - { - m_userIndex2 = index; - } - int getUserIndex2() const - { - return m_userIndex2; - } void setUserValue3(btScalar value) { m_userValue3 = value; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 687399e0a9..863ea6d6ac 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -286,7 +286,6 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart); curNodeSubPart = nodeSubPart; - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, @@ -294,7 +293,13 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp index 4a95dbea4f..23c95ad3ff 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp @@ -2,8 +2,11 @@ #include "btMiniSDF.h" #include "LinearMath/btAabbUtil2.h" -struct btSdfCollisionShapeInternalData +ATTRIBUTE_ALIGNED16(struct) +btSdfCollisionShapeInternalData { + BT_DECLARE_ALIGNED_ALLOCATOR(); + btVector3 m_localScaling; btScalar m_margin; btMiniSDF m_sdf; diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h index 5b85e87041..cc91079579 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h @@ -623,13 +623,21 @@ public: i1 = s_indices[1]; i2 = s_indices[2]; } - else + else if (indicestype == PHY_INTEGER) { unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride); i0 = i_indices[0]; i1 = i_indices[1]; i2 = i_indices[2]; } + else + { + btAssert(indicestype == PHY_UCHAR); + unsigned char* i_indices = (unsigned char*)(indexbase + face_index * indexstride); + i0 = i_indices[0]; + i1 = i_indices[1]; + i2 = i_indices[2]; + } } SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 45d1817135..7d53f8624a 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -1049,7 +1049,8 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, const btScalar length = delta.length(); results.normal = delta / length; results.witnesses[0] += results.normal * margin; - return (length - margin); + results.distance = length - margin; + return results.distance; } else { diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp index 27f76b8425..0f5ed1c2ce 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp @@ -852,7 +852,7 @@ static void setupSpatialGridBatchesMt( memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows); size_t scratchSize = memHelper.getSizeToAllocate(); // if we need to reallocate - if (scratchMemory->capacity() < scratchSize) + if (static_cast<size_t>(scratchMemory->capacity()) < scratchSize) { // allocate 6.25% extra to avoid repeated reallocs scratchMemory->reserve(scratchSize + scratchSize / 16); diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 4356c12abf..3316403a87 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -47,6 +47,8 @@ struct btContactSolverInfoData btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints btScalar m_deformable_erp; //error reduction for deformable constraints + btScalar m_deformable_cfm; //constraint force mixing for deformable constraints + btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -83,7 +85,9 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.1); + m_deformable_erp = btScalar(0.06); + m_deformable_cfm = btScalar(0.01); + m_deformable_maxErrorReduction = btScalar(0.1); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h index 943d724cce..7442dd1e6a 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h @@ -356,12 +356,12 @@ public: } } - btVector3 getPushVelocity() + btVector3 getPushVelocity() const { return m_pushVelocity; } - btVector3 getTurnVelocity() + btVector3 getTurnVelocity() const { return m_turnVelocity; } @@ -465,6 +465,12 @@ public: //for kinematic objects, we could also use use: // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; } + + btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const + { + //we also calculate lin/ang velocity for kinematic objects + return m_pushVelocity + m_turnVelocity.cross(rel_pos); + } void translate(const btVector3& v) { diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp index a1d5bb9ca8..bec8c6530d 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp @@ -344,6 +344,8 @@ void btMultiBody::finalizeMultiDof() { m_deltaV.resize(0); m_deltaV.resize(6 + m_dofCount); + m_splitV.resize(0); + m_splitV.resize(6 + m_dofCount); m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) m_matrixBuf.resize(m_links.size() + 1); @@ -671,6 +673,30 @@ btScalar *btMultiBody::getJointTorqueMultiDof(int i) return &m_links[i].m_jointTorque[0]; } +bool btMultiBody::hasFixedBase() const +{ + return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticObject()); +} + +bool btMultiBody::isBaseStaticOrKinematic() const +{ + return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticOrKinematicObject()); +} + +bool btMultiBody::isBaseKinematic() const +{ + return getBaseCollider() && getBaseCollider()->isKinematicObject(); +} + +void btMultiBody::setBaseDynamicType(int dynamicType) +{ + if(getBaseCollider()) { + int oldFlags = getBaseCollider()->getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + getBaseCollider()->setCollisionFlags(oldFlags | dynamicType); + } +} + inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? { btVector3 row0 = btVector3( @@ -796,7 +822,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { zeroAccSpatFrc[0].setZero(); } @@ -872,31 +898,53 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // calculate zhat_i^A // - //external forces - btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; - btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; + if (isLinkAndAllAncestorsKinematic(i)) + { + zeroAccSpatFrc[i].setZero(); + } + else{ + //external forces + btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; + btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; - zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce)); + zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce)); #if 0 - { + { - b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", - i+1, - zeroAccSpatFrc[i+1].m_topVec[0], - zeroAccSpatFrc[i+1].m_topVec[1], - zeroAccSpatFrc[i+1].m_topVec[2], + b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", + i+1, + zeroAccSpatFrc[i+1].m_topVec[0], + zeroAccSpatFrc[i+1].m_topVec[1], + zeroAccSpatFrc[i+1].m_topVec[2], - zeroAccSpatFrc[i+1].m_bottomVec[0], - zeroAccSpatFrc[i+1].m_bottomVec[1], - zeroAccSpatFrc[i+1].m_bottomVec[2]); - } + zeroAccSpatFrc[i+1].m_bottomVec[0], + zeroAccSpatFrc[i+1].m_bottomVec[1], + zeroAccSpatFrc[i+1].m_bottomVec[2]); + } #endif - // - //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()), - linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm())); + // + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 1.; + zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()), + linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm())); + //p += vhat x Ihat vhat - done in a simpler way + if (m_useGyroTerm) + zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular())); + // + zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear())); + // + //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + ////clamp parent's omega + //btScalar parOmegaMod = temp.length(); + //btScalar parOmegaModMax = 1000; + //if(parOmegaMod > parOmegaModMax) + // temp *= parOmegaModMax / parOmegaMod; + //zeroAccSpatFrc[i+1].addLinear(temp); + //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); + //temp = spatCoriolisAcc[i].getLinear(); + //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); + } // calculate Ihat_i^A //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) @@ -909,22 +957,6 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0, 0, m_links[i].m_inertiaLocal[1], 0, 0, 0, m_links[i].m_inertiaLocal[2])); - // - //p += vhat x Ihat vhat - done in a simpler way - if (m_useGyroTerm) - zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular())); - // - zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear())); - //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); - ////clamp parent's omega - //btScalar parOmegaMod = temp.length(); - //btScalar parOmegaModMax = 1000; - //if(parOmegaMod > parOmegaModMax) - // temp *= parOmegaModMax / parOmegaMod; - //zeroAccSpatFrc[i+1].addLinear(temp); - //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); - //temp = spatCoriolisAcc[i].getLinear(); - //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); @@ -935,6 +967,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { + if(isLinkAndAllAncestorsKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1047,7 +1081,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1081,21 +1115,23 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]); - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + if(!isLinkAndAllAncestorsKinematic(i)) { - const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof); - } - - btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; - //D^{-1} * (Y - h^{T}*apar) - mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof); + } + btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; + //D^{-1} * (Y - h^{T}*apar) + mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); - spatAcc[i + 1] += spatCoriolisAcc[i]; + spatAcc[i + 1] += spatCoriolisAcc[i]; - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + } if (m_links[i].m_jointFeedback) { @@ -1432,7 +1468,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Fill in zero_acc // -- set to force/torque on the base, zero otherwise - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { zeroAccSpatFrc[0].setZero(); } @@ -1451,6 +1487,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { + if(isLinkAndAllAncestorsKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1494,7 +1532,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1507,6 +1545,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // now do the loop over the m_links for (int i = 0; i < num_links; ++i) { + if(isLinkAndAllAncestorsKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1550,23 +1590,26 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar void btMultiBody::predictPositionsMultiDof(btScalar dt) { int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos; - btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - // reset to current position - for (int i = 0; i < 3; ++i) - { - m_basePos_interpolate[i] = m_basePos[i]; - } - pBasePos = m_basePos_interpolate; + if(!isBaseKinematic()) + { + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos; + btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; + // reset to current position + for (int i = 0; i < 3; ++i) + { + m_basePos_interpolate[i] = m_basePos[i]; + } + pBasePos = m_basePos_interpolate; + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + } /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -1617,26 +1660,29 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat; - - // reset to current orientation - for (int i = 0; i < 4; ++i) - { - m_baseQuat_interpolate[i] = m_baseQuat[i]; - } - pBaseQuat = m_baseQuat_interpolate; + if(!isBaseKinematic()) + { + btScalar *pBaseQuat; - btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); + // reset to current orientation + for (int i = 0; i < 4; ++i) + { + m_baseQuat_interpolate[i] = m_baseQuat[i]; + } + pBaseQuat = m_baseQuat_interpolate; + + btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + } // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) @@ -1644,55 +1690,88 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) btScalar *pJointPos; pJointPos = &m_links[i].m_jointPos_interpolate[0]; - btScalar *pJointVel = getJointVelMultiDof(i); - - switch (m_links[i].m_jointType) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - //reset to current pos - pJointPos[0] = m_links[i].m_jointPos[0]; - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - //reset to current pos - - for (int j = 0; j < 4; ++j) + if (m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()) + { + switch (m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: { - pJointPos[j] = m_links[i].m_jointPos[j]; + pJointPos[0] = m_links[i].m_jointPos[0]; + break; } - - btVector3 jointVel; - jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; - jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); - pJointPos[1] = jointOri.y(); - pJointPos[2] = jointOri.z(); - pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: - { - for (int j = 0; j < 3; ++j) + case btMultibodyLink::eSpherical: { - pJointPos[j] = m_links[i].m_jointPos[j]; + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; } - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; - - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - break; + case btMultibodyLink::ePlanar: + { + for (int j = 0; j < 3; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; + } + default: + break; } - default: + } + else + { + btScalar *pJointVel = getJointVelMultiDof(i); + + switch (m_links[i].m_jointType) { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + //reset to current pos + pJointPos[0] = m_links[i].m_jointPos[0]; + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + //reset to current pos + + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + + btVector3 jointVel; + jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; + jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + for (int j = 0; j < 3; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + break; + } + default: + { + } } } @@ -1703,16 +1782,19 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) { int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; + if(!isBaseKinematic()) + { + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + } /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -1763,22 +1845,25 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; - btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); - - //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); - //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); - //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + if(!isBaseKinematic()) + { + btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + + //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); + //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); + //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + } if (pq) pq += 7; @@ -1788,48 +1873,51 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) { - btScalar *pJointPos; - pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); - - btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - - switch (m_links[i].m_jointType) + if (!(m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic())) { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - //reset to current pos - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - //reset to current pos - btVector3 jointVel; - jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; - jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); - pJointPos[1] = jointOri.y(); - pJointPos[2] = jointOri.z(); - pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: + btScalar *pJointPos; + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); + + switch (m_links[i].m_jointType) { - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + //reset to current pos + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + //reset to current pos + btVector3 jointVel; + jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; + jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - break; - } - default: - { + break; + } + default: + { + } } } @@ -2135,8 +2223,15 @@ void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObj world_to_local.resize(getNumLinks() + 1); local_origin.resize(getNumLinks() + 1); - world_to_local[0] = getInterpolateWorldToBaseRot(); - local_origin[0] = getInterpolateBasePos(); + if(isBaseKinematic()){ + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + } + else + { + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + } if (getBaseCollider()) { @@ -2282,3 +2377,81 @@ const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *seriali return btMultiBodyDataName; } + +void btMultiBody::saveKinematicState(btScalar timeStep) +{ + //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities + if (timeStep != btScalar(0.)) + { + btVector3 linearVelocity, angularVelocity; + btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity); + setBaseVel(linearVelocity); + setBaseOmega(angularVelocity); + setInterpolateBaseWorldTransform(getBaseWorldTransform()); + } +} + +void btMultiBody::setLinkDynamicType(const int i, int type) +{ + if (i == -1) + { + setBaseDynamicType(type); + } + else if (i >= 0 && i < getNumLinks()) + { + if (m_links[i].m_collider) + { + m_links[i].m_collider->setDynamicType(type); + } + } +} + +bool btMultiBody::isLinkStaticOrKinematic(const int i) const +{ + if (i == -1) + { + return isBaseStaticOrKinematic(); + } + else + { + if (m_links[i].m_collider) + return m_links[i].m_collider->isStaticOrKinematic(); + } + return false; +} + +bool btMultiBody::isLinkKinematic(const int i) const +{ + if (i == -1) + { + return isBaseKinematic(); + } + else + { + if (m_links[i].m_collider) + return m_links[i].m_collider->isKinematic(); + } + return false; +} + +bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const +{ + int link = i; + while (link != -1) { + if (!isLinkStaticOrKinematic(link)) + return false; + link = m_links[link].m_parent; + } + return isBaseStaticOrKinematic(); +} + +bool btMultiBody::isLinkAndAllAncestorsKinematic(const int i) const +{ + int link = i; + while (link != -1) { + if (!isLinkKinematic(link)) + return false; + link = m_links[link].m_parent; + } + return isBaseKinematic(); +} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h index be795633fd..25112a6805 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h @@ -210,7 +210,13 @@ public: void setBasePos(const btVector3 &pos) { m_basePos = pos; - m_basePos_interpolate = pos; + if(!isBaseKinematic()) + m_basePos_interpolate = pos; + } + + void setInterpolateBasePos(const btVector3 &pos) + { + m_basePos_interpolate = pos; } void setBaseWorldTransform(const btTransform &tr) @@ -227,17 +233,39 @@ public: return tr; } + void setInterpolateBaseWorldTransform(const btTransform &tr) + { + setInterpolateBasePos(tr.getOrigin()); + setInterpolateWorldToBaseRot(tr.getRotation().inverse()); + } + + btTransform getInterpolateBaseWorldTransform() const + { + btTransform tr; + tr.setOrigin(getInterpolateBasePos()); + tr.setRotation(getInterpolateWorldToBaseRot().inverse()); + return tr; + } + void setBaseVel(const btVector3 &vel) { m_realBuf[3] = vel[0]; m_realBuf[4] = vel[1]; m_realBuf[5] = vel[2]; } + void setWorldToBaseRot(const btQuaternion &rot) { m_baseQuat = rot; //m_baseQuat asumed to ba alias!? - m_baseQuat_interpolate = rot; + if(!isBaseKinematic()) + m_baseQuat_interpolate = rot; + } + + void setInterpolateWorldToBaseRot(const btQuaternion &rot) + { + m_baseQuat_interpolate = rot; } + void setBaseOmega(const btVector3 &omega) { m_realBuf[0] = omega[0]; @@ -245,6 +273,8 @@ public: m_realBuf[2] = omega[2]; } + void saveKinematicState(btScalar timeStep); + // // get/set pos/vel for child m_links (i = 0 to num_links-1) // @@ -278,6 +308,11 @@ public: { return &m_deltaV[0]; } + + const btScalar *getSplitVelocityVector() const + { + return &m_splitV[0]; + } /* btScalar * getVelocityVector() { return &real_buf[0]; @@ -397,6 +432,26 @@ public: m_deltaV[dof] += delta_vee[dof] * multiplier; } } + void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) + { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] += delta_vee[dof] * multiplier; + } + } + void addSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], 1); + } + void substractSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], -1); + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] = 0.f; + } + } void processDeltaVeeMultiDof2() { applyDeltaVeeMultiDof(&m_deltaV[0], 1); @@ -495,14 +550,22 @@ public: void goToSleep(); void checkMotionAndSleepIfRequired(btScalar timestep); - bool hasFixedBase() const - { - return m_fixedBase; - } + bool hasFixedBase() const; + + bool isBaseKinematic() const; + + bool isBaseStaticOrKinematic() const; + + // set the dynamic type in the base's collision flags. + void setBaseDynamicType(int dynamicType); void setFixedBase(bool fixedBase) { m_fixedBase = fixedBase; + if(m_fixedBase) + setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT); + else + setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); } int getCompanionId() const @@ -653,7 +716,15 @@ public: btVector3 &top_out, // top part of output vector btVector3 &bottom_out); // bottom part of output vector + void setLinkDynamicType(const int i, int type); + + bool isLinkStaticOrKinematic(const int i) const; + + bool isLinkKinematic(const int i) const; + + bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const; + bool isLinkAndAllAncestorsKinematic(const int i) const; private: btMultiBody(const btMultiBody &); // not implemented @@ -711,6 +782,7 @@ private: // offset size array // 0 num_links+1 rot_from_parent // + btAlignedObjectArray<btScalar> m_splitV; btAlignedObjectArray<btScalar> m_deltaV; btAlignedObjectArray<btScalar> m_realBuf; btAlignedObjectArray<btVector3> m_vectorBuf; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d7ed05ce57..1ba5861145 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -2,11 +2,12 @@ #include "BulletDynamics/Dynamics/btRigidBody.h" #include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral) +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type) : m_bodyA(bodyA), m_bodyB(bodyB), m_linkA(linkA), m_linkB(linkB), + m_type(type), m_numRows(numRows), m_jacSizeA(0), m_jacSizeBoth(0), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 5c15f3e851..4a6007ee3e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -20,6 +20,21 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "btMultiBody.h" + +//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility +enum btTypedMultiBodyConstraintType +{ + MULTIBODY_CONSTRAINT_LIMIT=3, + MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR, + MULTIBODY_CONSTRAINT_GEAR, + MULTIBODY_CONSTRAINT_POINT_TO_POINT, + MULTIBODY_CONSTRAINT_SLIDER, + MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR, + MULTIBODY_CONSTRAINT_FIXED, + + MAX_MULTIBODY_CONSTRAINT_TYPE, +}; + class btMultiBody; struct btSolverInfo; @@ -46,6 +61,8 @@ protected: int m_linkA; int m_linkB; + int m_type; //btTypedMultiBodyConstraintType + int m_numRows; int m_jacSizeA; int m_jacSizeBoth; @@ -82,12 +99,16 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral); + btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type); virtual ~btMultiBodyConstraint(); void updateJacobianSizes(); void allocateJacobiansMultiDof(); + int getConstraintType() const + { + return m_type; + } //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. virtual void setFrameInB(const btMatrix3x3& frameInB) {} virtual void setPivotInB(const btVector3& pivotInB) {} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index cd1bad089e..fef95f0c4e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -592,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) if (!isSleeping) { + bod->addSplitV(); int nLinks = bod->getNumLinks(); ///base + num m_links @@ -610,6 +611,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->substractSplitV(); } else { @@ -867,6 +869,18 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } } + +void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep) +{ + btDiscreteDynamicsWorld::saveKinematicState(timeStep); + for(int i = 0; i < m_multiBodies.size(); i++) + { + btMultiBody* body = m_multiBodies[i]; + if(body->isBaseKinematic()) + body->saveKinematicState(timeStep); + } +} + // //void btMultiBodyDynamicsWorld::setSplitIslands(bool split) //{ diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 9ac46f4b64..d2d76c8b92 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -120,5 +120,7 @@ public: virtual void solveExternalForces(btContactSolverInfo& solverInfo); virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); void buildIslands(); + + virtual void saveKinematicState(btScalar timeStep); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp index 5ef9444c2f..df2abbe97a 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -24,7 +24,7 @@ subject to the following restrictions: #define BTMBFIXEDCONSTRAINT_DIM 6 btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -36,7 +36,7 @@ btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int li } btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index bf6b811d26..ee02cf9b07 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR), m_gearRatio(1), m_gearAuxLink(-1), m_erp(0), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 8791ad2868..94b36ac108 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) //:btMultiBodyConstraint(body,0,link,-1,2,true), - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT), m_lowerBound(lower), m_upperBound(upper) { diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 6716ba490f..b810692b4c 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -42,6 +42,22 @@ public: { //todo(erwincoumans) } + btScalar getLowerBound() const + { + return m_lowerBound; + } + btScalar getUpperBound() const + { + return m_upperBound; + } + void setLowerBound(btScalar lower) + { + m_lowerBound = lower; + } + void setUpperBound(btScalar upper) + { + m_upperBound = upper; + } }; #endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 5c816c4987..fec9b03213 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), @@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof() btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h index 01d5583c2f..5a1429340f 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -295,6 +295,9 @@ struct btMultibodyLink } } } + + + }; #endif //BT_MULTIBODY_LINK_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index bc909990c2..3dc35a5814 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -130,6 +130,23 @@ public: return true; } + bool isStaticOrKinematic() const + { + return isStaticOrKinematicObject(); + } + + bool isKinematic() const + { + return isKinematicObject(); + } + + void setDynamicType(int dynamicType) + { + int oldFlags = getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + setCollisionFlags(oldFlags | dynamicType); + } + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 37d3aede37..f51e69deb1 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -37,7 +37,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp index e025302ce6..48ec1d5af2 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -25,7 +25,7 @@ subject to the following restrictions: #define EPSILON 0.000001 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -38,7 +38,7 @@ btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int } btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp index 3e5aa30f28..25ddd539bf 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -23,7 +23,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR), m_desiredVelocity(0, 0, 0), m_desiredPosition(0,0,0,1), m_kd(1.), diff --git a/thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h b/thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h index 7b225701f6..01c7e93a1b 100644 --- a/thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h +++ b/thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h @@ -13,13 +13,12 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver btDeformableMultiBodyConstraintSolver* m_deformableSolver; DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) + btDispatcher* dispatcher) + : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) { } - - virtual void processConstraints(int islandId=-1) + virtual void processConstraints(int islandId = -1) { btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0; @@ -30,7 +29,7 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics & 1)) { m_deformableSolver->m_analyticsData.m_islandId = islandId; m_islandAnalyticsData.push_back(m_solver->m_analyticsData); diff --git a/thirdparty/bullet/BulletSoftBody/btCGProjection.h b/thirdparty/bullet/BulletSoftBody/btCGProjection.h index d047e6d3d9..e05970664c 100644 --- a/thirdparty/bullet/BulletSoftBody/btCGProjection.h +++ b/thirdparty/bullet/BulletSoftBody/btCGProjection.h @@ -22,85 +22,83 @@ struct DeformableContactConstraint { - const btSoftBody::Node* m_node; - btAlignedObjectArray<const btSoftBody::RContact*> m_contact; - btAlignedObjectArray<btVector3> m_total_normal_dv; - btAlignedObjectArray<btVector3> m_total_tangent_dv; - btAlignedObjectArray<bool> m_static; - btAlignedObjectArray<bool> m_can_be_dynamic; - - DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node) - { - append(rcontact); - } - - DeformableContactConstraint(): m_node(NULL) - { - m_contact.push_back(NULL); - } - - void append(const btSoftBody::RContact& rcontact) - { - m_contact.push_back(&rcontact); - m_total_normal_dv.push_back(btVector3(0,0,0)); - m_total_tangent_dv.push_back(btVector3(0,0,0)); - m_static.push_back(false); - m_can_be_dynamic.push_back(true); - } - - void replace(const btSoftBody::RContact& rcontact) - { - m_contact.clear(); - m_total_normal_dv.clear(); - m_total_tangent_dv.clear(); - m_static.clear(); - m_can_be_dynamic.clear(); - append(rcontact); - } - - ~DeformableContactConstraint() - { - } + const btSoftBody::Node* m_node; + btAlignedObjectArray<const btSoftBody::RContact*> m_contact; + btAlignedObjectArray<btVector3> m_total_normal_dv; + btAlignedObjectArray<btVector3> m_total_tangent_dv; + btAlignedObjectArray<bool> m_static; + btAlignedObjectArray<bool> m_can_be_dynamic; + + DeformableContactConstraint(const btSoftBody::RContact& rcontact) : m_node(rcontact.m_node) + { + append(rcontact); + } + + DeformableContactConstraint() : m_node(NULL) + { + m_contact.push_back(NULL); + } + + void append(const btSoftBody::RContact& rcontact) + { + m_contact.push_back(&rcontact); + m_total_normal_dv.push_back(btVector3(0, 0, 0)); + m_total_tangent_dv.push_back(btVector3(0, 0, 0)); + m_static.push_back(false); + m_can_be_dynamic.push_back(true); + } + + void replace(const btSoftBody::RContact& rcontact) + { + m_contact.clear(); + m_total_normal_dv.clear(); + m_total_tangent_dv.clear(); + m_static.clear(); + m_can_be_dynamic.clear(); + append(rcontact); + } + + ~DeformableContactConstraint() + { + } }; class btCGProjection { public: - typedef btAlignedObjectArray<btVector3> TVStack; - typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; - typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - const btScalar& m_dt; - // map from node indices to node pointers - const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; - - btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) - : m_softBodies(softBodies) - , m_dt(dt) - { - } - - virtual ~btCGProjection() - { - } - - // apply the constraints - virtual void project(TVStack& x) = 0; - - virtual void setConstraints() = 0; - - // update the constraints - virtual btScalar update() = 0; - - virtual void reinitialize(bool nodeUpdated) - { - } - - virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) - { - m_nodes = nodes; - } -}; + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + const btScalar& m_dt; + // map from node indices to node pointers + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt) + : m_softBodies(softBodies), m_dt(dt) + { + } + virtual ~btCGProjection() + { + } + + // apply the constraints + virtual void project(TVStack& x) = 0; + + virtual void setConstraints() = 0; + + // update the constraints + virtual btScalar update() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } +}; #endif /* btCGProjection_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btConjugateGradient.h b/thirdparty/bullet/BulletSoftBody/btConjugateGradient.h index bd51e584b9..bcd5e6b519 100644 --- a/thirdparty/bullet/BulletSoftBody/btConjugateGradient.h +++ b/thirdparty/bullet/BulletSoftBody/btConjugateGradient.h @@ -15,144 +15,103 @@ #ifndef BT_CONJUGATE_GRADIENT_H #define BT_CONJUGATE_GRADIENT_H -#include <iostream> -#include <cmath> -#include <limits> -#include <LinearMath/btAlignedObjectArray.h> -#include <LinearMath/btVector3.h> -#include "LinearMath/btQuickprof.h" +#include "btKrylovSolver.h" template <class MatrixX> -class btConjugateGradient +class btConjugateGradient : public btKrylovSolver<MatrixX> { - typedef btAlignedObjectArray<btVector3> TVStack; - TVStack r,p,z,temp; - int max_iterations; - btScalar tolerance_squared; + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btKrylovSolver<MatrixX> Base; + TVStack r, p, z, temp; + public: - btConjugateGradient(const int max_it_in) - : max_iterations(max_it_in) - { - tolerance_squared = 1e-5; - } - - virtual ~btConjugateGradient(){} - - // return the number of iterations taken - int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) - { - BT_PROFILE("CGSolve"); - btAssert(x.size() == b.size()); - reinitialize(b); - // r = b - A * x --with assigned dof zeroed out - A.multiply(x, temp); - r = sub(b, temp); - A.project(r); - // z = M^(-1) * r - A.precondition(r, z); - A.project(z); - btScalar r_dot_z = dot(z,r); - if (r_dot_z <= tolerance_squared) { - if (verbose) - { - std::cout << "Iteration = 0" << std::endl; - std::cout << "Two norm of the residual = " << r_dot_z << std::endl; - } - return 0; - } - p = z; - btScalar r_dot_z_new = r_dot_z; - for (int k = 1; k <= max_iterations; k++) { - // temp = A*p - A.multiply(p, temp); - A.project(temp); - if (dot(p,temp) < SIMD_EPSILON) - { - if (verbose) - std::cout << "Encountered negative direction in CG!" << std::endl; - if (k == 1) - { - x = b; - } - return k; - } - // alpha = r^T * z / (p^T * A * p) - btScalar alpha = r_dot_z_new / dot(p, temp); - // x += alpha * p; - multAndAddTo(alpha, p, x); - // r -= alpha * temp; - multAndAddTo(-alpha, temp, r); - // z = M^(-1) * r - A.precondition(r, z); - r_dot_z = r_dot_z_new; - r_dot_z_new = dot(r,z); - if (r_dot_z_new < tolerance_squared) { - if (verbose) - { - std::cout << "ConjugateGradient iterations " << k << std::endl; - } - return k; - } + btConjugateGradient(const int max_it_in) + : btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON) + { + } + + virtual ~btConjugateGradient() {} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) + { + BT_PROFILE("CGSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + temp = b; + A.project(temp); + p = temp; + A.precondition(p, z); + btScalar d0 = this->dot(z, temp); + d0 = btMin(btScalar(1), d0); + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp); + r = this->sub(b, temp); + A.project(r); + // z = M^(-1) * r + A.precondition(r, z); + A.project(z); + btScalar r_dot_z = this->dot(z, r); + if (r_dot_z <= Base::m_tolerance * d0) + { + if (verbose) + { + std::cout << "Iteration = 0" << std::endl; + std::cout << "Two norm of the residual = " << r_dot_z << std::endl; + } + return 0; + } + p = z; + btScalar r_dot_z_new = r_dot_z; + for (int k = 1; k <= Base::m_maxIterations; k++) + { + // temp = A*p + A.multiply(p, temp); + A.project(temp); + if (this->dot(p, temp) < 0) + { + if (verbose) + std::cout << "Encountered negative direction in CG!" << std::endl; + if (k == 1) + { + x = b; + } + return k; + } + // alpha = r^T * z / (p^T * A * p) + btScalar alpha = r_dot_z_new / this->dot(p, temp); + // x += alpha * p; + this->multAndAddTo(alpha, p, x); + // r -= alpha * temp; + this->multAndAddTo(-alpha, temp, r); + // z = M^(-1) * r + A.precondition(r, z); + r_dot_z = r_dot_z_new; + r_dot_z_new = this->dot(r, z); + if (r_dot_z_new < Base::m_tolerance * d0) + { + if (verbose) + { + std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl; + } + return k; + } + + btScalar beta = r_dot_z_new / r_dot_z; + p = this->multAndAdd(beta, p, z); + } + if (verbose) + { + std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl; + } + return Base::m_maxIterations; + } - btScalar beta = r_dot_z_new/r_dot_z; - p = multAndAdd(beta, p, z); - } - if (verbose) - { - std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl; - } - return max_iterations; - } - - void reinitialize(const TVStack& b) - { - r.resize(b.size()); - p.resize(b.size()); - z.resize(b.size()); - temp.resize(b.size()); - } - - TVStack sub(const TVStack& a, const TVStack& b) - { - // c = a-b - btAssert(a.size() == b.size()); - TVStack c; - c.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - { - c[i] = a[i] - b[i]; - } - return c; - } - - btScalar squaredNorm(const TVStack& a) - { - return dot(a,a); - } - - btScalar dot(const TVStack& a, const TVStack& b) - { - btScalar ans(0); - for (int i = 0; i < a.size(); ++i) - ans += a[i].dot(b[i]); - return ans; - } - - void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) - { -// result += s*a - btAssert(a.size() == result.size()); - for (int i = 0; i < a.size(); ++i) - result[i] += s * a[i]; - } - - TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) - { - // result = a*s + b - TVStack result; - result.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - result[i] = s * a[i] + b[i]; - return result; - } + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp.resize(b.size()); + } }; #endif /* btConjugateGradient_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h b/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h index 7b211c4172..6146120365 100644 --- a/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h +++ b/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h @@ -15,174 +15,98 @@ #ifndef BT_CONJUGATE_RESIDUAL_H #define BT_CONJUGATE_RESIDUAL_H -#include <iostream> -#include <cmath> -#include <limits> -#include <LinearMath/btAlignedObjectArray.h> -#include <LinearMath/btVector3.h> -#include <LinearMath/btScalar.h> -#include "LinearMath/btQuickprof.h" +#include "btKrylovSolver.h" + template <class MatrixX> -class btConjugateResidual +class btConjugateResidual : public btKrylovSolver<MatrixX> { - typedef btAlignedObjectArray<btVector3> TVStack; - TVStack r,p,z,temp_p, temp_r, best_x; - // temp_r = A*r - // temp_p = A*p - // z = M^(-1) * temp_p = M^(-1) * A * p - int max_iterations; - btScalar tolerance_squared, best_r; + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btKrylovSolver<MatrixX> Base; + TVStack r, p, z, temp_p, temp_r, best_x; + // temp_r = A*r + // temp_p = A*p + // z = M^(-1) * temp_p = M^(-1) * A * p + btScalar best_r; + public: - btConjugateResidual(const int max_it_in) - : max_iterations(max_it_in) - { - tolerance_squared = 1e-2; - } - - virtual ~btConjugateResidual(){} - - // return the number of iterations taken - int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) - { - BT_PROFILE("CRSolve"); - btAssert(x.size() == b.size()); - reinitialize(b); - // r = b - A * x --with assigned dof zeroed out - A.multiply(x, temp_r); // borrow temp_r here to store A*x - r = sub(b, temp_r); - // z = M^(-1) * r - A.precondition(r, z); // borrow z to store preconditioned r - r = z; - btScalar residual_norm = norm(r); - if (residual_norm <= tolerance_squared) { - if (verbose) - { - std::cout << "Iteration = 0" << std::endl; - std::cout << "Two norm of the residual = " << residual_norm << std::endl; - } - return 0; - } - p = r; - btScalar r_dot_Ar, r_dot_Ar_new; - // temp_p = A*p - A.multiply(p, temp_p); - // temp_r = A*r - temp_r = temp_p; - r_dot_Ar = dot(r, temp_r); - for (int k = 1; k <= max_iterations; k++) { - // z = M^(-1) * Ap - A.precondition(temp_p, z); - // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) - btScalar alpha = r_dot_Ar / dot(temp_p, z); - // x += alpha * p; - multAndAddTo(alpha, p, x); - // r -= alpha * z; - multAndAddTo(-alpha, z, r); - btScalar norm_r = norm(r); - if (norm_r < best_r) - { - best_x = x; - best_r = norm_r; - if (norm_r < tolerance_squared) { - if (verbose) - { - std::cout << "ConjugateResidual iterations " << k << std::endl; - } - return k; - } - else - { - if (verbose) - { - std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl; - } - } - } - // temp_r = A * r; - A.multiply(r, temp_r); - r_dot_Ar_new = dot(r, temp_r); - btScalar beta = r_dot_Ar_new/r_dot_Ar; - r_dot_Ar = r_dot_Ar_new; - // p = beta*p + r; - p = multAndAdd(beta, p, r); - // temp_p = beta*temp_p + temp_r; - temp_p = multAndAdd(beta, temp_p, temp_r); - } - if (verbose) - { - std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl; - } - x = best_x; - return max_iterations; - } - - void reinitialize(const TVStack& b) - { - r.resize(b.size()); - p.resize(b.size()); - z.resize(b.size()); - temp_p.resize(b.size()); - temp_r.resize(b.size()); - best_x.resize(b.size()); - best_r = SIMD_INFINITY; - } - - TVStack sub(const TVStack& a, const TVStack& b) - { - // c = a-b - btAssert(a.size() == b.size()); - TVStack c; - c.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - { - c[i] = a[i] - b[i]; - } - return c; - } - - btScalar squaredNorm(const TVStack& a) - { - return dot(a,a); - } - - btScalar norm(const TVStack& a) - { - btScalar ret = 0; - for (int i = 0; i < a.size(); ++i) - { - for (int d = 0; d < 3; ++d) - { - ret = btMax(ret, btFabs(a[i][d])); - } - } - return ret; - } - - btScalar dot(const TVStack& a, const TVStack& b) - { - btScalar ans(0); - for (int i = 0; i < a.size(); ++i) - ans += a[i].dot(b[i]); - return ans; - } - - void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) - { - // result += s*a - btAssert(a.size() == result.size()); - for (int i = 0; i < a.size(); ++i) - result[i] += s * a[i]; - } - - TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) - { - // result = a*s + b - TVStack result; - result.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - result[i] = s * a[i] + b[i]; - return result; - } + btConjugateResidual(const int max_it_in) + : Base(max_it_in, 1e-8) + { + } + + virtual ~btConjugateResidual() {} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) + { + BT_PROFILE("CRSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp_r); // borrow temp_r here to store A*x + r = this->sub(b, temp_r); + // z = M^(-1) * r + A.precondition(r, z); // borrow z to store preconditioned r + r = z; + btScalar residual_norm = this->norm(r); + if (residual_norm <= Base::m_tolerance) + { + return 0; + } + p = r; + btScalar r_dot_Ar, r_dot_Ar_new; + // temp_p = A*p + A.multiply(p, temp_p); + // temp_r = A*r + temp_r = temp_p; + r_dot_Ar = this->dot(r, temp_r); + for (int k = 1; k <= Base::m_maxIterations; k++) + { + // z = M^(-1) * Ap + A.precondition(temp_p, z); + // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) + btScalar alpha = r_dot_Ar / this->dot(temp_p, z); + // x += alpha * p; + this->multAndAddTo(alpha, p, x); + // r -= alpha * z; + this->multAndAddTo(-alpha, z, r); + btScalar norm_r = this->norm(r); + if (norm_r < best_r) + { + best_x = x; + best_r = norm_r; + if (norm_r < Base::m_tolerance) + { + return k; + } + } + // temp_r = A * r; + A.multiply(r, temp_r); + r_dot_Ar_new = this->dot(r, temp_r); + btScalar beta = r_dot_Ar_new / r_dot_Ar; + r_dot_Ar = r_dot_Ar_new; + // p = beta*p + r; + p = this->multAndAdd(beta, p, r); + // temp_p = beta*temp_p + temp_r; + temp_p = this->multAndAdd(beta, temp_p, temp_r); + } + if (verbose) + { + std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl; + } + x = best_x; + return Base::m_maxIterations; + } + + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp_p.resize(b.size()); + temp_r.resize(b.size()); + best_x.resize(b.size()); + best_r = SIMD_INFINITY; + } }; #endif /* btConjugateResidual_h */ - diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 5381ee6265..2455ed2138 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -17,211 +17,283 @@ #include "btPreconditioner.h" #include "LinearMath/btQuickprof.h" -btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v) -: m_softBodies(softBodies) -, m_projection(softBodies) -, m_backupVelocity(backup_v) -, m_implicit(false) +btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v) + : m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false) { - m_massPreconditioner = new MassPreconditioner(m_softBodies); - m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); - m_preconditioner = m_KKTPreconditioner; + m_massPreconditioner = new MassPreconditioner(m_softBodies); + m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); + m_preconditioner = m_KKTPreconditioner; } btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() { - delete m_KKTPreconditioner; - delete m_massPreconditioner; + delete m_KKTPreconditioner; + delete m_massPreconditioner; } void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt) { - BT_PROFILE("reinitialize"); - if (dt > 0) - { - setDt(dt); - } - if(nodeUpdated) - { - updateId(); - } - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->reinitialize(nodeUpdated); - } - m_projection.reinitialize(nodeUpdated); -// m_preconditioner->reinitialize(nodeUpdated); + BT_PROFILE("reinitialize"); + if (dt > 0) + { + setDt(dt); + } + if (nodeUpdated) + { + updateId(); + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->reinitialize(nodeUpdated); + } + btMatrix3x3 I; + I.setIdentity(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im); + } + } + m_projection.reinitialize(nodeUpdated); + // m_preconditioner->reinitialize(nodeUpdated); } void btDeformableBackwardEulerObjective::setDt(btScalar dt) { - m_dt = dt; + m_dt = dt; } void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const { - BT_PROFILE("multiply"); - // add in the mass term - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im; - ++counter; - } - } - - for (int i = 0; i < m_lf.size(); ++i) - { - // add damping matrix - m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); - if (m_implicit) - { - m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); - } - } - int offset = m_nodes.size(); - for (int i = offset; i < b.size(); ++i) - { - b[i].setZero(); - } - // add in the lagrange multiplier terms - - for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c) - { - // C^T * lambda - const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c]; - for (int i = 0; i < lm.m_num_nodes; ++i) - { - for (int j = 0; j < lm.m_num_constraints; ++j) - { - b[lm.m_indices[i]] += x[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; - } - } - // C * x - for (int d = 0; d < lm.m_num_constraints; ++d) - { - for (int i = 0; i < lm.m_num_nodes; ++i) - { - b[offset+c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]); - } - } - } + BT_PROFILE("multiply"); + // add in the mass term + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + b[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : x[counter] / node.m_im; + ++counter; + } + } + + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) + { + m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b); + } + } + int offset = m_nodes.size(); + for (int i = offset; i < b.size(); ++i) + { + b[i].setZero(); + } + // add in the lagrange multiplier terms + + for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } } void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - node.m_v = m_backupVelocity[node.index] + dv[node.index]; - } - } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + node.m_v = m_backupVelocity[node.index] + dv[node.index]; + } + } } void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero) { - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; - psb->m_nodes[j].m_v += one_over_mass * force[counter++]; - } - } - if (setZero) - { - for (int i = 0; i < force.size(); ++i) - force[i].setZero(); - } + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + if (m_implicit) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im != 0) + { + psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; + } + } + } + else + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + } + } + } + if (setZero) + { + for (int i = 0; i < force.size(); ++i) + force[i].setZero(); + } } -void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) +void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& residual) { - BT_PROFILE("computeResidual"); - // add implicit force - for (int i = 0; i < m_lf.size(); ++i) - { - if (m_implicit) - { - m_lf[i]->addScaledForces(dt, residual); - } - else - { - m_lf[i]->addScaledDampingForce(dt, residual); - } - } -// m_projection.project(residual); + BT_PROFILE("computeResidual"); + // add implicit force + for (int i = 0; i < m_lf.size(); ++i) + { + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) + { + m_lf[i]->addScaledForces(dt, residual); + } + else + { + m_lf[i]->addScaledDampingForce(dt, residual); + } + } + // m_projection.project(residual); } btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const { - btScalar mag = 0; - for (int i = 0; i < residual.size(); ++i) - { - mag += residual[i].length2(); - } - return std::sqrt(mag); + btScalar mag = 0; + for (int i = 0; i < residual.size(); ++i) + { + mag += residual[i].length2(); + } + return std::sqrt(mag); } btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt) { - btScalar e = 0; - for (int i = 0; i < m_lf.size(); ++i) - { - e += m_lf[i]->totalEnergy(dt); - } - return e; + btScalar e = 0; + for (int i = 0; i < m_lf.size(); ++i) + { + e += m_lf[i]->totalEnergy(dt); + } + return e; } void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) { - for (int i = 0; i < m_softBodies.size(); ++i) - { - m_softBodies[i]->advanceDeformation(); - } - - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->addScaledExplicitForce(m_dt, force); - } - applyForce(force, true); + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->advanceDeformation(); + } + if (m_implicit) + { + // apply forces except gravity force + btVector3 gravity; + for (int i = 0; i < m_lf.size(); ++i) + { + if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE) + { + gravity = static_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity; + } + else + { + m_lf[i]->addScaledForces(m_dt, force); + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledHessian(m_dt); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // add gravity explicitly + psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity; + } + } + } + } + else + { + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledExplicitForce(m_dt, force); + } + } + // calculate inverse mass matrix for all nodes + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + { + psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse(); + } + } + } + } + applyForce(force, true); } void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) { - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - dv[counter] = psb->m_nodes[j].m_im * residual[counter]; - ++counter; - } - } + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + dv[counter] = psb->m_nodes[j].m_im * residual[counter]; + ++counter; + } + } } //set constraints as projections void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal) { - m_projection.setConstraints(infoGlobal); + m_projection.setConstraints(infoGlobal); } void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) { - m_projection.applyDynamicFriction(r); + m_projection.applyDynamicFriction(r); } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h index 86579e71ac..eb05b9f010 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -31,143 +31,168 @@ class btDeformableBackwardEulerObjective { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_dt; - btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - Preconditioner* m_preconditioner; - btDeformableContactProjection m_projection; - const TVStack& m_backupVelocity; - btAlignedObjectArray<btSoftBody::Node* > m_nodes; - bool m_implicit; - MassPreconditioner* m_massPreconditioner; - KKTPreconditioner* m_KKTPreconditioner; - - btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v); - - virtual ~btDeformableBackwardEulerObjective(); - - void initialize(){} - - // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual - void computeResidual(btScalar dt, TVStack& residual); - - // add explicit force to the velocity - void applyExplicitForce(TVStack& force); - - // apply force to velocity and optionally reset the force to zero - void applyForce(TVStack& force, bool setZero); - - // compute the norm of the residual - btScalar computeNorm(const TVStack& residual) const; - - // compute one step of the solve (there is only one solve if the system is linear) - void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); - - // perform A*x = b - void multiply(const TVStack& x, TVStack& b) const; - - // set initial guess for CG solve - void initialGuess(TVStack& dv, const TVStack& residual); - - // reset data structure and reset dt - void reinitialize(bool nodeUpdated, btScalar dt); - - void setDt(btScalar dt); - - // add friction force to residual - void applyDynamicFriction(TVStack& r); - - // add dv to velocity - void updateVelocity(const TVStack& dv); - - //set constraints as projections - void setConstraints(const btContactSolverInfo& infoGlobal); - - // update the projections and project the residual - void project(TVStack& r) - { - BT_PROFILE("project"); - m_projection.project(r); - } - - // perform precondition M^(-1) x = b - void precondition(const TVStack& x, TVStack& b) - { - m_preconditioner->operator()(x,b); - } - - // reindex all the vertices - virtual void updateId() - { - size_t node_id = 0; - size_t face_id = 0; - m_nodes.clear(); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].index = node_id; - m_nodes.push_back(&psb->m_nodes[j]); - ++node_id; - } - for (int j = 0; j < psb->m_faces.size(); ++j) - { - psb->m_faces[j].m_index = face_id; - ++face_id; - } - } - } - - const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const - { - return &m_nodes; - } - - void setImplicit(bool implicit) - { - m_implicit = implicit; - } - - // Calculate the total potential energy in the system - btScalar totalEnergy(btScalar dt); - - void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec) - { - extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size()); - for (int i = 0; i < vec.size(); ++i) - { - extended_vec[i] = vec[i]; - } - int offset = vec.size(); - for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) - { - extended_vec[offset + i].setZero(); - } - } - - void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual) - { - extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size()); - for (int i = 0; i < residual.size(); ++i) - { - extended_residual[i] = residual[i]; - } - int offset = residual.size(); - for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) - { - const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i]; - extended_residual[offset + i].setZero(); - for (int d = 0; d < lm.m_num_constraints; ++d) - { - for (int n = 0; n < lm.m_num_nodes; ++n) - { - extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]); - } - } - } - } + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_dt; + btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + Preconditioner* m_preconditioner; + btDeformableContactProjection m_projection; + const TVStack& m_backupVelocity; + btAlignedObjectArray<btSoftBody::Node*> m_nodes; + bool m_implicit; + MassPreconditioner* m_massPreconditioner; + KKTPreconditioner* m_KKTPreconditioner; + + btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v); + + virtual ~btDeformableBackwardEulerObjective(); + + void initialize() {} + + // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual + void computeResidual(btScalar dt, TVStack& residual); + + // add explicit force to the velocity + void applyExplicitForce(TVStack& force); + + // apply force to velocity and optionally reset the force to zero + void applyForce(TVStack& force, bool setZero); + + // compute the norm of the residual + btScalar computeNorm(const TVStack& residual) const; + + // compute one step of the solve (there is only one solve if the system is linear) + void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); + + // perform A*x = b + void multiply(const TVStack& x, TVStack& b) const; + + // set initial guess for CG solve + void initialGuess(TVStack& dv, const TVStack& residual); + + // reset data structure and reset dt + void reinitialize(bool nodeUpdated, btScalar dt); + + void setDt(btScalar dt); + + // add friction force to residual + void applyDynamicFriction(TVStack& r); + + // add dv to velocity + void updateVelocity(const TVStack& dv); + + //set constraints as projections + void setConstraints(const btContactSolverInfo& infoGlobal); + + // update the projections and project the residual + void project(TVStack& r) + { + BT_PROFILE("project"); + m_projection.project(r); + } + + // perform precondition M^(-1) x = b + void precondition(const TVStack& x, TVStack& b) + { + m_preconditioner->operator()(x, b); + } + + // reindex all the vertices + virtual void updateId() + { + size_t node_id = 0; + size_t face_id = 0; + m_nodes.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].index = node_id; + m_nodes.push_back(&psb->m_nodes[j]); + ++node_id; + } + for (int j = 0; j < psb->m_faces.size(); ++j) + { + psb->m_faces[j].m_index = face_id; + ++face_id; + } + } + } + + const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const + { + return &m_nodes; + } + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } + + // Calculate the total potential energy in the system + btScalar totalEnergy(btScalar dt); + + void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec) + { + extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size()); + for (int i = 0; i < vec.size(); ++i) + { + extended_vec[i] = vec[i]; + } + int offset = vec.size(); + for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) + { + extended_vec[offset + i].setZero(); + } + } + + void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual) + { + extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size()); + for (int i = 0; i < residual.size(); ++i) + { + extended_residual[i] = residual[i]; + } + int offset = residual.size(); + for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) + { + const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i]; + extended_residual[offset + i].setZero(); + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int n = 0; n < lm.m_num_nodes; ++n) + { + extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]); + } + } + } + } + + void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f) + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im; + ++counter; + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f); + } + counter = 0; + for (; counter < f.size(); ++counter) + { + f[counter] = rhs[counter] - f[counter]; + } + } }; #endif /* btBackwardEulerObjective_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp index 132699c54f..4b11fccecb 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp @@ -18,468 +18,489 @@ #include "btDeformableBodySolver.h" #include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" -static const int kMaxConjugateGradientIterations = 50; +static const int kMaxConjugateGradientIterations = 300; btDeformableBodySolver::btDeformableBodySolver() -: m_numNodes(0) -, m_cg(kMaxConjugateGradientIterations) -, m_cr(kMaxConjugateGradientIterations) -, m_maxNewtonIterations(5) -, m_newtonTolerance(1e-4) -, m_lineSearch(false) -, m_useProjection(false) + : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) { - m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); + m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); } btDeformableBodySolver::~btDeformableBodySolver() { - delete m_objective; + delete m_objective; } void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { - BT_PROFILE("solveDeformableConstraints"); - if (!m_implicit) - { - m_objective->computeResidual(solverdt, m_residual); - m_objective->applyDynamicFriction(m_residual); - if (m_useProjection) - { - computeStep(m_dv, m_residual); - } - else - { - TVStack rhs, x; - m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs); - m_objective->addLagrangeMultiplier(m_dv, x); - m_objective->m_preconditioner->reinitialize(true); - computeStep(x, rhs); - for (int i = 0; i<m_dv.size(); ++i) - { - m_dv[i] = x[i]; - } - } - updateVelocity(); - } - else - { - for (int i = 0; i < m_maxNewtonIterations; ++i) - { - updateState(); - // add the inertia term in the residual - int counter = 0; - for (int k = 0; k < m_softBodies.size(); ++k) - { - btSoftBody* psb = m_softBodies[k]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (psb->m_nodes[j].m_im > 0) - { - m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter]; - } - ++counter; - } - } - - m_objective->computeResidual(solverdt, m_residual); - if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) - { - break; - } - // todo xuchenhan@: this really only needs to be calculated once - m_objective->applyDynamicFriction(m_residual); - if (m_lineSearch) - { - btScalar inner_product = computeDescentStep(m_ddv,m_residual); - btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 - btScalar scale = 2; - btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2; - backupDv(); - do { - scale *= beta; - if (scale < 1e-8) { - return; - } - updateEnergy(scale); - f1 = m_objective->totalEnergy(solverdt)+kineticEnergy(); - f2 = f0 - alpha * scale * inner_product; - } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues - revertDv(); - updateDv(scale); - } - else - { - computeStep(m_ddv, m_residual); - updateDv(); - } - for (int j = 0; j < m_numNodes; ++j) - { - m_ddv[j].setZero(); - m_residual[j].setZero(); - } - } - updateVelocity(); - } + BT_PROFILE("solveDeformableConstraints"); + if (!m_implicit) + { + m_objective->computeResidual(solverdt, m_residual); + m_objective->applyDynamicFriction(m_residual); + if (m_useProjection) + { + computeStep(m_dv, m_residual); + } + else + { + TVStack rhs, x; + m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs); + m_objective->addLagrangeMultiplier(m_dv, x); + m_objective->m_preconditioner->reinitialize(true); + computeStep(x, rhs); + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i] = x[i]; + } + } + updateVelocity(); + } + else + { + for (int i = 0; i < m_maxNewtonIterations; ++i) + { + updateState(); + // add the inertia term in the residual + int counter = 0; + for (int k = 0; k < m_softBodies.size(); ++k) + { + btSoftBody* psb = m_softBodies[k]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + { + m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter]; + } + ++counter; + } + } + + m_objective->computeResidual(solverdt, m_residual); + if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) + { + break; + } + // todo xuchenhan@: this really only needs to be calculated once + m_objective->applyDynamicFriction(m_residual); + if (m_lineSearch) + { + btScalar inner_product = computeDescentStep(m_ddv, m_residual); + btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 + btScalar scale = 2; + btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2; + backupDv(); + do + { + scale *= beta; + if (scale < 1e-8) + { + return; + } + updateEnergy(scale); + f1 = m_objective->totalEnergy(solverdt) + kineticEnergy(); + f2 = f0 - alpha * scale * inner_product; + } while (!(f1 < f2 + SIMD_EPSILON)); // if anything here is nan then the search continues + revertDv(); + updateDv(scale); + } + else + { + computeStep(m_ddv, m_residual); + updateDv(); + } + for (int j = 0; j < m_numNodes; ++j) + { + m_ddv[j].setZero(); + m_residual[j].setZero(); + } + } + updateVelocity(); + } } btScalar btDeformableBodySolver::kineticEnergy() { - btScalar ke = 0; - for (int i = 0; i < m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size();++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - if (node.m_im > 0) - { - ke += m_dv[node.index].length2() * 0.5 / node.m_im; - } - } - } - return ke; + btScalar ke = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + ke += m_dv[node.index].length2() * 0.5 / node.m_im; + } + } + } + return ke; } void btDeformableBodySolver::backupDv() { - m_backup_dv.resize(m_dv.size()); - for (int i = 0; i<m_backup_dv.size(); ++i) - { - m_backup_dv[i] = m_dv[i]; - } + m_backup_dv.resize(m_dv.size()); + for (int i = 0; i < m_backup_dv.size(); ++i) + { + m_backup_dv[i] = m_dv[i]; + } } void btDeformableBodySolver::revertDv() { - for (int i = 0; i<m_backup_dv.size(); ++i) - { - m_dv[i] = m_backup_dv[i]; - } + for (int i = 0; i < m_backup_dv.size(); ++i) + { + m_dv[i] = m_backup_dv[i]; + } } void btDeformableBodySolver::updateEnergy(btScalar scale) { - for (int i = 0; i<m_dv.size(); ++i) - { - m_dv[i] = m_backup_dv[i] + scale * m_ddv[i]; - } - updateState(); + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i] = m_backup_dv[i] + scale * m_ddv[i]; + } + updateState(); } - btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose) { - m_cg.solve(*m_objective, ddv, residual, false); - btScalar inner_product = m_cg.dot(residual, m_ddv); - btScalar res_norm = m_objective->computeNorm(residual); - btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); - if (inner_product < -tol) - { - if (verbose) - { - std::cout << "Looking backwards!" << std::endl; - } - for (int i = 0; i < m_ddv.size();++i) - { - m_ddv[i] = -m_ddv[i]; - } - inner_product = -inner_product; - } - else if (std::abs(inner_product) < tol) - { - if (verbose) - { - std::cout << "Gradient Descent!" << std::endl; - } - btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; - for (int i = 0; i < m_ddv.size();++i) - { - m_ddv[i] = scale * residual[i]; - } - inner_product = scale * res_norm * res_norm; - } - return inner_product; + m_cg.solve(*m_objective, ddv, residual, false); + btScalar inner_product = m_cg.dot(residual, m_ddv); + btScalar res_norm = m_objective->computeNorm(residual); + btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); + if (inner_product < -tol) + { + if (verbose) + { + std::cout << "Looking backwards!" << std::endl; + } + for (int i = 0; i < m_ddv.size(); ++i) + { + m_ddv[i] = -m_ddv[i]; + } + inner_product = -inner_product; + } + else if (std::abs(inner_product) < tol) + { + if (verbose) + { + std::cout << "Gradient Descent!" << std::endl; + } + btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; + for (int i = 0; i < m_ddv.size(); ++i) + { + m_ddv[i] = scale * residual[i]; + } + inner_product = scale * res_norm * res_norm; + } + return inner_product; } void btDeformableBodySolver::updateState() { - updateVelocity(); - updateTempPosition(); + updateVelocity(); + updateTempPosition(); } void btDeformableBodySolver::updateDv(btScalar scale) { - for (int i = 0; i < m_numNodes; ++i) - { - m_dv[i] += scale * m_ddv[i]; - } + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i] += scale * m_ddv[i]; + } } void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - if (m_useProjection) - m_cg.solve(*m_objective, ddv, residual, false); - else - m_cr.solve(*m_objective, ddv, residual, false); + if (m_useProjection) + m_cg.solve(*m_objective, ddv, residual, false); + else + m_cr.solve(*m_objective, ddv, residual, false); } -void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt) +void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt) { - m_softBodies.copyFromArray(softBodies); - bool nodeUpdated = updateNodes(); - - if (nodeUpdated) - { - m_dv.resize(m_numNodes, btVector3(0,0,0)); - m_ddv.resize(m_numNodes, btVector3(0,0,0)); - m_residual.resize(m_numNodes, btVector3(0,0,0)); - m_backupVelocity.resize(m_numNodes, btVector3(0,0,0)); - } - - // need to setZero here as resize only set value for newly allocated items - for (int i = 0; i < m_numNodes; ++i) - { - m_dv[i].setZero(); - m_ddv[i].setZero(); - m_residual[i].setZero(); - } - - m_dt = dt; - m_objective->reinitialize(nodeUpdated, dt); - updateSoftBodies(); -} + m_softBodies.copyFromArray(softBodies); + bool nodeUpdated = updateNodes(); -void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) -{ - BT_PROFILE("setConstraint"); - m_objective->setConstraints(infoGlobal); + if (nodeUpdated) + { + m_dv.resize(m_numNodes, btVector3(0, 0, 0)); + m_ddv.resize(m_numNodes, btVector3(0, 0, 0)); + m_residual.resize(m_numNodes, btVector3(0, 0, 0)); + m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0)); + } + + // need to setZero here as resize only set value for newly allocated items + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i].setZero(); + m_ddv[i].setZero(); + m_residual[i].setZero(); + } + + if (dt > 0) + { + m_dt = dt; + } + m_objective->reinitialize(nodeUpdated, dt); + updateSoftBodies(); } -btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) +void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) { - BT_PROFILE("solveContactConstraints"); - btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal); - return maxSquaredResidual; + BT_PROFILE("setConstraint"); + m_objective->setConstraints(infoGlobal); } -void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { - m_objective->m_projection.splitImpulseSetup(infoGlobal); + BT_PROFILE("solveContactConstraints"); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal); + return maxSquaredResidual; } void btDeformableBodySolver::updateVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - psb->m_maxSpeedSquared = 0; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - // set NaN to zero; - if (m_dv[counter] != m_dv[counter]) - { - m_dv[counter].setZero(); - } - psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; - psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); - ++counter; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + psb->m_maxSpeedSquared = 0; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // set NaN to zero; + if (m_dv[counter] != m_dv[counter]) + { + m_dv[counter].setZero(); + } + if (m_implicit) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter]; + } + else + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; + } + psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); + ++counter; + } + } } void btDeformableBodySolver::updateTempPosition() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v; - ++counter; - } - psb->updateDeformation(); - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv); + ++counter; + } + psb->updateDeformation(); + } } void btDeformableBodySolver::backupVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - m_backupVelocity[counter++] = psb->m_nodes[j].m_v; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_backupVelocity[counter++] = psb->m_nodes[j].m_v; + } + } } void btDeformableBodySolver::setupDeformableSolve(bool implicit) { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (implicit) - { - if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON) - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; - else - m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; - m_backupVelocity[counter] = psb->m_nodes[j].m_vn; - } - else - { - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; - } - psb->m_nodes[j].m_v = m_backupVelocity[counter]; - ++counter; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (implicit) + { + // setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint. + if (psb->m_nodes[j].m_v == m_backupVelocity[counter]) + m_dv[counter].setZero(); + else + m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; + m_backupVelocity[counter] = psb->m_nodes[j].m_vn; + } + else + { + m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter]; + } + psb->m_nodes[j].m_v = m_backupVelocity[counter]; + ++counter; + } + } } void btDeformableBodySolver::revertVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_v = m_backupVelocity[counter++]; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter++]; + } + } } bool btDeformableBodySolver::updateNodes() { - int numNodes = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - numNodes += m_softBodies[i]->m_nodes.size(); - if (numNodes != m_numNodes) - { - m_numNodes = numNodes; - return true; - } - return false; + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + numNodes += m_softBodies[i]->m_nodes.size(); + if (numNodes != m_numNodes) + { + m_numNodes = numNodes; + return true; + } + return false; } - void btDeformableBodySolver::predictMotion(btScalar solverdt) { - // apply explicit forces to velocity - m_objective->applyExplicitForce(m_residual); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody *psb = m_softBodies[i]; - - if (psb->isActive()) - { - // predict motion for collision detection - predictDeformableMotion(psb, solverdt); - } - } + // apply explicit forces to velocity + if (m_implicit) + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt; + } + } + } + } + m_objective->applyExplicitForce(m_residual); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + + if (psb->isActive()) + { + // predict motion for collision detection + predictDeformableMotion(psb, solverdt); + } + } } void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt) { - BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); - int i, ni; - - /* Update */ - if (psb->m_bUpdateRtCst) - { - psb->m_bUpdateRtCst = false; - psb->updateConstants(); - psb->m_fdbvt.clear(); - if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD) - { - psb->initializeFaceTree(); - } - } - - /* Prepare */ - psb->m_sst.sdt = dt * psb->m_cfg.timescale; - psb->m_sst.isdt = 1 / psb->m_sst.sdt; - psb->m_sst.velmrg = psb->m_sst.sdt * 3; - psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); - psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; - /* Bounds */ - psb->updateBounds(); - - /* Integrate */ - // do not allow particles to move more than the bounding box size - btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt; - for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) - { - btSoftBody::Node& n = psb->m_nodes[i]; - // apply drag - n.m_v *= (1 - psb->m_cfg.drag); - // scale velocity back - if (n.m_v.norm() > max_v) - { - n.m_v.safeNormalize(); - n.m_v *= max_v; - } - n.m_q = n.m_x + n.m_v * dt; - n.m_penetration = 0; - } - - /* Nodes */ - psb->updateNodeTree(true, true); - if (!psb->m_fdbvt.empty()) - { - psb->updateFaceTree(true, true); - } - /* Clear contacts */ - psb->m_nodeRigidContacts.resize(0); - psb->m_faceRigidContacts.resize(0); - psb->m_faceNodeContacts.resize(0); - /* Optimize dbvt's */ -// psb->m_ndbvt.optimizeIncremental(1); -// psb->m_fdbvt.optimizeIncremental(1); -} + BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); + int i, ni; + + /* Update */ + if (psb->m_bUpdateRtCst) + { + psb->m_bUpdateRtCst = false; + psb->updateConstants(); + psb->m_fdbvt.clear(); + if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD) + { + psb->initializeFaceTree(); + } + } + /* Prepare */ + psb->m_sst.sdt = dt * psb->m_cfg.timescale; + psb->m_sst.isdt = 1 / psb->m_sst.sdt; + psb->m_sst.velmrg = psb->m_sst.sdt * 3; + psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); + psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; + /* Bounds */ + psb->updateBounds(); + + /* Integrate */ + // do not allow particles to move more than the bounding box size + btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt; + for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + // apply drag + n.m_v *= (1 - psb->m_cfg.drag); + // scale velocity back + if (m_implicit) + { + n.m_q = n.m_x; + } + else + { + if (n.m_v.norm() > max_v) + { + n.m_v.safeNormalize(); + n.m_v *= max_v; + } + n.m_q = n.m_x + n.m_v * dt; + } + n.m_splitv.setZero(); + n.m_constrained = false; + } + + /* Nodes */ + psb->updateNodeTree(true, true); + if (!psb->m_fdbvt.empty()) + { + psb->updateFaceTree(true, true); + } + /* Clear contacts */ + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); + psb->m_faceNodeContacts.resize(0); + /* Optimize dbvt's */ + // psb->m_ndbvt.optimizeIncremental(1); + // psb->m_fdbvt.optimizeIncremental(1); +} void btDeformableBodySolver::updateSoftBodies() { - BT_PROFILE("updateSoftBodies"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody *psb = (btSoftBody *)m_softBodies[i]; - if (psb->isActive()) - { - psb->updateNormals(); - } - } + BT_PROFILE("updateSoftBodies"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; + if (psb->isActive()) + { + psb->updateNormals(); + } + } } void btDeformableBodySolver::setImplicit(bool implicit) { - m_implicit = implicit; - m_objective->setImplicit(implicit); + m_implicit = implicit; + m_objective->setImplicit(implicit); } void btDeformableBodySolver::setLineSearch(bool lineSearch) { - m_lineSearch = lineSearch; + m_lineSearch = lineSearch; } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h index d4e5f4c603..ae674d6e89 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h @@ -16,7 +16,6 @@ #ifndef BT_DEFORMABLE_BODY_SOLVERS_H #define BT_DEFORMABLE_BODY_SOLVERS_H - #include "btSoftBodySolvers.h" #include "btDeformableBackwardEulerObjective.h" #include "btDeformableMultiBodyDynamicsWorld.h" @@ -30,133 +29,132 @@ class btDeformableMultiBodyDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { - typedef btAlignedObjectArray<btVector3> TVStack; + typedef btAlignedObjectArray<btVector3> TVStack; + protected: - int m_numNodes; // total number of deformable body nodes - TVStack m_dv; // v_{n+1} - v_n - TVStack m_backup_dv; // backed up dv - TVStack m_ddv; // incremental dv - TVStack m_residual; // rhs of the linear solve - btAlignedObjectArray<btSoftBody *> m_softBodies; // all deformable bodies - TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit - btScalar m_dt; // dt - btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver - btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver - bool m_implicit; // use implicit scheme if true, explicit scheme if false - int m_maxNewtonIterations; // max number of newton iterations - btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance - bool m_lineSearch; // If true, use newton's method with line search under implicit scheme + int m_numNodes; // total number of deformable body nodes + TVStack m_dv; // v_{n+1} - v_n + TVStack m_backup_dv; // backed up dv + TVStack m_ddv; // incremental dv + TVStack m_residual; // rhs of the linear solve + btAlignedObjectArray<btSoftBody*> m_softBodies; // all deformable bodies + TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit + btScalar m_dt; // dt + btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver + btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver + bool m_implicit; // use implicit scheme if true, explicit scheme if false + int m_maxNewtonIterations; // max number of newton iterations + btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance + bool m_lineSearch; // If true, use newton's method with line search under implicit scheme public: - // handles data related to objective function - btDeformableBackwardEulerObjective* m_objective; - bool m_useProjection; - - btDeformableBodySolver(); - - virtual ~btDeformableBodySolver(); - - virtual SolverTypes getSolverType() const - { - return DEFORMABLE_SOLVER; - } - - // update soft body normals - virtual void updateSoftBodies(); - - virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); - - // solve the momentum equation - virtual void solveDeformableConstraints(btScalar solverdt); - - // set up the position error in split impulse - void splitImpulseSetup(const btContactSolverInfo& infoGlobal); - - // resize/clear data structures - void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt); - - // set up contact constraints - void setConstraints(const btContactSolverInfo& infoGlobal); - - // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion - virtual void predictMotion(btScalar solverdt); - - // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* - // x_{n+1}^* is stored in m_q - void predictDeformableMotion(btSoftBody* psb, btScalar dt); - - // save the current velocity to m_backupVelocity - void backupVelocity(); - - // set m_dv and m_backupVelocity to desired value to prepare for momentum solve - void setupDeformableSolve(bool implicit); - - // set the current velocity to that backed up in m_backupVelocity - void revertVelocity(); - - // set velocity to m_dv + m_backupVelocity - void updateVelocity(); - - // update the node count - bool updateNodes(); - - // calculate the change in dv resulting from the momentum solve - void computeStep(TVStack& ddv, const TVStack& residual); - - // calculate the change in dv resulting from the momentum solve when line search is turned on - btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose=false); - - virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} - - // process collision between deformable and rigid - virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap) - { - softBody->defaultCollisionHandler(collisionObjectWrap); - } - - // process collision between deformable and deformable - virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { - softBody->defaultCollisionHandler(otherSoftBody); - } - - // If true, implicit time stepping scheme is used. - // Otherwise, explicit time stepping scheme is used - void setImplicit(bool implicit); - - // If true, newton's method with line search is used when implicit time stepping scheme is turned on - void setLineSearch(bool lineSearch); - - // set temporary position x^* = x_n + dt * v - // update the deformation gradient at position x^* - void updateState(); - - // set dv = dv + scale * ddv - void updateDv(btScalar scale = 1); - - // set temporary position x^* = x_n + dt * v^* - void updateTempPosition(); - - // save the current dv to m_backup_dv; - void backupDv(); - - // set dv to the backed-up value - void revertDv(); - - // set dv = dv + scale * ddv - // set v^* = v_n + dv - // set temporary position x^* = x_n + dt * v^* - // update the deformation gradient at position x^* - void updateEnergy(btScalar scale); - - // calculates the appropriately scaled kinetic energy in the system, which is - // 1/2 * dv^T * M * dv - // used in line search - btScalar kineticEnergy(); - - // unused functions - virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){} - virtual void solveConstraints(btScalar dt){} - virtual bool checkInitialized(){return true;} - virtual void copyBackToSoftBodies(bool bMove = true) {} + // handles data related to objective function + btDeformableBackwardEulerObjective* m_objective; + bool m_useProjection; + + btDeformableBodySolver(); + + virtual ~btDeformableBodySolver(); + + virtual SolverTypes getSolverType() const + { + return DEFORMABLE_SOLVER; + } + + // update soft body normals + virtual void updateSoftBodies(); + + virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // solve the momentum equation + virtual void solveDeformableConstraints(btScalar solverdt); + + // resize/clear data structures + void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt); + + // set up contact constraints + void setConstraints(const btContactSolverInfo& infoGlobal); + + // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion + virtual void predictMotion(btScalar solverdt); + + // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* + // x_{n+1}^* is stored in m_q + void predictDeformableMotion(btSoftBody* psb, btScalar dt); + + // save the current velocity to m_backupVelocity + void backupVelocity(); + + // set m_dv and m_backupVelocity to desired value to prepare for momentum solve + void setupDeformableSolve(bool implicit); + + // set the current velocity to that backed up in m_backupVelocity + void revertVelocity(); + + // set velocity to m_dv + m_backupVelocity + void updateVelocity(); + + // update the node count + bool updateNodes(); + + // calculate the change in dv resulting from the momentum solve + void computeStep(TVStack& ddv, const TVStack& residual); + + // calculate the change in dv resulting from the momentum solve when line search is turned on + btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose = false); + + virtual void copySoftBodyToVertexBuffer(const btSoftBody* const softBody, btVertexBufferDescriptor* vertexBuffer) {} + + // process collision between deformable and rigid + virtual void processCollision(btSoftBody* softBody, const btCollisionObjectWrapper* collisionObjectWrap) + { + softBody->defaultCollisionHandler(collisionObjectWrap); + } + + // process collision between deformable and deformable + virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody) + { + softBody->defaultCollisionHandler(otherSoftBody); + } + + // If true, implicit time stepping scheme is used. + // Otherwise, explicit time stepping scheme is used + void setImplicit(bool implicit); + + // If true, newton's method with line search is used when implicit time stepping scheme is turned on + void setLineSearch(bool lineSearch); + + // set temporary position x^* = x_n + dt * v + // update the deformation gradient at position x^* + void updateState(); + + // set dv = dv + scale * ddv + void updateDv(btScalar scale = 1); + + // set temporary position x^* = x_n + dt * v^* + void updateTempPosition(); + + // save the current dv to m_backup_dv; + void backupDv(); + + // set dv to the backed-up value + void revertDv(); + + // set dv = dv + scale * ddv + // set v^* = v_n + dv + // set temporary position x^* = x_n + dt * v^* + // update the deformation gradient at position x^* + void updateEnergy(btScalar scale); + + // calculates the appropriately scaled kinetic energy in the system, which is + // 1/2 * dv^T * M * dv + // used in line search + btScalar kineticEnergy(); + + // unused functions + virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {} + virtual void solveConstraints(btScalar dt) {} + virtual bool checkInitialized() { return true; } + virtual void copyBackToSoftBodies(bool bMove = true) {} }; #endif /* btDeformableBodySolver_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp index 2864446de6..09398d79a5 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp @@ -16,387 +16,503 @@ #include "btDeformableContactConstraint.h" /* ================ Deformable Node Anchor =================== */ btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal) -: m_anchor(&a) -, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) + : m_anchor(&a), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) { } btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other) -: m_anchor(other.m_anchor) -, btDeformableContactConstraint(other) + : m_anchor(other.m_anchor), btDeformableContactConstraint(other) { } btVector3 btDeformableNodeAnchorConstraint::getVa() const { - const btSoftBody::sCti& cti = m_anchor->m_cti; - btVector3 va(0, 0, 0); - if (cti.m_colObj->hasContactResponse()) - { - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; - const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_n[k]; - } - va = cti.m_normal * vel; - // add in the tangential components of the va - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t1[k]; - } - va += m_anchor->t1 * vel; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t2[k]; - } - va += m_anchor->t2 * vel; - } - } - } - return va; + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t1[k]; + } + va += m_anchor->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t2[k]; + } + va += m_anchor->t2 * vel; + } + } + } + return va; } btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - const btSoftBody::sCti& cti = m_anchor->m_cti; - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = (vb - va); - // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 - const btScalar dn = btDot(vr, vr); - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_anchor->m_c0 * vr; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - - // apply impulse to the rigid/multibodies involved and change their velocities - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - btRigidBody* rigidCol = 0; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - if (rigidCol) - { - rigidCol->applyImpulse(impulse, m_anchor->m_c1); - } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = 0; - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); - const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); - } - } - return residualSquare; + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = (vb - va); + // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 + const btScalar dn = btDot(vr, vr); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn * dn; + btVector3 impulse = m_anchor->m_c0 * vr; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_anchor->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); + const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); + } + } + return residualSquare; } btVector3 btDeformableNodeAnchorConstraint::getVb() const { - return m_anchor->m_node->m_v; + return m_anchor->m_node->m_v; } void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) { - btVector3 dv = impulse * m_anchor->m_c2; - m_anchor->m_node->m_v -= dv; + btVector3 dv = impulse * m_anchor->m_c2; + m_anchor->m_node->m_v -= dv; } /* ================ Deformable vs. Rigid =================== */ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal) -: m_contact(&c) -, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) + : m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) { - m_total_normal_dv.setZero(); - m_total_tangent_dv.setZero(); - // The magnitude of penetration is the depth of penetration. - m_penetration = c.m_cti.m_offset; -// m_penetration = btMin(btScalar(0),c.m_cti.m_offset); + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); + // The magnitude of penetration is the depth of penetration. + m_penetration = c.m_cti.m_offset; + m_total_split_impulse = 0; + m_binding = false; } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) -: m_contact(other.m_contact) -, btDeformableContactConstraint(other) -, m_penetration(other.m_penetration) + : m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding) { - m_total_normal_dv = other.m_total_normal_dv; - m_total_tangent_dv = other.m_total_tangent_dv; + m_total_normal_dv = other.m_total_normal_dv; + m_total_tangent_dv = other.m_total_tangent_dv; } - btVector3 btDeformableRigidContactConstraint::getVa() const { - const btSoftBody::sCti& cti = m_contact->m_cti; - btVector3 va(0, 0, 0); - if (cti.m_colObj->hasContactResponse()) - { - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; - const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_n[k]; - } - va = cti.m_normal * vel; - // add in the tangential components of the va - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t1[k]; - } - va += m_contact->t1 * vel; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t2[k]; - } - va += m_contact->t2 * vel; - } - } - } - return va; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; +} + +btVector3 btDeformableRigidContactConstraint::getSplitVa() const +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; } btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - const btSoftBody::sCti& cti = m_contact->m_cti; - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = vb - va; - btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ; - const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - btVector3 old_total_tangent_dv = m_total_tangent_dv; - // m_c2 is the inverse mass of the deformable node/face - m_total_normal_dv -= impulse_normal * m_contact->m_c2; - m_total_tangent_dv -= impulse_tangent * m_contact->m_c2; - - if (m_total_normal_dv.dot(cti.m_normal) < 0) - { - // separating in the normal direction - m_static = false; - m_total_tangent_dv = btVector3(0,0,0); - impulse_tangent.setZero(); - } - else - { - if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) - { - // dynamic friction - // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. - m_static = false; - if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) - { - m_total_tangent_dv = btVector3(0,0,0); - } - else - { - m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; - } - impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); - } - else - { - // static friction - m_static = true; - } - } - impulse = impulse_normal + impulse_tangent; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - if (residualSquare < 1e-7) - return residualSquare; - // apply impulse to the rigid/multibodies involved and change their velocities - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - btRigidBody* rigidCol = 0; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - if (rigidCol) - { - rigidCol->applyImpulse(impulse, m_contact->m_c1); - } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = 0; - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - if (impulse_tangent.norm() > SIMD_EPSILON) - { - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); - const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); - } - } - } -// va = getVa(); -// vb = getVb(); -// vr = vb - va; -// btScalar dn1 = btDot(vr, cti.m_normal) / 150; -// m_penetration += dn1; - return residualSquare; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm; + if (m_penetration > 0) + { + dn += m_penetration / infoGlobal.m_timeStep; + } + if (!infoGlobal.m_splitImpulse) + { + dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + } + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0))); + if (!infoGlobal.m_splitImpulse) + { + impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); + } + btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + if (dn > 0) + { + return 0; + } + m_binding = true; + btScalar residualSquare = dn * dn; + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c5 is the inverse mass of the deformable node/face + m_total_normal_dv -= m_contact->m_c5 * impulse_normal; + m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent; + + if (m_total_normal_dv.dot(cti.m_normal) < 0) + { + // separating in the normal direction + m_binding = false; + m_static = false; + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0, 0, 0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; + } + // impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); + impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + if (impulse_tangent.norm() > SIMD_EPSILON) + { + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); + const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); + } + } + } + return residualSquare; +} + +btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +{ + btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 vb = getSplitVb(); + btVector3 va = getSplitVa(); + btScalar p = m_penetration; + if (p > 0) + { + return 0; + } + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + if (dn > 0) + { + return 0; + } + if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION) + { + dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION) + { + dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + m_total_split_impulse += dn; + + btScalar residualSquare = dn * dn; + const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn); + applySplitImpulse(impulse); + + // apply split impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyPushImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); + } + } + return residualSquare; } /* ================ Node vs. Rigid =================== */ btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal) - : m_node(contact.m_node) - , btDeformableRigidContactConstraint(contact, infoGlobal) - { - } + : m_node(contact.m_node), btDeformableRigidContactConstraint(contact, infoGlobal) +{ +} btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other) -: m_node(other.m_node) -, btDeformableRigidContactConstraint(other) + : m_node(other.m_node), btDeformableRigidContactConstraint(other) { } btVector3 btDeformableNodeRigidContactConstraint::getVb() const { - return m_node->m_v; + return m_node->m_v; } +btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const +{ + return m_node->m_splitv; +} btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const { - return m_total_normal_dv + m_total_tangent_dv; + return m_total_normal_dv + m_total_tangent_dv; } void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableNodeRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - contact->m_node->m_v -= dv; + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = contact->m_c5 * impulse; + contact->m_node->m_v -= dv; +} + +void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = contact->m_c5 * impulse; + contact->m_node->m_splitv -= dv; } /* ================ Face vs. Rigid =================== */ btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting) -: m_face(contact.m_face) -, m_useStrainLimiting(useStrainLimiting) -, btDeformableRigidContactConstraint(contact, infoGlobal) + : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal) { } btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other) -: m_face(other.m_face) -, m_useStrainLimiting(other.m_useStrainLimiting) -, btDeformableRigidContactConstraint(other) + : m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other) { } btVector3 btDeformableFaceRigidContactConstraint::getVb() const { - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; - return vb; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; } - btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const { - btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - if (m_face->m_n[0] == node) - { - return face_dv * contact->m_weights[0]; - } - if (m_face->m_n[1] == node) - { - return face_dv * contact->m_weights[1]; - } - btAssert(node == m_face->m_n[2]); - return face_dv * contact->m_weights[2]; + btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + if (m_face->m_n[0] == node) + { + return face_dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == node) + { + return face_dv * contact->m_weights[1]; + } + btAssert(node == m_face->m_n[2]); + return face_dv * contact->m_weights[2]; } void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - btSoftBody::Face* face = contact->m_face; - - btVector3& v0 = face->m_n[0]->m_v; - btVector3& v1 = face->m_n[1]->m_v; - btVector3& v2 = face->m_n[2]->m_v; - const btScalar& im0 = face->m_n[0]->m_im; - const btScalar& im1 = face->m_n[1]->m_im; - const btScalar& im2 = face->m_n[2]->m_im; - if (im0 > 0) - v0 -= dv * contact->m_weights[0]; - if (im1 > 0) - v1 -= dv * contact->m_weights[1]; - if (im2 > 0) - v2 -= dv * contact->m_weights[2]; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + v0 -= dv * contact->m_weights[0]; + if (im1 > 0) + v1 -= dv * contact->m_weights[1]; + if (im2 > 0) + v2 -= dv * contact->m_weights[2]; if (m_useStrainLimiting) { - btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations); - btScalar m01 = (relaxation/(im0 + im1)); - btScalar m02 = (relaxation/(im0 + im2)); - btScalar m12 = (relaxation/(im1 + im2)); - #ifdef USE_STRAIN_RATE_LIMITING + btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations); + btScalar m01 = (relaxation / (im0 + im1)); + btScalar m02 = (relaxation / (im0 + im2)); + btScalar m12 = (relaxation / (im1 + im2)); +#ifdef USE_STRAIN_RATE_LIMITING // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. btScalar p = 0.01; btVector3& x0 = face->m_n[0]->m_x; btVector3& x1 = face->m_n[1]->m_x; btVector3& x2 = face->m_n[2]->m_x; - const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1}; - const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1}; + const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1}; + const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1}; btVector3 u[3]; btScalar x_diff_dot_u, dn[3]; btScalar dt = m_infoGlobal->m_timeStep; @@ -404,172 +520,201 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul { btScalar x_diff_norm = x_diff[i].safeNorm(); btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); - btScalar strainRate = x_diff_norm_new/x_diff_norm; + btScalar strainRate = x_diff_norm_new / x_diff_norm; u[i] = v_diff[i]; u[i].safeNormalize(); - if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p)) + if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p)) { dn[i] = 0; continue; } x_diff_dot_u = btDot(x_diff[i], u[i]); btScalar s; - if (1-p > strainRate) + if (1 - p > strainRate) { - s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm)); + s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm)); } else { - s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm)); + s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm)); } // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); // strainRate = x_diff_norm_new/x_diff_norm; dn[i] = s - v_diff[i].safeNorm(); } - btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1])); - btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2])); - btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1])); - #else + btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1])); + btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2])); + btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1])); +#else // apply strain limiting to prevent undamped modes - btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); - btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); - btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); - #endif + btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0)); + btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1)); + btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2)); +#endif v0 += dv0; v1 += dv1; v2 += dv2; } } +btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2]; + return vb; +} + +void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_splitv; + btVector3& v1 = face->m_n[1]->m_splitv; + btVector3& v2 = face->m_n[2]->m_splitv; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dv * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dv * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dv * contact->m_weights[2]; + } +} + /* ================ Face vs. Node =================== */ btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal) -: m_node(contact.m_node) -, m_face(contact.m_face) -, m_contact(&contact) -, btDeformableContactConstraint(contact.m_normal, infoGlobal) + : m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal) { - m_total_normal_dv.setZero(); - m_total_tangent_dv.setZero(); + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); } btVector3 btDeformableFaceNodeContactConstraint::getVa() const { - return m_node->m_v; + return m_node->m_v; } btVector3 btDeformableFaceNodeContactConstraint::getVb() const { - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; - return vb; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; } btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const { - btVector3 dv = m_total_normal_dv + m_total_tangent_dv; - if (n == m_node) - return dv; - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - if (m_face->m_n[0] == n) - { - return dv * contact->m_weights[0]; - } - if (m_face->m_n[1] == n) - { - return dv * contact->m_weights[1]; - } - btAssert(n == m_face->m_n[2]); - return dv * contact->m_weights[2]; + btVector3 dv = m_total_normal_dv + m_total_tangent_dv; + if (n == m_node) + return dv; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + if (m_face->m_n[0] == n) + { + return dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == n) + { + return dv * contact->m_weights[1]; + } + btAssert(n == m_face->m_n[2]); + return dv * contact->m_weights[2]; } btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = vb - va; - const btScalar dn = btDot(vr, m_contact->m_normal); - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_contact->m_c0 * vr; - const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - - btVector3 old_total_tangent_dv = m_total_tangent_dv; - // m_c2 is the inverse mass of the deformable node/face - if (m_node->m_im > 0) - { - m_total_normal_dv -= impulse_normal * m_node->m_im; - m_total_tangent_dv -= impulse_tangent * m_node->m_im; - } - else - { - m_total_normal_dv -= impulse_normal * m_contact->m_imf; - m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; - } - - if (m_total_normal_dv.dot(m_contact->m_normal) > 0) - { - // separating in the normal direction - m_static = false; - m_total_tangent_dv = btVector3(0,0,0); - impulse_tangent.setZero(); - } - else - { - if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm()) - { - // dynamic friction - // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. - m_static = false; - if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) - { - m_total_tangent_dv = btVector3(0,0,0); - } - else - { - m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction; - } - impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); - } - else - { - // static friction - m_static = true; - } - } - impulse = impulse_normal + impulse_tangent; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - return residualSquare; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + const btScalar dn = btDot(vr, m_contact->m_normal); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn * dn; + btVector3 impulse = m_contact->m_c0 * vr; + const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c2 is the inverse mass of the deformable node/face + if (m_node->m_im > 0) + { + m_total_normal_dv -= impulse_normal * m_node->m_im; + m_total_tangent_dv -= impulse_tangent * m_node->m_im; + } + else + { + m_total_normal_dv -= impulse_normal * m_contact->m_imf; + m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; + } + + if (m_total_normal_dv.dot(m_contact->m_normal) > 0) + { + // separating in the normal direction + m_static = false; + m_total_tangent_dv = btVector3(0, 0, 0); + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0, 0, 0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction; + } + impulse_tangent = -btScalar(1) / m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + return residualSquare; } void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - btVector3 dva = impulse * contact->m_node->m_im; - btVector3 dvb = impulse * contact->m_imf; - if (contact->m_node->m_im > 0) - { - contact->m_node->m_v += dva; - } - - btSoftBody::Face* face = contact->m_face; - btVector3& v0 = face->m_n[0]->m_v; - btVector3& v1 = face->m_n[1]->m_v; - btVector3& v2 = face->m_n[2]->m_v; - const btScalar& im0 = face->m_n[0]->m_im; - const btScalar& im1 = face->m_n[1]->m_im; - const btScalar& im2 = face->m_n[2]->m_im; - if (im0 > 0) - { - v0 -= dvb * contact->m_weights[0]; - } - if (im1 > 0) - { - v1 -= dvb * contact->m_weights[1]; - } - if (im2 > 0) - { - v2 -= dvb * contact->m_weights[2]; - } + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 dva = impulse * contact->m_node->m_im; + btVector3 dvb = impulse * contact->m_imf; + if (contact->m_node->m_im > 0) + { + contact->m_node->m_v += dva; + } + + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dvb * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dvb * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dvb * contact->m_weights[2]; + } } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h index 9f9d5bf0a3..1e2c9f5bce 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h @@ -21,51 +21,49 @@ class btDeformableContactConstraint { public: - // True if the friction is static - // False if the friction is dynamic - bool m_static; + // True if the friction is static + // False if the friction is dynamic + bool m_static; const btContactSolverInfo* m_infoGlobal; // normal of the contact btVector3 m_normal; - btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) + btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) { } - btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) + btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) { } - - btDeformableContactConstraint(){} + + btDeformableContactConstraint() {} btDeformableContactConstraint(const btDeformableContactConstraint& other) - : m_static(other.m_static) - , m_normal(other.m_normal) - , m_infoGlobal(other.m_infoGlobal) + : m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal) { } - virtual ~btDeformableContactConstraint(){} - - // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence - // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; - - // get the velocity of the object A in the contact - virtual btVector3 getVa() const = 0; - - // get the velocity of the object B in the contact - virtual btVector3 getVb() const = 0; - - // get the velocity change of the soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const = 0; - - // apply impulse to the soft body node and/or face involved - virtual void applyImpulse(const btVector3& impulse) = 0; - - // scale the penetration depth by erp - virtual void setPenetrationScale(btScalar scale) = 0; + virtual ~btDeformableContactConstraint() {} + + // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence + // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const = 0; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const = 0; + + // get the velocity change of the soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const = 0; + + // apply impulse to the soft body node and/or face involved + virtual void applyImpulse(const btVector3& impulse) = 0; + + // scale the penetration depth by erp + virtual void setPenetrationScale(btScalar scale) = 0; }; // @@ -73,42 +71,41 @@ public: class btDeformableStaticConstraint : public btDeformableContactConstraint { public: - btSoftBody::Node* m_node; - - btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal) - { - } - btDeformableStaticConstraint(){} - btDeformableStaticConstraint(const btDeformableStaticConstraint& other) - : m_node(other.m_node) - , btDeformableContactConstraint(other) - { - } - - virtual ~btDeformableStaticConstraint(){} - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) - { - return 0; - } - - virtual btVector3 getVa() const - { - return btVector3(0,0,0); - } - - virtual btVector3 getVb() const - { - return btVector3(0,0,0); - } - - virtual btVector3 getDv(const btSoftBody::Node* n) const - { - return btVector3(0,0,0); - } - - virtual void applyImpulse(const btVector3& impulse){} - virtual void setPenetrationScale(btScalar scale){} + btSoftBody::Node* m_node; + + btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal) + { + } + btDeformableStaticConstraint() {} + btDeformableStaticConstraint(const btDeformableStaticConstraint& other) + : m_node(other.m_node), btDeformableContactConstraint(other) + { + } + + virtual ~btDeformableStaticConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) + { + return 0; + } + + virtual btVector3 getVa() const + { + return btVector3(0, 0, 0); + } + + virtual btVector3 getVb() const + { + return btVector3(0, 0, 0); + } + + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0, 0, 0); + } + + virtual void applyImpulse(const btVector3& impulse) {} + virtual void setPenetrationScale(btScalar scale) {} }; // @@ -116,56 +113,67 @@ public: class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint { public: - const btSoftBody::DeformableNodeRigidAnchor* m_anchor; - - btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); - btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); - btDeformableNodeAnchorConstraint(){} - virtual ~btDeformableNodeAnchorConstraint() - { - } - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - // object A is the rigid/multi body, and object B is the deformable node/face - virtual btVector3 getVa() const; - // get the velocity of the deformable node in contact - virtual btVector3 getVb() const; - virtual btVector3 getDv(const btSoftBody::Node* n) const - { - return btVector3(0,0,0); - } - virtual void applyImpulse(const btVector3& impulse); - - virtual void setPenetrationScale(btScalar scale){} -}; + const btSoftBody::DeformableNodeRigidAnchor* m_anchor; + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); + btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + btDeformableNodeAnchorConstraint() {} + virtual ~btDeformableNodeAnchorConstraint() + { + } + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0, 0, 0); + } + virtual void applyImpulse(const btVector3& impulse); + + virtual void setPenetrationScale(btScalar scale) {} +}; // // Constraint between rigid/multi body and deformable objects class btDeformableRigidContactConstraint : public btDeformableContactConstraint { public: - btVector3 m_total_normal_dv; - btVector3 m_total_tangent_dv; - btScalar m_penetration; - const btSoftBody::DeformableRigidContact* m_contact; - - btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); - btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); - btDeformableRigidContactConstraint(){} - virtual ~btDeformableRigidContactConstraint() - { - } - - // object A is the rigid/multi body, and object B is the deformable node/face - virtual btVector3 getVa() const; - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - virtual void setPenetrationScale(btScalar scale) - { - m_penetration *= scale; - } + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + btScalar m_penetration; + btScalar m_total_split_impulse; + bool m_binding; + const btSoftBody::DeformableRigidContact* m_contact; + + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); + btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + btDeformableRigidContactConstraint() {} + virtual ~btDeformableRigidContactConstraint() + { + } + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const = 0; + + // get the split impulse velocity of the rigid/multibdoy at the contaft + virtual btVector3 getSplitVa() const; + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + virtual void setPenetrationScale(btScalar scale) + { + m_penetration *= scale; + } + + btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); + + virtual void applySplitImpulse(const btVector3& impulse) = 0; }; // @@ -173,29 +181,34 @@ public: class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint { public: - // the deformable node in contact - btSoftBody::Node* m_node; - - btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); - btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); - btDeformableNodeRigidContactConstraint(){} - virtual ~btDeformableNodeRigidContactConstraint() - { - } - - // get the velocity of the deformable node in contact - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableNodeRigidContact* getContact() const - { - return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); + // the deformable node in contact + btSoftBody::Node* m_node; + + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); + btDeformableNodeRigidContactConstraint() {} + virtual ~btDeformableNodeRigidContactConstraint() + { + } + + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableNodeRigidContact* getContact() const + { + return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -203,28 +216,33 @@ public: class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint { public: - const btSoftBody::Face* m_face; - bool m_useStrainLimiting; - btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); - btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); - btDeformableFaceRigidContactConstraint(): m_useStrainLimiting(false) {} - virtual ~btDeformableFaceRigidContactConstraint() - { - } - - // get the velocity of the deformable face at the contact point - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableFaceRigidContact* getContact() const - { - return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); + btSoftBody::Face* m_face; + bool m_useStrainLimiting; + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); + btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); + btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {} + virtual ~btDeformableFaceRigidContactConstraint() + { + } + + // get the velocity of the deformable face at the contact point + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceRigidContact* getContact() const + { + return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -232,35 +250,35 @@ public: class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint { public: - btSoftBody::Node* m_node; - btSoftBody::Face* m_face; - const btSoftBody::DeformableFaceNodeContact* m_contact; - btVector3 m_total_normal_dv; - btVector3 m_total_tangent_dv; - - btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); - btDeformableFaceNodeContactConstraint(){} - virtual ~btDeformableFaceNodeContactConstraint(){} - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - // get the velocity of the object A in the contact - virtual btVector3 getVa() const; - - // get the velocity of the object B in the contact - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableFaceNodeContact* getContact() const - { - return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); - - virtual void setPenetrationScale(btScalar scale){} + btSoftBody::Node* m_node; + btSoftBody::Face* m_face; + const btSoftBody::DeformableFaceNodeContact* m_contact; + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableFaceNodeContactConstraint() {} + virtual ~btDeformableFaceNodeContactConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceNodeContact* getContact() const + { + return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void setPenetrationScale(btScalar scale) {} }; #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp index 22ca8bf582..7f67260ce6 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include <algorithm> #include <cmath> -btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; for (int i = 0; i < numDeformableBodies; ++i) @@ -58,27 +58,37 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod return residualSquare; } -void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < m_softBodies.size(); ++i) + btScalar residualSquare = 0; + for (int i = 0; i < numDeformableBodies; ++i) { - // node constraints - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); - } - // face constraints - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + for (int j = 0; j < m_softBodies.size(); ++j) { - btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); + btCollisionObject* psb = m_softBodies[j]; + if (psb != deformableBodies[i]) + { + continue; + } + for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } } } + return residualSquare; } void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) -{ +{ BT_PROFILE("setConstraints"); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -97,7 +107,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in m_staticConstraints[i].push_back(static_constraint); } } - + // set up deformable anchors for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) { @@ -111,7 +121,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); m_nodeAnchorConstraints[i].push_back(constraint); } - + // set Deformable Node vs. Rigid constraint for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { @@ -122,17 +132,9 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_nodeRigidConstraints[i].push_back(constraint); - } + m_nodeRigidConstraints[i].push_back(constraint); } - + // set Deformable Face vs. Rigid constraint for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) { @@ -143,15 +145,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_faceRigidConstraints[i].push_back(constraint); - } + m_faceRigidConstraints[i].push_back(constraint); } } } @@ -159,267 +153,269 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in void btDeformableContactProjection::project(TVStack& x) { #ifndef USE_MGS - const int dim = 3; - for (int index = 0; index < m_projectionsDict.size(); ++index) - { - btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); - size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); - if (projectionDirs.size() >= dim) - { - // static node - x[i].setZero(); - continue; - } - else if (projectionDirs.size() == 2) - { - btVector3 dir0 = projectionDirs[0]; - btVector3 dir1 = projectionDirs[1]; - btVector3 free_dir = btCross(dir0, dir1); - if (free_dir.safeNorm() < SIMD_EPSILON) - { - x[i] -= x[i].dot(dir0) * dir0; - x[i] -= x[i].dot(dir1) * dir1; - } - else - { - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; - } - } - else - { - btAssert(projectionDirs.size() == 1); - btVector3 dir0 = projectionDirs[0]; - x[i] -= x[i].dot(dir0) * dir0; - } - } + const int dim = 3; + for (int index = 0; index < m_projectionsDict.size(); ++index) + { + btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); + size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); + if (projectionDirs.size() >= dim) + { + // static node + x[i].setZero(); + continue; + } + else if (projectionDirs.size() == 2) + { + btVector3 dir0 = projectionDirs[0]; + btVector3 dir1 = projectionDirs[1]; + btVector3 free_dir = btCross(dir0, dir1); + if (free_dir.safeNorm() < SIMD_EPSILON) + { + x[i] -= x[i].dot(dir0) * dir0; + } + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + } + else + { + btAssert(projectionDirs.size() == 1); + btVector3 dir0 = projectionDirs[0]; + x[i] -= x[i].dot(dir0) * dir0; + } + } #else - btReducedVector p(x.size()); - for (int i = 0; i < m_projections.size(); ++i) - { - p += (m_projections[i].dot(x) * m_projections[i]); - } - for (int i = 0; i < p.m_indices.size(); ++i) - { - x[p.m_indices[i]] -= p.m_vecs[i]; - } + btReducedVector p(x.size()); + for (int i = 0; i < m_projections.size(); ++i) + { + p += (m_projections[i].dot(x) * m_projections[i]); + } + for (int i = 0; i < p.m_indices.size(); ++i) + { + x[p.m_indices[i]] -= p.m_vecs[i]; + } #endif } void btDeformableContactProjection::setProjection() { #ifndef USE_MGS - BT_PROFILE("btDeformableContactProjection::setProjection"); - btAlignedObjectArray<btVector3> units; - units.push_back(btVector3(1,0,0)); - units.push_back(btVector3(0,1,0)); - units.push_back(btVector3(0,0,1)); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) - { - int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - if (m_nodeRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray<btVector3> projections; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - } - } - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; - for (int k = 0; k < 3; ++k) - { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); - } - for (int k = 0; k < 3; ++k) - { - btSoftBody::Node* node = face->m_n[k]; - node->m_penetration = true; - int index = node->index; - if (m_faceRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray<btVector3> projections; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); - } - } - } - } - } + BT_PROFILE("btDeformableContactProjection::setProjection"); + btAlignedObjectArray<btVector3> units; + units.push_back(btVector3(1, 0, 0)); + units.push_back(btVector3(0, 1, 0)); + units.push_back(btVector3(0, 0, 1)); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_constrained = true; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + if (m_nodeRigidConstraints[i][j].m_binding) + { + if (m_nodeRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray<btVector3> projections; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + } + } + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + if (m_faceRigidConstraints[i][j].m_binding) + { + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_constrained = true; + } + } + for (int k = 0; k < 3; ++k) + { + btSoftBody::Node* node = face->m_n[k]; + int index = node->index; + if (m_faceRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int l = 0; l < 3; ++l) + { + projections.push_back(units[l]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray<btVector3> projections; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + } + } + } + } + } #else - int dof = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - dof += m_softBodies[i]->m_nodes.size(); - } - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; - btAlignedObjectArray<int> indices; - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - indices.push_back(index); - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - - for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) - { - int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; - btAlignedObjectArray<int> indices; - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - indices.push_back(index); - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - btAlignedObjectArray<int> indices; - indices.push_back(index); - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - if (m_nodeRigidConstraints[i][j].m_static) - { - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - else - { - vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - } - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + int dof = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + dof += m_softBodies[i]->m_nodes.size(); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; + btAlignedObjectArray<int> indices; + indices.push_back(index); + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + if (m_nodeRigidConstraints[i][j].m_static) + { + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + else + { + vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; - for (int k = 0; k < 3; ++k) - { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); - } + btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + } if (m_faceRigidConstraints[i][j].m_static) { for (int l = 0; l < 3; ++l) { - btReducedVector rv(dof); for (int k = 0; k < 3; ++k) { rv.m_indices.push_back(face->m_n[k]->index); - btVector3 v(0,0,0); + btVector3 v(0, 0, 0); v[l] = bary[k]; rv.m_vecs.push_back(v); - rv.sort(); + rv.sort(); } m_projections.push_back(rv); } @@ -431,121 +427,134 @@ void btDeformableContactProjection::setProjection() { rv.m_indices.push_back(face->m_n[k]->index); rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal); - rv.sort(); + rv.sort(); } m_projections.push_back(rv); } } - } - btModifiedGramSchmidt<btReducedVector> mgs(m_projections); - mgs.solve(); - m_projections = mgs.m_out; + } + btModifiedGramSchmidt<btReducedVector> mgs(m_projections); + mgs.solve(); + m_projections = mgs.m_out; #endif } void btDeformableContactProjection::checkConstraints(const TVStack& x) { - for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) - { - btVector3 d(0,0,0); - const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; - for (int j = 0; j < lm.m_num_constraints; ++j) - { - for (int k = 0; k < lm.m_num_nodes; ++k) - { - d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); - } - } - printf("d = %f, %f, %f\n",d[0],d[1],d[2]); - } + for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) + { + btVector3 d(0, 0, 0); + const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int k = 0; k < lm.m_num_nodes; ++k) + { + d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); + } + } + // printf("d = %f, %f, %f\n", d[0], d[1], d[2]); + // printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]); + } } void btDeformableContactProjection::setLagrangeMultiplier() { - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - m_lagrangeMultipliers.push_back(lm); - } - for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) - { - int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - m_lagrangeMultipliers.push_back(lm); - } - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - if (m_nodeRigidConstraints[i][j].m_static) - { - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - } - else - { - lm.m_num_constraints = 1; - lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; - } - m_lagrangeMultipliers.push_back(lm); - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; - - btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + if (!m_nodeRigidConstraints[i][j].m_binding) + { + continue; + } + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + if (m_nodeRigidConstraints[i][j].m_static) + { + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + } + else + { + lm.m_num_constraints = 1; + lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; + } + m_lagrangeMultipliers.push_back(lm); + } + + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + if (!m_faceRigidConstraints[i][j].m_binding) + { + continue; + } + btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + + btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; LagrangeMultiplier lm; lm.m_num_nodes = 3; - for (int k = 0; k<3; ++k) + + for (int k = 0; k < 3; ++k) { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + face->m_n[k]->m_constrained = true; lm.m_indices[k] = face->m_n[k]->index; lm.m_weights[k] = bary[k]; } - if (m_faceRigidConstraints[i][j].m_static) - { + if (m_faceRigidConstraints[i][j].m_static) + { + face->m_pcontact[3] = 1; lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); } else { + face->m_pcontact[3] = 0; lm.m_num_constraints = 1; lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; } - m_lagrangeMultipliers.push_back(lm); + m_lagrangeMultipliers.push_back(lm); } } } @@ -562,7 +571,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) @@ -575,7 +584,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } } @@ -587,7 +596,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } for (int k = 0; k < 3; ++k) { @@ -595,7 +604,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } } @@ -612,9 +621,8 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_nodeRigidConstraints.resize(N); m_faceRigidConstraints.resize(N); m_deformableConstraints.resize(N); - } - for (int i = 0 ; i < N; ++i) + for (int i = 0; i < N; ++i) { m_staticConstraints[i].clear(); m_nodeAnchorConstraints[i].clear(); @@ -623,12 +631,9 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_deformableConstraints[i].clear(); } #ifndef USE_MGS - m_projectionsDict.clear(); + m_projectionsDict.clear(); #else - m_projections.clear(); + m_projections.clear(); #endif - m_lagrangeMultipliers.clear(); + m_lagrangeMultipliers.clear(); } - - - diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h index 8d7e94d4fb..4964eaf990 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h @@ -27,31 +27,30 @@ struct LagrangeMultiplier { - int m_num_constraints; // Number of constraints - int m_num_nodes; // Number of nodes in these constraints - btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes - btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; - int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; + int m_num_constraints; // Number of constraints + int m_num_nodes; // Number of nodes in these constraints + btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes + btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; + int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; }; - class btDeformableContactProjection { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - - // all constraints involving face - btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + + // all constraints involving face + btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; #ifndef USE_MGS - // map from node index to projection directions - btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; + // map from node index to projection directions + btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; #else - btAlignedObjectArray<btReducedVector> m_projections; + btAlignedObjectArray<btReducedVector> m_projections; #endif - - btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers; - + + btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers; + // map from node index to static constraint btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints; // map from node index to node rigid constraint @@ -62,39 +61,39 @@ public: btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints; // map from node index to node anchor constraint btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints; - - bool m_useStrainLimiting; - - btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies) - : m_softBodies(softBodies) - { - } - - virtual ~btDeformableContactProjection() - { - } - - // apply the constraints to the rhs of the linear solve - virtual void project(TVStack& x); - - // add friction force to the rhs of the linear solve - virtual void applyDynamicFriction(TVStack& f); - - // update and solve the constraints - virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); - - // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. - virtual void setConstraints(const btContactSolverInfo& infoGlobal); - - // Set up projections for each vertex by adding the projection direction to - virtual void setProjection(); - - virtual void reinitialize(bool nodeUpdated); - - virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal); - - virtual void setLagrangeMultiplier(); - - void checkConstraints(const TVStack& x); + + bool m_useStrainLimiting; + + btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies) + : m_softBodies(softBodies) + { + } + + virtual ~btDeformableContactProjection() + { + } + + // apply the constraints to the rhs of the linear solve + virtual void project(TVStack& x); + + // add friction force to the rhs of the linear solve + virtual void applyDynamicFriction(TVStack& f); + + // update and solve the constraints + virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. + virtual void setConstraints(const btContactSolverInfo& infoGlobal); + + // Set up projections for each vertex by adding the projection direction to + virtual void setProjection(); + + virtual void reinitialize(bool nodeUpdated); + + btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + virtual void setLagrangeMultiplier(); + + void checkConstraints(const TVStack& x); }; #endif /* btDeformableContactProjection_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h index 2d042df729..dfd85523bc 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h @@ -21,107 +21,104 @@ static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) { - static const btPolarDecomposition polar; - return polar.decompose(m, q, s); + static const btPolarDecomposition polar; + return polar.decompose(m, q, s); } class btDeformableCorotatedForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; - btDeformableCorotatedForce(): m_mu(1), m_lambda(1) - { - - } - - btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(tetra.m_F,P); - btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - // explicit elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) - { - // btMatrix3x3 JFinvT = F.adjoint(); - btScalar J = F.determinant(); - P = F.adjoint().transpose() * (m_lambda * (J-1)); - if (m_mu > SIMD_EPSILON) - { - btMatrix3x3 R,S; - if (J < 1024 * SIMD_EPSILON) - R.setIdentity(); - else - PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd - /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ - P += (F-R) * 2 * m_mu; - } - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_COROTATED_FORCE; - } - -}; + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; + btDeformableCorotatedForce() : m_mu(1), m_lambda(1) + { + } + + btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(tetra.m_F, P); + btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + // elastic force + // explicit elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) + { + // btMatrix3x3 JFinvT = F.adjoint(); + btScalar J = F.determinant(); + P = F.adjoint().transpose() * (m_lambda * (J - 1)); + if (m_mu > SIMD_EPSILON) + { + btMatrix3x3 R, S; + if (J < 1024 * SIMD_EPSILON) + R.setIdentity(); + else + PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd + /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ + P += (F - R) * 2 * m_mu; + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_COROTATED_FORCE; + } +}; #endif /* btCorotated_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h index 13ee3eacb6..d91867f457 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h @@ -21,87 +21,85 @@ class btDeformableGravityForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btVector3 m_gravity; - - btDeformableGravityForce(const btVector3& g) : m_gravity(g) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledGravityForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledGravityForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} - - virtual void addScaledGravityForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& n = psb->m_nodes[j]; - size_t id = n.index; - btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; - btVector3 scaled_force = scale * m_gravity * mass; - force[id] += scaled_force; - } - } - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_GRAVITY_FORCE; - } + typedef btAlignedObjectArray<btVector3> TVStack; + btVector3 m_gravity; - // the gravitational potential energy - virtual double totalEnergy(btScalar dt) - { - double e = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - if (node.m_im > 0) - { - e -= m_gravity.dot(node.m_q)/node.m_im; - } - } - } - return e; - } - - + btDeformableGravityForce(const btVector3& g) : m_gravity(g) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual void addScaledGravityForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& n = psb->m_nodes[j]; + size_t id = n.index; + btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; + btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor; + force[id] += scaled_force; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_GRAVITY_FORCE; + } + + // the gravitational potential energy + virtual double totalEnergy(btScalar dt) + { + double e = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + e -= m_gravity.dot(node.m_q) / node.m_im; + } + } + } + return e; + } }; #endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h index 0b6447442d..d58d825d1c 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h @@ -22,352 +22,351 @@ enum btDeformableLagrangianForceType { - BT_GRAVITY_FORCE = 1, - BT_MASSSPRING_FORCE = 2, - BT_COROTATED_FORCE = 3, - BT_NEOHOOKEAN_FORCE = 4, - BT_LINEAR_ELASTICITY_FORCE = 5, - BT_MOUSE_PICKING_FORCE = 6 + BT_GRAVITY_FORCE = 1, + BT_MASSSPRING_FORCE = 2, + BT_COROTATED_FORCE = 3, + BT_NEOHOOKEAN_FORCE = 4, + BT_LINEAR_ELASTICITY_FORCE = 5, + BT_MOUSE_PICKING_FORCE = 6 }; static inline double randomDouble(double low, double high) { - return low + static_cast<double>(rand()) / RAND_MAX * (high - low); + return low + static_cast<double>(rand()) / RAND_MAX * (high - low); } class btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btAlignedObjectArray<btSoftBody *> m_softBodies; - const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; - - btDeformableLagrangianForce() - { - } - - virtual ~btDeformableLagrangianForce(){} - - // add all forces - virtual void addScaledForces(btScalar scale, TVStack& force) = 0; - - // add damping df - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; - - // build diagonal of A matrix - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0; - - // add elastic df - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; - - // add all forces that are explicit in explicit solve - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; - - // add all damping forces - virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; - - virtual btDeformableLagrangianForceType getForceType() = 0; - - virtual void reinitialize(bool nodeUpdated) - { - } - - // get number of nodes that have the force - virtual int getNumNodes() - { - int numNodes = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - numNodes += m_softBodies[i]->m_nodes.size(); - } - return numNodes; - } - - // add a soft body to be affected by the particular lagrangian force - virtual void addSoftBody(btSoftBody* psb) - { - m_softBodies.push_back(psb); - } - - virtual void removeSoftBody(btSoftBody* psb) - { - m_softBodies.remove(psb); - } - - virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) - { - m_nodes = nodes; - } - - // Calculate the incremental deformable generated from the input dx - virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) - { - btVector3 c1 = dx[id1] - dx[id0]; - btVector3 c2 = dx[id2] - dx[id0]; - btVector3 c3 = dx[id3] - dx[id0]; - return btMatrix3x3(c1,c2,c3).transpose(); - } - - // Calculate the incremental deformable generated from the current velocity - virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) - { - btVector3 c1 = n1->m_v - n0->m_v; - btVector3 c2 = n2->m_v - n0->m_v; - btVector3 c3 = n3->m_v - n0->m_v; - return btMatrix3x3(c1,c2,c3).transpose(); - } - - // test for addScaledElasticForce function - virtual void testDerivative() - { - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); - } - psb->updateDeformation(); - } - - TVStack dx; - dx.resize(getNumNodes()); - TVStack dphi_dx; - dphi_dx.resize(dx.size()); - for (int i =0; i < dphi_dx.size();++i) - { - dphi_dx[i].setZero(); - } - addScaledForces(-1, dphi_dx); - - // write down the current position - TVStack x; - x.resize(dx.size()); - int counter = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - x[counter] = psb->m_nodes[j].m_q; - counter++; - } - } - counter = 0; - - // populate dx with random vectors - for (int i = 0; i < dx.size(); ++i) - { - dx[i].setX(randomDouble(-1, 1)); - dx[i].setY(randomDouble(-1, 1)); - dx[i].setZ(randomDouble(-1, 1)); - } - - btAlignedObjectArray<double> errors; - for (int it = 0; it < 10; ++it) - { - for (int i = 0; i < dx.size(); ++i) - { - dx[i] *= 0.5; - } - - // get dphi/dx * dx - double dphi = 0; - for (int i = 0; i < dx.size(); ++i) - { - dphi += dphi_dx[i].dot(dx[i]); - } - - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] + dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double f1 = totalElasticEnergy(0); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] - dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - double f2 = totalElasticEnergy(0); - - //restore m_q - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double error = f1-f2-2*dphi; - errors.push_back(error); - std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl; - } - for (int i = 1; i < errors.size(); ++i) - { - std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; - } - } - - // test for addScaledElasticForce function - virtual void testHessian() - { - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); - } - psb->updateDeformation(); - } - - - TVStack dx; - dx.resize(getNumNodes()); - TVStack df; - df.resize(dx.size()); - TVStack f1; - f1.resize(dx.size()); - TVStack f2; - f2.resize(dx.size()); - - - // write down the current position - TVStack x; - x.resize(dx.size()); - int counter = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - x[counter] = psb->m_nodes[j].m_q; - counter++; - } - } - counter = 0; - - // populate dx with random vectors - for (int i = 0; i < dx.size(); ++i) - { - dx[i].setX(randomDouble(-1, 1)); - dx[i].setY(randomDouble(-1, 1)); - dx[i].setZ(randomDouble(-1, 1)); - } - - btAlignedObjectArray<double> errors; - for (int it = 0; it < 10; ++it) - { - for (int i = 0; i < dx.size(); ++i) - { - dx[i] *= 0.5; - } - - // get df - for (int i =0; i < df.size();++i) - { - df[i].setZero(); - f1[i].setZero(); - f2[i].setZero(); - } - - //set df - addScaledElasticForceDifferential(-1, dx, df); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] + dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - //set f1 - addScaledForces(-1, f1); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] - dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - //set f2 - addScaledForces(-1, f2); - - //restore m_q - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double error = 0; - for (int i = 0; i < df.size();++i) - { - btVector3 error_vector = f1[i]-f2[i]-2*df[i]; - error += error_vector.length2(); - } - error = btSqrt(error); - errors.push_back(error); - std::cout << "Iteration = " << it << ", error = " << error << std::endl; - } - for (int i = 1; i < errors.size(); ++i) - { - std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; - } - } - - // - virtual double totalElasticEnergy(btScalar dt) - { - return 0; - } - - // - virtual double totalDampingEnergy(btScalar dt) - { - return 0; - } - - // total Energy takes dt as input because certain energies depend on dt - virtual double totalEnergy(btScalar dt) - { - return totalElasticEnergy(dt) + totalDampingEnergy(dt); - } + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody*> m_softBodies; + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btDeformableLagrangianForce() + { + } + + virtual ~btDeformableLagrangianForce() {} + + // add all forces + virtual void addScaledForces(btScalar scale, TVStack& force) = 0; + + // add damping df + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + + // build diagonal of A matrix + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0; + + // add elastic df + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; + + // add all forces that are explicit in explicit solve + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + + // add all damping forces + virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; + + virtual void addScaledHessian(btScalar scale) {} + + virtual btDeformableLagrangianForceType getForceType() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + // get number of nodes that have the force + virtual int getNumNodes() + { + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + numNodes += m_softBodies[i]->m_nodes.size(); + } + return numNodes; + } + + // add a soft body to be affected by the particular lagrangian force + virtual void addSoftBody(btSoftBody* psb) + { + m_softBodies.push_back(psb); + } + + virtual void removeSoftBody(btSoftBody* psb) + { + m_softBodies.remove(psb); + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } + + // Calculate the incremental deformable generated from the input dx + virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) + { + btVector3 c1 = dx[id1] - dx[id0]; + btVector3 c2 = dx[id2] - dx[id0]; + btVector3 c3 = dx[id3] - dx[id0]; + return btMatrix3x3(c1, c2, c3).transpose(); + } + + // Calculate the incremental deformable generated from the current velocity + virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) + { + btVector3 c1 = n1->m_v - n0->m_v; + btVector3 c2 = n2->m_v - n0->m_v; + btVector3 c3 = n3->m_v - n0->m_v; + return btMatrix3x3(c1, c2, c3).transpose(); + } + + // test for addScaledElasticForce function + virtual void testDerivative() + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + TVStack dx; + dx.resize(getNumNodes()); + TVStack dphi_dx; + dphi_dx.resize(dx.size()); + for (int i = 0; i < dphi_dx.size(); ++i) + { + dphi_dx[i].setZero(); + } + addScaledForces(-1, dphi_dx); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray<double> errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get dphi/dx * dx + double dphi = 0; + for (int i = 0; i < dx.size(); ++i) + { + dphi += dphi_dx[i].dot(dx[i]); + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double f1 = totalElasticEnergy(0); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + double f2 = totalElasticEnergy(0); + + //restore m_q + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = f1 - f2 - 2 * dphi; + errors.push_back(error); + std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl; + } + } + + // test for addScaledElasticForce function + virtual void testHessian() + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + TVStack dx; + dx.resize(getNumNodes()); + TVStack df; + df.resize(dx.size()); + TVStack f1; + f1.resize(dx.size()); + TVStack f2; + f2.resize(dx.size()); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray<double> errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get df + for (int i = 0; i < df.size(); ++i) + { + df[i].setZero(); + f1[i].setZero(); + f2[i].setZero(); + } + + //set df + addScaledElasticForceDifferential(-1, dx, df); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f1 + addScaledForces(-1, f1); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f2 + addScaledForces(-1, f2); + + //restore m_q + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = 0; + for (int i = 0; i < df.size(); ++i) + { + btVector3 error_vector = f1[i] - f2[i] - 2 * df[i]; + error += error_vector.length2(); + } + error = btSqrt(error); + errors.push_back(error); + std::cout << "Iteration = " << it << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl; + } + } + + // + virtual double totalElasticEnergy(btScalar dt) + { + return 0; + } + + // + virtual double totalDampingEnergy(btScalar dt) + { + return 0; + } + + // total Energy takes dt as input because certain energies depend on dt + virtual double totalEnergy(btScalar dt) + { + return totalElasticEnergy(dt) + totalDampingEnergy(dt); + } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h index 106dc10ad6..971192050b 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h @@ -18,323 +18,445 @@ #include "btDeformableLagrangianForce.h" #include "LinearMath/btQuickprof.h" +#include "btSoftBodyInternals.h" +#define TETRA_FLAT_THRESHOLD 0.01 class btDeformableLinearElasticityForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; - btScalar m_mu_damp, m_lambda_damp; - btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1) - { - btScalar damping = 0.05; - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; - } - - btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) - { - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; - // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * df_on_node0; - force[id1] -= scale1 * df_on_node123.getColumn(0); - force[id2] -= scale1 * df_on_node123.getColumn(1); - force[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetraScratches.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; - energy += tetra.m_element_measure * elasticEnergyDensity(s); - } - } - return energy; - } - - // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - double elasticEnergyDensity(const btSoftBody::TetraScratch& s) - { - double density = 0; - btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); - btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; - density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); - density += m_lambda * trace * trace * 0.5; - return density; - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar max_p = psb->m_cfg.m_maxStress; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(psb->m_tetraScratches[j],P); + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_damping_alpha, m_damping_beta; + btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1), m_damping_alpha(0.01), m_damping_beta(0.01) + { + updateYoungsModulusAndPoissonRatio(); + } + + btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta) + { + updateYoungsModulusAndPoissonRatio(); + } + + void updateYoungsModulusAndPoissonRatio() + { + // conversion from Lame Parameters to Young's modulus and Poisson ratio + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); + m_nu = m_lambda * 0.5 / (m_mu + m_lambda); + } + + void updateLameParameters() + { + // conversion from Young's modulus and Poisson ratio to Lame Parameters + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_mu = m_E * 0.5 / (1 + m_nu); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); + } + + void setYoungsModulus(btScalar E) + { + m_E = E; + updateLameParameters(); + } + + void setPoissonRatio(btScalar nu) + { + m_nu = nu; + updateLameParameters(); + } + + void setDamping(btScalar damping_alpha, btScalar damping_beta) + { + m_damping_alpha = damping_alpha; + m_damping_beta = damping_beta; + } + + void setLameParameters(btScalar mu, btScalar lambda) + { + m_mu = mu; + m_lambda = lambda; + updateYoungsModulusAndPoissonRatio(); + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_damping_alpha == 0 && m_damping_beta == 0) + return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + force[id] -= scale * node.m_v / node.m_im * m_damping_alpha; + } + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); + density += m_lambda * trace * trace * 0.5; + return density; + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); #if USE_SVD - if (max_p > 0) - { - // since we want to clamp the principal stress to max_p, we only need to - // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p - btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); - if (trPTP > max_p * max_p) - { - btMatrix3x3 U, V; - btVector3 sigma; - singularValueDecomposition(P, U, sigma, V); - sigma[0] = btMin(sigma[0], max_p); - sigma[1] = btMin(sigma[1], max_p); - sigma[2] = btMin(sigma[2], max_p); - sigma[0] = btMax(sigma[0], -max_p); - sigma[1] = btMax(sigma[1], -max_p); - sigma[2] = btMax(sigma[2], -max_p); - btMatrix3x3 Sigma; - Sigma.setIdentity(); - Sigma[0][0] = sigma[0]; - Sigma[1][1] = sigma[1]; - Sigma[2][2] = sigma[2]; - P = U * Sigma * V.transpose(); - } - } + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } #endif - // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; - // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; - btMatrix3x3 dP; - firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); - // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // elastic force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) - { - btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); - btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; - P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; - } - - // Let P be the first piola stress. - // This function calculates the dP = dP/dF * dF - void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); - dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; - } - - // Let Q be the damping stress. - // This function calculates the dP = dQ/dF * dF - void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); - dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace; - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_LINEAR_ELASTICITY_FORCE; - } - + // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_damping_alpha == 0 && m_damping_beta == 0) + return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + df[id] -= scale * dv[id] / node.m_im * m_damping_alpha; + } + } + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F; + + btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace; + } + + virtual void addScaledHessian(btScalar scale) + { + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); // make sure scratch is evaluated at x_n + dt * vn + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure; // stiff and stiffness-damping terms; + node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1; + node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1; + node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1; + node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + btMatrix3x3 I; + I.setIdentity(); + node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha); + } + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_LINEAR_ELASTICITY_FORCE; + } }; #endif /* BT_LINEAR_ELASTICITY_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h index b128df92cc..8c97bd1ba8 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h @@ -20,282 +20,282 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce { - // If true, the damping force will be in the direction of the spring - // If false, the damping force will be in the direction of the velocity - bool m_momentum_conserving; - btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + bool m_momentum_conserving; + btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; + public: - typedef btAlignedObjectArray<btVector3> TVStack; - btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) - { - } - btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) - { - if (m_bendingStiffness < btScalar(0)) - { - m_bendingStiffness = m_elasticStiffness; - } - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - - // damping force - btVector3 v_diff = (node2->m_v - node1->m_v); - btVector3 scaled_force = scale * m_dampingStiffness * v_diff; - if (m_momentum_conserving) - { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) - { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); - scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; - } - } - force[id1] += scaled_force; - force[id2] -= scaled_force; - } - } - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - btScalar r = link.m_rl; - size_t id1 = node1->index; - size_t id2 = node2->index; - - // elastic force - btVector3 dir = (node2->m_q - node1->m_q); - btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); - btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); - btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r); - force[id1] += scaled_force; - force[id2] -= scaled_force; - } - } - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - // implicit damping force differential - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar scaled_k_damp = m_dampingStiffness * scale; - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) + { + } + btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) + { + if (m_bendingStiffness < btScalar(0)) + { + m_bendingStiffness = m_elasticStiffness; + } + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + } + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r); + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; + } + } + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + for (int d = 0; d < 3; ++d) + { + if (node1->m_im > 0) + diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d]; + if (node2->m_im > 0) + diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d]; + } + } + } + else + { + for (int d = 0; d < 3; ++d) + { + if (node1->m_im > 0) + diagA[id1][d] -= scaled_k_damp; + if (node2->m_im > 0) + diagA[id2][d] -= scaled_k_damp; + } + } + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r); + } + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + btScalar r = link.m_rl; - btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); - if (m_momentum_conserving) - { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) - { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); - local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; - } - } - df[id1] += local_scaled_df; - df[id2] -= local_scaled_df; - } - } - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) - { - // implicit damping force differential - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar scaled_k_damp = m_dampingStiffness * scale; - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - if (m_momentum_conserving) - { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) - { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); - for (int d = 0; d < 3; ++d) - { - if (node1->m_im > 0) - diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d]; - if (node2->m_im > 0) - diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d]; - } - } - } - else - { - for (int d = 0; d < 3; ++d) - { - if (node1->m_im > 0) - diagA[id1][d] -= scaled_k_damp; - if (node2->m_im > 0) - diagA[id2][d] -= scaled_k_damp; - } - } - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - btScalar r = link.m_rl; + btVector3 dir = (node1->m_q - node2->m_q); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + btVector3 dx_diff = dx[id1] - dx[id2]; + btVector3 scaled_df = btVector3(0, 0, 0); + btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff; + } - // elastic force - btVector3 dir = (node2->m_q - node1->m_q); - energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r); - } - } - return energy; - } - - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - // implicit damping force differential - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - btScalar r = link.m_rl; + df[id1] += scaled_df; + df[id2] -= scaled_df; + } + } + } - btVector3 dir = (node1->m_q - node2->m_q); - btScalar dir_norm = dir.norm(); - btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); - btVector3 dx_diff = dx[id1] - dx[id2]; - btVector3 scaled_df = btVector3(0,0,0); - btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); - if (dir_norm > SIMD_EPSILON) - { - scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; - scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized; - scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff; - } - - df[id1] += scaled_df; - df[id2] -= scaled_df; - } - } - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_MASSSPRING_FORCE; - } - + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MASSSPRING_FORCE; + } }; #endif /* btMassSpring_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h index 07c10935f4..d218d96214 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h @@ -20,126 +20,143 @@ class btDeformableMousePickingForce : public btDeformableLagrangianForce { - // If true, the damping force will be in the direction of the spring - // If false, the damping force will be in the direction of the velocity - btScalar m_elasticStiffness, m_dampingStiffness; - const btSoftBody::Face& m_face; - btVector3 m_mouse_pos; - btScalar m_maxForce; + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + btScalar m_elasticStiffness, m_dampingStiffness; + const btSoftBody::Face& m_face; + btVector3 m_mouse_pos; + btScalar m_maxForce; + public: - typedef btAlignedObjectArray<btVector3> TVStack; - btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - for (int i = 0; i < 3; ++i) - { - btVector3 v_diff = m_face.m_n[i]->m_v; - btVector3 scaled_force = scale * m_dampingStiffness * v_diff; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; - } - force[m_face.m_n[i]->index] -= scaled_force; - } - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - btScalar scaled_stiffness = scale * m_elasticStiffness; - for (int i = 0; i < 3; ++i) - { - btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); - btVector3 scaled_force = scaled_stiffness * dir; - if (scaled_force.safeNorm() > m_maxForce) - { - scaled_force.safeNormalize(); - scaled_force *= m_maxForce; - } - force[m_face.m_n[i]->index] -= scaled_force; - } - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - btScalar scaled_k_damp = m_dampingStiffness * scale; - for (int i = 0; i < 3; ++i) - { - btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; - } - df[m_face.m_n[i]->index] -= local_scaled_df; - } - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < 3; ++i) - { - btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); - btVector3 scaled_force = m_elasticStiffness * dir; - if (scaled_force.safeNorm() > m_maxForce) - { - scaled_force.safeNormalize(); - scaled_force *= m_maxForce; - } - energy += 0.5 * scaled_force.dot(dir); - } - return energy; - } - - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < 3; ++i) - { - btVector3 v_diff = m_face.m_n[i]->m_v; - btVector3 scaled_force = m_dampingStiffness * v_diff; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; - } - energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; - } - return energy; - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - //TODO - } - - void setMousePos(const btVector3& p) - { - m_mouse_pos = p; - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_MOUSE_PICKING_FORCE; - } - + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = scaled_stiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int i = 0; i < 3; ++i) + { + btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; + } + df[m_face.m_n[i]->index] -= local_scaled_df; + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = m_elasticStiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + energy += 0.5 * scaled_force.dot(dir); + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; + } + energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + int id = m_face.m_n[i]->index; + btVector3 dx_diff = dx[id]; + btScalar r = 0; // rest length is 0 for picking spring + btVector3 scaled_df = btVector3(0, 0, 0); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff; + } + df[id] += scaled_df; + } + } + + void setMousePos(const btVector3& p) + { + m_mouse_pos = p; + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MOUSE_PICKING_FORCE; + } }; #endif /* btMassSpring_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index c8cc47923e..631fd5fbed 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -13,131 +13,132 @@ 3. This notice may not be removed or altered from any source distribution. */ - #include "btDeformableMultiBodyConstraintSolver.h" #include <iostream> // override the iterations method to include deformable/multibody contact -btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - { - ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + { + ///this is a special step to resolve penetrations (just for contacts) + solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + for (int iteration = 0; iteration < maxIterations; iteration++) + { + // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body - int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - for (int iteration = 0; iteration < maxIterations; iteration++) - { - // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body - - // solve rigid/rigid in solver body - m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - // solver body velocity -> rigid body velocity - solverBodyWriteBack(infoGlobal); - btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal); - // update rigid body velocity in rigid/deformable contact - m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); - // solver body velocity <- rigid body velocity - writeToSolverBody(bodies, numBodies, infoGlobal); - - if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) - { + // solve rigid/rigid in solver body + m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + // solver body velocity -> rigid body velocity + solverBodyWriteBack(infoGlobal); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal); + // update rigid body velocity in rigid/deformable contact + m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); + // solver body velocity <- rigid body velocity + writeToSolverBody(bodies, numBodies, infoGlobal); + + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) + { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); + if (iteration >= (maxIterations - 1)) + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif - m_analyticsData.m_numSolverCalls++; - m_analyticsData.m_numIterationsUsed = iteration+1; - m_analyticsData.m_islandId = -2; - if (numBodies>0) - m_analyticsData.m_islandId = bodies[0]->getCompanionId(); - m_analyticsData.m_numBodies = numBodies; - m_analyticsData.m_numContactManifolds = numManifolds; - m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; - break; - } - } - } - return 0.f; + m_analyticsData.m_numSolverCalls++; + m_analyticsData.m_numIterationsUsed = iteration + 1; + m_analyticsData.m_islandId = -2; + if (numBodies > 0) + m_analyticsData.m_islandId = bodies[0]->getCompanionId(); + m_analyticsData.m_numBodies = numBodies; + m_analyticsData.m_numContactManifolds = numManifolds; + m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; + break; + } + } + } + return 0.f; } -void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // overriden - solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlyFinish(bodies, numBodies, info); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // overriden + solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlyFinish(bodies, numBodies, info); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; } void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < numBodies; i++) - { - int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + for (int i = 0; i < numBodies; i++) + { + int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body && body->getInvMass()) - { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; - solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; - } - } + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; + solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; + } + } } void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) - { - btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; - if (body) - { - m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity); - } - } + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) + { + btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; + if (body) + { + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity); + } + } } -void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); - int iteration; - if (infoGlobal.m_splitImpulse) - { - { -// m_deformableSolver->splitImpulseSetup(infoGlobal); - for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) - { - btScalar leastSquaresResidual = 0.f; - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j = 0; j < numPoolConstraints; j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); - } - // solve the position correction between deformable and rigid/multibody -// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); -// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); - } - if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) - { + BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); + int iteration; + if (infoGlobal.m_splitImpulse) + { + { + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + { + btScalar leastSquaresResidual = 0.f; + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + for (j = 0; j < numPoolConstraints; j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + + btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + // solve the position correction between deformable and rigid/multibody + // btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); + btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) + { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration); + if (iteration >= (infoGlobal.m_numIterations - 1)) + printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration); #endif - break; - } - } - } - } + break; + } + } + } + } } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 0c7cc26a83..94aabce838 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -13,7 +13,6 @@ 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H #define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H @@ -32,30 +31,31 @@ class btDeformableBodySolver; ATTRIBUTE_ALIGNED16(class) btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver { - btDeformableBodySolver* m_deformableSolver; - + btDeformableBodySolver* m_deformableSolver; + protected: - // override the iterations method to include deformable/multibody contact -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - // write the velocity of the the solver body to the underlying rigid body - void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); - - // write the velocity of the underlying rigid body to the the the solver body - void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - - virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - - virtual btScalar solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + // override the iterations method to include deformable/multibody contact + // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + // write the velocity of the the solver body to the underlying rigid body + void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); + + // write the velocity of the underlying rigid body to the the the solver body + void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + + virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - void setDeformableSolver(btDeformableBodySolver* deformableSolver) - { - m_deformableSolver = deformableSolver; - } - - virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + BT_DECLARE_ALIGNED_ALLOCATOR(); + + void setDeformableSolver(btDeformableBodySolver * deformableSolver) + { + m_deformableSolver = deformableSolver; + } + + virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); }; #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 6b742978ef..983e622b5f 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -40,8 +40,9 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed #include "LinearMath/btQuickprof.h" #include "btSoftBodyInternals.h" btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver) -: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), -m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) + : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), + m_deformableBodySolver(deformableBodySolver), + m_solverCallback(0) { m_drawFlags = fDrawFlags::Std; m_drawNodeTree = true; @@ -52,7 +53,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_sbi.m_sparsesdf.Initialize(); m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005); m_sbi.m_sparsesdf.Reset(); - + m_sbi.air_density = (btScalar)1.2; m_sbi.water_density = 0; m_sbi.water_offset = 0; @@ -61,57 +62,57 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_internalTime = 0.0; m_implicit = false; m_lineSearch = false; - m_useProjection = true; + m_useProjection = false; m_ccdIterations = 5; m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); } btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld() { - delete m_solverDeformableBodyIslandCallback; + delete m_solverDeformableBodyIslandCallback; } void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - BT_PROFILE("internalSingleStepSimulation"); - if (0 != m_internalPreTickCallback) - { - (*m_internalPreTickCallback)(this, timeStep); - } - reinitialize(timeStep); - - // add gravity to velocity of rigid and multi bodys - applyRigidBodyGravity(timeStep); - - ///apply gravity and explicit force to velocity, predict motion - predictUnconstraintMotion(timeStep); - - ///perform collision detection that involves rigid/multi bodies - btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); - - btMultiBodyDynamicsWorld::calculateSimulationIslands(); - - beforeSolverCallbacks(timeStep); - - ///solve contact constraints and then deformable bodies momemtum equation - solveConstraints(timeStep); - - afterSolverCallbacks(timeStep); + BT_PROFILE("internalSingleStepSimulation"); + if (0 != m_internalPreTickCallback) + { + (*m_internalPreTickCallback)(this, timeStep); + } + reinitialize(timeStep); + + // add gravity to velocity of rigid and multi bodys + applyRigidBodyGravity(timeStep); + + ///apply gravity and explicit force to velocity, predict motion + predictUnconstraintMotion(timeStep); + + ///perform collision detection that involves rigid/multi bodies + btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + + btMultiBodyDynamicsWorld::calculateSimulationIslands(); + + beforeSolverCallbacks(timeStep); + + ///solve contact constraints and then deformable bodies momemtum equation + solveConstraints(timeStep); + + afterSolverCallbacks(timeStep); performDeformableCollisionDetection(); - applyRepulsionForce(timeStep); + applyRepulsionForce(timeStep); + + performGeometricCollisions(timeStep); + + integrateTransforms(timeStep); - performGeometricCollisions(timeStep); + ///update vehicle simulation + btMultiBodyDynamicsWorld::updateActions(timeStep); - integrateTransforms(timeStep); - - ///update vehicle simulation - btMultiBodyDynamicsWorld::updateActions(timeStep); - - updateActivationState(timeStep); - // End solver-wise simulation step - // /////////////////////////////// + updateActivationState(timeStep); + // End solver-wise simulation step + // /////////////////////////////// } void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() @@ -120,7 +121,7 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() { m_softBodies[i]->m_softSoftCollision = true; } - + for (int i = 0; i < m_softBodies.size(); ++i) { for (int j = i; j < m_softBodies.size(); ++j) @@ -128,7 +129,7 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]); } } - + for (int i = 0; i < m_softBodies.size(); ++i) { m_softBodies[i]->m_softSoftCollision = false; @@ -137,45 +138,45 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) { - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - psb->updateDeactivation(timeStep); - if (psb->wantsSleeping()) - { - if (psb->getActivationState() == ACTIVE_TAG) - psb->setActivationState(WANTS_DEACTIVATION); - if (psb->getActivationState() == ISLAND_SLEEPING) - { - psb->setZeroVelocity(); - } - } - else - { - if (psb->getActivationState() != DISABLE_DEACTIVATION) - psb->setActivationState(ACTIVE_TAG); - } - } - btMultiBodyDynamicsWorld::updateActivationState(timeStep); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + psb->updateDeactivation(timeStep); + if (psb->wantsSleeping()) + { + if (psb->getActivationState() == ACTIVE_TAG) + psb->setActivationState(WANTS_DEACTIVATION); + if (psb->getActivationState() == ISLAND_SLEEPING) + { + psb->setZeroVelocity(); + } + } + else + { + if (psb->getActivationState() != DISABLE_DEACTIVATION) + psb->setActivationState(ACTIVE_TAG); + } + } + btMultiBodyDynamicsWorld::updateActivationState(timeStep); } void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep) { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - if (psb->isActive()) - { + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { psb->applyRepulsionForce(timeStep, true); - } - } + } + } } void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep) { BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions"); - // refit the BVH tree for CCD + // refit the BVH tree for CCD for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -214,7 +215,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep; } } - } + } // apply CCD to register new contact points for (int i = 0; i < m_softBodies.size(); ++i) @@ -228,7 +229,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]); } } - } + } int penetration_count = 0; for (int i = 0; i < m_softBodies.size(); ++i) @@ -258,294 +259,292 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - if (psb->isActive()) - { - psb->defaultCollisionHandler(psb); - } - } + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + psb->defaultCollisionHandler(psb); + } + } } void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep) { - // correct the position of rigid bodies with temporary velocity generated from split impulse - btContactSolverInfo infoGlobal; - btVector3 zero(0,0,0); - for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) - { - btRigidBody* rb = m_nonStaticRigidBodies[i]; - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btVector3 pushVelocity = rb->getPushVelocity(); - btVector3 turnVelocity = rb->getTurnVelocity(); - if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0) - { - btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform); - rb->setWorldTransform(newTransform); - rb->setPushVelocity(zero); - rb->setTurnVelocity(zero); - } - } + // correct the position of rigid bodies with temporary velocity generated from split impulse + btContactSolverInfo infoGlobal; + btVector3 zero(0, 0, 0); + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btVector3 pushVelocity = rb->getPushVelocity(); + btVector3 turnVelocity = rb->getTurnVelocity(); + if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0) + { + btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform); + rb->setWorldTransform(newTransform); + rb->setPushVelocity(zero); + rb->setTurnVelocity(zero); + } + } } void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { - BT_PROFILE("integrateTransforms"); - positionCorrection(timeStep); - btMultiBodyDynamicsWorld::integrateTransforms(timeStep); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / timeStep; - for (int c = 0; c < 3; c++) - { - if (node.m_v[c] > clampDeltaV) - { - node.m_v[c] = clampDeltaV; - } - if (node.m_v[c] < -clampDeltaV) - { - node.m_v[c] = -clampDeltaV; - } - } - node.m_x = node.m_x + timeStep * node.m_v; - node.m_q = node.m_x; - node.m_vn = node.m_v; - } - // enforce anchor constraints - for (int j = 0; j < psb->m_deformableAnchors.size();++j) - { - btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; - btSoftBody::Node* n = a.m_node; - n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; - - // update multibody anchor info - if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 nrm; - const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); - const btTransform& wtr = multibodyLinkCol->getWorldTransform(); - psb->m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(n->m_x), - shp, - nrm, - 0); - a.m_cti.m_normal = wtr.getBasis() * nrm; - btVector3 normal = a.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - a.m_c0 = rot.transpose() * local_impulse_matrix * rot; - a.jacobianData_normal = jacobianData_normal; - a.jacobianData_t1 = jacobianData_t1; - a.jacobianData_t2 = jacobianData_t2; - a.t1 = t1; - a.t2 = t2; - } - } - } - psb->interpolateRenderMesh(); - } + BT_PROFILE("integrateTransforms"); + positionCorrection(timeStep); + btMultiBodyDynamicsWorld::integrateTransforms(timeStep); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / timeStep; + for (int c = 0; c < 3; c++) + { + if (node.m_v[c] > clampDeltaV) + { + node.m_v[c] = clampDeltaV; + } + if (node.m_v[c] < -clampDeltaV) + { + node.m_v[c] = -clampDeltaV; + } + } + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); + node.m_q = node.m_x; + node.m_vn = node.m_v; + } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + + // update multibody anchor info + if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 nrm; + const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); + const btTransform& wtr = multibodyLinkCol->getWorldTransform(); + psb->m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(n->m_x), + shp, + nrm, + 0); + a.m_cti.m_normal = wtr.getBasis() * nrm; + btVector3 normal = a.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + a.m_c0 = rot.transpose() * local_impulse_matrix * rot; + a.jacobianData_normal = jacobianData_normal; + a.jacobianData_t1 = jacobianData_t1; + a.jacobianData_t2 = jacobianData_t2; + a.t1 = t1; + a.t2 = t2; + } + } + } + psb->interpolateRenderMesh(); + } } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); - - // set up constraints among multibodies and between multibodies and deformable bodies - setupConstraints(); - - // solve contact constraints - solveContactConstraints(); - - // set up the directions in which the velocity does not change in the momentum solve - if (m_useProjection) - m_deformableBodySolver->m_objective->m_projection.setProjection(); - else - m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); - - // for explicit scheme, m_backupVelocity = v_{n+1}^* - // for implicit scheme, m_backupVelocity = v_n - // Here, set dv = v_{n+1} - v_n for nodes in contact - m_deformableBodySolver->setupDeformableSolve(m_implicit); - - // At this point, dv should be golden for nodes in contact - // proceed to solve deformable momentum equation - m_deformableBodySolver->solveDeformableConstraints(timeStep); + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); + + // set up constraints among multibodies and between multibodies and deformable bodies + setupConstraints(); + + // solve contact constraints + solveContactConstraints(); + + // set up the directions in which the velocity does not change in the momentum solve + if (m_useProjection) + m_deformableBodySolver->m_objective->m_projection.setProjection(); + else + m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); + + // for explicit scheme, m_backupVelocity = v_{n+1}^* + // for implicit scheme, m_backupVelocity = v_n + // Here, set dv = v_{n+1} - v_n for nodes in contact + m_deformableBodySolver->setupDeformableSolve(m_implicit); + + // At this point, dv should be golden for nodes in contact + // proceed to solve deformable momentum equation + m_deformableBodySolver->solveDeformableConstraints(timeStep); } void btDeformableMultiBodyDynamicsWorld::setupConstraints() { - // set up constraints between multibody and deformable bodies - m_deformableBodySolver->setConstraints(m_solverInfo); - - // set up constraints among multibodies - { - sortConstraints(); - // setup the solver callback - btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - - // build islands - m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); - } + // set up constraints between multibody and deformable bodies + m_deformableBodySolver->setConstraints(m_solverInfo); + + // set up constraints among multibodies + { + sortConstraints(); + // setup the solver callback + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + + // build islands + m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); + } } void btDeformableMultiBodyDynamicsWorld::sortConstraints() { - m_sortedConstraints.resize(m_constraints.size()); - int i; - for (i = 0; i < getNumConstraints(); i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i = 0; i < m_multiBodyConstraints.size(); i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); } - - + void btDeformableMultiBodyDynamicsWorld::solveContactConstraints() { - // process constraints on each island - m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback); - - // process deferred - m_solverDeformableBodyIslandCallback->processConstraints(); - m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); - - // write joint feedback - { - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - if (bod->internalNeedsJointFeedback()) - { - if (!bod->isUsingRK4Integration()) - { - if (bod->internalNeedsJointFeedback()) - { - bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } - } - } - } - - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->processDeltaVeeMultiDof2(); - } + // process constraints on each island + m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback); + + // process deferred + m_solverDeformableBodyIslandCallback->processConstraints(); + m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); + + // write joint feedback + { + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + if (bod->internalNeedsJointFeedback()) + { + if (!bod->isUsingRK4Integration()) + { + if (bod->internalNeedsJointFeedback()) + { + bool isConstraintPass = true; + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + } + } + } + + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); + } } void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) { - m_softBodies.push_back(body); - - // Set the soft body solver that will deal with this body - // to be the world's solver - body->setSoftBodySolver(m_deformableBodySolver); - - btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); + m_softBodies.push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver(m_deformableBodySolver); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); } void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - BT_PROFILE("predictUnconstraintMotion"); - btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); - m_deformableBodySolver->predictMotion(timeStep); + BT_PROFILE("predictUnconstraintMotion"); + btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); + m_deformableBodySolver->predictMotion(timeStep); } void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) { - m_internalTime += timeStep; - m_deformableBodySolver->setImplicit(m_implicit); - m_deformableBodySolver->setLineSearch(m_lineSearch); - m_deformableBodySolver->reinitialize(m_softBodies, timeStep); - btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); - btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; - if (m_useProjection) - { - m_deformableBodySolver->m_useProjection = true; -// m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; - m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; - } - else - { - m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; - } - + m_internalTime += timeStep; + m_deformableBodySolver->setImplicit(m_implicit); + m_deformableBodySolver->setLineSearch(m_lineSearch); + m_deformableBodySolver->reinitialize(m_softBodies, timeStep); + btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); + btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; + if (m_useProjection) + { + m_deformableBodySolver->m_useProjection = true; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; + } + else + { + m_deformableBodySolver->m_useProjection = false; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; + } } - void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() { - btMultiBodyDynamicsWorld::debugDrawWorld(); for (int i = 0; i < getSoftBodyArray().size(); i++) @@ -556,253 +555,260 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags()); } } - - } void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) { - // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again - // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep - // when there are multiple substeps - btMultiBodyDynamicsWorld::applyGravity(); - // integrate rigid body gravity - for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) - { - btRigidBody* rb = m_nonStaticRigidBodies[i]; - rb->integrateVelocities(timeStep); - } - - // integrate multibody gravity - { - forwardKinematics(); - clearMultiBodyConstraintForces(); - { - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - m_scratch_r.resize(bod->getNumLinks() + 1); - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - bool isConstraintPass = false; - { - if (!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, - m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - else - { - btAssert(" RK4Integration is not supported" ); - } - } - } - } - } - } - clearGravity(); + // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again + // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep + // when there are multiple substeps + btMultiBodyDynamicsWorld::applyGravity(); + // integrate rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + rb->integrateVelocities(timeStep); + } + + // integrate multibody gravity + { + forwardKinematics(); + clearMultiBodyConstraintForces(); + { + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + m_scratch_r.resize(bod->getNumLinks() + 1); + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool isConstraintPass = false; + { + if (!bod->isUsingRK4Integration()) + { + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + else + { + btAssert(" RK4Integration is not supported"); + } + } + } + } + } + } + clearGravity(); } void btDeformableMultiBodyDynamicsWorld::clearGravity() { - BT_PROFILE("btMultiBody clearGravity"); - // clear rigid body gravity - for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive()) - { - body->clearGravity(); - } - } - // clear multibody gravity - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - bod->addBaseForce(-m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j)); - } - } - } + BT_PROFILE("btMultiBody clearGravity"); + // clear rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) + { + body->clearGravity(); + } + } + // clear multibody gravity + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + bod->addBaseForce(-m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j)); + } + } + } } void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) { - if (0 != m_internalTickCallback) - { - (*m_internalTickCallback)(this, timeStep); - } - - if (0 != m_solverCallback) - { - (*m_solverCallback)(m_internalTime, this); - } + if (0 != m_internalTickCallback) + { + (*m_internalTickCallback)(this, timeStep); + } + + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } } void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep) { - if (0 != m_solverCallback) - { - (*m_solverCallback)(m_internalTime, this); - } + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } } void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; - bool added = false; - for (int i = 0; i < forces.size(); ++i) - { - if (forces[i]->getForceType() == force->getForceType()) - { - forces[i]->addSoftBody(psb); - added = true; - break; - } - } - if (!added) - { - force->addSoftBody(psb); - force->setIndices(m_deformableBodySolver->m_objective->getIndices()); - forces.push_back(force); - } + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + bool added = false; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->addSoftBody(psb); + added = true; + break; + } + } + if (!added) + { + force->addSoftBody(psb); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + forces.push_back(force); + } } void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; - int removed_index = -1; - for (int i = 0; i < forces.size(); ++i) - { - if (forces[i]->getForceType() == force->getForceType()) - { - forces[i]->removeSoftBody(psb); - if (forces[i]->m_softBodies.size() == 0) - removed_index = i; - break; - } - } - if (removed_index >= 0) - forces.removeAtIndex(removed_index); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + int removed_index = -1; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->removeSoftBody(psb); + if (forces[i]->m_softBodies.size() == 0) + removed_index = i; + break; + } + } + if (removed_index >= 0) + forces.removeAtIndex(removed_index); +} + +void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb) +{ + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + for (int i = 0; i < forces.size(); ++i) + { + forces[i]->removeSoftBody(psb); + } } void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) { - m_softBodies.remove(body); - btCollisionWorld::removeCollisionObject(body); - // force a reinitialize so that node indices get updated. - m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1)); + removeSoftBodyForce(body); + m_softBodies.remove(body); + btCollisionWorld::removeCollisionObject(body); + // force a reinitialize so that node indices get updated. + m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1)); } void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { - btSoftBody* body = btSoftBody::upcast(collisionObject); - if (body) - removeSoftBody(body); - else - btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); } - int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { - startProfiling(timeStep); - - int numSimulationSubSteps = 0; - - if (maxSubSteps) - { - //fixed timestep with interpolation - m_fixedTimeStep = fixedTimeStep; - m_localTime += timeStep; - if (m_localTime >= fixedTimeStep) - { - numSimulationSubSteps = int(m_localTime / fixedTimeStep); - m_localTime -= numSimulationSubSteps * fixedTimeStep; - } - } - else - { - //variable timestep - fixedTimeStep = timeStep; - m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; - m_fixedTimeStep = 0; - if (btFuzzyZero(timeStep)) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } - else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (getDebugDrawer()) - { - btIDebugDraw* debugDrawer = getDebugDrawer(); - gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; - } - if (numSimulationSubSteps) - { - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; - - saveKinematicState(fixedTimeStep * clampedSimulationSteps); - - for (int i = 0; i < clampedSimulationSteps; i++) - { - internalSingleStepSimulation(fixedTimeStep); - synchronizeMotionStates(); - } - } - else - { - synchronizeMotionStates(); - } - - clearForces(); - + startProfiling(timeStep); + + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_fixedTimeStep = fixedTimeStep; + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int(m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } + else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; + m_fixedTimeStep = 0; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } + else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (getDebugDrawer()) + { + btIDebugDraw* debugDrawer = getDebugDrawer(); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (numSimulationSubSteps) + { + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; + + saveKinematicState(fixedTimeStep * clampedSimulationSteps); + + for (int i = 0; i < clampedSimulationSteps; i++) + { + internalSingleStepSimulation(fixedTimeStep); + synchronizeMotionStates(); + } + } + else + { + synchronizeMotionStates(); + } + + clearForces(); + #ifndef BT_NO_PROFILE - CProfileManager::Increment_Frame_Counter(); + CProfileManager::Increment_Frame_Counter(); #endif //BT_NO_PROFILE - - return numSimulationSubSteps; + + return numSimulationSubSteps; } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 76b58a0378..4b7069aac7 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -36,185 +36,192 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld { - typedef btAlignedObjectArray<btVector3> TVStack; - ///Solver classes that encapsulate multiple deformable bodies for solving - btDeformableBodySolver* m_deformableBodySolver; - btSoftBodyArray m_softBodies; - int m_drawFlags; - bool m_drawNodeTree; - bool m_drawFaceTree; - bool m_drawClusterTree; - btSoftBodyWorldInfo m_sbi; - btScalar m_internalTime; - int m_ccdIterations; - bool m_implicit; - bool m_lineSearch; - bool m_useProjection; - DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; - - typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); - btSolverCallback m_solverCallback; - + typedef btAlignedObjectArray<btVector3> TVStack; + ///Solver classes that encapsulate multiple deformable bodies for solving + btDeformableBodySolver* m_deformableBodySolver; + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + btScalar m_internalTime; + int m_ccdIterations; + bool m_implicit; + bool m_lineSearch; + bool m_useProjection; + DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; + + typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); + btSolverCallback m_solverCallback; + protected: - virtual void internalSingleStepSimulation(btScalar timeStep); - - virtual void integrateTransforms(btScalar timeStep); - - void positionCorrection(btScalar timeStep); - - void solveConstraints(btScalar timeStep); - - void updateActivationState(btScalar timeStep); - - void clearGravity(); - + virtual void internalSingleStepSimulation(btScalar timeStep); + + virtual void integrateTransforms(btScalar timeStep); + + void positionCorrection(btScalar timeStep); + + void solveConstraints(btScalar timeStep); + + void updateActivationState(btScalar timeStep); + + void clearGravity(); + public: btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0); - virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); virtual void debugDrawWorld(); - void setSolverCallback(btSolverCallback cb) - { - m_solverCallback = cb; - } - - virtual ~btDeformableMultiBodyDynamicsWorld(); - - virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() - { - return (btMultiBodyDynamicsWorld*)(this); - } - - virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const - { - return (const btMultiBodyDynamicsWorld*)(this); - } - - virtual btDynamicsWorldType getWorldType() const - { - return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; - } - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); - - btSoftBodyArray& getSoftBodyArray() - { - return m_softBodies; - } - - const btSoftBodyArray& getSoftBodyArray() const - { - return m_softBodies; - } - - btSoftBodyWorldInfo& getWorldInfo() - { - return m_sbi; - } - - const btSoftBodyWorldInfo& getWorldInfo() const - { - return m_sbi; - } - - void reinitialize(btScalar timeStep); - - void applyRigidBodyGravity(btScalar timeStep); - - void beforeSolverCallbacks(btScalar timeStep); - - void afterSolverCallbacks(btScalar timeStep); - - void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); - - void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); - - void removeSoftBody(btSoftBody* body); - - void removeCollisionObject(btCollisionObject* collisionObject); - - int getDrawFlags() const { return (m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags = f; } - - void setupConstraints(); - - void performDeformableCollisionDetection(); - - void solveMultiBodyConstraints(); - - void solveContactConstraints(); - - void sortConstraints(); - - void softBodySelfCollision(); - - void setImplicit(bool implicit) - { - m_implicit = implicit; - } - - void setLineSearch(bool lineSearch) - { - m_lineSearch = lineSearch; - } - - void applyRepulsionForce(btScalar timeStep); - - void performGeometricCollisions(btScalar timeStep); - - struct btDeformableSingleRayCallback : public btBroadphaseRayCallback - { - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btDeformableMultiBodyDynamicsWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) - : m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) - { - m_rayFromTrans.setIdentity(); - m_rayFromTrans.setOrigin(m_rayFromWorld); - m_rayToTrans.setIdentity(); - m_rayToTrans.setOrigin(m_rayToWorld); - - btVector3 rayDir = (rayToWorld - rayFromWorld); - - rayDir.normalize(); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; - m_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); - } - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further ray tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + void setSolverCallback(btSolverCallback cb) + { + m_solverCallback = cb; + } + + virtual ~btDeformableMultiBodyDynamicsWorld(); + + virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() + { + return (btMultiBodyDynamicsWorld*)(this); + } + + virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const + { + return (const btMultiBodyDynamicsWorld*)(this); + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; + } + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + void reinitialize(btScalar timeStep); + + void applyRigidBodyGravity(btScalar timeStep); + + void beforeSolverCallbacks(btScalar timeStep); + + void afterSolverCallbacks(btScalar timeStep); + + void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + void removeSoftBodyForce(btSoftBody* psb); + + void removeSoftBody(btSoftBody* body); + + void removeCollisionObject(btCollisionObject* collisionObject); + + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } + + void setupConstraints(); + + void performDeformableCollisionDetection(); + + void solveMultiBodyConstraints(); + + void solveContactConstraints(); + + void sortConstraints(); + + void softBodySelfCollision(); + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } + + void setLineSearch(bool lineSearch) + { + m_lineSearch = lineSearch; + } + + void setUseProjection(bool useProjection) + { + m_useProjection = useProjection; + } + + void applyRepulsionForce(btScalar timeStep); + + void performGeometricCollisions(btScalar timeStep); + + struct btDeformableSingleRayCallback : public btBroadphaseRayCallback + { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btDeformableMultiBodyDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) + { + m_rayFromTrans.setIdentity(); + m_rayFromTrans.setOrigin(m_rayFromWorld); + m_rayToTrans.setIdentity(); + m_rayToTrans.setOrigin(m_rayToWorld); + + btVector3 rayDir = (rayToWorld - rayFromWorld); + + rayDir.normalize(); + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; #if 0 #ifdef RECALCULATE_AABB btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -225,87 +232,85 @@ public: const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; #endif #endif - //btScalar hitLambda = m_resultCallback.m_closestHitFraction; - //culling already done by broadphase - //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) - { - m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); - } - } - return true; - } - }; - - - - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const - { - BT_PROFILE("rayTest"); - /// use the broadphase to accelerate the search for objects, based on their aabb - /// and for each object with ray-aabb overlap, perform an exact ray test - btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); - + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) + { + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); + } + } + return true; + } + }; + + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const + { + BT_PROFILE("rayTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); + #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i = 0; i < this->getNumCollisionObjects(); i++) - { - rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } + for (int i = 0; i < this->getNumCollisionObjects(); i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } #endif //USE_BRUTEFORCE_RAYBROADPHASE - } - - void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) const - { - if (collisionShape->isSoftBody()) - { - btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) - { - btSoftBody::sRayCast softResult; - if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) - { - if (softResult.fraction <= resultCallback.m_closestHitFraction) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = 0; - shapeInfo.m_triangleIndex = softResult.index; - // get the normal - btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); - btVector3 normal = -rayDir; - normal.normalize(); - { - normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) - { - // normal always point toward origin of the ray - normal = -normal; - } - } - - btCollisionWorld::LocalRayResult rayResult(collisionObject, - &shapeInfo, - normal, - softResult.fraction); - bool normalInWorldSpace = true; - resultCallback.addSingleResult(rayResult, normalInWorldSpace); - } - } - } - } - else - { - btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); - } - } + } + + void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) const + { + if (collisionShape->isSoftBody()) + { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) + { + btSoftBody::sRayCast softResult; + if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + { + if (softResult.fraction <= resultCallback.m_closestHitFraction) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = 0; + shapeInfo.m_triangleIndex = softResult.index; + // get the normal + btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); + btVector3 normal = -rayDir; + normal.normalize(); + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) + { + // normal always point toward origin of the ray + normal = -normal; + } + } + + btCollisionWorld::LocalRayResult rayResult(collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult, normalInWorldSpace); + } + } + } + } + else + { + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); + } + } }; #endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h index d89bc4aca4..60798c5bcd 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -23,30 +23,30 @@ subject to the following restrictions: class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; // Lame Parameters - btScalar m_E, m_nu; // Young's modulus and Poisson ratio - btScalar m_mu_damp, m_lambda_damp; - btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) - { - btScalar damping = 0.05; - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; // Lame Parameters + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_mu_damp, m_lambda_damp; + btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1) + { + btScalar damping = 0.05; + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; updateYoungsModulusAndPoissonRatio(); - } - - btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) - { - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; + } + + btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda) + { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; updateYoungsModulusAndPoissonRatio(); - } + } void updateYoungsModulusAndPoissonRatio() { // conversion from Lame Parameters to Young's modulus and Poisson ratio // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters - m_E = m_mu * (3*m_lambda + 2*m_mu)/(m_lambda + m_mu); + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); m_nu = m_lambda * 0.5 / (m_mu + m_lambda); } @@ -55,21 +55,21 @@ public: // conversion from Young's modulus and Poisson ratio to Lame Parameters // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters m_mu = m_E * 0.5 / (1 + m_nu); - m_lambda = m_E * m_nu / ((1 + m_nu) * (1- 2*m_nu)); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); } - void setYoungsModulus(btScalar E) - { + void setYoungsModulus(btScalar E) + { m_E = E; updateLameParameters(); - } + } void setPoissonRatio(btScalar nu) { m_nu = nu; updateLameParameters(); } - + void setDamping(btScalar damping) { m_mu_damp = damping * m_mu; @@ -83,339 +83,338 @@ public: updateYoungsModulusAndPoissonRatio(); } - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; -// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + density += m_mu * 0.5 * (s.m_trace - 3.); + density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda); + density -= m_mu * 0.5 * log(s.m_trace + 1); + return density; + } - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * df_on_node0; - force[id1] -= scale1 * df_on_node123.getColumn(0); - force[id2] -= scale1 * df_on_node123.getColumn(1); - force[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetraScratches.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; - energy += tetra.m_element_measure * elasticEnergyDensity(s); - } - } - return energy; - } - - // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - double elasticEnergyDensity(const btSoftBody::TetraScratch& s) - { - double density = 0; - density += m_mu * 0.5 * (s.m_trace - 3.); - density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda); - density -= m_mu * 0.5 * log(s.m_trace+1); - return density; - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar max_p = psb->m_cfg.m_maxStress; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(psb->m_tetraScratches[j],P); + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); #ifdef USE_SVD - if (max_p > 0) - { - // since we want to clamp the principal stress to max_p, we only need to - // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p - btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); - if (trPTP > max_p * max_p) - { - btMatrix3x3 U, V; - btVector3 sigma; - singularValueDecomposition(P, U, sigma, V); - sigma[0] = btMin(sigma[0], max_p); - sigma[1] = btMin(sigma[1], max_p); - sigma[2] = btMin(sigma[2], max_p); - sigma[0] = btMax(sigma[0], -max_p); - sigma[1] = btMax(sigma[1], -max_p); - sigma[2] = btMax(sigma[2], -max_p); - btMatrix3x3 Sigma; - Sigma.setIdentity(); - Sigma[0][0] = sigma[0]; - Sigma[1][1] = sigma[1]; - Sigma[2][2] = sigma[2]; - P = U * Sigma * V.transpose(); - } - } + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } #endif -// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; -// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); -// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.))); + btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); + P = s.m_F * c1 + s.m_cofF * c2; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.)); + btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))); + btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP); + dP += s.m_cofF * c3; + } - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; - btMatrix3x3 dP; - firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); -// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // elastic force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) - { - btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.))); - btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); - P = s.m_F * c1 + s.m_cofF * c2; - } - - // Let P be the first piola stress. - // This function calculates the dP = dP/dF * dF - void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.)); - btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); - btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); - dP = dF * c1 + s.m_F * c2; - addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP); - dP += s.m_cofF * c3; - } - - // Let Q be the damping stress. - // This function calculates the dP = dQ/dF * dF - void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.))); - btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace)))); - btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); - dP = dF * c1 + s.m_F * c2; - addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP); - dP += s.m_cofF * c3; - } - - btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) - { - btScalar ans = 0; - for (int i = 0; i < 3; ++i) - { - ans += A[i].dot(B[i]); - } - return ans; - } - - // Let C(A) be the cofactor of the matrix A - // Let H = the derivative of C(A) with respect to A evaluated at F = A - // This function calculates H*dF - void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) - { - M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); - M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); - M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); - M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); - M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); - M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); - M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); - M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); - M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_NEOHOOKEAN_FORCE; - } - + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.))); + btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)))); + btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP); + dP += s.m_cofF * c3; + } + + btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) + { + btScalar ans = 0; + for (int i = 0; i < 3; ++i) + { + ans += A[i].dot(B[i]); + } + return ans; + } + + // Let C(A) be the cofactor of the matrix A + // Let H = the derivative of C(A) with respect to A evaluated at F = A + // This function calculates H*dF + void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) + { + M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); + M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); + M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); + M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); + M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); + M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); + M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); + M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); + M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_NEOHOOKEAN_FORCE; + } }; #endif /* BT_NEOHOOKEAN_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btKrylovSolver.h b/thirdparty/bullet/BulletSoftBody/btKrylovSolver.h new file mode 100644 index 0000000000..59126b47ae --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/btKrylovSolver.h @@ -0,0 +1,107 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_KRYLOV_SOLVER_H +#define BT_KRYLOV_SOLVER_H +#include <iostream> +#include <cmath> +#include <limits> +#include <LinearMath/btAlignedObjectArray.h> +#include <LinearMath/btVector3.h> +#include <LinearMath/btScalar.h> +#include "LinearMath/btQuickprof.h" + +template <class MatrixX> +class btKrylovSolver +{ + typedef btAlignedObjectArray<btVector3> TVStack; + +public: + int m_maxIterations; + btScalar m_tolerance; + btKrylovSolver(int maxIterations, btScalar tolerance) + : m_maxIterations(maxIterations), m_tolerance(tolerance) + { + } + + virtual ~btKrylovSolver() {} + + virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0; + + virtual void reinitialize(const TVStack& b) = 0; + + virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b) + { + // c = a-b + btAssert(a.size() == b.size()); + TVStack c; + c.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + { + c[i] = a[i] - b[i]; + } + return c; + } + + virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a) + { + return dot(a, a); + } + + virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a) + { + btScalar ret = 0; + for (int i = 0; i < a.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + ret = btMax(ret, btFabs(a[i][d])); + } + } + return ret; + } + + virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b) + { + btScalar ans(0); + for (int i = 0; i < a.size(); ++i) + ans += a[i].dot(b[i]); + return ans; + } + + virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) + { + // result += s*a + btAssert(a.size() == result.size()); + for (int i = 0; i < a.size(); ++i) + result[i] += s * a[i]; + } + + virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) + { + // result = a*s + b + TVStack result; + result.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + result[i] = s * a[i] + b[i]; + return result; + } + + virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance) + { + m_tolerance = tolerance; + } +}; +#endif /* BT_KRYLOV_SOLVER_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btPreconditioner.h b/thirdparty/bullet/BulletSoftBody/btPreconditioner.h index c2db448ef8..21c1106a42 100644 --- a/thirdparty/bullet/BulletSoftBody/btPreconditioner.h +++ b/thirdparty/bullet/BulletSoftBody/btPreconditioner.h @@ -19,269 +19,266 @@ class Preconditioner { public: - typedef btAlignedObjectArray<btVector3> TVStack; - virtual void operator()(const TVStack& x, TVStack& b) = 0; - virtual void reinitialize(bool nodeUpdated) = 0; - virtual ~Preconditioner(){} + typedef btAlignedObjectArray<btVector3> TVStack; + virtual void operator()(const TVStack& x, TVStack& b) = 0; + virtual void reinitialize(bool nodeUpdated) = 0; + virtual ~Preconditioner() {} }; class DefaultPreconditioner : public Preconditioner { public: - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - for (int i = 0; i < b.size(); ++i) - b[i] = x[i]; - } - virtual void reinitialize(bool nodeUpdated) - { - } - - virtual ~DefaultPreconditioner(){} + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i]; + } + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual ~DefaultPreconditioner() {} }; class MassPreconditioner : public Preconditioner { - btAlignedObjectArray<btScalar> m_inv_mass; - const btAlignedObjectArray<btSoftBody *>& m_softBodies; + btAlignedObjectArray<btScalar> m_inv_mass; + const btAlignedObjectArray<btSoftBody*>& m_softBodies; + public: - MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies) - : m_softBodies(softBodies) - { - } - - virtual void reinitialize(bool nodeUpdated) - { - if (nodeUpdated) - { - m_inv_mass.clear(); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - m_inv_mass.push_back(psb->m_nodes[j].m_im); - } - } - } - - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - btAssert(m_inv_mass.size() <= x.size()); - for (int i = 0; i < m_inv_mass.size(); ++i) - { - b[i] = x[i] * m_inv_mass[i]; - } - for (int i = m_inv_mass.size(); i < b.size(); ++i) - { - b[i] = x[i]; - } - } -}; + MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies) + : m_softBodies(softBodies) + { + } + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + m_inv_mass.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + m_inv_mass.push_back(psb->m_nodes[j].m_im); + } + } + } + + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + btAssert(m_inv_mass.size() <= x.size()); + for (int i = 0; i < m_inv_mass.size(); ++i) + { + b[i] = x[i] * m_inv_mass[i]; + } + for (int i = m_inv_mass.size(); i < b.size(); ++i) + { + b[i] = x[i]; + } + } +}; class KKTPreconditioner : public Preconditioner { - const btAlignedObjectArray<btSoftBody *>& m_softBodies; - const btDeformableContactProjection& m_projections; - const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf; - TVStack m_inv_A, m_inv_S; - const btScalar& m_dt; - const bool& m_implicit; + const btAlignedObjectArray<btSoftBody*>& m_softBodies; + const btDeformableContactProjection& m_projections; + const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf; + TVStack m_inv_A, m_inv_S; + const btScalar& m_dt; + const bool& m_implicit; + public: - KKTPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit) - : m_softBodies(softBodies) - , m_projections(projections) - , m_lf(lf) - , m_dt(dt) - , m_implicit(implicit) - { - } - - virtual void reinitialize(bool nodeUpdated) - { - if (nodeUpdated) - { - int num_nodes = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - num_nodes += psb->m_nodes.size(); - } - m_inv_A.resize(num_nodes); - } - buildDiagonalA(m_inv_A); - for (int i = 0; i < m_inv_A.size(); ++i) - { -// printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]); - for (int d = 0; d < 3; ++d) - { - m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0/ m_inv_A[i][d]; - } - } - m_inv_S.resize(m_projections.m_lagrangeMultipliers.size()); -// printf("S.size() = %d \n", m_inv_S.size()); - buildDiagonalS(m_inv_A, m_inv_S); - for (int i = 0; i < m_inv_S.size(); ++i) - { -// printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]); - for (int d = 0; d < 3; ++d) - { - m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0/ m_inv_S[i][d]; - } - } - } - - void buildDiagonalA(TVStack& diagA) const - { - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - diagA[counter] = (node.m_im == 0) ? btVector3(0,0,0) : btVector3(1.0/node.m_im, 1.0 / node.m_im, 1.0 / node.m_im); - ++counter; - } - } - if (m_implicit) - { - printf("implicit not implemented\n"); - btAssert(false); - } - for (int i = 0; i < m_lf.size(); ++i) - { - // add damping matrix - m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA); - } - } - - void buildDiagonalS(const TVStack& inv_A, TVStack& diagS) - { - for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) - { - // S[k,k] = e_k^T * C A_d^-1 C^T * e_k - const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; - btVector3& t = diagS[c]; - t.setZero(); - for (int j = 0; j < lm.m_num_constraints; ++j) - { - for (int i = 0; i < lm.m_num_nodes; ++i) - { - for (int d = 0; d < 3; ++d) - { - t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i]; - } - } - } - } - } -#define USE_FULL_PRECONDITIONER + KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit) + : m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit) + { + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + int num_nodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + num_nodes += psb->m_nodes.size(); + } + m_inv_A.resize(num_nodes); + } + buildDiagonalA(m_inv_A); + for (int i = 0; i < m_inv_A.size(); ++i) + { + // printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]); + for (int d = 0; d < 3; ++d) + { + m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d]; + } + } + m_inv_S.resize(m_projections.m_lagrangeMultipliers.size()); + // printf("S.size() = %d \n", m_inv_S.size()); + buildDiagonalS(m_inv_A, m_inv_S); + for (int i = 0; i < m_inv_S.size(); ++i) + { + // printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]); + for (int d = 0; d < 3; ++d) + { + m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d]; + } + } + } + + void buildDiagonalA(TVStack& diagA) const + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im); + ++counter; + } + } + if (m_implicit) + { + printf("implicit not implemented\n"); + btAssert(false); + } + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA); + } + } + + void buildDiagonalS(const TVStack& inv_A, TVStack& diagS) + { + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // S[k,k] = e_k^T * C A_d^-1 C^T * e_k + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + btVector3& t = diagS[c]; + t.setZero(); + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int d = 0; d < 3; ++d) + { + t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i]; + } + } + } + } + } +//#define USE_FULL_PRECONDITIONER #ifndef USE_FULL_PRECONDITIONER - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i] = x[i] * m_inv_A[i]; - } - int offset = m_inv_A.size(); - for (int i = 0; i < m_inv_S.size(); ++i) - { - b[i+offset] = x[i+offset] * m_inv_S[i]; - } - } + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } + int offset = m_inv_A.size(); + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] = x[i + offset] * m_inv_S[i]; + } + } #else - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - int offset = m_inv_A.size(); + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + int offset = m_inv_A.size(); - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i] = x[i] * m_inv_A[i]; - } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } - for (int i = 0; i < m_inv_S.size(); ++i) - { - b[i+offset].setZero(); - } + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset].setZero(); + } - for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) - { - const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; - // C * x - for (int d = 0; d < lm.m_num_constraints; ++d) - { - for (int i = 0; i < lm.m_num_nodes; ++i) - { - b[offset+c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]); - } - } - } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } - for (int i = 0; i < m_inv_S.size(); ++i) - { - b[i+offset] = b[i+offset] * m_inv_S[i]; - } + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] = b[i + offset] * m_inv_S[i]; + } - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i].setZero(); - } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i].setZero(); + } - for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) - { - // C^T * lambda - const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; - for (int i = 0; i < lm.m_num_nodes; ++i) - { - for (int j = 0; j < lm.m_num_constraints; ++j) - { - b[lm.m_indices[i]] += b[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; - } - } - } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i] = (x[i] - b[i]) * m_inv_A[i]; - } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = (x[i] - b[i]) * m_inv_A[i]; + } - TVStack t; - t.resize(b.size()); - for (int i = 0; i < m_inv_S.size(); ++i) - { - t[i+offset] = x[i+offset] * m_inv_S[i]; - } - for (int i = 0; i < m_inv_A.size(); ++i) - { - t[i].setZero(); - } - for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) - { - // C^T * lambda - const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; - for (int i = 0; i < lm.m_num_nodes; ++i) - { - for (int j = 0; j < lm.m_num_constraints; ++j) - { - t[lm.m_indices[i]] += t[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; - } - } - } - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i] += t[i] * m_inv_A[i]; - } + TVStack t; + t.resize(b.size()); + for (int i = 0; i < m_inv_S.size(); ++i) + { + t[i + offset] = x[i + offset] * m_inv_S[i]; + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + t[i].setZero(); + } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] += t[i] * m_inv_A[i]; + } - for (int i = 0; i < m_inv_S.size(); ++i) - { - b[i+offset] -= x[i+offset] * m_inv_S[i]; - } - } + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] -= x[i + offset] * m_inv_S[i]; + } + } #endif }; diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp index 81b846d7f8..d1980ea6c5 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp @@ -37,12 +37,12 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l { btAlignedObjectArray<bool> marked; btAlignedObjectArray<btDbvtNode*> newLeafNodes; - btAlignedObjectArray<std::pair<int,int> > childIds; + btAlignedObjectArray<std::pair<int, int> > childIds; btAlignedObjectArray<btAlignedObjectArray<int> > newAdj; marked.resize(N); for (int i = 0; i < N; ++i) marked[i] = false; - + // pair adjacent nodes into new(parent) node for (int i = 0; i < N; ++i) { @@ -61,7 +61,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l leafNodes[i]->parent = node; leafNodes[n]->parent = node; newLeafNodes.push_back(node); - childIds.push_back(std::make_pair(i,n)); + childIds.push_back(std::make_pair(i, n)); merged = true; marked[n] = true; break; @@ -70,7 +70,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l if (!merged) { newLeafNodes.push_back(leafNodes[i]); - childIds.push_back(std::make_pair(i,-1)); + childIds.push_back(std::make_pair(i, -1)); } marked[i] = true; } @@ -78,7 +78,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l newAdj.resize(newLeafNodes.size()); for (int i = 0; i < newLeafNodes.size(); ++i) { - for (int j = i+1; j < newLeafNodes.size(); ++j) + for (int j = i + 1; j < newLeafNodes.size(); ++j) { bool neighbor = false; const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first]; @@ -143,7 +143,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV /* Nodes */ const btScalar margin = getCollisionShape()->getMargin(); m_nodes.resize(node_count); - m_X.resize(node_count); + m_X.resize(node_count); for (int i = 0, ni = node_count; i < ni; ++i) { Node& n = m_nodes[i]; @@ -154,7 +154,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV n.m_im = n.m_im > 0 ? 1 / n.m_im : 0; n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); n.m_material = pm; - m_X[i] = n.m_x; + m_X[i] = n.m_x; } updateBounds(); setCollisionQuadrature(3); @@ -195,8 +195,8 @@ void btSoftBody::initDefaults() m_cfg.piterations = 1; m_cfg.diterations = 0; m_cfg.citerations = 4; - m_cfg.drag = 0; - m_cfg.m_maxStress = 0; + m_cfg.drag = 0; + m_cfg.m_maxStress = 0; m_cfg.collisions = fCollision::Default; m_pose.m_bvolume = false; m_pose.m_bframe = false; @@ -222,12 +222,14 @@ void btSoftBody::initDefaults() m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); m_dampingCoefficient = 1.0; - m_sleepingThreshold = .4; + m_sleepingThreshold = .04; m_useSelfCollision = false; m_collisionFlags = 0; m_softSoftCollision = false; m_maxSpeedSquared = 0; m_repulsionStiffness = 0.5; + m_gravityFactor = 1; + m_cacheBarycenter = false; m_fdbvnt = 0; } @@ -436,7 +438,7 @@ void btSoftBody::appendFace(int model, Material* mat) ZeroInitialize(f); f.m_material = mat ? mat : m_materials[0]; } - m_faces.push_back(f); + m_faces.push_back(f); } // @@ -525,94 +527,111 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca // void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) { - DeformableNodeRigidAnchor c; - btSoftBody::Node& n = m_nodes[node]; - const btScalar ima = n.m_im; - const btScalar imb = body->getInvMass(); - btVector3 nrm; - const btCollisionShape* shp = body->getCollisionShape(); - const btTransform& wtr = body->getWorldTransform(); - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(m_nodes[node].m_x), - shp, - nrm, - 0); - - c.m_cti.m_colObj = body; - c.m_cti.m_normal = wtr.getBasis() * nrm; - c.m_cti.m_offset = dst; - c.m_node = &m_nodes[node]; - const btScalar fc = m_cfg.kDF * body->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); - const btVector3 ra = n.m_x - wtr.getOrigin(); - - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; - c.m_node->m_battach = 1; - m_deformableAnchors.push_back(c); + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + const btScalar imb = body->getInvMass(); + btVector3 nrm; + const btCollisionShape* shp = body->getCollisionShape(); + const btTransform& wtr = body->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + + c.m_cti.m_colObj = body; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * body->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); +} + +void btSoftBody::removeAnchor(int node) +{ + const btSoftBody::Node& n = m_nodes[node]; + for (int i = 0; i < m_deformableAnchors.size();) + { + const DeformableNodeRigidAnchor& c = m_deformableAnchors[i]; + if (c.m_node == &n) + { + m_deformableAnchors.removeAtIndex(i); + } + else + { + i++; + } + } } // void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link) { - DeformableNodeRigidAnchor c; - btSoftBody::Node& n = m_nodes[node]; - const btScalar ima = n.m_im; - btVector3 nrm; - const btCollisionShape* shp = link->getCollisionShape(); - const btTransform& wtr = link->getWorldTransform(); - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(m_nodes[node].m_x), - shp, - nrm, - 0); - c.m_cti.m_colObj = link; - c.m_cti.m_normal = wtr.getBasis() * nrm; - c.m_cti.m_offset = dst; - c.m_node = &m_nodes[node]; - const btScalar fc = m_cfg.kDF * link->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; - btVector3 normal = c.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); - findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); - findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = link->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - const btVector3 ra = n.m_x - wtr.getOrigin(); - c.m_c1 = ra; - c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; - c.m_node->m_battach = 1; - m_deformableAnchors.push_back(c); + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + btVector3 nrm; + const btCollisionShape* shp = link->getCollisionShape(); + const btTransform& wtr = link->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + c.m_cti.m_colObj = link; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * link->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + btVector3 normal = c.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = link->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + const btVector3 ra = n.m_x - wtr.getOrigin(); + c.m_c1 = ra; + c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); } // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) @@ -731,7 +750,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack - // cos(10º) = 0.98480 + // cos(10º) = 0.98480 if (0 < n_dot_v && n_dot_v < 0.98480f) fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); @@ -817,7 +836,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack - // cos(10º) = 0.98480 + // cos(10º) = 0.98480 if (0 < n_dot_v && n_dot_v < 0.98480f) fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); @@ -882,6 +901,7 @@ void btSoftBody::setVelocity(const btVector3& velocity) if (n.m_im > 0) { n.m_v = velocity; + n.m_vn = velocity; } } } @@ -1010,66 +1030,70 @@ void btSoftBody::setVolumeDensity(btScalar density) // btVector3 btSoftBody::getLinearVelocity() { - btVector3 total_momentum = btVector3(0,0,0); - for (int i = 0; i < m_nodes.size(); ++i) - { - btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0/m_nodes[i].m_im; - total_momentum += mass * m_nodes[i].m_v; - } - btScalar total_mass = getTotalMass(); - return total_mass == 0 ? total_momentum : total_momentum / total_mass; + btVector3 total_momentum = btVector3(0, 0, 0); + for (int i = 0; i < m_nodes.size(); ++i) + { + btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im; + total_momentum += mass * m_nodes[i].m_v; + } + btScalar total_mass = getTotalMass(); + return total_mass == 0 ? total_momentum : total_momentum / total_mass; } // void btSoftBody::setLinearVelocity(const btVector3& linVel) { - btVector3 old_vel = getLinearVelocity(); - btVector3 diff = linVel - old_vel; - for (int i = 0; i < m_nodes.size(); ++i) - m_nodes[i].m_v += diff; + btVector3 old_vel = getLinearVelocity(); + btVector3 diff = linVel - old_vel; + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].m_v += diff; } // void btSoftBody::setAngularVelocity(const btVector3& angVel) { - btVector3 old_vel = getLinearVelocity(); - btVector3 com = getCenterOfMass(); - for (int i = 0; i < m_nodes.size(); ++i) - { - m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel; - } + btVector3 old_vel = getLinearVelocity(); + btVector3 com = getCenterOfMass(); + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel; + } } // btTransform btSoftBody::getRigidTransform() { - btVector3 t = getCenterOfMass(); - btMatrix3x3 S; - S.setZero(); - // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || - for (int i = 0; i < m_nodes.size(); ++i) - { - S += OuterProduct(m_X[i], t-m_nodes[i].m_x); - } - btVector3 sigma; - btMatrix3x3 U,V; - singularValueDecomposition(S,U,sigma,V); - btMatrix3x3 R = V * U.transpose(); - btTransform trs; - trs.setIdentity(); - trs.setOrigin(t); - trs.setBasis(R); - return trs; + btVector3 t = getCenterOfMass(); + btMatrix3x3 S; + S.setZero(); + // Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || + // It's important to make sure that S has the correct signs. + // SVD is only unique up to the ordering of singular values. + // SVD will manipulate U and V to ensure the ordering of singular values. If all three singular + // vaues are negative, SVD will permute colums of U to make two of them positive. + for (int i = 0; i < m_nodes.size(); ++i) + { + S -= OuterProduct(m_X[i], t - m_nodes[i].m_x); + } + btVector3 sigma; + btMatrix3x3 U, V; + singularValueDecomposition(S, U, sigma, V); + btMatrix3x3 R = V * U.transpose(); + btTransform trs; + trs.setIdentity(); + trs.setOrigin(t); + trs.setBasis(R); + return trs; } // void btSoftBody::transformTo(const btTransform& trs) { - // get the current best rigid fit - btTransform current_transform = getRigidTransform(); - // apply transform in material space - btTransform new_transform = trs * current_transform.inverse(); - transform(new_transform); + // get the current best rigid fit + btTransform current_transform = getRigidTransform(); + // apply transform in material space + btTransform new_transform = trs * current_transform.inverse(); + transform(new_transform); } // @@ -1130,7 +1154,7 @@ void btSoftBody::scale(const btVector3& scl) updateNormals(); updateBounds(); updateConstants(); - initializeDmInverse(); + initializeDmInverse(); } // @@ -2010,22 +2034,22 @@ bool btSoftBody::rayTest(const btVector3& rayFrom, } bool btSoftBody::rayFaceTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) + const btVector3& rayTo, + sRayCast& results) { if (m_faces.size() == 0) return false; else { - if (m_fdbvt.empty()) - initializeFaceTree(); + if (m_fdbvt.empty()) + initializeFaceTree(); } - - results.body = this; - results.fraction = 1.f; - results.index = -1; - - return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); + + results.body = this; + results.fraction = 1.f; + results.index = -1; + + return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); } // @@ -2056,112 +2080,111 @@ void btSoftBody::setSolver(eSolverPresets::_ preset) void btSoftBody::predictMotion(btScalar dt) { - int i, ni; - - /* Update */ - if (m_bUpdateRtCst) - { - m_bUpdateRtCst = false; - updateConstants(); - m_fdbvt.clear(); - if (m_cfg.collisions & fCollision::VF_SS) - { - initializeFaceTree(); - } - } - - /* Prepare */ - m_sst.sdt = dt * m_cfg.timescale; - m_sst.isdt = 1 / m_sst.sdt; - m_sst.velmrg = m_sst.sdt * 3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; - /* Forces */ - addVelocity(m_worldInfo->m_gravity * m_sst.sdt); - applyForces(); - /* Integrate */ - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - n.m_q = n.m_x; - btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; - { - btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / m_sst.sdt; - for (int c = 0; c < 3; c++) - { - if (deltaV[c] > clampDeltaV) - { - deltaV[c] = clampDeltaV; - } - if (deltaV[c] < -clampDeltaV) - { - deltaV[c] = -clampDeltaV; - } - } - } - n.m_v += deltaV; - n.m_x += n.m_v * m_sst.sdt; - n.m_f = btVector3(0, 0, 0); - } - /* Clusters */ - updateClusters(); - /* Bounds */ - updateBounds(); - /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) - vol; - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); - m_ndbvt.update(n.m_leaf, - vol, - n.m_v * m_sst.velmrg, - m_sst.updmrg); - } - /* Faces */ - if (!m_fdbvt.empty()) - { - for (int i = 0; i < m_faces.size(); ++i) - { - Face& f = m_faces[i]; - const btVector3 v = (f.m_n[0]->m_v + - f.m_n[1]->m_v + - f.m_n[2]->m_v) / - 3; - vol = VolumeOf(f, m_sst.radmrg); - m_fdbvt.update(f.m_leaf, - vol, - v * m_sst.velmrg, - m_sst.updmrg); - } - } - /* Pose */ - updatePose(); - /* Match */ - if (m_pose.m_bframe && (m_cfg.kMT > 0)) - { - const btMatrix3x3 posetrs = m_pose.m_rot; - for (int i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - if (n.m_im > 0) - { - const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; - n.m_x = Lerp(n.m_x, x, m_cfg.kMT); - } - } - } - /* Clear contacts */ - m_rcontacts.resize(0); - m_scontacts.resize(0); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); -} + int i, ni; + /* Update */ + if (m_bUpdateRtCst) + { + m_bUpdateRtCst = false; + updateConstants(); + m_fdbvt.clear(); + if (m_cfg.collisions & fCollision::VF_SS) + { + initializeFaceTree(); + } + } + + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); + applyForces(); + /* Integrate */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + n.m_q = n.m_x; + btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; + { + btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) + { + if (deltaV[c] > clampDeltaV) + { + deltaV[c] = clampDeltaV; + } + if (deltaV[c] < -clampDeltaV) + { + deltaV[c] = -clampDeltaV; + } + } + } + n.m_v += deltaV; + n.m_x += n.m_v * m_sst.sdt; + n.m_f = btVector3(0, 0, 0); + } + /* Clusters */ + updateClusters(); + /* Bounds */ + updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); + m_ndbvt.update(n.m_leaf, + vol, + n.m_v * m_sst.velmrg, + m_sst.updmrg); + } + /* Faces */ + if (!m_fdbvt.empty()) + { + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + vol, + v * m_sst.velmrg, + m_sst.updmrg); + } + } + /* Pose */ + updatePose(); + /* Match */ + if (m_pose.m_bframe && (m_cfg.kMT > 0)) + { + const btMatrix3x3 posetrs = m_pose.m_rot; + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + if (n.m_im > 0) + { + const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; + n.m_x = Lerp(n.m_x, x, m_cfg.kMT); + } + } + } + /* Clear contacts */ + m_rcontacts.resize(0); + m_scontacts.resize(0); + /* Optimize dbvt's */ + m_ndbvt.optimizeIncremental(1); + m_fdbvt.optimizeIncremental(1); + m_cdbvt.optimizeIncremental(1); +} // void btSoftBody::solveConstraints() @@ -2534,12 +2557,12 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, } int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, - btScalar& mint, int& index) const + btScalar& mint, int& index) const { int cnt = 0; { /* Use dbvt */ RayFromToCaster collider(rayFrom, rayTo, mint); - + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); if (collider.m_face) { @@ -2551,7 +2574,6 @@ int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, return (cnt); } - // static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) { @@ -2580,7 +2602,7 @@ static inline void calculateNormalCone(btDbvntNode* root) } else { - btVector3 n0(0,0,0), n1(0,0,0); + btVector3 n0(0, 0, 0), n1(0, 0, 0); btScalar a0 = 0, a1 = 0; if (root->childs[0]) { @@ -2594,8 +2616,8 @@ static inline void calculateNormalCone(btDbvntNode* root) n1 = root->childs[1]->normal; a1 = root->childs[1]->angle; } - root->normal = (n0+n1).safeNormalize(); - root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5; + root->normal = (n0 + n1).safeNormalize(); + root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5; } } @@ -2609,7 +2631,8 @@ void btSoftBody::initializeFaceTree() for (int i = 0; i < m_faces.size(); ++i) { Face& f = m_faces[i]; - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = VolumeOf(f, 0); btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); node->parent = NULL; node->data = &f; @@ -2623,7 +2646,7 @@ void btSoftBody::initializeFaceTree() // construct the adjacency list for triangles for (int i = 0; i < adj.size(); ++i) { - for (int j = i+1; j < adj.size(); ++j) + for (int j = i + 1; j < adj.size(); ++j) { int dup = 0; for (int k = 0; k < 3; ++k) @@ -2661,7 +2684,8 @@ void btSoftBody::rebuildNodeTree() for (int i = 0; i < m_nodes.size(); ++i) { Node& n = m_nodes[i]; - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = btDbvtVolume::FromCR(n.m_x, 0); btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); node->parent = NULL; node->data = &n; @@ -2704,61 +2728,61 @@ btVector3 btSoftBody::evaluateCom() const } bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti) const -{ - btVector3 nrm; - const btCollisionShape* shp = colObjWrap->getCollisionShape(); - // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); - //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform& wtr = colObjWrap->getWorldTransform(); - //todo: check which transform is needed here - - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(x), - shp, - nrm, - margin); - if (dst < 0) - { - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); - return (true); - } - return (false); + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); + //todo: check which transform is needed here + + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); + if (dst < 0) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); + } + return (false); } // bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti, bool predict) const + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); - // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect - // but resolve contact at x_n - btTransform wtr = (predict) ? - (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) - : colObjWrap->getWorldTransform(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), shp, nrm, margin); + if (!predict) { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = dst; + cti.m_offset = dst; } - if (dst < 0) - return true; + if (dst < 0) + return true; return (false); } @@ -2767,175 +2791,131 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr // point p with respect to triangle (a, b, c) static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary) { - btVector3 v0 = b - a, v1 = c - a, v2 = p - a; - btScalar d00 = v0.dot(v0); - btScalar d01 = v0.dot(v1); - btScalar d11 = v1.dot(v1); - btScalar d20 = v2.dot(v0); - btScalar d21 = v2.dot(v1); - btScalar denom = d00 * d11 - d01 * d01; - bary.setY((d11 * d20 - d01 * d21) / denom); - bary.setZ((d00 * d21 - d01 * d20) / denom); - bary.setX(btScalar(1) - bary.getY() - bary.getZ()); + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = v0.dot(v0); + btScalar d01 = v0.dot(v1); + btScalar d11 = v1.dot(v1); + btScalar d20 = v2.dot(v0); + btScalar d21 = v2.dot(v1); + btScalar denom = d00 * d11 - d01 * d01; + bary.setY((d11 * d20 - d01 * d21) / denom); + bary.setZ((d00 * d21 - d01 * d20) / denom); + bary.setX(btScalar(1) - bary.getY() - bary.getZ()); } // bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, - Face& f, - btVector3& contact_point, - btVector3& bary, - btScalar margin, - btSoftBody::sCti& cti, bool predict) const -{ - btVector3 nrm; - const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); - // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect - // but resolve contact at x_n - btTransform wtr = (predict) ? - (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) - : colObjWrap->getWorldTransform(); - btScalar dst; - -//#define USE_QUADRATURE 1 -//#define CACHE_PREV_COLLISION - - // use the contact position of the previous collision -#ifdef CACHE_PREV_COLLISION - if (f.m_pcontact[3] != 0) - { - for (int i = 0; i < 3; ++i) - bary[i] = f.m_pcontact[i]; - contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - dst = m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(contact_point), - shp, - nrm, - margin); - nrm = wtr.getBasis() * nrm; - cti.m_colObj = colObjWrap->getCollisionObject(); - // use cached contact point - } - else - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_x); - btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); - btVector3 guess(0,0,0); - const btConvexShape* csh = static_cast<const btConvexShape*>(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance - margin; - contact_point = results.witnesses[0]; - getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - nrm = results.normal; - cti.m_colObj = colObjWrap->getCollisionObject(); - for (int i = 0; i < 3; ++i) - f.m_pcontact[i] = bary[i]; - } - return (dst < 0); -#endif + Face& f, + btVector3& contact_point, + btVector3& bary, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + btScalar dst; + btGjkEpaSolver2::sResults results; + +// #define USE_QUADRATURE 1 - // use collision quadrature point + // use collision quadrature point #ifdef USE_QUADRATURE - { - dst = SIMD_INFINITY; - btVector3 local_nrm; - for (int q = 0; q < m_quads.size(); ++q) - { - btVector3 p; - if (predict) - p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); - else - p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); - btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(p), - shp, - local_nrm, - margin); - if (local_dst < dst) - { - if (local_dst < 0 && predict) - return true; - dst = local_dst; - contact_point = p; - bary = m_quads[q]; - nrm = local_nrm; - } - if (!predict) - { - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = dst; - } - } - return (dst < 0); - } + { + dst = SIMD_INFINITY; + btVector3 local_nrm; + for (int q = 0; q < m_quads.size(); ++q) + { + btVector3 p; + if (predict) + p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); + else + p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); + btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(p), + shp, + local_nrm, + margin); + if (local_dst < dst) + { + if (local_dst < 0 && predict) + return true; + dst = local_dst; + contact_point = p; + bary = m_quads[q]; + nrm = local_nrm; + } + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; + } + } + return (dst < 0); + } #endif - -// // regular face contact -// { -// btGjkEpaSolver2::sResults results; -// btTransform triangle_transform; -// triangle_transform.setIdentity(); -// triangle_transform.setOrigin(f.m_n[0]->m_x); -// btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); -// btVector3 guess(0,0,0); -// if (predict) -// { -// triangle_transform.setOrigin(f.m_n[0]->m_q); -// triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); -// } -// const btConvexShape* csh = static_cast<const btConvexShape*>(shp); -//// btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); -//// dst = results.distance - margin; -//// contact_point = results.witnesses[0]; -// btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results); -// if (results.status == btGjkEpaSolver2::sResults::Separated) -// return false; -// dst = results.distance - margin; -// contact_point = results.witnesses[1]; -// getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); -// nrm = results.normal; -// for (int i = 0; i < 3; ++i) -// f.m_pcontact[i] = bary[i]; -// } -// -// if (!predict) -// { -// cti.m_colObj = colObjWrap->getCollisionObject(); -// cti.m_normal = nrm; -// cti.m_offset = dst; -// } -// - - // regular face contact - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_q); - btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); - btVector3 guess(0,0,0); - const btConvexShape* csh = static_cast<const btConvexShape*>(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance-csh->getMargin(); - dst -= margin; - if (dst >= 0) - return false; - contact_point = results.witnesses[0]; - getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary); - btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - nrm = results.normal; - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = nrm; - cti.m_offset = dst + (curr - contact_point).dot(nrm); - } - return (dst < 0); + + // collision detection using x* + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_q); + btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q); + btVector3 guess(0, 0, 0); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + dst = results.distance - 2.0 * csh->getMargin() - margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable + if (dst >= 0) + return false; + + // Use consistent barycenter to recalculate distance. + if (this->m_cacheBarycenter) + { + if (f.m_pcontact[3] != 0) + { + for (int i = 0; i < 3; ++i) + bary[i] = f.m_pcontact[i]; + contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results); + cti.m_colObj = colObjWrap->getCollisionObject(); + dst = results.distance; + cti.m_normal = results.normal; + cti.m_offset = dst; + + //point-convex CD + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + + dst = results.distance - csh->getMargin() - margin; + return true; + } + } + + // Use triangle-convex CD. + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + + for (int i = 0; i < 3; ++i) + f.m_pcontact[i] = bary[i]; + + dst = results.distance - csh->getMargin() - margin; + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = results.normal; + cti.m_offset = dst; + return true; } -// void btSoftBody::updateNormals() { const btVector3 zv(0, 0, 0); @@ -2979,63 +2959,63 @@ void btSoftBody::updateBounds() m_bounds[1] = btVector3(1000, 1000, 1000); } else {*/ -// if (m_ndbvt.m_root) -// { -// const btVector3& mins = m_ndbvt.m_root->volume.Mins(); -// const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); -// const btScalar csm = getCollisionShape()->getMargin(); -// const btVector3 mrg = btVector3(csm, -// csm, -// csm) * -// 1; // ??? to investigate... -// m_bounds[0] = mins - mrg; -// m_bounds[1] = maxs + mrg; -// if (0 != getBroadphaseHandle()) -// { -// m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), -// m_bounds[0], -// m_bounds[1], -// m_worldInfo->m_dispatcher); -// } -// } -// else -// { -// m_bounds[0] = -// m_bounds[1] = btVector3(0, 0, 0); -// } - if (m_nodes.size()) - { - btVector3 mins = m_nodes[0].m_x; - btVector3 maxs = m_nodes[0].m_x; - for (int i = 1; i < m_nodes.size(); ++i) - { - for (int d = 0; d < 3; ++d) - { - if (m_nodes[i].m_x[d] > maxs[d]) - maxs[d] = m_nodes[i].m_x[d]; - if (m_nodes[i].m_x[d] < mins[d]) - mins[d] = m_nodes[i].m_x[d]; - } - } - const btScalar csm = getCollisionShape()->getMargin(); - const btVector3 mrg = btVector3(csm, - csm, - csm); - m_bounds[0] = mins - mrg; - m_bounds[1] = maxs + mrg; - if (0 != getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); - } - } - else - { - m_bounds[0] = - m_bounds[1] = btVector3(0, 0, 0); - } + // if (m_ndbvt.m_root) + // { + // const btVector3& mins = m_ndbvt.m_root->volume.Mins(); + // const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); + // const btScalar csm = getCollisionShape()->getMargin(); + // const btVector3 mrg = btVector3(csm, + // csm, + // csm) * + // 1; // ??? to investigate... + // m_bounds[0] = mins - mrg; + // m_bounds[1] = maxs + mrg; + // if (0 != getBroadphaseHandle()) + // { + // m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + // m_bounds[0], + // m_bounds[1], + // m_worldInfo->m_dispatcher); + // } + // } + // else + // { + // m_bounds[0] = + // m_bounds[1] = btVector3(0, 0, 0); + // } + if (m_nodes.size()) + { + btVector3 mins = m_nodes[0].m_x; + btVector3 maxs = m_nodes[0].m_x; + for (int i = 1; i < m_nodes.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + if (m_nodes[i].m_x[d] > maxs[d]) + maxs[d] = m_nodes[i].m_x[d]; + if (m_nodes[i].m_x[d] < mins[d]) + mins[d] = m_nodes[i].m_x[d]; + } + } + const btScalar csm = getCollisionShape()->getMargin(); + const btVector3 mrg = btVector3(csm, + csm, + csm); + m_bounds[0] = mins - mrg; + m_bounds[1] = maxs + mrg; + if (0 != getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } + } + else + { + m_bounds[0] = + m_bounds[1] = btVector3(0, 0, 0); + } } // @@ -3454,60 +3434,120 @@ void btSoftBody::dampClusters() void btSoftBody::setSpringStiffness(btScalar k) { - for (int i = 0; i < m_links.size(); ++i) - { - m_links[i].Feature::m_material->m_kLST = k; - } - m_repulsionStiffness = k; + for (int i = 0; i < m_links.size(); ++i) + { + m_links[i].Feature::m_material->m_kLST = k; + } + m_repulsionStiffness = k; +} + +void btSoftBody::setGravityFactor(btScalar gravFactor) +{ + m_gravityFactor = gravFactor; +} + +void btSoftBody::setCacheBarycenter(bool cacheBarycenter) +{ + m_cacheBarycenter = cacheBarycenter; } void btSoftBody::initializeDmInverse() { - btScalar unit_simplex_measure = 1./6.; - - for (int i = 0; i < m_tetras.size(); ++i) - { - Tetra &t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; - btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; - btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; - btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - t.m_element_measure = Dm.determinant() * unit_simplex_measure; - t.m_Dm_inverse = Dm.inverse(); - } + btScalar unit_simplex_measure = 1. / 6.; + + for (int i = 0; i < m_tetras.size(); ++i) + { + Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; + btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; + btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; + btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_element_measure = Dm.determinant() * unit_simplex_measure; + t.m_Dm_inverse = Dm.inverse(); + + // calculate the first three columns of P^{-1} + btVector3 a = t.m_n[0]->m_x; + btVector3 b = t.m_n[1]->m_x; + btVector3 c = t.m_n[2]->m_x; + btVector3 d = t.m_n[3]->m_x; + + btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]); + + btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2]; + btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2]; + btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1]; + btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2]; + btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2]; + btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1]; + btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2]; + btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2]; + btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1]; + btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2]; + btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2]; + btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1]; + + btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det); + btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det); + btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det); + + t.m_P_inv[0] = p1; + t.m_P_inv[1] = p2; + t.m_P_inv[2] = p3; + } +} + +static btScalar Dot4(const btVector4& a, const btVector4& b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; } void btSoftBody::updateDeformation() { - for (int i = 0; i < m_tetras.size(); ++i) - { - btSoftBody::Tetra& t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; - btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; - btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; - btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - t.m_F = Ds * t.m_Dm_inverse; - - btSoftBody::TetraScratch& s = m_tetraScratches[i]; - s.m_F = t.m_F; - s.m_J = t.m_F.determinant(); - btMatrix3x3 C = t.m_F.transpose()*t.m_F; - s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); - s.m_cofF = t.m_F.adjoint().transpose(); - } + btQuaternion q; + for (int i = 0; i < m_tetras.size(); ++i) + { + btSoftBody::Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; + btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; + btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_F = Ds * t.m_Dm_inverse; + + btSoftBody::TetraScratch& s = m_tetraScratches[i]; + s.m_F = t.m_F; + s.m_J = t.m_F.determinant(); + btMatrix3x3 C = t.m_F.transpose() * t.m_F; + s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); + s.m_cofF = t.m_F.adjoint().transpose(); + + btVector3 a = t.m_n[0]->m_q; + btVector3 b = t.m_n[1]->m_q; + btVector3 c = t.m_n[2]->m_q; + btVector3 d = t.m_n[3]->m_q; + btVector4 q1(a[0], b[0], c[0], d[0]); + btVector4 q2(a[1], b[1], c[1], d[1]); + btVector4 q3(a[2], b[2], c[2], d[2]); + btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]), + Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]), + Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2])); + q.setRotation(btVector3(0, 0, 1), 0); + B.extractRotation(q, 0.01); // precision of the rotation is not very important for visual correctness. + btMatrix3x3 Q(q); + s.m_corotation = Q; + } } void btSoftBody::advanceDeformation() { - updateDeformation(); - for (int i = 0; i < m_tetras.size(); ++i) - { - m_tetraScratchesTn[i] = m_tetraScratches[i]; - } + updateDeformation(); + for (int i = 0; i < m_tetras.size(); ++i) + { + m_tetraScratchesTn[i] = m_tetraScratches[i]; + } } // void btSoftBody::Joint::Prepare(btScalar dt, int) @@ -3750,7 +3790,7 @@ void btSoftBody::applyForces() // void btSoftBody::setMaxStress(btScalar maxStress) { - m_cfg.m_maxStress = maxStress; + m_cfg.m_maxStress = maxStress; } // @@ -3765,7 +3805,7 @@ void btSoftBody::interpolateRenderMesh() const Node* p2 = m_renderNodesParents[i][2]; btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x); btVector3 unit_normal = normal.normalized(); - Node& n = m_renderNodes[i]; + RenderNode& n = m_renderNodes[i]; n.m_x.setZero(); for (int j = 0; j < 3; ++j) { @@ -3778,7 +3818,7 @@ void btSoftBody::interpolateRenderMesh() { for (int i = 0; i < m_renderNodes.size(); ++i) { - Node& n = m_renderNodes[i]; + RenderNode& n = m_renderNodes[i]; n.m_x.setZero(); for (int j = 0; j < 4; ++j) { @@ -3793,13 +3833,13 @@ void btSoftBody::interpolateRenderMesh() void btSoftBody::setCollisionQuadrature(int N) { - for (int i = 0; i <= N; ++i) - { - for (int j = 0; i+j <= N; ++j) - { - m_quads.push_back(btVector3(btScalar(i)/btScalar(N), btScalar(j)/btScalar(N), btScalar(N-i-j)/btScalar(N))); - } - } + for (int i = 0; i <= N; ++i) + { + for (int j = 0; i + j <= N; ++j) + { + m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N))); + } + } } // @@ -4006,12 +4046,12 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) void btSoftBody::setSelfCollision(bool useSelfCollision) { - m_useSelfCollision = useSelfCollision; + m_useSelfCollision = useSelfCollision; } bool btSoftBody::useSelfCollision() { - return m_useSelfCollision; + return m_useSelfCollision; } // @@ -4052,23 +4092,23 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap collider.ProcessColObj(this, pcoWrap); } break; - case fCollision::SDF_RD: - { - btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); - if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) - { - const btTransform wtr = pcoWrap->getWorldTransform(); - const btScalar timemargin = 0; - const btScalar basemargin = getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) - volume; - pcoWrap->getCollisionShape()->getAabb(wtr, - mins, - maxs); - volume = btDbvtVolume::FromMM(mins, maxs); - volume.Expand(btVector3(basemargin, basemargin, basemargin)); + case fCollision::SDF_RD: + { + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) + { + const btTransform wtr = pcoWrap->getWorldTransform(); + const btScalar timemargin = 0; + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(wtr, + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); if (m_cfg.collisions & fCollision::SDF_RDN) { btSoftColliders::CollideSDF_RD docollideNode; @@ -4080,26 +4120,26 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); } - if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF))) - { - btSoftColliders::CollideSDF_RDF docollideFace; - docollideFace.psb = this; - docollideFace.m_colObj1Wrap = pcoWrap; - docollideFace.m_rigidBody = prb1; + if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF))) + { + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; docollideFace.dynmargin = basemargin + timemargin; docollideFace.stamargin = basemargin; - m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); - } - } - } - break; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + } + } + } + break; } } // void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { - BT_PROFILE("Deformable Collision"); + BT_PROFILE("Deformable Collision"); const int cf = m_cfg.collisions & psb->m_cfg.collisions; switch (cf & fCollision::SVSmask) { @@ -4137,60 +4177,60 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } } break; - case fCollision::VF_DD: - { - if (!psb->m_softSoftCollision) - return; - if (psb->isActive() || this->isActive()) - { - if (this != psb) - { - btSoftColliders::CollideVF_DD docollide; - /* common */ - docollide.mrg = getCollisionShape()->getMargin() + - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - if (psb->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - docollide.psb[0] = this; - docollide.psb[1] = psb; - docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - - /* psb1 nodes vs psb0 faces */ - if (this->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - docollide.psb[0] = psb; - docollide.psb[1] = this; - docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - } - else - { - if (psb->useSelfCollision()) - { - btSoftColliders::CollideFF_DD docollide; - docollide.mrg = 2*getCollisionShape()->getMargin(); - docollide.psb[0] = this; - docollide.psb[1] = psb; - if (this->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - /* psb0 faces vs psb0 faces */ - calculateNormalCone(this->m_fdbvnt); - this->m_fdbvt.selfCollideT(m_fdbvnt,docollide); - } - } - } - } - break; + case fCollision::VF_DD: + { + if (!psb->m_softSoftCollision) + return; + if (psb->isActive() || this->isActive()) + { + if (this != psb) + { + btSoftColliders::CollideVF_DD docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + if (psb->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + + /* psb1 nodes vs psb0 faces */ + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = 2 * getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + /* psb0 faces vs psb0 faces */ + calculateNormalCone(this->m_fdbvnt); + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); + } + } + } + } + break; default: { } @@ -4205,7 +4245,7 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) { btSoftColliders::CollideCCD docollide; /* common */ - docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin + docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin docollide.dt = psb->m_sst.sdt; /* psb0 nodes vs psb1 faces */ if (psb->m_tetras.size() > 0) @@ -4215,8 +4255,8 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.psb[0] = this; docollide.psb[1] = psb; docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + docollide.psb[1]->m_fdbvt.m_root, + docollide); /* psb1 nodes vs psb0 faces */ if (this->m_tetras.size() > 0) docollide.useFaceNormal = true; @@ -4225,8 +4265,8 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.psb[0] = psb; docollide.psb[1] = this; docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + docollide.psb[1]->m_fdbvt.m_root, + docollide); } else { @@ -4236,14 +4276,14 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.mrg = SAFE_EPSILON; docollide.psb[0] = this; docollide.psb[1] = psb; - docollide.dt = psb->m_sst.sdt; + docollide.dt = psb->m_sst.sdt; if (this->m_tetras.size() > 0) docollide.useFaceNormal = true; else docollide.useFaceNormal = false; /* psb0 faces vs psb0 faces */ calculateNormalCone(this->m_fdbvnt); // should compute this outside of this scope - this->m_fdbvt.selfCollideT(m_fdbvnt,docollide); + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); } } } @@ -4648,44 +4688,43 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ void btSoftBody::updateDeactivation(btScalar timeStep) { - if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) - return; + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + return; - if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) - { - m_deactivationTime += timeStep; - } - else - { - m_deactivationTime = btScalar(0.); - setActivationState(0); - } + if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) + { + m_deactivationTime += timeStep; + } + else + { + m_deactivationTime = btScalar(0.); + setActivationState(0); + } } - void btSoftBody::setZeroVelocity() { - for (int i = 0; i < m_nodes.size(); ++i) - { - m_nodes[i].m_v.setZero(); - } + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v.setZero(); + } } bool btSoftBody::wantsSleeping() { - if (getActivationState() == DISABLE_DEACTIVATION) - return false; + if (getActivationState() == DISABLE_DEACTIVATION) + return false; - //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) - return false; + //disable deactivation + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) + return false; - if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) - return true; + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + return true; - if (m_deactivationTime > gDeactivationTime) - { - return true; - } - return false; + if (m_deactivationTime > gDeactivationTime) + { + return true; + } + return false; } diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.h b/thirdparty/bullet/BulletSoftBody/btSoftBody.h index 6a55eccbd2..f578487b8c 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBody.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.h @@ -35,7 +35,7 @@ subject to the following restrictions: //#else #define btSoftBodyData btSoftBodyFloatData #define btSoftBodyDataName "btSoftBodyFloatData" -static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; +static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; static unsigned long seed = 243703; //#endif //BT_USE_DOUBLE_PRECISION @@ -171,10 +171,10 @@ public: CL_SELF = 0x0040, ///Cluster soft body self collision VF_DD = 0x0080, ///Vertex vs face soft vs soft handling - RVDFmask = 0x0f00, /// Rigid versus deformable face mask - SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face - SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face - SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node + RVDFmask = 0x0f00, /// Rigid versus deformable face mask + SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face + SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face + SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node /* presets */ Default = SDF_RS, END @@ -226,7 +226,7 @@ public: const btCollisionObject* m_colObj; /* Rigid body */ btVector3 m_normal; /* Outward normal */ btScalar m_offset; /* Offset from origin */ - btVector3 m_bary; /* Barycentric weights for faces */ + btVector3 m_bary; /* Barycentric weights for faces */ }; /* sMedium */ @@ -258,20 +258,29 @@ public: Material* m_material; // Material }; /* Node */ + struct RenderNode + { + btVector3 m_x; + btVector3 m_uv1; + btVector3 m_normal; + }; struct Node : Feature { btVector3 m_x; // Position btVector3 m_q; // Previous step position/Test position btVector3 m_v; // Velocity - btVector3 m_vn; // Previous step velocity + btVector3 m_vn; // Previous step velocity btVector3 m_f; // Force accumulator btVector3 m_n; // Normal btScalar m_im; // 1/mass btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data - btScalar m_penetration; // depth of penetration + int m_constrained; // depth of penetration int m_battach : 1; // Attached - int index; + int index; + btVector3 m_splitv; // velocity associated with split impulse + btMatrix3x3 m_effectiveMass; // effective mass in contact + btMatrix3x3 m_effectiveMass_inv; // inverse of effective mass }; /* Link */ ATTRIBUTE_ALIGNED16(struct) @@ -287,40 +296,47 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); }; + struct RenderFace + { + RenderNode* m_n[3]; // Node pointers + }; + /* Face */ struct Face : Feature { - Node* m_n[3]; // Node pointers - btVector3 m_normal; // Normal - btScalar m_ra; // Rest area - btDbvtNode* m_leaf; // Leaf data - btVector4 m_pcontact; // barycentric weights of the persistent contact - btVector3 m_n0, m_n1, m_vn; - int m_index; + Node* m_n[3]; // Node pointers + btVector3 m_normal; // Normal + btScalar m_ra; // Rest area + btDbvtNode* m_leaf; // Leaf data + btVector4 m_pcontact; // barycentric weights of the persistent contact + btVector3 m_n0, m_n1, m_vn; + int m_index; }; /* Tetra */ struct Tetra : Feature { - Node* m_n[4]; // Node pointers - btScalar m_rv; // Rest volume - btDbvtNode* m_leaf; // Leaf data - btVector3 m_c0[4]; // gradients - btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) - btScalar m_c2; // m_c1/sum(|g0..3|^2) - btMatrix3x3 m_Dm_inverse; // rest Dm^-1 - btMatrix3x3 m_F; - btScalar m_element_measure; + Node* m_n[4]; // Node pointers + btScalar m_rv; // Rest volume + btDbvtNode* m_leaf; // Leaf data + btVector3 m_c0[4]; // gradients + btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) + btScalar m_c2; // m_c1/sum(|g0..3|^2) + btMatrix3x3 m_Dm_inverse; // rest Dm^-1 + btMatrix3x3 m_F; + btScalar m_element_measure; + btVector4 m_P_inv[3]; // first three columns of P_inv matrix + }; + + /* TetraScratch */ + struct TetraScratch + { + btMatrix3x3 m_F; // deformation gradient F + btScalar m_trace; // trace of F^T * F + btScalar m_J; // det(F) + btMatrix3x3 m_cofF; // cofactor of F + btMatrix3x3 m_corotation; // corotatio of the tetra }; - - /* TetraScratch */ - struct TetraScratch - { - btMatrix3x3 m_F; // deformation gradient F - btScalar m_trace; // trace of F^T * F - btScalar m_J; // det(F) - btMatrix3x3 m_cofF; // cofactor of F - }; - + /* RContact */ struct RContact { @@ -331,67 +347,68 @@ public: btScalar m_c2; // ima*dt btScalar m_c3; // Friction btScalar m_c4; // Hardness - - // jacobians and unit impulse responses for multibody - btMultiBodyJacobianData jacobianData_normal; - btMultiBodyJacobianData jacobianData_t1; - btMultiBodyJacobianData jacobianData_t2; - btVector3 t1; - btVector3 t2; + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; }; - - class DeformableRigidContact - { - public: - sCti m_cti; // Contact infos - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // inverse mass of node/face - btScalar m_c3; // Friction - btScalar m_c4; // Hardness - - // jacobians and unit impulse responses for multibody - btMultiBodyJacobianData jacobianData_normal; - btMultiBodyJacobianData jacobianData_t1; - btMultiBodyJacobianData jacobianData_t2; - btVector3 t1; - btVector3 t2; - }; - - class DeformableNodeRigidContact : public DeformableRigidContact - { - public: - Node* m_node; // Owner node - }; - - class DeformableNodeRigidAnchor : public DeformableNodeRigidContact - { - public: - btVector3 m_local; // Anchor position in body space - }; - - class DeformableFaceRigidContact : public DeformableRigidContact - { - public: - Face* m_face; // Owner face - btVector3 m_contactPoint; // Contact point - btVector3 m_bary; // Barycentric weights - btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; - }; - - struct DeformableFaceNodeContact - { - Node* m_node; // Node - Face* m_face; // Face - btVector3 m_bary; // Barycentric weights - btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; - btVector3 m_normal; // Normal - btScalar m_margin; // Margin - btScalar m_friction; // Friction - btScalar m_imf; // inverse mass of the face at contact point - btScalar m_c0; // scale of the impulse matrix; - }; - + + class DeformableRigidContact + { + public: + sCti m_cti; // Contact infos + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // inverse mass of node/face + btScalar m_c3; // Friction + btScalar m_c4; // Hardness + btMatrix3x3 m_c5; // inverse effective mass + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; + }; + + class DeformableNodeRigidContact : public DeformableRigidContact + { + public: + Node* m_node; // Owner node + }; + + class DeformableNodeRigidAnchor : public DeformableNodeRigidContact + { + public: + btVector3 m_local; // Anchor position in body space + }; + + class DeformableFaceRigidContact : public DeformableRigidContact + { + public: + Face* m_face; // Owner face + btVector3 m_contactPoint; // Contact point + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + }; + + struct DeformableFaceNodeContact + { + Node* m_node; // Node + Face* m_face; // Face + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + btVector3 m_normal; // Normal + btScalar m_margin; // Margin + btScalar m_friction; // Friction + btScalar m_imf; // inverse mass of the face at contact point + btScalar m_c0; // scale of the impulse matrix; + }; + /* SContact */ struct SContact { @@ -718,19 +735,19 @@ public: tVSolverArray m_vsequence; // Velocity solvers sequence tPSolverArray m_psequence; // Position solvers sequence tPSolverArray m_dsequence; // Drift solvers sequence - btScalar drag; // deformable air drag - btScalar m_maxStress; // Maximum principle first Piola stress + btScalar drag; // deformable air drag + btScalar m_maxStress; // Maximum principle first Piola stress }; /* SolverState */ struct SolverState { //if you add new variables, always initialize them! SolverState() - :sdt(0), - isdt(0), - velmrg(0), - radmrg(0), - updmrg(0) + : sdt(0), + isdt(0), + velmrg(0), + radmrg(0), + updmrg(0) { } btScalar sdt; // dt*timescale @@ -769,9 +786,11 @@ public: typedef btAlignedObjectArray<Cluster*> tClusterArray; typedef btAlignedObjectArray<Note> tNoteArray; typedef btAlignedObjectArray<Node> tNodeArray; + typedef btAlignedObjectArray< RenderNode> tRenderNodeArray; typedef btAlignedObjectArray<btDbvtNode*> tLeafArray; typedef btAlignedObjectArray<Link> tLinkArray; typedef btAlignedObjectArray<Face> tFaceArray; + typedef btAlignedObjectArray<RenderFace> tRenderFaceArray; typedef btAlignedObjectArray<Tetra> tTetraArray; typedef btAlignedObjectArray<Anchor> tAnchorArray; typedef btAlignedObjectArray<RContact> tRContactArray; @@ -791,40 +810,42 @@ public: btSoftBodyWorldInfo* m_worldInfo; // World info tNoteArray m_notes; // Notes tNodeArray m_nodes; // Nodes - tNodeArray m_renderNodes; // Nodes + tRenderNodeArray m_renderNodes; // Render Nodes tLinkArray m_links; // Links tFaceArray m_faces; // Faces - tFaceArray m_renderFaces; // Faces + tRenderFaceArray m_renderFaces; // Faces tTetraArray m_tetras; // Tetras - btAlignedObjectArray<TetraScratch> m_tetraScratches; - btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; - tAnchorArray m_anchors; // Anchors - btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; - tRContactArray m_rcontacts; // Rigid contacts - btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; - btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; - btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; - tSContactArray m_scontacts; // Soft contacts - tJointArray m_joints; // Joints - tMaterialArray m_materials; // Materials - btScalar m_timeacc; // Time accumulator - btVector3 m_bounds[2]; // Spatial bounds - bool m_bUpdateRtCst; // Update runtime constants - btDbvt m_ndbvt; // Nodes tree - btDbvt m_fdbvt; // Faces tree - btDbvntNode* m_fdbvnt; // Faces tree with normals - btDbvt m_cdbvt; // Clusters tree - tClusterArray m_clusters; // Clusters - btScalar m_dampingCoefficient; // Damping Coefficient + btAlignedObjectArray<TetraScratch> m_tetraScratches; + btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; + tAnchorArray m_anchors; // Anchors + btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; + tRContactArray m_rcontacts; // Rigid contacts + btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; + btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; + btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; + tSContactArray m_scontacts; // Soft contacts + tJointArray m_joints; // Joints + tMaterialArray m_materials; // Materials + btScalar m_timeacc; // Time accumulator + btVector3 m_bounds[2]; // Spatial bounds + bool m_bUpdateRtCst; // Update runtime constants + btDbvt m_ndbvt; // Nodes tree + btDbvt m_fdbvt; // Faces tree + btDbvntNode* m_fdbvnt; // Faces tree with normals + btDbvt m_cdbvt; // Clusters tree + tClusterArray m_clusters; // Clusters + btScalar m_dampingCoefficient; // Damping Coefficient btScalar m_sleepingThreshold; btScalar m_maxSpeedSquared; - btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection + btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection btScalar m_repulsionStiffness; - btAlignedObjectArray<btVector3> m_X; // initial positions + btScalar m_gravityFactor; + bool m_cacheBarycenter; + btAlignedObjectArray<btVector3> m_X; // initial positions btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights; btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents; - btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation + btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation bool m_useSelfCollision; bool m_softSoftCollision; @@ -856,11 +877,11 @@ public: { return m_worldInfo; } - - void setDampingCoefficient(btScalar damping_coeff) - { - m_dampingCoefficient = damping_coeff; - } + + void setDampingCoefficient(btScalar damping_coeff) + { + m_dampingCoefficient = damping_coeff; + } ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) @@ -921,11 +942,12 @@ public: Material* mat = 0); /* Append anchor */ - void appendDeformableAnchor(int node, btRigidBody* body); - void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); - void appendAnchor(int node, + void appendDeformableAnchor(int node, btRigidBody* body); + void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); + void appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); + void removeAnchor(int node); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1); void appendLinearJoint(const LJoint::Specs& specs, Body body = Body()); @@ -976,10 +998,10 @@ public: void setLinearVelocity(const btVector3& linVel); /* Set the angular velocity of the center of mass */ void setAngularVelocity(const btVector3& angVel); - /* Get best fit rigid transform */ - btTransform getRigidTransform(); - /* Transform to given pose */ - void transformTo(const btTransform& trs); + /* Get best fit rigid transform */ + btTransform getRigidTransform(); + /* Transform to given pose */ + void transformTo(const btTransform& trs); /* Transform */ void transform(const btTransform& trs); /* Translate */ @@ -1068,11 +1090,11 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); void defaultCollisionHandler(btSoftBody* psb); - void setSelfCollision(bool useSelfCollision); - bool useSelfCollision(); - void updateDeactivation(btScalar timeStep); - void setZeroVelocity(); - bool wantsSleeping(); + void setSelfCollision(bool useSelfCollision); + bool useSelfCollision(); + void updateDeactivation(btScalar timeStep); + void setZeroVelocity(); + bool wantsSleeping(); // // Functionality to deal with new accelerated solvers. @@ -1151,8 +1173,8 @@ public: void rebuildNodeTree(); btVector3 evaluateCom() const; bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; - bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; + bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); void updatePose(); @@ -1166,14 +1188,16 @@ public: void solveClusters(btScalar sor); void applyClusters(bool drift); void dampClusters(); - void setSpringStiffness(btScalar k); - void initializeDmInverse(); - void updateDeformation(); - void advanceDeformation(); + void setSpringStiffness(btScalar k); + void setGravityFactor(btScalar gravFactor); + void setCacheBarycenter(bool cacheBarycenter); + void initializeDmInverse(); + void updateDeformation(); + void advanceDeformation(); void applyForces(); - void setMaxStress(btScalar maxStress); - void interpolateRenderMesh(); - void setCollisionQuadrature(int N); + void setMaxStress(btScalar maxStress); + void interpolateRenderMesh(); + void setCollisionQuadrature(int N); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti); @@ -1182,14 +1206,15 @@ public: static psolver_t getSolver(ePSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver); void geometricCollisionHandler(btSoftBody* psb); -#define SAFE_EPSILON SIMD_EPSILON*100.0 +#define SAFE_EPSILON SIMD_EPSILON * 100.0 void updateNode(btDbvtNode* node, bool use_velocity, bool margin) { if (node->isleaf()) { btSoftBody::Node* n = (btSoftBody::Node*)(node->data); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision if (use_velocity) { btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v}; @@ -1207,38 +1232,40 @@ public: { updateNode(node->childs[0], use_velocity, margin); updateNode(node->childs[1], use_velocity, margin); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; Merge(node->childs[0]->volume, node->childs[1]->volume, vol); node->volume = vol; } } - - void updateNodeTree(bool use_velocity, bool margin) + + void updateNodeTree(bool use_velocity, bool margin) { if (m_ndbvt.m_root) updateNode(m_ndbvt.m_root, use_velocity, margin); } - template <class DBVTNODE> // btDbvtNode or btDbvntNode + template <class DBVTNODE> // btDbvtNode or btDbvntNode void updateFace(DBVTNODE* node, bool use_velocity, bool margin) { if (node->isleaf()) { btSoftBody::Face* f = (btSoftBody::Face*)(node->data); - btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; if (use_velocity) { btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v, - f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, - f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; + f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, + f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; vol = btDbvtVolume::FromPoints(points, 6); } else { btVector3 points[3] = {f->m_n[0]->m_x, - f->m_n[1]->m_x, - f->m_n[2]->m_x}; + f->m_n[1]->m_x, + f->m_n[2]->m_x}; vol = btDbvtVolume::FromPoints(points, 3); } vol.Expand(btVector3(pad, pad, pad)); @@ -1249,7 +1276,8 @@ public: { updateFace(node->childs[0], use_velocity, margin); updateFace(node->childs[1], use_velocity, margin); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; Merge(node->childs[0]->volume, node->childs[1]->volume, vol); node->volume = vol; } @@ -1271,7 +1299,7 @@ public: return (a * coord.x() + b * coord.y() + c * coord.z()); } - void applyRepulsionForce(btScalar timeStep, bool applySpringForce) + void applyRepulsionForce(btScalar timeStep, bool applySpringForce) { btAlignedObjectArray<int> indices; { @@ -1297,58 +1325,60 @@ public: const btVector3& n = c.m_normal; btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w); btScalar d = c.m_margin - n.dot(l); - d = btMax(btScalar(0),d); - + d = btMax(btScalar(0), d); + const btVector3& va = node->m_v; btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w); btVector3 vr = va - vb; - const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing + const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep) continue; - btVector3 vt = vr - vn*n; + btVector3 vt = vr - vn * n; btScalar I = 0; - btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im; + btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im; if (applySpringForce) I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); if (vn < 0) I += 0.5 * mass * vn; - btScalar face_penetration = 0, node_penetration = node->m_penetration; + int face_penetration = 0, node_penetration = node->m_constrained; for (int i = 0; i < 3; ++i) - face_penetration = btMax(face_penetration, face->m_n[i]->m_penetration); - btScalar I_tilde = .5 *I /(1.0+w.length2()); - -// double the impulse if node or face is constrained. - if (face_penetration > 0 || node_penetration > 0) - I_tilde *= 2.0; - if (face_penetration <= node_penetration) + face_penetration |= face->m_n[i]->m_constrained; + btScalar I_tilde = 2.0 * I / (1.0 + w.length2()); + + // double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) + { + I_tilde *= 2.0; + } + if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) - face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im; + face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im; } - if (face_penetration >= node_penetration) + if (node_penetration <= 0) { - node->m_v -= I_tilde*node->m_im*n; + node->m_v -= I_tilde * node->m_im * n; } - + // apply frictional impulse btScalar vt_norm = vt.safeNorm(); if (vt_norm > SIMD_EPSILON) { btScalar delta_vn = -2 * I * node->m_im; btScalar mu = c.m_friction; - btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0))*vt_norm; - I = 0.5 * mass * (vt_norm-vt_new); + btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm; + I = 0.5 * mass * (vt_norm - vt_new); vt.safeNormalize(); - I_tilde = .5 *I /(1.0+w.length2()); -// double the impulse if node or face is constrained. -// if (face_penetration > 0 || node_penetration > 0) -// I_tilde *= 2.0; - if (face_penetration <= node_penetration) + I_tilde = 2.0 * I / (1.0 + w.length2()); + // double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) + I_tilde *= 2.0; + if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im; } - if (face_penetration >= node_penetration) + if (node_penetration <= 0) { node->m_v -= I_tilde * node->m_im * vt; } @@ -1356,7 +1386,7 @@ public: } } virtual int calculateSerializeBufferSize() const; - + ///fills the dataBuffer and returns the struct name (and 0 on failure) virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; }; diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp index c1a87c7d57..f63e48f9a5 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp @@ -727,7 +727,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const int resy, int fixeds, bool gendiags, - btScalar perturbation) + btScalar perturbation) { #define IDX(_x_, _y_) ((_y_)*rx + (_x_)) /* Create nodes */ @@ -747,12 +747,12 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const for (int ix = 0; ix < rx; ++ix) { const btScalar tx = ix / (btScalar)(rx - 1); - btScalar pert = perturbation * btScalar(rand())/RAND_MAX; - btVector3 temp1 = py1; - temp1.setY(py1.getY() + pert); - btVector3 temp = py0; - pert = perturbation * btScalar(rand())/RAND_MAX; - temp.setY(py0.getY() + pert); + btScalar pert = perturbation * btScalar(rand()) / RAND_MAX; + btVector3 temp1 = py1; + temp1.setY(py1.getY() + pert); + btVector3 temp = py0; + pert = perturbation * btScalar(rand()) / RAND_MAX; + temp.setY(py0.getY() + pert); x[IDX(ix, iy)] = lerp(temp, temp1, tx); m[IDX(ix, iy)] = 1; } @@ -1233,9 +1233,9 @@ if(face&&face[0]) } } } - psb->initializeDmInverse(); - psb->m_tetraScratches.resize(psb->m_tetras.size()); - psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Links: %u\r\n", psb->m_links.size()); printf("Faces: %u\r\n", psb->m_faces.size()); @@ -1245,61 +1245,62 @@ if(face&&face[0]) btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) { - std::ifstream fs; - fs.open(vtk_file); - btAssert(fs); - - typedef btAlignedObjectArray<int> Index; - std::string line; - btAlignedObjectArray<btVector3> X; - btVector3 position; - btAlignedObjectArray<Index> indices; - bool reading_points = false; - bool reading_tets = false; - size_t n_points = 0; - size_t n_tets = 0; - size_t x_count = 0; - size_t indices_count = 0; - while (std::getline(fs, line)) - { - std::stringstream ss(line); - if (line.size() == (size_t)(0)) - { - } - else if (line.substr(0, 6) == "POINTS") - { - reading_points = true; - reading_tets = false; - ss.ignore(128, ' '); // ignore "POINTS" - ss >> n_points; - X.resize(n_points); - } - else if (line.substr(0, 5) == "CELLS") - { - reading_points = false; - reading_tets = true; - ss.ignore(128, ' '); // ignore "CELLS" - ss >> n_tets; - indices.resize(n_tets); - } - else if (line.substr(0, 10) == "CELL_TYPES") - { - reading_points = false; - reading_tets = false; - } - else if (reading_points) - { - btScalar p; - ss >> p; - position.setX(p); - ss >> p; - position.setY(p); - ss >> p; - position.setZ(p); - X[x_count++] = position; - } - else if (reading_tets) - { + std::ifstream fs; + fs.open(vtk_file); + btAssert(fs); + + typedef btAlignedObjectArray<int> Index; + std::string line; + btAlignedObjectArray<btVector3> X; + btVector3 position; + btAlignedObjectArray<Index> indices; + bool reading_points = false; + bool reading_tets = false; + size_t n_points = 0; + size_t n_tets = 0; + size_t x_count = 0; + size_t indices_count = 0; + while (std::getline(fs, line)) + { + std::stringstream ss(line); + if (line.size() == (size_t)(0)) + { + } + else if (line.substr(0, 6) == "POINTS") + { + reading_points = true; + reading_tets = false; + ss.ignore(128, ' '); // ignore "POINTS" + ss >> n_points; + X.resize(n_points); + } + else if (line.substr(0, 5) == "CELLS") + { + reading_points = false; + reading_tets = true; + ss.ignore(128, ' '); // ignore "CELLS" + ss >> n_tets; + indices.resize(n_tets); + } + else if (line.substr(0, 10) == "CELL_TYPES") + { + reading_points = false; + reading_tets = false; + } + else if (reading_points) + { + btScalar p; + ss >> p; + position.setX(p); + ss >> p; + position.setY(p); + ss >> p; + position.setZ(p); + //printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ()); + X[x_count++] = position; + } + else if (reading_tets) + { int d; ss >> d; if (d != 4) @@ -1308,317 +1309,355 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, fs.close(); return 0; } - ss.ignore(128, ' '); // ignore "4" - Index tet; - tet.resize(4); - for (size_t i = 0; i < 4; i++) - { - ss >> tet[i]; - printf("%d ", tet[i]); - } - printf("\n"); - indices[indices_count++] = tet; - } - } - btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); - - for (int i = 0; i < n_tets; ++i) - { - const Index& ni = indices[i]; - psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); - { - psb->appendLink(ni[0], ni[1], 0, true); - psb->appendLink(ni[1], ni[2], 0, true); - psb->appendLink(ni[2], ni[0], 0, true); - psb->appendLink(ni[0], ni[3], 0, true); - psb->appendLink(ni[1], ni[3], 0, true); - psb->appendLink(ni[2], ni[3], 0, true); - } - } - - - generateBoundaryFaces(psb); - psb->initializeDmInverse(); - psb->m_tetraScratches.resize(psb->m_tetras.size()); - psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); - printf("Nodes: %u\r\n", psb->m_nodes.size()); - printf("Links: %u\r\n", psb->m_links.size()); - printf("Faces: %u\r\n", psb->m_faces.size()); - printf("Tetras: %u\r\n", psb->m_tetras.size()); - - fs.close(); - return psb; + ss.ignore(128, ' '); // ignore "4" + Index tet; + tet.resize(4); + for (size_t i = 0; i < 4; i++) + { + ss >> tet[i]; + //printf("%d ", tet[i]); + } + //printf("\n"); + indices[indices_count++] = tet; + } + } + btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); + + for (int i = 0; i < n_tets; ++i) + { + const Index& ni = indices[i]; + psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); + { + psb->appendLink(ni[0], ni[1], 0, true); + psb->appendLink(ni[1], ni[2], 0, true); + psb->appendLink(ni[2], ni[0], 0, true); + psb->appendLink(ni[0], ni[3], 0, true); + psb->appendLink(ni[1], ni[3], 0, true); + psb->appendLink(ni[2], ni[3], 0, true); + } + } + + generateBoundaryFaces(psb); + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + printf("Nodes: %u\r\n", psb->m_nodes.size()); + printf("Links: %u\r\n", psb->m_links.size()); + printf("Faces: %u\r\n", psb->m_faces.size()); + printf("Tetras: %u\r\n", psb->m_tetras.size()); + + fs.close(); + return psb; } void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb) { - int counter = 0; - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - psb->m_nodes[i].index = counter++; - } - typedef btAlignedObjectArray<int> Index; - btAlignedObjectArray<Index> indices; - indices.resize(psb->m_tetras.size()); - for (int i = 0; i < indices.size(); ++i) - { - Index index; - index.push_back(psb->m_tetras[i].m_n[0]->index); - index.push_back(psb->m_tetras[i].m_n[1]->index); - index.push_back(psb->m_tetras[i].m_n[2]->index); - index.push_back(psb->m_tetras[i].m_n[3]->index); - indices[i] = index; - } - - std::map<std::vector<int>, std::vector<int> > dict; - for (int i = 0; i < indices.size(); ++i) - { - for (int j = 0; j < 4; ++j) - { - std::vector<int> f; - if (j == 0) - { - f.push_back(indices[i][1]); - f.push_back(indices[i][0]); - f.push_back(indices[i][2]); - } - if (j == 1) - { - f.push_back(indices[i][3]); - f.push_back(indices[i][0]); - f.push_back(indices[i][1]); - } - if (j == 2) - { - f.push_back(indices[i][3]); - f.push_back(indices[i][1]); - f.push_back(indices[i][2]); - } - if (j == 3) - { - f.push_back(indices[i][2]); - f.push_back(indices[i][0]); - f.push_back(indices[i][3]); - } - std::vector<int> f_sorted = f; - std::sort(f_sorted.begin(), f_sorted.end()); - if (dict.find(f_sorted) != dict.end()) - { - dict.erase(f_sorted); - } - else - { - dict.insert(std::make_pair(f_sorted, f)); - } - } - } - - for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) - { - std::vector<int> f = it->second; - psb->appendFace(f[0], f[1], f[2]); - } + int counter = 0; + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + psb->m_nodes[i].index = counter++; + } + typedef btAlignedObjectArray<int> Index; + btAlignedObjectArray<Index> indices; + indices.resize(psb->m_tetras.size()); + for (int i = 0; i < indices.size(); ++i) + { + Index index; + index.push_back(psb->m_tetras[i].m_n[0]->index); + index.push_back(psb->m_tetras[i].m_n[1]->index); + index.push_back(psb->m_tetras[i].m_n[2]->index); + index.push_back(psb->m_tetras[i].m_n[3]->index); + indices[i] = index; + } + + std::map<std::vector<int>, std::vector<int> > dict; + for (int i = 0; i < indices.size(); ++i) + { + for (int j = 0; j < 4; ++j) + { + std::vector<int> f; + if (j == 0) + { + f.push_back(indices[i][1]); + f.push_back(indices[i][0]); + f.push_back(indices[i][2]); + } + if (j == 1) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][0]); + f.push_back(indices[i][1]); + } + if (j == 2) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][1]); + f.push_back(indices[i][2]); + } + if (j == 3) + { + f.push_back(indices[i][2]); + f.push_back(indices[i][0]); + f.push_back(indices[i][3]); + } + std::vector<int> f_sorted = f; + std::sort(f_sorted.begin(), f_sorted.end()); + if (dict.find(f_sorted) != dict.end()) + { + dict.erase(f_sorted); + } + else + { + dict.insert(std::make_pair(f_sorted, f)); + } + } + } + + for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) + { + std::vector<int> f = it->second; + psb->appendFace(f[0], f[1], f[2]); + //printf("f %d %d %d\n", f[0] + 1, f[1] + 1, f[2] + 1); + } } +//Write the surface mesh to an obj file. void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) { - std::ofstream fs; - fs.open(filename); - btAssert(fs); - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - fs << "v"; - for (int d = 0; d < 3; d++) - { - fs << " " << psb->m_nodes[i].m_x[d]; - } - fs << "\n"; - } - - for (int i = 0; i < psb->m_faces.size(); ++i) - { - fs << "f"; - for (int n = 0; n < 3; n++) - { - fs << " " << psb->m_faces[i].m_n[n]->index + 1; - } - fs << "\n"; - } - fs.close(); + std::ofstream fs; + fs.open(filename); + btAssert(fs); + + if (psb->m_tetras.size() > 0) + { + // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/ + std::map<int, int> dict; + for (int i = 0; i < psb->m_faces.size(); i++) + { + for (int d = 0; d < 3; d++) + { + int index = psb->m_faces[i].m_n[d]->index; + if (dict.find(index) == dict.end()) + { + int dict_size = dict.size(); + dict[index] = dict_size; + fs << "v"; + for (int k = 0; k < 3; k++) + { + fs << " " << psb->m_nodes[index].m_x[k]; + } + fs << "\n"; + } + } + } + // Write surface mesh. + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1; + } + fs << "\n"; + } + } + else + { + // For trimesh, directly write out all the nodes and faces.xs + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_x[d]; + } + fs << "\n"; + } + + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << psb->m_faces[i].m_n[n]->index + 1; + } + fs << "\n"; + } + } + fs.close(); } void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) { - std::ifstream fs_read; - fs_read.open(filename); - std::string line; - btVector3 pos; - btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; - while (std::getline(fs_read, line)) - { - std::stringstream ss(line); - if (line[0] == 'v') - { - } - else if (line[0] == 'f') - { - ss.ignore(); - int id0, id1, id2; - ss >> id0; - ss >> id1; - ss >> id2; - btAlignedObjectArray<int> new_face; - new_face.push_back(id1); - new_face.push_back(id0); - new_face.push_back(id2); - additional_faces.push_back(new_face); - } - } - fs_read.close(); - - std::ofstream fs_write; - fs_write.open(filename, std::ios_base::app); - for (int i = 0; i < additional_faces.size(); ++i) - { - fs_write << "f"; - for (int n = 0; n < 3; n++) - { - fs_write << " " << additional_faces[i][n]; - } - fs_write << "\n"; - } - fs_write.close(); + std::ifstream fs_read; + fs_read.open(filename); + std::string line; + btVector3 pos; + btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; + while (std::getline(fs_read, line)) + { + std::stringstream ss(line); + if (line[0] == 'v') + { + } + else if (line[0] == 'f') + { + ss.ignore(); + int id0, id1, id2; + ss >> id0; + ss >> id1; + ss >> id2; + btAlignedObjectArray<int> new_face; + new_face.push_back(id1); + new_face.push_back(id0); + new_face.push_back(id2); + additional_faces.push_back(new_face); + } + } + fs_read.close(); + + std::ofstream fs_write; + fs_write.open(filename, std::ios_base::app); + for (int i = 0; i < additional_faces.size(); ++i) + { + fs_write << "f"; + for (int n = 0; n < 3; n++) + { + fs_write << " " << additional_faces[i][n]; + } + fs_write << "\n"; + } + fs_write.close(); } // Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary) { - btVector3 vap = p - a; - btVector3 vbp = p - b; - - btVector3 vab = b - a; - btVector3 vac = c - a; - btVector3 vad = d - a; - - btVector3 vbc = c - b; - btVector3 vbd = d - b; - btScalar va6 = (vbp.cross(vbd)).dot(vbc); - btScalar vb6 = (vap.cross(vac)).dot(vad); - btScalar vc6 = (vap.cross(vad)).dot(vab); - btScalar vd6 = (vap.cross(vab)).dot(vac); - btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); - bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6); + btVector3 vap = p - a; + btVector3 vbp = p - b; + + btVector3 vab = b - a; + btVector3 vac = c - a; + btVector3 vad = d - a; + + btVector3 vbc = c - b; + btVector3 vbd = d - b; + btScalar va6 = (vbp.cross(vbd)).dot(vbc); + btScalar vb6 = (vap.cross(vac)).dot(vad); + btScalar vc6 = (vap.cross(vad)).dot(vab); + btScalar vd6 = (vap.cross(vab)).dot(vac); + btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); + bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6); } // Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0. void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary) { - btVector3 v0 = b - a, v1 = c - a, v2 = p - a; - btScalar d00 = btDot(v0, v0); - btScalar d01 = btDot(v0, v1); - btScalar d11 = btDot(v1, v1); - btScalar d20 = btDot(v2, v0); - btScalar d21 = btDot(v2, v1); - btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); - bary[1] = (d11 * d20 - d01 * d21) * invDenom; - bary[2] = (d00 * d21 - d01 * d20) * invDenom; - bary[0] = 1.0 - bary[1] - bary[2]; - bary[3] = 0; + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = btDot(v0, v0); + btScalar d01 = btDot(v0, v1); + btScalar d11 = btDot(v1, v1); + btScalar d20 = btDot(v2, v0); + btScalar d21 = btDot(v2, v1); + btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); + bary[1] = (d11 * d20 - d01 * d21) * invDenom; + bary[2] = (d00 * d21 - d01 * d20) * invDenom; + bary[0] = 1.0 - bary[1] - bary[2]; + bary[3] = 0; } // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) { - psb->m_z.resize(0); - psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); - psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); - for (int i = 0; i < psb->m_renderNodes.size(); ++i) - { - const btVector3& p = psb->m_renderNodes[i].m_x; - btVector4 bary; - btVector4 optimal_bary; - btScalar min_bary_weight = -1e3; - btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - const btSoftBody::Tetra& t = psb->m_tetras[j]; - getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); - btScalar new_min_bary_weight = bary[0]; - for (int k = 1; k < 4; ++k) - { - new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); - } - if (new_min_bary_weight > min_bary_weight) - { - btAlignedObjectArray<const btSoftBody::Node*> parents; - parents.push_back(t.m_n[0]); - parents.push_back(t.m_n[1]); - parents.push_back(t.m_n[2]); - parents.push_back(t.m_n[3]); - optimal_parents = parents; - optimal_bary = bary; - min_bary_weight = new_min_bary_weight; - // stop searching if p is inside the tetrahedron at hand - if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.) - { - break; - } - } - } - psb->m_renderNodesInterpolationWeights[i] = optimal_bary; - psb->m_renderNodesParents[i] = optimal_parents; - } + psb->m_z.resize(0); + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -1e3; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + const btSoftBody::Tetra& t = psb->m_tetras[j]; + getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 4; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + if (new_min_bary_weight > min_bary_weight) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(t.m_n[0]); + parents.push_back(t.m_n[1]); + parents.push_back(t.m_n[2]); + parents.push_back(t.m_n[3]); + optimal_parents = parents; + optimal_bary = bary; + min_bary_weight = new_min_bary_weight; + // stop searching if p is inside the tetrahedron at hand + if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.) + { + break; + } + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + } } - // Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense. void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) { - psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); - psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); - psb->m_z.resize(psb->m_renderNodes.size()); - for (int i = 0; i < psb->m_renderNodes.size(); ++i) - { - const btVector3& p = psb->m_renderNodes[i].m_x; - btVector4 bary; - btVector4 optimal_bary; - btScalar min_bary_weight = -SIMD_INFINITY; - btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; - btScalar dist = 0, optimal_dist = 0; - for (int j = 0; j < psb->m_faces.size(); ++j) - { - const btSoftBody::Face& f = psb->m_faces[j]; - btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); - btVector3 unit_n = n.normalized(); - dist = (p-f.m_n[0]->m_x).dot(unit_n); - btVector3 proj_p = p - dist*unit_n; - getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); - btScalar new_min_bary_weight = bary[0]; - for (int k = 1; k < 3; ++k) - { - new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); - } - - // p is out of the current best triangle, we found a traingle that's better - bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.); - // p is inside of the current best triangle, we found a triangle that's better - bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist)); - - if (better_than_closest_outisde || better_than_best_inside) - { - btAlignedObjectArray<const btSoftBody::Node*> parents; - parents.push_back(f.m_n[0]); - parents.push_back(f.m_n[1]); - parents.push_back(f.m_n[2]); - optimal_parents = parents; - optimal_bary = bary; - optimal_dist = dist; - min_bary_weight = new_min_bary_weight; - } - } - psb->m_renderNodesInterpolationWeights[i] = optimal_bary; - psb->m_renderNodesParents[i] = optimal_parents; - psb->m_z[i] = optimal_dist; - } + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + psb->m_z.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -SIMD_INFINITY; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + btScalar dist = 0, optimal_dist = 0; + for (int j = 0; j < psb->m_faces.size(); ++j) + { + const btSoftBody::Face& f = psb->m_faces[j]; + btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + btVector3 unit_n = n.normalized(); + dist = (p - f.m_n[0]->m_x).dot(unit_n); + btVector3 proj_p = p - dist * unit_n; + getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 3; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + + // p is out of the current best triangle, we found a traingle that's better + bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.); + // p is inside of the current best triangle, we found a triangle that's better + bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist)); + + if (better_than_closest_outisde || better_than_best_inside) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(f.m_n[0]); + parents.push_back(f.m_n[1]); + parents.push_back(f.m_n[2]); + optimal_parents = parents; + optimal_bary = bary; + optimal_dist = dist; + min_bary_weight = new_min_bary_weight; + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + psb->m_z[i] = optimal_dist; + } } diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h index abe1870890..237d29761d 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h @@ -93,7 +93,7 @@ struct btSoftBodyHelpers int resy, int fixeds, bool gendiags, - btScalar perturbation = 0.); + btScalar perturbation = 0.); /* Create a patch with UV Texture Coordinates */ static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, const btVector3& corner00, @@ -142,21 +142,21 @@ struct btSoftBodyHelpers bool bfacelinks, bool btetralinks, bool bfacesfromtetras); - static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); + static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); - static void writeObj(const char* file, const btSoftBody* psb); - - static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); - - static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); - - static void interpolateBarycentricWeights(btSoftBody* psb); - - static void extrapolateBarycentricWeights(btSoftBody* psb); - - static void generateBoundaryFaces(btSoftBody* psb); - - static void duplicateFaces(const char* filename, const btSoftBody* psb); + static void writeObj(const char* file, const btSoftBody* psb); + + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); + + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); + + static void interpolateBarycentricWeights(btSoftBody* psb); + + static void extrapolateBarycentricWeights(btSoftBody* psb); + + static void generateBoundaryFaces(btSoftBody* psb); + + static void duplicateFaces(const char* filename, const btSoftBody* psb); /// Sort the list of links to move link calculations that are dependent upon earlier /// ones as far as possible away from the calculation of those values /// This tends to make adjacent loop iterations not dependent upon one another, diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h index b9ebc95b6b..c17bbb5cd4 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h @@ -32,86 +32,85 @@ subject to the following restrictions: // Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, - btMultiBodyJacobianData& jacobianData, - const btVector3& contact_point, - const btVector3& dir) -{ - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - jacobianData.m_jacobians.resize(ndof); - jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); - btScalar* jac = &jacobianData.m_jacobians[0]; - - multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); - multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac = &jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); } static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u) { - btScalar ux = u.getX(); - btScalar uy = u.getY(); - btScalar uz = u.getZ(); - btScalar ax = std::abs(ux); - btScalar ay = std::abs(uy); - btScalar az = std::abs(uz); - btVector3 v; - if (ax <= ay && ax <= az) - v = btVector3(0, -uz, uy); - else if (ay <= ax && ay <= az) - v = btVector3(-uz, 0, ux); - else - v = btVector3(-uy, ux, 0); - v.normalize(); - return v; + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; } static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary) { - btVector3 x43 = x4-x3; - if (std::abs(x43.dot(normal)) > mrg) - return false; - btVector3 x13 = x1-x3; - btVector3 x23 = x2-x3; - btScalar a11 = x13.length2(); - btScalar a22 = x23.length2(); - btScalar a12 = x13.dot(x23); - btScalar b1 = x13.dot(x43); - btScalar b2 = x23.dot(x43); - btScalar det = a11*a22 - a12*a12; - if (det < SIMD_EPSILON) - return false; - btScalar w1 = (b1*a22-b2*a12)/det; - btScalar w2 = (b2*a11-b1*a12)/det; - btScalar w3 = 1-w1-w2; - btScalar delta = mrg / std::sqrt(0.5*std::abs(x13.cross(x23).safeNorm())); - bary = btVector3(w1,w2,w3); - for (int i = 0; i < 3; ++i) - { - if (bary[i] < -delta || bary[i] > 1+delta) - return false; - } - return true; + btVector3 x43 = x4 - x3; + if (std::abs(x43.dot(normal)) > mrg) + return false; + btVector3 x13 = x1 - x3; + btVector3 x23 = x2 - x3; + btScalar a11 = x13.length2(); + btScalar a22 = x23.length2(); + btScalar a12 = x13.dot(x23); + btScalar b1 = x13.dot(x43); + btScalar b2 = x23.dot(x43); + btScalar det = a11 * a22 - a12 * a12; + if (det < SIMD_EPSILON) + return false; + btScalar w1 = (b1 * a22 - b2 * a12) / det; + btScalar w2 = (b2 * a11 - b1 * a12) / det; + btScalar w3 = 1 - w1 - w2; + btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm())); + bary = btVector3(w1, w2, w3); + for (int i = 0; i < 3; ++i) + { + if (bary[i] < -delta || bary[i] > 1 + delta) + return false; + } + return true; } static const int KDOP_COUNT = 13; -static btVector3 dop[KDOP_COUNT]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1), - btVector3(1,1,0), - btVector3(1,0,1), - btVector3(0,1,1), - btVector3(1,-1,0), - btVector3(1,0,-1), - btVector3(0,1,-1), - btVector3(1,1,1), - btVector3(1,-1,1), - btVector3(1,1,-1), - btVector3(1,-1,-1) -}; +static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1), + btVector3(1, 1, 0), + btVector3(1, 0, 1), + btVector3(0, 1, 1), + btVector3(1, -1, 0), + btVector3(1, 0, -1), + btVector3(0, 1, -1), + btVector3(1, 1, 1), + btVector3(1, -1, 1), + btVector3(1, 1, -1), + btVector3(1, -1, -1)}; static inline int getSign(const btVector3& n, const btVector3& x) { btScalar d = n.dot(x); - if (d>SIMD_EPSILON) + if (d > SIMD_EPSILON) return 1; - if (d<-SIMD_EPSILON) + if (d < -SIMD_EPSILON) return -1; return 0; } @@ -119,13 +118,12 @@ static inline int getSign(const btVector3& n, const btVector3& x) static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x, - face->m_n[1]->m_x - node->m_x, - face->m_n[2]->m_x - node->m_x, - face->m_n[0]->m_x + dt*face->m_n[0]->m_v - node->m_x, - face->m_n[1]->m_x + dt*face->m_n[1]->m_v - node->m_x, - face->m_n[2]->m_x + dt*face->m_n[2]->m_v - node->m_x - }; - btVector3 segment = dt*node->m_v; + face->m_n[1]->m_x - node->m_x, + face->m_n[2]->m_x - node->m_x, + face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x, + face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x, + face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x}; + btVector3 segment = dt * node->m_v; for (int i = 0; i < KDOP_COUNT; ++i) { int s = getSign(dop[i], segment); @@ -143,488 +141,494 @@ static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, c static SIMD_FORCE_INLINE bool nearZero(const btScalar& a) { - return (a>-SAFE_EPSILON && a<SAFE_EPSILON); + return (a > -SAFE_EPSILON && a < SAFE_EPSILON); } static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b) { - return (nearZero(a) || nearZero(b) || (a>SAFE_EPSILON && b>SAFE_EPSILON) || (a<-SAFE_EPSILON && b<-SAFE_EPSILON)); + return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON)); } static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b) { - return !sameSign(a, b); -} -inline btScalar evaluateBezier2(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &t, const btScalar &s) -{ - btScalar s2 = s*s; - btScalar t2 = t*t; - - return p0*s2+p1*btScalar(2.0)*s*t+p2*t2; -} -inline btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &p3, const btScalar &t, const btScalar &s) -{ - btScalar s2 = s*s; - btScalar s3 = s2*s; - btScalar t2 = t*t; - btScalar t3 = t2*t; - - return p0*s3+p1*btScalar(3.0)*s2*t+p2*btScalar(3.0)*s*t2+p3*t3; -} -static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar <0, btScalar <1) -{ - if (sameSign(t0, t1)) { - lt0 = t0; - lt1 = t0; - return true; - } - - if (type_c || diffSign(k0, k3)) { - btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); - if (t0<-0) - ft = -ft; - - if (sameSign(ft, k0)) { - lt0 = t1; - lt1 = t1; - } - else { - lt0 = t0; - lt1 = t0; - } - return true; - } - - if (!type_c) { - btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); - if (t0<-0) - ft = -ft; - - if (diffSign(ft, k0)) { - lt0 = t0; - lt1 = t1; - return true; - } - - btScalar fk = evaluateBezier2(k1-k0, k2-k1, k3-k2, t0, -t1); - - if (sameSign(fk, k1-k0)) - lt0 = lt1 = t1; - else - lt0 = lt1 = t0; - - return true; - } - return false; + return !sameSign(a, b); +} +inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s) +{ + btScalar s2 = s * s; + btScalar t2 = t * t; + + return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2; +} +inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s) +{ + btScalar s2 = s * s; + btScalar s3 = s2 * s; + btScalar t2 = t * t; + btScalar t3 = t2 * t; + + return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3; +} +static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1) +{ + if (sameSign(t0, t1)) + { + lt0 = t0; + lt1 = t0; + return true; + } + + if (type_c || diffSign(k0, k3)) + { + btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); + if (t0 < -0) + ft = -ft; + + if (sameSign(ft, k0)) + { + lt0 = t1; + lt1 = t1; + } + else + { + lt0 = t0; + lt1 = t0; + } + return true; + } + + if (!type_c) + { + btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); + if (t0 < -0) + ft = -ft; + + if (diffSign(ft, k0)) + { + lt0 = t0; + lt1 = t1; + return true; + } + + btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1); + + if (sameSign(fk, k1 - k0)) + lt0 = lt1 = t1; + else + lt0 = lt1 = t0; + + return true; + } + return false; } static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3) { - const btVector3& n0 = face->m_n0; - const btVector3& n1 = face->m_n1; - btVector3 n_hat = n0 + n1 - face->m_vn; - btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x; - btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q; - k0 = (p0ma0).dot(n0) * 3.0; - k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0); - k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1); - k3 = (p1ma1).dot(n1) * 3.0; + const btVector3& n0 = face->m_n0; + const btVector3& n1 = face->m_n1; + btVector3 n_hat = n0 + n1 - face->m_vn; + btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x; + btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q; + k0 = (p0ma0).dot(n0) * 3.0; + k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0); + k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1); + k3 = (p1ma1).dot(n1) * 3.0; } static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1) { - btScalar denom = 4.0 * (j1-j2) * (j1-j0) + (j2-j0) * (j2-j0); - u0 = (2.0*(j1-j2)*(3.0*k1-2.0*k0-k3) - (j0-j2)*(3.0*k2-2.0*k3-k0)) / denom; - u1 = (2.0*(j1-j0)*(3.0*k2-2.0*k3-k0) - (j2-j0)*(3.0*k1-2.0*k0-k3)) / denom; - v0 = k0-u0*j0; - v1 = k3-u1*j2; + btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0); + u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom; + u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom; + v0 = k0 - u0 * j0; + v1 = k3 - u1 * j2; } static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3) { - btScalar u0, u1, v0, v1; - btScalar j0 = 3.0*(k1-k0); - btScalar j1 = 3.0*(k2-k1); - btScalar j2 = 3.0*(k3-k2); - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (sameSign(v0, v1)) - { - btScalar Ypa = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*v0*(1.0-v0) + j2*v0*v0; // Y'(v0) - if (sameSign(Ypa, j0)) - { - return (diffSign(k0,v1)); - } - } - return diffSign(k0,v0); -} - -static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2) -{ - const btVector3& a0 = a->m_x; - const btVector3& b0 = b->m_x; - const btVector3& c0 = c->m_x; - const btVector3& va = a->m_v; - const btVector3& vb = b->m_v; - const btVector3& vc = c->m_v; - const btVector3 a1 = a0 + dt*va; - const btVector3 b1 = b0 + dt*vb; - const btVector3 c1 = c0 + dt*vc; - btVector3 n0 = (b0-a0).cross(c0-a0); - btVector3 n1 = (b1-a1).cross(c1-a1); - btVector3 n_hat = n0+n1 - dt*dt*(vb-va).cross(vc-va); - const btVector3& p0 = p->m_x; - const btVector3& vp = p->m_v; - btVector3 p1 = p0 + dt*vp; - btVector3 m0 = (b0-p0).cross(c0-p0); - btVector3 m1 = (b1-p1).cross(c1-p1); - btVector3 m_hat = m0+m1 - dt*dt*(vb-vp).cross(vc-vp); - btScalar l0 = m0.dot(n0); - btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0)); - btScalar l2 = btScalar(1)/btScalar(6)*(m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0)); - btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat)); - btScalar l4 = m1.dot(n1); - - btScalar k1p = 0.25 * k0 + 0.75 * k1; - btScalar k2p = 0.5 * k1 + 0.5 * k2; - btScalar k3p = 0.75 * k2 + 0.25 * k3; - - btScalar s0 = (l1 * k0 - l0 * k1p)*4.0; - btScalar s1 = (l2 * k0 - l0 * k2p)*2.0; - btScalar s2 = (l3 * k0 - l0 * k3p)*btScalar(4)/btScalar(3); - btScalar s3 = l4 * k0 - l0 * k3; - - j0 = (s1*k0 - s0*k1) * 3.0; - j1 = (s2*k0 - s0*k2) * 1.5; - j2 = (s3*k0 - s0*k3); + btScalar u0, u1, v0, v1; + btScalar j0 = 3.0 * (k1 - k0); + btScalar j1 = 3.0 * (k2 - k1); + btScalar j2 = 3.0 * (k3 - k2); + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (sameSign(v0, v1)) + { + btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0; // Y'(v0) + if (sameSign(Ypa, j0)) + { + return (diffSign(k0, v1)); + } + } + return diffSign(k0, v0); +} + +static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2) +{ + const btVector3& a0 = a->m_x; + const btVector3& b0 = b->m_x; + const btVector3& c0 = c->m_x; + const btVector3& va = a->m_v; + const btVector3& vb = b->m_v; + const btVector3& vc = c->m_v; + const btVector3 a1 = a0 + dt * va; + const btVector3 b1 = b0 + dt * vb; + const btVector3 c1 = c0 + dt * vc; + btVector3 n0 = (b0 - a0).cross(c0 - a0); + btVector3 n1 = (b1 - a1).cross(c1 - a1); + btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va); + const btVector3& p0 = p->m_x; + const btVector3& vp = p->m_v; + btVector3 p1 = p0 + dt * vp; + btVector3 m0 = (b0 - p0).cross(c0 - p0); + btVector3 m1 = (b1 - p1).cross(c1 - p1); + btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp); + btScalar l0 = m0.dot(n0); + btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0)); + btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0)); + btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat)); + btScalar l4 = m1.dot(n1); + + btScalar k1p = 0.25 * k0 + 0.75 * k1; + btScalar k2p = 0.5 * k1 + 0.5 * k2; + btScalar k3p = 0.75 * k2 + 0.25 * k3; + + btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0; + btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0; + btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3); + btScalar s3 = l4 * k0 - l0 * k3; + + j0 = (s1 * k0 - s0 * k1) * 3.0; + j1 = (s2 * k0 - s0 * k2) * 1.5; + j2 = (s3 * k0 - s0 * k3); } static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) { - btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0) - btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(v0) + btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) + btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(v0) - btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0; - btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0; - sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; - btScalar K = (sameSign(sign_Ytp,k0)) ? v1 : v0; - return diffSign(L,K); + btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0; + btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0; + sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; + btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0; + return diffSign(L, K); } static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) { - btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0) - btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2; - if (diffSign(sign_Ytp,k0)) - { - L1 = u0; - L2 = u1; - } - else - { - btScalar Yp_u0 = j0*(1.0-u0)*(1.0-u0) + 2.0*j1*(1.0-u0)*u0 + j2*u0*u0; - if (sameSign(Yp_u0,j0)) - { - L1 = u1; - L2 = u1; - } - else - { - L1 = u0; - L2 = u0; - } - } - btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(uv0) - sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; - btScalar K1, K2; - if (diffSign(sign_Ytp,k0)) - { - K1 = v0; - K2 = v1; - } - else - { - btScalar Yp_v0 = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*(1.0-v0)*v0 + j2*v0*v0; - if (sameSign(Yp_v0,j0)) - { - K1 = v1; - K2 = v1; - } - else - { - K1 = v0; - K2 = v0; - } - } - return (diffSign(K1, L1) || diffSign(L2, K2)); + btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) + btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2; + if (diffSign(sign_Ytp, k0)) + { + L1 = u0; + L2 = u1; + } + else + { + btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0; + if (sameSign(Yp_u0, j0)) + { + L1 = u1; + L2 = u1; + } + else + { + L1 = u0; + L2 = u0; + } + } + btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(uv0) + sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; + btScalar K1, K2; + if (diffSign(sign_Ytp, k0)) + { + K1 = v0; + K2 = v1; + } + else + { + btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0; + if (sameSign(Yp_v0, j0)) + { + K1 = v1; + K2 = v1; + } + else + { + K1 = v0; + K2 = v0; + } + } + return (diffSign(K1, L1) || diffSign(L2, K2)); } static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - btScalar j0, j1, j2, u0, u1, v0, v1; - // p1 - getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - // p2 - getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - // p3 - getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - return true; + btScalar j0, j1, j2, u0, u1, v0, v1; + // p1 + getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + // p2 + getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + // p3 + getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + return true; } static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - btScalar j0, j1, j2, u0, u1, v0, v1; - // p1 - getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0 = true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - // p2 - getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0=true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - // p3 - getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0=true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - return true; + btScalar j0, j1, j2, u0, u1, v0, v1; + // p1 + getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + // p2 + getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + // p3 + getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + return true; } static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - // Coplanar test - if (diffSign(k1-k0, k3-k2)) - { - // Case b: - if (sameSign(k0, k3) && !rootFindingLemma(k0,k1,k2,k3)) - return false; - // inside test - return signDetermination2(k0, k1, k2, k3, face, node, dt); - } - else - { - // Case c: - if (sameSign(k0, k3)) - return false; - // inside test - return signDetermination1(k0, k1, k2, k3, face, node, dt); - } - return false; + // Coplanar test + if (diffSign(k1 - k0, k3 - k2)) + { + // Case b: + if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3)) + return false; + // inside test + return signDetermination2(k0, k1, k2, k3, face, node, dt); + } + else + { + // Case c: + if (sameSign(k0, k3)) + return false; + // inside test + return signDetermination1(k0, k1, k2, k3, face, node, dt); + } + return false; } static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg) { - if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg) - return true; - if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg) - return true; - return false; + if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg) + return true; + if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg) + return true; + return false; } static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - if (conservativeCulling(k0, k1, k2, k3, mrg)) - return false; - return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); + if (conservativeCulling(k0, k1, k2, k3, mrg)) + return false; + return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); } static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12) { - k10 = k0*(1.0-t0) + k1*t0; - btScalar k11 = k1*(1.0-t0) + k2*t0; - k12 = k2*(1.0-t0) + k3*t0; - k20 = k10*(1.0-t0) + k11*t0; - k21 = k11*(1.0-t0) + k12*t0; - k30 = k20*(1.0-t0) + k21*t0; + k10 = k0 * (1.0 - t0) + k1 * t0; + btScalar k11 = k1 * (1.0 - t0) + k2 * t0; + k12 = k2 * (1.0 - t0) + k3 * t0; + k20 = k10 * (1.0 - t0) + k11 * t0; + k21 = k11 * (1.0 - t0) + k12 * t0; + k30 = k20 * (1.0 - t0) + k21 * t0; } static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg) { - btScalar k0, k1, k2, k3; - getBernsteinCoeff(face, node, dt, k0, k1, k2, k3); - if (conservativeCulling(k0, k1, k2, k3, mrg)) - return false; - return true; - if (diffSign(k2-2.0*k1+k0, k3-2.0*k2+k1)) - { - btScalar k10, k20, k30, k21, k12; - btScalar t0 = (k2-2.0*k1+k0)/(k0-3.0*k1+3.0*k2-k3); - deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12); - return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt); - } - return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); + btScalar k0, k1, k2, k3; + getBernsteinCoeff(face, node, dt, k0, k1, k2, k3); + if (conservativeCulling(k0, k1, k2, k3, mrg)) + return false; + return true; + if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1)) + { + btScalar k10, k20, k30, k21, k12; + btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3); + deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12); + return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt); + } + return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); } static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) { - if (hasSeparatingPlane(face, node, dt)) - return false; - btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x; - btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x; - btVector3 x41 = node->m_x - face->m_n[0]->m_x; - btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v; - btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v; - btVector3 v41 = node->m_v - face->m_n[0]->m_v; - btVector3 a = x21.cross(x31); - btVector3 b = x21.cross(v31) + v21.cross(x31); - btVector3 c = v21.cross(v31); - btVector3 d = x41; - btVector3 e = v41; - btScalar a0 = a.dot(d); - btScalar a1 = a.dot(e) + b.dot(d); - btScalar a2 = c.dot(d) + b.dot(e); - btScalar a3 = c.dot(e); - btScalar eps = SAFE_EPSILON; - int num_roots = 0; - btScalar roots[3]; - if (std::abs(a3) < eps) - { - // cubic term is zero - if (std::abs(a2) < eps) - { - if (std::abs(a1) < eps) - { - if (std::abs(a0) < eps) - { - num_roots = 2; - roots[0] = 0; - roots[1] = dt; - } - } - else - { - num_roots = 1; - roots[0] = -a0/a1; - } - } - else - { - num_roots = SolveP2(roots, a1/a2, a0/a2); - } - } - else - { - num_roots = SolveP3(roots, a2/a3, a1/a3, a0/a3); - } -// std::sort(roots, roots+num_roots); - if (num_roots > 1) - { - if (roots[0] > roots[1]) - btSwap(roots[0], roots[1]); - } - if (num_roots > 2) - { - if (roots[0] > roots[2]) - btSwap(roots[0], roots[2]); - if (roots[1] > roots[2]) - btSwap(roots[1], roots[2]); - } - for (int r = 0; r < num_roots; ++r) - { - double root = roots[r]; - if (root <= 0) - continue; - if (root > dt + SIMD_EPSILON) - return false; - btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v; - btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v; - btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v; - btVector3 x4 = node->m_x + root * node->m_v; - btVector3 normal = (x2-x1).cross(x3-x1); - normal.safeNormalize(); - if (proximityTest(x1, x2, x3, x4, normal, mrg, bary)) - return true; - } - return false; + if (hasSeparatingPlane(face, node, dt)) + return false; + btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x; + btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x; + btVector3 x41 = node->m_x - face->m_n[0]->m_x; + btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v; + btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v; + btVector3 v41 = node->m_v - face->m_n[0]->m_v; + btVector3 a = x21.cross(x31); + btVector3 b = x21.cross(v31) + v21.cross(x31); + btVector3 c = v21.cross(v31); + btVector3 d = x41; + btVector3 e = v41; + btScalar a0 = a.dot(d); + btScalar a1 = a.dot(e) + b.dot(d); + btScalar a2 = c.dot(d) + b.dot(e); + btScalar a3 = c.dot(e); + btScalar eps = SAFE_EPSILON; + int num_roots = 0; + btScalar roots[3]; + if (std::abs(a3) < eps) + { + // cubic term is zero + if (std::abs(a2) < eps) + { + if (std::abs(a1) < eps) + { + if (std::abs(a0) < eps) + { + num_roots = 2; + roots[0] = 0; + roots[1] = dt; + } + } + else + { + num_roots = 1; + roots[0] = -a0 / a1; + } + } + else + { + num_roots = SolveP2(roots, a1 / a2, a0 / a2); + } + } + else + { + num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3); + } + // std::sort(roots, roots+num_roots); + if (num_roots > 1) + { + if (roots[0] > roots[1]) + btSwap(roots[0], roots[1]); + } + if (num_roots > 2) + { + if (roots[0] > roots[2]) + btSwap(roots[0], roots[2]); + if (roots[1] > roots[2]) + btSwap(roots[1], roots[2]); + } + for (int r = 0; r < num_roots; ++r) + { + double root = roots[r]; + if (root <= 0) + continue; + if (root > dt + SIMD_EPSILON) + return false; + btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v; + btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v; + btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v; + btVector3 x4 = node->m_x + root * node->m_v; + btVector3 normal = (x2 - x1).cross(x3 - x1); + normal.safeNormalize(); + if (proximityTest(x1, x2, x3, x4, normal, mrg, bary)) + return true; + } + return false; } static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) { - if (!bernsteinVFTest(face, node, dt, mrg)) - return false; - if (!continuousCollisionDetection(face, node, dt, 1e-6, bary)) - return false; - return true; + if (!bernsteinVFTest(face, node, dt, mrg)) + return false; + if (!continuousCollisionDetection(face, node, dt, 1e-6, bary)) + return false; + return true; } // @@ -902,62 +906,61 @@ static inline btMatrix3x3 Diagonal(btScalar x) static inline btMatrix3x3 Diagonal(const btVector3& v) { - btMatrix3x3 m; - m[0] = btVector3(v.getX(), 0, 0); - m[1] = btVector3(0, v.getY(), 0); - m[2] = btVector3(0, 0, v.getZ()); - return (m); -} - -static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof) -{ - btScalar result = 0; - for (int i = 0; i < ndof; ++i) - result += a[i] * b[i]; - return result; -} - -static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3, - const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) -{ - btMatrix3x3 m; - btScalar a11 = Dot(v1,u1,ndof); - btScalar a12 = Dot(v1,u2,ndof); - btScalar a13 = Dot(v1,u3,ndof); - - btScalar a21 = Dot(v2,u1,ndof); - btScalar a22 = Dot(v2,u2,ndof); - btScalar a23 = Dot(v2,u3,ndof); - - btScalar a31 = Dot(v3,u1,ndof); - btScalar a32 = Dot(v3,u2,ndof); - btScalar a33 = Dot(v3,u3,ndof); - m[0] = btVector3(a11, a12, a13); - m[1] = btVector3(a21, a22, a23); - m[2] = btVector3(a31, a32, a33); - return (m); -} - -static inline btMatrix3x3 OuterProduct(const btVector3& v1,const btVector3& v2) -{ - btMatrix3x3 m; - btScalar a11 = v1[0] * v2[0]; - btScalar a12 = v1[0] * v2[1]; - btScalar a13 = v1[0] * v2[2]; - - btScalar a21 = v1[1] * v2[0]; - btScalar a22 = v1[1] * v2[1]; - btScalar a23 = v1[1] * v2[2]; - - btScalar a31 = v1[2] * v2[0]; - btScalar a32 = v1[2] * v2[1]; - btScalar a33 = v1[2] * v2[2]; - m[0] = btVector3(a11, a12, a13); - m[1] = btVector3(a21, a22, a23); - m[2] = btVector3(a31, a32, a33); - return (m); + btMatrix3x3 m; + m[0] = btVector3(v.getX(), 0, 0); + m[1] = btVector3(0, v.getY(), 0); + m[2] = btVector3(0, 0, v.getZ()); + return (m); +} + +static inline btScalar Dot(const btScalar* a, const btScalar* b, int ndof) +{ + btScalar result = 0; + for (int i = 0; i < ndof; ++i) + result += a[i] * b[i]; + return result; } +static inline btMatrix3x3 OuterProduct(const btScalar* v1, const btScalar* v2, const btScalar* v3, + const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) +{ + btMatrix3x3 m; + btScalar a11 = Dot(v1, u1, ndof); + btScalar a12 = Dot(v1, u2, ndof); + btScalar a13 = Dot(v1, u3, ndof); + + btScalar a21 = Dot(v2, u1, ndof); + btScalar a22 = Dot(v2, u2, ndof); + btScalar a23 = Dot(v2, u3, ndof); + + btScalar a31 = Dot(v3, u1, ndof); + btScalar a32 = Dot(v3, u2, ndof); + btScalar a33 = Dot(v3, u3, ndof); + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} + +static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2) +{ + btMatrix3x3 m; + btScalar a11 = v1[0] * v2[0]; + btScalar a12 = v1[0] * v2[1]; + btScalar a13 = v1[0] * v2[2]; + + btScalar a21 = v1[1] * v2[0]; + btScalar a22 = v1[1] * v2[1]; + btScalar a23 = v1[1] * v2[2]; + + btScalar a31 = v1[2] * v2[0]; + btScalar a32 = v1[2] * v2[1]; + btScalar a33 = v1[2] * v2[2]; + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} // static inline btMatrix3x3 Add(const btMatrix3x3& a, @@ -1008,6 +1011,20 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt, } // +static inline btMatrix3x3 ImpulseMatrix(btScalar dt, + const btMatrix3x3& effective_mass_inv, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) +{ + return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse()); + // btMatrix3x3 iimb = MassMatrix(imb, iwi, r); + // if (iimb.determinant() == 0) + // return effective_mass_inv.inverse(); + // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse(); +} + +// static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra, btScalar imb, const btMatrix3x3& iib, const btVector3& rb) { @@ -1091,73 +1108,70 @@ static inline void ProjectOrigin(const btVector3& a, // static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t) { - btScalar a, f, u, v; - - btVector3 e1 = v1 - v0; - btVector3 e2 = v2 - v0; - btVector3 h = dir.cross(e2); - a = e1.dot(h); - - if (a > -0.00001 && a < 0.00001) - return (false); - - f = btScalar(1) / a; - btVector3 s = origin - v0; - u = f * s.dot(h); - - if (u < 0.0 || u > 1.0) - return (false); - - btVector3 q = s.cross(e1); - v = f * dir.dot(q); - if (v < 0.0 || u + v > 1.0) - return (false); - // at this stage we can compute t to find out where - // the intersection point is on the line - t = f * e2.dot(q); - if (t > 0) // ray intersection - return (true); - else // this means that there is a line intersection - // but not a ray intersection - return (false); + btScalar a, f, u, v; + + btVector3 e1 = v1 - v0; + btVector3 e2 = v2 - v0; + btVector3 h = dir.cross(e2); + a = e1.dot(h); + + if (a > -0.00001 && a < 0.00001) + return (false); + + f = btScalar(1) / a; + btVector3 s = origin - v0; + u = f * s.dot(h); + + if (u < 0.0 || u > 1.0) + return (false); + + btVector3 q = s.cross(e1); + v = f * dir.dot(q); + if (v < 0.0 || u + v > 1.0) + return (false); + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * e2.dot(q); + if (t > 0) // ray intersection + return (true); + else // this means that there is a line intersection + // but not a ray intersection + return (false); } static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal) { - btVector3 dir = rayEnd - rayStart; - btScalar dir_norm = dir.norm(); - if (dir_norm < SIMD_EPSILON) - return false; - dir.normalize(); - - btScalar t; - - bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); - - if (ret) - { - if (t <= dir_norm) - { - sect = rayStart + dir * t; - } - else - { - ret = false; - } - } - - if (ret) - { - btVector3 n = (p3-p1).cross(p2-p1); - n.safeNormalize(); - if (n.dot(dir) < 0) - normal = n; - else - normal = -n; - } - return ret; -} + btVector3 dir = rayEnd - rayStart; + btScalar dir_norm = dir.norm(); + if (dir_norm < SIMD_EPSILON) + return false; + dir.normalize(); + btScalar t; + bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); + + if (ret) + { + if (t <= dir_norm) + { + sect = rayStart + dir * t; + } + else + { + ret = false; + } + } + if (ret) + { + btVector3 n = (p3 - p1).cross(p2 - p1); + n.safeNormalize(); + if (n.dot(dir) < 0) + normal = n; + else + normal = -n; + } + return ret; +} // template <typename T> @@ -1586,57 +1600,57 @@ struct btSoftColliders psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); } }; - // - // CollideSDF_RS - // - struct CollideSDF_RS : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; - DoNode(*node); - } - void DoNode(btSoftBody::Node& n) const - { - const btScalar m = n.m_im > 0 ? dynmargin : stamargin; - btSoftBody::RContact c; - - if ((!n.m_battach) && - psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) - { - const btScalar ima = n.m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); - const btVector3 vb = n.m_x - n.m_q; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, c.m_cti.m_normal); - const btVector3 fv = vr - c.m_cti.m_normal * dn; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_c2 = ima * psb->m_sst.sdt; - c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } - } - } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; - }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; + + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + { + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; // // CollideSDF_RD @@ -1654,72 +1668,74 @@ struct btSoftColliders btSoftBody::DeformableNodeRigidContact c; if (!n.m_battach) - { + { // check for collision at x_{n+1}^* if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) - { - const btScalar ima = n.m_im; - // todo: collision between multibody and fixed deformable node will be missed. - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - // resolve contact at x_n - psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); - btSoftBody::sCti& cti = c.m_cti; - c.m_node = &n; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 normal = cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - } - } - psb->m_nodeRigidContacts.push_back(c); - } - } + { + const btScalar ima = n.m_im; + // todo: collision between multibody and fixed deformable node will be missed. + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_node = &n; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = n.m_effectiveMass_inv; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); + // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_nodeRigidContacts.push_back(c); + } + } } } btSoftBody* psb; @@ -1728,112 +1744,111 @@ struct btSoftColliders btScalar dynmargin; btScalar stamargin; }; - - // - // CollideSDF_RDF - // - struct CollideSDF_RDF : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; - DoNode(*face); - } - void DoNode(btSoftBody::Face& f) const - { - btSoftBody::Node* n0 = f.m_n[0]; - btSoftBody::Node* n1 = f.m_n[1]; - btSoftBody::Node* n2 = f.m_n[2]; - const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; - btSoftBody::DeformableFaceRigidContact c; - btVector3 contact_point; - btVector3 bary; - if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) - { - f.m_pcontact[3] = 1; - btScalar ima = n0->m_im + n1->m_im + n2->m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - // todo: collision between multibody and fixed deformable face will be missed. - const btScalar ms = ima + imb; - if (ms > 0) - { - // resolve contact at x_n -// psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); - btSoftBody::sCti& cti = c.m_cti; - c.m_contactPoint = contact_point; - c.m_bary = bary; - // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices - c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary; - c.m_face = &f; + + // + // CollideSDF_RDF + // + struct CollideSDF_RDF : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; + DoNode(*face); + } + void DoNode(btSoftBody::Face& f) const + { + btSoftBody::Node* n0 = f.m_n[0]; + btSoftBody::Node* n1 = f.m_n[1]; + btSoftBody::Node* n2 = f.m_n[2]; + const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin; + btSoftBody::DeformableFaceRigidContact c; + btVector3 contact_point; + btVector3 bary; + if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) + { + btScalar ima = n0->m_im + n1->m_im + n2->m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + // todo: collision between multibody and fixed deformable face will be missed. + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + // psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_contactPoint = contact_point; + c.m_bary = bary; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices + c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary; + c.m_face = &f; // friction is handled by the nodes to prevent sticking -// const btScalar fc = 0; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - - // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf - ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im; - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = contact_point - wtr.getOrigin(); - - // we do not scale the impulse matrix by dt - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 normal = cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - } - } - psb->m_faceRigidContacts.push_back(c); - } - } - else - { - f.m_pcontact[3] = 0; - } - } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; - }; - + // const btScalar fc = 0; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf + ima = bary.getX() * c.m_weights.getX() * n0->m_im + bary.getY() * c.m_weights.getY() * n1->m_im + bary.getZ() * c.m_weights.getZ() * n2->m_im; + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = Diagonal(ima); + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = contact_point - wtr.getOrigin(); + + // we do not scale the impulse matrix by dt + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_faceRigidContacts.push_back(c); + } + } + // Set caching barycenters to be false after collision detection. + // Only turn on when contact is static. + f.m_pcontact[3] = 0; + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + // // CollideVF_SS // @@ -1844,12 +1859,12 @@ struct btSoftColliders { btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - for (int i = 0; i < 3; ++i) - { - if (face->m_n[i] == node) - continue; - } - + for (int i = 0; i < 3; ++i) + { + if (face->m_n[i] == node) + continue; + } + btVector3 o = node->m_x; btVector3 p; btScalar d = SIMD_INFINITY; @@ -1879,7 +1894,7 @@ struct btSoftColliders c.m_node = node; c.m_face = face; c.m_weights = w; - c.m_friction = btMax (psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR; c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR; psb[0]->m_scontacts.push_back(c); @@ -1889,206 +1904,205 @@ struct btSoftColliders btSoftBody* psb[2]; btScalar mrg; }; - - - // - // CollideVF_DD - // - struct CollideVF_DD : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; - btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - btVector3 bary; - if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) - { - const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; - const btVector3 w = bary; - const btScalar ma = node->m_im; - btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); - if ((n[0]->m_im <= 0) || - (n[1]->m_im <= 0) || - (n[2]->m_im <= 0)) - { - mb = 0; - } - const btScalar ms = ma + mb; - if (ms > 0) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = w; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar mrg; - bool useFaceNormal; - }; - - // - // CollideFF_DD - // - struct CollideFF_DD : btDbvt::ICollide - { - void Process(const btDbvntNode* lface1, - const btDbvntNode* lface2) - { - btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; - if (f1 != f2) - { - Repel(f1, f2); - Repel(f2, f1); - } - } - void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) - { - //#define REPEL_NEIGHBOR 1 + + // + // CollideVF_DD + // + struct CollideVF_DD : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 bary; + if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) + { + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + const btVector3 w = bary; + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + if ((n[0]->m_im <= 0) || + (n[1]->m_im <= 0) || + (n[2]->m_im <= 0)) + { + mb = 0; + } + const btScalar ms = ma + mb; + if (ms > 0) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = w; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + bool useFaceNormal; + }; + + // + // CollideFF_DD + // + struct CollideFF_DD : btDbvt::ICollide + { + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; + if (f1 != f2) + { + Repel(f1, f2); + Repel(f2, f1); + } + } + void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) + { + //#define REPEL_NEIGHBOR 1 #ifndef REPEL_NEIGHBOR - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - return; - } - } + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + return; + } + } #endif - bool skip = false; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; + bool skip = false; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; #ifdef REPEL_NEIGHBOR - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - { - skip = true; - break; - } - } - if (skip) - { - skip = false; - continue; - } + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + { + skip = false; + continue; + } #endif - btSoftBody::Face* face = f2; - btVector3 bary; - if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) - continue; - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - btSoftBody* psb[2]; - btScalar mrg; - bool useFaceNormal; - }; - - struct CollideCCD : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; - btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - btVector3 bary; - if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - void Process(const btDbvntNode* lface1, - const btDbvntNode* lface2) - { - btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; - if (f1 != f2) - { - Repel(f1, f2); - Repel(f2, f1); - } - } - void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) - { - //#define REPEL_NEIGHBOR 1 + btSoftBody::Face* face = f2; + btVector3 bary; + if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) + continue; + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + btSoftBody* psb[2]; + btScalar mrg; + bool useFaceNormal; + }; + + struct CollideCCD : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 bary; + if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; + if (f1 != f2) + { + Repel(f1, f2); + Repel(f2, f1); + } + } + void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) + { + //#define REPEL_NEIGHBOR 1 #ifndef REPEL_NEIGHBOR - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - return; - } - } + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + return; + } + } #endif - bool skip = false; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; + bool skip = false; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; #ifdef REPEL_NEIGHBOR - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - { - skip = true; - break; - } - } - if (skip) - { - skip = false; - continue; - } + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + { + skip = false; + continue; + } #endif - btSoftBody::Face* face = f2; - btVector3 bary; + btSoftBody::Face* face = f2; + btVector3 bary; if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar dt, mrg; - bool useFaceNormal; - }; + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar dt, mrg; + bool useFaceNormal; + }; }; #endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h b/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h index c4ac4141aa..dbb2624eee 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h @@ -36,7 +36,7 @@ public: CL_SIMD_SOLVER, DX_SOLVER, DX_SIMD_SOLVER, - DEFORMABLE_SOLVER + DEFORMABLE_SOLVER }; protected: diff --git a/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp index 282dbf75f0..329bd19d71 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp @@ -100,6 +100,11 @@ void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeSte ///update soft bodies m_softBodySolver->updateSoftBodies(); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; + psb->interpolateRenderMesh(); + } // End solver-wise simulation step // /////////////////////////////// } diff --git a/thirdparty/bullet/BulletSoftBody/btSparseSDF.h b/thirdparty/bullet/BulletSoftBody/btSparseSDF.h index eb290a1dbd..d611726bcd 100644 --- a/thirdparty/bullet/BulletSoftBody/btSparseSDF.h +++ b/thirdparty/bullet/BulletSoftBody/btSparseSDF.h @@ -22,36 +22,36 @@ subject to the following restrictions: // Fast Hash -#if !defined (get16bits) -#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\ -+(unsigned int)(((const unsigned char *)(d))[0]) ) +#if !defined(get16bits) +#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0])) #endif // // super hash function by Paul Hsieh // -inline unsigned int HsiehHash (const char * data, int len) { - unsigned int hash = len, tmp; - len>>=2; - - /* Main loop */ - for (;len > 0; len--) { - hash += get16bits (data); - tmp = (get16bits (data+2) << 11) ^ hash; - hash = (hash << 16) ^ tmp; - data += 2*sizeof (unsigned short); - hash += hash >> 11; - } +inline unsigned int HsiehHash(const char* data, int len) +{ + unsigned int hash = len, tmp; + len >>= 2; + /* Main loop */ + for (; len > 0; len--) + { + hash += get16bits(data); + tmp = (get16bits(data + 2) << 11) ^ hash; + hash = (hash << 16) ^ tmp; + data += 2 * sizeof(unsigned short); + hash += hash >> 11; + } - /* Force "avalanching" of final 127 bits */ - hash ^= hash << 3; - hash += hash >> 5; - hash ^= hash << 4; - hash += hash >> 17; - hash ^= hash << 25; - hash += hash >> 6; + /* Force "avalanching" of final 127 bits */ + hash ^= hash << 3; + hash += hash >> 5; + hash ^= hash << 4; + hash += hash >> 17; + hash ^= hash << 25; + hash += hash >> 6; - return hash; + return hash; } template <const int CELLSIZE> @@ -81,7 +81,7 @@ struct btSparseSdf btAlignedObjectArray<Cell*> cells; btScalar voxelsz; - btScalar m_defaultVoxelsz; + btScalar m_defaultVoxelsz; int puid; int ncells; int m_clampCells; @@ -103,16 +103,16 @@ struct btSparseSdf //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) m_clampCells = clampCells; cells.resize(hashsize, 0); - m_defaultVoxelsz = 0.25; + m_defaultVoxelsz = 0.25; Reset(); } // - - void setDefaultVoxelsz(btScalar sz) - { - m_defaultVoxelsz = sz; - } - + + void setDefaultVoxelsz(btScalar sz) + { + m_defaultVoxelsz = sz; + } + void Reset() { for (int i = 0, ni = cells.size(); i < ni; ++i) @@ -162,7 +162,7 @@ struct btSparseSdf nqueries = 1; nprobes = 1; ++puid; ///@todo: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ + /* else setup a priority list... */ } // int RemoveReferences(btCollisionShape* pcs) @@ -221,7 +221,7 @@ struct btSparseSdf else { // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]); - //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); + //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); c = c->next; } @@ -363,7 +363,7 @@ struct btSparseSdf myset.p = (void*)shape; const char* ptr = (const char*)&myset; - unsigned int result = HsiehHash(ptr, sizeof(btS) ); + unsigned int result = HsiehHash(ptr, sizeof(btS)); return result; } diff --git a/thirdparty/bullet/BulletSoftBody/poly34.cpp b/thirdparty/bullet/BulletSoftBody/poly34.cpp index 819d0c79f7..ec7549c8e8 100644 --- a/thirdparty/bullet/BulletSoftBody/poly34.cpp +++ b/thirdparty/bullet/BulletSoftBody/poly34.cpp @@ -6,7 +6,7 @@ // #include <math.h> -#include "poly34.h" // solution of cubic and quartic equation +#include "poly34.h" // solution of cubic and quartic equation #define TwoPi 6.28318530717958648 const btScalar eps = SIMD_EPSILON; @@ -15,50 +15,53 @@ const btScalar eps = SIMD_EPSILON; //============================================================================= static SIMD_FORCE_INLINE btScalar _root3(btScalar x) { - btScalar s = 1.; - while (x < 1.) { - x *= 8.; - s *= 0.5; - } - while (x > 8.) { - x *= 0.125; - s *= 2.; - } - btScalar r = 1.5; - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - return r * s; + btScalar s = 1.; + while (x < 1.) + { + x *= 8.; + s *= 0.5; + } + while (x > 8.) + { + x *= 0.125; + s *= 2.; + } + btScalar r = 1.5; + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + return r * s; } btScalar SIMD_FORCE_INLINE root3(btScalar x) { - if (x > 0) - return _root3(x); - else if (x < 0) - return -_root3(-x); - else - return 0.; + if (x > 0) + return _root3(x); + else if (x < 0) + return -_root3(-x); + else + return 0.; } // x - array of size 2 // return 2: 2 real roots x[0], x[1] // return 0: pair of complex roots: x[0]i*x[1] int SolveP2(btScalar* x, btScalar a, btScalar b) -{ // solve equation x^2 + a*x + b = 0 - btScalar D = 0.25 * a * a - b; - if (D >= 0) { - D = sqrt(D); - x[0] = -0.5 * a + D; - x[1] = -0.5 * a - D; - return 2; - } - x[0] = -0.5 * a; - x[1] = sqrt(-D); - return 0; +{ // solve equation x^2 + a*x + b = 0 + btScalar D = 0.25 * a * a - b; + if (D >= 0) + { + D = sqrt(D); + x[0] = -0.5 * a + D; + x[1] = -0.5 * a - D; + return 2; + } + x[0] = -0.5 * a; + x[1] = sqrt(-D); + return 0; } //--------------------------------------------------------------------------- // x - array of size 3 @@ -66,217 +69,228 @@ int SolveP2(btScalar* x, btScalar a, btScalar b) // 2 real roots: x[0], x[1], return 2 // 1 real root : x[0], x[1] i*x[2], return 1 int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c) -{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 - btScalar a2 = a * a; - btScalar q = (a2 - 3 * b) / 9; - if (q < 0) - q = eps; - btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; - // equation x^3 + q*x + r = 0 - btScalar r2 = r * r; - btScalar q3 = q * q * q; - btScalar A, B; - if (r2 <= (q3 + eps)) { //<<-- FIXED! - btScalar t = r / sqrt(q3); - if (t < -1) - t = -1; - if (t > 1) - t = 1; - t = acos(t); - a /= 3; - q = -2 * sqrt(q); - x[0] = q * cos(t / 3) - a; - x[1] = q * cos((t + TwoPi) / 3) - a; - x[2] = q * cos((t - TwoPi) / 3) - a; - return (3); - } - else { - //A =-pow(fabs(r)+sqrt(r2-q3),1./3); - A = -root3(fabs(r) + sqrt(r2 - q3)); - if (r < 0) - A = -A; - B = (A == 0 ? 0 : q / A); - - a /= 3; - x[0] = (A + B) - a; - x[1] = -0.5 * (A + B) - a; - x[2] = 0.5 * sqrt(3.) * (A - B); - if (fabs(x[2]) < eps) { - x[2] = x[1]; - return (2); - } - return (1); - } -} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { +{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 + btScalar a2 = a * a; + btScalar q = (a2 - 3 * b) / 9; + if (q < 0) + q = eps; + btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; + // equation x^3 + q*x + r = 0 + btScalar r2 = r * r; + btScalar q3 = q * q * q; + btScalar A, B; + if (r2 <= (q3 + eps)) + { //<<-- FIXED! + btScalar t = r / sqrt(q3); + if (t < -1) + t = -1; + if (t > 1) + t = 1; + t = acos(t); + a /= 3; + q = -2 * sqrt(q); + x[0] = q * cos(t / 3) - a; + x[1] = q * cos((t + TwoPi) / 3) - a; + x[2] = q * cos((t - TwoPi) / 3) - a; + return (3); + } + else + { + //A =-pow(fabs(r)+sqrt(r2-q3),1./3); + A = -root3(fabs(r) + sqrt(r2 - q3)); + if (r < 0) + A = -A; + B = (A == 0 ? 0 : q / A); + + a /= 3; + x[0] = (A + B) - a; + x[1] = -0.5 * (A + B) - a; + x[2] = 0.5 * sqrt(3.) * (A - B); + if (fabs(x[2]) < eps) + { + x[2] = x[1]; + return (2); + } + return (1); + } +} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { //--------------------------------------------------------------------------- // a>=0! -void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) { - btScalar r = sqrt(x * x + y * y); - if (y == 0) { - r = sqrt(r); - if (x >= 0) { - a = r; - b = 0; - } - else { - a = 0; - b = r; - } - } - else { // y != 0 - a = sqrt(0.5 * (x + r)); - b = 0.5 * y / a; - } + btScalar r = sqrt(x * x + y * y); + if (y == 0) + { + r = sqrt(r); + if (x >= 0) + { + a = r; + b = 0; + } + else + { + a = 0; + b = r; + } + } + else + { // y != 0 + a = sqrt(0.5 * (x + r)); + b = 0.5 * y / a; + } } //--------------------------------------------------------------------------- -int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 +int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 { - btScalar D = b * b - 4 * d; - if (D >= 0) { - btScalar sD = sqrt(D); - btScalar x1 = (-b + sD) / 2; - btScalar x2 = (-b - sD) / 2; // x2 <= x1 - if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots - { - btScalar sx1 = sqrt(x1); - btScalar sx2 = sqrt(x2); - x[0] = -sx1; - x[1] = sx1; - x[2] = -sx2; - x[3] = sx2; - return 4; - } - if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots - { - btScalar sx1 = sqrt(-x1); - btScalar sx2 = sqrt(-x2); - x[0] = 0; - x[1] = sx1; - x[2] = 0; - x[3] = sx2; - return 0; - } - // now x2 < 0 <= x1 , two real roots and one pair of imginary root - btScalar sx1 = sqrt(x1); - btScalar sx2 = sqrt(-x2); - x[0] = -sx1; - x[1] = sx1; - x[2] = 0; - x[3] = sx2; - return 2; - } - else { // if( D < 0 ), two pair of compex roots - btScalar sD2 = 0.5 * sqrt(-D); - CSqrt(-0.5 * b, sD2, x[0], x[1]); - CSqrt(-0.5 * b, -sD2, x[2], x[3]); - return 0; - } // if( D>=0 ) -} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d + btScalar D = b * b - 4 * d; + if (D >= 0) + { + btScalar sD = sqrt(D); + btScalar x1 = (-b + sD) / 2; + btScalar x2 = (-b - sD) / 2; // x2 <= x1 + if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots + { + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(x2); + x[0] = -sx1; + x[1] = sx1; + x[2] = -sx2; + x[3] = sx2; + return 4; + } + if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots + { + btScalar sx1 = sqrt(-x1); + btScalar sx2 = sqrt(-x2); + x[0] = 0; + x[1] = sx1; + x[2] = 0; + x[3] = sx2; + return 0; + } + // now x2 < 0 <= x1 , two real roots and one pair of imginary root + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(-x2); + x[0] = -sx1; + x[1] = sx1; + x[2] = 0; + x[3] = sx2; + return 2; + } + else + { // if( D < 0 ), two pair of compex roots + btScalar sD2 = 0.5 * sqrt(-D); + CSqrt(-0.5 * b, sD2, x[0], x[1]); + CSqrt(-0.5 * b, -sD2, x[2], x[3]); + return 0; + } // if( D>=0 ) +} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d //--------------------------------------------------------------------------- #define SWAP(a, b) \ -{ \ -t = b; \ -b = a; \ -a = t; \ -} -static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c + { \ + t = b; \ + b = a; \ + a = t; \ + } +static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c { - btScalar t; - if (a > b) - SWAP(a, b); // now a<=b - if (c < b) { - SWAP(b, c); // now a<=b, b<=c - if (a > b) - SWAP(a, b); // now a<=b - } + btScalar t; + if (a > b) + SWAP(a, b); // now a<=b + if (c < b) + { + SWAP(b, c); // now a<=b, b<=c + if (a > b) + SWAP(a, b); // now a<=b + } } //--------------------------------------------------------------------------- -int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d +int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d { - //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0 - if (fabs(c) < 1e-14 * (fabs(b) + fabs(d))) - return SolveP4Bi(x, b, d); // After that, c!=0 - - int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent - // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0 - if (res3 > 1) // 3 real roots, - { - dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2] - // Note: x[0]*x[1]*x[2]= c*c > 0 - if (x[0] > 0) // all roots are positive - { - btScalar sz1 = sqrt(x[0]); - btScalar sz2 = sqrt(x[1]); - btScalar sz3 = sqrt(x[2]); - // Note: sz1*sz2*sz3= -c (and not equal to 0) - if (c > 0) { - x[0] = (-sz1 - sz2 - sz3) / 2; - x[1] = (-sz1 + sz2 + sz3) / 2; - x[2] = (+sz1 - sz2 + sz3) / 2; - x[3] = (+sz1 + sz2 - sz3) / 2; - return 4; - } - // now: c<0 - x[0] = (-sz1 - sz2 + sz3) / 2; - x[1] = (-sz1 + sz2 - sz3) / 2; - x[2] = (+sz1 - sz2 - sz3) / 2; - x[3] = (+sz1 + sz2 + sz3) / 2; - return 4; - } // if( x[0] > 0) // all roots are positive - // now x[0] <= x[1] < 0, x[2] > 0 - // two pair of comlex roots - btScalar sz1 = sqrt(-x[0]); - btScalar sz2 = sqrt(-x[1]); - btScalar sz3 = sqrt(x[2]); - - if (c > 0) // sign = -1 - { - x[0] = -sz3 / 2; - x[1] = (sz1 - sz2) / 2; // x[0]i*x[1] - x[2] = sz3 / 2; - x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3] - return 0; - } - // now: c<0 , sign = +1 - x[0] = sz3 / 2; - x[1] = (-sz1 + sz2) / 2; - x[2] = -sz3 / 2; - x[3] = (sz1 + sz2) / 2; - return 0; - } // if( res3>1 ) // 3 real roots, - // now resoventa have 1 real and pair of compex roots - // x[0] - real root, and x[0]>0, - // x[1]i*x[2] - complex roots, - // x[0] must be >=0. But one times x[0]=~ 1e-17, so: - if (x[0] < 0) - x[0] = 0; - btScalar sz1 = sqrt(x[0]); - btScalar szr, szi; - CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2] - if (c > 0) // sign = -1 - { - x[0] = -sz1 / 2 - szr; // 1st real root - x[1] = -sz1 / 2 + szr; // 2nd real root - x[2] = sz1 / 2; - x[3] = szi; - return 2; - } - // now: c<0 , sign = +1 - x[0] = sz1 / 2 - szr; // 1st real root - x[1] = sz1 / 2 + szr; // 2nd real root - x[2] = -sz1 / 2; - x[3] = szi; - return 2; -} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d + //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0 + if (fabs(c) < 1e-14 * (fabs(b) + fabs(d))) + return SolveP4Bi(x, b, d); // After that, c!=0 + + int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent + // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0 + if (res3 > 1) // 3 real roots, + { + dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2] + // Note: x[0]*x[1]*x[2]= c*c > 0 + if (x[0] > 0) // all roots are positive + { + btScalar sz1 = sqrt(x[0]); + btScalar sz2 = sqrt(x[1]); + btScalar sz3 = sqrt(x[2]); + // Note: sz1*sz2*sz3= -c (and not equal to 0) + if (c > 0) + { + x[0] = (-sz1 - sz2 - sz3) / 2; + x[1] = (-sz1 + sz2 + sz3) / 2; + x[2] = (+sz1 - sz2 + sz3) / 2; + x[3] = (+sz1 + sz2 - sz3) / 2; + return 4; + } + // now: c<0 + x[0] = (-sz1 - sz2 + sz3) / 2; + x[1] = (-sz1 + sz2 - sz3) / 2; + x[2] = (+sz1 - sz2 - sz3) / 2; + x[3] = (+sz1 + sz2 + sz3) / 2; + return 4; + } // if( x[0] > 0) // all roots are positive + // now x[0] <= x[1] < 0, x[2] > 0 + // two pair of comlex roots + btScalar sz1 = sqrt(-x[0]); + btScalar sz2 = sqrt(-x[1]); + btScalar sz3 = sqrt(x[2]); + + if (c > 0) // sign = -1 + { + x[0] = -sz3 / 2; + x[1] = (sz1 - sz2) / 2; // x[0]i*x[1] + x[2] = sz3 / 2; + x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3] + return 0; + } + // now: c<0 , sign = +1 + x[0] = sz3 / 2; + x[1] = (-sz1 + sz2) / 2; + x[2] = -sz3 / 2; + x[3] = (sz1 + sz2) / 2; + return 0; + } // if( res3>1 ) // 3 real roots, + // now resoventa have 1 real and pair of compex roots + // x[0] - real root, and x[0]>0, + // x[1]i*x[2] - complex roots, + // x[0] must be >=0. But one times x[0]=~ 1e-17, so: + if (x[0] < 0) + x[0] = 0; + btScalar sz1 = sqrt(x[0]); + btScalar szr, szi; + CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2] + if (c > 0) // sign = -1 + { + x[0] = -sz1 / 2 - szr; // 1st real root + x[1] = -sz1 / 2 + szr; // 2nd real root + x[2] = sz1 / 2; + x[3] = szi; + return 2; + } + // now: c<0 , sign = +1 + x[0] = sz1 / 2 - szr; // 1st real root + x[1] = sz1 / 2 + szr; // 2nd real root + x[2] = -sz1 / 2; + x[3] = szi; + return 2; +} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d //----------------------------------------------------------------------------- -btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d { - btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) - if (fxs == 0) - return x; //return 1e99; <<-- FIXED! - btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) - return x - fx / fxs; + btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) + if (fxs == 0) + return x; //return 1e99; <<-- FIXED! + btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) + return x - fx / fxs; } //----------------------------------------------------------------------------- // x - array of size 4 @@ -284,136 +298,150 @@ btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // o // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], // return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d) -{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method - // move to a=0: - btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); - btScalar c1 = c + 0.5 * a * (0.25 * a * a - b); - btScalar b1 = b - 0.375 * a * a; - int res = SolveP4De(x, b1, c1, d1); - if (res == 4) { - x[0] -= a / 4; - x[1] -= a / 4; - x[2] -= a / 4; - x[3] -= a / 4; - } - else if (res == 2) { - x[0] -= a / 4; - x[1] -= a / 4; - x[2] -= a / 4; - } - else { - x[0] -= a / 4; - x[2] -= a / 4; - } - // one Newton step for each real root: - if (res > 0) { - x[0] = N4Step(x[0], a, b, c, d); - x[1] = N4Step(x[1], a, b, c, d); - } - if (res > 2) { - x[2] = N4Step(x[2], a, b, c, d); - x[3] = N4Step(x[3], a, b, c, d); - } - return res; +{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method + // move to a=0: + btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); + btScalar c1 = c + 0.5 * a * (0.25 * a * a - b); + btScalar b1 = b - 0.375 * a * a; + int res = SolveP4De(x, b1, c1, d1); + if (res == 4) + { + x[0] -= a / 4; + x[1] -= a / 4; + x[2] -= a / 4; + x[3] -= a / 4; + } + else if (res == 2) + { + x[0] -= a / 4; + x[1] -= a / 4; + x[2] -= a / 4; + } + else + { + x[0] -= a / 4; + x[2] -= a / 4; + } + // one Newton step for each real root: + if (res > 0) + { + x[0] = N4Step(x[0], a, b, c, d); + x[1] = N4Step(x[1], a, b, c, d); + } + if (res > 2) + { + x[2] = N4Step(x[2], a, b, c, d); + x[3] = N4Step(x[3], a, b, c, d); + } + return res; } //----------------------------------------------------------------------------- #define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e) //----------------------------------------------------------------------------- -btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 { - int cnt; - if (fabs(e) < eps) - return 0; - - btScalar brd = fabs(a); // brd - border of real roots - if (fabs(b) > brd) - brd = fabs(b); - if (fabs(c) > brd) - brd = fabs(c); - if (fabs(d) > brd) - brd = fabs(d); - if (fabs(e) > brd) - brd = fabs(e); - brd++; // brd - border of real roots - - btScalar x0, f0; // less than root - btScalar x1, f1; // greater than root - btScalar x2, f2, f2s; // next values, f(x2), f'(x2) - btScalar dx = 0; - - if (e < 0) { - x0 = 0; - x1 = brd; - f0 = e; - f1 = F5(x1); - x2 = 0.01 * brd; - } // positive root - else { - x0 = -brd; - x1 = 0; - f0 = F5(x0); - f1 = e; - x2 = -0.01 * brd; - } // negative root - - if (fabs(f0) < eps) - return x0; - if (fabs(f1) < eps) - return x1; - - // now x0<x1, f(x0)<0, f(x1)>0 - // Firstly 10 bisections - for (cnt = 0; cnt < 10; cnt++) { - x2 = (x0 + x1) / 2; // next point - //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point - f2 = F5(x2); // f(x2) - if (fabs(f2) < eps) - return x2; - if (f2 > 0) { - x1 = x2; - f1 = f2; - } - else { - x0 = x2; - f0 = f2; - } - } - - // At each step: - // x0<x1, f(x0)<0, f(x1)>0. - // x2 - next value - // we hope that x0 < x2 < x1, but not necessarily - do { - if (cnt++ > 50) - break; - if (x2 <= x0 || x2 >= x1) - x2 = (x0 + x1) / 2; // now x0 < x2 < x1 - f2 = F5(x2); // f(x2) - if (fabs(f2) < eps) - return x2; - if (f2 > 0) { - x1 = x2; - f1 = f2; - } - else { - x0 = x2; - f0 = f2; - } - f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2) - if (fabs(f2s) < eps) { - x2 = 1e99; - continue; - } - dx = f2 / f2s; - x2 -= dx; - } while (fabs(dx) > eps); - return x2; -} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 + int cnt; + if (fabs(e) < eps) + return 0; + + btScalar brd = fabs(a); // brd - border of real roots + if (fabs(b) > brd) + brd = fabs(b); + if (fabs(c) > brd) + brd = fabs(c); + if (fabs(d) > brd) + brd = fabs(d); + if (fabs(e) > brd) + brd = fabs(e); + brd++; // brd - border of real roots + + btScalar x0, f0; // less than root + btScalar x1, f1; // greater than root + btScalar x2, f2, f2s; // next values, f(x2), f'(x2) + btScalar dx = 0; + + if (e < 0) + { + x0 = 0; + x1 = brd; + f0 = e; + f1 = F5(x1); + x2 = 0.01 * brd; + } // positive root + else + { + x0 = -brd; + x1 = 0; + f0 = F5(x0); + f1 = e; + x2 = -0.01 * brd; + } // negative root + + if (fabs(f0) < eps) + return x0; + if (fabs(f1) < eps) + return x1; + + // now x0<x1, f(x0)<0, f(x1)>0 + // Firstly 10 bisections + for (cnt = 0; cnt < 10; cnt++) + { + x2 = (x0 + x1) / 2; // next point + //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point + f2 = F5(x2); // f(x2) + if (fabs(f2) < eps) + return x2; + if (f2 > 0) + { + x1 = x2; + f1 = f2; + } + else + { + x0 = x2; + f0 = f2; + } + } + + // At each step: + // x0<x1, f(x0)<0, f(x1)>0. + // x2 - next value + // we hope that x0 < x2 < x1, but not necessarily + do + { + if (cnt++ > 50) + break; + if (x2 <= x0 || x2 >= x1) + x2 = (x0 + x1) / 2; // now x0 < x2 < x1 + f2 = F5(x2); // f(x2) + if (fabs(f2) < eps) + return x2; + if (f2 > 0) + { + x1 = x2; + f1 = f2; + } + else + { + x0 = x2; + f0 = f2; + } + f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2) + if (fabs(f2s) < eps) + { + x2 = 1e99; + continue; + } + dx = f2 / f2s; + x2 -= dx; + } while (fabs(dx) > eps); + return x2; +} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- -int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 { - btScalar r = x[0] = SolveP5_1(a, b, c, d, e); - btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; - return 1 + SolveP4(x + 1, a1, b1, c1, d1); -} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 + btScalar r = x[0] = SolveP5_1(a, b, c, d, e); + btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; + return 1 + SolveP4(x + 1, a1, b1, c1, d1); +} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- diff --git a/thirdparty/bullet/BulletSoftBody/poly34.h b/thirdparty/bullet/BulletSoftBody/poly34.h index 32ad5d7da5..35a52c5fec 100644 --- a/thirdparty/bullet/BulletSoftBody/poly34.h +++ b/thirdparty/bullet/BulletSoftBody/poly34.h @@ -8,31 +8,31 @@ // x - array of size 2 // return 2: 2 real roots x[0], x[1] // return 0: pair of complex roots: x[0]i*x[1] -int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0 +int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0 // x - array of size 3 // return 3: 3 real roots x[0], x[1], x[2] // return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2] -int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 +int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 // x - array of size 4 // return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], // return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], -int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method +int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method // x - array of size 5 // return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots // return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4], // return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2], x[3]i*x[4], -int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- // And some additional functions for internal use. // Your may remove this definitions from here -int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0 -int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0 -void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y) -btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d -btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0 +int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0 +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y) +btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 #endif diff --git a/thirdparty/bullet/LinearMath/btAlignedAllocator.cpp b/thirdparty/bullet/LinearMath/btAlignedAllocator.cpp index 39b302b600..be8f8aa6d0 100644 --- a/thirdparty/bullet/LinearMath/btAlignedAllocator.cpp +++ b/thirdparty/bullet/LinearMath/btAlignedAllocator.cpp @@ -138,7 +138,7 @@ struct btDebugPtrMagic }; }; -void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filename) +void *btAlignedAllocInternal(size_t size, int alignment, int line, const char *filename) { if (size == 0) { @@ -195,7 +195,7 @@ void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filenam return (ret); } -void btAlignedFreeInternal(void *ptr, int line, char *filename) +void btAlignedFreeInternal(void *ptr, int line, const char *filename) { void *real; diff --git a/thirdparty/bullet/LinearMath/btAlignedAllocator.h b/thirdparty/bullet/LinearMath/btAlignedAllocator.h index ce4d3585f1..971f62bfb0 100644 --- a/thirdparty/bullet/LinearMath/btAlignedAllocator.h +++ b/thirdparty/bullet/LinearMath/btAlignedAllocator.h @@ -35,9 +35,9 @@ int btDumpMemoryLeaks(); #define btAlignedFree(ptr) \ btAlignedFreeInternal(ptr, __LINE__, __FILE__) -void* btAlignedAllocInternal(size_t size, int alignment, int line, char* filename); +void* btAlignedAllocInternal(size_t size, int alignment, int line, const char* filename); -void btAlignedFreeInternal(void* ptr, int line, char* filename); +void btAlignedFreeInternal(void* ptr, int line, const char* filename); #else void* btAlignedAllocInternal(size_t size, int alignment); diff --git a/thirdparty/bullet/LinearMath/btConvexHullComputer.cpp b/thirdparty/bullet/LinearMath/btConvexHullComputer.cpp index 8bbfdc5f25..12125fd2de 100644 --- a/thirdparty/bullet/LinearMath/btConvexHullComputer.cpp +++ b/thirdparty/bullet/LinearMath/btConvexHullComputer.cpp @@ -105,7 +105,7 @@ public: Point64 cross(const Point32& b) const { - return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + return Point64(((int64_t)y) * b.z - ((int64_t)z) * b.y, ((int64_t)z) * b.x - ((int64_t)x) * b.z, ((int64_t)x) * b.y - ((int64_t)y) * b.x); } Point64 cross(const Point64& b) const @@ -115,7 +115,7 @@ public: int64_t dot(const Point32& b) const { - return x * b.x + y * b.y + z * b.z; + return ((int64_t)x) * b.x + ((int64_t)y) * b.y + ((int64_t)z) * b.z; } int64_t dot(const Point64& b) const @@ -2673,6 +2673,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in } vertices.resize(0); + original_vertex_index.resize(0); edges.resize(0); faces.resize(0); @@ -2683,6 +2684,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in { btConvexHullInternal::Vertex* v = oldVertices[copied]; vertices.push_back(hull.getCoordinates(v)); + original_vertex_index.push_back(v->point.index); btConvexHullInternal::Edge* firstEdge = v->edges; if (firstEdge) { diff --git a/thirdparty/bullet/LinearMath/btConvexHullComputer.h b/thirdparty/bullet/LinearMath/btConvexHullComputer.h index cba684f2dc..18b26eea9a 100644 --- a/thirdparty/bullet/LinearMath/btConvexHullComputer.h +++ b/thirdparty/bullet/LinearMath/btConvexHullComputer.h @@ -66,6 +66,9 @@ public: // Vertices of the output hull btAlignedObjectArray<btVector3> vertices; + // The original vertex index in the input coords array + btAlignedObjectArray<int> original_vertex_index; + // Edges of the output hull btAlignedObjectArray<Edge> edges; diff --git a/thirdparty/bullet/LinearMath/btReducedVector.h b/thirdparty/bullet/LinearMath/btReducedVector.h index 83b5e581e5..313a4271f0 100644 --- a/thirdparty/bullet/LinearMath/btReducedVector.h +++ b/thirdparty/bullet/LinearMath/btReducedVector.h @@ -267,7 +267,7 @@ public: std::sort(tuples.begin(), tuples.end()); btAlignedObjectArray<int> new_indices; btAlignedObjectArray<btVector3> new_vecs; - for (int i = 0; i < tuples.size(); ++i) + for (size_t i = 0; i < tuples.size(); ++i) { new_indices.push_back(tuples[i].b); new_vecs.push_back(m_vecs[tuples[i].a]); diff --git a/thirdparty/bullet/LinearMath/btScalar.h b/thirdparty/bullet/LinearMath/btScalar.h index 86d94e8974..36b90cc944 100644 --- a/thirdparty/bullet/LinearMath/btScalar.h +++ b/thirdparty/bullet/LinearMath/btScalar.h @@ -25,7 +25,7 @@ subject to the following restrictions: #include <float.h> /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 289 +#define BT_BULLET_VERSION 307 inline int btGetVersion() { diff --git a/thirdparty/bullet/LinearMath/btSerializer.h b/thirdparty/bullet/LinearMath/btSerializer.h index 2ee712047f..9abcf031d0 100644 --- a/thirdparty/bullet/LinearMath/btSerializer.h +++ b/thirdparty/bullet/LinearMath/btSerializer.h @@ -479,9 +479,9 @@ public: buffer[8] = 'V'; } - buffer[9] = '2'; - buffer[10] = '8'; - buffer[11] = '9'; + buffer[9] = '3'; + buffer[10] = '0'; + buffer[11] = '7'; } virtual void startSerialization() diff --git a/thirdparty/icu4c/APIChangeReport.md b/thirdparty/icu4c/APIChangeReport.md index 0cf9ed5bfc..5385904fd1 100644 --- a/thirdparty/icu4c/APIChangeReport.md +++ b/thirdparty/icu4c/APIChangeReport.md @@ -23,8 +23,10 @@ Removed from ICU 67 | File | API | ICU 67 | ICU 68 | |---|---|---|---| +| fmtable.h | const UFormattable* icu::Formattable::toUFormattable() | StableICU 52 | (missing) | measunit.h | LocalArray<MeasureUnit> icu::MeasureUnit::splitToSingleUnits(int32_t&, UErrorCode&) const | InternalICU 67 | (missing) | measunit.h | int32_t icu::MeasureUnit::getIndex() const | Internal | (missing) +| measunit.h | <tt>static</tt> MeasureUnit icu::MeasureUnit::resolveUnitPerUnit(const MeasureUnit&, const MeasureUnit&, bool*) | Internal | (missing) | measunit.h | <tt>static</tt> int32_t icu::MeasureUnit::getIndexCount() | Internal | (missing) | measunit.h | <tt>static</tt> int32_t icu::MeasureUnit::internalGetIndexForTypeAndSubtype(const char*, const char*) | Internal | (missing) | nounit.h | UClassID icu::NoUnit::getDynamicClassID() const | DraftICU 60 | (missing) @@ -35,6 +37,7 @@ Removed from ICU 67 | nounit.h | <tt>static</tt> NoUnit icu::NoUnit::permille() | DraftICU 60 | (missing) | nounit.h | <tt>static</tt> UClassID icu::NoUnit::getStaticClassID() | DraftICU 60 | (missing) | nounit.h | void* icu::NoUnit::clone() const | DraftICU 60 | (missing) +| uniset.h | const USet* icu::UnicodeSet::toUSet() | StableICU 4.2 | (missing) ## Deprecated @@ -57,6 +60,7 @@ Changed in ICU 68 (old, new) |---|---|---|---| | bytestrie.h | BytesTrie& icu::BytesTrie::resetToState64(uint64_t) | Draft→StableICU 65 | bytestrie.h | uint64_t icu::BytesTrie::getState64() const | Draft→StableICU 65 +| listformatter.h | <tt>static</tt> ListFormatter* icu::ListFormatter::createInstance(const Locale&, UListFormatterType, UListFormatterWidth, UErrorCode&) | Draft→StableICU 67 | localebuilder.h | UBool icu::LocaleBuilder::copyErrorTo(UErrorCode&) const | Draft→StableICU 65 | localematcher.h | Builder& icu::LocaleMatcher::Builder::addSupportedLocale(const Locale&) | Draft→StableICU 65 | localematcher.h | Builder& icu::LocaleMatcher::Builder::operator=(Builder&&) | Draft→StableICU 65 @@ -132,6 +136,13 @@ Changed in ICU 68 (old, new) | ucal.h | int32_t ucal_getHostTimeZone(UChar*, int32_t, UErrorCode*) | Draft→StableICU 65 | ucharstrie.h | UCharsTrie& icu::UCharsTrie::resetToState64(uint64_t) | Draft→StableICU 65 | ucharstrie.h | uint64_t icu::UCharsTrie::getState64() const | Draft→StableICU 65 +| ulistformatter.h | UListFormatter* ulistfmt_openForType(const char*, UListFormatterType, UListFormatterWidth, UErrorCode*) | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_AND | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_OR | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_UNITS | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_NARROW | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_SHORT | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_WIDE | Draft→StableICU 67 | uloc.h | UEnumeration* uloc_openAvailableByType(ULocAvailableType, UErrorCode*) | Draft→StableICU 65 | uloc.h | <tt>enum</tt> ULocAvailableType::ULOC_AVAILABLE_DEFAULT | Draft→StableICU 65 | uloc.h | <tt>enum</tt> ULocAvailableType::ULOC_AVAILABLE_ONLY_LEGACY_ALIASES | Draft→StableICU 65 @@ -151,6 +162,8 @@ Promoted to stable in ICU 68 |---|---|---|---| | bytestrie.h | BytesTrie& icu::BytesTrie::resetToState64(uint64_t) | Draft→StableICU 65 | bytestrie.h | uint64_t icu::BytesTrie::getState64() const | Draft→StableICU 65 +| fmtable.h | UFormattable* icu::Formattable::toUFormattable() | (missing) | StableICU 52 +| listformatter.h | <tt>static</tt> ListFormatter* icu::ListFormatter::createInstance(const Locale&, UListFormatterType, UListFormatterWidth, UErrorCode&) | Draft→StableICU 67 | localebuilder.h | UBool icu::LocaleBuilder::copyErrorTo(UErrorCode&) const | Draft→StableICU 65 | localematcher.h | Builder& icu::LocaleMatcher::Builder::addSupportedLocale(const Locale&) | Draft→StableICU 65 | localematcher.h | Builder& icu::LocaleMatcher::Builder::operator=(Builder&&) | Draft→StableICU 65 @@ -224,10 +237,18 @@ Promoted to stable in ICU 68 | ucal.h | int32_t ucal_getHostTimeZone(UChar*, int32_t, UErrorCode*) | Draft→StableICU 65 | ucharstrie.h | UCharsTrie& icu::UCharsTrie::resetToState64(uint64_t) | Draft→StableICU 65 | ucharstrie.h | uint64_t icu::UCharsTrie::getState64() const | Draft→StableICU 65 +| ulistformatter.h | UListFormatter* ulistfmt_openForType(const char*, UListFormatterType, UListFormatterWidth, UErrorCode*) | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_AND | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_OR | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_UNITS | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_NARROW | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_SHORT | Draft→StableICU 67 +| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_WIDE | Draft→StableICU 67 | uloc.h | UEnumeration* uloc_openAvailableByType(ULocAvailableType, UErrorCode*) | Draft→StableICU 65 | uloc.h | <tt>enum</tt> ULocAvailableType::ULOC_AVAILABLE_DEFAULT | Draft→StableICU 65 | uloc.h | <tt>enum</tt> ULocAvailableType::ULOC_AVAILABLE_ONLY_LEGACY_ALIASES | Draft→StableICU 65 | uloc.h | <tt>enum</tt> ULocAvailableType::ULOC_AVAILABLE_WITH_LEGACY_ALIASES | Draft→StableICU 65 +| uniset.h | USet* icu::UnicodeSet::toUSet() | (missing) | StableICU 4.2 | utrace.h | <tt>enum</tt> UTraceFunctionNumber::UTRACE_UDATA_BUNDLE | Draft→StableICU 65 | utrace.h | <tt>enum</tt> UTraceFunctionNumber::UTRACE_UDATA_DATA_FILE | Draft→StableICU 65 | utrace.h | <tt>enum</tt> UTraceFunctionNumber::UTRACE_UDATA_RES_FILE | Draft→StableICU 65 @@ -242,6 +263,7 @@ Added in ICU 68 | dtitvfmt.h | UDisplayContext icu::DateIntervalFormat::getContext(UDisplayContextType, UErrorCode&) const | (missing) | DraftICU 68 | dtitvfmt.h | void icu::DateIntervalFormat::setContext(UDisplayContext, UErrorCode&) | (missing) | DraftICU 68 | dtptngen.h | <tt>static</tt> DateTimePatternGenerator* icu::DateTimePatternGenerator::createInstanceNoStdPat(const Locale&, UErrorCode&) | (missing) | Internal +| fmtable.h | UFormattable* icu::Formattable::toUFormattable() | (missing) | StableICU 52 | localematcher.h | Builder& icu::LocaleMatcher::Builder::setMaxDistance(const Locale&, const Locale&) | (missing) | DraftICU 68 | localematcher.h | Builder& icu::LocaleMatcher::Builder::setNoDefaultLocale() | (missing) | DraftICU 68 | localematcher.h | UBool icu::LocaleMatcher::isMatch(const Locale&, const Locale&, UErrorCode&) const | (missing) | DraftICU 68 @@ -285,12 +307,14 @@ Added in ICU 68 | numberrangeformatter.h | std::pair< StringClass, StringClass > icu::number::FormattedNumberRange::getDecimalNumbers(UErrorCode&) const | (missing) | DraftICU 68 | plurrule.h | UnicodeString icu::PluralRules::select(const number::FormattedNumberRange&, UErrorCode&) const | (missing) | DraftICU 68 | plurrule.h | UnicodeString icu::PluralRules::select(const number::impl::UFormattedNumberRangeData*, UErrorCode&) const | (missing) | Internal +| plurrule.h | int32_t icu::PluralRules::getSamples(const UnicodeString&, FixedDecimal*, int32_t, UErrorCode&) | (missing) | Internal | timezone.h | <tt>static</tt> TimeZone* icu::TimeZone::forLocaleOrDefault(const Locale&) | (missing) | Internal | ucurr.h | <tt>enum</tt> UCurrNameStyle::UCURR_FORMAL_SYMBOL_NAME | (missing) | DraftICU 68 | ucurr.h | <tt>enum</tt> UCurrNameStyle::UCURR_VARIANT_SYMBOL_NAME | (missing) | DraftICU 68 | udateintervalformat.h | UDisplayContext udtitvfmt_getContext(const UDateIntervalFormat*, UDisplayContextType, UErrorCode*) | (missing) | DraftICU 68 | udateintervalformat.h | void udtitvfmt_setContext(UDateIntervalFormat*, UDisplayContext, UErrorCode*) | (missing) | DraftICU 68 | umachine.h | <tt>#define</tt> U_DEFINE_FALSE_AND_TRUE | (missing) | InternalICU 68 +| uniset.h | USet* icu::UnicodeSet::toUSet() | (missing) | StableICU 4.2 | unum.h | <tt>enum</tt> UNumberFormatMinimumGroupingDigits::UNUM_MINIMUM_GROUPING_DIGITS_AUTO | (missing) | DraftICU 68 | unum.h | <tt>enum</tt> UNumberFormatMinimumGroupingDigits::UNUM_MINIMUM_GROUPING_DIGITS_MIN2 | (missing) | DraftICU 68 | unumberformatter.h | <tt>enum</tt> UNumberUnitWidth::UNUM_UNIT_WIDTH_FORMAL | (missing) | DraftICU 68 @@ -317,7 +341,6 @@ Other existing drafts in ICU 68 | bytestream.h | void icu::ByteSink::AppendU8(const char*, int32_t) | DraftICU 67 | | bytestream.h | void icu::ByteSink::AppendU8(const char8_t*, int32_t) | DraftICU 67 | | dtptngen.h | UDateFormatHourCycle icu::DateTimePatternGenerator::getDefaultHourCycle(UErrorCode&) const | DraftICU 67 | -| listformatter.h | <tt>static</tt> ListFormatter* icu::ListFormatter::createInstance(const Locale&, UListFormatterType, UListFormatterWidth, UErrorCode&) | DraftICU 67 | | localematcher.h | Builder& icu::LocaleMatcher::Builder::setDirection(ULocMatchDirection) | DraftICU 67 | | localematcher.h | <tt>enum</tt> ULocMatchDirection::ULOCMATCH_DIRECTION_ONLY_TWO_WAY | DraftICU 67 | | localematcher.h | <tt>enum</tt> ULocMatchDirection::ULOCMATCH_DIRECTION_WITH_ONE_WAY | DraftICU 67 | @@ -349,13 +372,6 @@ Other existing drafts in ICU 68 | udateintervalformat.h | void udtitvfmt_formatCalendarToResult(const UDateIntervalFormat*, UCalendar*, UCalendar*, UFormattedDateInterval*, UErrorCode*) | DraftICU 67 | | udateintervalformat.h | void udtitvfmt_formatToResult(const UDateIntervalFormat*, UDate, UDate, UFormattedDateInterval*, UErrorCode*) | DraftICU 67 | | udatpg.h | UDateFormatHourCycle udatpg_getDefaultHourCycle(const UDateTimePatternGenerator*, UErrorCode*) | DraftICU 67 | -| ulistformatter.h | UListFormatter* ulistfmt_openForType(const char*, UListFormatterType, UListFormatterWidth, UErrorCode*) | DraftICU 67 | -| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_AND | DraftICU 67 | -| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_OR | DraftICU 67 | -| ulistformatter.h | <tt>enum</tt> UListFormatterType::ULISTFMT_TYPE_UNITS | DraftICU 67 | -| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_NARROW | DraftICU 67 | -| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_SHORT | DraftICU 67 | -| ulistformatter.h | <tt>enum</tt> UListFormatterWidth::ULISTFMT_WIDTH_WIDE | DraftICU 67 | | uregex.h | <tt>enum</tt> URegexpFlag::UREGEX_CANON_EQ | DraftICU 2.4 | | utrace.h | <tt>enum</tt> UTraceFunctionNumber::UTRACE_UBRK_CREATE_BREAK_ENGINE | DraftICU 67 | | utrace.h | <tt>enum</tt> UTraceFunctionNumber::UTRACE_UBRK_CREATE_CHARACTER | DraftICU 67 | @@ -373,7 +389,7 @@ This section shows cases where the signature was "simplified" for the sake of co ## Colophon -Contents generated by StableAPI tool on Wed Sep 30 17:44:26 PDT 2020 +Contents generated by StableAPI tool on Fri Oct 23 11:32:42 PDT 2020 Copyright © 2019 and later: Unicode, Inc. and others. License & terms of use: http://www.unicode.org/copyright.html diff --git a/thirdparty/icu4c/common/cmemory.h b/thirdparty/icu4c/common/cmemory.h index 210bc7645e..a9d9424b4e 100644 --- a/thirdparty/icu4c/common/cmemory.h +++ b/thirdparty/icu4c/common/cmemory.h @@ -725,9 +725,14 @@ public: } MemoryPool& operator=(MemoryPool&& other) U_NOEXCEPT { - fCount = other.fCount; - fPool = std::move(other.fPool); - other.fCount = 0; + // Since `this` may contain instances that need to be deleted, we can't + // just throw them away and replace them with `other`. The normal way of + // dealing with this in C++ is to swap `this` and `other`, rather than + // simply overwrite: the destruction of `other` can then take care of + // running MemoryPool::~MemoryPool() over the still-to-be-deallocated + // instances. + std::swap(fCount, other.fCount); + std::swap(fPool, other.fPool); return *this; } @@ -796,9 +801,6 @@ protected: template<typename T, int32_t stackCapacity = 8> class MaybeStackVector : protected MemoryPool<T, stackCapacity> { public: - using MemoryPool<T, stackCapacity>::MemoryPool; - using MemoryPool<T, stackCapacity>::operator=; - template<typename... Args> T* emplaceBack(Args&&... args) { return this->create(args...); diff --git a/thirdparty/icu4c/common/locid.cpp b/thirdparty/icu4c/common/locid.cpp index 2804e36bf6..874e4a7055 100644 --- a/thirdparty/icu4c/common/locid.cpp +++ b/thirdparty/icu4c/common/locid.cpp @@ -35,6 +35,7 @@ #include "unicode/bytestream.h" #include "unicode/locid.h" +#include "unicode/localebuilder.h" #include "unicode/strenum.h" #include "unicode/stringpiece.h" #include "unicode/uloc.h" @@ -1028,7 +1029,7 @@ public: // place the the replaced locale ID in out and return true. // Otherwise return false for no replacement or error. bool replace( - const Locale& locale, CharString& out, UErrorCode status); + const Locale& locale, CharString& out, UErrorCode& status); private: const char* language; @@ -1336,10 +1337,13 @@ AliasReplacer::replaceTerritory(UVector& toBeFreed, UErrorCode& status) // Cannot use nullptr for language because that will construct // the default locale, in that case, use "und" to get the correct // locale. - Locale l(language == nullptr ? "und" : language, nullptr, script); + Locale l = LocaleBuilder() + .setLanguage(language == nullptr ? "und" : language) + .setScript(script) + .build(status); l.addLikelySubtags(status); const char* likelyRegion = l.getCountry(); - CharString* item = nullptr; + LocalPointer<CharString> item; if (likelyRegion != nullptr && uprv_strlen(likelyRegion) > 0) { size_t len = uprv_strlen(likelyRegion); const char* foundInReplacement = uprv_strstr(replacement, @@ -1351,20 +1355,22 @@ AliasReplacer::replaceTerritory(UVector& toBeFreed, UErrorCode& status) *(foundInReplacement-1) == ' '); U_ASSERT(foundInReplacement[len] == ' ' || foundInReplacement[len] == '\0'); - item = new CharString(foundInReplacement, (int32_t)len, status); + item.adoptInsteadAndCheckErrorCode( + new CharString(foundInReplacement, (int32_t)len, status), status); } } - if (item == nullptr) { - item = new CharString(replacement, - (int32_t)(firstSpace - replacement), status); + if (item.isNull() && U_SUCCESS(status)) { + item.adoptInsteadAndCheckErrorCode( + new CharString(replacement, + (int32_t)(firstSpace - replacement), status), status); } if (U_FAILURE(status)) { return false; } - if (item == nullptr) { + if (item.isNull()) { status = U_MEMORY_ALLOCATION_ERROR; return false; } replacedRegion = item->data(); - toBeFreed.addElement(item, status); + toBeFreed.addElement(item.orphan(), status); } U_ASSERT(!same(region, replacedRegion)); region = replacedRegion; @@ -1453,7 +1459,7 @@ AliasReplacer::outputToString( int32_t variantsStart = out.length(); for (int32_t i = 0; i < variants.size(); i++) { out.append(SEP_CHAR, status) - .append((const char*)((UVector*)variants.elementAt(i)), + .append((const char*)(variants.elementAt(i)), status); } T_CString_toUpperCase(out.data() + variantsStart); @@ -1470,7 +1476,7 @@ AliasReplacer::outputToString( } bool -AliasReplacer::replace(const Locale& locale, CharString& out, UErrorCode status) +AliasReplacer::replace(const Locale& locale, CharString& out, UErrorCode& status) { data = AliasData::singleton(status); if (U_FAILURE(status)) { @@ -2453,9 +2459,13 @@ Locale::setKeywordValue(const char* keywordName, const char* keywordValue, UErro if (U_FAILURE(status)) { return; } + if (status == U_STRING_NOT_TERMINATED_WARNING) { + status = U_ZERO_ERROR; + } int32_t bufferLength = uprv_max((int32_t)(uprv_strlen(fullName) + 1), ULOC_FULLNAME_CAPACITY); int32_t newLength = uloc_setKeywordValue(keywordName, keywordValue, fullName, bufferLength, &status) + 1; + U_ASSERT(status != U_STRING_NOT_TERMINATED_WARNING); /* Handle the case the current buffer is not enough to hold the new id */ if (status == U_BUFFER_OVERFLOW_ERROR) { U_ASSERT(newLength > bufferLength); @@ -2472,6 +2482,7 @@ Locale::setKeywordValue(const char* keywordName, const char* keywordValue, UErro fullName = newFullName; status = U_ZERO_ERROR; uloc_setKeywordValue(keywordName, keywordValue, fullName, newLength, &status); + U_ASSERT(status != U_STRING_NOT_TERMINATED_WARNING); } else { U_ASSERT(newLength <= bufferLength); } diff --git a/thirdparty/icu4c/common/rbbitblb.cpp b/thirdparty/icu4c/common/rbbitblb.cpp index bcbdab9227..70e260fc08 100644 --- a/thirdparty/icu4c/common/rbbitblb.cpp +++ b/thirdparty/icu4c/common/rbbitblb.cpp @@ -1402,12 +1402,13 @@ void RBBITableBuilder::exportTable(void *where) { U_ASSERT (sd->fAccepting <= 255); U_ASSERT (sd->fLookAhead <= 255); U_ASSERT (0 <= sd->fTagsIdx && sd->fTagsIdx <= 255); - row->r8.fAccepting = sd->fAccepting; - row->r8.fLookAhead = sd->fLookAhead; - row->r8.fTagsIdx = sd->fTagsIdx; + RBBIStateTableRow8 *r8 = (RBBIStateTableRow8*)row; + r8->fAccepting = sd->fAccepting; + r8->fLookAhead = sd->fLookAhead; + r8->fTagsIdx = sd->fTagsIdx; for (col=0; col<catCount; col++) { U_ASSERT (sd->fDtran->elementAti(col) <= kMaxStateFor8BitsTable); - row->r8.fNextState[col] = sd->fDtran->elementAti(col); + r8->fNextState[col] = sd->fDtran->elementAti(col); } } else { U_ASSERT (sd->fAccepting <= 0xffff); @@ -1603,12 +1604,13 @@ void RBBITableBuilder::exportSafeTable(void *where) { UnicodeString *rowString = (UnicodeString *)fSafeTable->elementAt(state); RBBIStateTableRow *row = (RBBIStateTableRow *)(table->fTableData + state*table->fRowLen); if (use8BitsForSafeTable()) { - row->r8.fAccepting = 0; - row->r8.fLookAhead = 0; - row->r8.fTagsIdx = 0; + RBBIStateTableRow8 *r8 = (RBBIStateTableRow8*)row; + r8->fAccepting = 0; + r8->fLookAhead = 0; + r8->fTagsIdx = 0; for (col=0; col<catCount; col++) { U_ASSERT(rowString->charAt(col) <= kMaxStateFor8BitsTable); - row->r8.fNextState[col] = static_cast<uint8_t>(rowString->charAt(col)); + r8->fNextState[col] = static_cast<uint8_t>(rowString->charAt(col)); } } else { row->r16.fAccepting = 0; diff --git a/thirdparty/icu4c/common/uloc.cpp b/thirdparty/icu4c/common/uloc.cpp index 522f33dbe2..ebfbb50650 100644 --- a/thirdparty/icu4c/common/uloc.cpp +++ b/thirdparty/icu4c/common/uloc.cpp @@ -877,6 +877,9 @@ uloc_setKeywordValue(const char* keywordName, if(U_FAILURE(*status)) { return -1; } + if (*status == U_STRING_NOT_TERMINATED_WARNING) { + *status = U_ZERO_ERROR; + } if (keywordName == NULL || keywordName[0] == 0 || bufferCapacity <= 1) { *status = U_ILLEGAL_ARGUMENT_ERROR; return 0; @@ -914,6 +917,7 @@ uloc_setKeywordValue(const char* keywordName, startSearchHere = (char*)locale_getKeywordsStart(buffer); if(startSearchHere == NULL || (startSearchHere[1]==0)) { if(keywordValueLen == 0) { /* no keywords = nothing to remove */ + U_ASSERT(*status != U_STRING_NOT_TERMINATED_WARNING); return bufLen; } @@ -933,6 +937,7 @@ uloc_setKeywordValue(const char* keywordName, startSearchHere += keywordNameLen; *startSearchHere++ = '='; uprv_strcpy(startSearchHere, keywordValueBuffer); + U_ASSERT(*status != U_STRING_NOT_TERMINATED_WARNING); return needLen; } /* end shortcut - no @ */ @@ -1047,13 +1052,27 @@ uloc_setKeywordValue(const char* keywordName, if (!handledInputKeyAndValue || U_FAILURE(*status)) { /* if input key/value specified removal of a keyword not present in locale, or * there was an error in CharString.append, leave original locale alone. */ + U_ASSERT(*status != U_STRING_NOT_TERMINATED_WARNING); return bufLen; } // needLen = length of the part before '@' needLen = (int32_t)(startSearchHere - buffer); - return needLen + updatedKeysAndValues.extract( + // Check to see can we fit the startSearchHere, if not, return + // U_BUFFER_OVERFLOW_ERROR without copy updatedKeysAndValues into it. + // We do this because this API function does not behave like most others: + // It promises never to set a U_STRING_NOT_TERMINATED_WARNING. + // When the contents fits but without the terminating NUL, in this case we need to not change + // the buffer contents and return with a buffer overflow error. + int32_t appendLength = updatedKeysAndValues.length(); + if (appendLength >= bufferCapacity - needLen) { + *status = U_BUFFER_OVERFLOW_ERROR; + return needLen + appendLength; + } + needLen += updatedKeysAndValues.extract( startSearchHere, bufferCapacity - needLen, *status); + U_ASSERT(*status != U_STRING_NOT_TERMINATED_WARNING); + return needLen; } /* ### ID parsing implementation **************************************************/ diff --git a/thirdparty/icu4c/common/unicode/docmain.h b/thirdparty/icu4c/common/unicode/docmain.h index b7984ada03..edcb5d4e83 100644 --- a/thirdparty/icu4c/common/unicode/docmain.h +++ b/thirdparty/icu4c/common/unicode/docmain.h @@ -143,6 +143,11 @@ * <td>icu::MessageFormat</td> * </tr> * <tr> + * <td>List Formatting</td> + * <td>ulistformatter.h</td> + * <td>icu::ListFormatter</td> + * </tr> + * <tr> * <td>Number Formatting<br/>(includes currency and unit formatting)</td> * <td>unumberformatter.h, unum.h</td> * <td>icu::number::NumberFormatter (ICU 60+) or icu::NumberFormat (older versions)</td> diff --git a/thirdparty/icu4c/common/unicode/urename.h b/thirdparty/icu4c/common/unicode/urename.h index 20232cd209..fe59fdd893 100644 --- a/thirdparty/icu4c/common/unicode/urename.h +++ b/thirdparty/icu4c/common/unicode/urename.h @@ -1137,6 +1137,7 @@ #define ulocimp_toLanguageTag U_ICU_ENTRY_POINT_RENAME(ulocimp_toLanguageTag) #define ulocimp_toLegacyKey U_ICU_ENTRY_POINT_RENAME(ulocimp_toLegacyKey) #define ulocimp_toLegacyType U_ICU_ENTRY_POINT_RENAME(ulocimp_toLegacyType) +#define ultag_getTKeyStart U_ICU_ENTRY_POINT_RENAME(ultag_getTKeyStart) #define ultag_isExtensionSubtags U_ICU_ENTRY_POINT_RENAME(ultag_isExtensionSubtags) #define ultag_isLanguageSubtag U_ICU_ENTRY_POINT_RENAME(ultag_isLanguageSubtag) #define ultag_isPrivateuseValueSubtags U_ICU_ENTRY_POINT_RENAME(ultag_isPrivateuseValueSubtags) diff --git a/thirdparty/icu4c/common/unicode/uvernum.h b/thirdparty/icu4c/common/unicode/uvernum.h index a4cbb9e0fe..a46481a3fe 100644 --- a/thirdparty/icu4c/common/unicode/uvernum.h +++ b/thirdparty/icu4c/common/unicode/uvernum.h @@ -66,7 +66,7 @@ * This value will change in the subsequent releases of ICU * @stable ICU 2.6 */ -#define U_ICU_VERSION_MINOR_NUM 1 +#define U_ICU_VERSION_MINOR_NUM 2 /** The current ICU patchlevel version as an integer. * This value will change in the subsequent releases of ICU @@ -139,7 +139,7 @@ * This value will change in the subsequent releases of ICU * @stable ICU 2.4 */ -#define U_ICU_VERSION "68.1" +#define U_ICU_VERSION "68.2" /** * The current ICU library major version number as a string, for library name suffixes. @@ -158,7 +158,7 @@ /** Data version in ICU4C. * @internal ICU 4.4 Internal Use Only **/ -#define U_ICU_DATA_VERSION "68.1" +#define U_ICU_DATA_VERSION "68.2" #endif /* U_HIDE_INTERNAL_API */ /*=========================================================================== diff --git a/thirdparty/icu4c/common/wintz.cpp b/thirdparty/icu4c/common/wintz.cpp index 3730232286..580cedadb6 100644 --- a/thirdparty/icu4c/common/wintz.cpp +++ b/thirdparty/icu4c/common/wintz.cpp @@ -36,17 +36,58 @@ U_NAMESPACE_BEGIN +// Note these constants and the struct are only used when dealing with the fallback path for RDP sesssions. + +// This is the location of the time zones in the registry on Vista+ systems. +// See: https://docs.microsoft.com/windows/win32/api/timezoneapi/ns-timezoneapi-dynamic_time_zone_information +#define WINDOWS_TIMEZONES_REG_KEY_PATH L"SOFTWARE\\Microsoft\\Windows NT\\CurrentVersion\\Time Zones" + +// Max length for a registry key is 255. +1 for null. +// See: https://docs.microsoft.com/windows/win32/sysinfo/registry-element-size-limits +#define WINDOWS_MAX_REG_KEY_LENGTH 256 + +#if U_PLATFORM_HAS_WINUWP_API == 0 + +// This is the layout of the TZI binary value in the registry. +// See: https://docs.microsoft.com/windows/win32/api/timezoneapi/ns-timezoneapi-time_zone_information +typedef struct _REG_TZI_FORMAT { + LONG Bias; + LONG StandardBias; + LONG DaylightBias; + SYSTEMTIME StandardDate; + SYSTEMTIME DaylightDate; +} REG_TZI_FORMAT; + +#endif // U_PLATFORM_HAS_WINUWP_API + /** -* Main Windows time zone detection function. -* Returns the Windows time zone converted to an ICU time zone as a heap-allocated buffer, or nullptr upon failure. +* This is main Windows time zone detection function. +* +* It returns the Windows time zone converted to an ICU time zone as a heap-allocated buffer, or nullptr upon failure. * -* Note: We use the Win32 API GetDynamicTimeZoneInformation (available since Vista+) to get the current time zone info. -* This API returns a non-localized time zone name, which is mapped to an ICU time zone ID (~ Olsen ID). +* We use the Win32 API GetDynamicTimeZoneInformation (which is available since Vista) to get the current time zone info, +* as this API returns a non-localized time zone name which can be then mapped to an ICU time zone. +* +* However, in some RDP/terminal services situations, this struct isn't always fully complete, and the TimeZoneKeyName +* field of the struct might be NULL. This can happen with some 3rd party RDP clients, and also when using older versions +* of the RDP protocol, which don't send the newer TimeZoneKeyNamei information and only send the StandardName and DaylightName. +* +* Since these 3rd party clients and older RDP clients only send the pre-Vista time zone information to the server, this means that we +* need to fallback on using the pre-Vista methods to determine the time zone. This unfortunately requires examining the registry directly +* in order to try and determine the current time zone. +* +* Note that this can however still fail in some cases though if the client and server are using different languages, as the StandardName +* that is sent by client is localized in the client's language. However, we must compare this to the names that are on the server, which +* are localized in registry using the server's language. Despite that, this is the best we can do. +* +* Note: This fallback method won't work for the UWP version though, as we can't use the registry APIs in UWP. +* +* Once we have the current Windows time zone, then we can then map it to an ICU time zone ID (~ Olsen ID). */ U_CAPI const char* U_EXPORT2 uprv_detectWindowsTimeZone() { - // Obtain the DYNAMIC_TIME_ZONE_INFORMATION info to get the non-localized time zone name. + // We first try to obtain the time zone directly by using the TimeZoneKeyName field of the DYNAMIC_TIME_ZONE_INFORMATION struct. DYNAMIC_TIME_ZONE_INFORMATION dynamicTZI; uprv_memset(&dynamicTZI, 0, sizeof(dynamicTZI)); SYSTEMTIME systemTimeAllZero; @@ -86,22 +127,138 @@ uprv_detectWindowsTimeZone() // Note '-' before 'utcOffsetMin'. The timezone ID's sign convention // is that a timezone ahead of UTC is Etc/GMT-<offset> and a timezone // behind UTC is Etc/GMT+<offset>. - int ret = snprintf(gmtOffsetTz, UPRV_LENGTHOF(gmtOffsetTz), "Etc/GMT%+d", -utcOffsetMins / 60); + int ret = snprintf(gmtOffsetTz, UPRV_LENGTHOF(gmtOffsetTz), "Etc/GMT%+ld", -utcOffsetMins / 60); if (ret > 0 && ret < UPRV_LENGTHOF(gmtOffsetTz)) { return uprv_strdup(gmtOffsetTz); } } } - // If DST is NOT disabled, but we have an empty TimeZoneKeyName, then it is unclear - // what we should do as this should not happen. + // If DST is NOT disabled, but the TimeZoneKeyName field of the struct is NULL, then we may be dealing with a + // RDP/terminal services session where the 'Time Zone Redirection' feature is enabled. However, either the RDP + // client sent the server incomplete info (some 3rd party RDP clients only send the StandardName and DaylightName, + // but do not send the important TimeZoneKeyName), or if the RDP server has not appropriately populated the struct correctly. + // + // In this case we unfortunately have no choice but to fallback to using the pre-Vista method of determining the + // time zone, which requires examining the registry directly. + // + // Note that this can however still fail though if the client and server are using different languages, as the StandardName + // that is sent by client is *localized* in the client's language. However, we must compare this to the names that are + // on the server, which are *localized* in registry using the server's language. + // + // One other note is that this fallback method doesn't work for the UWP version, as we can't use the registry APIs. + + // windowsTimeZoneName will point at timezoneSubKeyName if we had to fallback to using the registry, and we found a match. + WCHAR timezoneSubKeyName[WINDOWS_MAX_REG_KEY_LENGTH]; + WCHAR *windowsTimeZoneName = dynamicTZI.TimeZoneKeyName; + if (dynamicTZI.TimeZoneKeyName[0] == 0) { + +// We can't use the registry APIs in the UWP version. +#if U_PLATFORM_HAS_WINUWP_API == 1 + (void)timezoneSubKeyName; // suppress unused variable warnings. return nullptr; +#else + // Open the path to the time zones in the Windows registry. + LONG ret; + HKEY hKeyAllTimeZones = nullptr; + ret = RegOpenKeyExW(HKEY_LOCAL_MACHINE, WINDOWS_TIMEZONES_REG_KEY_PATH, 0, KEY_READ, + reinterpret_cast<PHKEY>(&hKeyAllTimeZones)); + + if (ret != ERROR_SUCCESS) { + // If we can't open the key, then we can't do much, so fail. + return nullptr; + } + + // Read the number of subkeys under the time zone registry path. + DWORD numTimeZoneSubKeys; + ret = RegQueryInfoKeyW(hKeyAllTimeZones, nullptr, nullptr, nullptr, &numTimeZoneSubKeys, + nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr); + + if (ret != ERROR_SUCCESS) { + RegCloseKey(hKeyAllTimeZones); + return nullptr; + } + + // Examine each of the subkeys to try and find a match for the localized standard name ("Std"). + // + // Note: The name of the time zone subkey itself is not localized, but the "Std" name is localized. This means + // that we could fail to find a match if the RDP client and RDP server are using different languages, but unfortunately + // there isn't much we can do about it. + HKEY hKeyTimeZoneSubKey = nullptr; + ULONG registryValueType; + WCHAR registryStandardName[WINDOWS_MAX_REG_KEY_LENGTH]; + + for (DWORD i = 0; i < numTimeZoneSubKeys; i++) { + // Note: RegEnumKeyExW wants the size of the buffer in characters. + DWORD size = UPRV_LENGTHOF(timezoneSubKeyName); + ret = RegEnumKeyExW(hKeyAllTimeZones, i, timezoneSubKeyName, &size, nullptr, nullptr, nullptr, nullptr); + + if (ret != ERROR_SUCCESS) { + RegCloseKey(hKeyAllTimeZones); + return nullptr; + } + + ret = RegOpenKeyExW(hKeyAllTimeZones, timezoneSubKeyName, 0, KEY_READ, + reinterpret_cast<PHKEY>(&hKeyTimeZoneSubKey)); + + if (ret != ERROR_SUCCESS) { + RegCloseKey(hKeyAllTimeZones); + return nullptr; + } + + // Note: RegQueryValueExW wants the size of the buffer in bytes. + size = sizeof(registryStandardName); + ret = RegQueryValueExW(hKeyTimeZoneSubKey, L"Std", nullptr, ®istryValueType, + reinterpret_cast<LPBYTE>(registryStandardName), &size); + + if (ret != ERROR_SUCCESS || registryValueType != REG_SZ) { + RegCloseKey(hKeyTimeZoneSubKey); + RegCloseKey(hKeyAllTimeZones); + return nullptr; + } + + // Note: wcscmp does an ordinal (byte) comparison. + if (wcscmp(reinterpret_cast<WCHAR *>(registryStandardName), dynamicTZI.StandardName) == 0) { + // Since we are comparing the *localized* time zone name, it's possible that some languages might use + // the same string for more than one time zone. Thus we need to examine the TZI data in the registry to + // compare the GMT offset (the bias), and the DST transition dates, to ensure it's the same time zone + // as the currently reported one. + REG_TZI_FORMAT registryTziValue; + uprv_memset(®istryTziValue, 0, sizeof(registryTziValue)); + + // Note: RegQueryValueExW wants the size of the buffer in bytes. + DWORD timezoneTziValueSize = sizeof(registryTziValue); + ret = RegQueryValueExW(hKeyTimeZoneSubKey, L"TZI", nullptr, ®istryValueType, + reinterpret_cast<LPBYTE>(®istryTziValue), &timezoneTziValueSize); + + if (ret == ERROR_SUCCESS) { + if ((dynamicTZI.Bias == registryTziValue.Bias) && + (memcmp((const void *)&dynamicTZI.StandardDate, (const void *)®istryTziValue.StandardDate, sizeof(SYSTEMTIME)) == 0) && + (memcmp((const void *)&dynamicTZI.DaylightDate, (const void *)®istryTziValue.DaylightDate, sizeof(SYSTEMTIME)) == 0)) + { + // We found a matching time zone. + windowsTimeZoneName = timezoneSubKeyName; + break; + } + } + } + RegCloseKey(hKeyTimeZoneSubKey); + hKeyTimeZoneSubKey = nullptr; + } + + if (hKeyTimeZoneSubKey != nullptr) { + RegCloseKey(hKeyTimeZoneSubKey); + } + if (hKeyAllTimeZones != nullptr) { + RegCloseKey(hKeyAllTimeZones); + } +#endif // U_PLATFORM_HAS_WINUWP_API } CharString winTZ; UErrorCode status = U_ZERO_ERROR; - winTZ.appendInvariantChars(UnicodeString(TRUE, dynamicTZI.TimeZoneKeyName, -1), status); + winTZ.appendInvariantChars(UnicodeString(TRUE, windowsTimeZoneName, -1), status); // Map Windows Timezone name (non-localized) to ICU timezone ID (~ Olson timezone id). StackUResourceBundle winTZBundle; @@ -123,18 +280,29 @@ uprv_detectWindowsTimeZone() int regionCodeLen = GetGeoInfoW(geoId, GEO_ISO2, regionCodeW, UPRV_LENGTHOF(regionCodeW), 0); const UChar *icuTZ16 = nullptr; - int32_t tzLen; + int32_t tzListLen = 0; if (regionCodeLen != 0) { for (int i = 0; i < UPRV_LENGTHOF(regionCodeW); i++) { regionCode[i] = static_cast<char>(regionCodeW[i]); } - icuTZ16 = ures_getStringByKey(winTZBundle.getAlias(), regionCode, &tzLen, &status); + icuTZ16 = ures_getStringByKey(winTZBundle.getAlias(), regionCode, &tzListLen, &status); } if (regionCodeLen == 0 || U_FAILURE(status)) { // fallback to default "001" (world) status = U_ZERO_ERROR; - icuTZ16 = ures_getStringByKey(winTZBundle.getAlias(), "001", &tzLen, &status); + icuTZ16 = ures_getStringByKey(winTZBundle.getAlias(), "001", &tzListLen, &status); + } + + // Note: We want the first entry in the string returned by ures_getStringByKey. + // However this string can be a space delimited list of timezones: + // Ex: "America/New_York America/Detroit America/Indiana/Petersburg ..." + // We need to stop at the first space, so we pass tzLen (instead of tzListLen) to appendInvariantChars below. + int32_t tzLen = 0; + if (tzListLen > 0) { + while (!(icuTZ16[tzLen] == u'\0' || icuTZ16[tzLen] == u' ')) { + tzLen++; + } } // Note: cloneData returns nullptr if the status is a failure, so this diff --git a/thirdparty/icu4c/icudt68l.dat b/thirdparty/icu4c/icudt68l.dat Binary files differindex 548c1a5a72..9ecea5d548 100644 --- a/thirdparty/icu4c/icudt68l.dat +++ b/thirdparty/icu4c/icudt68l.dat diff --git a/thirdparty/mbedtls/include/mbedtls/bignum.h b/thirdparty/mbedtls/include/mbedtls/bignum.h index 4bb9fa3d43..4d04b336e7 100644 --- a/thirdparty/mbedtls/include/mbedtls/bignum.h +++ b/thirdparty/mbedtls/include/mbedtls/bignum.h @@ -88,12 +88,12 @@ * Maximum window size used for modular exponentiation. Default: 6 * Minimum value: 1. Maximum value: 6. * - * Result is an array of ( 2 << MBEDTLS_MPI_WINDOW_SIZE ) MPIs used + * Result is an array of ( 2 ** MBEDTLS_MPI_WINDOW_SIZE ) MPIs used * for the sliding window calculation. (So 64 by default) * * Reduction in size, reduces speed. */ -#define MBEDTLS_MPI_WINDOW_SIZE 6 /**< Maximum windows size used. */ +#define MBEDTLS_MPI_WINDOW_SIZE 6 /**< Maximum window size used. */ #endif /* !MBEDTLS_MPI_WINDOW_SIZE */ #if !defined(MBEDTLS_MPI_MAX_SIZE) diff --git a/thirdparty/mbedtls/include/mbedtls/ccm.h b/thirdparty/mbedtls/include/mbedtls/ccm.h index 3dcdc91894..d50c6ec993 100644 --- a/thirdparty/mbedtls/include/mbedtls/ccm.h +++ b/thirdparty/mbedtls/include/mbedtls/ccm.h @@ -175,7 +175,7 @@ void mbedtls_ccm_free( mbedtls_ccm_context *ctx ); * than zero, \p output must be a writable buffer of at least * that length. * \param tag The buffer holding the authentication field. This must be a - * readable buffer of at least \p tag_len Bytes. + * writable buffer of at least \p tag_len Bytes. * \param tag_len The length of the authentication field to generate in Bytes: * 4, 6, 8, 10, 12, 14 or 16. * @@ -220,7 +220,7 @@ int mbedtls_ccm_encrypt_and_tag( mbedtls_ccm_context *ctx, size_t length, * than zero, \p output must be a writable buffer of at least * that length. * \param tag The buffer holding the authentication field. This must be a - * readable buffer of at least \p tag_len Bytes. + * writable buffer of at least \p tag_len Bytes. * \param tag_len The length of the authentication field to generate in Bytes: * 0, 4, 6, 8, 10, 12, 14 or 16. * diff --git a/thirdparty/mbedtls/include/mbedtls/config.h b/thirdparty/mbedtls/include/mbedtls/config.h index 217998a5eb..e17bc7e306 100644 --- a/thirdparty/mbedtls/include/mbedtls/config.h +++ b/thirdparty/mbedtls/include/mbedtls/config.h @@ -3128,7 +3128,7 @@ */ /* MPI / BIGNUM options */ -//#define MBEDTLS_MPI_WINDOW_SIZE 6 /**< Maximum windows size used. */ +//#define MBEDTLS_MPI_WINDOW_SIZE 6 /**< Maximum window size used. */ //#define MBEDTLS_MPI_MAX_SIZE 1024 /**< Maximum number of bytes for usable MPIs. */ /* CTR_DRBG options */ diff --git a/thirdparty/mbedtls/include/mbedtls/ctr_drbg.h b/thirdparty/mbedtls/include/mbedtls/ctr_drbg.h index 7e5f2e5769..278fbbbb7a 100644 --- a/thirdparty/mbedtls/include/mbedtls/ctr_drbg.h +++ b/thirdparty/mbedtls/include/mbedtls/ctr_drbg.h @@ -224,6 +224,11 @@ mbedtls_ctr_drbg_context; * and prepares it for mbedtls_ctr_drbg_seed() * or mbedtls_ctr_drbg_free(). * + * \note The reseed interval is + * #MBEDTLS_CTR_DRBG_RESEED_INTERVAL by default. + * You can override it by calling + * mbedtls_ctr_drbg_set_reseed_interval(). + * * \param ctx The CTR_DRBG context to initialize. */ void mbedtls_ctr_drbg_init( mbedtls_ctr_drbg_context *ctx ); @@ -305,7 +310,8 @@ int mbedtls_ctr_drbg_seed( mbedtls_ctr_drbg_context *ctx, size_t len ); /** - * \brief This function clears CTR_CRBG context data. + * \brief This function resets CTR_DRBG context to the state immediately + * after initial call of mbedtls_ctr_drbg_init(). * * \param ctx The CTR_DRBG context to clear. */ diff --git a/thirdparty/mbedtls/include/mbedtls/gcm.h b/thirdparty/mbedtls/include/mbedtls/gcm.h index 4e4434ed4d..1201fbd4f1 100644 --- a/thirdparty/mbedtls/include/mbedtls/gcm.h +++ b/thirdparty/mbedtls/include/mbedtls/gcm.h @@ -182,7 +182,7 @@ int mbedtls_gcm_setkey( mbedtls_gcm_context *ctx, * than zero, this must be a writable buffer of at least that * size in Bytes. * \param tag_len The length of the tag to generate. - * \param tag The buffer for holding the tag. This must be a readable + * \param tag The buffer for holding the tag. This must be a writable * buffer of at least \p tag_len Bytes. * * \return \c 0 if the encryption or decryption was performed @@ -310,7 +310,7 @@ int mbedtls_gcm_update( mbedtls_gcm_context *ctx, * tag. The tag can have a maximum length of 16 Bytes. * * \param ctx The GCM context. This must be initialized. - * \param tag The buffer for holding the tag. This must be a readable + * \param tag The buffer for holding the tag. This must be a writable * buffer of at least \p tag_len Bytes. * \param tag_len The length of the tag to generate. This must be at least * four. diff --git a/thirdparty/mbedtls/include/mbedtls/hmac_drbg.h b/thirdparty/mbedtls/include/mbedtls/hmac_drbg.h index 6883678204..970c033c15 100644 --- a/thirdparty/mbedtls/include/mbedtls/hmac_drbg.h +++ b/thirdparty/mbedtls/include/mbedtls/hmac_drbg.h @@ -138,6 +138,10 @@ typedef struct mbedtls_hmac_drbg_context * This function makes the context ready for mbedtls_hmac_drbg_seed(), * mbedtls_hmac_drbg_seed_buf() or mbedtls_hmac_drbg_free(). * + * \note The reseed interval is #MBEDTLS_HMAC_DRBG_RESEED_INTERVAL + * by default. Override this value by calling + * mbedtls_hmac_drbg_set_reseed_interval(). + * * \param ctx HMAC_DRBG context to be initialized. */ void mbedtls_hmac_drbg_init( mbedtls_hmac_drbg_context *ctx ); @@ -361,7 +365,8 @@ int mbedtls_hmac_drbg_random_with_add( void *p_rng, int mbedtls_hmac_drbg_random( void *p_rng, unsigned char *output, size_t out_len ); /** - * \brief Free an HMAC_DRBG context + * \brief This function resets HMAC_DRBG context to the state immediately + * after initial call of mbedtls_hmac_drbg_init(). * * \param ctx The HMAC_DRBG context to free. */ diff --git a/thirdparty/mbedtls/include/mbedtls/sha512.h b/thirdparty/mbedtls/include/mbedtls/sha512.h index 9ff78ecf41..5e5a15e000 100644 --- a/thirdparty/mbedtls/include/mbedtls/sha512.h +++ b/thirdparty/mbedtls/include/mbedtls/sha512.h @@ -152,8 +152,7 @@ int mbedtls_sha512_update_ret( mbedtls_sha512_context *ctx, /** * \brief This function finishes the SHA-512 operation, and writes - * the result to the output buffer. This function is for - * internal use only. + * the result to the output buffer. * * \param ctx The SHA-512 context. This must be initialized * and have a hash operation started. @@ -169,6 +168,7 @@ int mbedtls_sha512_finish_ret( mbedtls_sha512_context *ctx, /** * \brief This function processes a single data block within * the ongoing SHA-512 computation. + * This function is for internal use only. * * \param ctx The SHA-512 context. This must be initialized. * \param data The buffer holding one block of data. This diff --git a/thirdparty/mbedtls/include/mbedtls/ssl.h b/thirdparty/mbedtls/include/mbedtls/ssl.h index d3ee3c4e6f..fe33ac8d57 100644 --- a/thirdparty/mbedtls/include/mbedtls/ssl.h +++ b/thirdparty/mbedtls/include/mbedtls/ssl.h @@ -1409,7 +1409,7 @@ void mbedtls_ssl_conf_dbg( mbedtls_ssl_config *conf, * \note For DTLS, you need to provide either a non-NULL * f_recv_timeout callback, or a f_recv that doesn't block. * - * \note See the documentations of \c mbedtls_ssl_sent_t, + * \note See the documentations of \c mbedtls_ssl_send_t, * \c mbedtls_ssl_recv_t and \c mbedtls_ssl_recv_timeout_t for * the conventions those callbacks must follow. * diff --git a/thirdparty/mbedtls/include/mbedtls/version.h b/thirdparty/mbedtls/include/mbedtls/version.h index d09b45002d..5f0a8f114c 100644 --- a/thirdparty/mbedtls/include/mbedtls/version.h +++ b/thirdparty/mbedtls/include/mbedtls/version.h @@ -65,16 +65,16 @@ */ #define MBEDTLS_VERSION_MAJOR 2 #define MBEDTLS_VERSION_MINOR 16 -#define MBEDTLS_VERSION_PATCH 8 +#define MBEDTLS_VERSION_PATCH 9 /** * The single version number has the following structure: * MMNNPP00 * Major version | Minor version | Patch version */ -#define MBEDTLS_VERSION_NUMBER 0x02100800 -#define MBEDTLS_VERSION_STRING "2.16.8" -#define MBEDTLS_VERSION_STRING_FULL "mbed TLS 2.16.8" +#define MBEDTLS_VERSION_NUMBER 0x02100900 +#define MBEDTLS_VERSION_STRING "2.16.9" +#define MBEDTLS_VERSION_STRING_FULL "mbed TLS 2.16.9" #if defined(MBEDTLS_VERSION_C) diff --git a/thirdparty/mbedtls/library/aes.c b/thirdparty/mbedtls/library/aes.c index 9b337505fd..da0e5b6bdc 100644 --- a/thirdparty/mbedtls/library/aes.c +++ b/thirdparty/mbedtls/library/aes.c @@ -760,6 +760,7 @@ exit: return( ret ); } +#endif /* !MBEDTLS_AES_SETKEY_DEC_ALT */ #if defined(MBEDTLS_CIPHER_MODE_XTS) static int mbedtls_aes_xts_decode_keys( const unsigned char *key, @@ -838,8 +839,6 @@ int mbedtls_aes_xts_setkey_dec( mbedtls_aes_xts_context *ctx, } #endif /* MBEDTLS_CIPHER_MODE_XTS */ -#endif /* !MBEDTLS_AES_SETKEY_DEC_ALT */ - #define AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3) \ do \ { \ @@ -897,63 +896,56 @@ int mbedtls_internal_aes_encrypt( mbedtls_aes_context *ctx, unsigned char output[16] ) { int i; - uint32_t *RK, X0, X1, X2, X3, Y0, Y1, Y2, Y3; - - RK = ctx->rk; + uint32_t *RK = ctx->rk; + struct + { + uint32_t X[4]; + uint32_t Y[4]; + } t; - GET_UINT32_LE( X0, input, 0 ); X0 ^= *RK++; - GET_UINT32_LE( X1, input, 4 ); X1 ^= *RK++; - GET_UINT32_LE( X2, input, 8 ); X2 ^= *RK++; - GET_UINT32_LE( X3, input, 12 ); X3 ^= *RK++; + GET_UINT32_LE( t.X[0], input, 0 ); t.X[0] ^= *RK++; + GET_UINT32_LE( t.X[1], input, 4 ); t.X[1] ^= *RK++; + GET_UINT32_LE( t.X[2], input, 8 ); t.X[2] ^= *RK++; + GET_UINT32_LE( t.X[3], input, 12 ); t.X[3] ^= *RK++; for( i = ( ctx->nr >> 1 ) - 1; i > 0; i-- ) { - AES_FROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 ); - AES_FROUND( X0, X1, X2, X3, Y0, Y1, Y2, Y3 ); + AES_FROUND( t.Y[0], t.Y[1], t.Y[2], t.Y[3], t.X[0], t.X[1], t.X[2], t.X[3] ); + AES_FROUND( t.X[0], t.X[1], t.X[2], t.X[3], t.Y[0], t.Y[1], t.Y[2], t.Y[3] ); } - AES_FROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 ); - - X0 = *RK++ ^ \ - ( (uint32_t) FSb[ ( Y0 ) & 0xFF ] ) ^ - ( (uint32_t) FSb[ ( Y1 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) FSb[ ( Y2 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) FSb[ ( Y3 >> 24 ) & 0xFF ] << 24 ); - - X1 = *RK++ ^ \ - ( (uint32_t) FSb[ ( Y1 ) & 0xFF ] ) ^ - ( (uint32_t) FSb[ ( Y2 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) FSb[ ( Y3 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) FSb[ ( Y0 >> 24 ) & 0xFF ] << 24 ); - - X2 = *RK++ ^ \ - ( (uint32_t) FSb[ ( Y2 ) & 0xFF ] ) ^ - ( (uint32_t) FSb[ ( Y3 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) FSb[ ( Y0 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) FSb[ ( Y1 >> 24 ) & 0xFF ] << 24 ); - - X3 = *RK++ ^ \ - ( (uint32_t) FSb[ ( Y3 ) & 0xFF ] ) ^ - ( (uint32_t) FSb[ ( Y0 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) FSb[ ( Y1 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) FSb[ ( Y2 >> 24 ) & 0xFF ] << 24 ); - - PUT_UINT32_LE( X0, output, 0 ); - PUT_UINT32_LE( X1, output, 4 ); - PUT_UINT32_LE( X2, output, 8 ); - PUT_UINT32_LE( X3, output, 12 ); - - mbedtls_platform_zeroize( &X0, sizeof( X0 ) ); - mbedtls_platform_zeroize( &X1, sizeof( X1 ) ); - mbedtls_platform_zeroize( &X2, sizeof( X2 ) ); - mbedtls_platform_zeroize( &X3, sizeof( X3 ) ); - - mbedtls_platform_zeroize( &Y0, sizeof( Y0 ) ); - mbedtls_platform_zeroize( &Y1, sizeof( Y1 ) ); - mbedtls_platform_zeroize( &Y2, sizeof( Y2 ) ); - mbedtls_platform_zeroize( &Y3, sizeof( Y3 ) ); - - mbedtls_platform_zeroize( &RK, sizeof( RK ) ); + AES_FROUND( t.Y[0], t.Y[1], t.Y[2], t.Y[3], t.X[0], t.X[1], t.X[2], t.X[3] ); + + t.X[0] = *RK++ ^ \ + ( (uint32_t) FSb[ ( t.Y[0] ) & 0xFF ] ) ^ + ( (uint32_t) FSb[ ( t.Y[1] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) FSb[ ( t.Y[2] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) FSb[ ( t.Y[3] >> 24 ) & 0xFF ] << 24 ); + + t.X[1] = *RK++ ^ \ + ( (uint32_t) FSb[ ( t.Y[1] ) & 0xFF ] ) ^ + ( (uint32_t) FSb[ ( t.Y[2] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) FSb[ ( t.Y[3] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) FSb[ ( t.Y[0] >> 24 ) & 0xFF ] << 24 ); + + t.X[2] = *RK++ ^ \ + ( (uint32_t) FSb[ ( t.Y[2] ) & 0xFF ] ) ^ + ( (uint32_t) FSb[ ( t.Y[3] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) FSb[ ( t.Y[0] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) FSb[ ( t.Y[1] >> 24 ) & 0xFF ] << 24 ); + + t.X[3] = *RK++ ^ \ + ( (uint32_t) FSb[ ( t.Y[3] ) & 0xFF ] ) ^ + ( (uint32_t) FSb[ ( t.Y[0] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) FSb[ ( t.Y[1] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) FSb[ ( t.Y[2] >> 24 ) & 0xFF ] << 24 ); + + PUT_UINT32_LE( t.X[0], output, 0 ); + PUT_UINT32_LE( t.X[1], output, 4 ); + PUT_UINT32_LE( t.X[2], output, 8 ); + PUT_UINT32_LE( t.X[3], output, 12 ); + + mbedtls_platform_zeroize( &t, sizeof( t ) ); return( 0 ); } @@ -977,63 +969,56 @@ int mbedtls_internal_aes_decrypt( mbedtls_aes_context *ctx, unsigned char output[16] ) { int i; - uint32_t *RK, X0, X1, X2, X3, Y0, Y1, Y2, Y3; - - RK = ctx->rk; + uint32_t *RK = ctx->rk; + struct + { + uint32_t X[4]; + uint32_t Y[4]; + } t; - GET_UINT32_LE( X0, input, 0 ); X0 ^= *RK++; - GET_UINT32_LE( X1, input, 4 ); X1 ^= *RK++; - GET_UINT32_LE( X2, input, 8 ); X2 ^= *RK++; - GET_UINT32_LE( X3, input, 12 ); X3 ^= *RK++; + GET_UINT32_LE( t.X[0], input, 0 ); t.X[0] ^= *RK++; + GET_UINT32_LE( t.X[1], input, 4 ); t.X[1] ^= *RK++; + GET_UINT32_LE( t.X[2], input, 8 ); t.X[2] ^= *RK++; + GET_UINT32_LE( t.X[3], input, 12 ); t.X[3] ^= *RK++; for( i = ( ctx->nr >> 1 ) - 1; i > 0; i-- ) { - AES_RROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 ); - AES_RROUND( X0, X1, X2, X3, Y0, Y1, Y2, Y3 ); + AES_RROUND( t.Y[0], t.Y[1], t.Y[2], t.Y[3], t.X[0], t.X[1], t.X[2], t.X[3] ); + AES_RROUND( t.X[0], t.X[1], t.X[2], t.X[3], t.Y[0], t.Y[1], t.Y[2], t.Y[3] ); } - AES_RROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 ); - - X0 = *RK++ ^ \ - ( (uint32_t) RSb[ ( Y0 ) & 0xFF ] ) ^ - ( (uint32_t) RSb[ ( Y3 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) RSb[ ( Y2 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) RSb[ ( Y1 >> 24 ) & 0xFF ] << 24 ); - - X1 = *RK++ ^ \ - ( (uint32_t) RSb[ ( Y1 ) & 0xFF ] ) ^ - ( (uint32_t) RSb[ ( Y0 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) RSb[ ( Y3 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) RSb[ ( Y2 >> 24 ) & 0xFF ] << 24 ); - - X2 = *RK++ ^ \ - ( (uint32_t) RSb[ ( Y2 ) & 0xFF ] ) ^ - ( (uint32_t) RSb[ ( Y1 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) RSb[ ( Y0 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) RSb[ ( Y3 >> 24 ) & 0xFF ] << 24 ); - - X3 = *RK++ ^ \ - ( (uint32_t) RSb[ ( Y3 ) & 0xFF ] ) ^ - ( (uint32_t) RSb[ ( Y2 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) RSb[ ( Y1 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) RSb[ ( Y0 >> 24 ) & 0xFF ] << 24 ); - - PUT_UINT32_LE( X0, output, 0 ); - PUT_UINT32_LE( X1, output, 4 ); - PUT_UINT32_LE( X2, output, 8 ); - PUT_UINT32_LE( X3, output, 12 ); - - mbedtls_platform_zeroize( &X0, sizeof( X0 ) ); - mbedtls_platform_zeroize( &X1, sizeof( X1 ) ); - mbedtls_platform_zeroize( &X2, sizeof( X2 ) ); - mbedtls_platform_zeroize( &X3, sizeof( X3 ) ); - - mbedtls_platform_zeroize( &Y0, sizeof( Y0 ) ); - mbedtls_platform_zeroize( &Y1, sizeof( Y1 ) ); - mbedtls_platform_zeroize( &Y2, sizeof( Y2 ) ); - mbedtls_platform_zeroize( &Y3, sizeof( Y3 ) ); - - mbedtls_platform_zeroize( &RK, sizeof( RK ) ); + AES_RROUND( t.Y[0], t.Y[1], t.Y[2], t.Y[3], t.X[0], t.X[1], t.X[2], t.X[3] ); + + t.X[0] = *RK++ ^ \ + ( (uint32_t) RSb[ ( t.Y[0] ) & 0xFF ] ) ^ + ( (uint32_t) RSb[ ( t.Y[3] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) RSb[ ( t.Y[2] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) RSb[ ( t.Y[1] >> 24 ) & 0xFF ] << 24 ); + + t.X[1] = *RK++ ^ \ + ( (uint32_t) RSb[ ( t.Y[1] ) & 0xFF ] ) ^ + ( (uint32_t) RSb[ ( t.Y[0] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) RSb[ ( t.Y[3] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) RSb[ ( t.Y[2] >> 24 ) & 0xFF ] << 24 ); + + t.X[2] = *RK++ ^ \ + ( (uint32_t) RSb[ ( t.Y[2] ) & 0xFF ] ) ^ + ( (uint32_t) RSb[ ( t.Y[1] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) RSb[ ( t.Y[0] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) RSb[ ( t.Y[3] >> 24 ) & 0xFF ] << 24 ); + + t.X[3] = *RK++ ^ \ + ( (uint32_t) RSb[ ( t.Y[3] ) & 0xFF ] ) ^ + ( (uint32_t) RSb[ ( t.Y[2] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) RSb[ ( t.Y[1] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) RSb[ ( t.Y[0] >> 24 ) & 0xFF ] << 24 ); + + PUT_UINT32_LE( t.X[0], output, 0 ); + PUT_UINT32_LE( t.X[1], output, 4 ); + PUT_UINT32_LE( t.X[2], output, 8 ); + PUT_UINT32_LE( t.X[3], output, 12 ); + + mbedtls_platform_zeroize( &t, sizeof( t ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/bignum.c b/thirdparty/mbedtls/library/bignum.c index dfe976d648..2feb727d89 100644 --- a/thirdparty/mbedtls/library/bignum.c +++ b/thirdparty/mbedtls/library/bignum.c @@ -1364,7 +1364,10 @@ int mbedtls_mpi_sub_abs( mbedtls_mpi *X, const mbedtls_mpi *A, const mbedtls_mpi /* If we ran out of space for the carry, it means that the result * is negative. */ if( n == X->n ) - return( MBEDTLS_ERR_MPI_NEGATIVE_VALUE ); + { + ret = MBEDTLS_ERR_MPI_NEGATIVE_VALUE; + goto cleanup; + } --X->p[n]; } @@ -2044,7 +2047,7 @@ int mbedtls_mpi_exp_mod( mbedtls_mpi *X, const mbedtls_mpi *A, size_t i, j, nblimbs; size_t bufsize, nbits; mbedtls_mpi_uint ei, mm, state; - mbedtls_mpi RR, T, W[ 2 << MBEDTLS_MPI_WINDOW_SIZE ], Apos; + mbedtls_mpi RR, T, W[ 1 << MBEDTLS_MPI_WINDOW_SIZE ], Apos; int neg; MPI_VALIDATE_RET( X != NULL ); @@ -2058,6 +2061,10 @@ int mbedtls_mpi_exp_mod( mbedtls_mpi *X, const mbedtls_mpi *A, if( mbedtls_mpi_cmp_int( E, 0 ) < 0 ) return( MBEDTLS_ERR_MPI_BAD_INPUT_DATA ); + if( mbedtls_mpi_bitlen( E ) > MBEDTLS_MPI_MAX_BITS || + mbedtls_mpi_bitlen( N ) > MBEDTLS_MPI_MAX_BITS ) + return ( MBEDTLS_ERR_MPI_BAD_INPUT_DATA ); + /* * Init temps and window size */ @@ -2334,7 +2341,7 @@ int mbedtls_mpi_fill_random( mbedtls_mpi *X, size_t size, MBEDTLS_MPI_CHK( mbedtls_mpi_lset( X, 0 ) ); Xp = (unsigned char*) X->p; - f_rng( p_rng, Xp + overhead, size ); + MBEDTLS_MPI_CHK( f_rng( p_rng, Xp + overhead, size ) ); mpi_bigendian_to_host( X->p, limbs ); diff --git a/thirdparty/mbedtls/library/cipher_wrap.c b/thirdparty/mbedtls/library/cipher_wrap.c index 1dcac21be1..5973ca6ba2 100644 --- a/thirdparty/mbedtls/library/cipher_wrap.c +++ b/thirdparty/mbedtls/library/cipher_wrap.c @@ -779,7 +779,7 @@ static const mbedtls_cipher_info_t camellia_128_ecb_info = { MBEDTLS_MODE_ECB, 128, "CAMELLIA-128-ECB", - 16, + 0, 0, 16, &camellia_info @@ -790,7 +790,7 @@ static const mbedtls_cipher_info_t camellia_192_ecb_info = { MBEDTLS_MODE_ECB, 192, "CAMELLIA-192-ECB", - 16, + 0, 0, 16, &camellia_info @@ -801,7 +801,7 @@ static const mbedtls_cipher_info_t camellia_256_ecb_info = { MBEDTLS_MODE_ECB, 256, "CAMELLIA-256-ECB", - 16, + 0, 0, 16, &camellia_info @@ -1155,7 +1155,7 @@ static const mbedtls_cipher_info_t aria_128_ecb_info = { MBEDTLS_MODE_ECB, 128, "ARIA-128-ECB", - 16, + 0, 0, 16, &aria_info @@ -1166,7 +1166,7 @@ static const mbedtls_cipher_info_t aria_192_ecb_info = { MBEDTLS_MODE_ECB, 192, "ARIA-192-ECB", - 16, + 0, 0, 16, &aria_info @@ -1177,7 +1177,7 @@ static const mbedtls_cipher_info_t aria_256_ecb_info = { MBEDTLS_MODE_ECB, 256, "ARIA-256-ECB", - 16, + 0, 0, 16, &aria_info @@ -1579,7 +1579,7 @@ static const mbedtls_cipher_info_t des_ecb_info = { MBEDTLS_MODE_ECB, MBEDTLS_KEY_LENGTH_DES, "DES-ECB", - 8, + 0, 0, 8, &des_info @@ -1630,7 +1630,7 @@ static const mbedtls_cipher_info_t des_ede_ecb_info = { MBEDTLS_MODE_ECB, MBEDTLS_KEY_LENGTH_DES_EDE, "DES-EDE-ECB", - 8, + 0, 0, 8, &des_ede_info @@ -1681,7 +1681,7 @@ static const mbedtls_cipher_info_t des_ede3_ecb_info = { MBEDTLS_MODE_ECB, MBEDTLS_KEY_LENGTH_DES_EDE3, "DES-EDE3-ECB", - 8, + 0, 0, 8, &des_ede3_info @@ -1796,7 +1796,7 @@ static const mbedtls_cipher_info_t blowfish_ecb_info = { MBEDTLS_MODE_ECB, 128, "BLOWFISH-ECB", - 8, + 0, MBEDTLS_CIPHER_VARIABLE_KEY_LEN, 8, &blowfish_info diff --git a/thirdparty/mbedtls/library/cmac.c b/thirdparty/mbedtls/library/cmac.c index 1a1200b52b..409f67958e 100644 --- a/thirdparty/mbedtls/library/cmac.c +++ b/thirdparty/mbedtls/library/cmac.c @@ -450,7 +450,7 @@ exit: */ int mbedtls_aes_cmac_prf_128( const unsigned char *key, size_t key_length, const unsigned char *input, size_t in_len, - unsigned char *output ) + unsigned char output[16] ) { int ret; const mbedtls_cipher_info_t *cipher_info; diff --git a/thirdparty/mbedtls/library/ctr_drbg.c b/thirdparty/mbedtls/library/ctr_drbg.c index b98df29a9b..e92008bbe8 100644 --- a/thirdparty/mbedtls/library/ctr_drbg.c +++ b/thirdparty/mbedtls/library/ctr_drbg.c @@ -82,11 +82,17 @@ void mbedtls_ctr_drbg_init( mbedtls_ctr_drbg_context *ctx ) { memset( ctx, 0, sizeof( mbedtls_ctr_drbg_context ) ); + ctx->reseed_interval = MBEDTLS_CTR_DRBG_RESEED_INTERVAL; + #if defined(MBEDTLS_THREADING_C) mbedtls_mutex_init( &ctx->mutex ); #endif } +/* + * This function resets CTR_DRBG context to the state immediately + * after initial call of mbedtls_ctr_drbg_init(). + */ void mbedtls_ctr_drbg_free( mbedtls_ctr_drbg_context *ctx ) { if( ctx == NULL ) @@ -97,6 +103,10 @@ void mbedtls_ctr_drbg_free( mbedtls_ctr_drbg_context *ctx ) #endif mbedtls_aes_free( &ctx->aes_ctx ); mbedtls_platform_zeroize( ctx, sizeof( mbedtls_ctr_drbg_context ) ); + ctx->reseed_interval = MBEDTLS_CTR_DRBG_RESEED_INTERVAL; +#if defined(MBEDTLS_THREADING_C) + mbedtls_mutex_init( &ctx->mutex ); +#endif } void mbedtls_ctr_drbg_set_prediction_resistance( mbedtls_ctr_drbg_context *ctx, int resistance ) @@ -419,7 +429,6 @@ int mbedtls_ctr_drbg_seed( mbedtls_ctr_drbg_context *ctx, if( ctx->entropy_len == 0 ) ctx->entropy_len = MBEDTLS_CTR_DRBG_ENTROPY_LEN; - ctx->reseed_interval = MBEDTLS_CTR_DRBG_RESEED_INTERVAL; /* * Initialize with an empty key diff --git a/thirdparty/mbedtls/library/ecp_curves.c b/thirdparty/mbedtls/library/ecp_curves.c index cc4c5b71c0..b04596b561 100644 --- a/thirdparty/mbedtls/library/ecp_curves.c +++ b/thirdparty/mbedtls/library/ecp_curves.c @@ -1044,7 +1044,7 @@ static inline void sub32( uint32_t *dst, uint32_t src, signed char *carry ) STORE32; i++; \ cur = c > 0 ? c : 0; STORE32; \ cur = 0; while( ++i < MAX32 ) { STORE32; } \ - if( c < 0 ) fix_negative( N, c, &C, bits ); + if( c < 0 ) MBEDTLS_MPI_CHK( fix_negative( N, c, &C, bits ) ); /* * If the result is negative, we get it in the form diff --git a/thirdparty/mbedtls/library/entropy_poll.c b/thirdparty/mbedtls/library/entropy_poll.c index 26b7e4e2b9..2095a7dd34 100644 --- a/thirdparty/mbedtls/library/entropy_poll.c +++ b/thirdparty/mbedtls/library/entropy_poll.c @@ -44,7 +44,7 @@ * ********** */ -#if defined(__linux__) +#if defined(__linux__) && !defined(_GNU_SOURCE) /* Ensure that syscall() is available even when compiling with -std=c99 */ #define _GNU_SOURCE #endif diff --git a/thirdparty/mbedtls/library/error.c b/thirdparty/mbedtls/library/error.c index eb52052b51..b83b8d1f1b 100644 --- a/thirdparty/mbedtls/library/error.c +++ b/thirdparty/mbedtls/library/error.c @@ -51,20 +51,19 @@ #endif #if defined(MBEDTLS_ERROR_C) || defined(MBEDTLS_ERROR_STRERROR_DUMMY) + #include "mbedtls/error.h" -#include <string.h> -#endif + +#if defined(MBEDTLS_ERROR_C) #if defined(MBEDTLS_PLATFORM_C) #include "mbedtls/platform.h" #else #define mbedtls_snprintf snprintf -#define mbedtls_time_t time_t #endif -#if defined(MBEDTLS_ERROR_C) - #include <stdio.h> +#include <string.h> #if defined(MBEDTLS_AES_C) #include "mbedtls/aes.h" @@ -929,8 +928,6 @@ void mbedtls_strerror( int ret, char *buf, size_t buflen ) #else /* MBEDTLS_ERROR_C */ -#if defined(MBEDTLS_ERROR_STRERROR_DUMMY) - /* * Provide an non-function in case MBEDTLS_ERROR_C is not defined */ @@ -942,6 +939,6 @@ void mbedtls_strerror( int ret, char *buf, size_t buflen ) buf[0] = '\0'; } -#endif /* MBEDTLS_ERROR_STRERROR_DUMMY */ - #endif /* MBEDTLS_ERROR_C */ + +#endif /* MBEDTLS_ERROR_C || MBEDTLS_ERROR_STRERROR_DUMMY */ diff --git a/thirdparty/mbedtls/library/hmac_drbg.c b/thirdparty/mbedtls/library/hmac_drbg.c index 9fbfc30660..10cbd462ba 100644 --- a/thirdparty/mbedtls/library/hmac_drbg.c +++ b/thirdparty/mbedtls/library/hmac_drbg.c @@ -83,6 +83,8 @@ void mbedtls_hmac_drbg_init( mbedtls_hmac_drbg_context *ctx ) { memset( ctx, 0, sizeof( mbedtls_hmac_drbg_context ) ); + ctx->reseed_interval = MBEDTLS_HMAC_DRBG_RESEED_INTERVAL; + #if defined(MBEDTLS_THREADING_C) mbedtls_mutex_init( &ctx->mutex ); #endif @@ -296,8 +298,6 @@ int mbedtls_hmac_drbg_seed( mbedtls_hmac_drbg_context *ctx, ctx->f_entropy = f_entropy; ctx->p_entropy = p_entropy; - ctx->reseed_interval = MBEDTLS_HMAC_DRBG_RESEED_INTERVAL; - if( ctx->entropy_len == 0 ) { /* @@ -442,7 +442,8 @@ int mbedtls_hmac_drbg_random( void *p_rng, unsigned char *output, size_t out_len } /* - * Free an HMAC_DRBG context + * This function resets HMAC_DRBG context to the state immediately + * after initial call of mbedtls_hmac_drbg_init(). */ void mbedtls_hmac_drbg_free( mbedtls_hmac_drbg_context *ctx ) { @@ -454,6 +455,10 @@ void mbedtls_hmac_drbg_free( mbedtls_hmac_drbg_context *ctx ) #endif mbedtls_md_free( &ctx->md_ctx ); mbedtls_platform_zeroize( ctx, sizeof( mbedtls_hmac_drbg_context ) ); + ctx->reseed_interval = MBEDTLS_HMAC_DRBG_RESEED_INTERVAL; +#if defined(MBEDTLS_THREADING_C) + mbedtls_mutex_init( &ctx->mutex ); +#endif } #if defined(MBEDTLS_FS_IO) diff --git a/thirdparty/mbedtls/library/md2.c b/thirdparty/mbedtls/library/md2.c index cbdaaabdc7..fdcb630a1f 100644 --- a/thirdparty/mbedtls/library/md2.c +++ b/thirdparty/mbedtls/library/md2.c @@ -177,6 +177,9 @@ int mbedtls_internal_md2_process( mbedtls_md2_context *ctx ) t = ctx->cksum[i]; } + /* Zeroise variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &t, sizeof( t ) ); + return( 0 ); } diff --git a/thirdparty/mbedtls/library/md4.c b/thirdparty/mbedtls/library/md4.c index cb16dce54a..95e893e654 100644 --- a/thirdparty/mbedtls/library/md4.c +++ b/thirdparty/mbedtls/library/md4.c @@ -143,31 +143,34 @@ void mbedtls_md4_starts( mbedtls_md4_context *ctx ) int mbedtls_internal_md4_process( mbedtls_md4_context *ctx, const unsigned char data[64] ) { - uint32_t X[16], A, B, C, D; - - GET_UINT32_LE( X[ 0], data, 0 ); - GET_UINT32_LE( X[ 1], data, 4 ); - GET_UINT32_LE( X[ 2], data, 8 ); - GET_UINT32_LE( X[ 3], data, 12 ); - GET_UINT32_LE( X[ 4], data, 16 ); - GET_UINT32_LE( X[ 5], data, 20 ); - GET_UINT32_LE( X[ 6], data, 24 ); - GET_UINT32_LE( X[ 7], data, 28 ); - GET_UINT32_LE( X[ 8], data, 32 ); - GET_UINT32_LE( X[ 9], data, 36 ); - GET_UINT32_LE( X[10], data, 40 ); - GET_UINT32_LE( X[11], data, 44 ); - GET_UINT32_LE( X[12], data, 48 ); - GET_UINT32_LE( X[13], data, 52 ); - GET_UINT32_LE( X[14], data, 56 ); - GET_UINT32_LE( X[15], data, 60 ); + struct + { + uint32_t X[16], A, B, C, D; + } local; + + GET_UINT32_LE( local.X[ 0], data, 0 ); + GET_UINT32_LE( local.X[ 1], data, 4 ); + GET_UINT32_LE( local.X[ 2], data, 8 ); + GET_UINT32_LE( local.X[ 3], data, 12 ); + GET_UINT32_LE( local.X[ 4], data, 16 ); + GET_UINT32_LE( local.X[ 5], data, 20 ); + GET_UINT32_LE( local.X[ 6], data, 24 ); + GET_UINT32_LE( local.X[ 7], data, 28 ); + GET_UINT32_LE( local.X[ 8], data, 32 ); + GET_UINT32_LE( local.X[ 9], data, 36 ); + GET_UINT32_LE( local.X[10], data, 40 ); + GET_UINT32_LE( local.X[11], data, 44 ); + GET_UINT32_LE( local.X[12], data, 48 ); + GET_UINT32_LE( local.X[13], data, 52 ); + GET_UINT32_LE( local.X[14], data, 56 ); + GET_UINT32_LE( local.X[15], data, 60 ); #define S(x,n) (((x) << (n)) | (((x) & 0xFFFFFFFF) >> (32 - (n)))) - A = ctx->state[0]; - B = ctx->state[1]; - C = ctx->state[2]; - D = ctx->state[3]; + local.A = ctx->state[0]; + local.B = ctx->state[1]; + local.C = ctx->state[2]; + local.D = ctx->state[3]; #define F(x, y, z) (((x) & (y)) | ((~(x)) & (z))) #define P(a,b,c,d,x,s) \ @@ -178,22 +181,22 @@ int mbedtls_internal_md4_process( mbedtls_md4_context *ctx, } while( 0 ) - P( A, B, C, D, X[ 0], 3 ); - P( D, A, B, C, X[ 1], 7 ); - P( C, D, A, B, X[ 2], 11 ); - P( B, C, D, A, X[ 3], 19 ); - P( A, B, C, D, X[ 4], 3 ); - P( D, A, B, C, X[ 5], 7 ); - P( C, D, A, B, X[ 6], 11 ); - P( B, C, D, A, X[ 7], 19 ); - P( A, B, C, D, X[ 8], 3 ); - P( D, A, B, C, X[ 9], 7 ); - P( C, D, A, B, X[10], 11 ); - P( B, C, D, A, X[11], 19 ); - P( A, B, C, D, X[12], 3 ); - P( D, A, B, C, X[13], 7 ); - P( C, D, A, B, X[14], 11 ); - P( B, C, D, A, X[15], 19 ); + P( local.A, local.B, local.C, local.D, local.X[ 0], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 1], 7 ); + P( local.C, local.D, local.A, local.B, local.X[ 2], 11 ); + P( local.B, local.C, local.D, local.A, local.X[ 3], 19 ); + P( local.A, local.B, local.C, local.D, local.X[ 4], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 5], 7 ); + P( local.C, local.D, local.A, local.B, local.X[ 6], 11 ); + P( local.B, local.C, local.D, local.A, local.X[ 7], 19 ); + P( local.A, local.B, local.C, local.D, local.X[ 8], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 9], 7 ); + P( local.C, local.D, local.A, local.B, local.X[10], 11 ); + P( local.B, local.C, local.D, local.A, local.X[11], 19 ); + P( local.A, local.B, local.C, local.D, local.X[12], 3 ); + P( local.D, local.A, local.B, local.C, local.X[13], 7 ); + P( local.C, local.D, local.A, local.B, local.X[14], 11 ); + P( local.B, local.C, local.D, local.A, local.X[15], 19 ); #undef P #undef F @@ -206,22 +209,22 @@ int mbedtls_internal_md4_process( mbedtls_md4_context *ctx, (a) = S((a),(s)); \ } while( 0 ) - P( A, B, C, D, X[ 0], 3 ); - P( D, A, B, C, X[ 4], 5 ); - P( C, D, A, B, X[ 8], 9 ); - P( B, C, D, A, X[12], 13 ); - P( A, B, C, D, X[ 1], 3 ); - P( D, A, B, C, X[ 5], 5 ); - P( C, D, A, B, X[ 9], 9 ); - P( B, C, D, A, X[13], 13 ); - P( A, B, C, D, X[ 2], 3 ); - P( D, A, B, C, X[ 6], 5 ); - P( C, D, A, B, X[10], 9 ); - P( B, C, D, A, X[14], 13 ); - P( A, B, C, D, X[ 3], 3 ); - P( D, A, B, C, X[ 7], 5 ); - P( C, D, A, B, X[11], 9 ); - P( B, C, D, A, X[15], 13 ); + P( local.A, local.B, local.C, local.D, local.X[ 0], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 4], 5 ); + P( local.C, local.D, local.A, local.B, local.X[ 8], 9 ); + P( local.B, local.C, local.D, local.A, local.X[12], 13 ); + P( local.A, local.B, local.C, local.D, local.X[ 1], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 5], 5 ); + P( local.C, local.D, local.A, local.B, local.X[ 9], 9 ); + P( local.B, local.C, local.D, local.A, local.X[13], 13 ); + P( local.A, local.B, local.C, local.D, local.X[ 2], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 6], 5 ); + P( local.C, local.D, local.A, local.B, local.X[10], 9 ); + P( local.B, local.C, local.D, local.A, local.X[14], 13 ); + P( local.A, local.B, local.C, local.D, local.X[ 3], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 7], 5 ); + P( local.C, local.D, local.A, local.B, local.X[11], 9 ); + P( local.B, local.C, local.D, local.A, local.X[15], 13 ); #undef P #undef F @@ -234,30 +237,33 @@ int mbedtls_internal_md4_process( mbedtls_md4_context *ctx, (a) = S((a),(s)); \ } while( 0 ) - P( A, B, C, D, X[ 0], 3 ); - P( D, A, B, C, X[ 8], 9 ); - P( C, D, A, B, X[ 4], 11 ); - P( B, C, D, A, X[12], 15 ); - P( A, B, C, D, X[ 2], 3 ); - P( D, A, B, C, X[10], 9 ); - P( C, D, A, B, X[ 6], 11 ); - P( B, C, D, A, X[14], 15 ); - P( A, B, C, D, X[ 1], 3 ); - P( D, A, B, C, X[ 9], 9 ); - P( C, D, A, B, X[ 5], 11 ); - P( B, C, D, A, X[13], 15 ); - P( A, B, C, D, X[ 3], 3 ); - P( D, A, B, C, X[11], 9 ); - P( C, D, A, B, X[ 7], 11 ); - P( B, C, D, A, X[15], 15 ); + P( local.A, local.B, local.C, local.D, local.X[ 0], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 8], 9 ); + P( local.C, local.D, local.A, local.B, local.X[ 4], 11 ); + P( local.B, local.C, local.D, local.A, local.X[12], 15 ); + P( local.A, local.B, local.C, local.D, local.X[ 2], 3 ); + P( local.D, local.A, local.B, local.C, local.X[10], 9 ); + P( local.C, local.D, local.A, local.B, local.X[ 6], 11 ); + P( local.B, local.C, local.D, local.A, local.X[14], 15 ); + P( local.A, local.B, local.C, local.D, local.X[ 1], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 9], 9 ); + P( local.C, local.D, local.A, local.B, local.X[ 5], 11 ); + P( local.B, local.C, local.D, local.A, local.X[13], 15 ); + P( local.A, local.B, local.C, local.D, local.X[ 3], 3 ); + P( local.D, local.A, local.B, local.C, local.X[11], 9 ); + P( local.C, local.D, local.A, local.B, local.X[ 7], 11 ); + P( local.B, local.C, local.D, local.A, local.X[15], 15 ); #undef F #undef P - ctx->state[0] += A; - ctx->state[1] += B; - ctx->state[2] += C; - ctx->state[3] += D; + ctx->state[0] += local.A; + ctx->state[1] += local.B; + ctx->state[2] += local.C; + ctx->state[3] += local.D; + + /* Zeroise variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/md5.c b/thirdparty/mbedtls/library/md5.c index fe25925214..d2b634fbb1 100644 --- a/thirdparty/mbedtls/library/md5.c +++ b/thirdparty/mbedtls/library/md5.c @@ -142,128 +142,134 @@ void mbedtls_md5_starts( mbedtls_md5_context *ctx ) int mbedtls_internal_md5_process( mbedtls_md5_context *ctx, const unsigned char data[64] ) { - uint32_t X[16], A, B, C, D; - - GET_UINT32_LE( X[ 0], data, 0 ); - GET_UINT32_LE( X[ 1], data, 4 ); - GET_UINT32_LE( X[ 2], data, 8 ); - GET_UINT32_LE( X[ 3], data, 12 ); - GET_UINT32_LE( X[ 4], data, 16 ); - GET_UINT32_LE( X[ 5], data, 20 ); - GET_UINT32_LE( X[ 6], data, 24 ); - GET_UINT32_LE( X[ 7], data, 28 ); - GET_UINT32_LE( X[ 8], data, 32 ); - GET_UINT32_LE( X[ 9], data, 36 ); - GET_UINT32_LE( X[10], data, 40 ); - GET_UINT32_LE( X[11], data, 44 ); - GET_UINT32_LE( X[12], data, 48 ); - GET_UINT32_LE( X[13], data, 52 ); - GET_UINT32_LE( X[14], data, 56 ); - GET_UINT32_LE( X[15], data, 60 ); + struct + { + uint32_t X[16], A, B, C, D; + } local; + + GET_UINT32_LE( local.X[ 0], data, 0 ); + GET_UINT32_LE( local.X[ 1], data, 4 ); + GET_UINT32_LE( local.X[ 2], data, 8 ); + GET_UINT32_LE( local.X[ 3], data, 12 ); + GET_UINT32_LE( local.X[ 4], data, 16 ); + GET_UINT32_LE( local.X[ 5], data, 20 ); + GET_UINT32_LE( local.X[ 6], data, 24 ); + GET_UINT32_LE( local.X[ 7], data, 28 ); + GET_UINT32_LE( local.X[ 8], data, 32 ); + GET_UINT32_LE( local.X[ 9], data, 36 ); + GET_UINT32_LE( local.X[10], data, 40 ); + GET_UINT32_LE( local.X[11], data, 44 ); + GET_UINT32_LE( local.X[12], data, 48 ); + GET_UINT32_LE( local.X[13], data, 52 ); + GET_UINT32_LE( local.X[14], data, 56 ); + GET_UINT32_LE( local.X[15], data, 60 ); #define S(x,n) \ ( ( (x) << (n) ) | ( ( (x) & 0xFFFFFFFF) >> ( 32 - (n) ) ) ) -#define P(a,b,c,d,k,s,t) \ - do \ - { \ - (a) += F((b),(c),(d)) + X[(k)] + (t); \ - (a) = S((a),(s)) + (b); \ +#define P(a,b,c,d,k,s,t) \ + do \ + { \ + (a) += F((b),(c),(d)) + local.X[(k)] + (t); \ + (a) = S((a),(s)) + (b); \ } while( 0 ) - A = ctx->state[0]; - B = ctx->state[1]; - C = ctx->state[2]; - D = ctx->state[3]; + local.A = ctx->state[0]; + local.B = ctx->state[1]; + local.C = ctx->state[2]; + local.D = ctx->state[3]; #define F(x,y,z) ((z) ^ ((x) & ((y) ^ (z)))) - P( A, B, C, D, 0, 7, 0xD76AA478 ); - P( D, A, B, C, 1, 12, 0xE8C7B756 ); - P( C, D, A, B, 2, 17, 0x242070DB ); - P( B, C, D, A, 3, 22, 0xC1BDCEEE ); - P( A, B, C, D, 4, 7, 0xF57C0FAF ); - P( D, A, B, C, 5, 12, 0x4787C62A ); - P( C, D, A, B, 6, 17, 0xA8304613 ); - P( B, C, D, A, 7, 22, 0xFD469501 ); - P( A, B, C, D, 8, 7, 0x698098D8 ); - P( D, A, B, C, 9, 12, 0x8B44F7AF ); - P( C, D, A, B, 10, 17, 0xFFFF5BB1 ); - P( B, C, D, A, 11, 22, 0x895CD7BE ); - P( A, B, C, D, 12, 7, 0x6B901122 ); - P( D, A, B, C, 13, 12, 0xFD987193 ); - P( C, D, A, B, 14, 17, 0xA679438E ); - P( B, C, D, A, 15, 22, 0x49B40821 ); + P( local.A, local.B, local.C, local.D, 0, 7, 0xD76AA478 ); + P( local.D, local.A, local.B, local.C, 1, 12, 0xE8C7B756 ); + P( local.C, local.D, local.A, local.B, 2, 17, 0x242070DB ); + P( local.B, local.C, local.D, local.A, 3, 22, 0xC1BDCEEE ); + P( local.A, local.B, local.C, local.D, 4, 7, 0xF57C0FAF ); + P( local.D, local.A, local.B, local.C, 5, 12, 0x4787C62A ); + P( local.C, local.D, local.A, local.B, 6, 17, 0xA8304613 ); + P( local.B, local.C, local.D, local.A, 7, 22, 0xFD469501 ); + P( local.A, local.B, local.C, local.D, 8, 7, 0x698098D8 ); + P( local.D, local.A, local.B, local.C, 9, 12, 0x8B44F7AF ); + P( local.C, local.D, local.A, local.B, 10, 17, 0xFFFF5BB1 ); + P( local.B, local.C, local.D, local.A, 11, 22, 0x895CD7BE ); + P( local.A, local.B, local.C, local.D, 12, 7, 0x6B901122 ); + P( local.D, local.A, local.B, local.C, 13, 12, 0xFD987193 ); + P( local.C, local.D, local.A, local.B, 14, 17, 0xA679438E ); + P( local.B, local.C, local.D, local.A, 15, 22, 0x49B40821 ); #undef F #define F(x,y,z) ((y) ^ ((z) & ((x) ^ (y)))) - P( A, B, C, D, 1, 5, 0xF61E2562 ); - P( D, A, B, C, 6, 9, 0xC040B340 ); - P( C, D, A, B, 11, 14, 0x265E5A51 ); - P( B, C, D, A, 0, 20, 0xE9B6C7AA ); - P( A, B, C, D, 5, 5, 0xD62F105D ); - P( D, A, B, C, 10, 9, 0x02441453 ); - P( C, D, A, B, 15, 14, 0xD8A1E681 ); - P( B, C, D, A, 4, 20, 0xE7D3FBC8 ); - P( A, B, C, D, 9, 5, 0x21E1CDE6 ); - P( D, A, B, C, 14, 9, 0xC33707D6 ); - P( C, D, A, B, 3, 14, 0xF4D50D87 ); - P( B, C, D, A, 8, 20, 0x455A14ED ); - P( A, B, C, D, 13, 5, 0xA9E3E905 ); - P( D, A, B, C, 2, 9, 0xFCEFA3F8 ); - P( C, D, A, B, 7, 14, 0x676F02D9 ); - P( B, C, D, A, 12, 20, 0x8D2A4C8A ); + P( local.A, local.B, local.C, local.D, 1, 5, 0xF61E2562 ); + P( local.D, local.A, local.B, local.C, 6, 9, 0xC040B340 ); + P( local.C, local.D, local.A, local.B, 11, 14, 0x265E5A51 ); + P( local.B, local.C, local.D, local.A, 0, 20, 0xE9B6C7AA ); + P( local.A, local.B, local.C, local.D, 5, 5, 0xD62F105D ); + P( local.D, local.A, local.B, local.C, 10, 9, 0x02441453 ); + P( local.C, local.D, local.A, local.B, 15, 14, 0xD8A1E681 ); + P( local.B, local.C, local.D, local.A, 4, 20, 0xE7D3FBC8 ); + P( local.A, local.B, local.C, local.D, 9, 5, 0x21E1CDE6 ); + P( local.D, local.A, local.B, local.C, 14, 9, 0xC33707D6 ); + P( local.C, local.D, local.A, local.B, 3, 14, 0xF4D50D87 ); + P( local.B, local.C, local.D, local.A, 8, 20, 0x455A14ED ); + P( local.A, local.B, local.C, local.D, 13, 5, 0xA9E3E905 ); + P( local.D, local.A, local.B, local.C, 2, 9, 0xFCEFA3F8 ); + P( local.C, local.D, local.A, local.B, 7, 14, 0x676F02D9 ); + P( local.B, local.C, local.D, local.A, 12, 20, 0x8D2A4C8A ); #undef F #define F(x,y,z) ((x) ^ (y) ^ (z)) - P( A, B, C, D, 5, 4, 0xFFFA3942 ); - P( D, A, B, C, 8, 11, 0x8771F681 ); - P( C, D, A, B, 11, 16, 0x6D9D6122 ); - P( B, C, D, A, 14, 23, 0xFDE5380C ); - P( A, B, C, D, 1, 4, 0xA4BEEA44 ); - P( D, A, B, C, 4, 11, 0x4BDECFA9 ); - P( C, D, A, B, 7, 16, 0xF6BB4B60 ); - P( B, C, D, A, 10, 23, 0xBEBFBC70 ); - P( A, B, C, D, 13, 4, 0x289B7EC6 ); - P( D, A, B, C, 0, 11, 0xEAA127FA ); - P( C, D, A, B, 3, 16, 0xD4EF3085 ); - P( B, C, D, A, 6, 23, 0x04881D05 ); - P( A, B, C, D, 9, 4, 0xD9D4D039 ); - P( D, A, B, C, 12, 11, 0xE6DB99E5 ); - P( C, D, A, B, 15, 16, 0x1FA27CF8 ); - P( B, C, D, A, 2, 23, 0xC4AC5665 ); + P( local.A, local.B, local.C, local.D, 5, 4, 0xFFFA3942 ); + P( local.D, local.A, local.B, local.C, 8, 11, 0x8771F681 ); + P( local.C, local.D, local.A, local.B, 11, 16, 0x6D9D6122 ); + P( local.B, local.C, local.D, local.A, 14, 23, 0xFDE5380C ); + P( local.A, local.B, local.C, local.D, 1, 4, 0xA4BEEA44 ); + P( local.D, local.A, local.B, local.C, 4, 11, 0x4BDECFA9 ); + P( local.C, local.D, local.A, local.B, 7, 16, 0xF6BB4B60 ); + P( local.B, local.C, local.D, local.A, 10, 23, 0xBEBFBC70 ); + P( local.A, local.B, local.C, local.D, 13, 4, 0x289B7EC6 ); + P( local.D, local.A, local.B, local.C, 0, 11, 0xEAA127FA ); + P( local.C, local.D, local.A, local.B, 3, 16, 0xD4EF3085 ); + P( local.B, local.C, local.D, local.A, 6, 23, 0x04881D05 ); + P( local.A, local.B, local.C, local.D, 9, 4, 0xD9D4D039 ); + P( local.D, local.A, local.B, local.C, 12, 11, 0xE6DB99E5 ); + P( local.C, local.D, local.A, local.B, 15, 16, 0x1FA27CF8 ); + P( local.B, local.C, local.D, local.A, 2, 23, 0xC4AC5665 ); #undef F #define F(x,y,z) ((y) ^ ((x) | ~(z))) - P( A, B, C, D, 0, 6, 0xF4292244 ); - P( D, A, B, C, 7, 10, 0x432AFF97 ); - P( C, D, A, B, 14, 15, 0xAB9423A7 ); - P( B, C, D, A, 5, 21, 0xFC93A039 ); - P( A, B, C, D, 12, 6, 0x655B59C3 ); - P( D, A, B, C, 3, 10, 0x8F0CCC92 ); - P( C, D, A, B, 10, 15, 0xFFEFF47D ); - P( B, C, D, A, 1, 21, 0x85845DD1 ); - P( A, B, C, D, 8, 6, 0x6FA87E4F ); - P( D, A, B, C, 15, 10, 0xFE2CE6E0 ); - P( C, D, A, B, 6, 15, 0xA3014314 ); - P( B, C, D, A, 13, 21, 0x4E0811A1 ); - P( A, B, C, D, 4, 6, 0xF7537E82 ); - P( D, A, B, C, 11, 10, 0xBD3AF235 ); - P( C, D, A, B, 2, 15, 0x2AD7D2BB ); - P( B, C, D, A, 9, 21, 0xEB86D391 ); + P( local.A, local.B, local.C, local.D, 0, 6, 0xF4292244 ); + P( local.D, local.A, local.B, local.C, 7, 10, 0x432AFF97 ); + P( local.C, local.D, local.A, local.B, 14, 15, 0xAB9423A7 ); + P( local.B, local.C, local.D, local.A, 5, 21, 0xFC93A039 ); + P( local.A, local.B, local.C, local.D, 12, 6, 0x655B59C3 ); + P( local.D, local.A, local.B, local.C, 3, 10, 0x8F0CCC92 ); + P( local.C, local.D, local.A, local.B, 10, 15, 0xFFEFF47D ); + P( local.B, local.C, local.D, local.A, 1, 21, 0x85845DD1 ); + P( local.A, local.B, local.C, local.D, 8, 6, 0x6FA87E4F ); + P( local.D, local.A, local.B, local.C, 15, 10, 0xFE2CE6E0 ); + P( local.C, local.D, local.A, local.B, 6, 15, 0xA3014314 ); + P( local.B, local.C, local.D, local.A, 13, 21, 0x4E0811A1 ); + P( local.A, local.B, local.C, local.D, 4, 6, 0xF7537E82 ); + P( local.D, local.A, local.B, local.C, 11, 10, 0xBD3AF235 ); + P( local.C, local.D, local.A, local.B, 2, 15, 0x2AD7D2BB ); + P( local.B, local.C, local.D, local.A, 9, 21, 0xEB86D391 ); #undef F - ctx->state[0] += A; - ctx->state[1] += B; - ctx->state[2] += C; - ctx->state[3] += D; + ctx->state[0] += local.A; + ctx->state[1] += local.B; + ctx->state[2] += local.C; + ctx->state[3] += local.D; + + /* Zeroise variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/pem.c b/thirdparty/mbedtls/library/pem.c index a7a2f7f5cf..50e663ccdb 100644 --- a/thirdparty/mbedtls/library/pem.c +++ b/thirdparty/mbedtls/library/pem.c @@ -508,8 +508,12 @@ int mbedtls_pem_write_buffer( const char *header, const char *footer, *p++ = '\0'; *olen = p - buf; + /* Clean any remaining data previously written to the buffer */ + memset( buf + *olen, 0, buf_len - *olen ); + mbedtls_free( encode_buf ); return( 0 ); } #endif /* MBEDTLS_PEM_WRITE_C */ #endif /* MBEDTLS_PEM_PARSE_C || MBEDTLS_PEM_WRITE_C */ + diff --git a/thirdparty/mbedtls/library/pkcs5.c b/thirdparty/mbedtls/library/pkcs5.c index 8a80aa5d05..c4447f1546 100644 --- a/thirdparty/mbedtls/library/pkcs5.c +++ b/thirdparty/mbedtls/library/pkcs5.c @@ -247,7 +247,7 @@ int mbedtls_pkcs5_pbkdf2_hmac( mbedtls_md_context_t *ctx, const unsigned char *p unsigned int iteration_count, uint32_t key_length, unsigned char *output ) { - int ret, j; + int ret = 0, j; unsigned int i; unsigned char md1[MBEDTLS_MD_MAX_SIZE]; unsigned char work[MBEDTLS_MD_MAX_SIZE]; @@ -269,16 +269,16 @@ int mbedtls_pkcs5_pbkdf2_hmac( mbedtls_md_context_t *ctx, const unsigned char *p // U1 ends up in work // if( ( ret = mbedtls_md_hmac_starts( ctx, password, plen ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_update( ctx, salt, slen ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_update( ctx, counter, 4 ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_finish( ctx, work ) ) != 0 ) - return( ret ); + goto cleanup; memcpy( md1, work, md_size ); @@ -287,13 +287,13 @@ int mbedtls_pkcs5_pbkdf2_hmac( mbedtls_md_context_t *ctx, const unsigned char *p // U2 ends up in md1 // if( ( ret = mbedtls_md_hmac_starts( ctx, password, plen ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_update( ctx, md1, md_size ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_finish( ctx, md1 ) ) != 0 ) - return( ret ); + goto cleanup; // U1 xor U2 // @@ -312,7 +312,12 @@ int mbedtls_pkcs5_pbkdf2_hmac( mbedtls_md_context_t *ctx, const unsigned char *p break; } - return( 0 ); +cleanup: + /* Zeroise buffers to clear sensitive data from memory. */ + mbedtls_platform_zeroize( work, MBEDTLS_MD_MAX_SIZE ); + mbedtls_platform_zeroize( md1, MBEDTLS_MD_MAX_SIZE ); + + return( ret ); } #if defined(MBEDTLS_SELF_TEST) diff --git a/thirdparty/mbedtls/library/pkparse.c b/thirdparty/mbedtls/library/pkparse.c index 086807d836..e410f3aae1 100644 --- a/thirdparty/mbedtls/library/pkparse.c +++ b/thirdparty/mbedtls/library/pkparse.c @@ -692,7 +692,7 @@ int mbedtls_pk_parse_subpubkey( unsigned char **p, const unsigned char *end, ret = MBEDTLS_ERR_PK_UNKNOWN_PK_ALG; if( ret == 0 && *p != end ) - ret = MBEDTLS_ERR_PK_INVALID_PUBKEY + ret = MBEDTLS_ERR_PK_INVALID_PUBKEY + MBEDTLS_ERR_ASN1_LENGTH_MISMATCH; if( ret != 0 ) diff --git a/thirdparty/mbedtls/library/platform_util.c b/thirdparty/mbedtls/library/platform_util.c index 3ba2aead12..c8cd52d52a 100644 --- a/thirdparty/mbedtls/library/platform_util.c +++ b/thirdparty/mbedtls/library/platform_util.c @@ -115,7 +115,7 @@ void mbedtls_platform_zeroize( void *buf, size_t len ) #if !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) || \ ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) && \ - _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) + _POSIX_THREAD_SAFE_FUNCTIONS >= 200112L ) ) /* * This is a convenience shorthand macro to avoid checking the long * preprocessor conditions above. Ideally, we could expose this macro in @@ -129,7 +129,7 @@ void mbedtls_platform_zeroize( void *buf, size_t len ) #endif /* !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) || \ ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) && \ - _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) */ + _POSIX_THREAD_SAFE_FUNCTIONS >= 200112L ) ) */ struct tm *mbedtls_platform_gmtime_r( const mbedtls_time_t *tt, struct tm *tm_buf ) diff --git a/thirdparty/mbedtls/library/ripemd160.c b/thirdparty/mbedtls/library/ripemd160.c index 0b6efcb574..d6ee933b2e 100644 --- a/thirdparty/mbedtls/library/ripemd160.c +++ b/thirdparty/mbedtls/library/ripemd160.c @@ -147,30 +147,33 @@ void mbedtls_ripemd160_starts( mbedtls_ripemd160_context *ctx ) int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, const unsigned char data[64] ) { - uint32_t A, B, C, D, E, Ap, Bp, Cp, Dp, Ep, X[16]; - - GET_UINT32_LE( X[ 0], data, 0 ); - GET_UINT32_LE( X[ 1], data, 4 ); - GET_UINT32_LE( X[ 2], data, 8 ); - GET_UINT32_LE( X[ 3], data, 12 ); - GET_UINT32_LE( X[ 4], data, 16 ); - GET_UINT32_LE( X[ 5], data, 20 ); - GET_UINT32_LE( X[ 6], data, 24 ); - GET_UINT32_LE( X[ 7], data, 28 ); - GET_UINT32_LE( X[ 8], data, 32 ); - GET_UINT32_LE( X[ 9], data, 36 ); - GET_UINT32_LE( X[10], data, 40 ); - GET_UINT32_LE( X[11], data, 44 ); - GET_UINT32_LE( X[12], data, 48 ); - GET_UINT32_LE( X[13], data, 52 ); - GET_UINT32_LE( X[14], data, 56 ); - GET_UINT32_LE( X[15], data, 60 ); - - A = Ap = ctx->state[0]; - B = Bp = ctx->state[1]; - C = Cp = ctx->state[2]; - D = Dp = ctx->state[3]; - E = Ep = ctx->state[4]; + struct + { + uint32_t A, B, C, D, E, Ap, Bp, Cp, Dp, Ep, X[16]; + } local; + + GET_UINT32_LE( local.X[ 0], data, 0 ); + GET_UINT32_LE( local.X[ 1], data, 4 ); + GET_UINT32_LE( local.X[ 2], data, 8 ); + GET_UINT32_LE( local.X[ 3], data, 12 ); + GET_UINT32_LE( local.X[ 4], data, 16 ); + GET_UINT32_LE( local.X[ 5], data, 20 ); + GET_UINT32_LE( local.X[ 6], data, 24 ); + GET_UINT32_LE( local.X[ 7], data, 28 ); + GET_UINT32_LE( local.X[ 8], data, 32 ); + GET_UINT32_LE( local.X[ 9], data, 36 ); + GET_UINT32_LE( local.X[10], data, 40 ); + GET_UINT32_LE( local.X[11], data, 44 ); + GET_UINT32_LE( local.X[12], data, 48 ); + GET_UINT32_LE( local.X[13], data, 52 ); + GET_UINT32_LE( local.X[14], data, 56 ); + GET_UINT32_LE( local.X[15], data, 60 ); + + local.A = local.Ap = ctx->state[0]; + local.B = local.Bp = ctx->state[1]; + local.C = local.Cp = ctx->state[2]; + local.D = local.Dp = ctx->state[3]; + local.E = local.Ep = ctx->state[4]; #define F1( x, y, z ) ( (x) ^ (y) ^ (z) ) #define F2( x, y, z ) ( ( (x) & (y) ) | ( ~(x) & (z) ) ) @@ -180,12 +183,12 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define S( x, n ) ( ( (x) << (n) ) | ( (x) >> (32 - (n)) ) ) -#define P( a, b, c, d, e, r, s, f, k ) \ - do \ - { \ - (a) += f( (b), (c), (d) ) + X[r] + (k); \ - (a) = S( (a), (s) ) + (e); \ - (c) = S( (c), 10 ); \ +#define P( a, b, c, d, e, r, s, f, k ) \ + do \ + { \ + (a) += f( (b), (c), (d) ) + local.X[r] + (k); \ + (a) = S( (a), (s) ) + (e); \ + (c) = S( (c), 10 ); \ } while( 0 ) #define P2( a, b, c, d, e, r, s, rp, sp ) \ @@ -200,22 +203,22 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0x00000000 #define Fp F5 #define Kp 0x50A28BE6 - P2( A, B, C, D, E, 0, 11, 5, 8 ); - P2( E, A, B, C, D, 1, 14, 14, 9 ); - P2( D, E, A, B, C, 2, 15, 7, 9 ); - P2( C, D, E, A, B, 3, 12, 0, 11 ); - P2( B, C, D, E, A, 4, 5, 9, 13 ); - P2( A, B, C, D, E, 5, 8, 2, 15 ); - P2( E, A, B, C, D, 6, 7, 11, 15 ); - P2( D, E, A, B, C, 7, 9, 4, 5 ); - P2( C, D, E, A, B, 8, 11, 13, 7 ); - P2( B, C, D, E, A, 9, 13, 6, 7 ); - P2( A, B, C, D, E, 10, 14, 15, 8 ); - P2( E, A, B, C, D, 11, 15, 8, 11 ); - P2( D, E, A, B, C, 12, 6, 1, 14 ); - P2( C, D, E, A, B, 13, 7, 10, 14 ); - P2( B, C, D, E, A, 14, 9, 3, 12 ); - P2( A, B, C, D, E, 15, 8, 12, 6 ); + P2( local.A, local.B, local.C, local.D, local.E, 0, 11, 5, 8 ); + P2( local.E, local.A, local.B, local.C, local.D, 1, 14, 14, 9 ); + P2( local.D, local.E, local.A, local.B, local.C, 2, 15, 7, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 3, 12, 0, 11 ); + P2( local.B, local.C, local.D, local.E, local.A, 4, 5, 9, 13 ); + P2( local.A, local.B, local.C, local.D, local.E, 5, 8, 2, 15 ); + P2( local.E, local.A, local.B, local.C, local.D, 6, 7, 11, 15 ); + P2( local.D, local.E, local.A, local.B, local.C, 7, 9, 4, 5 ); + P2( local.C, local.D, local.E, local.A, local.B, 8, 11, 13, 7 ); + P2( local.B, local.C, local.D, local.E, local.A, 9, 13, 6, 7 ); + P2( local.A, local.B, local.C, local.D, local.E, 10, 14, 15, 8 ); + P2( local.E, local.A, local.B, local.C, local.D, 11, 15, 8, 11 ); + P2( local.D, local.E, local.A, local.B, local.C, 12, 6, 1, 14 ); + P2( local.C, local.D, local.E, local.A, local.B, 13, 7, 10, 14 ); + P2( local.B, local.C, local.D, local.E, local.A, 14, 9, 3, 12 ); + P2( local.A, local.B, local.C, local.D, local.E, 15, 8, 12, 6 ); #undef F #undef K #undef Fp @@ -225,22 +228,22 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0x5A827999 #define Fp F4 #define Kp 0x5C4DD124 - P2( E, A, B, C, D, 7, 7, 6, 9 ); - P2( D, E, A, B, C, 4, 6, 11, 13 ); - P2( C, D, E, A, B, 13, 8, 3, 15 ); - P2( B, C, D, E, A, 1, 13, 7, 7 ); - P2( A, B, C, D, E, 10, 11, 0, 12 ); - P2( E, A, B, C, D, 6, 9, 13, 8 ); - P2( D, E, A, B, C, 15, 7, 5, 9 ); - P2( C, D, E, A, B, 3, 15, 10, 11 ); - P2( B, C, D, E, A, 12, 7, 14, 7 ); - P2( A, B, C, D, E, 0, 12, 15, 7 ); - P2( E, A, B, C, D, 9, 15, 8, 12 ); - P2( D, E, A, B, C, 5, 9, 12, 7 ); - P2( C, D, E, A, B, 2, 11, 4, 6 ); - P2( B, C, D, E, A, 14, 7, 9, 15 ); - P2( A, B, C, D, E, 11, 13, 1, 13 ); - P2( E, A, B, C, D, 8, 12, 2, 11 ); + P2( local.E, local.A, local.B, local.C, local.D, 7, 7, 6, 9 ); + P2( local.D, local.E, local.A, local.B, local.C, 4, 6, 11, 13 ); + P2( local.C, local.D, local.E, local.A, local.B, 13, 8, 3, 15 ); + P2( local.B, local.C, local.D, local.E, local.A, 1, 13, 7, 7 ); + P2( local.A, local.B, local.C, local.D, local.E, 10, 11, 0, 12 ); + P2( local.E, local.A, local.B, local.C, local.D, 6, 9, 13, 8 ); + P2( local.D, local.E, local.A, local.B, local.C, 15, 7, 5, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 3, 15, 10, 11 ); + P2( local.B, local.C, local.D, local.E, local.A, 12, 7, 14, 7 ); + P2( local.A, local.B, local.C, local.D, local.E, 0, 12, 15, 7 ); + P2( local.E, local.A, local.B, local.C, local.D, 9, 15, 8, 12 ); + P2( local.D, local.E, local.A, local.B, local.C, 5, 9, 12, 7 ); + P2( local.C, local.D, local.E, local.A, local.B, 2, 11, 4, 6 ); + P2( local.B, local.C, local.D, local.E, local.A, 14, 7, 9, 15 ); + P2( local.A, local.B, local.C, local.D, local.E, 11, 13, 1, 13 ); + P2( local.E, local.A, local.B, local.C, local.D, 8, 12, 2, 11 ); #undef F #undef K #undef Fp @@ -250,22 +253,22 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0x6ED9EBA1 #define Fp F3 #define Kp 0x6D703EF3 - P2( D, E, A, B, C, 3, 11, 15, 9 ); - P2( C, D, E, A, B, 10, 13, 5, 7 ); - P2( B, C, D, E, A, 14, 6, 1, 15 ); - P2( A, B, C, D, E, 4, 7, 3, 11 ); - P2( E, A, B, C, D, 9, 14, 7, 8 ); - P2( D, E, A, B, C, 15, 9, 14, 6 ); - P2( C, D, E, A, B, 8, 13, 6, 6 ); - P2( B, C, D, E, A, 1, 15, 9, 14 ); - P2( A, B, C, D, E, 2, 14, 11, 12 ); - P2( E, A, B, C, D, 7, 8, 8, 13 ); - P2( D, E, A, B, C, 0, 13, 12, 5 ); - P2( C, D, E, A, B, 6, 6, 2, 14 ); - P2( B, C, D, E, A, 13, 5, 10, 13 ); - P2( A, B, C, D, E, 11, 12, 0, 13 ); - P2( E, A, B, C, D, 5, 7, 4, 7 ); - P2( D, E, A, B, C, 12, 5, 13, 5 ); + P2( local.D, local.E, local.A, local.B, local.C, 3, 11, 15, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 10, 13, 5, 7 ); + P2( local.B, local.C, local.D, local.E, local.A, 14, 6, 1, 15 ); + P2( local.A, local.B, local.C, local.D, local.E, 4, 7, 3, 11 ); + P2( local.E, local.A, local.B, local.C, local.D, 9, 14, 7, 8 ); + P2( local.D, local.E, local.A, local.B, local.C, 15, 9, 14, 6 ); + P2( local.C, local.D, local.E, local.A, local.B, 8, 13, 6, 6 ); + P2( local.B, local.C, local.D, local.E, local.A, 1, 15, 9, 14 ); + P2( local.A, local.B, local.C, local.D, local.E, 2, 14, 11, 12 ); + P2( local.E, local.A, local.B, local.C, local.D, 7, 8, 8, 13 ); + P2( local.D, local.E, local.A, local.B, local.C, 0, 13, 12, 5 ); + P2( local.C, local.D, local.E, local.A, local.B, 6, 6, 2, 14 ); + P2( local.B, local.C, local.D, local.E, local.A, 13, 5, 10, 13 ); + P2( local.A, local.B, local.C, local.D, local.E, 11, 12, 0, 13 ); + P2( local.E, local.A, local.B, local.C, local.D, 5, 7, 4, 7 ); + P2( local.D, local.E, local.A, local.B, local.C, 12, 5, 13, 5 ); #undef F #undef K #undef Fp @@ -275,22 +278,22 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0x8F1BBCDC #define Fp F2 #define Kp 0x7A6D76E9 - P2( C, D, E, A, B, 1, 11, 8, 15 ); - P2( B, C, D, E, A, 9, 12, 6, 5 ); - P2( A, B, C, D, E, 11, 14, 4, 8 ); - P2( E, A, B, C, D, 10, 15, 1, 11 ); - P2( D, E, A, B, C, 0, 14, 3, 14 ); - P2( C, D, E, A, B, 8, 15, 11, 14 ); - P2( B, C, D, E, A, 12, 9, 15, 6 ); - P2( A, B, C, D, E, 4, 8, 0, 14 ); - P2( E, A, B, C, D, 13, 9, 5, 6 ); - P2( D, E, A, B, C, 3, 14, 12, 9 ); - P2( C, D, E, A, B, 7, 5, 2, 12 ); - P2( B, C, D, E, A, 15, 6, 13, 9 ); - P2( A, B, C, D, E, 14, 8, 9, 12 ); - P2( E, A, B, C, D, 5, 6, 7, 5 ); - P2( D, E, A, B, C, 6, 5, 10, 15 ); - P2( C, D, E, A, B, 2, 12, 14, 8 ); + P2( local.C, local.D, local.E, local.A, local.B, 1, 11, 8, 15 ); + P2( local.B, local.C, local.D, local.E, local.A, 9, 12, 6, 5 ); + P2( local.A, local.B, local.C, local.D, local.E, 11, 14, 4, 8 ); + P2( local.E, local.A, local.B, local.C, local.D, 10, 15, 1, 11 ); + P2( local.D, local.E, local.A, local.B, local.C, 0, 14, 3, 14 ); + P2( local.C, local.D, local.E, local.A, local.B, 8, 15, 11, 14 ); + P2( local.B, local.C, local.D, local.E, local.A, 12, 9, 15, 6 ); + P2( local.A, local.B, local.C, local.D, local.E, 4, 8, 0, 14 ); + P2( local.E, local.A, local.B, local.C, local.D, 13, 9, 5, 6 ); + P2( local.D, local.E, local.A, local.B, local.C, 3, 14, 12, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 7, 5, 2, 12 ); + P2( local.B, local.C, local.D, local.E, local.A, 15, 6, 13, 9 ); + P2( local.A, local.B, local.C, local.D, local.E, 14, 8, 9, 12 ); + P2( local.E, local.A, local.B, local.C, local.D, 5, 6, 7, 5 ); + P2( local.D, local.E, local.A, local.B, local.C, 6, 5, 10, 15 ); + P2( local.C, local.D, local.E, local.A, local.B, 2, 12, 14, 8 ); #undef F #undef K #undef Fp @@ -300,33 +303,36 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0xA953FD4E #define Fp F1 #define Kp 0x00000000 - P2( B, C, D, E, A, 4, 9, 12, 8 ); - P2( A, B, C, D, E, 0, 15, 15, 5 ); - P2( E, A, B, C, D, 5, 5, 10, 12 ); - P2( D, E, A, B, C, 9, 11, 4, 9 ); - P2( C, D, E, A, B, 7, 6, 1, 12 ); - P2( B, C, D, E, A, 12, 8, 5, 5 ); - P2( A, B, C, D, E, 2, 13, 8, 14 ); - P2( E, A, B, C, D, 10, 12, 7, 6 ); - P2( D, E, A, B, C, 14, 5, 6, 8 ); - P2( C, D, E, A, B, 1, 12, 2, 13 ); - P2( B, C, D, E, A, 3, 13, 13, 6 ); - P2( A, B, C, D, E, 8, 14, 14, 5 ); - P2( E, A, B, C, D, 11, 11, 0, 15 ); - P2( D, E, A, B, C, 6, 8, 3, 13 ); - P2( C, D, E, A, B, 15, 5, 9, 11 ); - P2( B, C, D, E, A, 13, 6, 11, 11 ); + P2( local.B, local.C, local.D, local.E, local.A, 4, 9, 12, 8 ); + P2( local.A, local.B, local.C, local.D, local.E, 0, 15, 15, 5 ); + P2( local.E, local.A, local.B, local.C, local.D, 5, 5, 10, 12 ); + P2( local.D, local.E, local.A, local.B, local.C, 9, 11, 4, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 7, 6, 1, 12 ); + P2( local.B, local.C, local.D, local.E, local.A, 12, 8, 5, 5 ); + P2( local.A, local.B, local.C, local.D, local.E, 2, 13, 8, 14 ); + P2( local.E, local.A, local.B, local.C, local.D, 10, 12, 7, 6 ); + P2( local.D, local.E, local.A, local.B, local.C, 14, 5, 6, 8 ); + P2( local.C, local.D, local.E, local.A, local.B, 1, 12, 2, 13 ); + P2( local.B, local.C, local.D, local.E, local.A, 3, 13, 13, 6 ); + P2( local.A, local.B, local.C, local.D, local.E, 8, 14, 14, 5 ); + P2( local.E, local.A, local.B, local.C, local.D, 11, 11, 0, 15 ); + P2( local.D, local.E, local.A, local.B, local.C, 6, 8, 3, 13 ); + P2( local.C, local.D, local.E, local.A, local.B, 15, 5, 9, 11 ); + P2( local.B, local.C, local.D, local.E, local.A, 13, 6, 11, 11 ); #undef F #undef K #undef Fp #undef Kp - C = ctx->state[1] + C + Dp; - ctx->state[1] = ctx->state[2] + D + Ep; - ctx->state[2] = ctx->state[3] + E + Ap; - ctx->state[3] = ctx->state[4] + A + Bp; - ctx->state[4] = ctx->state[0] + B + Cp; - ctx->state[0] = C; + local.C = ctx->state[1] + local.C + local.Dp; + ctx->state[1] = ctx->state[2] + local.D + local.Ep; + ctx->state[2] = ctx->state[3] + local.E + local.Ap; + ctx->state[3] = ctx->state[4] + local.A + local.Bp; + ctx->state[4] = ctx->state[0] + local.B + local.Cp; + ctx->state[0] = local.C; + + /* Zeroise variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/rsa.c b/thirdparty/mbedtls/library/rsa.c index 42becbf17b..a25c633bc6 100644 --- a/thirdparty/mbedtls/library/rsa.c +++ b/thirdparty/mbedtls/library/rsa.c @@ -841,15 +841,14 @@ static int rsa_prepare_blinding( mbedtls_rsa_context *ctx, * which one, we just loop and choose new values for both of them. * (Each iteration succeeds with overwhelming probability.) */ ret = mbedtls_mpi_inv_mod( &ctx->Vi, &ctx->Vi, &ctx->N ); - if( ret == MBEDTLS_ERR_MPI_NOT_ACCEPTABLE ) - continue; - if( ret != 0 ) + if( ret != 0 && ret != MBEDTLS_ERR_MPI_NOT_ACCEPTABLE ) goto cleanup; - /* Finish the computation of Vf^-1 = R * (R Vf)^-1 */ - MBEDTLS_MPI_CHK( mbedtls_mpi_mul_mpi( &ctx->Vi, &ctx->Vi, &R ) ); - MBEDTLS_MPI_CHK( mbedtls_mpi_mod_mpi( &ctx->Vi, &ctx->Vi, &ctx->N ) ); - } while( 0 ); + } while( ret == MBEDTLS_ERR_MPI_NOT_ACCEPTABLE ); + + /* Finish the computation of Vf^-1 = R * (R Vf)^-1 */ + MBEDTLS_MPI_CHK( mbedtls_mpi_mul_mpi( &ctx->Vi, &ctx->Vi, &R ) ); + MBEDTLS_MPI_CHK( mbedtls_mpi_mod_mpi( &ctx->Vi, &ctx->Vi, &ctx->N ) ); /* Blinding value: Vi = Vf^(-e) mod N * (Vi already contains Vf^-1 at this point) */ diff --git a/thirdparty/mbedtls/library/sha1.c b/thirdparty/mbedtls/library/sha1.c index 8682abd740..e99a5e8635 100644 --- a/thirdparty/mbedtls/library/sha1.c +++ b/thirdparty/mbedtls/library/sha1.c @@ -155,35 +155,40 @@ void mbedtls_sha1_starts( mbedtls_sha1_context *ctx ) int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, const unsigned char data[64] ) { - uint32_t temp, W[16], A, B, C, D, E; + struct + { + uint32_t temp, W[16], A, B, C, D, E; + } local; SHA1_VALIDATE_RET( ctx != NULL ); SHA1_VALIDATE_RET( (const unsigned char *)data != NULL ); - GET_UINT32_BE( W[ 0], data, 0 ); - GET_UINT32_BE( W[ 1], data, 4 ); - GET_UINT32_BE( W[ 2], data, 8 ); - GET_UINT32_BE( W[ 3], data, 12 ); - GET_UINT32_BE( W[ 4], data, 16 ); - GET_UINT32_BE( W[ 5], data, 20 ); - GET_UINT32_BE( W[ 6], data, 24 ); - GET_UINT32_BE( W[ 7], data, 28 ); - GET_UINT32_BE( W[ 8], data, 32 ); - GET_UINT32_BE( W[ 9], data, 36 ); - GET_UINT32_BE( W[10], data, 40 ); - GET_UINT32_BE( W[11], data, 44 ); - GET_UINT32_BE( W[12], data, 48 ); - GET_UINT32_BE( W[13], data, 52 ); - GET_UINT32_BE( W[14], data, 56 ); - GET_UINT32_BE( W[15], data, 60 ); + GET_UINT32_BE( local.W[ 0], data, 0 ); + GET_UINT32_BE( local.W[ 1], data, 4 ); + GET_UINT32_BE( local.W[ 2], data, 8 ); + GET_UINT32_BE( local.W[ 3], data, 12 ); + GET_UINT32_BE( local.W[ 4], data, 16 ); + GET_UINT32_BE( local.W[ 5], data, 20 ); + GET_UINT32_BE( local.W[ 6], data, 24 ); + GET_UINT32_BE( local.W[ 7], data, 28 ); + GET_UINT32_BE( local.W[ 8], data, 32 ); + GET_UINT32_BE( local.W[ 9], data, 36 ); + GET_UINT32_BE( local.W[10], data, 40 ); + GET_UINT32_BE( local.W[11], data, 44 ); + GET_UINT32_BE( local.W[12], data, 48 ); + GET_UINT32_BE( local.W[13], data, 52 ); + GET_UINT32_BE( local.W[14], data, 56 ); + GET_UINT32_BE( local.W[15], data, 60 ); #define S(x,n) (((x) << (n)) | (((x) & 0xFFFFFFFF) >> (32 - (n)))) #define R(t) \ ( \ - temp = W[( (t) - 3 ) & 0x0F] ^ W[( (t) - 8 ) & 0x0F] ^ \ - W[( (t) - 14 ) & 0x0F] ^ W[ (t) & 0x0F], \ - ( W[(t) & 0x0F] = S(temp,1) ) \ + local.temp = local.W[( (t) - 3 ) & 0x0F] ^ \ + local.W[( (t) - 8 ) & 0x0F] ^ \ + local.W[( (t) - 14 ) & 0x0F] ^ \ + local.W[ (t) & 0x0F], \ + ( local.W[(t) & 0x0F] = S(local.temp,1) ) \ ) #define P(a,b,c,d,e,x) \ @@ -193,35 +198,35 @@ int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, (b) = S((b),30); \ } while( 0 ) - A = ctx->state[0]; - B = ctx->state[1]; - C = ctx->state[2]; - D = ctx->state[3]; - E = ctx->state[4]; + local.A = ctx->state[0]; + local.B = ctx->state[1]; + local.C = ctx->state[2]; + local.D = ctx->state[3]; + local.E = ctx->state[4]; #define F(x,y,z) ((z) ^ ((x) & ((y) ^ (z)))) #define K 0x5A827999 - P( A, B, C, D, E, W[0] ); - P( E, A, B, C, D, W[1] ); - P( D, E, A, B, C, W[2] ); - P( C, D, E, A, B, W[3] ); - P( B, C, D, E, A, W[4] ); - P( A, B, C, D, E, W[5] ); - P( E, A, B, C, D, W[6] ); - P( D, E, A, B, C, W[7] ); - P( C, D, E, A, B, W[8] ); - P( B, C, D, E, A, W[9] ); - P( A, B, C, D, E, W[10] ); - P( E, A, B, C, D, W[11] ); - P( D, E, A, B, C, W[12] ); - P( C, D, E, A, B, W[13] ); - P( B, C, D, E, A, W[14] ); - P( A, B, C, D, E, W[15] ); - P( E, A, B, C, D, R(16) ); - P( D, E, A, B, C, R(17) ); - P( C, D, E, A, B, R(18) ); - P( B, C, D, E, A, R(19) ); + P( local.A, local.B, local.C, local.D, local.E, local.W[0] ); + P( local.E, local.A, local.B, local.C, local.D, local.W[1] ); + P( local.D, local.E, local.A, local.B, local.C, local.W[2] ); + P( local.C, local.D, local.E, local.A, local.B, local.W[3] ); + P( local.B, local.C, local.D, local.E, local.A, local.W[4] ); + P( local.A, local.B, local.C, local.D, local.E, local.W[5] ); + P( local.E, local.A, local.B, local.C, local.D, local.W[6] ); + P( local.D, local.E, local.A, local.B, local.C, local.W[7] ); + P( local.C, local.D, local.E, local.A, local.B, local.W[8] ); + P( local.B, local.C, local.D, local.E, local.A, local.W[9] ); + P( local.A, local.B, local.C, local.D, local.E, local.W[10] ); + P( local.E, local.A, local.B, local.C, local.D, local.W[11] ); + P( local.D, local.E, local.A, local.B, local.C, local.W[12] ); + P( local.C, local.D, local.E, local.A, local.B, local.W[13] ); + P( local.B, local.C, local.D, local.E, local.A, local.W[14] ); + P( local.A, local.B, local.C, local.D, local.E, local.W[15] ); + P( local.E, local.A, local.B, local.C, local.D, R(16) ); + P( local.D, local.E, local.A, local.B, local.C, R(17) ); + P( local.C, local.D, local.E, local.A, local.B, R(18) ); + P( local.B, local.C, local.D, local.E, local.A, R(19) ); #undef K #undef F @@ -229,26 +234,26 @@ int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, #define F(x,y,z) ((x) ^ (y) ^ (z)) #define K 0x6ED9EBA1 - P( A, B, C, D, E, R(20) ); - P( E, A, B, C, D, R(21) ); - P( D, E, A, B, C, R(22) ); - P( C, D, E, A, B, R(23) ); - P( B, C, D, E, A, R(24) ); - P( A, B, C, D, E, R(25) ); - P( E, A, B, C, D, R(26) ); - P( D, E, A, B, C, R(27) ); - P( C, D, E, A, B, R(28) ); - P( B, C, D, E, A, R(29) ); - P( A, B, C, D, E, R(30) ); - P( E, A, B, C, D, R(31) ); - P( D, E, A, B, C, R(32) ); - P( C, D, E, A, B, R(33) ); - P( B, C, D, E, A, R(34) ); - P( A, B, C, D, E, R(35) ); - P( E, A, B, C, D, R(36) ); - P( D, E, A, B, C, R(37) ); - P( C, D, E, A, B, R(38) ); - P( B, C, D, E, A, R(39) ); + P( local.A, local.B, local.C, local.D, local.E, R(20) ); + P( local.E, local.A, local.B, local.C, local.D, R(21) ); + P( local.D, local.E, local.A, local.B, local.C, R(22) ); + P( local.C, local.D, local.E, local.A, local.B, R(23) ); + P( local.B, local.C, local.D, local.E, local.A, R(24) ); + P( local.A, local.B, local.C, local.D, local.E, R(25) ); + P( local.E, local.A, local.B, local.C, local.D, R(26) ); + P( local.D, local.E, local.A, local.B, local.C, R(27) ); + P( local.C, local.D, local.E, local.A, local.B, R(28) ); + P( local.B, local.C, local.D, local.E, local.A, R(29) ); + P( local.A, local.B, local.C, local.D, local.E, R(30) ); + P( local.E, local.A, local.B, local.C, local.D, R(31) ); + P( local.D, local.E, local.A, local.B, local.C, R(32) ); + P( local.C, local.D, local.E, local.A, local.B, R(33) ); + P( local.B, local.C, local.D, local.E, local.A, R(34) ); + P( local.A, local.B, local.C, local.D, local.E, R(35) ); + P( local.E, local.A, local.B, local.C, local.D, R(36) ); + P( local.D, local.E, local.A, local.B, local.C, R(37) ); + P( local.C, local.D, local.E, local.A, local.B, R(38) ); + P( local.B, local.C, local.D, local.E, local.A, R(39) ); #undef K #undef F @@ -256,26 +261,26 @@ int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, #define F(x,y,z) (((x) & (y)) | ((z) & ((x) | (y)))) #define K 0x8F1BBCDC - P( A, B, C, D, E, R(40) ); - P( E, A, B, C, D, R(41) ); - P( D, E, A, B, C, R(42) ); - P( C, D, E, A, B, R(43) ); - P( B, C, D, E, A, R(44) ); - P( A, B, C, D, E, R(45) ); - P( E, A, B, C, D, R(46) ); - P( D, E, A, B, C, R(47) ); - P( C, D, E, A, B, R(48) ); - P( B, C, D, E, A, R(49) ); - P( A, B, C, D, E, R(50) ); - P( E, A, B, C, D, R(51) ); - P( D, E, A, B, C, R(52) ); - P( C, D, E, A, B, R(53) ); - P( B, C, D, E, A, R(54) ); - P( A, B, C, D, E, R(55) ); - P( E, A, B, C, D, R(56) ); - P( D, E, A, B, C, R(57) ); - P( C, D, E, A, B, R(58) ); - P( B, C, D, E, A, R(59) ); + P( local.A, local.B, local.C, local.D, local.E, R(40) ); + P( local.E, local.A, local.B, local.C, local.D, R(41) ); + P( local.D, local.E, local.A, local.B, local.C, R(42) ); + P( local.C, local.D, local.E, local.A, local.B, R(43) ); + P( local.B, local.C, local.D, local.E, local.A, R(44) ); + P( local.A, local.B, local.C, local.D, local.E, R(45) ); + P( local.E, local.A, local.B, local.C, local.D, R(46) ); + P( local.D, local.E, local.A, local.B, local.C, R(47) ); + P( local.C, local.D, local.E, local.A, local.B, R(48) ); + P( local.B, local.C, local.D, local.E, local.A, R(49) ); + P( local.A, local.B, local.C, local.D, local.E, R(50) ); + P( local.E, local.A, local.B, local.C, local.D, R(51) ); + P( local.D, local.E, local.A, local.B, local.C, R(52) ); + P( local.C, local.D, local.E, local.A, local.B, R(53) ); + P( local.B, local.C, local.D, local.E, local.A, R(54) ); + P( local.A, local.B, local.C, local.D, local.E, R(55) ); + P( local.E, local.A, local.B, local.C, local.D, R(56) ); + P( local.D, local.E, local.A, local.B, local.C, R(57) ); + P( local.C, local.D, local.E, local.A, local.B, R(58) ); + P( local.B, local.C, local.D, local.E, local.A, R(59) ); #undef K #undef F @@ -283,35 +288,38 @@ int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, #define F(x,y,z) ((x) ^ (y) ^ (z)) #define K 0xCA62C1D6 - P( A, B, C, D, E, R(60) ); - P( E, A, B, C, D, R(61) ); - P( D, E, A, B, C, R(62) ); - P( C, D, E, A, B, R(63) ); - P( B, C, D, E, A, R(64) ); - P( A, B, C, D, E, R(65) ); - P( E, A, B, C, D, R(66) ); - P( D, E, A, B, C, R(67) ); - P( C, D, E, A, B, R(68) ); - P( B, C, D, E, A, R(69) ); - P( A, B, C, D, E, R(70) ); - P( E, A, B, C, D, R(71) ); - P( D, E, A, B, C, R(72) ); - P( C, D, E, A, B, R(73) ); - P( B, C, D, E, A, R(74) ); - P( A, B, C, D, E, R(75) ); - P( E, A, B, C, D, R(76) ); - P( D, E, A, B, C, R(77) ); - P( C, D, E, A, B, R(78) ); - P( B, C, D, E, A, R(79) ); + P( local.A, local.B, local.C, local.D, local.E, R(60) ); + P( local.E, local.A, local.B, local.C, local.D, R(61) ); + P( local.D, local.E, local.A, local.B, local.C, R(62) ); + P( local.C, local.D, local.E, local.A, local.B, R(63) ); + P( local.B, local.C, local.D, local.E, local.A, R(64) ); + P( local.A, local.B, local.C, local.D, local.E, R(65) ); + P( local.E, local.A, local.B, local.C, local.D, R(66) ); + P( local.D, local.E, local.A, local.B, local.C, R(67) ); + P( local.C, local.D, local.E, local.A, local.B, R(68) ); + P( local.B, local.C, local.D, local.E, local.A, R(69) ); + P( local.A, local.B, local.C, local.D, local.E, R(70) ); + P( local.E, local.A, local.B, local.C, local.D, R(71) ); + P( local.D, local.E, local.A, local.B, local.C, R(72) ); + P( local.C, local.D, local.E, local.A, local.B, R(73) ); + P( local.B, local.C, local.D, local.E, local.A, R(74) ); + P( local.A, local.B, local.C, local.D, local.E, R(75) ); + P( local.E, local.A, local.B, local.C, local.D, R(76) ); + P( local.D, local.E, local.A, local.B, local.C, R(77) ); + P( local.C, local.D, local.E, local.A, local.B, R(78) ); + P( local.B, local.C, local.D, local.E, local.A, R(79) ); #undef K #undef F - ctx->state[0] += A; - ctx->state[1] += B; - ctx->state[2] += C; - ctx->state[3] += D; - ctx->state[4] += E; + ctx->state[0] += local.A; + ctx->state[1] += local.B; + ctx->state[2] += local.C; + ctx->state[3] += local.D; + ctx->state[4] += local.E; + + /* Zeroise buffers and variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/sha256.c b/thirdparty/mbedtls/library/sha256.c index 5169584b68..75a8f8a2b2 100644 --- a/thirdparty/mbedtls/library/sha256.c +++ b/thirdparty/mbedtls/library/sha256.c @@ -209,77 +209,104 @@ static const uint32_t K[] = #define F0(x,y,z) (((x) & (y)) | ((z) & ((x) | (y)))) #define F1(x,y,z) ((z) ^ ((x) & ((y) ^ (z)))) -#define R(t) \ - ( \ - W[t] = S1(W[(t) - 2]) + W[(t) - 7] + \ - S0(W[(t) - 15]) + W[(t) - 16] \ +#define R(t) \ + ( \ + local.W[t] = S1(local.W[(t) - 2]) + local.W[(t) - 7] + \ + S0(local.W[(t) - 15]) + local.W[(t) - 16] \ ) -#define P(a,b,c,d,e,f,g,h,x,K) \ - do \ - { \ - temp1 = (h) + S3(e) + F1((e),(f),(g)) + (K) + (x); \ - temp2 = S2(a) + F0((a),(b),(c)); \ - (d) += temp1; (h) = temp1 + temp2; \ +#define P(a,b,c,d,e,f,g,h,x,K) \ + do \ + { \ + local.temp1 = (h) + S3(e) + F1((e),(f),(g)) + (K) + (x); \ + local.temp2 = S2(a) + F0((a),(b),(c)); \ + (d) += local.temp1; (h) = local.temp1 + local.temp2; \ } while( 0 ) int mbedtls_internal_sha256_process( mbedtls_sha256_context *ctx, const unsigned char data[64] ) { - uint32_t temp1, temp2, W[64]; - uint32_t A[8]; + struct + { + uint32_t temp1, temp2, W[64]; + uint32_t A[8]; + } local; + unsigned int i; SHA256_VALIDATE_RET( ctx != NULL ); SHA256_VALIDATE_RET( (const unsigned char *)data != NULL ); for( i = 0; i < 8; i++ ) - A[i] = ctx->state[i]; + local.A[i] = ctx->state[i]; #if defined(MBEDTLS_SHA256_SMALLER) for( i = 0; i < 64; i++ ) { if( i < 16 ) - GET_UINT32_BE( W[i], data, 4 * i ); + GET_UINT32_BE( local.W[i], data, 4 * i ); else R( i ); - P( A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7], W[i], K[i] ); + P( local.A[0], local.A[1], local.A[2], local.A[3], local.A[4], + local.A[5], local.A[6], local.A[7], local.W[i], K[i] ); - temp1 = A[7]; A[7] = A[6]; A[6] = A[5]; A[5] = A[4]; A[4] = A[3]; - A[3] = A[2]; A[2] = A[1]; A[1] = A[0]; A[0] = temp1; + local.temp1 = local.A[7]; local.A[7] = local.A[6]; + local.A[6] = local.A[5]; local.A[5] = local.A[4]; + local.A[4] = local.A[3]; local.A[3] = local.A[2]; + local.A[2] = local.A[1]; local.A[1] = local.A[0]; + local.A[0] = local.temp1; } #else /* MBEDTLS_SHA256_SMALLER */ for( i = 0; i < 16; i++ ) - GET_UINT32_BE( W[i], data, 4 * i ); + GET_UINT32_BE( local.W[i], data, 4 * i ); for( i = 0; i < 16; i += 8 ) { - P( A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7], W[i+0], K[i+0] ); - P( A[7], A[0], A[1], A[2], A[3], A[4], A[5], A[6], W[i+1], K[i+1] ); - P( A[6], A[7], A[0], A[1], A[2], A[3], A[4], A[5], W[i+2], K[i+2] ); - P( A[5], A[6], A[7], A[0], A[1], A[2], A[3], A[4], W[i+3], K[i+3] ); - P( A[4], A[5], A[6], A[7], A[0], A[1], A[2], A[3], W[i+4], K[i+4] ); - P( A[3], A[4], A[5], A[6], A[7], A[0], A[1], A[2], W[i+5], K[i+5] ); - P( A[2], A[3], A[4], A[5], A[6], A[7], A[0], A[1], W[i+6], K[i+6] ); - P( A[1], A[2], A[3], A[4], A[5], A[6], A[7], A[0], W[i+7], K[i+7] ); + P( local.A[0], local.A[1], local.A[2], local.A[3], local.A[4], + local.A[5], local.A[6], local.A[7], local.W[i+0], K[i+0] ); + P( local.A[7], local.A[0], local.A[1], local.A[2], local.A[3], + local.A[4], local.A[5], local.A[6], local.W[i+1], K[i+1] ); + P( local.A[6], local.A[7], local.A[0], local.A[1], local.A[2], + local.A[3], local.A[4], local.A[5], local.W[i+2], K[i+2] ); + P( local.A[5], local.A[6], local.A[7], local.A[0], local.A[1], + local.A[2], local.A[3], local.A[4], local.W[i+3], K[i+3] ); + P( local.A[4], local.A[5], local.A[6], local.A[7], local.A[0], + local.A[1], local.A[2], local.A[3], local.W[i+4], K[i+4] ); + P( local.A[3], local.A[4], local.A[5], local.A[6], local.A[7], + local.A[0], local.A[1], local.A[2], local.W[i+5], K[i+5] ); + P( local.A[2], local.A[3], local.A[4], local.A[5], local.A[6], + local.A[7], local.A[0], local.A[1], local.W[i+6], K[i+6] ); + P( local.A[1], local.A[2], local.A[3], local.A[4], local.A[5], + local.A[6], local.A[7], local.A[0], local.W[i+7], K[i+7] ); } for( i = 16; i < 64; i += 8 ) { - P( A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7], R(i+0), K[i+0] ); - P( A[7], A[0], A[1], A[2], A[3], A[4], A[5], A[6], R(i+1), K[i+1] ); - P( A[6], A[7], A[0], A[1], A[2], A[3], A[4], A[5], R(i+2), K[i+2] ); - P( A[5], A[6], A[7], A[0], A[1], A[2], A[3], A[4], R(i+3), K[i+3] ); - P( A[4], A[5], A[6], A[7], A[0], A[1], A[2], A[3], R(i+4), K[i+4] ); - P( A[3], A[4], A[5], A[6], A[7], A[0], A[1], A[2], R(i+5), K[i+5] ); - P( A[2], A[3], A[4], A[5], A[6], A[7], A[0], A[1], R(i+6), K[i+6] ); - P( A[1], A[2], A[3], A[4], A[5], A[6], A[7], A[0], R(i+7), K[i+7] ); + P( local.A[0], local.A[1], local.A[2], local.A[3], local.A[4], + local.A[5], local.A[6], local.A[7], R(i+0), K[i+0] ); + P( local.A[7], local.A[0], local.A[1], local.A[2], local.A[3], + local.A[4], local.A[5], local.A[6], R(i+1), K[i+1] ); + P( local.A[6], local.A[7], local.A[0], local.A[1], local.A[2], + local.A[3], local.A[4], local.A[5], R(i+2), K[i+2] ); + P( local.A[5], local.A[6], local.A[7], local.A[0], local.A[1], + local.A[2], local.A[3], local.A[4], R(i+3), K[i+3] ); + P( local.A[4], local.A[5], local.A[6], local.A[7], local.A[0], + local.A[1], local.A[2], local.A[3], R(i+4), K[i+4] ); + P( local.A[3], local.A[4], local.A[5], local.A[6], local.A[7], + local.A[0], local.A[1], local.A[2], R(i+5), K[i+5] ); + P( local.A[2], local.A[3], local.A[4], local.A[5], local.A[6], + local.A[7], local.A[0], local.A[1], R(i+6), K[i+6] ); + P( local.A[1], local.A[2], local.A[3], local.A[4], local.A[5], + local.A[6], local.A[7], local.A[0], R(i+7), K[i+7] ); } #endif /* MBEDTLS_SHA256_SMALLER */ for( i = 0; i < 8; i++ ) - ctx->state[i] += A[i]; + ctx->state[i] += local.A[i]; + + /* Zeroise buffers and variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/sha512.c b/thirdparty/mbedtls/library/sha512.c index 36d5d96146..986037ab7c 100644 --- a/thirdparty/mbedtls/library/sha512.c +++ b/thirdparty/mbedtls/library/sha512.c @@ -243,8 +243,11 @@ int mbedtls_internal_sha512_process( mbedtls_sha512_context *ctx, const unsigned char data[128] ) { int i; - uint64_t temp1, temp2, W[80]; - uint64_t A, B, C, D, E, F, G, H; + struct + { + uint64_t temp1, temp2, W[80]; + uint64_t A, B, C, D, E, F, G, H; + } local; SHA512_VALIDATE_RET( ctx != NULL ); SHA512_VALIDATE_RET( (const unsigned char *)data != NULL ); @@ -261,56 +264,67 @@ int mbedtls_internal_sha512_process( mbedtls_sha512_context *ctx, #define F0(x,y,z) (((x) & (y)) | ((z) & ((x) | (y)))) #define F1(x,y,z) ((z) ^ ((x) & ((y) ^ (z)))) -#define P(a,b,c,d,e,f,g,h,x,K) \ - do \ - { \ - temp1 = (h) + S3(e) + F1((e),(f),(g)) + (K) + (x); \ - temp2 = S2(a) + F0((a),(b),(c)); \ - (d) += temp1; (h) = temp1 + temp2; \ +#define P(a,b,c,d,e,f,g,h,x,K) \ + do \ + { \ + local.temp1 = (h) + S3(e) + F1((e),(f),(g)) + (K) + (x); \ + local.temp2 = S2(a) + F0((a),(b),(c)); \ + (d) += local.temp1; (h) = local.temp1 + local.temp2; \ } while( 0 ) for( i = 0; i < 16; i++ ) { - GET_UINT64_BE( W[i], data, i << 3 ); + GET_UINT64_BE( local.W[i], data, i << 3 ); } for( ; i < 80; i++ ) { - W[i] = S1(W[i - 2]) + W[i - 7] + - S0(W[i - 15]) + W[i - 16]; + local.W[i] = S1(local.W[i - 2]) + local.W[i - 7] + + S0(local.W[i - 15]) + local.W[i - 16]; } - A = ctx->state[0]; - B = ctx->state[1]; - C = ctx->state[2]; - D = ctx->state[3]; - E = ctx->state[4]; - F = ctx->state[5]; - G = ctx->state[6]; - H = ctx->state[7]; + local.A = ctx->state[0]; + local.B = ctx->state[1]; + local.C = ctx->state[2]; + local.D = ctx->state[3]; + local.E = ctx->state[4]; + local.F = ctx->state[5]; + local.G = ctx->state[6]; + local.H = ctx->state[7]; i = 0; do { - P( A, B, C, D, E, F, G, H, W[i], K[i] ); i++; - P( H, A, B, C, D, E, F, G, W[i], K[i] ); i++; - P( G, H, A, B, C, D, E, F, W[i], K[i] ); i++; - P( F, G, H, A, B, C, D, E, W[i], K[i] ); i++; - P( E, F, G, H, A, B, C, D, W[i], K[i] ); i++; - P( D, E, F, G, H, A, B, C, W[i], K[i] ); i++; - P( C, D, E, F, G, H, A, B, W[i], K[i] ); i++; - P( B, C, D, E, F, G, H, A, W[i], K[i] ); i++; + P( local.A, local.B, local.C, local.D, local.E, + local.F, local.G, local.H, local.W[i], K[i] ); i++; + P( local.H, local.A, local.B, local.C, local.D, + local.E, local.F, local.G, local.W[i], K[i] ); i++; + P( local.G, local.H, local.A, local.B, local.C, + local.D, local.E, local.F, local.W[i], K[i] ); i++; + P( local.F, local.G, local.H, local.A, local.B, + local.C, local.D, local.E, local.W[i], K[i] ); i++; + P( local.E, local.F, local.G, local.H, local.A, + local.B, local.C, local.D, local.W[i], K[i] ); i++; + P( local.D, local.E, local.F, local.G, local.H, + local.A, local.B, local.C, local.W[i], K[i] ); i++; + P( local.C, local.D, local.E, local.F, local.G, + local.H, local.A, local.B, local.W[i], K[i] ); i++; + P( local.B, local.C, local.D, local.E, local.F, + local.G, local.H, local.A, local.W[i], K[i] ); i++; } while( i < 80 ); - ctx->state[0] += A; - ctx->state[1] += B; - ctx->state[2] += C; - ctx->state[3] += D; - ctx->state[4] += E; - ctx->state[5] += F; - ctx->state[6] += G; - ctx->state[7] += H; + ctx->state[0] += local.A; + ctx->state[1] += local.B; + ctx->state[2] += local.C; + ctx->state[3] += local.D; + ctx->state[4] += local.E; + ctx->state[5] += local.F; + ctx->state[6] += local.G; + ctx->state[7] += local.H; + + /* Zeroise buffers and variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/ssl_srv.c b/thirdparty/mbedtls/library/ssl_srv.c index 97b778452c..cbf6142ac2 100644 --- a/thirdparty/mbedtls/library/ssl_srv.c +++ b/thirdparty/mbedtls/library/ssl_srv.c @@ -3587,11 +3587,12 @@ static int ssl_parse_encrypted_pms( mbedtls_ssl_context *ssl, /* In case of a failure in decryption, the decryption may write less than * 2 bytes of output, but we always read the first two bytes. It doesn't * matter in the end because diff will be nonzero in that case due to - * peer_pmslen being less than 48, and we only care whether diff is 0. - * But do initialize peer_pms for robustness anyway. This also makes - * memory analyzers happy (don't access uninitialized memory, even - * if it's an unsigned char). */ + * ret being nonzero, and we only care whether diff is 0. + * But do initialize peer_pms and peer_pmslen for robustness anyway. This + * also makes memory analyzers happy (don't access uninitialized memory, + * even if it's an unsigned char). */ peer_pms[0] = peer_pms[1] = ~0; + peer_pmslen = 0; ret = ssl_decrypt_encrypted_pms( ssl, p, end, peer_pms, diff --git a/thirdparty/mbedtls/library/ssl_tls.c b/thirdparty/mbedtls/library/ssl_tls.c index 2471600c9a..c749a8611c 100644 --- a/thirdparty/mbedtls/library/ssl_tls.c +++ b/thirdparty/mbedtls/library/ssl_tls.c @@ -621,7 +621,7 @@ static void ssl_calc_finished_tls( mbedtls_ssl_context *, unsigned char *, int ) #if defined(MBEDTLS_SSL_PROTO_TLS1_2) #if defined(MBEDTLS_SHA256_C) static void ssl_update_checksum_sha256( mbedtls_ssl_context *, const unsigned char *, size_t ); -static void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *,unsigned char * ); +static void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *, unsigned char * ); static void ssl_calc_finished_tls_sha256( mbedtls_ssl_context *,unsigned char *, int ); #endif @@ -1142,7 +1142,7 @@ int mbedtls_ssl_derive_keys( mbedtls_ssl_context *ssl ) } #if defined(MBEDTLS_SSL_PROTO_SSL3) -void ssl_calc_verify_ssl( mbedtls_ssl_context *ssl, unsigned char hash[36] ) +void ssl_calc_verify_ssl( mbedtls_ssl_context *ssl, unsigned char *hash ) { mbedtls_md5_context md5; mbedtls_sha1_context sha1; @@ -1191,7 +1191,7 @@ void ssl_calc_verify_ssl( mbedtls_ssl_context *ssl, unsigned char hash[36] ) #endif /* MBEDTLS_SSL_PROTO_SSL3 */ #if defined(MBEDTLS_SSL_PROTO_TLS1) || defined(MBEDTLS_SSL_PROTO_TLS1_1) -void ssl_calc_verify_tls( mbedtls_ssl_context *ssl, unsigned char hash[36] ) +void ssl_calc_verify_tls( mbedtls_ssl_context *ssl, unsigned char *hash ) { mbedtls_md5_context md5; mbedtls_sha1_context sha1; @@ -1219,7 +1219,7 @@ void ssl_calc_verify_tls( mbedtls_ssl_context *ssl, unsigned char hash[36] ) #if defined(MBEDTLS_SSL_PROTO_TLS1_2) #if defined(MBEDTLS_SHA256_C) -void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *ssl, unsigned char hash[32] ) +void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *ssl, unsigned char *hash ) { mbedtls_sha256_context sha256; @@ -1240,7 +1240,7 @@ void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *ssl, unsigned char hash[32 #endif /* MBEDTLS_SHA256_C */ #if defined(MBEDTLS_SHA512_C) -void ssl_calc_verify_tls_sha384( mbedtls_ssl_context *ssl, unsigned char hash[48] ) +void ssl_calc_verify_tls_sha384( mbedtls_ssl_context *ssl, unsigned char *hash ) { mbedtls_sha512_context sha512; @@ -6363,6 +6363,9 @@ static void ssl_calc_finished_tls_sha256( #endif /* MBEDTLS_SHA256_C */ #if defined(MBEDTLS_SHA512_C) + +typedef int (*finish_sha384_t)(mbedtls_sha512_context*, unsigned char*); + static void ssl_calc_finished_tls_sha384( mbedtls_ssl_context *ssl, unsigned char *buf, int from ) { @@ -6370,6 +6373,12 @@ static void ssl_calc_finished_tls_sha384( const char *sender; mbedtls_sha512_context sha512; unsigned char padbuf[48]; + /* + * For SHA-384, we can save 16 bytes by keeping padbuf 48 bytes long. + * However, to avoid stringop-overflow warning in gcc, we have to cast + * mbedtls_sha512_finish_ret(). + */ + finish_sha384_t finish_sha384 = (finish_sha384_t)mbedtls_sha512_finish_ret; mbedtls_ssl_session *session = ssl->session_negotiate; if( !session ) @@ -6396,7 +6405,7 @@ static void ssl_calc_finished_tls_sha384( ? "client finished" : "server finished"; - mbedtls_sha512_finish_ret( &sha512, padbuf ); + finish_sha384( &sha512, padbuf ); ssl->handshake->tls_prf( session->master, 48, sender, padbuf, 48, buf, len ); diff --git a/thirdparty/mbedtls/library/threading.c b/thirdparty/mbedtls/library/threading.c index 61c4b94041..f4f29cff5e 100644 --- a/thirdparty/mbedtls/library/threading.c +++ b/thirdparty/mbedtls/library/threading.c @@ -73,7 +73,7 @@ #if !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) || \ ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) && \ - _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) + _POSIX_THREAD_SAFE_FUNCTIONS >= 200112L ) ) /* * This is a convenience shorthand macro to avoid checking the long * preprocessor conditions above. Ideally, we could expose this macro in @@ -88,7 +88,7 @@ #endif /* !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) || \ ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) && \ - _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) */ + _POSIX_THREAD_SAFE_FUNCTIONS >= 200112L ) ) */ #endif /* MBEDTLS_HAVE_TIME_DATE && !MBEDTLS_PLATFORM_GMTIME_R_ALT */ diff --git a/thirdparty/mbedtls/library/x509_crt.c b/thirdparty/mbedtls/library/x509_crt.c index de40eaaf58..c458c25ff4 100644 --- a/thirdparty/mbedtls/library/x509_crt.c +++ b/thirdparty/mbedtls/library/x509_crt.c @@ -1101,6 +1101,7 @@ static int x509_crt_parse_der_core( mbedtls_x509_crt *crt, const unsigned char * if( crt->sig_oid.len != sig_oid2.len || memcmp( crt->sig_oid.p, sig_oid2.p, crt->sig_oid.len ) != 0 || + sig_params1.tag != sig_params2.tag || sig_params1.len != sig_params2.len || ( sig_params1.len != 0 && memcmp( sig_params1.p, sig_params2.p, sig_params1.len ) != 0 ) ) diff --git a/thirdparty/meshoptimizer/meshoptimizer.h b/thirdparty/meshoptimizer/meshoptimizer.h index a442d103c8..fde00f9c82 100644 --- a/thirdparty/meshoptimizer/meshoptimizer.h +++ b/thirdparty/meshoptimizer/meshoptimizer.h @@ -266,7 +266,10 @@ MESHOPTIMIZER_EXPERIMENTAL void meshopt_decodeFilterExp(void* buffer, size_t ver * destination must contain enough space for the *source* index buffer (since optimization is iterative, this means index_count elements - *not* target_index_count!) * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer */ -MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); +// -- GODOT start -- +//MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); +MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_resulting_error); +// -- GODOT end -- /** * Experimental: Mesh simplifier (sloppy) diff --git a/thirdparty/meshoptimizer/patches/simplifier_get_resulting_error.patch b/thirdparty/meshoptimizer/patches/simplifier_get_resulting_error.patch new file mode 100644 index 0000000000..1be38e45d2 --- /dev/null +++ b/thirdparty/meshoptimizer/patches/simplifier_get_resulting_error.patch @@ -0,0 +1,96 @@ +diff --git a/thirdparty/meshoptimizer/meshoptimizer.h b/thirdparty/meshoptimizer/meshoptimizer.h +index a442d103c8..fde00f9c82 100644 +--- a/thirdparty/meshoptimizer/meshoptimizer.h ++++ b/thirdparty/meshoptimizer/meshoptimizer.h +@@ -266,7 +266,10 @@ MESHOPTIMIZER_EXPERIMENTAL void meshopt_decodeFilterExp(void* buffer, size_t ver + * destination must contain enough space for the *source* index buffer (since optimization is iterative, this means index_count elements - *not* target_index_count!) + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + */ +-MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); ++// -- GODOT start -- ++//MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); ++MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_resulting_error); ++// -- GODOT end -- + + /** + * Experimental: Mesh simplifier (sloppy) +diff --git a/thirdparty/meshoptimizer/simplifier.cpp b/thirdparty/meshoptimizer/simplifier.cpp +index bd523275ce..51cf634186 100644 +--- a/thirdparty/meshoptimizer/simplifier.cpp ++++ b/thirdparty/meshoptimizer/simplifier.cpp +@@ -1143,7 +1143,10 @@ unsigned int* meshopt_simplifyDebugLoop = 0; + unsigned int* meshopt_simplifyDebugLoopBack = 0; + #endif + +-size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error) ++// -- GODOT start -- ++//size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error) ++size_t meshopt_simplify(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_resulting_error) ++// -- GODOT end -- + { + using namespace meshopt; + +@@ -1198,10 +1201,13 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, + if (result != indices) + memcpy(result, indices, index_count * sizeof(unsigned int)); + ++// -- GODOT start -- + #if TRACE + size_t pass_count = 0; +- float worst_error = 0; ++ //float worst_error = 0; + #endif ++ float worst_error = 0; ++// -- GODOT end -- + + Collapse* edge_collapses = allocator.allocate<Collapse>(index_count); + unsigned int* collapse_order = allocator.allocate<unsigned int>(index_count); +@@ -1213,6 +1219,12 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, + // target_error input is linear; we need to adjust it to match quadricError units + float error_limit = target_error * target_error; + ++// -- GODOT start -- ++ if (r_resulting_error) { ++ *r_resulting_error = 1.0; ++ } ++// -- GODOT end -- ++ + while (result_count > target_index_count) + { + size_t edge_collapse_count = pickEdgeCollapses(edge_collapses, result, result_count, remap, vertex_kind, loop); +@@ -1257,7 +1269,8 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, + size_t new_count = remapIndexBuffer(result, result_count, collapse_remap); + assert(new_count < result_count); + +-#if TRACE ++// -- GODOT start -- ++//#if TRACE + float pass_error = 0.f; + for (size_t i = 0; i < edge_collapse_count; ++i) + { +@@ -1267,15 +1280,24 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, + pass_error = c.error; + } + +- pass_count++; ++ //pass_count++; + worst_error = (worst_error < pass_error) ? pass_error : worst_error; + ++#if TRACE ++ pass_count++; + printf("pass %d: triangles: %d -> %d, collapses: %d/%d (goal: %d), error: %e (limit %e goal %e)\n", int(pass_count), int(result_count / 3), int(new_count / 3), int(collapses), int(edge_collapse_count), int(edge_collapse_goal), pass_error, error_limit, error_goal); + #endif ++// -- GODOT end -- + + result_count = new_count; + } + ++// -- GODOT start -- ++ if (r_resulting_error) { ++ *r_resulting_error = sqrt(worst_error); ++ } ++// -- GODOT end -- ++ + #if TRACE + printf("passes: %d, worst error: %e\n", int(pass_count), worst_error); + #endif diff --git a/thirdparty/meshoptimizer/simplifier.cpp b/thirdparty/meshoptimizer/simplifier.cpp index bd523275ce..b195a8cb5d 100644 --- a/thirdparty/meshoptimizer/simplifier.cpp +++ b/thirdparty/meshoptimizer/simplifier.cpp @@ -6,6 +6,7 @@ #include <math.h> #include <string.h> + #ifndef TRACE #define TRACE 0 #endif @@ -332,8 +333,11 @@ struct Vector3 { float x, y, z; }; +// -- GODOT start -- +//static void rescalePositions(Vector3* result, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride) +static float rescalePositions(Vector3* result, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride) +// -- GODOT end -- -static void rescalePositions(Vector3* result, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride) { size_t vertex_stride_float = vertex_positions_stride / sizeof(float); @@ -371,6 +375,10 @@ static void rescalePositions(Vector3* result, const float* vertex_positions_data result[i].y = (result[i].y - minv[1]) * scale; result[i].z = (result[i].z - minv[2]) * scale; } +// -- GODOT start -- + return extent; +// -- GODOT end -- + } struct Quadric @@ -1143,7 +1151,10 @@ unsigned int* meshopt_simplifyDebugLoop = 0; unsigned int* meshopt_simplifyDebugLoopBack = 0; #endif -size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error) +// -- GODOT start -- +//size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error) +size_t meshopt_simplify(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_resulting_error) +// -- GODOT end -- { using namespace meshopt; @@ -1187,7 +1198,10 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, #endif Vector3* vertex_positions = allocator.allocate<Vector3>(vertex_count); - rescalePositions(vertex_positions, vertex_positions_data, vertex_count, vertex_positions_stride); +// -- GODOT start -- + //rescalePositions(vertex_positions, vertex_positions_data, vertex_count, vertex_positions_stride); + float extent = rescalePositions(vertex_positions, vertex_positions_data, vertex_count, vertex_positions_stride); +// -- GODOT end -- Quadric* vertex_quadrics = allocator.allocate<Quadric>(vertex_count); memset(vertex_quadrics, 0, vertex_count * sizeof(Quadric)); @@ -1198,10 +1212,13 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, if (result != indices) memcpy(result, indices, index_count * sizeof(unsigned int)); +// -- GODOT start -- #if TRACE size_t pass_count = 0; - float worst_error = 0; + //float worst_error = 0; #endif + float worst_error = 0; +// -- GODOT end -- Collapse* edge_collapses = allocator.allocate<Collapse>(index_count); unsigned int* collapse_order = allocator.allocate<unsigned int>(index_count); @@ -1213,6 +1230,12 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, // target_error input is linear; we need to adjust it to match quadricError units float error_limit = target_error * target_error; +// -- GODOT start -- + if (r_resulting_error) { + *r_resulting_error = 1.0; + } +// -- GODOT end -- + while (result_count > target_index_count) { size_t edge_collapse_count = pickEdgeCollapses(edge_collapses, result, result_count, remap, vertex_kind, loop); @@ -1257,7 +1280,8 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t new_count = remapIndexBuffer(result, result_count, collapse_remap); assert(new_count < result_count); -#if TRACE +// -- GODOT start -- +//#if TRACE float pass_error = 0.f; for (size_t i = 0; i < edge_collapse_count; ++i) { @@ -1267,15 +1291,24 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, pass_error = c.error; } - pass_count++; + //pass_count++; worst_error = (worst_error < pass_error) ? pass_error : worst_error; +#if TRACE + pass_count++; printf("pass %d: triangles: %d -> %d, collapses: %d/%d (goal: %d), error: %e (limit %e goal %e)\n", int(pass_count), int(result_count / 3), int(new_count / 3), int(collapses), int(edge_collapse_count), int(edge_collapse_goal), pass_error, error_limit, error_goal); #endif +// -- GODOT end -- result_count = new_count; } +// -- GODOT start -- + if (r_resulting_error) { + *r_resulting_error = sqrt(worst_error) * extent; + } +// -- GODOT end -- + #if TRACE printf("passes: %d, worst error: %e\n", int(pass_count), worst_error); #endif diff --git a/thirdparty/rvo2/src/API.h b/thirdparty/rvo2/API.h index c64efb452c..c64efb452c 100644 --- a/thirdparty/rvo2/src/API.h +++ b/thirdparty/rvo2/API.h diff --git a/thirdparty/rvo2/src/Agent.cpp b/thirdparty/rvo2/Agent.cpp index 851d780758..851d780758 100644 --- a/thirdparty/rvo2/src/Agent.cpp +++ b/thirdparty/rvo2/Agent.cpp diff --git a/thirdparty/rvo2/src/Agent.h b/thirdparty/rvo2/Agent.h index 16f75a08f6..16f75a08f6 100644 --- a/thirdparty/rvo2/src/Agent.h +++ b/thirdparty/rvo2/Agent.h diff --git a/thirdparty/rvo2/src/Definitions.h b/thirdparty/rvo2/Definitions.h index a73aca9908..a73aca9908 100644 --- a/thirdparty/rvo2/src/Definitions.h +++ b/thirdparty/rvo2/Definitions.h diff --git a/thirdparty/rvo2/src/KdTree.cpp b/thirdparty/rvo2/KdTree.cpp index bc224614f0..bc224614f0 100644 --- a/thirdparty/rvo2/src/KdTree.cpp +++ b/thirdparty/rvo2/KdTree.cpp diff --git a/thirdparty/rvo2/src/KdTree.h b/thirdparty/rvo2/KdTree.h index 1dbad00ea4..1dbad00ea4 100644 --- a/thirdparty/rvo2/src/KdTree.h +++ b/thirdparty/rvo2/KdTree.h diff --git a/thirdparty/rvo2/src/Vector3.h b/thirdparty/rvo2/Vector3.h index 8c8835c865..8c8835c865 100644 --- a/thirdparty/rvo2/src/Vector3.h +++ b/thirdparty/rvo2/Vector3.h |
