diff options
Diffstat (limited to 'modules')
210 files changed, 34491 insertions, 324 deletions
diff --git a/modules/SCsub b/modules/SCsub index e16cc17b67..fea2f2eeb8 100644 --- a/modules/SCsub +++ b/modules/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * import os diff --git a/modules/astcenc/SCsub b/modules/astcenc/SCsub index 691c74b4a7..23e9fa87fc 100644 --- a/modules/astcenc/SCsub +++ b/modules/astcenc/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/basis_universal/SCsub b/modules/basis_universal/SCsub index 80bfd7e858..0142317e1e 100644 --- a/modules/basis_universal/SCsub +++ b/modules/basis_universal/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/bcdec/SCsub b/modules/bcdec/SCsub new file mode 100644 index 0000000000..32198eff96 --- /dev/null +++ b/modules/bcdec/SCsub @@ -0,0 +1,10 @@ +#!/usr/bin/env python +from misc.utility.scons_hints import * + +Import("env") +Import("env_modules") + +env_bcdec = env_modules.Clone() + +# Godot source files +env_bcdec.add_source_files(env.modules_sources, "*.cpp") diff --git a/modules/squish/config.py b/modules/bcdec/config.py index d22f9454ed..d22f9454ed 100644 --- a/modules/squish/config.py +++ b/modules/bcdec/config.py diff --git a/modules/bcdec/image_decompress_bcdec.cpp b/modules/bcdec/image_decompress_bcdec.cpp new file mode 100644 index 0000000000..30ca1fccb3 --- /dev/null +++ b/modules/bcdec/image_decompress_bcdec.cpp @@ -0,0 +1,181 @@ +/**************************************************************************/ +/* image_decompress_bcdec.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "image_decompress_bcdec.h" + +#include "core/os/os.h" +#include "core/string/print_string.h" + +#define BCDEC_IMPLEMENTATION +#include "thirdparty/misc/bcdec.h" + +inline void bcdec_bc6h_half_s(const void *compressedBlock, void *decompressedBlock, int destinationPitch) { + bcdec_bc6h_half(compressedBlock, decompressedBlock, destinationPitch, true); +} + +inline void bcdec_bc6h_half_u(const void *compressedBlock, void *decompressedBlock, int destinationPitch) { + bcdec_bc6h_half(compressedBlock, decompressedBlock, destinationPitch, false); +} + +static void decompress_image(BCdecFormat format, const void *src, void *dst, const uint64_t width, const uint64_t height) { + const uint8_t *src_blocks = reinterpret_cast<const uint8_t *>(src); + uint8_t *dec_blocks = reinterpret_cast<uint8_t *>(dst); + uint64_t src_pos = 0, dst_pos = 0; + +#define DECOMPRESS_LOOP(func, block_size, color_bytesize, color_components) \ + for (uint64_t y = 0; y < height; y += 4) { \ + for (uint64_t x = 0; x < width; x += 4) { \ + func(&src_blocks[src_pos], &dec_blocks[dst_pos], width *color_components); \ + src_pos += block_size; \ + dst_pos += 4 * color_bytesize; \ + } \ + dst_pos += 3 * width * color_bytesize; \ + } + + switch (format) { + case BCdec_BC1: { + DECOMPRESS_LOOP(bcdec_bc1, BCDEC_BC1_BLOCK_SIZE, 4, 4) + } break; + case BCdec_BC2: { + DECOMPRESS_LOOP(bcdec_bc2, BCDEC_BC2_BLOCK_SIZE, 4, 4) + } break; + case BCdec_BC3: { + DECOMPRESS_LOOP(bcdec_bc3, BCDEC_BC3_BLOCK_SIZE, 4, 4) + } break; + case BCdec_BC4: { + DECOMPRESS_LOOP(bcdec_bc4, BCDEC_BC4_BLOCK_SIZE, 1, 1) + } break; + case BCdec_BC5: { + DECOMPRESS_LOOP(bcdec_bc5, BCDEC_BC5_BLOCK_SIZE, 2, 2) + } break; + case BCdec_BC6U: { + DECOMPRESS_LOOP(bcdec_bc6h_half_u, BCDEC_BC6H_BLOCK_SIZE, 6, 3) + } break; + case BCdec_BC6S: { + DECOMPRESS_LOOP(bcdec_bc6h_half_s, BCDEC_BC6H_BLOCK_SIZE, 6, 3) + } break; + case BCdec_BC7: { + DECOMPRESS_LOOP(bcdec_bc7, BCDEC_BC7_BLOCK_SIZE, 4, 4) + } break; + } + +#undef DECOMPRESS_LOOP +} + +void image_decompress_bcdec(Image *p_image) { + uint64_t start_time = OS::get_singleton()->get_ticks_msec(); + + int w = p_image->get_width(); + int h = p_image->get_height(); + + Image::Format source_format = p_image->get_format(); + Image::Format target_format = Image::FORMAT_MAX; + + BCdecFormat bcdec_format = BCdec_BC1; + + switch (source_format) { + case Image::FORMAT_DXT1: + bcdec_format = BCdec_BC1; + target_format = Image::FORMAT_RGBA8; + break; + + case Image::FORMAT_DXT3: + bcdec_format = BCdec_BC2; + target_format = Image::FORMAT_RGBA8; + break; + + case Image::FORMAT_DXT5: + case Image::FORMAT_DXT5_RA_AS_RG: + bcdec_format = BCdec_BC3; + target_format = Image::FORMAT_RGBA8; + break; + + case Image::FORMAT_RGTC_R: + bcdec_format = BCdec_BC4; + target_format = Image::FORMAT_R8; + break; + + case Image::FORMAT_RGTC_RG: + bcdec_format = BCdec_BC5; + target_format = Image::FORMAT_RG8; + break; + + case Image::FORMAT_BPTC_RGBFU: + bcdec_format = BCdec_BC6U; + target_format = Image::FORMAT_RGBH; + break; + + case Image::FORMAT_BPTC_RGBF: + bcdec_format = BCdec_BC6S; + target_format = Image::FORMAT_RGBH; + break; + + case Image::FORMAT_BPTC_RGBA: + bcdec_format = BCdec_BC7; + target_format = Image::FORMAT_RGBA8; + break; + + default: + ERR_FAIL_MSG("bcdec: Can't decompress unknown format: " + Image::get_format_name(source_format) + "."); + break; + } + + int mm_count = p_image->get_mipmap_count(); + int64_t target_size = Image::get_image_data_size(w, h, target_format, p_image->has_mipmaps()); + + Vector<uint8_t> data; + data.resize(target_size); + + const uint8_t *rb = p_image->get_data().ptr(); + uint8_t *wb = data.ptrw(); + + // Decompress mipmaps. + for (int i = 0; i <= mm_count; i++) { + int64_t src_ofs = 0, mipmap_size = 0; + int mipmap_w = 0, mipmap_h = 0; + p_image->get_mipmap_offset_size_and_dimensions(i, src_ofs, mipmap_size, mipmap_w, mipmap_h); + + int64_t dst_ofs = Image::get_image_mipmap_offset(p_image->get_width(), p_image->get_height(), target_format, i); + decompress_image(bcdec_format, rb + src_ofs, wb + dst_ofs, mipmap_w, mipmap_h); + + w >>= 1; + h >>= 1; + } + + p_image->set_data(p_image->get_width(), p_image->get_height(), p_image->has_mipmaps(), target_format, data); + + // Swap channels if necessary. + if (source_format == Image::FORMAT_DXT5_RA_AS_RG) { + p_image->convert_ra_rgba8_to_rg(); + } + + print_verbose(vformat("bcdec: Decompression of a %dx%d %s image with %d mipmaps took %d ms.", + p_image->get_width(), p_image->get_height(), Image::get_format_name(source_format), p_image->get_mipmap_count(), OS::get_singleton()->get_ticks_msec() - start_time)); +} diff --git a/modules/bcdec/image_decompress_bcdec.h b/modules/bcdec/image_decompress_bcdec.h new file mode 100644 index 0000000000..b82ceed9a4 --- /dev/null +++ b/modules/bcdec/image_decompress_bcdec.h @@ -0,0 +1,49 @@ +/**************************************************************************/ +/* image_decompress_bcdec.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 IMAGE_DECOMPRESS_BCDEC_H +#define IMAGE_DECOMPRESS_BCDEC_H + +#include "core/io/image.h" + +enum BCdecFormat { + BCdec_BC1, + BCdec_BC2, + BCdec_BC3, + BCdec_BC4, + BCdec_BC5, + BCdec_BC6S, + BCdec_BC6U, + BCdec_BC7, +}; + +void image_decompress_bcdec(Image *p_image); + +#endif // IMAGE_DECOMPRESS_BCDEC_H diff --git a/modules/squish/register_types.cpp b/modules/bcdec/register_types.cpp index af7cf8f4f1..cbf9c0d383 100644 --- a/modules/squish/register_types.cpp +++ b/modules/bcdec/register_types.cpp @@ -30,17 +30,18 @@ #include "register_types.h" -#include "image_decompress_squish.h" +#include "image_decompress_bcdec.h" -void initialize_squish_module(ModuleInitializationLevel p_level) { +void initialize_bcdec_module(ModuleInitializationLevel p_level) { if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) { return; } - Image::_image_decompress_bc = image_decompress_squish; + Image::_image_decompress_bc = image_decompress_bcdec; + Image::_image_decompress_bptc = image_decompress_bcdec; } -void uninitialize_squish_module(ModuleInitializationLevel p_level) { +void uninitialize_bcdec_module(ModuleInitializationLevel p_level) { if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) { return; } diff --git a/modules/squish/register_types.h b/modules/bcdec/register_types.h index 1786b28ed3..eb721e3f2a 100644 --- a/modules/squish/register_types.h +++ b/modules/bcdec/register_types.h @@ -28,12 +28,12 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /**************************************************************************/ -#ifndef SQUISH_REGISTER_TYPES_H -#define SQUISH_REGISTER_TYPES_H +#ifndef BCDEC_REGISTER_TYPES_H +#define BCDEC_REGISTER_TYPES_H #include "modules/register_module_types.h" -void initialize_squish_module(ModuleInitializationLevel p_level); -void uninitialize_squish_module(ModuleInitializationLevel p_level); +void initialize_bcdec_module(ModuleInitializationLevel p_level); +void uninitialize_bcdec_module(ModuleInitializationLevel p_level); -#endif // SQUISH_REGISTER_TYPES_H +#endif // BCDEC_REGISTER_TYPES_H diff --git a/modules/betsy/SCsub b/modules/betsy/SCsub index ed5dcbf58b..2735116cc3 100644 --- a/modules/betsy/SCsub +++ b/modules/betsy/SCsub @@ -1,4 +1,6 @@ -# !/ usr / bin / env python +#!/usr/bin/env python +from misc.utility.scons_hints import * + Import("env") Import("env_modules") diff --git a/modules/bmp/SCsub b/modules/bmp/SCsub index 9d317887c3..cc3684b94b 100644 --- a/modules/bmp/SCsub +++ b/modules/bmp/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/camera/SCsub b/modules/camera/SCsub index 9a6147d433..aed5efd0d2 100644 --- a/modules/camera/SCsub +++ b/modules/camera/SCsub @@ -1,14 +1,21 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") env_camera = env_modules.Clone() -if env["platform"] == "windows": +if env["platform"] in ["windows", "macos", "linuxbsd"]: env_camera.add_source_files(env.modules_sources, "register_types.cpp") + +if env["platform"] == "windows": env_camera.add_source_files(env.modules_sources, "camera_win.cpp") elif env["platform"] == "macos": - env_camera.add_source_files(env.modules_sources, "register_types.cpp") env_camera.add_source_files(env.modules_sources, "camera_macos.mm") + +elif env["platform"] == "linuxbsd": + env_camera.add_source_files(env.modules_sources, "camera_linux.cpp") + env_camera.add_source_files(env.modules_sources, "camera_feed_linux.cpp") + env_camera.add_source_files(env.modules_sources, "buffer_decoder.cpp") diff --git a/modules/camera/buffer_decoder.cpp b/modules/camera/buffer_decoder.cpp new file mode 100644 index 0000000000..85cfea242c --- /dev/null +++ b/modules/camera/buffer_decoder.cpp @@ -0,0 +1,212 @@ +/**************************************************************************/ +/* buffer_decoder.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "buffer_decoder.h" + +#include "servers/camera/camera_feed.h" + +#include <linux/videodev2.h> + +BufferDecoder::BufferDecoder(CameraFeed *p_camera_feed) { + camera_feed = p_camera_feed; + width = camera_feed->get_format().width; + height = camera_feed->get_format().height; + image.instantiate(); +} + +AbstractYuyvBufferDecoder::AbstractYuyvBufferDecoder(CameraFeed *p_camera_feed) : + BufferDecoder(p_camera_feed) { + switch (camera_feed->get_format().pixel_format) { + case V4L2_PIX_FMT_YYUV: + component_indexes = new int[4]{ 0, 1, 2, 3 }; + break; + case V4L2_PIX_FMT_YVYU: + component_indexes = new int[4]{ 0, 2, 3, 1 }; + break; + case V4L2_PIX_FMT_UYVY: + component_indexes = new int[4]{ 1, 3, 0, 2 }; + break; + case V4L2_PIX_FMT_VYUY: + component_indexes = new int[4]{ 1, 3, 2, 0 }; + break; + default: + component_indexes = new int[4]{ 0, 2, 1, 3 }; + } +} + +AbstractYuyvBufferDecoder::~AbstractYuyvBufferDecoder() { + delete[] component_indexes; +} + +SeparateYuyvBufferDecoder::SeparateYuyvBufferDecoder(CameraFeed *p_camera_feed) : + AbstractYuyvBufferDecoder(p_camera_feed) { + y_image_data.resize(width * height); + cbcr_image_data.resize(width * height); + y_image.instantiate(); + cbcr_image.instantiate(); +} + +void SeparateYuyvBufferDecoder::decode(StreamingBuffer p_buffer) { + uint8_t *y_dst = (uint8_t *)y_image_data.ptrw(); + uint8_t *uv_dst = (uint8_t *)cbcr_image_data.ptrw(); + uint8_t *src = (uint8_t *)p_buffer.start; + uint8_t *y0_src = src + component_indexes[0]; + uint8_t *y1_src = src + component_indexes[1]; + uint8_t *u_src = src + component_indexes[2]; + uint8_t *v_src = src + component_indexes[3]; + + for (int i = 0; i < width * height; i += 2) { + *y_dst++ = *y0_src; + *y_dst++ = *y1_src; + *uv_dst++ = *u_src; + *uv_dst++ = *v_src; + + y0_src += 4; + y1_src += 4; + u_src += 4; + v_src += 4; + } + + if (y_image.is_valid()) { + y_image->set_data(width, height, false, Image::FORMAT_L8, y_image_data); + } else { + y_image.instantiate(width, height, false, Image::FORMAT_RGB8, y_image_data); + } + if (cbcr_image.is_valid()) { + cbcr_image->set_data(width, height, false, Image::FORMAT_L8, cbcr_image_data); + } else { + cbcr_image.instantiate(width, height, false, Image::FORMAT_RGB8, cbcr_image_data); + } + + camera_feed->set_ycbcr_images(y_image, cbcr_image); +} + +YuyvToGrayscaleBufferDecoder::YuyvToGrayscaleBufferDecoder(CameraFeed *p_camera_feed) : + AbstractYuyvBufferDecoder(p_camera_feed) { + image_data.resize(width * height); +} + +void YuyvToGrayscaleBufferDecoder::decode(StreamingBuffer p_buffer) { + uint8_t *dst = (uint8_t *)image_data.ptrw(); + uint8_t *src = (uint8_t *)p_buffer.start; + uint8_t *y0_src = src + component_indexes[0]; + uint8_t *y1_src = src + component_indexes[1]; + + for (int i = 0; i < width * height; i += 2) { + *dst++ = *y0_src; + *dst++ = *y1_src; + + y0_src += 4; + y1_src += 4; + } + + if (image.is_valid()) { + image->set_data(width, height, false, Image::FORMAT_L8, image_data); + } else { + image.instantiate(width, height, false, Image::FORMAT_RGB8, image_data); + } + + camera_feed->set_rgb_image(image); +} + +YuyvToRgbBufferDecoder::YuyvToRgbBufferDecoder(CameraFeed *p_camera_feed) : + AbstractYuyvBufferDecoder(p_camera_feed) { + image_data.resize(width * height * 3); +} + +void YuyvToRgbBufferDecoder::decode(StreamingBuffer p_buffer) { + uint8_t *src = (uint8_t *)p_buffer.start; + uint8_t *y0_src = src + component_indexes[0]; + uint8_t *y1_src = src + component_indexes[1]; + uint8_t *u_src = src + component_indexes[2]; + uint8_t *v_src = src + component_indexes[3]; + uint8_t *dst = (uint8_t *)image_data.ptrw(); + + for (int i = 0; i < width * height; i += 2) { + int u = *u_src; + int v = *v_src; + int u1 = (((u - 128) << 7) + (u - 128)) >> 6; + int rg = (((u - 128) << 1) + (u - 128) + ((v - 128) << 2) + ((v - 128) << 1)) >> 3; + int v1 = (((v - 128) << 1) + (v - 128)) >> 1; + + *dst++ = CLAMP(*y0_src + v1, 0, 255); + *dst++ = CLAMP(*y0_src - rg, 0, 255); + *dst++ = CLAMP(*y0_src + u1, 0, 255); + + *dst++ = CLAMP(*y1_src + v1, 0, 255); + *dst++ = CLAMP(*y1_src - rg, 0, 255); + *dst++ = CLAMP(*y1_src + u1, 0, 255); + + y0_src += 4; + y1_src += 4; + u_src += 4; + v_src += 4; + } + + if (image.is_valid()) { + image->set_data(width, height, false, Image::FORMAT_RGB8, image_data); + } else { + image.instantiate(width, height, false, Image::FORMAT_RGB8, image_data); + } + + camera_feed->set_rgb_image(image); +} + +CopyBufferDecoder::CopyBufferDecoder(CameraFeed *p_camera_feed, bool p_rgba) : + BufferDecoder(p_camera_feed) { + rgba = p_rgba; + image_data.resize(width * height * (rgba ? 4 : 2)); +} + +void CopyBufferDecoder::decode(StreamingBuffer p_buffer) { + uint8_t *dst = (uint8_t *)image_data.ptrw(); + memcpy(dst, p_buffer.start, p_buffer.length); + + if (image.is_valid()) { + image->set_data(width, height, false, rgba ? Image::FORMAT_RGBA8 : Image::FORMAT_LA8, image_data); + } else { + image.instantiate(width, height, false, rgba ? Image::FORMAT_RGBA8 : Image::FORMAT_LA8, image_data); + } + + camera_feed->set_rgb_image(image); +} + +JpegBufferDecoder::JpegBufferDecoder(CameraFeed *p_camera_feed) : + BufferDecoder(p_camera_feed) { +} + +void JpegBufferDecoder::decode(StreamingBuffer p_buffer) { + image_data.resize(p_buffer.length); + uint8_t *dst = (uint8_t *)image_data.ptrw(); + memcpy(dst, p_buffer.start, p_buffer.length); + if (image->load_jpg_from_buffer(image_data) == OK) { + camera_feed->set_rgb_image(image); + } +} diff --git a/modules/camera/buffer_decoder.h b/modules/camera/buffer_decoder.h new file mode 100644 index 0000000000..97cc66b6da --- /dev/null +++ b/modules/camera/buffer_decoder.h @@ -0,0 +1,116 @@ +/**************************************************************************/ +/* buffer_decoder.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 BUFFER_DECODER_H +#define BUFFER_DECODER_H + +#include "core/io/image.h" +#include "core/templates/vector.h" + +class CameraFeed; + +struct StreamingBuffer { + void *start = nullptr; + size_t length = 0; +}; + +class BufferDecoder { +protected: + CameraFeed *camera_feed = nullptr; + Ref<Image> image; + int width = 0; + int height = 0; + +public: + virtual void decode(StreamingBuffer p_buffer) = 0; + + BufferDecoder(CameraFeed *p_camera_feed); + virtual ~BufferDecoder() {} +}; + +class AbstractYuyvBufferDecoder : public BufferDecoder { +protected: + int *component_indexes = nullptr; + +public: + AbstractYuyvBufferDecoder(CameraFeed *p_camera_feed); + ~AbstractYuyvBufferDecoder(); +}; + +class SeparateYuyvBufferDecoder : public AbstractYuyvBufferDecoder { +private: + Vector<uint8_t> y_image_data; + Vector<uint8_t> cbcr_image_data; + Ref<Image> y_image; + Ref<Image> cbcr_image; + +public: + SeparateYuyvBufferDecoder(CameraFeed *p_camera_feed); + virtual void decode(StreamingBuffer p_buffer) override; +}; + +class YuyvToGrayscaleBufferDecoder : public AbstractYuyvBufferDecoder { +private: + Vector<uint8_t> image_data; + +public: + YuyvToGrayscaleBufferDecoder(CameraFeed *p_camera_feed); + virtual void decode(StreamingBuffer p_buffer) override; +}; + +class YuyvToRgbBufferDecoder : public AbstractYuyvBufferDecoder { +private: + Vector<uint8_t> image_data; + +public: + YuyvToRgbBufferDecoder(CameraFeed *p_camera_feed); + virtual void decode(StreamingBuffer p_buffer) override; +}; + +class CopyBufferDecoder : public BufferDecoder { +private: + Vector<uint8_t> image_data; + bool rgba = false; + +public: + CopyBufferDecoder(CameraFeed *p_camera_feed, bool p_rgba); + virtual void decode(StreamingBuffer p_buffer) override; +}; + +class JpegBufferDecoder : public BufferDecoder { +private: + Vector<uint8_t> image_data; + +public: + JpegBufferDecoder(CameraFeed *p_camera_feed); + virtual void decode(StreamingBuffer p_buffer) override; +}; + +#endif // BUFFER_DECODER_H diff --git a/modules/camera/camera_feed_linux.cpp b/modules/camera/camera_feed_linux.cpp new file mode 100644 index 0000000000..94bb2b6ad3 --- /dev/null +++ b/modules/camera/camera_feed_linux.cpp @@ -0,0 +1,363 @@ +/**************************************************************************/ +/* camera_feed_linux.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "camera_feed_linux.h" + +#include <fcntl.h> +#include <sys/ioctl.h> +#include <sys/mman.h> +#include <unistd.h> + +void CameraFeedLinux::update_buffer_thread_func(void *p_func) { + if (p_func) { + CameraFeedLinux *camera_feed_linux = (CameraFeedLinux *)p_func; + camera_feed_linux->_update_buffer(); + } +} + +void CameraFeedLinux::_update_buffer() { + while (!exit_flag.is_set()) { + _read_frame(); + usleep(10000); + } +} + +void CameraFeedLinux::_query_device(const String &p_device_name) { + file_descriptor = open(p_device_name.ascii(), O_RDWR | O_NONBLOCK, 0); + ERR_FAIL_COND_MSG(file_descriptor == -1, vformat("Cannot open file descriptor for %s. Error: %d.", p_device_name, errno)); + + struct v4l2_capability capability; + if (ioctl(file_descriptor, VIDIOC_QUERYCAP, &capability) == -1) { + ERR_FAIL_MSG(vformat("Cannot query device. Error: %d.", errno)); + } + name = String((char *)capability.card); + + for (int index = 0;; index++) { + struct v4l2_fmtdesc fmtdesc; + memset(&fmtdesc, 0, sizeof(fmtdesc)); + fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + fmtdesc.index = index; + + if (ioctl(file_descriptor, VIDIOC_ENUM_FMT, &fmtdesc) == -1) { + break; + } + + for (int res_index = 0;; res_index++) { + struct v4l2_frmsizeenum frmsizeenum; + memset(&frmsizeenum, 0, sizeof(frmsizeenum)); + frmsizeenum.pixel_format = fmtdesc.pixelformat; + frmsizeenum.index = res_index; + + if (ioctl(file_descriptor, VIDIOC_ENUM_FRAMESIZES, &frmsizeenum) == -1) { + break; + } + + for (int framerate_index = 0;; framerate_index++) { + struct v4l2_frmivalenum frmivalenum; + memset(&frmivalenum, 0, sizeof(frmivalenum)); + frmivalenum.pixel_format = fmtdesc.pixelformat; + frmivalenum.width = frmsizeenum.discrete.width; + frmivalenum.height = frmsizeenum.discrete.height; + frmivalenum.index = framerate_index; + + if (ioctl(file_descriptor, VIDIOC_ENUM_FRAMEINTERVALS, &frmivalenum) == -1) { + if (framerate_index == 0) { + _add_format(fmtdesc, frmsizeenum.discrete, -1, 1); + } + break; + } + + _add_format(fmtdesc, frmsizeenum.discrete, frmivalenum.discrete.numerator, frmivalenum.discrete.denominator); + } + } + } + + close(file_descriptor); +} + +void CameraFeedLinux::_add_format(v4l2_fmtdesc p_description, v4l2_frmsize_discrete p_size, int p_frame_numerator, int p_frame_denominator) { + FeedFormat feed_format; + feed_format.width = p_size.width; + feed_format.height = p_size.height; + feed_format.format = String((char *)p_description.description); + feed_format.frame_numerator = p_frame_numerator; + feed_format.frame_denominator = p_frame_denominator; + feed_format.pixel_format = p_description.pixelformat; + print_verbose(vformat("%s %dx%d@%d/%dfps", (char *)p_description.description, p_size.width, p_size.height, p_frame_denominator, p_frame_numerator)); + formats.push_back(feed_format); +} + +bool CameraFeedLinux::_request_buffers() { + struct v4l2_requestbuffers requestbuffers; + + memset(&requestbuffers, 0, sizeof(requestbuffers)); + requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + requestbuffers.memory = V4L2_MEMORY_MMAP; + requestbuffers.count = 4; + + if (ioctl(file_descriptor, VIDIOC_REQBUFS, &requestbuffers) == -1) { + ERR_FAIL_V_MSG(false, vformat("ioctl(VIDIOC_REQBUFS) error: %d.", errno)); + } + + ERR_FAIL_COND_V_MSG(requestbuffers.count < 2, false, "Not enough buffers granted."); + + buffer_count = requestbuffers.count; + buffers = new StreamingBuffer[buffer_count]; + + for (unsigned int i = 0; i < buffer_count; i++) { + struct v4l2_buffer buffer; + + memset(&buffer, 0, sizeof(buffer)); + buffer.type = requestbuffers.type; + buffer.memory = V4L2_MEMORY_MMAP; + buffer.index = i; + + if (ioctl(file_descriptor, VIDIOC_QUERYBUF, &buffer) == -1) { + delete[] buffers; + ERR_FAIL_V_MSG(false, vformat("ioctl(VIDIOC_QUERYBUF) error: %d.", errno)); + } + + buffers[i].length = buffer.length; + buffers[i].start = mmap(NULL, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, file_descriptor, buffer.m.offset); + + if (buffers[i].start == MAP_FAILED) { + for (unsigned int b = 0; b < i; b++) { + _unmap_buffers(i); + } + delete[] buffers; + ERR_FAIL_V_MSG(false, "Mapping buffers failed."); + } + } + + return true; +} + +bool CameraFeedLinux::_start_capturing() { + for (unsigned int i = 0; i < buffer_count; i++) { + struct v4l2_buffer buffer; + + memset(&buffer, 0, sizeof(buffer)); + buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buffer.memory = V4L2_MEMORY_MMAP; + buffer.index = i; + + if (ioctl(file_descriptor, VIDIOC_QBUF, &buffer) == -1) { + ERR_FAIL_V_MSG(false, vformat("ioctl(VIDIOC_QBUF) error: %d.", errno)); + } + } + + enum v4l2_buf_type type; + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + if (ioctl(file_descriptor, VIDIOC_STREAMON, &type) == -1) { + ERR_FAIL_V_MSG(false, vformat("ioctl(VIDIOC_STREAMON) error: %d.", errno)); + } + + return true; +} + +void CameraFeedLinux::_read_frame() { + struct v4l2_buffer buffer; + memset(&buffer, 0, sizeof(buffer)); + buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buffer.memory = V4L2_MEMORY_MMAP; + + if (ioctl(file_descriptor, VIDIOC_DQBUF, &buffer) == -1) { + if (errno != EAGAIN) { + print_error(vformat("ioctl(VIDIOC_DQBUF) error: %d.", errno)); + exit_flag.set(); + } + return; + } + + buffer_decoder->decode(buffers[buffer.index]); + + if (ioctl(file_descriptor, VIDIOC_QBUF, &buffer) == -1) { + print_error(vformat("ioctl(VIDIOC_QBUF) error: %d.", errno)); + } + + emit_signal(SNAME("frame_changed")); +} + +void CameraFeedLinux::_stop_capturing() { + enum v4l2_buf_type type; + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + if (ioctl(file_descriptor, VIDIOC_STREAMOFF, &type) == -1) { + print_error(vformat("ioctl(VIDIOC_STREAMOFF) error: %d.", errno)); + } +} + +void CameraFeedLinux::_unmap_buffers(unsigned int p_count) { + for (unsigned int i = 0; i < p_count; i++) { + munmap(buffers[i].start, buffers[i].length); + } +} + +void CameraFeedLinux::_start_thread() { + exit_flag.clear(); + thread = memnew(Thread); + thread->start(CameraFeedLinux::update_buffer_thread_func, this); +} + +String CameraFeedLinux::get_device_name() const { + return device_name; +} + +bool CameraFeedLinux::activate_feed() { + ERR_FAIL_COND_V_MSG(selected_format == -1, false, "CameraFeed format needs to be set before activating."); + file_descriptor = open(device_name.ascii(), O_RDWR | O_NONBLOCK, 0); + if (_request_buffers() && _start_capturing()) { + buffer_decoder = _create_buffer_decoder(); + _start_thread(); + return true; + } + ERR_FAIL_V_MSG(false, "Could not activate feed."); +} + +BufferDecoder *CameraFeedLinux::_create_buffer_decoder() { + switch (formats[selected_format].pixel_format) { + case V4L2_PIX_FMT_MJPEG: + case V4L2_PIX_FMT_JPEG: + return memnew(JpegBufferDecoder(this)); + case V4L2_PIX_FMT_YUYV: + case V4L2_PIX_FMT_YYUV: + case V4L2_PIX_FMT_YVYU: + case V4L2_PIX_FMT_UYVY: + case V4L2_PIX_FMT_VYUY: { + String output = parameters["output"]; + if (output == "separate") { + return memnew(SeparateYuyvBufferDecoder(this)); + } + if (output == "grayscale") { + return memnew(YuyvToGrayscaleBufferDecoder(this)); + } + if (output == "copy") { + return memnew(CopyBufferDecoder(this, false)); + } + return memnew(YuyvToRgbBufferDecoder(this)); + } + default: + return memnew(CopyBufferDecoder(this, true)); + } +} + +void CameraFeedLinux::deactivate_feed() { + exit_flag.set(); + thread->wait_to_finish(); + memdelete(thread); + _stop_capturing(); + _unmap_buffers(buffer_count); + delete[] buffers; + memdelete(buffer_decoder); + for (int i = 0; i < CameraServer::FEED_IMAGES; i++) { + RID placeholder = RenderingServer::get_singleton()->texture_2d_placeholder_create(); + RenderingServer::get_singleton()->texture_replace(texture[i], placeholder); + } + base_width = 0; + base_height = 0; + close(file_descriptor); + + emit_signal(SNAME("format_changed")); +} + +Array CameraFeedLinux::get_formats() const { + Array result; + for (const FeedFormat &format : formats) { + Dictionary dictionary; + dictionary["width"] = format.width; + dictionary["height"] = format.height; + dictionary["format"] = format.format; + dictionary["frame_numerator"] = format.frame_numerator; + dictionary["frame_denominator"] = format.frame_denominator; + result.push_back(dictionary); + } + return result; +} + +CameraFeed::FeedFormat CameraFeedLinux::get_format() const { + FeedFormat feed_format = {}; + return selected_format == -1 ? feed_format : formats[selected_format]; +} + +bool CameraFeedLinux::set_format(int p_index, const Dictionary &p_parameters) { + ERR_FAIL_COND_V_MSG(active, false, "Feed is active."); + ERR_FAIL_INDEX_V_MSG(p_index, formats.size(), false, "Invalid format index."); + + FeedFormat feed_format = formats[p_index]; + + file_descriptor = open(device_name.ascii(), O_RDWR | O_NONBLOCK, 0); + + struct v4l2_format format; + memset(&format, 0, sizeof(format)); + format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + format.fmt.pix.width = feed_format.width; + format.fmt.pix.height = feed_format.height; + format.fmt.pix.pixelformat = feed_format.pixel_format; + + if (ioctl(file_descriptor, VIDIOC_S_FMT, &format) == -1) { + close(file_descriptor); + ERR_FAIL_V_MSG(false, vformat("Cannot set format, error: %d.", errno)); + } + + if (feed_format.frame_numerator > 0) { + struct v4l2_streamparm param; + memset(¶m, 0, sizeof(param)); + + param.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + param.parm.capture.capability = V4L2_CAP_TIMEPERFRAME; + param.parm.capture.timeperframe.numerator = feed_format.frame_numerator; + param.parm.capture.timeperframe.denominator = feed_format.frame_denominator; + + if (ioctl(file_descriptor, VIDIOC_S_PARM, ¶m) == -1) { + close(file_descriptor); + ERR_FAIL_V_MSG(false, vformat("Cannot set framerate, error: %d.", errno)); + } + } + close(file_descriptor); + + parameters = p_parameters.duplicate(); + selected_format = p_index; + emit_signal(SNAME("format_changed")); + + return true; +} + +CameraFeedLinux::CameraFeedLinux(const String &p_device_name) : + CameraFeed() { + device_name = p_device_name; + _query_device(device_name); +} + +CameraFeedLinux::~CameraFeedLinux() { + if (is_active()) { + deactivate_feed(); + } +} diff --git a/modules/camera/camera_feed_linux.h b/modules/camera/camera_feed_linux.h new file mode 100644 index 0000000000..bf29201c99 --- /dev/null +++ b/modules/camera/camera_feed_linux.h @@ -0,0 +1,78 @@ +/**************************************************************************/ +/* camera_feed_linux.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 CAMERA_FEED_LINUX_H +#define CAMERA_FEED_LINUX_H + +#include "buffer_decoder.h" + +#include "core/os/thread.h" +#include "servers/camera/camera_feed.h" + +#include <linux/videodev2.h> + +struct StreamingBuffer; + +class CameraFeedLinux : public CameraFeed { +private: + SafeFlag exit_flag; + Thread *thread = nullptr; + String device_name; + int file_descriptor = -1; + StreamingBuffer *buffers = nullptr; + unsigned int buffer_count = 0; + BufferDecoder *buffer_decoder = nullptr; + + static void update_buffer_thread_func(void *p_func); + + void _update_buffer(); + void _query_device(const String &p_device_name); + void _add_format(v4l2_fmtdesc description, v4l2_frmsize_discrete size, int frame_numerator, int frame_denominator); + bool _request_buffers(); + bool _start_capturing(); + void _read_frame(); + void _stop_capturing(); + void _unmap_buffers(unsigned int p_count); + BufferDecoder *_create_buffer_decoder(); + void _start_thread(); + +public: + String get_device_name() const; + bool activate_feed(); + void deactivate_feed(); + bool set_format(int p_index, const Dictionary &p_parameters); + Array get_formats() const; + FeedFormat get_format() const; + + CameraFeedLinux(const String &p_device_name); + virtual ~CameraFeedLinux(); +}; + +#endif // CAMERA_FEED_LINUX_H diff --git a/modules/camera/camera_linux.cpp b/modules/camera/camera_linux.cpp new file mode 100644 index 0000000000..0cfb6b7b9e --- /dev/null +++ b/modules/camera/camera_linux.cpp @@ -0,0 +1,169 @@ +/**************************************************************************/ +/* camera_linux.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "camera_linux.h" + +#include "camera_feed_linux.h" + +#include <dirent.h> +#include <fcntl.h> +#include <sys/ioctl.h> +#include <sys/stat.h> +#include <unistd.h> + +void CameraLinux::camera_thread_func(void *p_camera_linux) { + if (p_camera_linux) { + CameraLinux *camera_linux = (CameraLinux *)p_camera_linux; + camera_linux->_update_devices(); + } +} + +void CameraLinux::_update_devices() { + while (!exit_flag.is_set()) { + { + MutexLock lock(camera_mutex); + + for (int i = feeds.size() - 1; i >= 0; i--) { + Ref<CameraFeedLinux> feed = (Ref<CameraFeedLinux>)feeds[i]; + String device_name = feed->get_device_name(); + if (!_is_active(device_name)) { + remove_feed(feed); + } + } + + DIR *devices = opendir("/dev"); + + if (devices) { + struct dirent *device; + + while ((device = readdir(devices)) != nullptr) { + if (strncmp(device->d_name, "video", 5) != 0) { + continue; + } + String device_name = String("/dev/") + String(device->d_name); + if (!_has_device(device_name)) { + _add_device(device_name); + } + } + } + + closedir(devices); + } + + usleep(1000000); + } +} + +bool CameraLinux::_has_device(const String &p_device_name) { + for (int i = 0; i < feeds.size(); i++) { + Ref<CameraFeedLinux> feed = (Ref<CameraFeedLinux>)feeds[i]; + if (feed->get_device_name() == p_device_name) { + return true; + } + } + return false; +} + +void CameraLinux::_add_device(const String &p_device_name) { + int file_descriptor = _open_device(p_device_name); + + if (file_descriptor != -1) { + if (_is_video_capture_device(file_descriptor)) { + Ref<CameraFeedLinux> feed = memnew(CameraFeedLinux(p_device_name)); + add_feed(feed); + } + } + + close(file_descriptor); +} + +int CameraLinux::_open_device(const String &p_device_name) { + struct stat s; + + if (stat(p_device_name.ascii(), &s) == -1) { + return -1; + } + + if (!S_ISCHR(s.st_mode)) { + return -1; + } + + return open(p_device_name.ascii(), O_RDWR | O_NONBLOCK, 0); +} + +// TODO any cheaper/cleaner way to check if file descriptor is invalid? +bool CameraLinux::_is_active(const String &p_device_name) { + struct v4l2_capability capability; + bool result = false; + int file_descriptor = _open_device(p_device_name); + if (file_descriptor != -1 && ioctl(file_descriptor, VIDIOC_QUERYCAP, &capability) != -1) { + result = true; + } + close(file_descriptor); + return result; +} + +bool CameraLinux::_is_video_capture_device(int p_file_descriptor) { + struct v4l2_capability capability; + + if (ioctl(p_file_descriptor, VIDIOC_QUERYCAP, &capability) == -1) { + print_verbose("Cannot query device"); + return false; + } + + if (!(capability.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { + print_verbose(vformat("%s is no video capture device\n", String((char *)capability.card))); + return false; + } + + if (!(capability.capabilities & V4L2_CAP_STREAMING)) { + print_verbose(vformat("%s does not support streaming", String((char *)capability.card))); + return false; + } + + return _can_query_format(p_file_descriptor, V4L2_BUF_TYPE_VIDEO_CAPTURE); +} + +bool CameraLinux::_can_query_format(int p_file_descriptor, int p_type) { + struct v4l2_format format; + memset(&format, 0, sizeof(format)); + format.type = p_type; + + return ioctl(p_file_descriptor, VIDIOC_G_FMT, &format) != -1; +} + +CameraLinux::CameraLinux() { + camera_thread.start(CameraLinux::camera_thread_func, this); +}; + +CameraLinux::~CameraLinux() { + exit_flag.set(); + camera_thread.wait_to_finish(); +} diff --git a/modules/camera/camera_linux.h b/modules/camera/camera_linux.h new file mode 100644 index 0000000000..66f6aa0ffb --- /dev/null +++ b/modules/camera/camera_linux.h @@ -0,0 +1,60 @@ +/**************************************************************************/ +/* camera_linux.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 CAMERA_LINUX_H +#define CAMERA_LINUX_H + +#include "core/os/mutex.h" +#include "core/os/thread.h" +#include "servers/camera_server.h" + +class CameraLinux : public CameraServer { +private: + SafeFlag exit_flag; + Thread camera_thread; + Mutex camera_mutex; + + static void camera_thread_func(void *p_camera_linux); + + void _update_devices(); + bool _has_device(const String &p_device_name); + void _add_device(const String &p_device_name); + void _remove_device(const String &p_device_name); + int _open_device(const String &p_device_name); + bool _is_active(const String &p_device_name); + bool _is_video_capture_device(int p_file_descriptor); + bool _can_query_format(int p_file_descriptor, int p_type); + +public: + CameraLinux(); + ~CameraLinux(); +}; + +#endif // CAMERA_LINUX_H diff --git a/modules/camera/camera_macos.mm b/modules/camera/camera_macos.mm index 578a1d6325..de4f814846 100644 --- a/modules/camera/camera_macos.mm +++ b/modules/camera/camera_macos.mm @@ -182,7 +182,7 @@ } // set our texture... - feed->set_YCbCr_imgs(img[0], img[1]); + feed->set_ycbcr_images(img[0], img[1]); } // and unlock diff --git a/modules/camera/config.py b/modules/camera/config.py index d2b2542dd9..7b368d2193 100644 --- a/modules/camera/config.py +++ b/modules/camera/config.py @@ -1,5 +1,5 @@ def can_build(env, platform): - return platform == "macos" or platform == "windows" + return platform == "macos" or platform == "windows" or platform == "linuxbsd" def configure(env): diff --git a/modules/camera/register_types.cpp b/modules/camera/register_types.cpp index feee6769f8..666ea8ba65 100644 --- a/modules/camera/register_types.cpp +++ b/modules/camera/register_types.cpp @@ -30,6 +30,9 @@ #include "register_types.h" +#if defined(LINUXBSD_ENABLED) +#include "camera_linux.h" +#endif #if defined(WINDOWS_ENABLED) #include "camera_win.h" #endif @@ -42,6 +45,9 @@ void initialize_camera_module(ModuleInitializationLevel p_level) { return; } +#if defined(LINUXBSD_ENABLED) + CameraServer::make_default<CameraLinux>(); +#endif #if defined(WINDOWS_ENABLED) CameraServer::make_default<CameraWindows>(); #endif diff --git a/modules/csg/SCsub b/modules/csg/SCsub index 1cf9974fc1..f71618ab22 100644 --- a/modules/csg/SCsub +++ b/modules/csg/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/csg/register_types.cpp b/modules/csg/register_types.cpp index de949e30d8..19e6be4be1 100644 --- a/modules/csg/register_types.cpp +++ b/modules/csg/register_types.cpp @@ -30,8 +30,6 @@ #include "register_types.h" -#ifndef _3D_DISABLED - #include "csg_shape.h" #ifdef TOOLS_ENABLED @@ -62,5 +60,3 @@ void uninitialize_csg_module(ModuleInitializationLevel p_level) { return; } } - -#endif // _3D_DISABLED diff --git a/modules/cvtt/SCsub b/modules/cvtt/SCsub index 1d5a7ff6a3..44e56ab6a7 100644 --- a/modules/cvtt/SCsub +++ b/modules/cvtt/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/cvtt/register_types.cpp b/modules/cvtt/register_types.cpp index 211d419349..80e3062d04 100644 --- a/modules/cvtt/register_types.cpp +++ b/modules/cvtt/register_types.cpp @@ -40,7 +40,6 @@ void initialize_cvtt_module(ModuleInitializationLevel p_level) { } Image::set_compress_bptc_func(image_compress_cvtt); - Image::_image_decompress_bptc = image_decompress_cvtt; } void uninitialize_cvtt_module(ModuleInitializationLevel p_level) { diff --git a/modules/dds/SCsub b/modules/dds/SCsub index 06980bd670..d1c67c31ea 100644 --- a/modules/dds/SCsub +++ b/modules/dds/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/enet/SCsub b/modules/enet/SCsub index 580e5a3eb0..0c31638e46 100644 --- a/modules/enet/SCsub +++ b/modules/enet/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/enet/doc_classes/ENetPacketPeer.xml b/modules/enet/doc_classes/ENetPacketPeer.xml index 3171da1f6d..659cea974c 100644 --- a/modules/enet/doc_classes/ENetPacketPeer.xml +++ b/modules/enet/doc_classes/ENetPacketPeer.xml @@ -18,6 +18,12 @@ Returns the number of channels allocated for communication with peer. </description> </method> + <method name="get_packet_flags" qualifiers="const"> + <return type="int" /> + <description> + Returns the ENet flags of the next packet in the received queue. See [code]FLAG_*[/code] constants for available packet flags. Note that not all flags are replicated from the sending peer to the receiving peer. + </description> + </method> <method name="get_remote_address" qualifiers="const"> <return type="String" /> <description> diff --git a/modules/enet/enet_packet_peer.cpp b/modules/enet/enet_packet_peer.cpp index edb33fc96b..9ec68465a5 100644 --- a/modules/enet/enet_packet_peer.cpp +++ b/modules/enet/enet_packet_peer.cpp @@ -175,6 +175,11 @@ int ENetPacketPeer::get_channels() const { return peer->channelCount; } +int ENetPacketPeer::get_packet_flags() const { + ERR_FAIL_COND_V(packet_queue.is_empty(), 0); + return packet_queue.front()->get()->flags; +} + void ENetPacketPeer::_on_disconnect() { if (peer) { peer->data = nullptr; @@ -206,6 +211,7 @@ void ENetPacketPeer::_bind_methods() { ClassDB::bind_method(D_METHOD("send", "channel", "packet", "flags"), &ENetPacketPeer::_send); ClassDB::bind_method(D_METHOD("throttle_configure", "interval", "acceleration", "deceleration"), &ENetPacketPeer::throttle_configure); ClassDB::bind_method(D_METHOD("set_timeout", "timeout", "timeout_min", "timeout_max"), &ENetPacketPeer::set_timeout); + ClassDB::bind_method(D_METHOD("get_packet_flags"), &ENetPacketPeer::get_packet_flags); ClassDB::bind_method(D_METHOD("get_remote_address"), &ENetPacketPeer::get_remote_address); ClassDB::bind_method(D_METHOD("get_remote_port"), &ENetPacketPeer::get_remote_port); ClassDB::bind_method(D_METHOD("get_statistic", "statistic"), &ENetPacketPeer::get_statistic); diff --git a/modules/enet/enet_packet_peer.h b/modules/enet/enet_packet_peer.h index fe40d06188..b41d67e86b 100644 --- a/modules/enet/enet_packet_peer.h +++ b/modules/enet/enet_packet_peer.h @@ -113,6 +113,7 @@ public: double get_statistic(PeerStatistic p_stat); PeerState get_state() const; int get_channels() const; + int get_packet_flags() const; // Extras IPAddress get_remote_address() const; diff --git a/modules/etcpak/SCsub b/modules/etcpak/SCsub index 2d3b69be75..a872e1cd03 100644 --- a/modules/etcpak/SCsub +++ b/modules/etcpak/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/fbx/SCsub b/modules/fbx/SCsub index 6a791094c6..6f9fbba0b4 100644 --- a/modules/fbx/SCsub +++ b/modules/fbx/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/freetype/SCsub b/modules/freetype/SCsub index 2813eaecd5..5edce96680 100644 --- a/modules/freetype/SCsub +++ b/modules/freetype/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/gdscript/SCsub b/modules/gdscript/SCsub index 61accd4fc9..8f50bf9588 100644 --- a/modules/gdscript/SCsub +++ b/modules/gdscript/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/gdscript/doc_classes/@GDScript.xml b/modules/gdscript/doc_classes/@GDScript.xml index f539f27848..5fe47d69df 100644 --- a/modules/gdscript/doc_classes/@GDScript.xml +++ b/modules/gdscript/doc_classes/@GDScript.xml @@ -669,6 +669,41 @@ [b]Note:[/b] Subgroups cannot be nested, they only provide one extra level of depth. Just like the next group ends the previous group, so do the subsequent subgroups. </description> </annotation> + <annotation name="@export_tool_button"> + <return type="void" /> + <param index="0" name="text" type="String" /> + <param index="1" name="icon" type="String" default="""" /> + <description> + Export a [Callable] property as a clickable button with the label [param text]. When the button is pressed, the callable is called. + If [param icon] is specified, it is used to fetch an icon for the button via [method Control.get_theme_icon], from the [code]"EditorIcons"[/code] theme type. If [param icon] is omitted, the default [code]"Callable"[/code] icon is used instead. + Consider using the [EditorUndoRedoManager] to allow the action to be reverted safely. + See also [constant PROPERTY_HINT_TOOL_BUTTON]. + [codeblock] + @tool + extends Sprite2D + + @export_tool_button("Hello") var hello_action = hello + @export_tool_button("Randomize the color!", "ColorRect") + var randomize_color_action = randomize_color + + func hello(): + print("Hello world!") + + func randomize_color(): + var undo_redo = EditorInterface.get_editor_undo_redo() + undo_redo.create_action("Randomized Sprite2D Color") + undo_redo.add_do_property(self, &"self_modulate", Color(randf(), randf(), randf())) + undo_redo.add_undo_property(self, &"self_modulate", self_modulate) + undo_redo.commit_action() + [/codeblock] + [b]Note:[/b] The property is exported without the [constant PROPERTY_USAGE_STORAGE] flag because a [Callable] cannot be properly serialized and stored in a file. + [b]Note:[/b] In an exported project neither [EditorInterface] nor [EditorUndoRedoManager] exist, which may cause some scripts to break. To prevent this, you can use [method Engine.get_singleton] and omit the static type from the variable declaration: + [codeblock] + var undo_redo = Engine.get_singleton(&"EditorInterface").get_editor_undo_redo() + [/codeblock] + [b]Note:[/b] Avoid storing lambda callables in member variables of [RefCounted]-based classes (e.g. resources), as this can lead to memory leaks. Use only method callables and optionally [method Callable.bind] or [method Callable.unbind]. + </description> + </annotation> <annotation name="@icon"> <return type="void" /> <param index="0" name="icon_path" type="String" /> diff --git a/modules/gdscript/editor/script_templates/SCsub b/modules/gdscript/editor/script_templates/SCsub index 5db7e3fc3b..28a27db3fa 100644 --- a/modules/gdscript/editor/script_templates/SCsub +++ b/modules/gdscript/editor/script_templates/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") diff --git a/modules/gdscript/gdscript_compiler.cpp b/modules/gdscript/gdscript_compiler.cpp index eebf282633..f4f445e096 100644 --- a/modules/gdscript/gdscript_compiler.cpp +++ b/modules/gdscript/gdscript_compiler.cpp @@ -2629,9 +2629,10 @@ Error GDScriptCompiler::_parse_setter_getter(GDScript *p_script, const GDScriptP return err; } -// Prepares given script, and inner class scripts, for compilation. It populates class members and initializes method -// RPC info for its base classes first, then for itself, then for inner classes. -// Warning: this function cannot initiate compilation of other classes, or it will result in cyclic dependency issues. +// Prepares given script, and inner class scripts, for compilation. It populates class members and +// initializes method RPC info for its base classes first, then for itself, then for inner classes. +// WARNING: This function cannot initiate compilation of other classes, or it will result in +// cyclic dependency issues. Error GDScriptCompiler::_prepare_compilation(GDScript *p_script, const GDScriptParser::ClassNode *p_class, bool p_keep_state) { if (parsed_classes.has(p_script)) { return OK; diff --git a/modules/gdscript/gdscript_editor.cpp b/modules/gdscript/gdscript_editor.cpp index cf1cd55355..0fd891aa80 100644 --- a/modules/gdscript/gdscript_editor.cpp +++ b/modules/gdscript/gdscript_editor.cpp @@ -97,8 +97,8 @@ Ref<Script> GDScriptLanguage::make_template(const String &p_template, const Stri } processed_template = processed_template.replace("_BASE_", p_base_class_name) - .replace("_CLASS_SNAKE_CASE_", p_class_name.to_snake_case().validate_ascii_identifier()) - .replace("_CLASS_", p_class_name.to_pascal_case().validate_ascii_identifier()) + .replace("_CLASS_SNAKE_CASE_", p_class_name.to_snake_case().validate_unicode_identifier()) + .replace("_CLASS_", p_class_name.to_pascal_case().validate_unicode_identifier()) .replace("_TS_", _get_indentation()); scr->set_source_code(processed_template); return scr; @@ -864,7 +864,8 @@ static void _get_directory_contents(EditorFileSystemDirectory *p_dir, HashMap<St } } -static void _find_annotation_arguments(const GDScriptParser::AnnotationNode *p_annotation, int p_argument, const String p_quote_style, HashMap<String, ScriptLanguage::CodeCompletionOption> &r_result) { +static void _find_annotation_arguments(const GDScriptParser::AnnotationNode *p_annotation, int p_argument, const String p_quote_style, HashMap<String, ScriptLanguage::CodeCompletionOption> &r_result, String &r_arghint) { + r_arghint = _make_arguments_hint(p_annotation->info->info, p_argument, true); if (p_annotation->name == SNAME("@export_range")) { if (p_argument == 3 || p_argument == 4 || p_argument == 5) { // Slider hint. @@ -2975,11 +2976,6 @@ static bool _get_subscript_type(GDScriptParser::CompletionContext &p_context, co } break; case GDScriptParser::Node::IDENTIFIER: { - if (p_subscript->base->datatype.type_source == GDScriptParser::DataType::ANNOTATED_EXPLICIT) { - // Annotated type takes precedence. - return false; - } - const GDScriptParser::IdentifierNode *identifier_node = static_cast<GDScriptParser::IdentifierNode *>(p_subscript->base); switch (identifier_node->source) { @@ -3017,6 +3013,14 @@ static bool _get_subscript_type(GDScriptParser::CompletionContext &p_context, co if (get_node != nullptr) { const Object *node = p_context.base->call("get_node_or_null", NodePath(get_node->full_path)); if (node != nullptr) { + GDScriptParser::DataType assigned_type = _type_from_variant(node, p_context).type; + GDScriptParser::DataType base_type = p_subscript->base->datatype; + + if (p_subscript->base->type == GDScriptParser::Node::IDENTIFIER && base_type.type_source == GDScriptParser::DataType::ANNOTATED_EXPLICIT && (assigned_type.kind != base_type.kind || assigned_type.script_path != base_type.script_path || assigned_type.native_type != base_type.native_type)) { + // Annotated type takes precedence. + return false; + } + if (r_base != nullptr) { *r_base = node; } @@ -3183,7 +3187,7 @@ static void _find_call_arguments(GDScriptParser::CompletionContext &p_context, c break; } const GDScriptParser::AnnotationNode *annotation = static_cast<const GDScriptParser::AnnotationNode *>(completion_context.node); - _find_annotation_arguments(annotation, completion_context.current_argument, quote_style, options); + _find_annotation_arguments(annotation, completion_context.current_argument, quote_style, options, r_call_hint); r_forced = true; } break; case GDScriptParser::COMPLETION_BUILT_IN_TYPE_CONSTANT_OR_STATIC_METHOD: { diff --git a/modules/gdscript/gdscript_parser.cpp b/modules/gdscript/gdscript_parser.cpp index 65aa150be3..111a39d730 100644 --- a/modules/gdscript/gdscript_parser.cpp +++ b/modules/gdscript/gdscript_parser.cpp @@ -122,6 +122,7 @@ GDScriptParser::GDScriptParser() { register_annotation(MethodInfo("@export_flags_avoidance"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations<PROPERTY_HINT_LAYERS_AVOIDANCE, Variant::INT>); register_annotation(MethodInfo("@export_storage"), AnnotationInfo::VARIABLE, &GDScriptParser::export_storage_annotation); register_annotation(MethodInfo("@export_custom", PropertyInfo(Variant::INT, "hint", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_CLASS_IS_ENUM, "PropertyHint"), PropertyInfo(Variant::STRING, "hint_string"), PropertyInfo(Variant::INT, "usage", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_CLASS_IS_BITFIELD, "PropertyUsageFlags")), AnnotationInfo::VARIABLE, &GDScriptParser::export_custom_annotation, varray(PROPERTY_USAGE_DEFAULT)); + register_annotation(MethodInfo("@export_tool_button", PropertyInfo(Variant::STRING, "text"), PropertyInfo(Variant::STRING, "icon")), AnnotationInfo::VARIABLE, &GDScriptParser::export_tool_button_annotation, varray("")); // Export grouping annotations. register_annotation(MethodInfo("@export_category", PropertyInfo(Variant::STRING, "name")), AnnotationInfo::STANDALONE, &GDScriptParser::export_group_annotations<PROPERTY_USAGE_CATEGORY>); register_annotation(MethodInfo("@export_group", PropertyInfo(Variant::STRING, "name"), PropertyInfo(Variant::STRING, "prefix")), AnnotationInfo::STANDALONE, &GDScriptParser::export_group_annotations<PROPERTY_USAGE_GROUP>, varray("")); @@ -1639,23 +1640,29 @@ GDScriptParser::AnnotationNode *GDScriptParser::parse_annotation(uint32_t p_vali advance(); // Arguments. push_completion_call(annotation); - make_completion_context(COMPLETION_ANNOTATION_ARGUMENTS, annotation, 0); int argument_index = 0; do { + make_completion_context(COMPLETION_ANNOTATION_ARGUMENTS, annotation, argument_index); + set_last_completion_call_arg(argument_index); if (check(GDScriptTokenizer::Token::PARENTHESIS_CLOSE)) { // Allow for trailing comma. break; } - make_completion_context(COMPLETION_ANNOTATION_ARGUMENTS, annotation, argument_index); - set_last_completion_call_arg(argument_index++); ExpressionNode *argument = parse_expression(false); + if (argument == nullptr) { push_error("Expected expression as the annotation argument."); valid = false; - continue; + } else { + annotation->arguments.push_back(argument); + + if (argument->type == Node::LITERAL) { + override_completion_context(argument, COMPLETION_ANNOTATION_ARGUMENTS, annotation, argument_index); + } } - annotation->arguments.push_back(argument); + + argument_index++; } while (match(GDScriptTokenizer::Token::COMMA) && !is_at_end()); pop_multiline(); @@ -4618,10 +4625,10 @@ bool GDScriptParser::export_annotations(AnnotationNode *p_annotation, Node *p_ta // For `@export_storage` and `@export_custom`, there is no need to check the variable type, argument values, // or handle array exports in a special way, so they are implemented as separate methods. -bool GDScriptParser::export_storage_annotation(AnnotationNode *p_annotation, Node *p_node, ClassNode *p_class) { - ERR_FAIL_COND_V_MSG(p_node->type != Node::VARIABLE, false, vformat(R"("%s" annotation can only be applied to variables.)", p_annotation->name)); +bool GDScriptParser::export_storage_annotation(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class) { + ERR_FAIL_COND_V_MSG(p_target->type != Node::VARIABLE, false, vformat(R"("%s" annotation can only be applied to variables.)", p_annotation->name)); - VariableNode *variable = static_cast<VariableNode *>(p_node); + VariableNode *variable = static_cast<VariableNode *>(p_target); if (variable->is_static) { push_error(vformat(R"(Annotation "%s" cannot be applied to a static variable.)", p_annotation->name), p_annotation); return false; @@ -4640,11 +4647,11 @@ bool GDScriptParser::export_storage_annotation(AnnotationNode *p_annotation, Nod return true; } -bool GDScriptParser::export_custom_annotation(AnnotationNode *p_annotation, Node *p_node, ClassNode *p_class) { - ERR_FAIL_COND_V_MSG(p_node->type != Node::VARIABLE, false, vformat(R"("%s" annotation can only be applied to variables.)", p_annotation->name)); +bool GDScriptParser::export_custom_annotation(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class) { + ERR_FAIL_COND_V_MSG(p_target->type != Node::VARIABLE, false, vformat(R"("%s" annotation can only be applied to variables.)", p_annotation->name)); ERR_FAIL_COND_V_MSG(p_annotation->resolved_arguments.size() < 2, false, R"(Annotation "@export_custom" requires 2 arguments.)"); - VariableNode *variable = static_cast<VariableNode *>(p_node); + VariableNode *variable = static_cast<VariableNode *>(p_target); if (variable->is_static) { push_error(vformat(R"(Annotation "%s" cannot be applied to a static variable.)", p_annotation->name), p_annotation); return false; @@ -4668,12 +4675,56 @@ bool GDScriptParser::export_custom_annotation(AnnotationNode *p_annotation, Node return true; } -template <PropertyUsageFlags t_usage> -bool GDScriptParser::export_group_annotations(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class) { - if (p_annotation->resolved_arguments.is_empty()) { +bool GDScriptParser::export_tool_button_annotation(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class) { +#ifdef TOOLS_ENABLED + ERR_FAIL_COND_V_MSG(p_target->type != Node::VARIABLE, false, vformat(R"("%s" annotation can only be applied to variables.)", p_annotation->name)); + ERR_FAIL_COND_V(p_annotation->resolved_arguments.is_empty(), false); + + if (!is_tool()) { + push_error(R"(Tool buttons can only be used in tool scripts (add "@tool" to the top of the script).)", p_annotation); + return false; + } + + VariableNode *variable = static_cast<VariableNode *>(p_target); + + if (variable->is_static) { + push_error(vformat(R"(Annotation "%s" cannot be applied to a static variable.)", p_annotation->name), p_annotation); + return false; + } + if (variable->exported) { + push_error(vformat(R"(Annotation "%s" cannot be used with another "@export" annotation.)", p_annotation->name), p_annotation); return false; } + const DataType variable_type = variable->get_datatype(); + if (!variable_type.is_variant() && variable_type.is_hard_type()) { + if (variable_type.kind != DataType::BUILTIN || variable_type.builtin_type != Variant::CALLABLE) { + push_error(vformat(R"("@export_tool_button" annotation requires a variable of type "Callable", but type "%s" was given instead.)", variable_type.to_string()), p_annotation); + return false; + } + } + + variable->exported = true; + + // Build the hint string (format: `<text>[,<icon>]`). + String hint_string = p_annotation->resolved_arguments[0].operator String(); // Button text. + if (p_annotation->resolved_arguments.size() > 1) { + hint_string += "," + p_annotation->resolved_arguments[1].operator String(); // Button icon. + } + + variable->export_info.type = Variant::CALLABLE; + variable->export_info.hint = PROPERTY_HINT_TOOL_BUTTON; + variable->export_info.hint_string = hint_string; + variable->export_info.usage = PROPERTY_USAGE_EDITOR; +#endif // TOOLS_ENABLED + + return true; // Only available in editor. +} + +template <PropertyUsageFlags t_usage> +bool GDScriptParser::export_group_annotations(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class) { + ERR_FAIL_COND_V(p_annotation->resolved_arguments.is_empty(), false); + p_annotation->export_info.name = p_annotation->resolved_arguments[0]; switch (t_usage) { diff --git a/modules/gdscript/gdscript_parser.h b/modules/gdscript/gdscript_parser.h index 7840474a89..7f64ae902b 100644 --- a/modules/gdscript/gdscript_parser.h +++ b/modules/gdscript/gdscript_parser.h @@ -1507,6 +1507,7 @@ private: bool export_annotations(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class); bool export_storage_annotation(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class); bool export_custom_annotation(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class); + bool export_tool_button_annotation(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class); template <PropertyUsageFlags t_usage> bool export_group_annotations(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class); bool warning_annotations(AnnotationNode *p_annotation, Node *p_target, ClassNode *p_class); diff --git a/modules/gdscript/tests/scripts/parser/errors/export_tool_button_requires_tool_mode.gd b/modules/gdscript/tests/scripts/parser/errors/export_tool_button_requires_tool_mode.gd new file mode 100644 index 0000000000..48be5b2541 --- /dev/null +++ b/modules/gdscript/tests/scripts/parser/errors/export_tool_button_requires_tool_mode.gd @@ -0,0 +1 @@ +@export_tool_button("Click me!") var action diff --git a/modules/gdscript/tests/scripts/parser/errors/export_tool_button_requires_tool_mode.out b/modules/gdscript/tests/scripts/parser/errors/export_tool_button_requires_tool_mode.out new file mode 100644 index 0000000000..fb148308e4 --- /dev/null +++ b/modules/gdscript/tests/scripts/parser/errors/export_tool_button_requires_tool_mode.out @@ -0,0 +1,2 @@ +GDTEST_ANALYZER_ERROR +Tool buttons can only be used in tool scripts (add "@tool" to the top of the script). diff --git a/modules/gdscript/tests/scripts/parser/features/export_variable.gd b/modules/gdscript/tests/scripts/parser/features/export_variable.gd index 1e134d0e0e..8aa449f602 100644 --- a/modules/gdscript/tests/scripts/parser/features/export_variable.gd +++ b/modules/gdscript/tests/scripts/parser/features/export_variable.gd @@ -1,3 +1,4 @@ +@tool class_name ExportVariableTest extends Node @@ -47,6 +48,10 @@ const PreloadedUnnamedClass = preload("./export_variable_unnamed.notest.gd") @export_custom(PROPERTY_HINT_ENUM, "A,B,C") var test_export_custom_weak_int = 5 @export_custom(PROPERTY_HINT_ENUM, "A,B,C") var test_export_custom_hard_int: int = 6 +# `@export_tool_button`. +@export_tool_button("Click me!") var test_tool_button_1: Callable +@export_tool_button("Click me!", "ColorRect") var test_tool_button_2: Callable + func test(): for property in get_property_list(): if str(property.name).begins_with("test_"): diff --git a/modules/gdscript/tests/scripts/parser/features/export_variable.out b/modules/gdscript/tests/scripts/parser/features/export_variable.out index d10462bb8d..0d915e00e6 100644 --- a/modules/gdscript/tests/scripts/parser/features/export_variable.out +++ b/modules/gdscript/tests/scripts/parser/features/export_variable.out @@ -55,3 +55,7 @@ var test_export_custom_weak_int: int = 5 hint=ENUM hint_string="A,B,C" usage=DEFAULT|SCRIPT_VARIABLE class_name=&"" var test_export_custom_hard_int: int = 6 hint=ENUM hint_string="A,B,C" usage=DEFAULT|SCRIPT_VARIABLE class_name=&"" +var test_tool_button_1: Callable = Callable() + hint=TOOL_BUTTON hint_string="Click me!" usage=EDITOR|SCRIPT_VARIABLE class_name=&"" +var test_tool_button_2: Callable = Callable() + hint=TOOL_BUTTON hint_string="Click me!,ColorRect" usage=EDITOR|SCRIPT_VARIABLE class_name=&"" diff --git a/modules/gdscript/tests/scripts/utils.notest.gd b/modules/gdscript/tests/scripts/utils.notest.gd index 1e2788f765..fa289e442f 100644 --- a/modules/gdscript/tests/scripts/utils.notest.gd +++ b/modules/gdscript/tests/scripts/utils.notest.gd @@ -205,6 +205,9 @@ static func get_property_hint_name(hint: PropertyHint) -> String: return "PROPERTY_HINT_HIDE_QUATERNION_EDIT" PROPERTY_HINT_PASSWORD: return "PROPERTY_HINT_PASSWORD" + PROPERTY_HINT_TOOL_BUTTON: + return "PROPERTY_HINT_TOOL_BUTTON" + printerr("Argument `hint` is invalid. Use `PROPERTY_HINT_*` constants.") return "<invalid hint>" diff --git a/modules/glslang/SCsub b/modules/glslang/SCsub index 3068377e60..b6e3da2316 100644 --- a/modules/glslang/SCsub +++ b/modules/glslang/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/gltf/SCsub b/modules/gltf/SCsub index 9d263cccac..1075116863 100644 --- a/modules/gltf/SCsub +++ b/modules/gltf/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/gltf/extensions/SCsub b/modules/gltf/extensions/SCsub index fdf14300f1..e403cd6fdc 100644 --- a/modules/gltf/extensions/SCsub +++ b/modules/gltf/extensions/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/godot_physics_2d/SCsub b/modules/godot_physics_2d/SCsub new file mode 100644 index 0000000000..39eb469978 --- /dev/null +++ b/modules/godot_physics_2d/SCsub @@ -0,0 +1,6 @@ +#!/usr/bin/env python +from misc.utility.scons_hints import * + +Import("env") + +env.add_source_files(env.modules_sources, "*.cpp") diff --git a/modules/godot_physics_2d/config.py b/modules/godot_physics_2d/config.py new file mode 100644 index 0000000000..d22f9454ed --- /dev/null +++ b/modules/godot_physics_2d/config.py @@ -0,0 +1,6 @@ +def can_build(env, platform): + return True + + +def configure(env): + pass diff --git a/modules/godot_physics_2d/godot_area_2d.cpp b/modules/godot_physics_2d/godot_area_2d.cpp new file mode 100644 index 0000000000..d6c786706c --- /dev/null +++ b/modules/godot_physics_2d/godot_area_2d.cpp @@ -0,0 +1,314 @@ +/**************************************************************************/ +/* godot_area_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_area_2d.h" +#include "godot_body_2d.h" +#include "godot_space_2d.h" + +GodotArea2D::BodyKey::BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +GodotArea2D::BodyKey::BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +void GodotArea2D::_shapes_changed() { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::set_transform(const Transform2D &p_transform) { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } + + _set_transform(p_transform); + _set_inv_transform(p_transform.affine_inverse()); +} + +void GodotArea2D::set_space(GodotSpace2D *p_space) { + if (get_space()) { + if (monitor_query_list.in_list()) { + get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + } + if (moved_list.in_list()) { + get_space()->area_remove_from_moved_list(&moved_list); + } + } + + monitored_bodies.clear(); + monitored_areas.clear(); + + _set_space(p_space); +} + +void GodotArea2D::set_monitor_callback(const Callable &p_callback) { + _unregister_shapes(); + + monitor_callback = p_callback; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::set_area_monitor_callback(const Callable &p_callback) { + _unregister_shapes(); + + area_monitor_callback = p_callback; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea2D::_set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode) { + bool do_override = p_new_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (r_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { + return; + } + _unregister_shapes(); + r_mode = p_new_mode; + _shape_changed(); +} + +void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: + _set_space_override_mode(gravity_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value); + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY: + gravity = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: + gravity_vector = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: + gravity_is_point = p_value; + break; + case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: + gravity_point_unit_distance = p_value; + break; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: + _set_space_override_mode(linear_damping_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value); + break; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: + linear_damp = p_value; + break; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: + _set_space_override_mode(angular_damping_override_mode, (PhysicsServer2D::AreaSpaceOverrideMode)(int)p_value); + break; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: + angular_damp = p_value; + break; + case PhysicsServer2D::AREA_PARAM_PRIORITY: + priority = p_value; + break; + } +} + +Variant GodotArea2D::get_param(PhysicsServer2D::AreaParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: + return gravity_override_mode; + case PhysicsServer2D::AREA_PARAM_GRAVITY: + return gravity; + case PhysicsServer2D::AREA_PARAM_GRAVITY_VECTOR: + return gravity_vector; + case PhysicsServer2D::AREA_PARAM_GRAVITY_IS_POINT: + return gravity_is_point; + case PhysicsServer2D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: + return gravity_point_unit_distance; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: + return linear_damping_override_mode; + case PhysicsServer2D::AREA_PARAM_LINEAR_DAMP: + return linear_damp; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: + return angular_damping_override_mode; + case PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP: + return angular_damp; + case PhysicsServer2D::AREA_PARAM_PRIORITY: + return priority; + } + + return Variant(); +} + +void GodotArea2D::_queue_monitor_update() { + ERR_FAIL_NULL(get_space()); + + if (!monitor_query_list.in_list()) { + get_space()->area_add_to_monitor_query_list(&monitor_query_list); + } +} + +void GodotArea2D::set_monitorable(bool p_monitorable) { + if (monitorable == p_monitorable) { + return; + } + + monitorable = p_monitorable; + _set_static(!monitorable); + _shapes_changed(); +} + +void GodotArea2D::call_queries() { + if (!monitor_callback.is_null() && !monitored_bodies.is_empty()) { + if (monitor_callback.is_valid()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); + E = next; + continue; + } + + res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; + + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); + E = next; + + Callable::CallError ce; + Variant ret; + monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(monitor_callback, (const Variant **)resptr, 5, ce)); + } + } + } else { + monitored_bodies.clear(); + monitor_callback = Callable(); + } + } + + if (!area_monitor_callback.is_null() && !monitored_areas.is_empty()) { + if (area_monitor_callback.is_valid()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); + E = next; + continue; + } + + res[0] = E->value.state > 0 ? PhysicsServer2D::AREA_BODY_ADDED : PhysicsServer2D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; + + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); + E = next; + + Callable::CallError ce; + Variant ret; + area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling event callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce)); + } + } + } else { + monitored_areas.clear(); + area_monitor_callback = Callable(); + } + } +} + +void GodotArea2D::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { + if (is_gravity_point()) { + const real_t gr_unit_dist = get_gravity_point_unit_distance(); + Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gr_unit_dist > 0) { + const real_t v_length_sq = v.length_squared(); + if (v_length_sq > 0) { + const real_t gravity_strength = get_gravity() * gr_unit_dist * gr_unit_dist / v_length_sq; + r_gravity = v.normalized() * gravity_strength; + } else { + r_gravity = Vector2(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + +GodotArea2D::GodotArea2D() : + GodotCollisionObject2D(TYPE_AREA), + monitor_query_list(this), + moved_list(this) { + _set_static(true); //areas are not active by default +} + +GodotArea2D::~GodotArea2D() { +} diff --git a/modules/godot_physics_2d/godot_area_2d.h b/modules/godot_physics_2d/godot_area_2d.h new file mode 100644 index 0000000000..e6c3b45d6c --- /dev/null +++ b/modules/godot_physics_2d/godot_area_2d.h @@ -0,0 +1,191 @@ +/**************************************************************************/ +/* godot_area_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_AREA_2D_H +#define GODOT_AREA_2D_H + +#include "godot_collision_object_2d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_2d.h" + +class GodotSpace2D; +class GodotBody2D; +class GodotConstraint2D; + +class GodotArea2D : public GodotCollisionObject2D { + PhysicsServer2D::AreaSpaceOverrideMode gravity_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer2D::AreaSpaceOverrideMode linear_damping_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer2D::AreaSpaceOverrideMode angular_damping_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; + + real_t gravity = 9.80665; + Vector2 gravity_vector = Vector2(0, -1); + bool gravity_is_point = false; + real_t gravity_point_unit_distance = 0.0; + real_t linear_damp = 0.1; + real_t angular_damp = 1.0; + int priority = 0; + bool monitorable = false; + + Callable monitor_callback; + + Callable area_monitor_callback; + + SelfList<GodotArea2D> monitor_query_list; + SelfList<GodotArea2D> moved_list; + + struct BodyKey { + RID rid; + ObjectID instance_id; + uint32_t body_shape = 0; + uint32_t area_shape = 0; + + static uint32_t hash(const BodyKey &p_key) { + uint32_t h = hash_one_uint64(p_key.rid.get_id()); + h = hash_murmur3_one_64(p_key.instance_id, h); + h = hash_murmur3_one_32(p_key.area_shape, h); + return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h)); + } + + _FORCE_INLINE_ bool operator==(const BodyKey &p_key) const { + return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape; + } + + _FORCE_INLINE_ BodyKey() {} + BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + }; + + struct BodyState { + int state = 0; + _FORCE_INLINE_ void inc() { state++; } + _FORCE_INLINE_ void dec() { state--; } + }; + + HashMap<BodyKey, BodyState, BodyKey> monitored_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_areas; + + HashSet<GodotConstraint2D *> constraints; + + virtual void _shapes_changed() override; + void _queue_monitor_update(); + + void _set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode); + +public: + void set_monitor_callback(const Callable &p_callback); + _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback.is_valid(); } + + void set_area_monitor_callback(const Callable &p_callback); + _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback.is_valid(); } + + _FORCE_INLINE_ void add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + + _FORCE_INLINE_ void add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + _FORCE_INLINE_ void remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + + void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer2D::AreaParameter p_param) const; + + _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } + _FORCE_INLINE_ real_t get_gravity() const { return gravity; } + + _FORCE_INLINE_ void set_gravity_vector(const Vector2 &p_gravity) { gravity_vector = p_gravity; } + _FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; } + + _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } + _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } + + _FORCE_INLINE_ void set_gravity_point_unit_distance(real_t scale) { gravity_point_unit_distance = scale; } + _FORCE_INLINE_ real_t get_gravity_point_unit_distance() const { return gravity_point_unit_distance; } + + _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } + _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } + + _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } + _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const HashSet<GodotConstraint2D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } + + void set_monitorable(bool p_monitorable); + _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } + + void set_transform(const Transform2D &p_transform); + + void set_space(GodotSpace2D *p_space) override; + + void call_queries(); + + void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const; + + GodotArea2D(); + ~GodotArea2D(); +}; + +void GodotArea2D::add_body_to_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::remove_body_from_query(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].dec(); + if (get_space() && !monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::add_area_to_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea2D::remove_area_from_query(GodotArea2D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].dec(); + if (get_space() && !monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +#endif // GODOT_AREA_2D_H diff --git a/modules/godot_physics_2d/godot_area_pair_2d.cpp b/modules/godot_physics_2d/godot_area_pair_2d.cpp new file mode 100644 index 0000000000..ca12e30c29 --- /dev/null +++ b/modules/godot_physics_2d/godot_area_pair_2d.cpp @@ -0,0 +1,203 @@ +/**************************************************************************/ +/* godot_area_pair_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_area_pair_2d.h" +#include "godot_collision_solver_2d.h" + +bool GodotAreaPair2D::setup(real_t p_step) { + bool result = false; + if (area->collides_with(body) && GodotCollisionSolver2D::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) { + result = true; + } + + process_collision = false; + has_space_override = false; + if (result != colliding) { + if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if ((int)area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE) != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } + process_collision = has_space_override; + + if (area->has_monitor_callback()) { + process_collision = true; + } + + colliding = result; + } + + return process_collision; +} + +bool GodotAreaPair2D::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (has_space_override) { + body_has_attached_area = true; + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (has_space_override) { + body_has_attached_area = false; + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. +} + +void GodotAreaPair2D::solve(real_t p_step) { + // Nothing to do. +} + +GodotAreaPair2D::GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape) { + body = p_body; + area = p_area; + body_shape = p_body_shape; + area_shape = p_area_shape; + body->add_constraint(this, 0); + area->add_constraint(this); + if (p_body->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { //need to be active to process pair + p_body->set_active(true); + } +} + +GodotAreaPair2D::~GodotAreaPair2D() { + if (colliding) { + if (body_has_attached_area) { + body_has_attached_area = false; + body->remove_area(area); + } + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + body->remove_constraint(this, 0); + area->remove_constraint(this); +} + +////////////////////////////////// + +bool GodotArea2Pair2D::setup(real_t p_step) { + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !GodotCollisionSolver2D::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) { + result_a = false; + result_b = false; + } + + bool process_collision = false; + + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b_monitorable) { + process_collision_a = true; + process_collision = true; + } + colliding_a = result_a; + } + + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a_monitorable) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; + } + + return process_collision; +} + +bool GodotArea2Pair2D::pre_solve(real_t p_step) { + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (process_collision_b) { + if (colliding_b) { + area_b->add_area_to_query(area_a, shape_a, shape_b); + } else { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + return false; // Never do any post solving. +} + +void GodotArea2Pair2D::solve(real_t p_step) { + // Nothing to do. +} + +GodotArea2Pair2D::GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b) { + area_a = p_area_a; + area_b = p_area_b; + shape_a = p_shape_a; + shape_b = p_shape_b; + area_a_monitorable = area_a->is_monitorable(); + area_b_monitorable = area_b->is_monitorable(); + area_a->add_constraint(this); + area_b->add_constraint(this); +} + +GodotArea2Pair2D::~GodotArea2Pair2D() { + if (colliding_a) { + if (area_a->has_area_monitor_callback() && area_b_monitorable) { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (colliding_b) { + if (area_b->has_area_monitor_callback() && area_a_monitorable) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + area_a->remove_constraint(this); + area_b->remove_constraint(this); +} diff --git a/modules/godot_physics_2d/godot_area_pair_2d.h b/modules/godot_physics_2d/godot_area_pair_2d.h new file mode 100644 index 0000000000..eb091288a9 --- /dev/null +++ b/modules/godot_physics_2d/godot_area_pair_2d.h @@ -0,0 +1,78 @@ +/**************************************************************************/ +/* godot_area_pair_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_AREA_PAIR_2D_H +#define GODOT_AREA_PAIR_2D_H + +#include "godot_area_2d.h" +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotAreaPair2D : public GodotConstraint2D { + GodotBody2D *body = nullptr; + GodotArea2D *area = nullptr; + int body_shape = 0; + int area_shape = 0; + bool colliding = false; + bool has_space_override = false; + bool process_collision = false; + bool body_has_attached_area = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape); + ~GodotAreaPair2D(); +}; + +class GodotArea2Pair2D : public GodotConstraint2D { + GodotArea2D *area_a = nullptr; + GodotArea2D *area_b = nullptr; + int shape_a = 0; + int shape_b = 0; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; + bool area_a_monitorable; + bool area_b_monitorable; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b); + ~GodotArea2Pair2D(); +}; + +#endif // GODOT_AREA_PAIR_2D_H diff --git a/modules/godot_physics_2d/godot_body_2d.cpp b/modules/godot_physics_2d/godot_body_2d.cpp new file mode 100644 index 0000000000..c401e6eee7 --- /dev/null +++ b/modules/godot_physics_2d/godot_body_2d.cpp @@ -0,0 +1,762 @@ +/**************************************************************************/ +/* godot_body_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_body_2d.h" + +#include "godot_area_2d.h" +#include "godot_body_direct_state_2d.h" +#include "godot_space_2d.h" + +void GodotBody2D::_mass_properties_changed() { + if (get_space() && !mass_properties_update_list.in_list()) { + get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); + } +} + +void GodotBody2D::update_mass_properties() { + //update shapes and motions + + switch (mode) { + case PhysicsServer2D::BODY_MODE_RIGID: { + real_t total_area = 0; + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + total_area += get_shape_aabb(i).get_area(); + } + + if (calculate_center_of_mass) { + // We have to recompute the center of mass. + center_of_mass_local = Vector2(); + + if (total_area != 0.0) { + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + real_t area = get_shape_aabb(i).get_area(); + + real_t mass_new = area * mass / total_area; + + // NOTE: we assume that the shape origin is also its center of mass. + center_of_mass_local += mass_new * get_shape_transform(i).get_origin(); + } + + center_of_mass_local /= mass; + } + } + + if (calculate_inertia) { + inertia = 0; + + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + const GodotShape2D *shape = get_shape(i); + + real_t area = get_shape_aabb(i).get_area(); + if (area == 0.0) { + continue; + } + + real_t mass_new = area * mass / total_area; + + Transform2D mtx = get_shape_transform(i); + Vector2 scale = mtx.get_scale(); + Vector2 shape_origin = mtx.get_origin() - center_of_mass_local; + inertia += shape->get_moment_of_inertia(mass_new, scale) + mass_new * shape_origin.length_squared(); + } + } + + _inv_inertia = inertia > 0.0 ? (1.0 / inertia) : 0.0; + + if (mass) { + _inv_mass = 1.0 / mass; + } else { + _inv_mass = 0; + } + + } break; + case PhysicsServer2D::BODY_MODE_KINEMATIC: + case PhysicsServer2D::BODY_MODE_STATIC: { + _inv_inertia = 0; + _inv_mass = 0; + } break; + case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: { + _inv_inertia = 0; + _inv_mass = 1.0 / mass; + + } break; + } + + _update_transform_dependent(); +} + +void GodotBody2D::reset_mass_properties() { + calculate_inertia = true; + calculate_center_of_mass = true; + _mass_properties_changed(); +} + +void GodotBody2D::set_active(bool p_active) { + if (active == p_active) { + return; + } + + active = p_active; + + if (active) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { + get_space()->body_add_to_active_list(&active_list); + } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); + } +} + +void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer2D::BODY_PARAM_BOUNCE: { + bounce = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_FRICTION: { + friction = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_MASS: { + real_t mass_value = p_value; + ERR_FAIL_COND(mass_value <= 0); + mass = mass_value; + if (mode >= PhysicsServer2D::BODY_MODE_RIGID) { + _mass_properties_changed(); + } + } break; + case PhysicsServer2D::BODY_PARAM_INERTIA: { + real_t inertia_value = p_value; + if (inertia_value <= 0.0) { + calculate_inertia = true; + if (mode == PhysicsServer2D::BODY_MODE_RIGID) { + _mass_properties_changed(); + } + } else { + calculate_inertia = false; + inertia = inertia_value; + if (mode == PhysicsServer2D::BODY_MODE_RIGID) { + _inv_inertia = 1.0 / inertia; + } + } + } break; + case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { + calculate_center_of_mass = false; + center_of_mass_local = p_value; + _update_transform_dependent(); + } break; + case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { + if (Math::is_zero_approx(gravity_scale)) { + wakeup(); + } + gravity_scale = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: { + int mode_value = p_value; + linear_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value; + } break; + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: { + int mode_value = p_value; + angular_damp_mode = (PhysicsServer2D::BodyDampMode)mode_value; + } break; + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { + linear_damp = p_value; + } break; + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { + angular_damp = p_value; + } break; + default: { + } + } +} + +Variant GodotBody2D::get_param(PhysicsServer2D::BodyParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::BODY_PARAM_BOUNCE: { + return bounce; + } + case PhysicsServer2D::BODY_PARAM_FRICTION: { + return friction; + } + case PhysicsServer2D::BODY_PARAM_MASS: { + return mass; + } + case PhysicsServer2D::BODY_PARAM_INERTIA: { + return inertia; + } + case PhysicsServer2D::BODY_PARAM_CENTER_OF_MASS: { + return center_of_mass_local; + } + case PhysicsServer2D::BODY_PARAM_GRAVITY_SCALE: { + return gravity_scale; + } + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP_MODE: { + return linear_damp_mode; + } + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP_MODE: { + return angular_damp_mode; + } + case PhysicsServer2D::BODY_PARAM_LINEAR_DAMP: { + return linear_damp; + } + case PhysicsServer2D::BODY_PARAM_ANGULAR_DAMP: { + return angular_damp; + } + default: { + } + } + + return 0; +} + +void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) { + PhysicsServer2D::BodyMode prev = mode; + mode = p_mode; + + switch (p_mode) { + //CLEAR UP EVERYTHING IN CASE IT NOT WORKS! + case PhysicsServer2D::BODY_MODE_STATIC: + case PhysicsServer2D::BODY_MODE_KINEMATIC: { + _set_inv_transform(get_transform().affine_inverse()); + _inv_mass = 0; + _inv_inertia = 0; + _set_static(p_mode == PhysicsServer2D::BODY_MODE_STATIC); + set_active(p_mode == PhysicsServer2D::BODY_MODE_KINEMATIC && contacts.size()); + linear_velocity = Vector2(); + angular_velocity = 0; + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && prev != mode) { + first_time_kinematic = true; + } + } break; + case PhysicsServer2D::BODY_MODE_RIGID: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + if (!calculate_inertia) { + _inv_inertia = 1.0 / inertia; + } + _mass_properties_changed(); + _set_static(false); + set_active(true); + + } break; + case PhysicsServer2D::BODY_MODE_RIGID_LINEAR: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + _inv_inertia = 0; + angular_velocity = 0; + _set_static(false); + set_active(true); + } + } +} + +PhysicsServer2D::BodyMode GodotBody2D::get_mode() const { + return mode; +} + +void GodotBody2D::_shapes_changed() { + _mass_properties_changed(); + wakeup(); + wakeup_neighbours(); +} + +void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer2D::BODY_STATE_TRANSFORM: { + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + new_transform = p_variant; + //wakeup_neighbours(); + set_active(true); + if (first_time_kinematic) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + first_time_kinematic = false; + } + } else if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + wakeup_neighbours(); + } else { + Transform2D t = p_variant; + t.orthonormalize(); + new_transform = get_transform(); //used as old to compute motion + if (t == new_transform) { + break; + } + _set_transform(t); + _set_inv_transform(get_transform().inverse()); + _update_transform_dependent(); + } + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { + linear_velocity = p_variant; + constant_linear_velocity = linear_velocity; + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { + angular_velocity = p_variant; + constant_angular_velocity = angular_velocity; + wakeup(); + + } break; + case PhysicsServer2D::BODY_STATE_SLEEPING: { + if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + break; + } + bool do_sleep = p_variant; + if (do_sleep) { + linear_velocity = Vector2(); + //biased_linear_velocity=Vector3(); + angular_velocity = 0; + //biased_angular_velocity=Vector3(); + set_active(false); + } else { + if (mode != PhysicsServer2D::BODY_MODE_STATIC) { + set_active(true); + } + } + } break; + case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { + can_sleep = p_variant; + if (mode >= PhysicsServer2D::BODY_MODE_RIGID && !active && !can_sleep) { + set_active(true); + } + + } break; + } +} + +Variant GodotBody2D::get_state(PhysicsServer2D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer2D::BODY_STATE_TRANSFORM: { + return get_transform(); + } + case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: { + return linear_velocity; + } + case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: { + return angular_velocity; + } + case PhysicsServer2D::BODY_STATE_SLEEPING: { + return !is_active(); + } + case PhysicsServer2D::BODY_STATE_CAN_SLEEP: { + return can_sleep; + } + } + + return Variant(); +} + +void GodotBody2D::set_space(GodotSpace2D *p_space) { + if (get_space()) { + wakeup_neighbours(); + + if (mass_properties_update_list.in_list()) { + get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); + } + if (active_list.in_list()) { + get_space()->body_remove_from_active_list(&active_list); + } + if (direct_state_query_list.in_list()) { + get_space()->body_remove_from_state_query_list(&direct_state_query_list); + } + } + + _set_space(p_space); + + if (get_space()) { + _mass_properties_changed(); + + if (active && !active_list.in_list()) { + get_space()->body_add_to_active_list(&active_list); + } + } +} + +void GodotBody2D::_update_transform_dependent() { + center_of_mass = get_transform().basis_xform(center_of_mass_local); +} + +void GodotBody2D::integrate_forces(real_t p_step) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + return; + } + + ERR_FAIL_NULL(get_space()); + + int ac = areas.size(); + + bool gravity_done = false; + bool linear_damp_done = false; + bool angular_damp_done = false; + + bool stopped = false; + + gravity = Vector2(0, 0); + + total_linear_damp = 0.0; + total_angular_damp = 0.0; + + // Combine gravity and damping from overlapping areas in priority order. + if (ac) { + areas.sort(); + const AreaCMP *aa = &areas[0]; + for (int i = ac - 1; i >= 0 && !stopped; i--) { + if (!gravity_done) { + PhysicsServer2D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_GRAVITY_OVERRIDE_MODE); + if (area_gravity_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + Vector2 area_gravity; + aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity); + switch (area_gravity_mode) { + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + gravity += area_gravity; + gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = area_gravity; + gravity_done = area_gravity_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!linear_damp_done) { + PhysicsServer2D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE); + if (area_linear_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + real_t area_linear_damp = aa[i].area->get_linear_damp(); + switch (area_linear_damp_mode) { + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + total_linear_damp += area_linear_damp; + linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + total_linear_damp = area_linear_damp; + linear_damp_done = area_linear_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!angular_damp_done) { + PhysicsServer2D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer2D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer2D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE); + if (area_angular_damp_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { + real_t area_angular_damp = aa[i].area->get_angular_damp(); + switch (area_angular_damp_mode) { + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + total_angular_damp += area_angular_damp; + angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + total_angular_damp = area_angular_damp; + angular_damp_done = area_angular_damp_mode == PhysicsServer2D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + stopped = gravity_done && linear_damp_done && angular_damp_done; + } + } + + // Add default gravity and damping from space area. + if (!stopped) { + GodotArea2D *default_area = get_space()->get_default_area(); + ERR_FAIL_NULL(default_area); + + if (!gravity_done) { + Vector2 default_gravity; + default_area->compute_gravity(get_transform().get_origin(), default_gravity); + gravity += default_gravity; + } + + if (!linear_damp_done) { + total_linear_damp += default_area->get_linear_damp(); + } + + if (!angular_damp_done) { + total_angular_damp += default_area->get_angular_damp(); + } + } + + // Override linear damping with body's value. + switch (linear_damp_mode) { + case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: { + total_linear_damp += linear_damp; + } break; + case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: { + total_linear_damp = linear_damp; + } break; + } + + // Override angular damping with body's value. + switch (angular_damp_mode) { + case PhysicsServer2D::BODY_DAMP_MODE_COMBINE: { + total_angular_damp += angular_damp; + } break; + case PhysicsServer2D::BODY_DAMP_MODE_REPLACE: { + total_angular_damp = angular_damp; + } break; + } + + gravity *= gravity_scale; + + prev_linear_velocity = linear_velocity; + prev_angular_velocity = angular_velocity; + + Vector2 motion; + bool do_motion = false; + + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + //compute motion, angular and etc. velocities from prev transform + motion = new_transform.get_origin() - get_transform().get_origin(); + linear_velocity = constant_linear_velocity + motion / p_step; + + real_t rot = new_transform.get_rotation() - get_transform().get_rotation(); + angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step; + + do_motion = true; + + } else { + if (!omit_force_integration) { + //overridden by direct state query + + Vector2 force = gravity * mass + applied_force + constant_force; + real_t torque = applied_torque + constant_torque; + + real_t damp = 1.0 - p_step * total_linear_damp; + + if (damp < 0) { // reached zero in the given time + damp = 0; + } + + real_t angular_damp_new = 1.0 - p_step * total_angular_damp; + + if (angular_damp_new < 0) { // reached zero in the given time + angular_damp_new = 0; + } + + linear_velocity *= damp; + angular_velocity *= angular_damp_new; + + linear_velocity += _inv_mass * force * p_step; + angular_velocity += _inv_inertia * torque * p_step; + } + + if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { + motion = linear_velocity * p_step; + do_motion = true; + } + } + + applied_force = Vector2(); + applied_torque = 0.0; + + biased_angular_velocity = 0.0; + biased_linear_velocity = Vector2(); + + if (do_motion) { //shapes temporarily extend for raycast + _update_shapes_with_motion(motion); + } + + contact_count = 0; +} + +void GodotBody2D::integrate_velocities(real_t p_step) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC) { + return; + } + + ERR_FAIL_NULL(get_space()); + + if (fi_callback_data || body_state_callback.is_valid()) { + get_space()->body_add_to_state_query_list(&direct_state_query_list); + } + + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + _set_transform(new_transform, false); + _set_inv_transform(new_transform.affine_inverse()); + if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0) { + set_active(false); //stopped moving, deactivate + } + return; + } + + real_t total_angular_velocity = angular_velocity + biased_angular_velocity; + Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity; + + real_t angle_delta = total_angular_velocity * p_step; + real_t angle = get_transform().get_rotation() + angle_delta; + Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step; + + if (center_of_mass.length_squared() > CMP_EPSILON2) { + // Calculate displacement due to center of mass offset. + pos += center_of_mass - center_of_mass.rotated(angle_delta); + } + + _set_transform(Transform2D(angle, pos), continuous_cd_mode == PhysicsServer2D::CCD_MODE_DISABLED); + _set_inv_transform(get_transform().inverse()); + + if (continuous_cd_mode != PhysicsServer2D::CCD_MODE_DISABLED) { + new_transform = get_transform(); + } + + _update_transform_dependent(); +} + +void GodotBody2D::wakeup_neighbours() { + for (const Pair<GodotConstraint2D *, int> &E : constraint_list) { + const GodotConstraint2D *c = E.first; + GodotBody2D **n = c->get_body_ptr(); + int bc = c->get_body_count(); + + for (int i = 0; i < bc; i++) { + if (i == E.second) { + continue; + } + GodotBody2D *b = n[i]; + if (b->mode < PhysicsServer2D::BODY_MODE_RIGID) { + continue; + } + + if (!b->is_active()) { + b->set_active(true); + } + } + } +} + +void GodotBody2D::call_queries() { + Variant direct_state_variant = get_direct_state(); + + if (fi_callback_data) { + if (!fi_callback_data->callable.is_valid()) { + set_force_integration_callback(Callable()); + } else { + const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; + + Callable::CallError ce; + Variant rv; + if (fi_callback_data->udata.get_type() != Variant::NIL) { + fi_callback_data->callable.callp(vp, 2, rv, ce); + + } else { + fi_callback_data->callable.callp(vp, 1, rv, ce); + } + } + } + + if (body_state_callback.is_valid()) { + body_state_callback.call(direct_state_variant); + } +} + +bool GodotBody2D::sleep_test(real_t p_step) { + if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return true; + } else if (!can_sleep) { + return false; + } + + ERR_FAIL_NULL_V(get_space(), true); + + if (Math::abs(angular_velocity) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { + still_time += p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + still_time = 0; //maybe this should be set to 0 on set_active? + return false; + } +} + +void GodotBody2D::set_state_sync_callback(const Callable &p_callable) { + body_state_callback = p_callable; +} + +void GodotBody2D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { + if (p_callable.is_valid()) { + if (!fi_callback_data) { + fi_callback_data = memnew(ForceIntegrationCallbackData); + } + fi_callback_data->callable = p_callable; + fi_callback_data->udata = p_udata; + } else if (fi_callback_data) { + memdelete(fi_callback_data); + fi_callback_data = nullptr; + } +} + +GodotPhysicsDirectBodyState2D *GodotBody2D::get_direct_state() { + if (!direct_state) { + direct_state = memnew(GodotPhysicsDirectBodyState2D); + direct_state->body = this; + } + return direct_state; +} + +GodotBody2D::GodotBody2D() : + GodotCollisionObject2D(TYPE_BODY), + active_list(this), + mass_properties_update_list(this), + direct_state_query_list(this) { + _set_static(false); +} + +GodotBody2D::~GodotBody2D() { + if (fi_callback_data) { + memdelete(fi_callback_data); + } + if (direct_state) { + memdelete(direct_state); + } +} diff --git a/modules/godot_physics_2d/godot_body_2d.h b/modules/godot_physics_2d/godot_body_2d.h new file mode 100644 index 0000000000..529305dbb2 --- /dev/null +++ b/modules/godot_physics_2d/godot_body_2d.h @@ -0,0 +1,389 @@ +/**************************************************************************/ +/* godot_body_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BODY_2D_H +#define GODOT_BODY_2D_H + +#include "godot_area_2d.h" +#include "godot_collision_object_2d.h" + +#include "core/templates/list.h" +#include "core/templates/pair.h" +#include "core/templates/vset.h" + +class GodotConstraint2D; +class GodotPhysicsDirectBodyState2D; + +class GodotBody2D : public GodotCollisionObject2D { + PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_RIGID; + + Vector2 biased_linear_velocity; + real_t biased_angular_velocity = 0.0; + + Vector2 linear_velocity; + real_t angular_velocity = 0.0; + + Vector2 prev_linear_velocity; + real_t prev_angular_velocity = 0.0; + + Vector2 constant_linear_velocity; + real_t constant_angular_velocity = 0.0; + + PhysicsServer2D::BodyDampMode linear_damp_mode = PhysicsServer2D::BODY_DAMP_MODE_COMBINE; + PhysicsServer2D::BodyDampMode angular_damp_mode = PhysicsServer2D::BODY_DAMP_MODE_COMBINE; + + real_t linear_damp = 0.0; + real_t angular_damp = 0.0; + + real_t total_linear_damp = 0.0; + real_t total_angular_damp = 0.0; + + real_t gravity_scale = 1.0; + + real_t bounce = 0.0; + real_t friction = 1.0; + + real_t mass = 1.0; + real_t _inv_mass = 1.0; + + real_t inertia = 0.0; + real_t _inv_inertia = 0.0; + + Vector2 center_of_mass_local; + Vector2 center_of_mass; + + bool calculate_inertia = true; + bool calculate_center_of_mass = true; + + Vector2 gravity; + + real_t still_time = 0.0; + + Vector2 applied_force; + real_t applied_torque = 0.0; + + Vector2 constant_force; + real_t constant_torque = 0.0; + + SelfList<GodotBody2D> active_list; + SelfList<GodotBody2D> mass_properties_update_list; + SelfList<GodotBody2D> direct_state_query_list; + + VSet<RID> exceptions; + PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; + bool omit_force_integration = false; + bool active = true; + bool can_sleep = true; + bool first_time_kinematic = false; + void _mass_properties_changed(); + virtual void _shapes_changed() override; + Transform2D new_transform; + + List<Pair<GodotConstraint2D *, int>> constraint_list; + + struct AreaCMP { + GodotArea2D *area = nullptr; + int refCount = 0; + _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } + _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } + _FORCE_INLINE_ AreaCMP() {} + _FORCE_INLINE_ AreaCMP(GodotArea2D *p_area) { + area = p_area; + refCount = 1; + } + }; + + Vector<AreaCMP> areas; + + struct Contact { + Vector2 local_pos; + Vector2 local_normal; + Vector2 local_velocity_at_pos; + real_t depth = 0.0; + int local_shape = 0; + Vector2 collider_pos; + int collider_shape = 0; + ObjectID collider_instance_id; + RID collider; + Vector2 collider_velocity_at_pos; + Vector2 impulse; + }; + + Vector<Contact> contacts; //no contacts by default + int contact_count = 0; + + Callable body_state_callback; + + struct ForceIntegrationCallbackData { + Callable callable; + Variant udata; + }; + + ForceIntegrationCallbackData *fi_callback_data = nullptr; + + GodotPhysicsDirectBodyState2D *direct_state = nullptr; + + uint64_t island_step = 0; + + void _update_transform_dependent(); + + friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose + +public: + void set_state_sync_callback(const Callable &p_callable); + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + + GodotPhysicsDirectBodyState2D *get_direct_state(); + + _FORCE_INLINE_ void add_area(GodotArea2D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount += 1; + } else { + areas.ordered_insert(AreaCMP(p_area)); + } + } + + _FORCE_INLINE_ void remove_area(GodotArea2D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount -= 1; + if (areas[index].refCount < 1) { + areas.remove_at(index); + } + } + } + + _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { + contacts.resize(p_size); + contact_count = 0; + if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC && p_size) { + set_active(true); + } + } + + _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } + + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } + _FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_local_velocity_at_pos, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &p_impulse); + + _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } + _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } + _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } + _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.erase({ p_constraint, p_pos }); } + const List<Pair<GodotConstraint2D *, int>> &get_constraint_list() const { return constraint_list; } + _FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } + + _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } + _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } + + _FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; } + _FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; } + + _FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; } + _FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; } + + _FORCE_INLINE_ Vector2 get_prev_linear_velocity() const { return prev_linear_velocity; } + _FORCE_INLINE_ real_t get_prev_angular_velocity() const { return prev_angular_velocity; } + + _FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; } + _FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; } + + _FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; } + _FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; } + + _FORCE_INLINE_ void apply_central_impulse(const Vector2 &p_impulse) { + linear_velocity += p_impulse * _inv_mass; + } + + _FORCE_INLINE_ void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) { + linear_velocity += p_impulse * _inv_mass; + angular_velocity += _inv_inertia * (p_position - center_of_mass).cross(p_impulse); + } + + _FORCE_INLINE_ void apply_torque_impulse(real_t p_torque) { + angular_velocity += _inv_inertia * p_torque; + } + + _FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2(), real_t p_max_delta_av = -1.0) { + biased_linear_velocity += p_impulse * _inv_mass; + if (p_max_delta_av != 0.0) { + real_t delta_av = _inv_inertia * (p_position - center_of_mass).cross(p_impulse); + if (p_max_delta_av > 0 && delta_av > p_max_delta_av) { + delta_av = p_max_delta_av; + } + biased_angular_velocity += delta_av; + } + } + + _FORCE_INLINE_ void apply_central_force(const Vector2 &p_force) { + applied_force += p_force; + } + + _FORCE_INLINE_ void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { + applied_force += p_force; + applied_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void apply_torque(real_t p_torque) { + applied_torque += p_torque; + } + + _FORCE_INLINE_ void add_constant_central_force(const Vector2 &p_force) { + constant_force += p_force; + } + + _FORCE_INLINE_ void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) { + constant_force += p_force; + constant_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void add_constant_torque(real_t p_torque) { + constant_torque += p_torque; + } + + void set_constant_force(const Vector2 &p_force) { constant_force = p_force; } + Vector2 get_constant_force() const { return constant_force; } + + void set_constant_torque(real_t p_torque) { constant_torque = p_torque; } + real_t get_constant_torque() const { return constant_torque; } + + void set_active(bool p_active); + _FORCE_INLINE_ bool is_active() const { return active; } + + _FORCE_INLINE_ void wakeup() { + if ((!get_space()) || mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return; + } + set_active(true); + } + + void set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer2D::BodyParameter p_param) const; + + void set_mode(PhysicsServer2D::BodyMode p_mode); + PhysicsServer2D::BodyMode get_mode() const; + + void set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer2D::BodyState p_state) const; + + _FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; } + _FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; } + + void set_space(GodotSpace2D *p_space) override; + + void update_mass_properties(); + void reset_mass_properties(); + + _FORCE_INLINE_ const Vector2 &get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ const Vector2 &get_center_of_mass_local() const { return center_of_mass_local; } + _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } + _FORCE_INLINE_ real_t get_inv_inertia() const { return _inv_inertia; } + _FORCE_INLINE_ real_t get_friction() const { return friction; } + _FORCE_INLINE_ real_t get_bounce() const { return bounce; } + + void integrate_forces(real_t p_step); + void integrate_velocities(real_t p_step); + + _FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const { + return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x); + } + + _FORCE_INLINE_ Vector2 get_motion() const { + if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { + return new_transform.get_origin() - get_transform().get_origin(); + } else if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { + return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward + } + return Vector2(); + } + + void call_queries(); + void wakeup_neighbours(); + + bool sleep_test(real_t p_step); + + GodotBody2D(); + ~GodotBody2D(); +}; + +//add contact inline + +void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_local_velocity_at_pos, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &p_impulse) { + int c_max = contacts.size(); + + if (c_max == 0) { + return; + } + + Contact *c = contacts.ptrw(); + + int idx = -1; + + if (contact_count < c_max) { + idx = contact_count++; + } else { + real_t least_depth = 1e20; + int least_deep = -1; + for (int i = 0; i < c_max; i++) { + if (i == 0 || c[i].depth < least_depth) { + least_deep = i; + least_depth = c[i].depth; + } + } + + if (least_deep >= 0 && least_depth < p_depth) { + idx = least_deep; + } + if (idx == -1) { + return; //none least deepe than this + } + } + + c[idx].local_pos = p_local_pos; + c[idx].local_normal = p_local_normal; + c[idx].local_velocity_at_pos = p_local_velocity_at_pos; + c[idx].depth = p_depth; + c[idx].local_shape = p_local_shape; + c[idx].collider_pos = p_collider_pos; + c[idx].collider_shape = p_collider_shape; + c[idx].collider_instance_id = p_collider_instance_id; + c[idx].collider = p_collider; + c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; + c[idx].impulse = p_impulse; +} + +#endif // GODOT_BODY_2D_H diff --git a/modules/godot_physics_2d/godot_body_direct_state_2d.cpp b/modules/godot_physics_2d/godot_body_direct_state_2d.cpp new file mode 100644 index 0000000000..b34c70831d --- /dev/null +++ b/modules/godot_physics_2d/godot_body_direct_state_2d.cpp @@ -0,0 +1,229 @@ +/**************************************************************************/ +/* godot_body_direct_state_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_body_direct_state_2d.h" + +#include "godot_body_2d.h" +#include "godot_physics_server_2d.h" +#include "godot_space_2d.h" + +Vector2 GodotPhysicsDirectBodyState2D::get_total_gravity() const { + return body->gravity; +} + +real_t GodotPhysicsDirectBodyState2D::get_total_angular_damp() const { + return body->total_angular_damp; +} + +real_t GodotPhysicsDirectBodyState2D::get_total_linear_damp() const { + return body->total_linear_damp; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass() const { + return body->get_center_of_mass(); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass_local() const { + return body->get_center_of_mass_local(); +} + +real_t GodotPhysicsDirectBodyState2D::get_inverse_mass() const { + return body->get_inv_mass(); +} + +real_t GodotPhysicsDirectBodyState2D::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +void GodotPhysicsDirectBodyState2D::set_linear_velocity(const Vector2 &p_velocity) { + body->wakeup(); + body->set_linear_velocity(p_velocity); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void GodotPhysicsDirectBodyState2D::set_angular_velocity(real_t p_velocity) { + body->wakeup(); + body->set_angular_velocity(p_velocity); +} + +real_t GodotPhysicsDirectBodyState2D::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void GodotPhysicsDirectBodyState2D::set_transform(const Transform2D &p_transform) { + body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform2D GodotPhysicsDirectBodyState2D::get_transform() const { + return body->get_transform(); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_velocity_at_local_position(const Vector2 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) { + body->wakeup(); + body->apply_central_impulse(p_impulse); +} + +void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { + body->wakeup(); + body->apply_impulse(p_impulse, p_position); +} + +void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) { + body->wakeup(); + body->apply_torque_impulse(p_torque); +} + +void GodotPhysicsDirectBodyState2D::apply_central_force(const Vector2 &p_force) { + body->wakeup(); + body->apply_central_force(p_force); +} + +void GodotPhysicsDirectBodyState2D::apply_force(const Vector2 &p_force, const Vector2 &p_position) { + body->wakeup(); + body->apply_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState2D::apply_torque(real_t p_torque) { + body->wakeup(); + body->apply_torque(p_torque); +} + +void GodotPhysicsDirectBodyState2D::add_constant_central_force(const Vector2 &p_force) { + body->wakeup(); + body->add_constant_central_force(p_force); +} + +void GodotPhysicsDirectBodyState2D::add_constant_force(const Vector2 &p_force, const Vector2 &p_position) { + body->wakeup(); + body->add_constant_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState2D::add_constant_torque(real_t p_torque) { + body->wakeup(); + body->add_constant_torque(p_torque); +} + +void GodotPhysicsDirectBodyState2D::set_constant_force(const Vector2 &p_force) { + if (!p_force.is_zero_approx()) { + body->wakeup(); + } + body->set_constant_force(p_force); +} + +Vector2 GodotPhysicsDirectBodyState2D::get_constant_force() const { + return body->get_constant_force(); +} + +void GodotPhysicsDirectBodyState2D::set_constant_torque(real_t p_torque) { + if (!Math::is_zero_approx(p_torque)) { + body->wakeup(); + } + body->set_constant_torque(p_torque); +} + +real_t GodotPhysicsDirectBodyState2D::get_constant_torque() const { + return body->get_constant_torque(); +} + +void GodotPhysicsDirectBodyState2D::set_sleep_state(bool p_enable) { + body->set_active(!p_enable); +} + +bool GodotPhysicsDirectBodyState2D::is_sleeping() const { + return !body->is_active(); +} + +int GodotPhysicsDirectBodyState2D::get_contact_count() const { + return body->contact_count; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_normal; +} + +int GodotPhysicsDirectBodyState2D::get_contact_local_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); + return body->contacts[p_contact_idx].local_shape; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].local_velocity_at_pos; +} + +RID GodotPhysicsDirectBodyState2D::get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); + return body->contacts[p_contact_idx].collider; +} +Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID GodotPhysicsDirectBodyState2D::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int GodotPhysicsDirectBodyState2D::get_contact_collider_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); + return body->contacts[p_contact_idx].collider_shape; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; +} + +Vector2 GodotPhysicsDirectBodyState2D::get_contact_impulse(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); + return body->contacts[p_contact_idx].impulse; +} + +PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t GodotPhysicsDirectBodyState2D::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/modules/godot_physics_2d/godot_body_direct_state_2d.h b/modules/godot_physics_2d/godot_body_direct_state_2d.h new file mode 100644 index 0000000000..90b7c1d369 --- /dev/null +++ b/modules/godot_physics_2d/godot_body_direct_state_2d.h @@ -0,0 +1,104 @@ +/**************************************************************************/ +/* godot_body_direct_state_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BODY_DIRECT_STATE_2D_H +#define GODOT_BODY_DIRECT_STATE_2D_H + +#include "servers/physics_server_2d.h" + +class GodotBody2D; + +class GodotPhysicsDirectBodyState2D : public PhysicsDirectBodyState2D { + GDCLASS(GodotPhysicsDirectBodyState2D, PhysicsDirectBodyState2D); + +public: + GodotBody2D *body = nullptr; + + virtual Vector2 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual Vector2 get_center_of_mass() const override; + virtual Vector2 get_center_of_mass_local() const override; + virtual real_t get_inverse_mass() const override; + virtual real_t get_inverse_inertia() const override; + + virtual void set_linear_velocity(const Vector2 &p_velocity) override; + virtual Vector2 get_linear_velocity() const override; + + virtual void set_angular_velocity(real_t p_velocity) override; + virtual real_t get_angular_velocity() const override; + + virtual void set_transform(const Transform2D &p_transform) override; + virtual Transform2D get_transform() const override; + + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override; + + virtual void apply_central_impulse(const Vector2 &p_impulse) override; + virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + virtual void apply_torque_impulse(real_t p_torque) override; + + virtual void apply_central_force(const Vector2 &p_force) override; + virtual void apply_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void apply_torque(real_t p_torque) override; + + virtual void add_constant_central_force(const Vector2 &p_force) override; + virtual void add_constant_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void add_constant_torque(real_t p_torque) override; + + virtual void set_constant_force(const Vector2 &p_force) override; + virtual Vector2 get_constant_force() const override; + + virtual void set_constant_torque(real_t p_torque) override; + virtual real_t get_constant_torque() const override; + + virtual void set_sleep_state(bool p_enable) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector2 get_contact_local_position(int p_contact_idx) const override; + virtual Vector2 get_contact_local_normal(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + virtual Vector2 get_contact_local_velocity_at_position(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector2 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + virtual Vector2 get_contact_impulse(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState2D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // GODOT_BODY_DIRECT_STATE_2D_H diff --git a/modules/godot_physics_2d/godot_body_pair_2d.cpp b/modules/godot_physics_2d/godot_body_pair_2d.cpp new file mode 100644 index 0000000000..98f11d6c07 --- /dev/null +++ b/modules/godot_physics_2d/godot_body_pair_2d.cpp @@ -0,0 +1,610 @@ +/**************************************************************************/ +/* godot_body_pair_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_body_pair_2d.h" + +#include "godot_collision_solver_2d.h" +#include "godot_space_2d.h" + +#define ACCUMULATE_IMPULSES + +#define MIN_VELOCITY 0.001 +#define MAX_BIAS_ROTATION (Math_PI / 8) + +void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { + GodotBodyPair2D *self = static_cast<GodotBodyPair2D *>(p_self); + + self->_contact_added_callback(p_point_A, p_point_B); +} + +void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) { + Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); + Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B); + + int new_index = contact_count; + + ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); + + Contact contact; + contact.local_A = local_A; + contact.local_B = local_B; + contact.normal = (p_point_A - p_point_B).normalized(); + contact.used = true; + + // Attempt to determine if the contact will be reused. + real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius(); + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (c.local_A.distance_squared_to(local_A) < (recycle_radius_2) && + c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) { + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; + c = contact; + return; + } + } + + // Figure out if the contact amount must be reduced to fit the new contact. + if (new_index == MAX_CONTACTS) { + // Remove the contact with the minimum depth. + + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + int least_deep = -1; + real_t min_depth; + + // Start with depth for new contact. + { + Vector2 global_A = transform_A.basis_xform(contact.local_A); + Vector2 global_B = transform_B.basis_xform(contact.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + min_depth = axis.dot(contact.normal); + } + + for (int i = 0; i < contact_count; i++) { + const Contact &c = contacts[i]; + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < min_depth) { + min_depth = depth; + least_deep = i; + } + } + + if (least_deep > -1) { + // Replace the least deep contact by the new one. + contacts[least_deep] = contact; + } + + return; + } + + contacts[new_index] = contact; + contact_count++; +} + +void GodotBodyPair2D::_validate_contacts() { + // Make sure to erase contacts that are no longer valid. + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; + + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + + bool erase = false; + if (!c.used) { + // Was left behind in previous frame. + erase = true; + } else { + c.used = false; + + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } + + if (erase) { + // Contact no longer needed, remove. + + if ((i + 1) < contact_count) { + // Swap with the last one. + SWAP(contacts[i], contacts[contact_count - 1]); + } + + i--; + contact_count--; + } + } +} + +// `_test_ccd` prevents tunneling by slowing down a high velocity body that is about to collide so +// that next frame it will be at an appropriate location to collide (i.e. slight overlap). +// WARNING: The way velocity is adjusted down to cause a collision means the momentum will be +// weaker than it should for a bounce! +// Process: Only proceed if body A's motion is high relative to its size. +// Cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does. +// Adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it. +bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) { + Vector2 motion = p_A->get_linear_velocity() * p_step; + real_t mlen = motion.length(); + if (mlen < CMP_EPSILON) { + return false; + } + + Vector2 mnormal = motion / mlen; + + real_t min = 0.0, max = 0.0; + p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max); + + // Did it move enough in this direction to even attempt raycast? + // Let's say it should move more than 1/3 the size of the object in that axis. + bool fast_object = mlen > (max - min) * 0.3; + if (!fast_object) { + return false; + } + + // A is moving fast enough that tunneling might occur. See if it's really about to collide. + + // Roughly predict body B's position in the next frame (ignoring collisions). + Transform2D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step); + + // Cast a segment from support in motion normal, in the same direction of motion by motion length. + // Support point will the farthest forward collision point along the movement vector. + // i.e. the point that should hit B first if any collision does occur. + + // convert mnormal into body A's local xform because get_support requires (and returns) local coordinates. + int a; + Vector2 s[2]; + p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform_inv(mnormal).normalized(), s, a); + Vector2 from = p_xform_A.xform(s[0]); + // Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast. + // This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close. + Vector2 to = from + motion; + + Transform2D from_inv = predicted_xform_B.affine_inverse(); + + // Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast. + // At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd. But it still works out. + Vector2 local_from = from_inv.xform(from - motion * 0.1); + Vector2 local_to = from_inv.xform(to); + + Vector2 rpos, rnorm; + if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) { + // there was no hit. Since the segment is the length of per-frame motion, this means the bodies will not + // actually collide yet on next frame. We'll probably check again next frame once they're closer. + return false; + } + + // Check one-way collision based on motion direction. + if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) { + Vector2 direction = predicted_xform_B.columns[1].normalized(); + if (direction.dot(mnormal) < CMP_EPSILON) { + collided = false; + oneway_disabled = true; + return false; + } + } + + // Shorten the linear velocity so it does not hit, but gets close enough, + // next frame will hit softly or soft enough. + Vector2 hitpos = predicted_xform_B.xform(rpos); + + real_t newlen = hitpos.distance_to(from) + (max - min) * 0.01; // adding 1% of body length to the distance between collision and support point should cause body A's support point to arrive just within B's collider next frame. + p_A->set_linear_velocity(mnormal * (newlen / p_step)); + + return true; +} + +real_t combine_bounce(GodotBody2D *A, GodotBody2D *B) { + return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); +} + +real_t combine_friction(GodotBody2D *A, GodotBody2D *B) { + return ABS(MIN(A->get_friction(), B->get_friction())); +} + +bool GodotBodyPair2D::setup(real_t p_step) { + check_ccd = false; + + if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { + collided = false; + return false; + } + + collide_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) && B->collides_with(A); + + report_contacts_only = false; + if (!collide_A && !collide_B) { + if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + + //use local A coordinates to avoid numerical issues on collision detection + offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); + + _validate_contacts(); + + const Vector2 &offset_A = A->get_transform().get_origin(); + Transform2D xform_Au = A->get_transform().untranslated(); + Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform2D xform_Bu = B->get_transform(); + xform_Bu.columns[2] -= offset_A; + Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + GodotShape2D *shape_A_ptr = A->get_shape(shape_A); + GodotShape2D *shape_B_ptr = B->get_shape(shape_B); + + Vector2 motion_A, motion_B; + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { + motion_A = A->get_motion(); + } + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_SHAPE) { + motion_B = B->get_motion(); + } + + bool prev_collided = collided; + + collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); + if (!collided) { + oneway_disabled = false; + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { + check_ccd = true; + return true; + } + + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { + check_ccd = true; + return true; + } + + return false; + } + + if (oneway_disabled) { + return false; + } + + if (!prev_collided) { + if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) { + Vector2 direction = xform_A.columns[1].normalized(); + bool valid = false; + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (c.normal.dot(direction) > -CMP_EPSILON) { // Greater (normal inverted). + continue; + } + valid = true; + break; + } + if (!valid) { + collided = false; + oneway_disabled = true; + return false; + } + } + + if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) { + Vector2 direction = xform_B.columns[1].normalized(); + bool valid = false; + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (c.normal.dot(direction) < CMP_EPSILON) { // Less (normal ok). + continue; + } + valid = true; + break; + } + if (!valid) { + collided = false; + oneway_disabled = true; + return false; + } + } + } + + return true; +} + +bool GodotBodyPair2D::pre_solve(real_t p_step) { + if (oneway_disabled) { + return false; + } + + if (!collided) { + if (check_ccd) { + const Vector2 &offset_A = A->get_transform().get_origin(); + Transform2D xform_Au = A->get_transform().untranslated(); + Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform2D xform_Bu = B->get_transform(); + xform_Bu.columns[2] -= offset_A; + Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { + _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); + } + + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { + _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); + } + } + + return false; + } + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + real_t bias = space->get_contact_bias(); + + GodotShape2D *shape_A_ptr = A->get_shape(shape_A); + GodotShape2D *shape_B_ptr = B->get_shape(shape_B); + + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { + if (shape_A_ptr->get_custom_bias() == 0) { + bias = shape_B_ptr->get_custom_bias(); + } else if (shape_B_ptr->get_custom_bias() == 0) { + bias = shape_A_ptr->get_custom_bias(); + } else { + bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; + } + } + + real_t inv_dt = 1.0 / p_step; + + bool do_process = false; + + const Vector2 &offset_A = A->get_transform().get_origin(); + const Transform2D &transform_A = A->get_transform(); + const Transform2D &transform_B = B->get_transform(); + + real_t inv_inertia_A = collide_A ? A->get_inv_inertia() : 0.0; + real_t inv_inertia_B = collide_B ? B->get_inv_inertia() : 0.0; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + c.active = false; + + Vector2 global_A = transform_A.basis_xform(c.local_A); + Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B; + + Vector2 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth <= 0.0) { + continue; + } + +#ifdef DEBUG_ENABLED + if (space->is_debugging_contacts()) { + space->add_debug_contact(global_A + offset_A); + space->add_debug_contact(global_B + offset_A); + } +#endif + + c.rA = global_A - A->get_center_of_mass(); + c.rB = global_B - B->get_center_of_mass() - offset_B; + + // Precompute normal mass, tangent mass, and bias. + real_t rnA = c.rA.dot(c.normal); + real_t rnB = c.rB.dot(c.normal); + real_t kNormal = inv_mass_A + inv_mass_B; + kNormal += inv_inertia_A * (c.rA.dot(c.rA) - rnA * rnA) + inv_inertia_B * (c.rB.dot(c.rB) - rnB * rnB); + c.mass_normal = 1.0f / kNormal; + + Vector2 tangent = c.normal.orthogonal(); + real_t rtA = c.rA.dot(tangent); + real_t rtB = c.rB.dot(tangent); + real_t kTangent = inv_mass_A + inv_mass_B; + kTangent += inv_inertia_A * (c.rA.dot(c.rA) - rtA * rtA) + inv_inertia_B * (c.rB.dot(c.rB) - rtB * rtB); + c.mass_tangent = 1.0f / kTangent; + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + c.depth = depth; + + Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; + + c.acc_impulse -= P; + + if (A->can_report_contacts() || B->can_report_contacts()) { + Vector2 crB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity(); + Vector2 crA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity(); + if (A->can_report_contacts()) { + A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse); + } + if (B->can_report_contacts()) { + B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, c.acc_impulse); + } + } + + if (report_contacts_only) { + collided = false; + continue; + } + +#ifdef ACCUMULATE_IMPULSES + { + // Apply normal + friction impulse + if (collide_A) { + A->apply_impulse(-P, c.rA + A->get_center_of_mass()); + } + if (collide_B) { + B->apply_impulse(P, c.rB + B->get_center_of_mass()); + } + } +#endif + + c.bounce = combine_bounce(A, B); + if (c.bounce) { + Vector2 crA(-A->get_prev_angular_velocity() * c.rA.y, A->get_prev_angular_velocity() * c.rA.x); + Vector2 crB(-B->get_prev_angular_velocity() * c.rB.y, B->get_prev_angular_velocity() * c.rB.x); + Vector2 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA; + c.bounce = c.bounce * dv.dot(c.normal); + } + + c.active = true; + do_process = true; + } + + return do_process; +} + +void GodotBodyPair2D::solve(real_t p_step) { + if (!collided || oneway_disabled) { + return; + } + + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + + for (int i = 0; i < contact_count; ++i) { + Contact &c = contacts[i]; + + if (!c.active) { + continue; + } + + // Relative velocity at contact + + Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x); + Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x); + Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + + Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x); + Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x); + Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + real_t vn = dv.dot(c.normal); + real_t vbn = dbv.dot(c.normal); + + Vector2 tangent = c.normal.orthogonal(); + real_t vt = dv.dot(tangent); + + real_t jbn = (c.bias - vbn) * c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + if (collide_A) { + A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); + } + if (collide_B) { + B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); + } + + crbA = Vector2(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x); + crbB = Vector2(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x); + dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + Vector2 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); + + if (collide_A) { + A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); + } + if (collide_B) { + B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + } + } + + real_t jn = -(c.bounce + vn) * c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + real_t friction = combine_friction(A, B); + + real_t jtMax = friction * c.acc_normal_impulse; + real_t jt = -vt * c.mass_tangent; + real_t jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax); + + Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld); + + if (collide_A) { + A->apply_impulse(-j, c.rA + A->get_center_of_mass()); + } + if (collide_B) { + B->apply_impulse(j, c.rB + B->get_center_of_mass()); + } + c.acc_impulse -= j; + } +} + +GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) : + GodotConstraint2D(_arr, 2) { + A = p_A; + B = p_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + space = A->get_space(); + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotBodyPair2D::~GodotBodyPair2D() { + A->remove_constraint(this, 0); + B->remove_constraint(this, 1); +} diff --git a/modules/godot_physics_2d/godot_body_pair_2d.h b/modules/godot_physics_2d/godot_body_pair_2d.h new file mode 100644 index 0000000000..4e9bfa6022 --- /dev/null +++ b/modules/godot_physics_2d/godot_body_pair_2d.h @@ -0,0 +1,101 @@ +/**************************************************************************/ +/* godot_body_pair_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BODY_PAIR_2D_H +#define GODOT_BODY_PAIR_2D_H + +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotBodyPair2D : public GodotConstraint2D { + enum { + MAX_CONTACTS = 2 + }; + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + int shape_A = 0; + int shape_B = 0; + + bool collide_A = false; + bool collide_B = false; + + GodotSpace2D *space = nullptr; + + struct Contact { + Vector2 position; + Vector2 normal; + Vector2 local_A, local_B; + Vector2 acc_impulse; // accumulated impulse + real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) + real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt) + real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) + real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com + real_t mass_normal, mass_tangent = 0.0; + real_t bias = 0.0; + + real_t depth = 0.0; + bool active = false; + bool used = false; + Vector2 rA, rB; + real_t bounce = 0.0; + }; + + Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection + + Vector2 sep_axis; + Contact contacts[MAX_CONTACTS]; + int contact_count = 0; + bool collided = false; + bool check_ccd = false; + bool oneway_disabled = false; + bool report_contacts_only = false; + + bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B); + void _validate_contacts(); + static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self); + _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B); + ~GodotBodyPair2D(); +}; + +#endif // GODOT_BODY_PAIR_2D_H diff --git a/modules/squish/image_decompress_squish.h b/modules/godot_physics_2d/godot_broad_phase_2d.cpp index 53c5d344a5..eb6bc21d60 100644 --- a/modules/squish/image_decompress_squish.h +++ b/modules/godot_physics_2d/godot_broad_phase_2d.cpp @@ -1,5 +1,5 @@ /**************************************************************************/ -/* image_decompress_squish.h */ +/* godot_broad_phase_2d.cpp */ /**************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,11 +28,9 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /**************************************************************************/ -#ifndef IMAGE_DECOMPRESS_SQUISH_H -#define IMAGE_DECOMPRESS_SQUISH_H +#include "godot_broad_phase_2d.h" -#include "core/io/image.h" +GodotBroadPhase2D::CreateFunction GodotBroadPhase2D::create_func = nullptr; -void image_decompress_squish(Image *p_image); - -#endif // IMAGE_DECOMPRESS_SQUISH_H +GodotBroadPhase2D::~GodotBroadPhase2D() { +} diff --git a/modules/godot_physics_2d/godot_broad_phase_2d.h b/modules/godot_physics_2d/godot_broad_phase_2d.h new file mode 100644 index 0000000000..f3c07a69bb --- /dev/null +++ b/modules/godot_physics_2d/godot_broad_phase_2d.h @@ -0,0 +1,71 @@ +/**************************************************************************/ +/* godot_broad_phase_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BROAD_PHASE_2D_H +#define GODOT_BROAD_PHASE_2D_H + +#include "core/math/math_funcs.h" +#include "core/math/rect2.h" + +class GodotCollisionObject2D; + +class GodotBroadPhase2D { +public: + typedef GodotBroadPhase2D *(*CreateFunction)(); + + static CreateFunction create_func; + + typedef uint32_t ID; + + typedef void *(*PairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_userdata); + typedef void (*UnpairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_userdata); + + // 0 is an invalid ID + virtual ID create(GodotCollisionObject2D *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; + virtual void move(ID p_id, const Rect2 &p_aabb) = 0; + virtual void set_static(ID p_id, bool p_static) = 0; + virtual void remove(ID p_id) = 0; + + virtual GodotCollisionObject2D *get_object(ID p_id) const = 0; + virtual bool is_static(ID p_id) const = 0; + virtual int get_subindex(ID p_id) const = 0; + + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; + + virtual void update() = 0; + + virtual ~GodotBroadPhase2D(); +}; + +#endif // GODOT_BROAD_PHASE_2D_H diff --git a/modules/godot_physics_2d/godot_broad_phase_2d_bvh.cpp b/modules/godot_physics_2d/godot_broad_phase_2d_bvh.cpp new file mode 100644 index 0000000000..59623a2667 --- /dev/null +++ b/modules/godot_physics_2d/godot_broad_phase_2d_bvh.cpp @@ -0,0 +1,123 @@ +/**************************************************************************/ +/* godot_broad_phase_2d_bvh.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_broad_phase_2d_bvh.h" +#include "godot_collision_object_2d.h" + +GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { + uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC; + uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC); + ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care? + return oid + 1; +} + +void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { + ERR_FAIL_COND(!p_id); + bvh.move(p_id - 1, p_aabb); +} + +void GodotBroadPhase2DBVH::set_static(ID p_id, bool p_static) { + ERR_FAIL_COND(!p_id); + uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC; + uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC); + bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false); +} + +void GodotBroadPhase2DBVH::remove(ID p_id) { + ERR_FAIL_COND(!p_id); + bvh.erase(p_id - 1); +} + +GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const { + ERR_FAIL_COND_V(!p_id, nullptr); + GodotCollisionObject2D *it = bvh.get(p_id - 1); + ERR_FAIL_NULL_V(it, nullptr); + return it; +} + +bool GodotBroadPhase2DBVH::is_static(ID p_id) const { + ERR_FAIL_COND_V(!p_id, false); + uint32_t tree_id = bvh.get_tree_id(p_id - 1); + return tree_id == 0; +} + +int GodotBroadPhase2DBVH::get_subindex(ID p_id) const { + ERR_FAIL_COND_V(!p_id, 0); + return bvh.get_subindex(p_id - 1); +} + +int GodotBroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); +} + +int GodotBroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_aabb(p_aabb, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); +} + +void *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B) { + GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self); + if (!bpo->pair_callback) { + return nullptr; + } + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void GodotBroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B, void *pairdata) { + GodotBroadPhase2DBVH *bpo = static_cast<GodotBroadPhase2DBVH *>(self); + if (!bpo->unpair_callback) { + return; + } + + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); +} + +void GodotBroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + pair_callback = p_pair_callback; + pair_userdata = p_userdata; +} + +void GodotBroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; +} + +void GodotBroadPhase2DBVH::update() { + bvh.update(); +} + +GodotBroadPhase2D *GodotBroadPhase2DBVH::_create() { + return memnew(GodotBroadPhase2DBVH); +} + +GodotBroadPhase2DBVH::GodotBroadPhase2DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); +} diff --git a/modules/godot_physics_2d/godot_broad_phase_2d_bvh.h b/modules/godot_physics_2d/godot_broad_phase_2d_bvh.h new file mode 100644 index 0000000000..6c1fae5cb2 --- /dev/null +++ b/modules/godot_physics_2d/godot_broad_phase_2d_bvh.h @@ -0,0 +1,101 @@ +/**************************************************************************/ +/* godot_broad_phase_2d_bvh.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BROAD_PHASE_2D_BVH_H +#define GODOT_BROAD_PHASE_2D_BVH_H + +#include "godot_broad_phase_2d.h" + +#include "core/math/bvh.h" +#include "core/math/rect2.h" +#include "core/math/vector2.h" + +class GodotBroadPhase2DBVH : public GodotBroadPhase2D { + template <typename T> + class UserPairTestFunction { + public: + static bool user_pair_check(const T *p_a, const T *p_b) { + // return false if no collision, decided by masks etc + return p_a->interacts_with(p_b); + } + }; + + template <typename T> + class UserCullTestFunction { + public: + static bool user_cull_check(const T *p_a, const T *p_b) { + return true; + } + }; + + enum Tree { + TREE_STATIC = 0, + TREE_DYNAMIC = 1, + }; + + enum TreeFlag { + TREE_FLAG_STATIC = 1 << TREE_STATIC, + TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC, + }; + + BVH_Manager<GodotCollisionObject2D, 2, true, 128, UserPairTestFunction<GodotCollisionObject2D>, UserCullTestFunction<GodotCollisionObject2D>, Rect2, Vector2> bvh; + + static void *_pair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int); + static void _unpair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int, void *); + + PairCallback pair_callback = nullptr; + void *pair_userdata = nullptr; + UnpairCallback unpair_callback = nullptr; + void *unpair_userdata = nullptr; + +public: + // 0 is an invalid ID + virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) override; + virtual void move(ID p_id, const Rect2 &p_aabb) override; + virtual void set_static(ID p_id, bool p_static) override; + virtual void remove(ID p_id) override; + + virtual GodotCollisionObject2D *get_object(ID p_id) const override; + virtual bool is_static(ID p_id) const override; + virtual int get_subindex(ID p_id) const override; + + virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override; + virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override; + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) override; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) override; + + virtual void update() override; + + static GodotBroadPhase2D *_create(); + GodotBroadPhase2DBVH(); +}; + +#endif // GODOT_BROAD_PHASE_2D_BVH_H diff --git a/modules/godot_physics_2d/godot_collision_object_2d.cpp b/modules/godot_physics_2d/godot_collision_object_2d.cpp new file mode 100644 index 0000000000..9851cac140 --- /dev/null +++ b/modules/godot_physics_2d/godot_collision_object_2d.cpp @@ -0,0 +1,244 @@ +/**************************************************************************/ +/* godot_collision_object_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_collision_object_2d.h" +#include "godot_physics_server_2d.h" +#include "godot_space_2d.h" + +void GodotCollisionObject2D::add_shape(GodotShape2D *p_shape, const Transform2D &p_transform, bool p_disabled) { + Shape s; + s.shape = p_shape; + s.xform = p_transform; + s.xform_inv = s.xform.affine_inverse(); + s.bpid = 0; //needs update + s.disabled = p_disabled; + s.one_way_collision = false; + s.one_way_collision_margin = 0; + shapes.push_back(s); + p_shape->add_owner(this); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape(int p_index, GodotShape2D *p_shape) { + ERR_FAIL_INDEX(p_index, shapes.size()); + shapes[p_index].shape->remove_owner(this); + shapes.write[p_index].shape = p_shape; + + p_shape->add_owner(this); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape_transform(int p_index, const Transform2D &p_transform) { + ERR_FAIL_INDEX(p_index, shapes.size()); + + shapes.write[p_index].xform = p_transform; + shapes.write[p_index].xform_inv = p_transform.affine_inverse(); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject2D::set_shape_disabled(int p_idx, bool p_disabled) { + ERR_FAIL_INDEX(p_idx, shapes.size()); + + GodotCollisionObject2D::Shape &shape = shapes.write[p_idx]; + if (shape.disabled == p_disabled) { + return; + } + + shape.disabled = p_disabled; + + if (!space) { + return; + } + + if (p_disabled && shape.bpid != 0) { + space->get_broadphase()->remove(shape.bpid); + shape.bpid = 0; + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } else if (!p_disabled && shape.bpid == 0) { + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } +} + +void GodotCollisionObject2D::remove_shape(GodotShape2D *p_shape) { + //remove a shape, all the times it appears + for (int i = 0; i < shapes.size(); i++) { + if (shapes[i].shape == p_shape) { + remove_shape(i); + i--; + } + } +} + +void GodotCollisionObject2D::remove_shape(int p_index) { + //remove anything from shape to be erased to end, so subindices don't change + ERR_FAIL_INDEX(p_index, shapes.size()); + for (int i = p_index; i < shapes.size(); i++) { + if (shapes[i].bpid == 0) { + continue; + } + //should never get here with a null owner + space->get_broadphase()->remove(shapes[i].bpid); + shapes.write[i].bpid = 0; + } + shapes[p_index].shape->remove_owner(this); + shapes.remove_at(p_index); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + // _update_shapes(); + // _shapes_changed(); +} + +void GodotCollisionObject2D::_set_static(bool p_static) { + if (_static == p_static) { + return; + } + _static = p_static; + + if (!space) { + return; + } + for (int i = 0; i < get_shape_count(); i++) { + const Shape &s = shapes[i]; + if (s.bpid > 0) { + space->get_broadphase()->set_static(s.bpid, _static); + } + } +} + +void GodotCollisionObject2D::_unregister_shapes() { + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid > 0) { + space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } +} + +void GodotCollisionObject2D::_update_shapes() { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + Rect2 shape_aabb = s.shape->get_aabb(); + Transform2D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); + s.aabb_cache = shape_aabb; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject2D::_update_shapes_with_motion(const Vector2 &p_motion) { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + Rect2 shape_aabb = s.shape->get_aabb(); + Transform2D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion + s.aabb_cache = shape_aabb; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject2D::_set_space(GodotSpace2D *p_space) { + GodotSpace2D *old_space = space; + space = p_space; + + if (old_space) { + old_space->remove_object(this); + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid) { + old_space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } + } + + if (space) { + space->add_object(this); + _update_shapes(); + } +} + +void GodotCollisionObject2D::_shape_changed() { + _update_shapes(); + _shapes_changed(); +} + +GodotCollisionObject2D::GodotCollisionObject2D(Type p_type) : + pending_shape_update_list(this) { + type = p_type; +} diff --git a/modules/godot_physics_2d/godot_collision_object_2d.h b/modules/godot_physics_2d/godot_collision_object_2d.h new file mode 100644 index 0000000000..129fa27ff3 --- /dev/null +++ b/modules/godot_physics_2d/godot_collision_object_2d.h @@ -0,0 +1,198 @@ +/**************************************************************************/ +/* godot_collision_object_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_COLLISION_OBJECT_2D_H +#define GODOT_COLLISION_OBJECT_2D_H + +#include "godot_broad_phase_2d.h" +#include "godot_shape_2d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_2d.h" + +class GodotSpace2D; + +class GodotCollisionObject2D : public GodotShapeOwner2D { +public: + enum Type { + TYPE_AREA, + TYPE_BODY + }; + +private: + Type type; + RID self; + ObjectID instance_id; + ObjectID canvas_instance_id; + bool pickable = true; + + struct Shape { + Transform2D xform; + Transform2D xform_inv; + GodotBroadPhase2D::ID bpid = 0; + Rect2 aabb_cache; //for rayqueries + GodotShape2D *shape = nullptr; + bool disabled = false; + bool one_way_collision = false; + real_t one_way_collision_margin = 0.0; + }; + + Vector<Shape> shapes; + GodotSpace2D *space = nullptr; + Transform2D transform; + Transform2D inv_transform; + uint32_t collision_mask = 1; + uint32_t collision_layer = 1; + real_t collision_priority = 1.0; + bool _static = true; + + SelfList<GodotCollisionObject2D> pending_shape_update_list; + + void _update_shapes(); + +protected: + void _update_shapes_with_motion(const Vector2 &p_motion); + void _unregister_shapes(); + + _FORCE_INLINE_ void _set_transform(const Transform2D &p_transform, bool p_update_shapes = true) { + transform = p_transform; + if (p_update_shapes) { + _update_shapes(); + } + } + _FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; } + void _set_static(bool p_static); + + virtual void _shapes_changed() = 0; + void _set_space(GodotSpace2D *p_space); + + GodotCollisionObject2D(Type p_type); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } + _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } + + _FORCE_INLINE_ void set_canvas_instance_id(const ObjectID &p_canvas_instance_id) { canvas_instance_id = p_canvas_instance_id; } + _FORCE_INLINE_ ObjectID get_canvas_instance_id() const { return canvas_instance_id; } + + void _shape_changed() override; + + _FORCE_INLINE_ Type get_type() const { return type; } + void add_shape(GodotShape2D *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false); + void set_shape(int p_index, GodotShape2D *p_shape); + void set_shape_transform(int p_index, const Transform2D &p_transform); + + _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } + _FORCE_INLINE_ GodotShape2D *get_shape(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].shape; + } + _FORCE_INLINE_ const Transform2D &get_shape_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform; + } + _FORCE_INLINE_ const Transform2D &get_shape_inv_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform_inv; + } + _FORCE_INLINE_ const Rect2 &get_shape_aabb(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].aabb_cache; + } + + _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ GodotSpace2D *get_space() const { return space; } + + void set_shape_disabled(int p_idx, bool p_disabled); + _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { + ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); + return shapes[p_idx].disabled; + } + + _FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) { + CRASH_BAD_INDEX(p_idx, shapes.size()); + shapes.write[p_idx].one_way_collision = p_one_way_collision; + shapes.write[p_idx].one_way_collision_margin = p_margin; + } + _FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const { + CRASH_BAD_INDEX(p_idx, shapes.size()); + return shapes[p_idx].one_way_collision; + } + + _FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const { + CRASH_BAD_INDEX(p_idx, shapes.size()); + return shapes[p_idx].one_way_collision_margin; + } + + void set_collision_mask(uint32_t p_mask) { + collision_mask = p_mask; + _shape_changed(); + } + _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } + + void set_collision_layer(uint32_t p_layer) { + collision_layer = p_layer; + _shape_changed(); + } + _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } + + _FORCE_INLINE_ void set_collision_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0."); + collision_priority = p_priority; + _shape_changed(); + } + _FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; } + + void remove_shape(GodotShape2D *p_shape) override; + void remove_shape(int p_index); + + virtual void set_space(GodotSpace2D *p_space) = 0; + + _FORCE_INLINE_ bool is_static() const { return _static; } + + void set_pickable(bool p_pickable) { pickable = p_pickable; } + _FORCE_INLINE_ bool is_pickable() const { return pickable; } + + _FORCE_INLINE_ bool collides_with(GodotCollisionObject2D *p_other) const { + return p_other->collision_layer & collision_mask; + } + + _FORCE_INLINE_ bool interacts_with(const GodotCollisionObject2D *p_other) const { + return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; + } + + virtual ~GodotCollisionObject2D() {} +}; + +#endif // GODOT_COLLISION_OBJECT_2D_H diff --git a/modules/godot_physics_2d/godot_collision_solver_2d.cpp b/modules/godot_physics_2d/godot_collision_solver_2d.cpp new file mode 100644 index 0000000000..a1acbe9cf0 --- /dev/null +++ b/modules/godot_physics_2d/godot_collision_solver_2d.cpp @@ -0,0 +1,274 @@ +/**************************************************************************/ +/* godot_collision_solver_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_collision_solver_2d.h" +#include "godot_collision_solver_2d_sat.h" + +#define collision_solver sat_2d_calculate_penetration +//#define collision_solver gjk_epa_calculate_penetration + +bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { + const GodotWorldBoundaryShape2D *world_boundary = static_cast<const GodotWorldBoundaryShape2D *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + return false; + } + + Vector2 n = p_transform_A.basis_xform(world_boundary->get_normal()).normalized(); + Vector2 p = p_transform_A.xform(world_boundary->get_normal() * world_boundary->get_d()); + real_t d = n.dot(p); + + Vector2 supports[2]; + int support_count; + + p_shape_B->get_supports(p_transform_B.affine_inverse().basis_xform(-n).normalized(), supports, support_count); + + bool found = false; + + for (int i = 0; i < support_count; i++) { + supports[i] += p_margin * supports[i].normalized(); + supports[i] = p_transform_B.xform(supports[i]); + supports[i] += p_motion_B; + real_t pd = n.dot(supports[i]); + if (pd >= d) { + continue; + } + found = true; + + Vector2 support_A = supports[i] - n * (pd - d); + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(supports[i], support_A, p_userdata); + } else { + p_result_callback(support_A, supports[i], p_userdata); + } + } + } + + return found; +} + +bool GodotCollisionSolver2D::solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin) { + const GodotSeparationRayShape2D *ray = static_cast<const GodotSeparationRayShape2D *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + return false; + } + + Vector2 from = p_transform_A.get_origin(); + Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin); + if (p_motion_A != Vector2()) { + //not the best but should be enough + Vector2 normal = (to - from).normalized(); + to += normal * MAX(0.0, normal.dot(p_motion_A)); + } + Vector2 support_A = to; + + Transform2D invb = p_transform_B.affine_inverse(); + from = invb.xform(from); + to = invb.xform(to); + + Vector2 p, n; + if (!p_shape_B->intersect_segment(from, to, p, n)) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector2()) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + if (r_sep_axis) { + *r_sep_axis = p_transform_A[1].normalized(); + } + return false; + } + + Vector2 support_B = p_transform_B.xform(p); + if (ray->get_slide_on_slope()) { + Vector2 global_n = invb.basis_xform_inv(n).normalized(); + support_B = support_A + (support_B - support_A).length() * global_n; + } + + if (p_result_callback) { + if (p_swap_result) { + p_result_callback(support_B, support_A, p_userdata); + } else { + p_result_callback(support_A, support_B, p_userdata); + } + } + return true; +} + +struct _ConcaveCollisionInfo2D { + const Transform2D *transform_A = nullptr; + const GodotShape2D *shape_A = nullptr; + const Transform2D *transform_B = nullptr; + Vector2 motion_A; + Vector2 motion_B; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + GodotCollisionSolver2D::CallbackResult result_callback = nullptr; + void *userdata = nullptr; + bool swap_result = false; + bool collided = false; + int aabb_tests = 0; + int collisions = 0; + Vector2 *sep_axis = nullptr; +}; + +bool GodotCollisionSolver2D::concave_callback(void *p_userdata, GodotShape2D *p_convex) { + _ConcaveCollisionInfo2D &cinfo = *(static_cast<_ConcaveCollisionInfo2D *>(p_userdata)); + cinfo.aabb_tests++; + + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B); + if (!collided) { + return false; + } + + cinfo.collided = true; + cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; +} + +bool GodotCollisionSolver2D::solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { + const GodotConcaveShape2D *concave_B = static_cast<const GodotConcaveShape2D *>(p_shape_B); + + _ConcaveCollisionInfo2D cinfo; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.motion_A = p_motion_A; + cinfo.result_callback = p_result_callback; + cinfo.userdata = p_userdata; + cinfo.swap_result = p_swap_result; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.sep_axis = r_sep_axis; + cinfo.margin_A = p_margin_A; + cinfo.margin_B = p_margin_B; + + cinfo.aabb_tests = 0; + + Transform2D rel_transform = p_transform_A; + rel_transform.columns[2] -= p_transform_B.get_origin(); + + // Quickly compute a local Rect2. + Rect2 local_aabb; + for (int i = 0; i < 2; i++) { + Vector2 axis(p_transform_B.columns[i]); + real_t axis_scale = 1.0 / axis.length(); + axis *= axis_scale; + + real_t smin = 0.0, smax = 0.0; + p_shape_A->project_rangev(axis, rel_transform, smin, smax); + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + // In case of motion, expand the Rect2 in the motion direction. + if (p_motion_A != Vector2()) { + Rect2 moved_aabb = local_aabb; + moved_aabb.position += p_motion_A; + local_aabb = local_aabb.merge(moved_aabb); + } + + concave_B->cull(local_aabb, concave_callback, &cinfo); + + return cinfo.collided; +} + +bool GodotCollisionSolver2D::solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { + PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); + PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); + bool concave_A = p_shape_A->is_concave(); + bool concave_B = p_shape_B->is_concave(); + real_t margin_A = p_margin_A, margin_B = p_margin_B; + + bool swap = false; + + if (type_A > type_B) { + SWAP(type_A, type_B); + SWAP(concave_A, concave_B); + SWAP(margin_A, margin_B); + swap = true; + } + + if (type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + if (type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { + WARN_PRINT_ONCE("Collisions between world boundaries are not supported."); + return false; + } + + if (swap) { + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, p_margin_A); + } else { + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, p_margin_B); + } + + } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY) { + WARN_PRINT_ONCE("Collisions between two rays are not supported."); + return false; //no ray-ray + } + + if (swap) { + return solve_separation_ray(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, r_sep_axis, p_margin_B); + } else { + return solve_separation_ray(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A); + } + + } else if (concave_B) { + if (concave_A) { + WARN_PRINT_ONCE("Collisions between two concave shapes are not supported."); + return false; + } + + if (!swap) { + return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); + } else { + return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B); + } + + } else { + return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B); + } +} diff --git a/modules/godot_physics_2d/godot_collision_solver_2d.h b/modules/godot_physics_2d/godot_collision_solver_2d.h new file mode 100644 index 0000000000..1c09714f76 --- /dev/null +++ b/modules/godot_physics_2d/godot_collision_solver_2d.h @@ -0,0 +1,50 @@ +/**************************************************************************/ +/* godot_collision_solver_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_COLLISION_SOLVER_2D_H +#define GODOT_COLLISION_SOLVER_2D_H + +#include "godot_shape_2d.h" + +class GodotCollisionSolver2D { +public: + typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); + +private: + static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); + static bool concave_callback(void *p_userdata, GodotShape2D *p_convex); + static bool solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0); + +public: + static bool solve(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); +}; + +#endif // GODOT_COLLISION_SOLVER_2D_H diff --git a/modules/godot_physics_2d/godot_collision_solver_2d_sat.cpp b/modules/godot_physics_2d/godot_collision_solver_2d_sat.cpp new file mode 100644 index 0000000000..daa9982b2e --- /dev/null +++ b/modules/godot_physics_2d/godot_collision_solver_2d_sat.cpp @@ -0,0 +1,1404 @@ +/**************************************************************************/ +/* godot_collision_solver_2d_sat.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_collision_solver_2d_sat.h" + +#include "core/math/geometry_2d.h" + +struct _CollectorCallback2D { + GodotCollisionSolver2D::CallbackResult callback = nullptr; + void *userdata = nullptr; + bool swap = false; + bool collided = false; + Vector2 normal; + Vector2 *sep_axis = nullptr; + + _FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) { + if (swap) { + callback(p_point_B, p_point_A, userdata); + } else { + callback(p_point_A, p_point_B, userdata); + } + } +}; + +typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *); + +_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 1); +#endif + + p_collector->call(*p_points_A, *p_points_B); +} + +_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 2); +#endif + + Vector2 closest_B = Geometry2D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); + p_collector->call(*p_points_A, closest_B); +} + +struct _generate_contacts_Pair { + bool a = false; + int idx = 0; + real_t d = 0.0; + _FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; } +}; + +_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix +#endif + + Vector2 n = p_collector->normal; + Vector2 t = n.orthogonal(); + real_t dA = n.dot(p_points_A[0]); + real_t dB = n.dot(p_points_B[0]); + + _generate_contacts_Pair dvec[4]; + + dvec[0].d = t.dot(p_points_A[0]); + dvec[0].a = true; + dvec[0].idx = 0; + dvec[1].d = t.dot(p_points_A[1]); + dvec[1].a = true; + dvec[1].idx = 1; + dvec[2].d = t.dot(p_points_B[0]); + dvec[2].a = false; + dvec[2].idx = 0; + dvec[3].d = t.dot(p_points_B[1]); + dvec[3].a = false; + dvec[3].idx = 1; + + SortArray<_generate_contacts_Pair> sa; + sa.sort(dvec, 4); + + for (int i = 1; i <= 2; i++) { + if (dvec[i].a) { + Vector2 a = p_points_A[dvec[i].idx]; + Vector2 b = n.plane_project(dB, a); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) { + continue; + } + p_collector->call(a, b); + } else { + Vector2 b = p_points_B[dvec[i].idx]; + Vector2 a = n.plane_project(dA, b); + if (n.dot(a) > n.dot(b) - CMP_EPSILON) { + continue; + } + p_collector->call(a, b); + } + } +} + +static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 1); + ERR_FAIL_COND(p_point_count_B < 1); +#endif + + static const GenerateContactsFunc generate_contacts_func_table[2][2] = { + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + }, + { + nullptr, + _generate_contacts_edge_edge, + } + }; + + int pointcount_B = 0; + int pointcount_A = 0; + const Vector2 *points_A = nullptr; + const Vector2 *points_B = nullptr; + + if (p_point_count_A > p_point_count_B) { + //swap + p_collector->swap = !p_collector->swap; + p_collector->normal = -p_collector->normal; + + pointcount_B = p_point_count_A; + pointcount_A = p_point_count_B; + points_A = p_points_B; + points_B = p_points_A; + } else { + pointcount_B = p_point_count_B; + pointcount_A = p_point_count_A; + points_A = p_points_A; + points_B = p_points_B; + } + + int version_A = (pointcount_A > 2 ? 2 : pointcount_A) - 1; + int version_B = (pointcount_B > 2 ? 2 : pointcount_B) - 1; + + GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; + ERR_FAIL_NULL(contacts_func); + contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector); +} + +template <typename ShapeA, typename ShapeB, bool castA = false, bool castB = false, bool withMargin = false> +class SeparatorAxisTest2D { + const ShapeA *shape_A = nullptr; + const ShapeB *shape_B = nullptr; + const Transform2D *transform_A = nullptr; + const Transform2D *transform_B = nullptr; + real_t best_depth = 1e15; + Vector2 best_axis; +#ifdef DEBUG_ENABLED + int best_axis_count = 0; + int best_axis_index = -1; +#endif + Vector2 motion_A; + Vector2 motion_B; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + _CollectorCallback2D *callback; + +public: + _FORCE_INLINE_ bool test_previous_axis() { + if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) { + return test_axis(*callback->sep_axis); + } else { +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + } + return true; + } + + _FORCE_INLINE_ bool test_cast() { + if (castA) { + Vector2 na = motion_A.normalized(); + if (!test_axis(na)) { + return false; + } + if (!test_axis(na.orthogonal())) { + return false; + } + } + + if (castB) { + Vector2 nb = motion_B.normalized(); + if (!test_axis(nb)) { + return false; + } + if (!test_axis(nb.orthogonal())) { + return false; + } + } + + return true; + } + + _FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) { + Vector2 axis = p_axis; + + if (Math::is_zero_approx(axis.x) && + Math::is_zero_approx(axis.y)) { + // strange case, try an upwards separator + axis = Vector2(0.0, 1.0); + } + + real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0; + + if (castA) { + shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A); + } else { + shape_A->project_range(axis, *transform_A, min_A, max_A); + } + + if (castB) { + shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B); + } else { + shape_B->project_range(axis, *transform_B, min_B, max_B); + } + + if (withMargin) { + min_A -= margin_A; + max_A += margin_A; + min_B -= margin_B; + max_B += margin_B; + } + + min_B -= (max_A - min_A) * 0.5; + max_B += (max_A - min_A) * 0.5; + + real_t dmin = min_B - (min_A + max_A) * 0.5; + real_t dmax = max_B - (min_A + max_A) * 0.5; + + if (dmin > 0.0 || dmax < 0.0) { + if (callback && callback->sep_axis) { + *callback->sep_axis = axis; + } +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + return false; // doesn't contain 0 + } + + //use the smallest depth + + dmin = Math::abs(dmin); + + if (dmax < dmin) { + if (dmax < best_depth) { + best_depth = dmax; + best_axis = axis; +#ifdef DEBUG_ENABLED + best_axis_index = best_axis_count; +#endif + } + } else { + if (dmin < best_depth) { + best_depth = dmin; + best_axis = -axis; // keep it as A axis +#ifdef DEBUG_ENABLED + best_axis_index = best_axis_count; +#endif + } + } + +#ifdef DEBUG_ENABLED + best_axis_count++; +#endif + + return true; + } + + _FORCE_INLINE_ void generate_contacts() { + // nothing to do, don't generate + if (best_axis == Vector2(0.0, 0.0)) { + return; + } + + if (callback) { + callback->collided = true; + + if (!callback->callback) { + return; //only collide, no callback + } + } + static const int max_supports = 2; + + Vector2 supports_A[max_supports]; + int support_count_A; + if (castA) { + shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A); + } else { + shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A); + for (int i = 0; i < support_count_A; i++) { + supports_A[i] = transform_A->xform(supports_A[i]); + } + } + + if (withMargin) { + for (int i = 0; i < support_count_A; i++) { + supports_A[i] += -best_axis * margin_A; + } + } + + Vector2 supports_B[max_supports]; + int support_count_B; + if (castB) { + shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B); + } else { + shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B); + for (int i = 0; i < support_count_B; i++) { + supports_B[i] = transform_B->xform(supports_B[i]); + } + } + + if (withMargin) { + for (int i = 0; i < support_count_B; i++) { + supports_B[i] += best_axis * margin_B; + } + } + if (callback) { + callback->normal = best_axis; + _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); + + if (callback->sep_axis && *callback->sep_axis != Vector2()) { + *callback->sep_axis = Vector2(); //invalidate previous axis (no test) + } + } + } + + _FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Transform2D &p_transform_a, const ShapeB *p_shape_B, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) { + margin_A = p_margin_A; + margin_B = p_margin_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + transform_A = &p_transform_a; + transform_B = &p_transform_b; + motion_A = p_motion_A; + motion_B = p_motion_B; + callback = p_collector; + } +}; + +/****** SAT TESTS *******/ + +#define TEST_POINT(m_a, m_b) \ + ((!separator.test_axis(((m_a) - (m_b)).normalized())) || \ + (castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) || \ + (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \ + (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized()))) + +typedef void (*CollisionFunc)(const GodotShape2D *, const Transform2D &, const GodotShape2D *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t); + +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_segment(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a); + const GodotSegmentShape2D *segment_B = static_cast<const GodotSegmentShape2D *>(p_b); + + SeparatorAxisTest2D<GodotSegmentShape2D, GodotSegmentShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + //this collision is kind of pointless + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b))) { + return; + } + + if (withMargin) { + //points grow to circles + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a()))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b()))) { + return; + } + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a); + const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotSegmentShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //segment normal + if (!separator.test_axis( + (p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().orthogonal())) { + return; + } + + //endpoint a vs circle + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin())) { + return; + } + //endpoint b vs circle + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin())) { + return; + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotSegmentShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_b.columns[1].normalized())) { + return; + } + + if (withMargin) { + Transform2D inv = p_transform_b.affine_inverse(); + + Vector2 a = p_transform_a.xform(segment_A->get_a()); + Vector2 b = p_transform_a.xform(segment_A->get_b()); + + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b))) { + return; + } + + if constexpr (castA) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a))) { + return; + } + } + + if constexpr (castB) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b))) { + return; + } + } + + if constexpr (castA && castB) { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a))) { + return; + } + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a))) { + return; + } + } + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotSegmentShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { + return; + } + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) { + return; + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_segment_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b); + + SeparatorAxisTest2D<GodotSegmentShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a))) { + return; + } + + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + + if (withMargin) { + if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + } + } + + separator.generate_contacts(); +} + +///////// + +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a); + const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotCircleShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin())) { + return; + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotCircleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + const Vector2 &sphere = p_transform_a.columns[2]; + const Vector2 *axis = &p_transform_b.columns[0]; + //const Vector2& half_extents = rectangle_B->get_half_extents(); + + if (!separator.test_axis(axis[0].normalized())) { + return; + } + + if (!separator.test_axis(axis[1].normalized())) { + return; + } + + Transform2D binv = p_transform_b.affine_inverse(); + { + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere))) { + return; + } + } + + if constexpr (castA) { + Vector2 sphereofs = sphere + p_motion_a; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + if constexpr (castB) { + Vector2 sphereofs = sphere - p_motion_b; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + if constexpr (castA && castB) { + Vector2 sphereofs = sphere - p_motion_b + p_motion_a; + if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs))) { + return; + } + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotCircleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { + return; + } + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + //capsule endpoints + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir))) { + return; + } + if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.columns[1] * capsule_dir))) { + return; + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_circle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b); + + SeparatorAxisTest2D<GodotCircleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //poly faces and poly points vs circle + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i)))) { + return; + } + + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + separator.generate_contacts(); +} + +///////// + +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a); + const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotRectangleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces A + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.columns[1].normalized())) { + return; + } + + //box faces B + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_b.columns[1].normalized())) { + return; + } + + if constexpr (withMargin) { + Transform2D invA = p_transform_a.affine_inverse(); + Transform2D invB = p_transform_b.affine_inverse(); + + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB))) { + return; + } + + if constexpr (castA || castB) { + Transform2D aofs = p_transform_a; + aofs.columns[2] += p_motion_a; + + Transform2D bofs = p_transform_b; + bofs.columns[2] += p_motion_b; + + [[maybe_unused]] Transform2D aofsinv = aofs.affine_inverse(); + [[maybe_unused]] Transform2D bofsinv = bofs.affine_inverse(); + + if constexpr (castA) { + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB))) { + return; + } + } + + if constexpr (castB) { + if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv))) { + return; + } + } + + if constexpr (castA && castB) { + if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotRectangleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.columns[1].normalized())) { + return; + } + + //capsule axis + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { + return; + } + + //box endpoints to capsule circles + + Transform2D boxinv = p_transform_a.affine_inverse(); + + real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + + for (int i = 0; i < 2; i++) { + { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if constexpr (castA) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir; + capsule_endpoint -= p_motion_a; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if constexpr (castB) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir; + capsule_endpoint += p_motion_b; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + if constexpr (castA && castB) { + Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir; + capsule_endpoint -= p_motion_a; + capsule_endpoint += p_motion_b; + + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) { + return; + } + } + + capsule_dir *= -1.0; + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b); + + SeparatorAxisTest2D<GodotRectangleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //box faces + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.columns[1].normalized())) { + return; + } + + //convex faces + Transform2D boxinv; + if constexpr (withMargin) { + boxinv = p_transform_a.affine_inverse(); + } + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + + if constexpr (withMargin) { + //all points vs all points need to be tested if margin exist + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i))))) { + return; + } + if constexpr (castA) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a))) { + return; + } + } + if constexpr (castB) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b))) { + return; + } + } + if constexpr (castA && castB) { + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +///////// + +template <bool castA, bool castB, bool withMargin> +static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a); + const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b); + + SeparatorAxisTest2D<GodotCapsuleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + + if (!separator.test_axis(p_transform_b.columns[0].normalized())) { + return; + } + + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { + return; + } + + //capsule endpoints + + real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); + for (int i = 0; i < 2; i++) { + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir_A; + + real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius(); + for (int j = 0; j < 2; j++) { + Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.columns[1] * capsule_dir_B; + + if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) { + return; + } + + capsule_dir_B *= -1.0; + } + + capsule_dir_A *= -1.0; + } + + separator.generate_contacts(); +} + +template <bool castA, bool castB, bool withMargin> +static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b); + + SeparatorAxisTest2D<GodotCapsuleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + //capsule axis + + if (!separator.test_axis(p_transform_a.columns[0].normalized())) { + return; + } + + //poly vs capsule + for (int i = 0; i < convex_B->get_point_count(); i++) { + Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i)); + + real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius(); + for (int j = 0; j < 2; j++) { + Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.columns[1] * capsule_dir; + + if (TEST_POINT(capsule_endpoint_A, cpoint)) { + return; + } + + capsule_dir *= -1.0; + } + + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + separator.generate_contacts(); +} + +///////// + +template <bool castA, bool castB, bool withMargin> +static void _collision_convex_polygon_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { + const GodotConvexPolygonShape2D *convex_A = static_cast<const GodotConvexPolygonShape2D *>(p_a); + const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b); + + SeparatorAxisTest2D<GodotConvexPolygonShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); + + if (!separator.test_previous_axis()) { + return; + } + + if (!separator.test_cast()) { + return; + } + + for (int i = 0; i < convex_A->get_point_count(); i++) { + if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i))) { + return; + } + } + + for (int i = 0; i < convex_B->get_point_count(); i++) { + if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) { + return; + } + } + + if (withMargin) { + for (int i = 0; i < convex_A->get_point_count(); i++) { + for (int j = 0; j < convex_B->get_point_count(); j++) { + if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j)))) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +//////// + +bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { + PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); + + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); + + PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); + + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_B == PhysicsServer2D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_B->is_concave(), false); + + static const CollisionFunc collision_table[5][5] = { + { _collision_segment_segment<false, false, false>, + _collision_segment_circle<false, false, false>, + _collision_segment_rectangle<false, false, false>, + _collision_segment_capsule<false, false, false>, + _collision_segment_convex_polygon<false, false, false> }, + { nullptr, + _collision_circle_circle<false, false, false>, + _collision_circle_rectangle<false, false, false>, + _collision_circle_capsule<false, false, false>, + _collision_circle_convex_polygon<false, false, false> }, + { nullptr, + nullptr, + _collision_rectangle_rectangle<false, false, false>, + _collision_rectangle_capsule<false, false, false>, + _collision_rectangle_convex_polygon<false, false, false> }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule<false, false, false>, + _collision_capsule_convex_polygon<false, false, false> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<false, false, false> } + + }; + + static const CollisionFunc collision_table_castA[5][5] = { + { _collision_segment_segment<true, false, false>, + _collision_segment_circle<true, false, false>, + _collision_segment_rectangle<true, false, false>, + _collision_segment_capsule<true, false, false>, + _collision_segment_convex_polygon<true, false, false> }, + { nullptr, + _collision_circle_circle<true, false, false>, + _collision_circle_rectangle<true, false, false>, + _collision_circle_capsule<true, false, false>, + _collision_circle_convex_polygon<true, false, false> }, + { nullptr, + nullptr, + _collision_rectangle_rectangle<true, false, false>, + _collision_rectangle_capsule<true, false, false>, + _collision_rectangle_convex_polygon<true, false, false> }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule<true, false, false>, + _collision_capsule_convex_polygon<true, false, false> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<true, false, false> } + + }; + + static const CollisionFunc collision_table_castB[5][5] = { + { _collision_segment_segment<false, true, false>, + _collision_segment_circle<false, true, false>, + _collision_segment_rectangle<false, true, false>, + _collision_segment_capsule<false, true, false>, + _collision_segment_convex_polygon<false, true, false> }, + { nullptr, + _collision_circle_circle<false, true, false>, + _collision_circle_rectangle<false, true, false>, + _collision_circle_capsule<false, true, false>, + _collision_circle_convex_polygon<false, true, false> }, + { nullptr, + nullptr, + _collision_rectangle_rectangle<false, true, false>, + _collision_rectangle_capsule<false, true, false>, + _collision_rectangle_convex_polygon<false, true, false> }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule<false, true, false>, + _collision_capsule_convex_polygon<false, true, false> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<false, true, false> } + + }; + + static const CollisionFunc collision_table_castA_castB[5][5] = { + { _collision_segment_segment<true, true, false>, + _collision_segment_circle<true, true, false>, + _collision_segment_rectangle<true, true, false>, + _collision_segment_capsule<true, true, false>, + _collision_segment_convex_polygon<true, true, false> }, + { nullptr, + _collision_circle_circle<true, true, false>, + _collision_circle_rectangle<true, true, false>, + _collision_circle_capsule<true, true, false>, + _collision_circle_convex_polygon<true, true, false> }, + { nullptr, + nullptr, + _collision_rectangle_rectangle<true, true, false>, + _collision_rectangle_capsule<true, true, false>, + _collision_rectangle_convex_polygon<true, true, false> }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule<true, true, false>, + _collision_capsule_convex_polygon<true, true, false> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<true, true, false> } + + }; + + static const CollisionFunc collision_table_margin[5][5] = { + { _collision_segment_segment<false, false, true>, + _collision_segment_circle<false, false, true>, + _collision_segment_rectangle<false, false, true>, + _collision_segment_capsule<false, false, true>, + _collision_segment_convex_polygon<false, false, true> }, + { nullptr, + _collision_circle_circle<false, false, true>, + _collision_circle_rectangle<false, false, true>, + _collision_circle_capsule<false, false, true>, + _collision_circle_convex_polygon<false, false, true> }, + { nullptr, + nullptr, + _collision_rectangle_rectangle<false, false, true>, + _collision_rectangle_capsule<false, false, true>, + _collision_rectangle_convex_polygon<false, false, true> }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule<false, false, true>, + _collision_capsule_convex_polygon<false, false, true> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<false, false, true> } + + }; + + static const CollisionFunc collision_table_castA_margin[5][5] = { + { _collision_segment_segment<true, false, true>, + _collision_segment_circle<true, false, true>, + _collision_segment_rectangle<true, false, true>, + _collision_segment_capsule<true, false, true>, + _collision_segment_convex_polygon<true, false, true> }, + { nullptr, + _collision_circle_circle<true, false, true>, + _collision_circle_rectangle<true, false, true>, + _collision_circle_capsule<true, false, true>, + _collision_circle_convex_polygon<true, false, true> }, + { nullptr, + nullptr, + _collision_rectangle_rectangle<true, false, true>, + _collision_rectangle_capsule<true, false, true>, + _collision_rectangle_convex_polygon<true, false, true> }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule<true, false, true>, + _collision_capsule_convex_polygon<true, false, true> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<true, false, true> } + + }; + + static const CollisionFunc collision_table_castB_margin[5][5] = { + { _collision_segment_segment<false, true, true>, + _collision_segment_circle<false, true, true>, + _collision_segment_rectangle<false, true, true>, + _collision_segment_capsule<false, true, true>, + _collision_segment_convex_polygon<false, true, true> }, + { nullptr, + _collision_circle_circle<false, true, true>, + _collision_circle_rectangle<false, true, true>, + _collision_circle_capsule<false, true, true>, + _collision_circle_convex_polygon<false, true, true> }, + { nullptr, + nullptr, + _collision_rectangle_rectangle<false, true, true>, + _collision_rectangle_capsule<false, true, true>, + _collision_rectangle_convex_polygon<false, true, true> }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule<false, true, true>, + _collision_capsule_convex_polygon<false, true, true> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<false, true, true> } + + }; + + static const CollisionFunc collision_table_castA_castB_margin[5][5] = { + { _collision_segment_segment<true, true, true>, + _collision_segment_circle<true, true, true>, + _collision_segment_rectangle<true, true, true>, + _collision_segment_capsule<true, true, true>, + _collision_segment_convex_polygon<true, true, true> }, + { nullptr, + _collision_circle_circle<true, true, true>, + _collision_circle_rectangle<true, true, true>, + _collision_circle_capsule<true, true, true>, + _collision_circle_convex_polygon<true, true, true> }, + { nullptr, + nullptr, + _collision_rectangle_rectangle<true, true, true>, + _collision_rectangle_capsule<true, true, true>, + _collision_rectangle_convex_polygon<true, true, true> }, + { nullptr, + nullptr, + nullptr, + _collision_capsule_capsule<true, true, true>, + _collision_capsule_convex_polygon<true, true, true> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<true, true, true> } + + }; + + _CollectorCallback2D callback; + callback.callback = p_result_callback; + callback.swap = p_swap; + callback.userdata = p_userdata; + callback.collided = false; + callback.sep_axis = sep_axis; + + const GodotShape2D *A = p_shape_A; + const GodotShape2D *B = p_shape_B; + const Transform2D *transform_A = &p_transform_A; + const Transform2D *transform_B = &p_transform_B; + const Vector2 *motion_A = &p_motion_A; + const Vector2 *motion_B = &p_motion_B; + real_t margin_A = p_margin_A, margin_B = p_margin_B; + + if (type_A > type_B) { + SWAP(A, B); + SWAP(transform_A, transform_B); + SWAP(type_A, type_B); + SWAP(motion_A, motion_B); + SWAP(margin_A, margin_B); + callback.swap = !callback.swap; + } + + CollisionFunc collision_func; + + if (p_margin_A || p_margin_B) { + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_margin[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA_margin[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB_margin[type_A - 2][type_B - 2]; + } else { + collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2]; + } + } else { + if (*motion_A == Vector2() && *motion_B == Vector2()) { + collision_func = collision_table[type_A - 2][type_B - 2]; + } else if (*motion_A != Vector2() && *motion_B == Vector2()) { + collision_func = collision_table_castA[type_A - 2][type_B - 2]; + } else if (*motion_A == Vector2() && *motion_B != Vector2()) { + collision_func = collision_table_castB[type_A - 2][type_B - 2]; + } else { + collision_func = collision_table_castA_castB[type_A - 2][type_B - 2]; + } + } + + ERR_FAIL_NULL_V(collision_func, false); + + collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B); + + return callback.collided; +} diff --git a/modules/godot_physics_2d/godot_collision_solver_2d_sat.h b/modules/godot_physics_2d/godot_collision_solver_2d_sat.h new file mode 100644 index 0000000000..c9183f7ecb --- /dev/null +++ b/modules/godot_physics_2d/godot_collision_solver_2d_sat.h @@ -0,0 +1,38 @@ +/**************************************************************************/ +/* godot_collision_solver_2d_sat.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_COLLISION_SOLVER_2D_SAT_H +#define GODOT_COLLISION_SOLVER_2D_SAT_H + +#include "godot_collision_solver_2d.h" + +bool sat_2d_calculate_penetration(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, GodotCollisionSolver2D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + +#endif // GODOT_COLLISION_SOLVER_2D_SAT_H diff --git a/modules/godot_physics_2d/godot_constraint_2d.h b/modules/godot_physics_2d/godot_constraint_2d.h new file mode 100644 index 0000000000..f4136f6643 --- /dev/null +++ b/modules/godot_physics_2d/godot_constraint_2d.h @@ -0,0 +1,70 @@ +/**************************************************************************/ +/* godot_constraint_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_CONSTRAINT_2D_H +#define GODOT_CONSTRAINT_2D_H + +#include "godot_body_2d.h" + +class GodotConstraint2D { + GodotBody2D **_body_ptr; + int _body_count; + uint64_t island_step = 0; + bool disabled_collisions_between_bodies = true; + + RID self; + +protected: + GodotConstraint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) { + _body_ptr = p_body_ptr; + _body_count = p_body_count; + } + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ GodotBody2D **get_body_ptr() const { return _body_ptr; } + _FORCE_INLINE_ int get_body_count() const { return _body_count; } + + _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } + _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } + + virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; + virtual void solve(real_t p_step) = 0; + + virtual ~GodotConstraint2D() {} +}; + +#endif // GODOT_CONSTRAINT_2D_H diff --git a/modules/godot_physics_2d/godot_joints_2d.cpp b/modules/godot_physics_2d/godot_joints_2d.cpp new file mode 100644 index 0000000000..5c76eb9dad --- /dev/null +++ b/modules/godot_physics_2d/godot_joints_2d.cpp @@ -0,0 +1,595 @@ +/**************************************************************************/ +/* godot_joints_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_joints_2d.h" + +#include "godot_space_2d.h" + +//based on chipmunk joint constraints + +/* Copyright (c) 2007 Scott Lembcke + * + * 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. + */ + +void GodotJoint2D::copy_settings_from(GodotJoint2D *p_joint) { + set_self(p_joint->get_self()); + set_max_force(p_joint->get_max_force()); + set_bias(p_joint->get_bias()); + set_max_bias(p_joint->get_max_bias()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); +} + +static inline real_t k_scalar(GodotBody2D *a, GodotBody2D *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) { + real_t value = 0.0; + + { + value += a->get_inv_mass(); + real_t rcn = (rA - a->get_center_of_mass()).cross(n); + value += a->get_inv_inertia() * rcn * rcn; + } + + if (b) { + value += b->get_inv_mass(); + real_t rcn = (rB - b->get_center_of_mass()).cross(n); + value += b->get_inv_inertia() * rcn * rcn; + } + + return value; +} + +static inline Vector2 +relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB) { + Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity(); + if (b) { + return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum; + } else { + return -sum; + } +} + +static inline real_t +normal_relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB, Vector2 n) { + return relative_velocity(a, b, rA, rB).dot(n); +} + +bool GodotPinJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + GodotSpace2D *space = A->get_space(); + ERR_FAIL_NULL_V(space, false); + + rA = A->get_transform().basis_xform(anchor_A); + rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B; + + real_t B_inv_mass = B ? B->get_inv_mass() : 0.0; + + Transform2D K1; + K1[0].x = A->get_inv_mass() + B_inv_mass; + K1[1].x = 0.0f; + K1[0].y = 0.0f; + K1[1].y = A->get_inv_mass() + B_inv_mass; + + Vector2 r1 = rA - A->get_center_of_mass(); + + Transform2D K2; + K2[0].x = A->get_inv_inertia() * r1.y * r1.y; + K2[1].x = -A->get_inv_inertia() * r1.x * r1.y; + K2[0].y = -A->get_inv_inertia() * r1.x * r1.y; + K2[1].y = A->get_inv_inertia() * r1.x * r1.x; + + Transform2D K; + K[0] = K1[0] + K2[0]; + K[1] = K1[1] + K2[1]; + + if (B) { + Vector2 r2 = rB - B->get_center_of_mass(); + + Transform2D K3; + K3[0].x = B->get_inv_inertia() * r2.y * r2.y; + K3[1].x = -B->get_inv_inertia() * r2.x * r2.y; + K3[0].y = -B->get_inv_inertia() * r2.x * r2.y; + K3[1].y = B->get_inv_inertia() * r2.x * r2.x; + + K[0] += K3[0]; + K[1] += K3[1]; + } + + K[0].x += softness; + K[1].y += softness; + + M = K.affine_inverse(); + + Vector2 gA = rA + A->get_transform().get_origin(); + Vector2 gB = B ? rB + B->get_transform().get_origin() : rB; + + Vector2 delta = gB - gA; + + bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step); + + // Compute max impulse. + jn_max = get_max_force() * p_step; + + return true; +} + +inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) { + return Vector2(p_other * p_vec.y, -p_other * p_vec.x); +} + +bool GodotPinJoint2D::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-P, rA); + } + if (B && dynamic_B) { + B->apply_impulse(P, rB); + } + // Angle limits joint pre_solve step taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpRotaryLimitJoint.c + real_t i_sum_local = A->get_inv_inertia(); + if (B) { + i_sum_local += B->get_inv_inertia(); + } + i_sum = 1.0 / (i_sum_local); + if (angular_limit_enabled && B) { + Vector2 diff_vector = B->get_transform().get_origin() - A->get_transform().get_origin(); + diff_vector = diff_vector.rotated(-initial_angle); + real_t dist = diff_vector.angle(); + real_t pdist = 0.0; + if (dist > angular_limit_upper) { + pdist = dist - angular_limit_upper; + } else if (dist < angular_limit_lower) { + pdist = dist - angular_limit_lower; + } + real_t error_bias = Math::pow(1.0 - 0.15, 60.0); + // Calculate bias velocity. + bias_velocity = -CLAMP((-1.0 - Math::pow(error_bias, p_step)) * pdist / p_step, -get_max_bias(), get_max_bias()); + // If the bias velocity is 0, the joint is not at a limit. + if (bias_velocity >= -CMP_EPSILON && bias_velocity <= CMP_EPSILON) { + j_acc = 0; + is_joint_at_limit = false; + } else { + is_joint_at_limit = true; + } + } else { + bias_velocity = 0.0; + } + + return true; +} + +void GodotPinJoint2D::solve(real_t p_step) { + // Compute relative velocity. + Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity()); + + Vector2 rel_vel; + if (B) { + rel_vel = B->get_linear_velocity() - custom_cross(rB - B->get_center_of_mass(), B->get_angular_velocity()) - vA; + } else { + rel_vel = -vA; + } + // Angle limits joint solve step taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpRotaryLimitJoint.c + if ((angular_limit_enabled || motor_enabled) && B) { + // Compute relative rotational velocity. + real_t wr = B->get_angular_velocity() - A->get_angular_velocity(); + // Motor solve part taken from https://github.com/slembcke/Chipmunk2D/blob/d0239ef4599b3688a5a336373f7d0a68426414ba/src/cpSimpleMotor.c + if (motor_enabled) { + wr -= motor_target_velocity; + } + real_t j_max = jn_max; + + // Compute normal impulse. + real_t j = -(bias_velocity + wr) * i_sum; + real_t j_old = j_acc; + // Only enable the limits if we have to. + if (angular_limit_enabled && is_joint_at_limit) { + if (bias_velocity < 0.0) { + j_acc = CLAMP(j_old + j, 0.0, j_max); + } else { + j_acc = CLAMP(j_old + j, -j_max, 0.0); + } + } else { + j_acc = CLAMP(j_old + j, -j_max, j_max); + } + j = j_acc - j_old; + A->apply_torque_impulse(-j * A->get_inv_inertia()); + B->apply_torque_impulse(j * B->get_inv_inertia()); + } + + Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P); + + if (dynamic_A) { + A->apply_impulse(-impulse, rA); + } + if (B && dynamic_B) { + B->apply_impulse(impulse, rB); + } + + P += impulse; +} + +void GodotPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::PIN_JOINT_SOFTNESS: { + softness = p_value; + } break; + case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: { + angular_limit_upper = p_value; + } break; + case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: { + angular_limit_lower = p_value; + } break; + case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: { + motor_target_velocity = p_value; + } break; + } +} + +real_t GodotPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const { + switch (p_param) { + case PhysicsServer2D::PIN_JOINT_SOFTNESS: { + return softness; + } + case PhysicsServer2D::PIN_JOINT_LIMIT_UPPER: { + return angular_limit_upper; + } + case PhysicsServer2D::PIN_JOINT_LIMIT_LOWER: { + return angular_limit_lower; + } + case PhysicsServer2D::PIN_JOINT_MOTOR_TARGET_VELOCITY: { + return motor_target_velocity; + } + } + ERR_FAIL_V(0); +} + +void GodotPinJoint2D::set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled) { + switch (p_flag) { + case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: { + angular_limit_enabled = p_enabled; + } break; + case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: { + motor_enabled = p_enabled; + } break; + } +} + +bool GodotPinJoint2D::get_flag(PhysicsServer2D::PinJointFlag p_flag) const { + switch (p_flag) { + case PhysicsServer2D::PIN_JOINT_FLAG_ANGULAR_LIMIT_ENABLED: { + return angular_limit_enabled; + } + case PhysicsServer2D::PIN_JOINT_FLAG_MOTOR_ENABLED: { + return motor_enabled; + } + } + ERR_FAIL_V(0); +} + +GodotPinJoint2D::GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, p_body_b ? 2 : 1) { + A = p_body_a; + B = p_body_b; + anchor_A = p_body_a->get_inv_transform().xform(p_pos); + anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos; + + p_body_a->add_constraint(this, 0); + if (p_body_b) { + p_body_b->add_constraint(this, 1); + initial_angle = A->get_transform().get_origin().angle_to_point(B->get_transform().get_origin()); + } +} + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + +static inline void +k_tensor(GodotBody2D *a, GodotBody2D *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) { + // calculate mass matrix + // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... + real_t k11, k12, k21, k22; + real_t m_sum = a->get_inv_mass() + b->get_inv_mass(); + + // start with I*m_sum + k11 = m_sum; + k12 = 0.0f; + k21 = 0.0f; + k22 = m_sum; + + r1 -= a->get_center_of_mass(); + r2 -= b->get_center_of_mass(); + + // add the influence from r1 + real_t a_i_inv = a->get_inv_inertia(); + real_t r1xsq = r1.x * r1.x * a_i_inv; + real_t r1ysq = r1.y * r1.y * a_i_inv; + real_t r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; + k12 += r1nxy; + k21 += r1nxy; + k22 += r1xsq; + + // add the influnce from r2 + real_t b_i_inv = b->get_inv_inertia(); + real_t r2xsq = r2.x * r2.x * b_i_inv; + real_t r2ysq = r2.y * r2.y * b_i_inv; + real_t r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; + k12 += r2nxy; + k21 += r2nxy; + k22 += r2xsq; + + // invert + real_t determinant = k11 * k22 - k12 * k21; + ERR_FAIL_COND(determinant == 0.0); + + real_t det_inv = 1.0f / determinant; + *k1 = Vector2(k22 * det_inv, -k12 * det_inv); + *k2 = Vector2(-k21 * det_inv, k11 * det_inv); +} + +static _FORCE_INLINE_ Vector2 +mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) { + return Vector2(vr.dot(k1), vr.dot(k2)); +} + +bool GodotGrooveJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + GodotSpace2D *space = A->get_space(); + ERR_FAIL_NULL_V(space, false); + + // calculate endpoints in worldspace + Vector2 ta = A->get_transform().xform(A_groove_1); + Vector2 tb = A->get_transform().xform(A_groove_2); + + // calculate axis + Vector2 n = -(tb - ta).orthogonal().normalized(); + real_t d = ta.dot(n); + + xf_normal = n; + rB = B->get_transform().basis_xform(B_anchor); + + // calculate tangential distance along the axis of rB + real_t td = (B->get_transform().get_origin() + rB).cross(n); + // calculate clamping factor and rB + if (td <= ta.cross(n)) { + clamp = 1.0f; + rA = ta - A->get_transform().get_origin(); + } else if (td >= tb.cross(n)) { + clamp = -1.0f; + rA = tb - A->get_transform().get_origin(); + } else { + clamp = 0.0f; + //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + rA = ((-n.orthogonal() * -td) + n * d) - A->get_transform().get_origin(); + } + + // Calculate mass tensor + k_tensor(A, B, rA, rB, &k1, &k2); + + // compute max impulse + jn_max = get_max_force() * p_step; + + // calculate bias velocity + //cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + //joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); + + real_t _b = get_bias(); + gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).limit_length(get_max_bias()); + + correct = true; + return true; +} + +bool GodotGrooveJoint2D::pre_solve(real_t p_step) { + // Apply accumulated impulse. + if (dynamic_A) { + A->apply_impulse(-jn_acc, rA); + } + if (dynamic_B) { + B->apply_impulse(jn_acc, rB); + } + + return true; +} + +void GodotGrooveJoint2D::solve(real_t p_step) { + // compute impulse + Vector2 vr = relative_velocity(A, B, rA, rB); + + Vector2 j = mult_k(gbias - vr, k1, k2); + Vector2 jOld = jn_acc; + j += jOld; + + jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : j.project(xf_normal)).limit_length(jn_max); + + j = jn_acc - jOld; + + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } +} + +GodotGrooveJoint2D::GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, 2) { + A = p_body_a; + B = p_body_b; + + A_groove_1 = A->get_inv_transform().xform(p_a_groove1); + A_groove_2 = A->get_inv_transform().xform(p_a_groove2); + B_anchor = B->get_inv_transform().xform(p_b_anchor); + A_groove_normal = -(A_groove_2 - A_groove_1).normalized().orthogonal(); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + +bool GodotDampedSpringJoint2D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + rA = A->get_transform().basis_xform(anchor_A); + rB = B->get_transform().basis_xform(anchor_B); + + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA); + real_t dist = delta.length(); + + if (dist) { + n = delta / dist; + } else { + n = Vector2(); + } + + real_t k = k_scalar(A, B, rA, rB, n); + n_mass = 1.0f / k; + + target_vrn = 0.0f; + v_coef = 1.0f - Math::exp(-damping * (p_step)*k); + + // Calculate spring force. + real_t f_spring = (rest_length - dist) * stiffness; + j = n * f_spring * (p_step); + + return true; +} + +bool GodotDampedSpringJoint2D::pre_solve(real_t p_step) { + // Apply spring force. + if (dynamic_A) { + A->apply_impulse(-j, rA); + } + if (dynamic_B) { + B->apply_impulse(j, rB); + } + + return true; +} + +void GodotDampedSpringJoint2D::solve(real_t p_step) { + // compute relative velocity + real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + real_t v_damp = -vrn * v_coef; + target_vrn = vrn + v_damp; + Vector2 j_new = n * v_damp * n_mass; + + if (dynamic_A) { + A->apply_impulse(-j_new, rA); + } + if (dynamic_B) { + B->apply_impulse(j_new, rB); + } +} + +void GodotDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { + rest_length = p_value; + } break; + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { + damping = p_value; + } break; + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { + stiffness = p_value; + } break; + } +} + +real_t GodotDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const { + switch (p_param) { + case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { + return rest_length; + } break; + case PhysicsServer2D::DAMPED_SPRING_DAMPING: { + return damping; + } break; + case PhysicsServer2D::DAMPED_SPRING_STIFFNESS: { + return stiffness; + } break; + } + + ERR_FAIL_V(0); +} + +GodotDampedSpringJoint2D::GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b) : + GodotJoint2D(_arr, 2) { + A = p_body_a; + B = p_body_b; + anchor_A = A->get_inv_transform().xform(p_anchor_a); + anchor_B = B->get_inv_transform().xform(p_anchor_b); + + rest_length = p_anchor_a.distance_to(p_anchor_b); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} diff --git a/modules/godot_physics_2d/godot_joints_2d.h b/modules/godot_physics_2d/godot_joints_2d.h new file mode 100644 index 0000000000..c6a1fdb692 --- /dev/null +++ b/modules/godot_physics_2d/godot_joints_2d.h @@ -0,0 +1,192 @@ +/**************************************************************************/ +/* godot_joints_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_JOINTS_2D_H +#define GODOT_JOINTS_2D_H + +#include "godot_body_2d.h" +#include "godot_constraint_2d.h" + +class GodotJoint2D : public GodotConstraint2D { + real_t bias = 0; + real_t max_bias = 3.40282e+38; + real_t max_force = 3.40282e+38; + +protected: + bool dynamic_A = false; + bool dynamic_B = false; + +public: + _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; } + _FORCE_INLINE_ real_t get_max_force() const { return max_force; } + + _FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; } + _FORCE_INLINE_ real_t get_bias() const { return bias; } + + _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; } + _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } + + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return false; } + virtual void solve(real_t p_step) override {} + + void copy_settings_from(GodotJoint2D *p_joint); + + virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } + GodotJoint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) : + GodotConstraint2D(p_body_ptr, p_body_count) {} + + virtual ~GodotJoint2D() { + for (int i = 0; i < get_body_count(); i++) { + GodotBody2D *body = get_body_ptr()[i]; + if (body) { + body->remove_constraint(this, i); + } + } + }; +}; + +class GodotPinJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Transform2D M; + Vector2 rA, rB; + Vector2 anchor_A; + Vector2 anchor_B; + Vector2 bias; + real_t initial_angle = 0.0; + real_t bias_velocity = 0.0; + real_t jn_max = 0.0; + real_t j_acc = 0.0; + real_t i_sum = 0.0; + Vector2 P; + real_t softness = 0.0; + real_t angular_limit_lower = 0.0; + real_t angular_limit_upper = 0.0; + real_t motor_target_velocity = 0.0; + bool is_joint_at_limit = false; + bool motor_enabled = false; + bool angular_limit_enabled = false; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_PIN; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::PinJointParam p_param) const; + + void set_flag(PhysicsServer2D::PinJointFlag p_flag, bool p_enabled); + bool get_flag(PhysicsServer2D::PinJointFlag p_flag) const; + + GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b = nullptr); +}; + +class GodotGrooveJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Vector2 A_groove_1; + Vector2 A_groove_2; + Vector2 A_groove_normal; + Vector2 B_anchor; + Vector2 jn_acc; + Vector2 gbias; + real_t jn_max = 0.0; + real_t clamp = 0.0; + Vector2 xf_normal; + Vector2 rA, rB; + Vector2 k1, k2; + + bool correct = false; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_GROOVE; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotGrooveJoint2D(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, GodotBody2D *p_body_a, GodotBody2D *p_body_b); +}; + +class GodotDampedSpringJoint2D : public GodotJoint2D { + union { + struct { + GodotBody2D *A; + GodotBody2D *B; + }; + + GodotBody2D *_arr[2] = { nullptr, nullptr }; + }; + + Vector2 anchor_A; + Vector2 anchor_B; + + real_t rest_length = 0.0; + real_t damping = 1.5; + real_t stiffness = 20.0; + + Vector2 rA, rB; + Vector2 n; + Vector2 j; + real_t n_mass = 0.0; + real_t target_vrn = 0.0; + real_t v_coef = 0.0; + +public: + virtual PhysicsServer2D::JointType get_type() const override { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; } + + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); + real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; + + GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b); +}; + +#endif // GODOT_JOINTS_2D_H diff --git a/modules/godot_physics_2d/godot_physics_server_2d.cpp b/modules/godot_physics_2d/godot_physics_server_2d.cpp new file mode 100644 index 0000000000..8df17992ea --- /dev/null +++ b/modules/godot_physics_2d/godot_physics_server_2d.cpp @@ -0,0 +1,1400 @@ +/**************************************************************************/ +/* godot_physics_server_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_physics_server_2d.h" + +#include "godot_body_direct_state_2d.h" +#include "godot_broad_phase_2d_bvh.h" +#include "godot_collision_solver_2d.h" + +#include "core/config/project_settings.h" +#include "core/debugger/engine_debugger.h" +#include "core/os/os.h" + +#define FLUSH_QUERY_CHECK(m_object) \ + ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); + +RID GodotPhysicsServer2D::_shape_create(ShapeType p_shape) { + GodotShape2D *shape = nullptr; + switch (p_shape) { + case SHAPE_WORLD_BOUNDARY: { + shape = memnew(GodotWorldBoundaryShape2D); + } break; + case SHAPE_SEPARATION_RAY: { + shape = memnew(GodotSeparationRayShape2D); + } break; + case SHAPE_SEGMENT: { + shape = memnew(GodotSegmentShape2D); + } break; + case SHAPE_CIRCLE: { + shape = memnew(GodotCircleShape2D); + } break; + case SHAPE_RECTANGLE: { + shape = memnew(GodotRectangleShape2D); + } break; + case SHAPE_CAPSULE: { + shape = memnew(GodotCapsuleShape2D); + } break; + case SHAPE_CONVEX_POLYGON: { + shape = memnew(GodotConvexPolygonShape2D); + } break; + case SHAPE_CONCAVE_POLYGON: { + shape = memnew(GodotConcavePolygonShape2D); + } break; + case SHAPE_CUSTOM: { + ERR_FAIL_V(RID()); + + } break; + } + + RID id = shape_owner.make_rid(shape); + shape->set_self(id); + + return id; +} + +RID GodotPhysicsServer2D::world_boundary_shape_create() { + return _shape_create(SHAPE_WORLD_BOUNDARY); +} + +RID GodotPhysicsServer2D::separation_ray_shape_create() { + return _shape_create(SHAPE_SEPARATION_RAY); +} + +RID GodotPhysicsServer2D::segment_shape_create() { + return _shape_create(SHAPE_SEGMENT); +} + +RID GodotPhysicsServer2D::circle_shape_create() { + return _shape_create(SHAPE_CIRCLE); +} + +RID GodotPhysicsServer2D::rectangle_shape_create() { + return _shape_create(SHAPE_RECTANGLE); +} + +RID GodotPhysicsServer2D::capsule_shape_create() { + return _shape_create(SHAPE_CAPSULE); +} + +RID GodotPhysicsServer2D::convex_polygon_shape_create() { + return _shape_create(SHAPE_CONVEX_POLYGON); +} + +RID GodotPhysicsServer2D::concave_polygon_shape_create() { + return _shape_create(SHAPE_CONCAVE_POLYGON); +} + +void GodotPhysicsServer2D::shape_set_data(RID p_shape, const Variant &p_data) { + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + shape->set_data(p_data); +}; + +void GodotPhysicsServer2D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + shape->set_custom_bias(p_bias); +} + +PhysicsServer2D::ShapeType GodotPhysicsServer2D::shape_get_type(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, SHAPE_CUSTOM); + return shape->get_type(); +}; + +Variant GodotPhysicsServer2D::shape_get_data(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, Variant()); + ERR_FAIL_COND_V(!shape->is_configured(), Variant()); + return shape->get_data(); +}; + +real_t GodotPhysicsServer2D::shape_get_custom_solver_bias(RID p_shape) const { + const GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, 0); + return shape->get_custom_bias(); +} + +void GodotPhysicsServer2D::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { + CollCbkData *cbk = static_cast<CollCbkData *>(p_userdata); + + if (cbk->max == 0) { + return; + } + + Vector2 rel_dir = (p_point_A - p_point_B); + real_t rel_length2 = rel_dir.length_squared(); + if (cbk->valid_dir != Vector2()) { + if (cbk->valid_depth < 10e20) { + if (rel_length2 > cbk->valid_depth * cbk->valid_depth || + (rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) { + cbk->invalid_by_dir++; + return; + } + } else { + if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) { + return; + } + } + } + + if (cbk->amount == cbk->max) { + //find least deep + real_t min_depth = 1e20; + int min_depth_idx = 0; + for (int i = 0; i < cbk->amount; i++) { + real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); + if (d < min_depth) { + min_depth = d; + min_depth_idx = i; + } + } + + if (rel_length2 < min_depth) { + return; + } + cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; + cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; + cbk->passed++; + + } else { + cbk->ptr[cbk->amount * 2 + 0] = p_point_A; + cbk->ptr[cbk->amount * 2 + 1] = p_point_B; + cbk->amount++; + cbk->passed++; + } +} + +bool GodotPhysicsServer2D::shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) { + GodotShape2D *shape_A = shape_owner.get_or_null(p_shape_A); + ERR_FAIL_NULL_V(shape_A, false); + GodotShape2D *shape_B = shape_owner.get_or_null(p_shape_B); + ERR_FAIL_NULL_V(shape_B, false); + + if (p_result_max == 0) { + return GodotCollisionSolver2D::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, nullptr, nullptr); + } + + CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = r_results; + + bool res = GodotCollisionSolver2D::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk); + r_result_count = cbk.amount; + return res; +} + +RID GodotPhysicsServer2D::space_create() { + GodotSpace2D *space = memnew(GodotSpace2D); + RID id = space_owner.make_rid(space); + space->set_self(id); + RID area_id = area_create(); + GodotArea2D *area = area_owner.get_or_null(area_id); + ERR_FAIL_NULL_V(area, RID()); + space->set_default_area(area); + area->set_space(space); + area->set_priority(-1); + + return id; +}; + +void GodotPhysicsServer2D::space_set_active(RID p_space, bool p_active) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + if (p_active) { + active_spaces.insert(space); + } else { + active_spaces.erase(space); + } +} + +bool GodotPhysicsServer2D::space_is_active(RID p_space) const { + const GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, false); + + return active_spaces.has(space); +} + +void GodotPhysicsServer2D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + + space->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::space_get_param(RID p_space, SpaceParameter p_param) const { + const GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, 0); + return space->get_param(p_param); +} + +void GodotPhysicsServer2D::space_set_debug_contacts(RID p_space, int p_max_contacts) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + space->set_debug_contacts(p_max_contacts); +} + +Vector<Vector2> GodotPhysicsServer2D::space_get_contacts(RID p_space) const { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, Vector<Vector2>()); + return space->get_debug_contacts(); +} + +int GodotPhysicsServer2D::space_get_contact_count(RID p_space) const { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, 0); + return space->get_debug_contact_count(); +} + +PhysicsDirectSpaceState2D *GodotPhysicsServer2D::space_get_direct_state(RID p_space) { + GodotSpace2D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, nullptr); + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + + return space->get_direct_state(); +} + +RID GodotPhysicsServer2D::area_create() { + GodotArea2D *area = memnew(GodotArea2D); + RID rid = area_owner.make_rid(area); + area->set_self(rid); + return rid; +} + +void GodotPhysicsServer2D::area_set_space(RID p_area, RID p_space) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + GodotSpace2D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + } + + if (area->get_space() == space) { + return; //pointless + } + + area->clear_constraints(); + area->set_space(space); +} + +RID GodotPhysicsServer2D::area_get_space(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, RID()); + + GodotSpace2D *space = area->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +} + +void GodotPhysicsServer2D::area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform, bool p_disabled) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + area->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer2D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + ERR_FAIL_COND(!shape->is_configured()); + + area->set_shape(p_shape_idx, shape); +} + +void GodotPhysicsServer2D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_shape_transform(p_shape_idx, p_transform); +} + +void GodotPhysicsServer2D::area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + ERR_FAIL_INDEX(p_shape, area->get_shape_count()); + FLUSH_QUERY_CHECK(area); + + area->set_shape_disabled(p_shape, p_disabled); +} + +int GodotPhysicsServer2D::area_get_shape_count(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, -1); + + return area->get_shape_count(); +} + +RID GodotPhysicsServer2D::area_get_shape(RID p_area, int p_shape_idx) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, RID()); + + GodotShape2D *shape = area->get_shape(p_shape_idx); + ERR_FAIL_NULL_V(shape, RID()); + + return shape->get_self(); +} + +Transform2D GodotPhysicsServer2D::area_get_shape_transform(RID p_area, int p_shape_idx) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, Transform2D()); + + return area->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer2D::area_remove_shape(RID p_area, int p_shape_idx) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer2D::area_clear_shapes(RID p_area) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + while (area->get_shape_count()) { + area->remove_shape(0); + } +} + +void GodotPhysicsServer2D::area_attach_object_instance_id(RID p_area, ObjectID p_id) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + area->set_instance_id(p_id); +} + +ObjectID GodotPhysicsServer2D::area_get_object_instance_id(RID p_area) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, ObjectID()); + return area->get_instance_id(); +} + +void GodotPhysicsServer2D::area_attach_canvas_instance_id(RID p_area, ObjectID p_id) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + area->set_canvas_instance_id(p_id); +} + +ObjectID GodotPhysicsServer2D::area_get_canvas_instance_id(RID p_area) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, ObjectID()); + return area->get_canvas_instance_id(); +} + +void GodotPhysicsServer2D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + area->set_param(p_param, p_value); +}; + +void GodotPhysicsServer2D::area_set_transform(RID p_area, const Transform2D &p_transform) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + area->set_transform(p_transform); +}; + +Variant GodotPhysicsServer2D::area_get_param(RID p_area, AreaParameter p_param) const { + if (space_owner.owns(p_area)) { + GodotSpace2D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, Variant()); + + return area->get_param(p_param); +}; + +Transform2D GodotPhysicsServer2D::area_get_transform(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, Transform2D()); + + return area->get_transform(); +}; + +void GodotPhysicsServer2D::area_set_pickable(RID p_area, bool p_pickable) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + area->set_pickable(p_pickable); +} + +void GodotPhysicsServer2D::area_set_monitorable(RID p_area, bool p_monitorable) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + FLUSH_QUERY_CHECK(area); + + area->set_monitorable(p_monitorable); +} + +void GodotPhysicsServer2D::area_set_collision_layer(RID p_area, uint32_t p_layer) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_collision_layer(p_layer); +} + +uint32_t GodotPhysicsServer2D::area_get_collision_layer(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, 0); + + return area->get_collision_layer(); +} + +void GodotPhysicsServer2D::area_set_collision_mask(RID p_area, uint32_t p_mask) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_collision_mask(p_mask); +} + +uint32_t GodotPhysicsServer2D::area_get_collision_mask(RID p_area) const { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, 0); + + return area->get_collision_mask(); +} + +void GodotPhysicsServer2D::area_set_monitor_callback(RID p_area, const Callable &p_callback) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_monitor_callback(p_callback.is_valid() ? p_callback : Callable()); +} + +void GodotPhysicsServer2D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) { + GodotArea2D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_area_monitor_callback(p_callback.is_valid() ? p_callback : Callable()); +} + +/* BODY API */ + +RID GodotPhysicsServer2D::body_create() { + GodotBody2D *body = memnew(GodotBody2D); + RID rid = body_owner.make_rid(body); + body->set_self(rid); + return rid; +} + +void GodotPhysicsServer2D::body_set_space(RID p_body, RID p_space) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + GodotSpace2D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + } + + if (body->get_space() == space) { + return; //pointless + } + + body->clear_constraint_list(); + body->set_space(space); +}; + +RID GodotPhysicsServer2D::body_get_space(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, RID()); + + GodotSpace2D *space = body->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +}; + +void GodotPhysicsServer2D::body_set_mode(RID p_body, BodyMode p_mode) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + FLUSH_QUERY_CHECK(body); + + body->set_mode(p_mode); +}; + +PhysicsServer2D::BodyMode GodotPhysicsServer2D::body_get_mode(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, BODY_MODE_STATIC); + + return body->get_mode(); +}; + +void GodotPhysicsServer2D::body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform, bool p_disabled) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + body->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer2D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + GodotShape2D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + ERR_FAIL_COND(!shape->is_configured()); + + body->set_shape(p_shape_idx, shape); +} + +void GodotPhysicsServer2D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_shape_transform(p_shape_idx, p_transform); +} + +int GodotPhysicsServer2D::body_get_shape_count(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, -1); + + return body->get_shape_count(); +} + +RID GodotPhysicsServer2D::body_get_shape(RID p_body, int p_shape_idx) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, RID()); + + GodotShape2D *shape = body->get_shape(p_shape_idx); + ERR_FAIL_NULL_V(shape, RID()); + + return shape->get_self(); +} + +Transform2D GodotPhysicsServer2D::body_get_shape_transform(RID p_body, int p_shape_idx) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Transform2D()); + + return body->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer2D::body_remove_shape(RID p_body, int p_shape_idx) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer2D::body_clear_shapes(RID p_body) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + while (body->get_shape_count()) { + body->remove_shape(0); + } +} + +void GodotPhysicsServer2D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); + FLUSH_QUERY_CHECK(body); + + body->set_shape_disabled(p_shape_idx, p_disabled); +} + +void GodotPhysicsServer2D::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); + FLUSH_QUERY_CHECK(body); + + body->set_shape_as_one_way_collision(p_shape_idx, p_enable, p_margin); +} + +void GodotPhysicsServer2D::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_continuous_collision_detection_mode(p_mode); +} + +GodotPhysicsServer2D::CCDMode GodotPhysicsServer2D::body_get_continuous_collision_detection_mode(RID p_body) const { + const GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, CCD_MODE_DISABLED); + + return body->get_continuous_collision_detection_mode(); +} + +void GodotPhysicsServer2D::body_attach_object_instance_id(RID p_body, ObjectID p_id) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_instance_id(p_id); +} + +ObjectID GodotPhysicsServer2D::body_get_object_instance_id(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, ObjectID()); + + return body->get_instance_id(); +} + +void GodotPhysicsServer2D::body_attach_canvas_instance_id(RID p_body, ObjectID p_id) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_canvas_instance_id(p_id); +} + +ObjectID GodotPhysicsServer2D::body_get_canvas_instance_id(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, ObjectID()); + + return body->get_canvas_instance_id(); +} + +void GodotPhysicsServer2D::body_set_collision_layer(RID p_body, uint32_t p_layer) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_collision_layer(p_layer); +} + +uint32_t GodotPhysicsServer2D::body_get_collision_layer(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_layer(); +} + +void GodotPhysicsServer2D::body_set_collision_mask(RID p_body, uint32_t p_mask) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_collision_mask(p_mask); +} + +uint32_t GodotPhysicsServer2D::body_get_collision_mask(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_mask(); +} + +void GodotPhysicsServer2D::body_set_collision_priority(RID p_body, real_t p_priority) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_priority(p_priority); +} + +real_t GodotPhysicsServer2D::body_get_collision_priority(RID p_body) const { + const GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_priority(); +} + +void GodotPhysicsServer2D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_param(p_param, p_value); +} + +Variant GodotPhysicsServer2D::body_get_param(RID p_body, BodyParameter p_param) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_param(p_param); +} + +void GodotPhysicsServer2D::body_reset_mass_properties(RID p_body) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->reset_mass_properties(); +} + +void GodotPhysicsServer2D::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_state(p_state, p_variant); +} + +Variant GodotPhysicsServer2D::body_get_state(RID p_body, BodyState p_state) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Variant()); + + return body->get_state(p_state); +} + +void GodotPhysicsServer2D::body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->apply_central_impulse(p_impulse); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_torque_impulse(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + _update_shapes(); + + body->apply_torque_impulse(p_torque); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + _update_shapes(); + + body->apply_impulse(p_impulse, p_position); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_central_force(RID p_body, const Vector2 &p_force) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->apply_central_force(p_force); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->apply_force(p_force, p_position); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_apply_torque(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->apply_torque(p_torque); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_add_constant_central_force(RID p_body, const Vector2 &p_force) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_central_force(p_force); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_force(p_force, p_position); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_add_constant_torque(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_torque(p_torque); + body->wakeup(); +} + +void GodotPhysicsServer2D::body_set_constant_force(RID p_body, const Vector2 &p_force) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_constant_force(p_force); + if (!p_force.is_zero_approx()) { + body->wakeup(); + } +} + +Vector2 GodotPhysicsServer2D::body_get_constant_force(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Vector2()); + return body->get_constant_force(); +} + +void GodotPhysicsServer2D::body_set_constant_torque(RID p_body, real_t p_torque) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_constant_torque(p_torque); + if (!Math::is_zero_approx(p_torque)) { + body->wakeup(); + } +} + +real_t GodotPhysicsServer2D::body_get_constant_torque(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_constant_torque(); +} + +void GodotPhysicsServer2D::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + _update_shapes(); + + Vector2 v = body->get_linear_velocity(); + Vector2 axis = p_axis_velocity.normalized(); + v -= axis * axis.dot(v); + v += p_axis_velocity; + body->set_linear_velocity(v); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_add_collision_exception(RID p_body, RID p_body_b) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_remove_collision_exception(RID p_body, RID p_body_b) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->remove_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer2D::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + for (int i = 0; i < body->get_exceptions().size(); i++) { + p_exceptions->push_back(body->get_exceptions()[i]); + } +}; + +void GodotPhysicsServer2D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); +}; + +real_t GodotPhysicsServer2D::body_get_contacts_reported_depth_threshold(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + return 0; +}; + +void GodotPhysicsServer2D::body_set_omit_force_integration(RID p_body, bool p_omit) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_omit_force_integration(p_omit); +}; + +bool GodotPhysicsServer2D::body_is_omitting_force_integration(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + return body->get_omit_force_integration(); +}; + +void GodotPhysicsServer2D::body_set_max_contacts_reported(RID p_body, int p_contacts) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_max_contacts_reported(p_contacts); +} + +int GodotPhysicsServer2D::body_get_max_contacts_reported(RID p_body) const { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, -1); + return body->get_max_contacts_reported(); +} + +void GodotPhysicsServer2D::body_set_state_sync_callback(RID p_body, const Callable &p_callable) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_state_sync_callback(p_callable); +} + +void GodotPhysicsServer2D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_force_integration_callback(p_callable, p_udata); +} + +bool GodotPhysicsServer2D::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false); + + return shape_collide(body->get_shape(p_body_shape)->get_self(), body->get_transform() * body->get_shape_transform(p_body_shape), Vector2(), p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count); +} + +void GodotPhysicsServer2D::body_set_pickable(RID p_body, bool p_pickable) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_pickable(p_pickable); +} + +bool GodotPhysicsServer2D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + ERR_FAIL_NULL_V(body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + _update_shapes(); + + return body->get_space()->test_body_motion(body, p_parameters, r_result); +} + +PhysicsDirectBodyState2D *GodotPhysicsServer2D::body_get_direct_state(RID p_body) { + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + if (!body_owner.owns(p_body)) { + return nullptr; + } + + GodotBody2D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, nullptr); + + if (!body->get_space()) { + return nullptr; + } + + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + return body->get_direct_state(); +} + +/* JOINT API */ + +RID GodotPhysicsServer2D::joint_create() { + GodotJoint2D *joint = memnew(GodotJoint2D); + RID joint_rid = joint_owner.make_rid(joint); + joint->set_self(joint_rid); + return joint_rid; +} + +void GodotPhysicsServer2D::joint_clear(RID p_joint) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + GodotJoint2D *empty_joint = memnew(GodotJoint2D); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + +void GodotPhysicsServer2D::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + switch (p_param) { + case JOINT_PARAM_BIAS: + joint->set_bias(p_value); + break; + case JOINT_PARAM_MAX_BIAS: + joint->set_max_bias(p_value); + break; + case JOINT_PARAM_MAX_FORCE: + joint->set_max_force(p_value); + break; + } +} + +real_t GodotPhysicsServer2D::joint_get_param(RID p_joint, JointParam p_param) const { + const GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, -1); + + switch (p_param) { + case JOINT_PARAM_BIAS: + return joint->get_bias(); + break; + case JOINT_PARAM_MAX_BIAS: + return joint->get_max_bias(); + break; + case JOINT_PARAM_MAX_FORCE: + return joint->get_max_force(); + break; + } + + return 0; +} + +void GodotPhysicsServer2D::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + joint->disable_collisions_between_bodies(p_disable); + + if (2 == joint->get_body_count()) { + GodotBody2D *body_a = *joint->get_body_ptr(); + GodotBody2D *body_b = *(joint->get_body_ptr() + 1); + + if (p_disable) { + body_add_collision_exception(body_a->get_self(), body_b->get_self()); + body_add_collision_exception(body_b->get_self(), body_a->get_self()); + } else { + body_remove_collision_exception(body_a->get_self(), body_b->get_self()); + body_remove_collision_exception(body_b->get_self(), body_a->get_self()); + } + } +} + +bool GodotPhysicsServer2D::joint_is_disabled_collisions_between_bodies(RID p_joint) const { + const GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, true); + + return joint->is_disabled_collisions_between_bodies(); +} + +void GodotPhysicsServer2D::joint_make_pin(RID p_joint, const Vector2 &p_pos, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_NULL(A); + GodotBody2D *B = nullptr; + if (body_owner.owns(p_body_b)) { + B = body_owner.get_or_null(p_body_b); + ERR_FAIL_NULL(B); + } + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint2D *joint = memnew(GodotPinJoint2D(p_pos, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_NULL(A); + + GodotBody2D *B = body_owner.get_or_null(p_body_b); + ERR_FAIL_NULL(B); + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint2D *joint = memnew(GodotGrooveJoint2D(p_a_groove1, p_a_groove2, p_b_anchor, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) { + GodotBody2D *A = body_owner.get_or_null(p_body_a); + ERR_FAIL_NULL(A); + + GodotBody2D *B = body_owner.get_or_null(p_body_b); + ERR_FAIL_NULL(B); + + GodotJoint2D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint2D *joint = memnew(GodotDampedSpringJoint2D(p_anchor_a, p_anchor_b, A, B)); + + joint_owner.replace(p_joint, joint); + joint->copy_settings_from(prev_joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer2D::pin_joint_set_flag(RID p_joint, PinJointFlag p_flag, bool p_enabled) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + + GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint); + pin_joint->set_flag(p_flag, p_enabled); +} + +bool GodotPhysicsServer2D::pin_joint_get_flag(RID p_joint, PinJointFlag p_flag) const { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); + + GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint); + return pin_joint->get_flag(p_flag); +} + +void GodotPhysicsServer2D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + + GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint); + pin_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); + + GodotPinJoint2D *pin_joint = static_cast<GodotPinJoint2D *>(joint); + return pin_joint->get_param(p_param); +} + +void GodotPhysicsServer2D::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_DAMPED_SPRING); + + GodotDampedSpringJoint2D *dsj = static_cast<GodotDampedSpringJoint2D *>(joint); + dsj->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer2D::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_DAMPED_SPRING, 0); + + GodotDampedSpringJoint2D *dsj = static_cast<GodotDampedSpringJoint2D *>(joint); + return dsj->get_param(p_param); +} + +PhysicsServer2D::JointType GodotPhysicsServer2D::joint_get_type(RID p_joint) const { + GodotJoint2D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, JOINT_TYPE_PIN); + + return joint->get_type(); +} + +void GodotPhysicsServer2D::free(RID p_rid) { + _update_shapes(); // just in case + + if (shape_owner.owns(p_rid)) { + GodotShape2D *shape = shape_owner.get_or_null(p_rid); + + while (shape->get_owners().size()) { + GodotShapeOwner2D *so = shape->get_owners().begin()->key; + so->remove_shape(shape); + } + + shape_owner.free(p_rid); + memdelete(shape); + } else if (body_owner.owns(p_rid)) { + GodotBody2D *body = body_owner.get_or_null(p_rid); + + body_set_space(p_rid, RID()); + + while (body->get_shape_count()) { + body->remove_shape(0); + } + + body_owner.free(p_rid); + memdelete(body); + + } else if (area_owner.owns(p_rid)) { + GodotArea2D *area = area_owner.get_or_null(p_rid); + + area->set_space(nullptr); + + while (area->get_shape_count()) { + area->remove_shape(0); + } + + area_owner.free(p_rid); + memdelete(area); + } else if (space_owner.owns(p_rid)) { + GodotSpace2D *space = space_owner.get_or_null(p_rid); + + while (space->get_objects().size()) { + GodotCollisionObject2D *co = static_cast<GodotCollisionObject2D *>(*space->get_objects().begin()); + co->set_space(nullptr); + } + + active_spaces.erase(space); + free(space->get_default_area()->get_self()); + space_owner.free(p_rid); + memdelete(space); + } else if (joint_owner.owns(p_rid)) { + GodotJoint2D *joint = joint_owner.get_or_null(p_rid); + + joint_owner.free(p_rid); + memdelete(joint); + + } else { + ERR_FAIL_MSG("Invalid ID."); + } +} + +void GodotPhysicsServer2D::set_active(bool p_active) { + active = p_active; +} + +void GodotPhysicsServer2D::init() { + doing_sync = false; + stepper = memnew(GodotStep2D); +} + +void GodotPhysicsServer2D::step(real_t p_step) { + if (!active) { + return; + } + + _update_shapes(); + + island_count = 0; + active_objects = 0; + collision_pairs = 0; + for (const GodotSpace2D *E : active_spaces) { + stepper->step(const_cast<GodotSpace2D *>(E), p_step); + island_count += E->get_island_count(); + active_objects += E->get_active_objects(); + collision_pairs += E->get_collision_pairs(); + } +} + +void GodotPhysicsServer2D::sync() { + doing_sync = true; +} + +void GodotPhysicsServer2D::flush_queries() { + if (!active) { + return; + } + + flushing_queries = true; + + uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); + + for (const GodotSpace2D *E : active_spaces) { + GodotSpace2D *space = const_cast<GodotSpace2D *>(E); + space->call_queries(); + } + + flushing_queries = false; + + if (EngineDebugger::is_profiling("servers")) { + uint64_t total_time[GodotSpace2D::ELAPSED_TIME_MAX]; + static const char *time_name[GodotSpace2D::ELAPSED_TIME_MAX] = { + "integrate_forces", + "generate_islands", + "setup_constraints", + "solve_constraints", + "integrate_velocities" + }; + + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + total_time[i] = 0; + } + + for (const GodotSpace2D *E : active_spaces) { + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + total_time[i] += E->get_elapsed_time(GodotSpace2D::ElapsedTime(i)); + } + } + + Array values; + values.resize(GodotSpace2D::ELAPSED_TIME_MAX * 2); + for (int i = 0; i < GodotSpace2D::ELAPSED_TIME_MAX; i++) { + values[i * 2 + 0] = time_name[i]; + values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); + } + values.push_back("flush_queries"); + values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); + + values.push_front("physics_2d"); + EngineDebugger::profiler_add_frame_data("servers", values); + } +} + +void GodotPhysicsServer2D::end_sync() { + doing_sync = false; +} + +void GodotPhysicsServer2D::finish() { + memdelete(stepper); +} + +void GodotPhysicsServer2D::_update_shapes() { + while (pending_shape_update_list.first()) { + pending_shape_update_list.first()->self()->_shape_changed(); + pending_shape_update_list.remove(pending_shape_update_list.first()); + } +} + +int GodotPhysicsServer2D::get_process_info(ProcessInfo p_info) { + switch (p_info) { + case INFO_ACTIVE_OBJECTS: { + return active_objects; + } break; + case INFO_COLLISION_PAIRS: { + return collision_pairs; + } break; + case INFO_ISLAND_COUNT: { + return island_count; + } break; + } + + return 0; +} + +GodotPhysicsServer2D *GodotPhysicsServer2D::godot_singleton = nullptr; + +GodotPhysicsServer2D::GodotPhysicsServer2D(bool p_using_threads) { + godot_singleton = this; + GodotBroadPhase2D::create_func = GodotBroadPhase2DBVH::_create; + + using_threads = p_using_threads; +} diff --git a/modules/godot_physics_2d/godot_physics_server_2d.h b/modules/godot_physics_2d/godot_physics_server_2d.h new file mode 100644 index 0000000000..991cf67c95 --- /dev/null +++ b/modules/godot_physics_2d/godot_physics_server_2d.h @@ -0,0 +1,307 @@ +/**************************************************************************/ +/* godot_physics_server_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_PHYSICS_SERVER_2D_H +#define GODOT_PHYSICS_SERVER_2D_H + +#include "godot_joints_2d.h" +#include "godot_shape_2d.h" +#include "godot_space_2d.h" +#include "godot_step_2d.h" + +#include "core/templates/rid_owner.h" +#include "servers/physics_server_2d.h" + +class GodotPhysicsServer2D : public PhysicsServer2D { + GDCLASS(GodotPhysicsServer2D, PhysicsServer2D); + + friend class GodotPhysicsDirectSpaceState2D; + friend class GodotPhysicsDirectBodyState2D; + bool active = true; + bool doing_sync = false; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + bool using_threads = false; + + bool flushing_queries = false; + + GodotStep2D *stepper = nullptr; + HashSet<const GodotSpace2D *> active_spaces; + + mutable RID_PtrOwner<GodotShape2D, true> shape_owner; + mutable RID_PtrOwner<GodotSpace2D, true> space_owner; + mutable RID_PtrOwner<GodotArea2D, true> area_owner; + mutable RID_PtrOwner<GodotBody2D, true> body_owner; + mutable RID_PtrOwner<GodotJoint2D, true> joint_owner; + + static GodotPhysicsServer2D *godot_singleton; + + friend class GodotCollisionObject2D; + SelfList<GodotCollisionObject2D>::List pending_shape_update_list; + void _update_shapes(); + + RID _shape_create(ShapeType p_shape); + +public: + struct CollCbkData { + Vector2 valid_dir; + real_t valid_depth = 0.0; + int max = 0; + int amount = 0; + int passed = 0; + int invalid_by_dir = 0; + Vector2 *ptr = nullptr; + }; + + virtual RID world_boundary_shape_create() override; + virtual RID separation_ray_shape_create() override; + virtual RID segment_shape_create() override; + virtual RID circle_shape_create() override; + virtual RID rectangle_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + + static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); + + virtual void shape_set_data(RID p_shape, const Variant &p_data) override; + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; + + virtual ShapeType shape_get_type(RID p_shape) const override; + virtual Variant shape_get_data(RID p_shape) const override; + virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; + + virtual bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override; + + /* SPACE API */ + + virtual RID space_create() override; + virtual void space_set_active(RID p_space, bool p_active) override; + virtual bool space_is_active(RID p_space) const override; + + virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override; + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; + + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; + virtual Vector<Vector2> space_get_contacts(RID p_space) const override; + virtual int space_get_contact_count(RID p_space) const override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override; + + /* AREA API */ + + virtual RID area_create() override; + + virtual void area_set_space(RID p_area, RID p_space) override; + virtual RID area_get_space(RID p_area) const override; + + virtual void area_add_shape(RID p_area, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false) override; + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform2D &p_transform) override; + + virtual int area_get_shape_count(RID p_area) const override; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; + virtual Transform2D area_get_shape_transform(RID p_area, int p_shape_idx) const override; + + virtual void area_set_shape_disabled(RID p_area, int p_shape, bool p_disabled) override; + + virtual void area_remove_shape(RID p_area, int p_shape_idx) override; + virtual void area_clear_shapes(RID p_area) override; + + virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override; + virtual ObjectID area_get_object_instance_id(RID p_area) const override; + + virtual void area_attach_canvas_instance_id(RID p_area, ObjectID p_id) override; + virtual ObjectID area_get_canvas_instance_id(RID p_area) const override; + + virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; + virtual void area_set_transform(RID p_area, const Transform2D &p_transform) override; + + virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; + virtual Transform2D area_get_transform(RID p_area) const override; + virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; + + virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; + virtual uint32_t area_get_collision_layer(RID p_area) const override; + + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual uint32_t area_get_collision_mask(RID p_area) const override; + + virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override; + virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override; + + virtual void area_set_pickable(RID p_area, bool p_pickable) override; + + /* BODY API */ + + // create a body of a given type + virtual RID body_create() override; + + virtual void body_set_space(RID p_body, RID p_space) override; + virtual RID body_get_space(RID p_body) const override; + + virtual void body_set_mode(RID p_body, BodyMode p_mode) override; + virtual BodyMode body_get_mode(RID p_body) const override; + + virtual void body_add_shape(RID p_body, RID p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false) override; + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform2D &p_transform) override; + + virtual int body_get_shape_count(RID p_body) const override; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; + virtual Transform2D body_get_shape_transform(RID p_body, int p_shape_idx) const override; + + virtual void body_remove_shape(RID p_body, int p_shape_idx) override; + virtual void body_clear_shapes(RID p_body) override; + + virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; + virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) override; + + virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override; + virtual ObjectID body_get_object_instance_id(RID p_body) const override; + + virtual void body_attach_canvas_instance_id(RID p_body, ObjectID p_id) override; + virtual ObjectID body_get_canvas_instance_id(RID p_body) const override; + + virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) override; + virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const override; + + virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t body_get_collision_layer(RID p_body) const override; + + virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t body_get_collision_mask(RID p_body) const override; + + virtual void body_set_collision_priority(RID p_body, real_t p_priority) override; + virtual real_t body_get_collision_priority(RID p_body) const override; + + virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; + virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; + + virtual void body_reset_mass_properties(RID p_body) override; + + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; + virtual Variant body_get_state(RID p_body, BodyState p_state) const override; + + virtual void body_apply_central_impulse(RID p_body, const Vector2 &p_impulse) override; + virtual void body_apply_torque_impulse(RID p_body, real_t p_torque) override; + virtual void body_apply_impulse(RID p_body, const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override; + + virtual void body_apply_central_force(RID p_body, const Vector2 &p_force) override; + virtual void body_apply_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void body_apply_torque(RID p_body, real_t p_torque) override; + + virtual void body_add_constant_central_force(RID p_body, const Vector2 &p_force) override; + virtual void body_add_constant_force(RID p_body, const Vector2 &p_force, const Vector2 &p_position = Vector2()) override; + virtual void body_add_constant_torque(RID p_body, real_t p_torque) override; + + virtual void body_set_constant_force(RID p_body, const Vector2 &p_force) override; + virtual Vector2 body_get_constant_force(RID p_body) const override; + + virtual void body_set_constant_torque(RID p_body, real_t p_torque) override; + virtual real_t body_get_constant_torque(RID p_body) const override; + + virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) override; + + virtual void body_add_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override; + + virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override; + virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override; + + virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override; + virtual bool body_is_omitting_force_integration(RID p_body) const override; + + virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; + virtual int body_get_max_contacts_reported(RID p_body) const override; + + virtual void body_set_state_sync_callback(RID p_body, const Callable &p_callable) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; + + virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override; + + virtual void body_set_pickable(RID p_body, bool p_pickable) override; + + virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override; + + /* JOINT API */ + + virtual RID joint_create() override; + + virtual void joint_clear(RID p_joint) override; + + virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override; + virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override; + + virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override; + virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; + + virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override; + virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override; + virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override; + + virtual void pin_joint_set_flag(RID p_joint, PinJointFlag p_flag, bool p_enabled) override; + virtual bool pin_joint_get_flag(RID p_joint, PinJointFlag p_flag) const override; + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; + virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; + virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override; + virtual real_t damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const override; + + virtual JointType joint_get_type(RID p_joint) const override; + + /* MISC */ + + virtual void free(RID p_rid) override; + + virtual void set_active(bool p_active) override; + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void flush_queries() override; + virtual void end_sync() override; + virtual void finish() override; + + virtual bool is_flushing_queries() const override { return flushing_queries; } + + int get_process_info(ProcessInfo p_info) override; + + GodotPhysicsServer2D(bool p_using_threads = false); + ~GodotPhysicsServer2D() {} +}; + +#endif // GODOT_PHYSICS_SERVER_2D_H diff --git a/modules/godot_physics_2d/godot_shape_2d.cpp b/modules/godot_physics_2d/godot_shape_2d.cpp new file mode 100644 index 0000000000..d77b1a77e3 --- /dev/null +++ b/modules/godot_physics_2d/godot_shape_2d.cpp @@ -0,0 +1,985 @@ +/**************************************************************************/ +/* godot_shape_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_shape_2d.h" + +#include "core/math/geometry_2d.h" +#include "core/templates/sort_array.h" + +void GodotShape2D::configure(const Rect2 &p_aabb) { + aabb = p_aabb; + configured = true; + for (const KeyValue<GodotShapeOwner2D *, int> &E : owners) { + GodotShapeOwner2D *co = const_cast<GodotShapeOwner2D *>(E.key); + co->_shape_changed(); + } +} + +Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const { + Vector2 res[2]; + int amnt; + get_supports(p_normal, res, amnt); + return res[0]; +} + +void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) { + HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner); + if (E) { + E->value++; + } else { + owners[p_owner] = 1; + } +} + +void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) { + HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->value--; + if (E->value == 0) { + owners.remove(E); + } +} + +bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const { + return owners.has(p_owner); +} + +const HashMap<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const { + return owners; +} + +GodotShape2D::~GodotShape2D() { + ERR_FAIL_COND(owners.size()); +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotWorldBoundaryShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 0; +} + +bool GodotWorldBoundaryShape2D::contains_point(const Vector2 &p_point) const { + return normal.dot(p_point) < d; +} + +bool GodotWorldBoundaryShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 segment = p_begin - p_end; + real_t den = normal.dot(segment); + + //printf("den is %i\n",den); + if (Math::abs(den) <= CMP_EPSILON) { + return false; + } + + real_t dist = (normal.dot(p_begin) - d) / den; + //printf("dist is %i\n",dist); + + if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { + return false; + } + + r_point = p_begin + segment * -dist; + r_normal = normal; + + return true; +} + +real_t GodotWorldBoundaryShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return 0; +} + +void GodotWorldBoundaryShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); + Array arr = p_data; + ERR_FAIL_COND(arr.size() != 2); + normal = arr[0]; + d = arr[1]; + configure(Rect2(Vector2(-1e15, -1e15), Vector2(1e15 * 2, 1e15 * 2))); +} + +Variant GodotWorldBoundaryShape2D::get_data() const { + Array arr; + arr.resize(2); + arr[0] = normal; + arr[1] = d; + return arr; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotSeparationRayShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 1; + + if (p_normal.y > 0) { + *r_supports = Vector2(0, length); + } else { + *r_supports = Vector2(); + } +} + +bool GodotSeparationRayShape2D::contains_point(const Vector2 &p_point) const { + return false; +} + +bool GodotSeparationRayShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + return false; //rays can't be intersected +} + +real_t GodotSeparationRayShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return 0; //rays are mass-less +} + +void GodotSeparationRayShape2D::set_data(const Variant &p_data) { + Dictionary d = p_data; + length = d["length"]; + slide_on_slope = d["slide_on_slope"]; + configure(Rect2(0, 0, 0.001, length)); +} + +Variant GodotSeparationRayShape2D::get_data() const { + Dictionary d; + d["length"] = length; + d["slide_on_slope"] = slide_on_slope; + return d; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotSegmentShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + if (Math::abs(p_normal.dot(n)) > segment_is_valid_support_threshold) { + r_supports[0] = a; + r_supports[1] = b; + r_amount = 2; + return; + } + + real_t dp = p_normal.dot(b - a); + if (dp > 0) { + *r_supports = b; + } else { + *r_supports = a; + } + r_amount = 1; +} + +bool GodotSegmentShape2D::contains_point(const Vector2 &p_point) const { + return false; +} + +bool GodotSegmentShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) { + return false; + } + + if (n.dot(p_begin) > n.dot(a)) { + r_normal = n; + } else { + r_normal = -n; + } + + return true; +} + +real_t GodotSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12; +} + +void GodotSegmentShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); + + Rect2 r = p_data; + a = r.position; + b = r.size; + n = (b - a).orthogonal(); + + Rect2 aabb_new; + aabb_new.position = a; + aabb_new.expand_to(b); + if (aabb_new.size.x == 0) { + aabb_new.size.x = 0.001; + } + if (aabb_new.size.y == 0) { + aabb_new.size.y = 0.001; + } + configure(aabb_new); +} + +Variant GodotSegmentShape2D::get_data() const { + Rect2 r; + r.position = a; + r.size = b; + return r; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotCircleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + r_amount = 1; + *r_supports = p_normal * radius; +} + +bool GodotCircleShape2D::contains_point(const Vector2 &p_point) const { + return p_point.length_squared() < radius * radius; +} + +bool GodotCircleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 line_vec = p_end - p_begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * p_begin.dot(line_vec); + c = p_begin.dot(p_begin) - radius * radius; + + real_t sqrtterm = b * b - 4 * a * c; + + if (sqrtterm < 0) { + return false; + } + sqrtterm = Math::sqrt(sqrtterm); + real_t res = (-b - sqrtterm) / (2 * a); + + if (res < 0 || res > 1 + CMP_EPSILON) { + return false; + } + + r_point = p_begin + line_vec * res; + r_normal = r_point.normalized(); + return true; +} + +real_t GodotCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + real_t a = radius * p_scale.x; + real_t b = radius * p_scale.y; + return p_mass * (a * a + b * b) / 4; +} + +void GodotCircleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(!p_data.is_num()); + radius = p_data; + configure(Rect2(-radius, -radius, radius * 2, radius * 2)); +} + +Variant GodotCircleShape2D::get_data() const { + return radius; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + for (int i = 0; i < 2; i++) { + Vector2 ag; + ag[i] = 1.0; + real_t dp = ag.dot(p_normal); + if (Math::abs(dp) <= segment_is_valid_support_threshold) { + continue; + } + + real_t sgn = dp > 0 ? 1.0 : -1.0; + + r_amount = 2; + + r_supports[0][i] = half_extents[i] * sgn; + r_supports[0][i ^ 1] = half_extents[i ^ 1]; + + r_supports[1][i] = half_extents[i] * sgn; + r_supports[1][i ^ 1] = -half_extents[i ^ 1]; + + return; + } + + /* USE POINT */ + + r_amount = 1; + r_supports[0] = Vector2( + (p_normal.x < 0) ? -half_extents.x : half_extents.x, + (p_normal.y < 0) ? -half_extents.y : half_extents.y); +} + +bool GodotRectangleShape2D::contains_point(const Vector2 &p_point) const { + real_t x = p_point.x; + real_t y = p_point.y; + real_t edge_x = half_extents.x; + real_t edge_y = half_extents.y; + return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y); +} + +bool GodotRectangleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal); +} + +real_t GodotRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + Vector2 he2 = half_extents * 2 * p_scale; + return p_mass * he2.dot(he2) / 12.0; +} + +void GodotRectangleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); + + half_extents = p_data; + configure(Rect2(-half_extents, half_extents * 2.0)); +} + +Variant GodotRectangleShape2D::get_data() const { + return half_extents; +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + Vector2 n = p_normal; + + real_t h = height * 0.5 - radius; // half-height of the rectangle part + + if (h > 0 && Math::abs(n.x) > segment_is_valid_support_threshold) { + // make it flat + n.y = 0.0; + n.x = SIGN(n.x) * radius; + + r_amount = 2; + r_supports[0] = n; + r_supports[0].y += h; + r_supports[1] = n; + r_supports[1].y -= h; + } else { + n *= radius; + n.y += (n.y > 0) ? h : -h; + r_amount = 1; + *r_supports = n; + } +} + +bool GodotCapsuleShape2D::contains_point(const Vector2 &p_point) const { + Vector2 p = p_point; + p.y = Math::abs(p.y); + p.y -= height * 0.5 - radius; + if (p.y < 0) { + p.y = 0; + } + + return p.length_squared() < radius * radius; +} + +bool GodotCapsuleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + real_t d = 1e10; + Vector2 n = (p_end - p_begin).normalized(); + bool collided = false; + + //try spheres + for (int i = 0; i < 2; i++) { + Vector2 begin = p_begin; + Vector2 end = p_end; + real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius; + begin.y += ofs; + end.y += ofs; + + Vector2 line_vec = end - begin; + + real_t a, b, c; + + a = line_vec.dot(line_vec); + b = 2 * begin.dot(line_vec); + c = begin.dot(begin) - radius * radius; + + real_t sqrtterm = b * b - 4 * a * c; + + if (sqrtterm < 0) { + continue; + } + + sqrtterm = Math::sqrt(sqrtterm); + real_t res = (-b - sqrtterm) / (2 * a); + + if (res < 0 || res > 1 + CMP_EPSILON) { + continue; + } + + Vector2 point = begin + line_vec * res; + Vector2 pointf(point.x, point.y - ofs); + real_t pd = n.dot(pointf); + if (pd < d) { + r_point = pointf; + r_normal = point.normalized(); + d = pd; + collided = true; + } + } + + Vector2 rpos, rnorm; + if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { + real_t pd = n.dot(rpos); + if (pd < d) { + r_point = rpos; + r_normal = rnorm; + d = pd; + collided = true; + } + } + + //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); + return collided; //todo +} + +real_t GodotCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + Vector2 he2 = Vector2(radius * 2, height) * p_scale; + return p_mass * he2.dot(he2) / 12.0; +} + +void GodotCapsuleShape2D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); + + if (p_data.get_type() == Variant::ARRAY) { + Array arr = p_data; + ERR_FAIL_COND(arr.size() != 2); + height = arr[0]; + radius = arr[1]; + } else { + Point2 p = p_data; + radius = p.x; + height = p.y; + } + + Point2 he(radius, height * 0.5); + configure(Rect2(-he, he * 2)); +} + +Variant GodotCapsuleShape2D::get_data() const { + return Point2(height, radius); +} + +/*********************************************************/ +/*********************************************************/ +/*********************************************************/ + +void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + int support_idx = -1; + real_t d = -1e10; + r_amount = 0; + + for (int i = 0; i < point_count; i++) { + //test point + real_t ld = p_normal.dot(points[i].pos); + if (ld > d) { + support_idx = i; + d = ld; + } + + //test segment + if (points[i].normal.dot(p_normal) > segment_is_valid_support_threshold) { + r_amount = 2; + r_supports[0] = points[i].pos; + r_supports[1] = points[(i + 1) % point_count].pos; + return; + } + } + + ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found."); + + r_amount = 1; + r_supports[0] = points[support_idx].pos; +} + +bool GodotConvexPolygonShape2D::contains_point(const Vector2 &p_point) const { + bool out = false; + bool in = false; + + for (int i = 0; i < point_count; i++) { + real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos); + if (d > 0) { + out = true; + } else { + in = true; + } + } + + return in != out; +} + +bool GodotConvexPolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + Vector2 n = (p_end - p_begin).normalized(); + real_t d = 1e10; + bool inters = false; + + for (int i = 0; i < point_count; i++) { + Vector2 res; + + if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) { + continue; + } + + real_t nd = n.dot(res); + if (nd < d) { + d = nd; + r_point = res; + r_normal = points[i].normal; + inters = true; + } + } + + return inters; +} + +real_t GodotConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { + ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); + Rect2 aabb_new; + aabb_new.position = points[0].pos * p_scale; + for (int i = 0; i < point_count; i++) { + aabb_new.expand_to(points[i].pos * p_scale); + } + + return p_mass * aabb_new.size.dot(aabb_new.size) / 12.0; +} + +void GodotConvexPolygonShape2D::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + if (points) { + memdelete_arr(points); + } + points = nullptr; + point_count = 0; + + if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { + Vector<Vector2> arr = p_data; + ERR_FAIL_COND(arr.is_empty()); + point_count = arr.size(); + points = memnew_arr(Point, point_count); + const Vector2 *r = arr.ptr(); + + for (int i = 0; i < point_count; i++) { + points[i].pos = r[i]; + } + + for (int i = 0; i < point_count; i++) { + Vector2 p = points[i].pos; + Vector2 pn = points[(i + 1) % point_count].pos; + points[i].normal = (pn - p).orthogonal().normalized(); + } + } else { + Vector<real_t> dvr = p_data; + point_count = dvr.size() / 4; + ERR_FAIL_COND(point_count == 0); + + points = memnew_arr(Point, point_count); + const real_t *r = dvr.ptr(); + + for (int i = 0; i < point_count; i++) { + int idx = i << 2; + points[i].pos.x = r[idx + 0]; + points[i].pos.y = r[idx + 1]; + points[i].normal.x = r[idx + 2]; + points[i].normal.y = r[idx + 3]; + } + } + + ERR_FAIL_COND(point_count == 0); + Rect2 aabb_new; + aabb_new.position = points[0].pos; + for (int i = 1; i < point_count; i++) { + aabb_new.expand_to(points[i].pos); + } + + configure(aabb_new); +} + +Variant GodotConvexPolygonShape2D::get_data() const { + Vector<Vector2> dvr; + + dvr.resize(point_count); + + for (int i = 0; i < point_count; i++) { + dvr.set(i, points[i].pos); + } + + return dvr; +} + +GodotConvexPolygonShape2D::~GodotConvexPolygonShape2D() { + if (points) { + memdelete_arr(points); + } +} + +////////////////////////////////////////////////// + +void GodotConcavePolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { + real_t d = -1e10; + int idx = -1; + for (int i = 0; i < points.size(); i++) { + real_t ld = p_normal.dot(points[i]); + if (ld > d) { + d = ld; + idx = i; + } + } + + r_amount = 1; + ERR_FAIL_COND(idx == -1); + *r_supports = points[idx]; +} + +bool GodotConcavePolygonShape2D::contains_point(const Vector2 &p_point) const { + return false; //sorry +} + +bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { + if (segments.size() == 0 || points.size() == 0) { + return false; + } + + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); + + enum { + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, + + }; + + Vector2 n = (p_end - p_begin).normalized(); + real_t d = 1e10; + bool inters = false; + + /* + for(int i=0;i<bvh_depth;i++) + stack[i]=0; + */ + + int level = 0; + + const Segment *segmentptr = &segments[0]; + const Vector2 *pointptr = &points[0]; + const BVH *bvhptr = &bvh[0]; + + stack[0] = 0; + while (true) { + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &bvh2 = bvhptr[node]; + bool done = false; + + switch (stack[level] >> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + bool valid = bvh2.aabb.intersects_segment(p_begin, p_end); + if (!valid) { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + if (bvh2.left < 0) { + const Segment &s = segmentptr[bvh2.right]; + Vector2 a = pointptr[s.points[0]]; + Vector2 b = pointptr[s.points[1]]; + + Vector2 res; + + if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) { + real_t nd = n.dot(res); + if (nd < d) { + d = nd; + r_point = res; + r_normal = (b - a).orthogonal().normalized(); + inters = true; + } + } + + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; + } + } + } + continue; + case VISIT_LEFT_BIT: { + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh2.left | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_RIGHT_BIT: { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh2.right | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_DONE_BIT: { + if (level == 0) { + done = true; + break; + } else { + level--; + } + } + continue; + } + + if (done) { + break; + } + } + + if (inters) { + if (n.dot(r_normal) > 0) { + r_normal = -r_normal; + } + } + + return inters; +} + +int GodotConcavePolygonShape2D::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { + if (p_len == 1) { + bvh_depth = MAX(p_depth, bvh_depth); + bvh.push_back(*p_bvh); + return bvh.size() - 1; + } + + //else sort best + + Rect2 global_aabb = p_bvh[0].aabb; + for (int i = 1; i < p_len; i++) { + global_aabb = global_aabb.merge(p_bvh[i].aabb); + } + + if (global_aabb.size.x > global_aabb.size.y) { + SortArray<BVH, BVH_CompareX> sort; + sort.sort(p_bvh, p_len); + + } else { + SortArray<BVH, BVH_CompareY> sort; + sort.sort(p_bvh, p_len); + } + + int median = p_len / 2; + + BVH node; + node.aabb = global_aabb; + int node_idx = bvh.size(); + bvh.push_back(node); + + int l = _generate_bvh(p_bvh, median, p_depth + 1); + int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1); + bvh.write[node_idx].left = l; + bvh.write[node_idx].right = r; + + return node_idx; +} + +void GodotConcavePolygonShape2D::set_data(const Variant &p_data) { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); +#else + ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); +#endif + + Rect2 aabb_new; + + if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { + Vector<Vector2> p2arr = p_data; + int len = p2arr.size(); + ERR_FAIL_COND(len % 2); + + segments.clear(); + points.clear(); + bvh.clear(); + bvh_depth = 1; + + if (len == 0) { + configure(aabb_new); + return; + } + + const Vector2 *arr = p2arr.ptr(); + + HashMap<Point2, int> pointmap; + for (int i = 0; i < len; i += 2) { + Point2 p1 = arr[i]; + Point2 p2 = arr[i + 1]; + int idx_p1, idx_p2; + + if (pointmap.has(p1)) { + idx_p1 = pointmap[p1]; + } else { + idx_p1 = pointmap.size(); + pointmap[p1] = idx_p1; + } + + if (pointmap.has(p2)) { + idx_p2 = pointmap[p2]; + } else { + idx_p2 = pointmap.size(); + pointmap[p2] = idx_p2; + } + + Segment s; + s.points[0] = idx_p1; + s.points[1] = idx_p2; + segments.push_back(s); + } + + points.resize(pointmap.size()); + aabb_new.position = pointmap.begin()->key; + for (const KeyValue<Point2, int> &E : pointmap) { + aabb_new.expand_to(E.key); + points.write[E.value] = E.key; + } + + Vector<BVH> main_vbh; + main_vbh.resize(segments.size()); + for (int i = 0; i < main_vbh.size(); i++) { + main_vbh.write[i].aabb.position = points[segments[i].points[0]]; + main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]); + main_vbh.write[i].left = -1; + main_vbh.write[i].right = i; + } + + _generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1); + + } else { + //dictionary with arrays + } + + configure(aabb_new); +} + +Variant GodotConcavePolygonShape2D::get_data() const { + Vector<Vector2> rsegments; + int len = segments.size(); + rsegments.resize(len * 2); + Vector2 *w = rsegments.ptrw(); + for (int i = 0; i < len; i++) { + w[(i << 1) + 0] = points[segments[i].points[0]]; + w[(i << 1) + 1] = points[segments[i].points[1]]; + } + + return rsegments; +} + +void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { + uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); + + enum { + TEST_AABB_BIT = 0, + VISIT_LEFT_BIT = 1, + VISIT_RIGHT_BIT = 2, + VISIT_DONE_BIT = 3, + VISITED_BIT_SHIFT = 29, + NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, + VISITED_BIT_MASK = ~NODE_IDX_MASK, + + }; + + /* + for(int i=0;i<bvh_depth;i++) + stack[i]=0; + */ + + if (segments.size() == 0 || points.size() == 0 || bvh.size() == 0) { + return; + } + + int level = 0; + + const Segment *segmentptr = &segments[0]; + const Vector2 *pointptr = &points[0]; + const BVH *bvhptr = &bvh[0]; + + stack[0] = 0; + while (true) { + uint32_t node = stack[level] & NODE_IDX_MASK; + const BVH &bvh2 = bvhptr[node]; + + switch (stack[level] >> VISITED_BIT_SHIFT) { + case TEST_AABB_BIT: { + bool valid = p_local_aabb.intersects(bvh2.aabb); + if (!valid) { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + if (bvh2.left < 0) { + const Segment &s = segmentptr[bvh2.right]; + Vector2 a = pointptr[s.points[0]]; + Vector2 b = pointptr[s.points[1]]; + + GodotSegmentShape2D ss(a, b, (b - a).orthogonal().normalized()); + + if (p_callback(p_userdata, &ss)) { + return; + } + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + + } else { + stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; + } + } + } + continue; + case VISIT_LEFT_BIT: { + stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh2.left | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_RIGHT_BIT: { + stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; + stack[level + 1] = bvh2.right | TEST_AABB_BIT; + level++; + } + continue; + case VISIT_DONE_BIT: { + if (level == 0) { + return; + } else { + level--; + } + } + continue; + } + } +} diff --git a/modules/godot_physics_2d/godot_shape_2d.h b/modules/godot_physics_2d/godot_shape_2d.h new file mode 100644 index 0000000000..28c69574a0 --- /dev/null +++ b/modules/godot_physics_2d/godot_shape_2d.h @@ -0,0 +1,539 @@ +/**************************************************************************/ +/* godot_shape_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_SHAPE_2D_H +#define GODOT_SHAPE_2D_H + +#include "servers/physics_server_2d.h" + +class GodotShape2D; + +class GodotShapeOwner2D { +public: + virtual void _shape_changed() = 0; + virtual void remove_shape(GodotShape2D *p_shape) = 0; + + virtual ~GodotShapeOwner2D() {} +}; + +class GodotShape2D { + RID self; + Rect2 aabb; + bool configured = false; + real_t custom_bias = 0.0; + + HashMap<GodotShapeOwner2D *, int> owners; + +protected: + const double segment_is_valid_support_threshold = 0.99998; + const double segment_is_valid_support_threshold_lower = + Math::sqrt(1.0 - segment_is_valid_support_threshold * segment_is_valid_support_threshold); + + void configure(const Rect2 &p_aabb); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + virtual PhysicsServer2D::ShapeType get_type() const = 0; + + _FORCE_INLINE_ Rect2 get_aabb() const { return aabb; } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + virtual bool allows_one_way_collision() const { return true; } + + virtual bool is_concave() const { return false; } + + virtual bool contains_point(const Vector2 &p_point) const = 0; + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual Vector2 get_support(const Vector2 &p_normal) const; + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0; + + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const = 0; + virtual void set_data(const Variant &p_data) = 0; + virtual Variant get_data() const = 0; + + _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } + _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } + + void add_owner(GodotShapeOwner2D *p_owner); + void remove_owner(GodotShapeOwner2D *p_owner); + bool is_owner(GodotShapeOwner2D *p_owner) const; + const HashMap<GodotShapeOwner2D *, int> &get_owners() const; + + _FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_xform, Vector2 *r_supports, int &r_amount) const { + get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount); + for (int i = 0; i < r_amount; i++) { + r_supports[i] = p_xform.xform(r_supports[i]); + } + + if (r_amount == 1) { + if (Math::abs(p_normal.dot(p_cast.normalized())) < segment_is_valid_support_threshold_lower) { + //make line because they are parallel + r_amount = 2; + r_supports[1] = r_supports[0] + p_cast; + } else if (p_cast.dot(p_normal) > 0) { + //normal points towards cast, add cast + r_supports[0] += p_cast; + } + + } else { + if (Math::abs(p_normal.dot(p_cast.normalized())) < segment_is_valid_support_threshold_lower) { + //optimize line and make it larger because they are parallel + if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) { + //larger towards 1 + r_supports[1] += p_cast; + } else { + //larger towards 0 + r_supports[0] += p_cast; + } + } else if (p_cast.dot(p_normal) > 0) { + //normal points towards cast, add cast + r_supports[0] += p_cast; + r_supports[1] += p_cast; + } + } + } + GodotShape2D() {} + virtual ~GodotShape2D(); +}; + +//let the optimizer do the magic +#define DEFAULT_PROJECT_RANGE_CAST \ + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { \ + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); \ + } \ + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { \ + real_t mina, maxa; \ + real_t minb, maxb; \ + Transform2D ofsb = p_transform; \ + ofsb.columns[2] += p_cast; \ + project_range(p_normal, p_transform, mina, maxa); \ + project_range(p_normal, ofsb, minb, maxb); \ + r_min = MIN(mina, minb); \ + r_max = MAX(maxa, maxb); \ + } + +class GodotWorldBoundaryShape2D : public GodotShape2D { + Vector2 normal; + real_t d = 0.0; + +public: + _FORCE_INLINE_ Vector2 get_normal() const { return normal; } + _FORCE_INLINE_ real_t get_d() const { return d; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_WORLD_BOUNDARY; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_min = -1e10; + r_max = 1e10; + } + + virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { + project_range_cast(p_cast, p_normal, p_transform, r_min, r_max); + } + + _FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_min = -1e10; + r_max = 1e10; + } +}; + +class GodotSeparationRayShape2D : public GodotShape2D { + real_t length = 0.0; + bool slide_on_slope = false; + +public: + _FORCE_INLINE_ real_t get_length() const { return length; } + _FORCE_INLINE_ bool get_slide_on_slope() const { return slide_on_slope; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEPARATION_RAY; } + + virtual bool allows_one_way_collision() const override { return false; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_max = p_normal.dot(p_transform.get_origin()); + r_min = p_normal.dot(p_transform.xform(Vector2(0, length))); + if (r_max < r_min) { + SWAP(r_max, r_min); + } + } + + DEFAULT_PROJECT_RANGE_CAST + + _FORCE_INLINE_ GodotSeparationRayShape2D() {} + _FORCE_INLINE_ GodotSeparationRayShape2D(real_t p_length) { length = p_length; } +}; + +class GodotSegmentShape2D : public GodotShape2D { + Vector2 a; + Vector2 b; + Vector2 n; + +public: + _FORCE_INLINE_ const Vector2 &get_a() const { return a; } + _FORCE_INLINE_ const Vector2 &get_b() const { return b; } + _FORCE_INLINE_ const Vector2 &get_normal() const { return n; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_SEGMENT; } + + _FORCE_INLINE_ Vector2 get_xformed_normal(const Transform2D &p_xform) const { + return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); + } + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + r_max = p_normal.dot(p_transform.xform(a)); + r_min = p_normal.dot(p_transform.xform(b)); + if (r_max < r_min) { + SWAP(r_max, r_min); + } + } + + DEFAULT_PROJECT_RANGE_CAST + + _FORCE_INLINE_ GodotSegmentShape2D() {} + _FORCE_INLINE_ GodotSegmentShape2D(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) { + a = p_a; + b = p_b; + n = p_n; + } +}; + +class GodotCircleShape2D : public GodotShape2D { + real_t radius; + +public: + _FORCE_INLINE_ const real_t &get_radius() const { return radius; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CIRCLE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + //real large + real_t d = p_normal.dot(p_transform.get_origin()); + + // figure out scale at point + Vector2 local_normal = p_transform.basis_xform_inv(p_normal); + real_t scale = local_normal.length(); + + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotRectangleShape2D : public GodotShape2D { + Vector2 half_extents; + +public: + _FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_RECTANGLE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + r_max = -1e20; + r_min = 1e20; + for (int i = 0; i < 4; i++) { + real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y))); + + if (d > r_max) { + r_max = d; + } + if (d < r_min) { + r_min = d; + } + } + } + + _FORCE_INLINE_ Vector2 get_circle_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const Vector2 &p_circle) const { + Vector2 local_v = p_xform_inv.xform(p_circle); + + Vector2 he( + (local_v.x < 0) ? -half_extents.x : half_extents.x, + (local_v.y < 0) ? -half_extents.y : half_extents.y); + + return (p_xform.xform(he) - p_circle).normalized(); + } + + _FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const GodotRectangleShape2D *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const { + Vector2 a, b; + + { + Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin()); + + Vector2 he( + (local_v.x < 0) ? -half_extents.x : half_extents.x, + (local_v.y < 0) ? -half_extents.y : half_extents.y); + + a = p_xform.xform(he); + } + { + Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin()); + + Vector2 he( + (local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x, + (local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y); + + b = p_B_xform.xform(he); + } + + return (a - b).normalized(); + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotCapsuleShape2D : public GodotShape2D { + real_t radius = 0.0; + real_t height = 0.0; + +public: + _FORCE_INLINE_ const real_t &get_radius() const { return radius; } + _FORCE_INLINE_ const real_t &get_height() const { return height; } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CAPSULE; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + Vector2 n = p_transform.basis_xform_inv(p_normal).normalized(); + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); + + if (r_max < r_min) { + SWAP(r_max, r_min); + } + + //ERR_FAIL_COND( r_max < r_min ); + } + + DEFAULT_PROJECT_RANGE_CAST +}; + +class GodotConvexPolygonShape2D : public GodotShape2D { + struct Point { + Vector2 pos; + Vector2 normal; //normal to next segment + }; + + Point *points = nullptr; + int point_count = 0; + +public: + _FORCE_INLINE_ int get_point_count() const { return point_count; } + _FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; } + _FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; } + _FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Transform2D &p_xform, int p_idx) const { + Vector2 a = points[p_idx].pos; + p_idx++; + Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos; + return (p_xform.xform(b) - p_xform.xform(a)).normalized().orthogonal(); + } + + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONVEX_POLYGON; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { project_range(p_normal, p_transform, r_min, r_max); } + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + _FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + if (!points || point_count <= 0) { + r_min = r_max = 0; + return; + } + + r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos)); + for (int i = 1; i < point_count; i++) { + real_t d = p_normal.dot(p_transform.xform(points[i].pos)); + if (d > r_max) { + r_max = d; + } + if (d < r_min) { + r_min = d; + } + } + } + + DEFAULT_PROJECT_RANGE_CAST + + GodotConvexPolygonShape2D() {} + ~GodotConvexPolygonShape2D(); +}; + +class GodotConcaveShape2D : public GodotShape2D { +public: + virtual bool is_concave() const override { return true; } + + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, GodotShape2D *p_convex); + + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; +}; + +class GodotConcavePolygonShape2D : public GodotConcaveShape2D { + struct Segment { + int points[2] = {}; + }; + + Vector<Segment> segments; + Vector<Point2> points; + + struct BVH { + Rect2 aabb; + int left = 0, right = 0; + }; + + Vector<BVH> bvh; + int bvh_depth = 0; + + struct BVH_CompareX { + _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { + return (a.aabb.position.x + a.aabb.size.x * 0.5) < (b.aabb.position.x + b.aabb.size.x * 0.5); + } + }; + + struct BVH_CompareY { + _FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const { + return (a.aabb.position.y + a.aabb.size.y * 0.5) < (b.aabb.position.y + b.aabb.size.y * 0.5); + } + }; + + int _generate_bvh(BVH *p_bvh, int p_len, int p_depth); + +public: + virtual PhysicsServer2D::ShapeType get_type() const override { return PhysicsServer2D::SHAPE_CONCAVE_POLYGON; } + + virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_rangev in GodotConcavePolygonShape2D"); + } + + void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { + r_min = 0; + r_max = 0; + ERR_FAIL_MSG("Unsupported call to project_range in GodotConcavePolygonShape2D"); + } + + virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override; + + virtual bool contains_point(const Vector2 &p_point) const override; + virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const override; + + virtual real_t get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const override { return 0; } + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + + DEFAULT_PROJECT_RANGE_CAST +}; + +#undef DEFAULT_PROJECT_RANGE_CAST + +#endif // GODOT_SHAPE_2D_H diff --git a/modules/godot_physics_2d/godot_space_2d.cpp b/modules/godot_physics_2d/godot_space_2d.cpp new file mode 100644 index 0000000000..2966818beb --- /dev/null +++ b/modules/godot_physics_2d/godot_space_2d.cpp @@ -0,0 +1,1240 @@ +/**************************************************************************/ +/* godot_space_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_space_2d.h" + +#include "godot_collision_solver_2d.h" +#include "godot_physics_server_2d.h" + +#include "core/os/os.h" +#include "core/templates/pair.h" + +#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001 +#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 + +_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (!(p_object->get_collision_layer() & p_collision_mask)) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject2D::TYPE_AREA && !p_collide_with_areas) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject2D::TYPE_BODY && !p_collide_with_bodies) { + return false; + } + + return true; +} + +int GodotPhysicsDirectSpaceState2D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) { + if (p_result_max <= 0) { + return 0; + } + + Rect2 aabb; + aabb.position = p_parameters.position - Vector2(0.00001, 0.00001); + aabb.size = Vector2(0.00002, 0.00002); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + int cc = 0; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_parameters.pick_point && !col_obj->is_pickable()) { + continue; + } + + if (col_obj->get_canvas_instance_id() != p_parameters.canvas_instance_id) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + GodotShape2D *shape = col_obj->get_shape(shape_idx); + + Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_parameters.position); + + if (!shape->contains_point(local_point)) { + continue; + } + + if (cc >= p_result_max) { + continue; + } + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +bool GodotPhysicsDirectSpaceState2D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) { + ERR_FAIL_COND_V(space->locked, false); + + Vector2 begin, end; + Vector2 normal; + begin = p_parameters.from; + end = p_parameters.to; + normal = (end - begin).normalized(); + + int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision + + bool collided = false; + Vector2 res_point, res_normal; + int res_shape = -1; + const GodotCollisionObject2D *res_obj = nullptr; + real_t min_d = 1e10; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + int shape_idx = space->intersection_query_subindex_results[i]; + Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + + Vector2 local_from = inv_xform.xform(begin); + Vector2 local_to = inv_xform.xform(end); + + const GodotShape2D *shape = col_obj->get_shape(shape_idx); + + Vector2 shape_point, shape_normal; + + if (shape->contains_point(local_from)) { + if (p_parameters.hit_from_inside) { + // Hit shape at starting point. + min_d = 0; + res_point = begin; + res_normal = Vector2(); + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + break; + } else { + // Ignore shape when starting inside. + continue; + } + } + + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) { + Transform2D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + shape_point = xform.xform(shape_point); + + real_t ld = normal.dot(shape_point); + + if (ld < min_d) { + min_d = ld; + res_point = shape_point; + res_normal = inv_xform.basis_xform_inv(shape_normal).normalized(); + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + } + } + } + + if (!collided) { + return false; + } + ERR_FAIL_NULL_V(res_obj, false); // Shouldn't happen but silences warning. + + r_result.collider_id = res_obj->get_instance_id(); + if (r_result.collider_id.is_valid()) { + r_result.collider = ObjectDB::get_instance(r_result.collider_id); + } + r_result.normal = res_normal; + r_result.position = res_point; + r_result.rid = res_obj->get_self(); + r_result.shape = res_shape; + + return true; +} + +int GodotPhysicsDirectSpaceState2D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) { + if (p_result_max <= 0) { + return 0; + } + + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, 0); + + Rect2 aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion + aabb = aabb.grow(p_parameters.margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + int cc = 0; + + for (int i = 0; i < amount; i++) { + if (cc >= p_result_max) { + break; + } + + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) { + continue; + } + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +bool GodotPhysicsDirectSpaceState2D::cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) { + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, false); + + Rect2 aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion + aabb = aabb.grow(p_parameters.margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { + continue; //ignore excluded + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + //test initial overlap, does it collide if going all the way? + if (!GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) { + continue; + } + + //test initial overlap, ignore objects it's inside of. + if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_parameters.margin)) { + continue; + } + + Vector2 mnormal = p_parameters.motion.normalized(); + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int j = 0; j < 8; j++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + Vector2 sep = mnormal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_parameters.margin); + + if (collided) { + hi = fraction; + if ((j == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + low = fraction; + if ((j == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + p_closest_safe = best_safe; + p_closest_unsafe = best_unsafe; + + return true; +} + +bool GodotPhysicsDirectSpaceState2D::collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) { + if (p_result_max <= 0) { + return false; + } + + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, 0); + + Rect2 aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion + aabb = aabb.grow(p_parameters.margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + bool collided = false; + r_result_count = 0; + + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = r_results; + GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk; + + GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_parameters.exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; + + if (GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) { + collided = cbk.amount > 0; + } + } + + r_result_count = cbk.amount; + + return collided; +} + +struct _RestCallbackData2D { + const GodotCollisionObject2D *object = nullptr; + const GodotCollisionObject2D *best_object = nullptr; + int local_shape = 0; + int best_local_shape = 0; + int shape = 0; + int best_shape = 0; + Vector2 best_contact; + Vector2 best_normal; + real_t best_len = 0.0; + Vector2 valid_dir; + real_t valid_depth = 0.0; + real_t min_allowed_depth = 0.0; +}; + +static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) { + _RestCallbackData2D *rd = static_cast<_RestCallbackData2D *>(p_userdata); + + Vector2 contact_rel = p_point_B - p_point_A; + real_t len = contact_rel.length(); + + if (len < rd->min_allowed_depth) { + return; + } + + if (len <= rd->best_len) { + return; + } + + Vector2 normal = contact_rel / len; + + if (rd->valid_dir != Vector2()) { + if (len > rd->valid_depth) { + return; + } + + if (rd->valid_dir.dot(normal) > -CMP_EPSILON) { + return; + } + } + + rd->best_len = len; + rd->best_contact = p_point_B; + rd->best_normal = normal; + rd->best_object = rd->object; + rd->best_shape = rd->shape; + rd->best_local_shape = rd->local_shape; +} + +bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) { + GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, 0); + + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); + + Rect2 aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion + aabb = aabb.grow(margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + _RestCallbackData2D rcd; + + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + real_t motion_length = p_parameters.motion.length(); + real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + const GodotCollisionObject2D *col_obj = space->intersection_query_results[i]; + + if (p_parameters.exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + rcd.valid_dir = Vector2(); + rcd.object = col_obj; + rcd.shape = shape_idx; + rcd.local_shape = 0; + bool sc = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, margin); + if (!sc) { + continue; + } + } + + if (rcd.best_len == 0 || !rcd.best_object) { + return false; + } + + r_info->collider_id = rcd.best_object->get_instance_id(); + r_info->shape = rcd.best_shape; + r_info->normal = rcd.best_normal; + r_info->point = rcd.best_contact; + r_info->rid = rcd.best_object->get_self(); + if (rcd.best_object->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *body = static_cast<const GodotBody2D *>(rcd.best_object); + Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass()); + r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + + } else { + r_info->linear_velocity = Vector2(); + } + + return true; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////// + +int GodotSpace2D::_cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb) { + int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); + + for (int i = 0; i < amount; i++) { + bool keep = true; + + if (intersection_query_results[i] == p_body) { + keep = false; + } else if (intersection_query_results[i]->get_type() == GodotCollisionObject2D::TYPE_AREA) { + keep = false; + } else if (!p_body->collides_with(static_cast<GodotBody2D *>(intersection_query_results[i]))) { + keep = false; + } else if (static_cast<GodotBody2D *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { + keep = false; + } + + if (!keep) { + if (i < amount - 1) { + SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); + SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); + } + + amount--; + i--; + } + } + + return amount; +} + +bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) { + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + if (r_result) { + r_result->collider_id = ObjectID(); + r_result->collider_shape = 0; + } + + Rect2 body_aabb; + + bool shapes_found = false; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + if (p_body->is_shape_disabled(i)) { + continue; + } + + if (!shapes_found) { + body_aabb = p_body->get_shape_aabb(i); + shapes_found = true; + } else { + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + } + + if (!shapes_found) { + if (r_result) { + *r_result = PhysicsServer2D::MotionResult(); + r_result->travel = p_parameters.motion; + } + return false; + } + + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); + + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(margin); + + static const int max_excluded_shape_pairs = 32; + ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs]; + int excluded_shape_pair_count = 0; + + real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + + real_t motion_length = p_parameters.motion.length(); + Vector2 motion_normal = p_parameters.motion / motion_length; + + Transform2D body_transform = p_parameters.from; + + bool recovered = false; + + { + //STEP 1, FREE BODY IF STUCK + + const int max_results = 32; + int recover_attempts = 4; + Vector2 sr[max_results * 2]; + real_t priorities[max_results]; + + do { + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = sr; + cbk.invalid_by_dir = 0; + excluded_shape_pair_count = 0; //last step is the one valid + + GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk; + GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk; + int priority_amount = 0; + + bool collided = false; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + GodotShape2D *body_shape = p_body->get_shape(j); + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + cbk.valid_dir = col_obj_shape_xform.columns[1].normalized(); + + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work + cbk.invalid_by_dir = 0; + + if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * last_step; + real_t motion_len = motion.length(); + motion.normalize(); + cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0); + } + } + } else { + cbk.valid_dir = Vector2(); + cbk.valid_depth = 0; + cbk.invalid_by_dir = 0; + } + + int current_passed = cbk.passed; //save how many points passed collision + bool did_collide = false; + + GodotShape2D *against_shape = col_obj->get_shape(shape_idx); + if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) { + did_collide = cbk.passed > current_passed; //more passed, so collision actually existed + } + while (cbk.amount > priority_amount) { + priorities[priority_amount] = col_obj->get_collision_priority(); + priority_amount++; + } + + if (!did_collide && cbk.invalid_by_dir > 0) { + //this shape must be excluded + if (excluded_shape_pair_count < max_excluded_shape_pairs) { + ExcludedShapeSW esp; + esp.local_shape = body_shape; + esp.against_object = col_obj; + esp.against_shape_index = shape_idx; + excluded_shape_pairs[excluded_shape_pair_count++] = esp; + } + } + + if (did_collide) { + collided = true; + } + } + } + + if (!collided) { + break; + } + + real_t inv_total_weight = 0.0; + for (int i = 0; i < cbk.amount; i++) { + inv_total_weight += priorities[i]; + } + inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight; + + recovered = true; + + Vector2 recover_motion; + for (int i = 0; i < cbk.amount; i++) { + Vector2 a = sr[i * 2 + 0]; + Vector2 b = sr[i * 2 + 1]; + + // Compute plane on b towards a. + Vector2 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > min_contact_depth + CMP_EPSILON) { + // Only recover if there is penetration. + recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight; + } + } + + if (recover_motion == Vector2()) { + collided = false; + break; + } + + body_transform.columns[2] += recover_motion; + body_aabb.position += recover_motion; + + recover_attempts--; + + } while (recover_attempts); + } + + real_t safe = 1.0; + real_t unsafe = 1.0; + int best_shape = -1; + + { + // STEP 2 ATTEMPT MOTION + + Rect2 motion_aabb = body_aabb; + motion_aabb.position += p_parameters.motion; + motion_aabb = motion_aabb.merge(body_aabb); + + int amount = _cull_aabb_for_body(p_body, motion_aabb); + + for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) { + if (p_body->is_shape_disabled(body_shape_idx)) { + continue; + } + + GodotShape2D *body_shape = p_body->get_shape(body_shape_idx); + + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast<GodotSeparationRayShape2D *>(body_shape)->get_slide_on_slope()) { + continue; + } + } + + Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(body_shape_idx); + + bool stuck = false; + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int col_shape_idx = intersection_query_subindex_results[i]; + GodotShape2D *against_shape = col_obj->get_shape(col_shape_idx); + + bool excluded = false; + + for (int k = 0; k < excluded_shape_pair_count; k++) { + if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == col_shape_idx) { + excluded = true; + break; + } + } + + if (excluded) { + continue; + } + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx); + //test initial overlap, does it collide if going all the way? + if (!GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { + continue; + } + + //test initial overlap + if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + Vector2 direction = col_obj_shape_xform.columns[1].normalized(); + if (motion_normal.dot(direction) < 0) { + continue; + } + } + + stuck = true; + break; + } + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int k = 0; k < 8; k++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + Vector2 sep = motion_normal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); + + if (collided) { + hi = fraction; + if ((k == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + low = fraction; + if ((k == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { + Vector2 cd[2]; + GodotPhysicsServer2D::CollCbkData cbk; + cbk.max = 1; + cbk.amount = 0; + cbk.passed = 0; + cbk.ptr = cd; + cbk.valid_dir = col_obj_shape_xform.columns[1].normalized(); + + cbk.valid_depth = 10e20; + + Vector2 sep = motion_normal; //important optimization for this to work fast enough + bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), GodotPhysicsServer2D::_shape_col_cbk, &cbk, &sep, 0); + if (!collided || cbk.amount == 0) { + continue; + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + if (stuck) { + safe = 0; + unsafe = 0; + best_shape = body_shape_idx; //sadly it's the best + break; + } + if (best_safe == 1.0) { + continue; + } + if (best_safe < safe) { + safe = best_safe; + unsafe = best_unsafe; + best_shape = body_shape_idx; + } + } + } + + bool collided = false; + + if ((p_parameters.recovery_as_collision && recovered) || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 + } + + //it collided, let's get the rest info in unsafe advance + Transform2D ugt = body_transform; + ugt.columns[2] += p_parameters.motion * unsafe; + + _RestCallbackData2D rcd; + + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); + + body_aabb.position += p_parameters.motion * unsafe; + int amount = _cull_aabb_for_body(p_body, body_aabb); + + int from_shape = best_shape != -1 ? best_shape : 0; + int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); + + for (int j = from_shape; j < to_shape; j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); + GodotShape2D *body_shape = p_body->get_shape(j); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject2D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + GodotShape2D *against_shape = col_obj->get_shape(shape_idx); + + bool excluded = false; + for (int k = 0; k < excluded_shape_pair_count; k++) { + if (excluded_shape_pairs[k].local_shape == body_shape && excluded_shape_pairs[k].against_object == col_obj && excluded_shape_pairs[k].against_shape_index == shape_idx) { + excluded = true; + break; + } + } + if (excluded) { + continue; + } + + Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + + if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) { + rcd.valid_dir = col_obj_shape_xform.columns[1].normalized(); + + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + rcd.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work + + if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) { + const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * last_step; + real_t motion_len = motion.length(); + motion.normalize(); + rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); + } + } + } else { + rcd.valid_dir = Vector2(); + rcd.valid_depth = 0; + } + + rcd.object = col_obj; + rcd.shape = shape_idx; + rcd.local_shape = j; + bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, margin); + if (!sc) { + continue; + } + } + } + + if (rcd.best_len != 0) { + if (r_result) { + r_result->collider = rcd.best_object->get_self(); + r_result->collider_id = rcd.best_object->get_instance_id(); + r_result->collider_shape = rcd.best_shape; + r_result->collision_local_shape = rcd.best_local_shape; + r_result->collision_normal = rcd.best_normal; + r_result->collision_point = rcd.best_contact; + r_result->collision_depth = rcd.best_len; + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; + + const GodotBody2D *body = static_cast<const GodotBody2D *>(rcd.best_object); + Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass()); + r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); + + r_result->travel = safe * p_parameters.motion; + r_result->remainder = p_parameters.motion - safe * p_parameters.motion; + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + } + + collided = true; + } + } + + if (!collided && r_result) { + r_result->travel = p_parameters.motion; + r_result->remainder = Vector2(); + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + } + + return collided; +} + +// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree. +void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self) { + GodotCollisionObject2D::Type type_A = A->get_type(); + GodotCollisionObject2D::Type type_B = B->get_type(); + if (type_A > type_B) { + SWAP(A, B); + SWAP(p_subindex_A, p_subindex_B); + SWAP(type_A, type_B); + } + + GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self); + self->collision_pairs++; + + if (type_A == GodotCollisionObject2D::TYPE_AREA) { + GodotArea2D *area = static_cast<GodotArea2D *>(A); + if (type_B == GodotCollisionObject2D::TYPE_AREA) { + GodotArea2D *area_b = static_cast<GodotArea2D *>(B); + GodotArea2Pair2D *area2_pair = memnew(GodotArea2Pair2D(area_b, p_subindex_B, area, p_subindex_A)); + return area2_pair; + } else { + GodotBody2D *body = static_cast<GodotBody2D *>(B); + GodotAreaPair2D *area_pair = memnew(GodotAreaPair2D(body, p_subindex_B, area, p_subindex_A)); + return area_pair; + } + + } else { + GodotBodyPair2D *b = memnew(GodotBodyPair2D(static_cast<GodotBody2D *>(A), p_subindex_A, static_cast<GodotBody2D *>(B), p_subindex_B)); + return b; + } +} + +void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self) { + if (!p_data) { + return; + } + + GodotSpace2D *self = static_cast<GodotSpace2D *>(p_self); + self->collision_pairs--; + GodotConstraint2D *c = static_cast<GodotConstraint2D *>(p_data); + memdelete(c); +} + +const SelfList<GodotBody2D>::List &GodotSpace2D::get_active_body_list() const { + return active_list; +} + +void GodotSpace2D::body_add_to_active_list(SelfList<GodotBody2D> *p_body) { + active_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_active_list(SelfList<GodotBody2D> *p_body) { + active_list.remove(p_body); +} + +void GodotSpace2D::body_add_to_mass_properties_update_list(SelfList<GodotBody2D> *p_body) { + mass_properties_update_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_mass_properties_update_list(SelfList<GodotBody2D> *p_body) { + mass_properties_update_list.remove(p_body); +} + +GodotBroadPhase2D *GodotSpace2D::get_broadphase() { + return broadphase; +} + +void GodotSpace2D::add_object(GodotCollisionObject2D *p_object) { + ERR_FAIL_COND(objects.has(p_object)); + objects.insert(p_object); +} + +void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) { + ERR_FAIL_COND(!objects.has(p_object)); + objects.erase(p_object); +} + +const HashSet<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const { + return objects; +} + +void GodotSpace2D::body_add_to_state_query_list(SelfList<GodotBody2D> *p_body) { + state_query_list.add(p_body); +} + +void GodotSpace2D::body_remove_from_state_query_list(SelfList<GodotBody2D> *p_body) { + state_query_list.remove(p_body); +} + +void GodotSpace2D::area_add_to_monitor_query_list(SelfList<GodotArea2D> *p_area) { + monitor_query_list.add(p_area); +} + +void GodotSpace2D::area_remove_from_monitor_query_list(SelfList<GodotArea2D> *p_area) { + monitor_query_list.remove(p_area); +} + +void GodotSpace2D::area_add_to_moved_list(SelfList<GodotArea2D> *p_area) { + area_moved_list.add(p_area); +} + +void GodotSpace2D::area_remove_from_moved_list(SelfList<GodotArea2D> *p_area) { + area_moved_list.remove(p_area); +} + +const SelfList<GodotArea2D>::List &GodotSpace2D::get_moved_area_list() const { + return area_moved_list; +} + +void GodotSpace2D::call_queries() { + while (state_query_list.first()) { + GodotBody2D *b = state_query_list.first()->self(); + state_query_list.remove(state_query_list.first()); + b->call_queries(); + } + + while (monitor_query_list.first()) { + GodotArea2D *a = monitor_query_list.first()->self(); + monitor_query_list.remove(monitor_query_list.first()); + a->call_queries(); + } +} + +void GodotSpace2D::setup() { + contact_debug_count = 0; + + while (mass_properties_update_list.first()) { + mass_properties_update_list.first()->self()->update_mass_properties(); + mass_properties_update_list.remove(mass_properties_update_list.first()); + } +} + +void GodotSpace2D::update() { + broadphase->update(); +} + +void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + contact_recycle_radius = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + contact_max_separation = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: + contact_max_allowed_penetration = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + contact_bias = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + body_linear_velocity_sleep_threshold = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + body_angular_velocity_sleep_threshold = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + body_time_to_sleep = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + constraint_bias = p_value; + break; + case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS: + solver_iterations = p_value; + break; + } +} + +real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const { + switch (p_param) { + case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + return contact_recycle_radius; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + return contact_max_separation; + case PhysicsServer2D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: + return contact_max_allowed_penetration; + case PhysicsServer2D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + return contact_bias; + case PhysicsServer2D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + return body_linear_velocity_sleep_threshold; + case PhysicsServer2D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + return body_angular_velocity_sleep_threshold; + case PhysicsServer2D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + return body_time_to_sleep; + case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: + return constraint_bias; + case PhysicsServer2D::SPACE_PARAM_SOLVER_ITERATIONS: + return solver_iterations; + } + return 0; +} + +void GodotSpace2D::lock() { + locked = true; +} + +void GodotSpace2D::unlock() { + locked = false; +} + +bool GodotSpace2D::is_locked() const { + return locked; +} + +GodotPhysicsDirectSpaceState2D *GodotSpace2D::get_direct_state() { + return direct_access; +} + +GodotSpace2D::GodotSpace2D() { + body_linear_velocity_sleep_threshold = GLOBAL_GET("physics/2d/sleep_threshold_linear"); + body_angular_velocity_sleep_threshold = GLOBAL_GET("physics/2d/sleep_threshold_angular"); + body_time_to_sleep = GLOBAL_GET("physics/2d/time_before_sleep"); + solver_iterations = GLOBAL_GET("physics/2d/solver/solver_iterations"); + contact_recycle_radius = GLOBAL_GET("physics/2d/solver/contact_recycle_radius"); + contact_max_separation = GLOBAL_GET("physics/2d/solver/contact_max_separation"); + contact_max_allowed_penetration = GLOBAL_GET("physics/2d/solver/contact_max_allowed_penetration"); + contact_bias = GLOBAL_GET("physics/2d/solver/default_contact_bias"); + constraint_bias = GLOBAL_GET("physics/2d/solver/default_constraint_bias"); + + broadphase = GodotBroadPhase2D::create_func(); + broadphase->set_pair_callback(_broadphase_pair, this); + broadphase->set_unpair_callback(_broadphase_unpair, this); + + direct_access = memnew(GodotPhysicsDirectSpaceState2D); + direct_access->space = this; +} + +GodotSpace2D::~GodotSpace2D() { + memdelete(broadphase); + memdelete(direct_access); +} diff --git a/modules/godot_physics_2d/godot_space_2d.h b/modules/godot_physics_2d/godot_space_2d.h new file mode 100644 index 0000000000..ded3b08d5b --- /dev/null +++ b/modules/godot_physics_2d/godot_space_2d.h @@ -0,0 +1,214 @@ +/**************************************************************************/ +/* godot_space_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_SPACE_2D_H +#define GODOT_SPACE_2D_H + +#include "godot_area_2d.h" +#include "godot_area_pair_2d.h" +#include "godot_body_2d.h" +#include "godot_body_pair_2d.h" +#include "godot_broad_phase_2d.h" +#include "godot_collision_object_2d.h" + +#include "core/config/project_settings.h" +#include "core/templates/hash_map.h" +#include "core/typedefs.h" + +class GodotPhysicsDirectSpaceState2D : public PhysicsDirectSpaceState2D { + GDCLASS(GodotPhysicsDirectSpaceState2D, PhysicsDirectSpaceState2D); + +public: + GodotSpace2D *space = nullptr; + + virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override; + virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe) override; + virtual bool collide_shape(const ShapeParameters &p_parameters, Vector2 *r_results, int p_result_max, int &r_result_count) override; + virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override; + + GodotPhysicsDirectSpaceState2D() {} +}; + +class GodotSpace2D { +public: + enum ElapsedTime { + ELAPSED_TIME_INTEGRATE_FORCES, + ELAPSED_TIME_GENERATE_ISLANDS, + ELAPSED_TIME_SETUP_CONSTRAINTS, + ELAPSED_TIME_SOLVE_CONSTRAINTS, + ELAPSED_TIME_INTEGRATE_VELOCITIES, + ELAPSED_TIME_MAX + + }; + +private: + struct ExcludedShapeSW { + GodotShape2D *local_shape = nullptr; + const GodotCollisionObject2D *against_object = nullptr; + int against_shape_index = 0; + }; + + uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; + + GodotPhysicsDirectSpaceState2D *direct_access = nullptr; + RID self; + + GodotBroadPhase2D *broadphase = nullptr; + SelfList<GodotBody2D>::List active_list; + SelfList<GodotBody2D>::List mass_properties_update_list; + SelfList<GodotBody2D>::List state_query_list; + SelfList<GodotArea2D>::List monitor_query_list; + SelfList<GodotArea2D>::List area_moved_list; + + static void *_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self); + static void _broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self); + + HashSet<GodotCollisionObject2D *> objects; + + GodotArea2D *area = nullptr; + + int solver_iterations = 0; + + real_t contact_recycle_radius = 0.0; + real_t contact_max_separation = 0.0; + real_t contact_max_allowed_penetration = 0.0; + real_t contact_bias = 0.0; + real_t constraint_bias = 0.0; + + enum { + INTERSECTION_QUERY_MAX = 2048 + }; + + GodotCollisionObject2D *intersection_query_results[INTERSECTION_QUERY_MAX]; + int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; + + real_t body_linear_velocity_sleep_threshold = 0.0; + real_t body_angular_velocity_sleep_threshold = 0.0; + real_t body_time_to_sleep = 0.0; + + bool locked = false; + + real_t last_step = 0.001; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + int _cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb); + + Vector<Vector2> contact_debug; + int contact_debug_count = 0; + + friend class GodotPhysicsDirectSpaceState2D; + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + void set_default_area(GodotArea2D *p_area) { area = p_area; } + GodotArea2D *get_default_area() const { return area; } + + const SelfList<GodotBody2D>::List &get_active_body_list() const; + void body_add_to_active_list(SelfList<GodotBody2D> *p_body); + void body_remove_from_active_list(SelfList<GodotBody2D> *p_body); + void body_add_to_mass_properties_update_list(SelfList<GodotBody2D> *p_body); + void body_remove_from_mass_properties_update_list(SelfList<GodotBody2D> *p_body); + void area_add_to_moved_list(SelfList<GodotArea2D> *p_area); + void area_remove_from_moved_list(SelfList<GodotArea2D> *p_area); + const SelfList<GodotArea2D>::List &get_moved_area_list() const; + + void body_add_to_state_query_list(SelfList<GodotBody2D> *p_body); + void body_remove_from_state_query_list(SelfList<GodotBody2D> *p_body); + + void area_add_to_monitor_query_list(SelfList<GodotArea2D> *p_area); + void area_remove_from_monitor_query_list(SelfList<GodotArea2D> *p_area); + + GodotBroadPhase2D *get_broadphase(); + + void add_object(GodotCollisionObject2D *p_object); + void remove_object(GodotCollisionObject2D *p_object); + const HashSet<GodotCollisionObject2D *> &get_objects() const; + + _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; } + _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } + _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } + _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_contact_bias() const { return contact_bias; } + _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; } + _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } + + void update(); + void setup(); + void call_queries(); + + bool is_locked() const; + void lock(); + void unlock(); + + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + + void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value); + real_t get_param(PhysicsServer2D::SpaceParameter p_param) const; + + void set_island_count(int p_island_count) { island_count = p_island_count; } + int get_island_count() const { return island_count; } + + void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } + int get_active_objects() const { return active_objects; } + + int get_collision_pairs() const { return collision_pairs; } + + bool test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result); + + void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } + _FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) { + if (contact_debug_count < contact_debug.size()) { + contact_debug.write[contact_debug_count++] = p_contact; + } + } + _FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; } + _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } + + GodotPhysicsDirectSpaceState2D *get_direct_state(); + + void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } + uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } + + GodotSpace2D(); + ~GodotSpace2D(); +}; + +#endif // GODOT_SPACE_2D_H diff --git a/modules/godot_physics_2d/godot_step_2d.cpp b/modules/godot_physics_2d/godot_step_2d.cpp new file mode 100644 index 0000000000..418b9313e8 --- /dev/null +++ b/modules/godot_physics_2d/godot_step_2d.cpp @@ -0,0 +1,306 @@ +/**************************************************************************/ +/* godot_step_2d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_step_2d.h" + +#include "core/object/worker_thread_pool.h" +#include "core/os/os.h" + +#define BODY_ISLAND_COUNT_RESERVE 128 +#define BODY_ISLAND_SIZE_RESERVE 512 +#define ISLAND_COUNT_RESERVE 128 +#define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 + +void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island) { + p_body->set_island_step(_step); + + if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { + // Only rigid bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (const Pair<GodotConstraint2D *, int> &E : p_body->get_constraint_list()) { + GodotConstraint2D *constraint = const_cast<GodotConstraint2D *>(E.first); + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + all_constraints.push_back(constraint); + + for (int i = 0; i < constraint->get_body_count(); i++) { + if (i == E.second) { + continue; + } + GodotBody2D *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer2D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(other_body, p_body_island, p_constraint_island); + } + } +} + +void GodotStep2D::_setup_constraint(uint32_t p_constraint_index, void *p_userdata) { + GodotConstraint2D *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void GodotStep2D::_pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + uint32_t valid_constraint_count = 0; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + GodotConstraint2D *constraint = p_constraint_island[constraint_index]; + if (p_constraint_island[constraint_index]->pre_solve(delta)) { + // Keep this constraint for solving. + p_constraint_island[valid_constraint_count++] = constraint; + } + } + p_constraint_island.resize(valid_constraint_count); +} + +void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const { + const LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[p_island_index]; + + for (int i = 0; i < iterations; i++) { + uint32_t constraint_count = constraint_island.size(); + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + constraint_island[constraint_index]->solve(delta); + } + } +} + +void GodotStep2D::_check_suspend(LocalVector<GodotBody2D *> &p_body_island) const { + bool can_sleep = true; + + uint32_t body_count = p_body_island.size(); + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody2D *body = p_body_island[body_index]; + + if (!body->sleep_test(delta)) { + can_sleep = false; + } + } + + // Put all to sleep or wake up everyone. + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody2D *body = p_body_island[body_index]; + + bool active = body->is_active(); + + if (active == can_sleep) { + body->set_active(!can_sleep); + } + } +} + +void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) { + p_space->lock(); // can't access space during this + + p_space->setup(); //update inertias, etc + + p_space->set_last_step(p_delta); + + iterations = p_space->get_solver_iterations(); + delta = p_delta; + + const SelfList<GodotBody2D>::List *body_list = &p_space->get_active_body_list(); + + /* INTEGRATE FORCES */ + + uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); + uint64_t profile_endtime = 0; + + int active_count = 0; + + const SelfList<GodotBody2D> *b = body_list->first(); + while (b) { + b->self()->integrate_forces(p_delta); + b = b->next(); + active_count++; + } + + p_space->set_active_objects(active_count); + + // Update the broadphase to register collision pairs. + p_space->update(); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList<GodotArea2D>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (GodotConstraint2D *E : aml.first()->self()->get_constraints()) { + GodotConstraint2D *constraint = E; + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList<GodotArea2D> *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ + + b = body_list->first(); + + uint32_t body_island_count = 0; + + while (b) { + GodotBody2D *body = b->self(); + + if (body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector<GodotBody2D *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island(body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } + } + b = b->next(); + } + + p_space->set_island_count((int)island_count); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ + + uint32_t total_constraint_count = all_constraints.size(); + WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_constraint, nullptr, total_constraint_count, -1, true, SNAME("Physics2DConstraintSetup")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* PRE-SOLVE CONSTRAINT ISLANDS */ + + // WARNING: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // WARNING: `_solve_island` modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics2DConstraintSolveIslands")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* INTEGRATE VELOCITIES */ + + b = body_list->first(); + while (b) { + const SelfList<GodotBody2D> *n = b->next(); + b->self()->integrate_velocities(p_delta); + b = n; // in case it shuts itself down + } + + /* SLEEP / WAKE UP ISLANDS */ + + for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { + _check_suspend(body_islands[island_index]); + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); + //profile_begtime=profile_endtime; + } + + all_constraints.clear(); + + p_space->unlock(); + _step++; +} + +GodotStep2D::GodotStep2D() { + body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); + constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); +} + +GodotStep2D::~GodotStep2D() { +} diff --git a/modules/godot_physics_2d/godot_step_2d.h b/modules/godot_physics_2d/godot_step_2d.h new file mode 100644 index 0000000000..c08c6379de --- /dev/null +++ b/modules/godot_physics_2d/godot_step_2d.h @@ -0,0 +1,60 @@ +/**************************************************************************/ +/* godot_step_2d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_STEP_2D_H +#define GODOT_STEP_2D_H + +#include "godot_space_2d.h" + +#include "core/templates/local_vector.h" + +class GodotStep2D { + uint64_t _step = 1; + + int iterations = 0; + real_t delta = 0.0; + + LocalVector<LocalVector<GodotBody2D *>> body_islands; + LocalVector<LocalVector<GodotConstraint2D *>> constraint_islands; + LocalVector<GodotConstraint2D *> all_constraints; + + void _populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island); + void _setup_constraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; + void _check_suspend(LocalVector<GodotBody2D *> &p_body_island) const; + +public: + void step(GodotSpace2D *p_space, real_t p_delta); + GodotStep2D(); + ~GodotStep2D(); +}; + +#endif // GODOT_STEP_2D_H diff --git a/modules/godot_physics_2d/register_types.cpp b/modules/godot_physics_2d/register_types.cpp new file mode 100644 index 0000000000..57422b1814 --- /dev/null +++ b/modules/godot_physics_2d/register_types.cpp @@ -0,0 +1,61 @@ +/**************************************************************************/ +/* register_types.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "register_types.h" + +#include "godot_physics_server_2d.h" +#include "servers/physics_server_2d.h" +#include "servers/physics_server_2d_wrap_mt.h" + +static PhysicsServer2D *_createGodotPhysics2DCallback() { +#ifdef THREADS_ENABLED + bool using_threads = GLOBAL_GET("physics/2d/run_on_separate_thread"); +#else + bool using_threads = false; +#endif + + PhysicsServer2D *physics_server_2d = memnew(GodotPhysicsServer2D(using_threads)); + + return memnew(PhysicsServer2DWrapMT(physics_server_2d, using_threads)); +} + +void initialize_godot_physics_2d_module(ModuleInitializationLevel p_level) { + if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) { + return; + } + PhysicsServer2DManager::get_singleton()->register_server("GodotPhysics2D", callable_mp_static(_createGodotPhysics2DCallback)); + PhysicsServer2DManager::get_singleton()->set_default_server("GodotPhysics2D"); +} + +void uninitialize_godot_physics_2d_module(ModuleInitializationLevel p_level) { + if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) { + return; + } +} diff --git a/modules/godot_physics_2d/register_types.h b/modules/godot_physics_2d/register_types.h new file mode 100644 index 0000000000..1d2d1301b9 --- /dev/null +++ b/modules/godot_physics_2d/register_types.h @@ -0,0 +1,39 @@ +/**************************************************************************/ +/* register_types.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_PHYSICS_2D_REGISTER_TYPES_H +#define GODOT_PHYSICS_2D_REGISTER_TYPES_H + +#include "modules/register_module_types.h" + +void initialize_godot_physics_2d_module(ModuleInitializationLevel p_level); +void uninitialize_godot_physics_2d_module(ModuleInitializationLevel p_level); + +#endif // GODOT_PHYSICS_2D_REGISTER_TYPES_H diff --git a/modules/godot_physics_3d/SCsub b/modules/godot_physics_3d/SCsub new file mode 100644 index 0000000000..1502eb39ee --- /dev/null +++ b/modules/godot_physics_3d/SCsub @@ -0,0 +1,8 @@ +#!/usr/bin/env python +from misc.utility.scons_hints import * + +Import("env") + +env.add_source_files(env.modules_sources, "*.cpp") + +SConscript("joints/SCsub") diff --git a/modules/godot_physics_3d/config.py b/modules/godot_physics_3d/config.py new file mode 100644 index 0000000000..a42f27fbe1 --- /dev/null +++ b/modules/godot_physics_3d/config.py @@ -0,0 +1,6 @@ +def can_build(env, platform): + return not env["disable_3d"] + + +def configure(env): + pass diff --git a/modules/godot_physics_3d/gjk_epa.cpp b/modules/godot_physics_3d/gjk_epa.cpp new file mode 100644 index 0000000000..e5678914fe --- /dev/null +++ b/modules/godot_physics_3d/gjk_epa.cpp @@ -0,0 +1,1025 @@ +/**************************************************************************/ +/* gjk_epa.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "gjk_epa.h" + +/* Disabling formatting for thirdparty code snippet */ +/* clang-format off */ + +/*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ + + // Config + +/* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURACY ((real_t)0.0001) +#define GJK_MIN_DISTANCE ((real_t)0.0001) +#define GJK_DUPLICATED_EPS ((real_t)0.0001) +#define GJK_SIMPLEX2_EPS ((real_t)0.0) +#define GJK_SIMPLEX3_EPS ((real_t)0.0) +#define GJK_SIMPLEX4_EPS ((real_t)0.0) + +/* EPA */ +#define EPA_MAX_VERTICES 128 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +// -- GODOT start -- +//#define EPA_ACCURACY ((real_t)0.0001) +#define EPA_ACCURACY ((real_t)0.00001) +// -- GODOT end -- +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((real_t)0.00001) +#define EPA_INSIDE_EPS ((real_t)0.01) + +namespace GjkEpa2 { + + +struct sResults { + enum eStatus { + Separated, /* Shapes doesn't penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + + Vector3 witnesses[2]; + Vector3 normal; + real_t distance = 0.0; +}; + +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +struct MinkowskiDiff { + const GodotShape3D* m_shapes[2]; + + Transform3D transform_A; + Transform3D transform_B; + + real_t margin_A = 0.0; + real_t margin_B = 0.0; + + Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t) = nullptr; + + void Initialize(const GodotShape3D* shape0, const Transform3D& wtrs0, const real_t margin0, + const GodotShape3D* shape1, const Transform3D& wtrs1, const real_t margin1) { + m_shapes[0] = shape0; + m_shapes[1] = shape1; + transform_A = wtrs0; + transform_B = wtrs1; + margin_A = margin0; + margin_B = margin1; + + if ((margin0 > 0.0) || (margin1 > 0.0)) { + get_support = get_support_with_margin; + } else { + get_support = get_support_without_margin; + } + } + + static Vector3 get_support_without_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) { + return p_shape->get_support(p_dir.normalized()); + } + + static Vector3 get_support_with_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) { + Vector3 local_dir_norm = p_dir; + if (local_dir_norm.length_squared() < CMP_EPSILON2) { + local_dir_norm = Vector3(-1.0, -1.0, -1.0); + } + local_dir_norm.normalize(); + + return p_shape->get_support(local_dir_norm) + p_margin * local_dir_norm; + } + + // i wonder how this could be sped up... if it can + _FORCE_INLINE_ Vector3 Support0(const Vector3& d) const { + return transform_A.xform(get_support(m_shapes[0], transform_A.basis.xform_inv(d), margin_A)); + } + + _FORCE_INLINE_ Vector3 Support1(const Vector3& d) const { + return transform_B.xform(get_support(m_shapes[1], transform_B.basis.xform_inv(d), margin_B)); + } + + _FORCE_INLINE_ Vector3 Support (const Vector3& d) const { + return (Support0(d) - Support1(-d)); + } + + _FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const { + if (index) { + return Support1(d); + } else { + return Support0(d); + } + } +}; + +typedef MinkowskiDiff tShape; + + +// GJK +struct GJK +{ + /* Types */ + struct sSV + { + Vector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + real_t p[4]; + U rank; + }; + struct eStatus { enum _ { + Valid, + Inside, + Failed };}; + /* Fields */ + tShape m_shape; + Vector3 m_ray; + real_t m_distance = 0.0f; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree = 0; + U m_current = 0; + sSimplex* m_simplex = nullptr; + eStatus::_ m_status; + /* Methods */ + GJK() + { + Initialize(); + } + void Initialize() + { + m_ray = Vector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } + eStatus::_ Evaluate(const tShape& shapearg,const Vector3& guess) + { + U iterations=0; + real_t sqdist=0; + real_t alpha=0; + Vector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const real_t sqrl= m_ray.length_squared(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const real_t rl=m_ray.length(); + if(rl<GJK_MIN_DISTANCE) + {/* Touching or inside */ + m_status=eStatus::Inside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs,-m_ray); + const Vector3& w=cs.c[cs.rank-1]->w; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length_squared()<GJK_DUPLICATED_EPS) + { found=true;break; } + } + if(found) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + {/* Update lastw */ + lastw[clastw=(clastw+1)&3]=w; + } + /* Check for termination */ + const real_t omega=vec3_dot(m_ray,w)/rl; + alpha=MAX(omega,alpha); + if(((rl-alpha)-(GJK_ACCURACY*rl))<=0) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + real_t weights[4]; + U mask=0; + switch(cs.rank) + { + case 2: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = Vector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;i<ni;++i) + { + if(mask&(1<<i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) { m_status=eStatus::Inside; +} + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; + } while(m_status==eStatus::Valid); + m_simplex=&m_simplices[m_current]; + switch(m_status) + { + case eStatus::Valid: m_distance=m_ray.length();break; + case eStatus::Inside: m_distance=0;break; + default: {} + } + return(m_status); + } + bool EncloseOrigin() + { + switch(m_simplex->rank) + { + case 1: + { + for(U i=0;i<3;++i) + { + Vector3 axis=Vector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) { return(true); +} + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) { return(true); +} + removevertice(*m_simplex); + } + } + break; + case 2: + { + const Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + Vector3 axis=Vector3(0,0,0); + axis[i]=1; + const Vector3 p=vec3_cross(d,axis); + if(p.length_squared()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) { return(true); +} + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) { return(true); +} + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const Vector3 n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length_squared()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) { return(true); +} + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) { return(true); +} + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(Math::abs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) { + return(true); +} + } + break; + } + return(false); + } + /* Internals */ + void getsupport(const Vector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex,const Vector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } + static real_t det(const Vector3& a,const Vector3& b,const Vector3& c) + { + return( a.y*b.z*c.x+a.z*b.x*c.y- + a.x*b.z*c.y-a.y*b.x*c.z+ + a.x*b.y*c.z-a.z*b.y*c.x); + } + static real_t projectorigin( const Vector3& a, + const Vector3& b, + real_t* w,U& m) + { + const Vector3 d=b-a; + const real_t l=d.length_squared(); + if(l>GJK_SIMPLEX2_EPS) + { + const real_t t(l>0?-vec3_dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length_squared()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length_squared()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); } + } + return(-1); + } + static real_t projectorigin( const Vector3& a, + const Vector3& b, + const Vector3& c, + real_t* w,U& m) + { + static const U imd3[]={1,2,0}; + const Vector3* vt[]={&a,&b,&c}; + const Vector3 dl[]={a-b,b-c,c-a}; + const Vector3 n=vec3_cross(dl[0],dl[1]); + const real_t l=n.length_squared(); + if(l>GJK_SIMPLEX3_EPS) + { + real_t mindist=-1; + real_t subw[2] = { 0 , 0}; + U subm = 0; + for(U i=0;i<3;++i) + { + if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0) + { + const U j=imd3[i]; + const real_t subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if(mindist<0) + { + const real_t d=vec3_dot(a,n); + const real_t s=Math::sqrt(l); + const Vector3 p=n*(d/l); + mindist = p.length_squared(); + m = 7; + w[0] = (vec3_cross(dl[1],b-p)).length()/s; + w[1] = (vec3_cross(dl[2],c-p)).length()/s; + w[2] = 1-(w[0]+w[1]); + } + return(mindist); + } + return(-1); + } + static real_t projectorigin( const Vector3& a, + const Vector3& b, + const Vector3& c, + const Vector3& d, + real_t* w,U& m) + { + static const U imd3[]={1,2,0}; + const Vector3* vt[]={&a,&b,&c,&d}; + const Vector3 dl[]={a-d,b-d,c-d}; + const real_t vl=det(dl[0],dl[1],dl[2]); + const bool ng=(vl*vec3_dot(a,vec3_cross(b-c,a-b)))<=0; + if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS)) + { + real_t mindist=-1; + real_t subw[3] = {0.f, 0.f, 0.f}; + U subm=0; + for(U i=0;i<3;++i) + { + const U j=imd3[i]; + const real_t s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j])); + if(s>0) + { + const real_t subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>((subm&1?1<<i:0)+ + (subm&2?1<<j:0)+ + (subm&4?8:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if(mindist<0) + { + mindist = 0; + m = 15; + w[0] = det(c,b,d)/vl; + w[1] = det(a,c,d)/vl; + w[2] = det(b,a,d)/vl; + w[3] = 1-(w[0]+w[1]+w[2]); + } + return(mindist); + } + return(-1); + } +}; + + // EPA + struct EPA + { + /* Types */ + typedef GJK::sSV sSV; + struct sFace + { + Vector3 n; + real_t d = 0.0f; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass = 0; + }; + struct sList + { + sFace* root = nullptr; + U count = 0; + sList() {} + }; + struct sHorizon + { + sFace* cf = nullptr; + sFace* ff = nullptr; + U nf = 0; + sHorizon() {} + }; + struct eStatus { enum _ { + Valid, + Touching, + Degenerated, + NonConvex, + InvalidHull, + OutOfFaces, + OutOfVertices, + AccuraryReached, + FallBack, + Failed };}; + /* Fields */ + eStatus::_ m_status; + GJK::sSimplex m_result; + Vector3 m_normal; + real_t m_depth = 0.0f; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv = 0; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() + { + Initialize(); + } + + + static inline void bind(sFace* fa,U ea,sFace* fb,U eb) + { + fa->e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; + } + static inline void append(sList& list,sFace* face) + { + face->l[0] = nullptr; + face->l[1] = list.root; + if(list.root) { list.root->l[0]=face; +} + list.root = face; + ++list.count; + } + static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) { face->l[1]->l[0]=face->l[0]; +} + if(face->l[0]) { face->l[0]->l[1]=face->l[1]; +} + if(face==list.root) { list.root=face->l[1]; +} + --list.count; + } + + + void Initialize() + { + m_status = eStatus::Failed; + m_normal = Vector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i<EPA_MAX_FACES;++i) + { + append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); + } + } + eStatus::_ Evaluate(GJK& gjk,const Vector3& guess) + { + GJK::sSimplex& simplex=*gjk.m_simplex; + if((simplex.rank>1)&&gjk.EncloseOrigin()) + { + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + SWAP(simplex.c[0],simplex.c[1]); + SWAP(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterations<EPA_MAX_ITERATIONS;++iterations) + { + if(m_nextsv<EPA_MAX_VERTICES) + { + sHorizon horizon; + sSV* w=&m_sv_store[m_nextsv++]; + bool valid=true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const real_t wdist=vec3_dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const Vector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = vec3_cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = vec3_cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = vec3_cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const real_t sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const real_t nl = m_normal.length(); + if (nl > 0) { + m_normal = m_normal/nl; + } else { + m_normal = Vector3(1,0,0); + } + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } + + bool getedgedist(sFace* face, sSV* a, sSV* b, real_t& dist) + { + const Vector3 ba = b->w - a->w; + const Vector3 n_ab = vec3_cross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const real_t a_dot_nab = vec3_dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if (a_dot_nab < 0) { + // Outside of edge a->b + const real_t ba_l2 = ba.length_squared(); + const real_t a_dot_ba = vec3_dot(a->w, ba); + const real_t b_dot_ba = vec3_dot(b->w, ba); + + if (a_dot_ba > 0) { + // Pick distance vertex a + dist = a->w.length(); + } else if (b_dot_ba < 0) { + // Pick distance vertex b + dist = b->w.length(); + } else { + // Pick distance to edge a->b + const real_t a_dot_b = vec3_dot(a->w, b->w); + dist = Math::sqrt(MAX((a->w.length_squared() * b->w.length_squared() - a_dot_b * a_dot_b) / ba_l2, 0.0)); + } + + return true; + } + + return false; + } + + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if (m_stock.root) { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = vec3_cross(b->w-a->w,c->w-a->w); + const real_t l=face->n.length(); + const bool v=l>EPA_ACCURACY; + if (v) { + if (!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = vec3_dot(a->w, face->n) / l; + } + face->n /= l; + if (forced||(face->d>=-EPA_PLANE_EPS)) { + return(face); + } else { + m_status=eStatus::NonConvex; + } + } else { + m_status=eStatus::Degenerated; + } + remove(m_hull,face); + append(m_stock,face); + return(nullptr); + } + // -- GODOT start -- + //m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + m_status=eStatus::OutOfFaces; + // -- GODOT end -- + return(nullptr); + } + sFace* findbest() + { + sFace* minf=m_hull.root; + real_t mind=minf->d*minf->d; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const real_t sqd=f->d*f->d; + if(sqd<mind) + { + minf=f; + mind=sqd; + } + } + return(minf); + } + bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + { + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const U e1=i1m3[e]; + if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) { bind(horizon.cf,1,nf,2); } else { horizon.ff=nf; +} + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } + + }; + + // + static void Initialize( const GodotShape3D* shape0, const Transform3D& wtrs0, real_t margin0, + const GodotShape3D* shape1, const Transform3D& wtrs1, real_t margin1, + sResults& results, + tShape& shape) + { + /* Results */ + results.witnesses[0] = Vector3(0,0,0); + results.witnesses[1] = Vector3(0,0,0); + results.status = sResults::Separated; + /* Shape */ + shape.Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1); + } + + + +// +// Api +// + +// + +// +bool Distance( const GodotShape3D* shape0, + const Transform3D& wtrs0, + real_t margin0, + const GodotShape3D* shape1, + const Transform3D& wtrs1, + real_t margin1, + const Vector3& guess, + sResults& results) +{ + tShape shape; + Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==GJK::eStatus::Valid) + { + Vector3 w0=Vector3(0,0,0); + Vector3 w1=Vector3(0,0,0); + for(U i=0;i<gjk.m_simplex->rank;++i) + { + const real_t p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = w0; + results.witnesses[1] = w1; + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); + } + else + { + results.status = gjk_status==GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed; + return(false); + } +} + + +// +bool Penetration( const GodotShape3D* shape0, + const Transform3D& wtrs0, + real_t margin0, + const GodotShape3D* shape1, + const Transform3D& wtrs1, + real_t margin1, + const Vector3& guess, + sResults& results + ) +{ + tShape shape; + Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); + switch(gjk_status) + { + case GJK::eStatus::Inside: + { + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) + { + Vector3 w0=Vector3(0,0,0); + for(U i=0;i<epa.m_result.rank;++i) + { + w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = w0; + results.witnesses[1] = w0-epa.m_normal*epa.m_depth; + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else { results.status=sResults::EPA_Failed; +} + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + default: {} + } + return(false); +} + + + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS +} // end of namespace + +/* clang-format on */ + +bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { + GjkEpa2::sResults res; + + if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) { + r_result_A = res.witnesses[0]; + r_result_B = res.witnesses[1]; + return true; + } + + return false; +} + +bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { + GjkEpa2::sResults res; + + if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) { + if (p_result_callback) { + if (p_swap) { + Vector3 normal = (res.witnesses[1] - res.witnesses[0]).normalized(); + p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, normal, p_userdata); + } else { + Vector3 normal = (res.witnesses[0] - res.witnesses[1]).normalized(); + p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, normal, p_userdata); + } + } + return true; + } + + return false; +} diff --git a/modules/godot_physics_3d/gjk_epa.h b/modules/godot_physics_3d/gjk_epa.h new file mode 100644 index 0000000000..48fda9969f --- /dev/null +++ b/modules/godot_physics_3d/gjk_epa.h @@ -0,0 +1,40 @@ +/**************************************************************************/ +/* gjk_epa.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GJK_EPA_H +#define GJK_EPA_H + +#include "godot_collision_solver_3d.h" +#include "godot_shape_3d.h" + +bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); +bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); + +#endif // GJK_EPA_H diff --git a/modules/godot_physics_3d/godot_area_3d.cpp b/modules/godot_physics_3d/godot_area_3d.cpp new file mode 100644 index 0000000000..d0b287b058 --- /dev/null +++ b/modules/godot_physics_3d/godot_area_3d.cpp @@ -0,0 +1,346 @@ +/**************************************************************************/ +/* godot_area_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_area_3d.h" + +#include "godot_body_3d.h" +#include "godot_soft_body_3d.h" +#include "godot_space_3d.h" + +GodotArea3D::BodyKey::BodyKey(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +GodotArea3D::BodyKey::BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +GodotArea3D::BodyKey::BodyKey(GodotArea3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + rid = p_body->get_self(); + instance_id = p_body->get_instance_id(); + body_shape = p_body_shape; + area_shape = p_area_shape; +} + +void GodotArea3D::_shapes_changed() { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea3D::set_transform(const Transform3D &p_transform) { + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } + + _set_transform(p_transform); + _set_inv_transform(p_transform.affine_inverse()); +} + +void GodotArea3D::set_space(GodotSpace3D *p_space) { + if (get_space()) { + if (monitor_query_list.in_list()) { + get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + } + if (moved_list.in_list()) { + get_space()->area_remove_from_moved_list(&moved_list); + } + } + + monitored_bodies.clear(); + monitored_areas.clear(); + + _set_space(p_space); +} + +void GodotArea3D::set_monitor_callback(const Callable &p_callback) { + _unregister_shapes(); + + monitor_callback = p_callback; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea3D::set_area_monitor_callback(const Callable &p_callback) { + _unregister_shapes(); + + area_monitor_callback = p_callback; + + monitored_bodies.clear(); + monitored_areas.clear(); + + _shape_changed(); + + if (!moved_list.in_list() && get_space()) { + get_space()->area_add_to_moved_list(&moved_list); + } +} + +void GodotArea3D::_set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode) { + bool do_override = p_new_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override == (r_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) { + return; + } + _unregister_shapes(); + r_mode = p_new_mode; + _shape_changed(); +} + +void GodotArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: + _set_space_override_mode(gravity_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value); + break; + case PhysicsServer3D::AREA_PARAM_GRAVITY: + gravity = p_value; + break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: + gravity_vector = p_value; + break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: + gravity_is_point = p_value; + break; + case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: + gravity_point_unit_distance = p_value; + break; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: + _set_space_override_mode(linear_damping_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value); + break; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: + linear_damp = p_value; + break; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: + _set_space_override_mode(angular_damping_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value); + break; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: + angular_damp = p_value; + break; + case PhysicsServer3D::AREA_PARAM_PRIORITY: + priority = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: + ERR_FAIL_COND_MSG(wind_force_magnitude < 0, "Wind force magnitude must be a non-negative real number, but a negative number was specified."); + wind_force_magnitude = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: + wind_source = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: + wind_direction = p_value; + break; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: + ERR_FAIL_COND_MSG(wind_attenuation_factor < 0, "Wind attenuation factor must be a non-negative real number, but a negative number was specified."); + wind_attenuation_factor = p_value; + break; + } +} + +Variant GodotArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: + return gravity_override_mode; + case PhysicsServer3D::AREA_PARAM_GRAVITY: + return gravity; + case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: + return gravity_vector; + case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: + return gravity_is_point; + case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: + return gravity_point_unit_distance; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: + return linear_damping_override_mode; + case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: + return linear_damp; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: + return angular_damping_override_mode; + case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: + return angular_damp; + case PhysicsServer3D::AREA_PARAM_PRIORITY: + return priority; + case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: + return wind_force_magnitude; + case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: + return wind_source; + case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: + return wind_direction; + case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: + return wind_attenuation_factor; + } + + return Variant(); +} + +void GodotArea3D::_queue_monitor_update() { + ERR_FAIL_NULL(get_space()); + + if (!monitor_query_list.in_list()) { + get_space()->area_add_to_monitor_query_list(&monitor_query_list); + } +} + +void GodotArea3D::set_monitorable(bool p_monitorable) { + if (monitorable == p_monitorable) { + return; + } + + monitorable = p_monitorable; + _set_static(!monitorable); + _shapes_changed(); +} + +void GodotArea3D::call_queries() { + if (!monitor_callback.is_null() && !monitored_bodies.is_empty()) { + if (monitor_callback.is_valid()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_bodies.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); + E = next; + continue; + } + + res[0] = E->value.state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; + + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_bodies.remove(E); + E = next; + + Callable::CallError ce; + Variant ret; + monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling monitor callback method " + Variant::get_callable_error_text(monitor_callback, (const Variant **)resptr, 5, ce)); + } + } + } else { + monitored_bodies.clear(); + monitor_callback = Callable(); + } + } + + if (!area_monitor_callback.is_null() && !monitored_areas.is_empty()) { + if (area_monitor_callback.is_valid()) { + Variant res[5]; + Variant *resptr[5]; + for (int i = 0; i < 5; i++) { + resptr[i] = &res[i]; + } + + for (HashMap<BodyKey, BodyState, BodyKey>::Iterator E = monitored_areas.begin(); E;) { + if (E->value.state == 0) { // Nothing happened + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); + E = next; + continue; + } + + res[0] = E->value.state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED; + res[1] = E->key.rid; + res[2] = E->key.instance_id; + res[3] = E->key.body_shape; + res[4] = E->key.area_shape; + + HashMap<BodyKey, BodyState, BodyKey>::Iterator next = E; + ++next; + monitored_areas.remove(E); + E = next; + + Callable::CallError ce; + Variant ret; + area_monitor_callback.callp((const Variant **)resptr, 5, ret, ce); + + if (ce.error != Callable::CallError::CALL_OK) { + ERR_PRINT_ONCE("Error calling area monitor callback method " + Variant::get_callable_error_text(area_monitor_callback, (const Variant **)resptr, 5, ce)); + } + } + } else { + monitored_areas.clear(); + area_monitor_callback = Callable(); + } + } +} + +void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const { + if (is_gravity_point()) { + const real_t gr_unit_dist = get_gravity_point_unit_distance(); + Vector3 v = get_transform().xform(get_gravity_vector()) - p_position; + if (gr_unit_dist > 0) { + const real_t v_length_sq = v.length_squared(); + if (v_length_sq > 0) { + const real_t gravity_strength = get_gravity() * gr_unit_dist * gr_unit_dist / v_length_sq; + r_gravity = v.normalized() * gravity_strength; + } else { + r_gravity = Vector3(); + } + } else { + r_gravity = v.normalized() * get_gravity(); + } + } else { + r_gravity = get_gravity_vector() * get_gravity(); + } +} + +GodotArea3D::GodotArea3D() : + GodotCollisionObject3D(TYPE_AREA), + monitor_query_list(this), + moved_list(this) { + _set_static(true); //areas are never active + set_ray_pickable(false); +} + +GodotArea3D::~GodotArea3D() { +} diff --git a/modules/godot_physics_3d/godot_area_3d.h b/modules/godot_physics_3d/godot_area_3d.h new file mode 100644 index 0000000000..2c1a782630 --- /dev/null +++ b/modules/godot_physics_3d/godot_area_3d.h @@ -0,0 +1,240 @@ +/**************************************************************************/ +/* godot_area_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_AREA_3D_H +#define GODOT_AREA_3D_H + +#include "godot_collision_object_3d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_3d.h" + +class GodotSpace3D; +class GodotBody3D; +class GodotSoftBody3D; +class GodotConstraint3D; + +class GodotArea3D : public GodotCollisionObject3D { + PhysicsServer3D::AreaSpaceOverrideMode gravity_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer3D::AreaSpaceOverrideMode linear_damping_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + PhysicsServer3D::AreaSpaceOverrideMode angular_damping_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; + + real_t gravity = 9.80665; + Vector3 gravity_vector = Vector3(0, -1, 0); + bool gravity_is_point = false; + real_t gravity_point_unit_distance = 0.0; + real_t linear_damp = 0.1; + real_t angular_damp = 0.1; + real_t wind_force_magnitude = 0.0; + real_t wind_attenuation_factor = 0.0; + Vector3 wind_source; + Vector3 wind_direction; + int priority = 0; + bool monitorable = false; + + Callable monitor_callback; + Callable area_monitor_callback; + + SelfList<GodotArea3D> monitor_query_list; + SelfList<GodotArea3D> moved_list; + + struct BodyKey { + RID rid; + ObjectID instance_id; + uint32_t body_shape = 0; + uint32_t area_shape = 0; + + static uint32_t hash(const BodyKey &p_key) { + uint32_t h = hash_one_uint64(p_key.rid.get_id()); + h = hash_murmur3_one_64(p_key.instance_id, h); + h = hash_murmur3_one_32(p_key.area_shape, h); + return hash_fmix32(hash_murmur3_one_32(p_key.body_shape, h)); + } + + _FORCE_INLINE_ bool operator==(const BodyKey &p_key) const { + return rid == p_key.rid && instance_id == p_key.instance_id && body_shape == p_key.body_shape && area_shape == p_key.area_shape; + } + + _FORCE_INLINE_ BodyKey() {} + BodyKey(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + BodyKey(GodotArea3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + }; + + struct BodyState { + int state = 0; + _FORCE_INLINE_ void inc() { state++; } + _FORCE_INLINE_ void dec() { state--; } + }; + + HashMap<BodyKey, BodyState, BodyKey> monitored_soft_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_bodies; + HashMap<BodyKey, BodyState, BodyKey> monitored_areas; + + HashSet<GodotConstraint3D *> constraints; + + virtual void _shapes_changed() override; + void _queue_monitor_update(); + + void _set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode); + +public: + void set_monitor_callback(const Callable &p_callback); + _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback.is_valid(); } + + void set_area_monitor_callback(const Callable &p_callback); + _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback.is_valid(); } + + _FORCE_INLINE_ void add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape); + + _FORCE_INLINE_ void add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); + _FORCE_INLINE_ void remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); + + _FORCE_INLINE_ void add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + _FORCE_INLINE_ void remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape); + + void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer3D::AreaParameter p_param) const; + + _FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; } + _FORCE_INLINE_ real_t get_gravity() const { return gravity; } + + _FORCE_INLINE_ void set_gravity_vector(const Vector3 &p_gravity) { gravity_vector = p_gravity; } + _FORCE_INLINE_ Vector3 get_gravity_vector() const { return gravity_vector; } + + _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; } + _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } + + _FORCE_INLINE_ void set_gravity_point_unit_distance(real_t scale) { gravity_point_unit_distance = scale; } + _FORCE_INLINE_ real_t get_gravity_point_unit_distance() const { return gravity_point_unit_distance; } + + _FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; } + _FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; } + + _FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; } + _FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _FORCE_INLINE_ void set_wind_force_magnitude(real_t p_wind_force_magnitude) { wind_force_magnitude = p_wind_force_magnitude; } + _FORCE_INLINE_ real_t get_wind_force_magnitude() const { return wind_force_magnitude; } + + _FORCE_INLINE_ void set_wind_attenuation_factor(real_t p_wind_attenuation_factor) { wind_attenuation_factor = p_wind_attenuation_factor; } + _FORCE_INLINE_ real_t get_wind_attenuation_factor() const { return wind_attenuation_factor; } + + _FORCE_INLINE_ void set_wind_source(const Vector3 &p_wind_source) { wind_source = p_wind_source; } + _FORCE_INLINE_ const Vector3 &get_wind_source() const { return wind_source; } + + _FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; } + _FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } + + void set_monitorable(bool p_monitorable); + _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } + + void set_transform(const Transform3D &p_transform); + + void set_space(GodotSpace3D *p_space) override; + + void call_queries(); + + void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const; + + GodotArea3D(); + ~GodotArea3D(); +}; + +void GodotArea3D::add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); + monitored_soft_bodies[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); + monitored_soft_bodies[bk].dec(); + if (get_space() && !monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { + BodyKey bk(p_body, p_body_shape, p_area_shape); + monitored_bodies[bk].dec(); + if (get_space() && !monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].inc(); + if (!monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +void GodotArea3D::remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { + BodyKey bk(p_area, p_area_shape, p_self_shape); + monitored_areas[bk].dec(); + if (get_space() && !monitor_query_list.in_list()) { + _queue_monitor_update(); + } +} + +struct AreaCMP { + GodotArea3D *area = nullptr; + int refCount = 0; + _FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); } + _FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); } + _FORCE_INLINE_ AreaCMP() {} + _FORCE_INLINE_ AreaCMP(GodotArea3D *p_area) { + area = p_area; + refCount = 1; + } +}; + +#endif // GODOT_AREA_3D_H diff --git a/modules/godot_physics_3d/godot_area_pair_3d.cpp b/modules/godot_physics_3d/godot_area_pair_3d.cpp new file mode 100644 index 0000000000..aaa96f5a28 --- /dev/null +++ b/modules/godot_physics_3d/godot_area_pair_3d.cpp @@ -0,0 +1,294 @@ +/**************************************************************************/ +/* godot_area_pair_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_area_pair_3d.h" + +#include "godot_collision_solver_3d.h" + +bool GodotAreaPair3D::setup(real_t p_step) { + bool result = false; + if (area->collides_with(body) && GodotCollisionSolver3D::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) { + result = true; + } + + process_collision = false; + has_space_override = false; + if (result != colliding) { + if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } + process_collision = has_space_override; + + if (area->has_monitor_callback()) { + process_collision = true; + } + + colliding = result; + } + + return process_collision; +} + +bool GodotAreaPair3D::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (has_space_override) { + body_has_attached_area = true; + body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_body_to_query(body, body_shape, area_shape); + } + } else { + if (has_space_override) { + body_has_attached_area = false; + body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + + return false; // Never do any post solving. +} + +void GodotAreaPair3D::solve(real_t p_step) { + // Nothing to do. +} + +GodotAreaPair3D::GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape) { + body = p_body; + area = p_area; + body_shape = p_body_shape; + area_shape = p_area_shape; + body->add_constraint(this, 0); + area->add_constraint(this); + if (p_body->get_mode() == PhysicsServer3D::BODY_MODE_KINEMATIC) { + p_body->set_active(true); + } +} + +GodotAreaPair3D::~GodotAreaPair3D() { + if (colliding) { + if (body_has_attached_area) { + body_has_attached_area = false; + body->remove_area(area); + } + if (area->has_monitor_callback()) { + area->remove_body_from_query(body, body_shape, area_shape); + } + } + body->remove_constraint(this); + area->remove_constraint(this); +} + +//////////////////////////////////////////////////// + +bool GodotArea2Pair3D::setup(real_t p_step) { + bool result_a = area_a->collides_with(area_b); + bool result_b = area_b->collides_with(area_a); + if ((result_a || result_b) && !GodotCollisionSolver3D::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) { + result_a = false; + result_b = false; + } + + bool process_collision = false; + + process_collision_a = false; + if (result_a != colliding_a) { + if (area_a->has_area_monitor_callback() && area_b_monitorable) { + process_collision_a = true; + process_collision = true; + } + colliding_a = result_a; + } + + process_collision_b = false; + if (result_b != colliding_b) { + if (area_b->has_area_monitor_callback() && area_a_monitorable) { + process_collision_b = true; + process_collision = true; + } + colliding_b = result_b; + } + + return process_collision; +} + +bool GodotArea2Pair3D::pre_solve(real_t p_step) { + if (process_collision_a) { + if (colliding_a) { + area_a->add_area_to_query(area_b, shape_b, shape_a); + } else { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (process_collision_b) { + if (colliding_b) { + area_b->add_area_to_query(area_a, shape_a, shape_b); + } else { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + return false; // Never do any post solving. +} + +void GodotArea2Pair3D::solve(real_t p_step) { + // Nothing to do. +} + +GodotArea2Pair3D::GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b) { + area_a = p_area_a; + area_b = p_area_b; + shape_a = p_shape_a; + shape_b = p_shape_b; + area_a_monitorable = area_a->is_monitorable(); + area_b_monitorable = area_b->is_monitorable(); + area_a->add_constraint(this); + area_b->add_constraint(this); +} + +GodotArea2Pair3D::~GodotArea2Pair3D() { + if (colliding_a) { + if (area_a->has_area_monitor_callback() && area_b_monitorable) { + area_a->remove_area_from_query(area_b, shape_b, shape_a); + } + } + + if (colliding_b) { + if (area_b->has_area_monitor_callback() && area_a_monitorable) { + area_b->remove_area_from_query(area_a, shape_a, shape_b); + } + } + + area_a->remove_constraint(this); + area_b->remove_constraint(this); +} + +//////////////////////////////////////////////////// + +bool GodotAreaSoftBodyPair3D::setup(real_t p_step) { + bool result = false; + if ( + area->collides_with(soft_body) && + GodotCollisionSolver3D::solve_static( + soft_body->get_shape(soft_body_shape), + soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape), + area->get_shape(area_shape), + area->get_transform() * area->get_shape_transform(area_shape), + nullptr, + this)) { + result = true; + } + + process_collision = false; + has_space_override = false; + if (result != colliding) { + if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + has_space_override = true; + } else if (area->get_wind_force_magnitude() > CMP_EPSILON) { + has_space_override = true; + } + + if (area->has_monitor_callback()) { + process_collision = true; + } + + colliding = result; + } + + return process_collision; +} + +bool GodotAreaSoftBodyPair3D::pre_solve(real_t p_step) { + if (!process_collision) { + return false; + } + + if (colliding) { + if (has_space_override) { + body_has_attached_area = true; + soft_body->add_area(area); + } + + if (area->has_monitor_callback()) { + area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape); + } + } else { + if (has_space_override) { + body_has_attached_area = false; + soft_body->remove_area(area); + } + + if (area->has_monitor_callback()) { + area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape); + } + } + + return false; // Never do any post solving. +} + +void GodotAreaSoftBodyPair3D::solve(real_t p_step) { + // Nothing to do. +} + +GodotAreaSoftBodyPair3D::GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_soft_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape) { + soft_body = p_soft_body; + area = p_area; + soft_body_shape = p_soft_body_shape; + area_shape = p_area_shape; + soft_body->add_constraint(this); + area->add_constraint(this); +} + +GodotAreaSoftBodyPair3D::~GodotAreaSoftBodyPair3D() { + if (colliding) { + if (body_has_attached_area) { + body_has_attached_area = false; + soft_body->remove_area(area); + } + if (area->has_monitor_callback()) { + area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape); + } + } + soft_body->remove_constraint(this); + area->remove_constraint(this); +} diff --git a/modules/godot_physics_3d/godot_area_pair_3d.h b/modules/godot_physics_3d/godot_area_pair_3d.h new file mode 100644 index 0000000000..a2c5df0f7a --- /dev/null +++ b/modules/godot_physics_3d/godot_area_pair_3d.h @@ -0,0 +1,98 @@ +/**************************************************************************/ +/* godot_area_pair_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_AREA_PAIR_3D_H +#define GODOT_AREA_PAIR_3D_H + +#include "godot_area_3d.h" +#include "godot_body_3d.h" +#include "godot_constraint_3d.h" +#include "godot_soft_body_3d.h" + +class GodotAreaPair3D : public GodotConstraint3D { + GodotBody3D *body = nullptr; + GodotArea3D *area = nullptr; + int body_shape; + int area_shape; + bool colliding = false; + bool process_collision = false; + bool has_space_override = false; + bool body_has_attached_area = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape); + ~GodotAreaPair3D(); +}; + +class GodotArea2Pair3D : public GodotConstraint3D { + GodotArea3D *area_a = nullptr; + GodotArea3D *area_b = nullptr; + int shape_a; + int shape_b; + bool colliding_a = false; + bool colliding_b = false; + bool process_collision_a = false; + bool process_collision_b = false; + bool area_a_monitorable; + bool area_b_monitorable; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b); + ~GodotArea2Pair3D(); +}; + +class GodotAreaSoftBodyPair3D : public GodotConstraint3D { + GodotSoftBody3D *soft_body = nullptr; + GodotArea3D *area = nullptr; + int soft_body_shape; + int area_shape; + bool colliding = false; + bool process_collision = false; + bool has_space_override = false; + bool body_has_attached_area = false; + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_sof_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape); + ~GodotAreaSoftBodyPair3D(); +}; + +#endif // GODOT_AREA_PAIR_3D_H diff --git a/modules/godot_physics_3d/godot_body_3d.cpp b/modules/godot_physics_3d/godot_body_3d.cpp new file mode 100644 index 0000000000..669c4b985b --- /dev/null +++ b/modules/godot_physics_3d/godot_body_3d.cpp @@ -0,0 +1,841 @@ +/**************************************************************************/ +/* godot_body_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_body_3d.h" + +#include "godot_area_3d.h" +#include "godot_body_direct_state_3d.h" +#include "godot_space_3d.h" + +void GodotBody3D::_mass_properties_changed() { + if (get_space() && !mass_properties_update_list.in_list()) { + get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); + } +} + +void GodotBody3D::_update_transform_dependent() { + center_of_mass = get_transform().basis.xform(center_of_mass_local); + principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; + + // Update inertia tensor. + Basis tb = principal_inertia_axes; + Basis tbt = tb.transposed(); + Basis diag; + diag.scale(_inv_inertia); + _inv_inertia_tensor = tb * diag * tbt; +} + +void GodotBody3D::update_mass_properties() { + // Update shapes and motions. + + switch (mode) { + case PhysicsServer3D::BODY_MODE_RIGID: { + real_t total_area = 0; + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + total_area += get_shape_area(i); + } + + if (calculate_center_of_mass) { + // We have to recompute the center of mass. + center_of_mass_local.zero(); + + if (total_area != 0.0) { + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + real_t area = get_shape_area(i); + + real_t mass_new = area * mass / total_area; + + // NOTE: we assume that the shape origin is also its center of mass. + center_of_mass_local += mass_new * get_shape_transform(i).origin; + } + + center_of_mass_local /= mass; + } + } + + if (calculate_inertia) { + // Recompute the inertia tensor. + Basis inertia_tensor; + inertia_tensor.set_zero(); + bool inertia_set = false; + + for (int i = 0; i < get_shape_count(); i++) { + if (is_shape_disabled(i)) { + continue; + } + + real_t area = get_shape_area(i); + if (area == 0.0) { + continue; + } + + inertia_set = true; + + const GodotShape3D *shape = get_shape(i); + + real_t mass_new = area * mass / total_area; + + Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass_new)); + Transform3D shape_transform = get_shape_transform(i); + Basis shape_basis = shape_transform.basis.orthonormalized(); + + // NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor! + shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed(); + + Vector3 shape_origin = shape_transform.origin - center_of_mass_local; + inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass_new; + } + + // Set the inertia to a valid value when there are no valid shapes. + if (!inertia_set) { + inertia_tensor = Basis(); + } + + // Handle partial custom inertia. + if (inertia.x > 0.0) { + inertia_tensor[0][0] = inertia.x; + } + if (inertia.y > 0.0) { + inertia_tensor[1][1] = inertia.y; + } + if (inertia.z > 0.0) { + inertia_tensor[2][2] = inertia.z; + } + + // Compute the principal axes of inertia. + principal_inertia_axes_local = inertia_tensor.diagonalize().transposed(); + _inv_inertia = inertia_tensor.get_main_diagonal().inverse(); + } + + if (mass) { + _inv_mass = 1.0 / mass; + } else { + _inv_mass = 0; + } + + } break; + case PhysicsServer3D::BODY_MODE_KINEMATIC: + case PhysicsServer3D::BODY_MODE_STATIC: { + _inv_inertia = Vector3(); + _inv_mass = 0; + } break; + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { + _inv_inertia_tensor.set_zero(); + _inv_mass = 1.0 / mass; + + } break; + } + + _update_transform_dependent(); +} + +void GodotBody3D::reset_mass_properties() { + calculate_inertia = true; + calculate_center_of_mass = true; + _mass_properties_changed(); +} + +void GodotBody3D::set_active(bool p_active) { + if (active == p_active) { + return; + } + + active = p_active; + + if (active) { + if (mode == PhysicsServer3D::BODY_MODE_STATIC) { + // Static bodies can't be active. + active = false; + } else if (get_space()) { + get_space()->body_add_to_active_list(&active_list); + } + } else if (get_space()) { + get_space()->body_remove_from_active_list(&active_list); + } +} + +void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) { + switch (p_param) { + case PhysicsServer3D::BODY_PARAM_BOUNCE: { + bounce = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_FRICTION: { + friction = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_MASS: { + real_t mass_value = p_value; + ERR_FAIL_COND(mass_value <= 0); + mass = mass_value; + if (mode >= PhysicsServer3D::BODY_MODE_RIGID) { + _mass_properties_changed(); + } + } break; + case PhysicsServer3D::BODY_PARAM_INERTIA: { + inertia = p_value; + if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) { + calculate_inertia = true; + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { + _mass_properties_changed(); + } + } else { + calculate_inertia = false; + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { + principal_inertia_axes_local = Basis(); + _inv_inertia = inertia.inverse(); + _update_transform_dependent(); + } + } + } break; + case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { + calculate_center_of_mass = false; + center_of_mass_local = p_value; + _update_transform_dependent(); + } break; + case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + if (Math::is_zero_approx(gravity_scale)) { + wakeup(); + } + gravity_scale = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: { + int mode_value = p_value; + linear_damp_mode = (PhysicsServer3D::BodyDampMode)mode_value; + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: { + int mode_value = p_value; + angular_damp_mode = (PhysicsServer3D::BodyDampMode)mode_value; + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { + linear_damp = p_value; + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { + angular_damp = p_value; + } break; + default: { + } + } +} + +Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::BODY_PARAM_BOUNCE: { + return bounce; + } break; + case PhysicsServer3D::BODY_PARAM_FRICTION: { + return friction; + } break; + case PhysicsServer3D::BODY_PARAM_MASS: { + return mass; + } break; + case PhysicsServer3D::BODY_PARAM_INERTIA: { + if (mode == PhysicsServer3D::BODY_MODE_RIGID) { + return _inv_inertia.inverse(); + } else { + return Vector3(); + } + } break; + case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: { + return center_of_mass_local; + } break; + case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: { + return gravity_scale; + } break; + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: { + return linear_damp_mode; + } + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: { + return angular_damp_mode; + } + case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: { + return linear_damp; + } break; + case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: { + return angular_damp; + } break; + + default: { + } + } + + return 0; +} + +void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) { + PhysicsServer3D::BodyMode prev = mode; + mode = p_mode; + + switch (p_mode) { + case PhysicsServer3D::BODY_MODE_STATIC: + case PhysicsServer3D::BODY_MODE_KINEMATIC: { + _set_inv_transform(get_transform().affine_inverse()); + _inv_mass = 0; + _inv_inertia = Vector3(); + _set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC); + set_active(p_mode == PhysicsServer3D::BODY_MODE_KINEMATIC && contacts.size()); + linear_velocity = Vector3(); + angular_velocity = Vector3(); + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) { + first_time_kinematic = true; + } + _update_transform_dependent(); + + } break; + case PhysicsServer3D::BODY_MODE_RIGID: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + if (!calculate_inertia) { + principal_inertia_axes_local = Basis(); + _inv_inertia = inertia.inverse(); + _update_transform_dependent(); + } + _mass_properties_changed(); + _set_static(false); + set_active(true); + + } break; + case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: { + _inv_mass = mass > 0 ? (1.0 / mass) : 0; + _inv_inertia = Vector3(); + angular_velocity = Vector3(); + _update_transform_dependent(); + _set_static(false); + set_active(true); + } + } +} + +PhysicsServer3D::BodyMode GodotBody3D::get_mode() const { + return mode; +} + +void GodotBody3D::_shapes_changed() { + _mass_properties_changed(); + wakeup(); + wakeup_neighbours(); +} + +void GodotBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + new_transform = p_variant; + //wakeup_neighbours(); + set_active(true); + if (first_time_kinematic) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + first_time_kinematic = false; + } + + } else if (mode == PhysicsServer3D::BODY_MODE_STATIC) { + _set_transform(p_variant); + _set_inv_transform(get_transform().affine_inverse()); + wakeup_neighbours(); + } else { + Transform3D t = p_variant; + t.orthonormalize(); + new_transform = get_transform(); //used as old to compute motion + if (new_transform == t) { + break; + } + _set_transform(t); + _set_inv_transform(get_transform().inverse()); + _update_transform_dependent(); + } + wakeup(); + + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + linear_velocity = p_variant; + constant_linear_velocity = linear_velocity; + wakeup(); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + angular_velocity = p_variant; + constant_angular_velocity = angular_velocity; + wakeup(); + + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + break; + } + bool do_sleep = p_variant; + if (do_sleep) { + linear_velocity = Vector3(); + //biased_linear_velocity=Vector3(); + angular_velocity = Vector3(); + //biased_angular_velocity=Vector3(); + set_active(false); + } else { + set_active(true); + } + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + can_sleep = p_variant; + if (mode >= PhysicsServer3D::BODY_MODE_RIGID && !active && !can_sleep) { + set_active(true); + } + + } break; + } +} + +Variant GodotBody3D::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + return linear_velocity; + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + return angular_velocity; + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + return !is_active(); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + return can_sleep; + } break; + } + + return Variant(); +} + +void GodotBody3D::set_space(GodotSpace3D *p_space) { + if (get_space()) { + if (mass_properties_update_list.in_list()) { + get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); + } + if (active_list.in_list()) { + get_space()->body_remove_from_active_list(&active_list); + } + if (direct_state_query_list.in_list()) { + get_space()->body_remove_from_state_query_list(&direct_state_query_list); + } + } + + _set_space(p_space); + + if (get_space()) { + _mass_properties_changed(); + + if (active && !active_list.in_list()) { + get_space()->body_add_to_active_list(&active_list); + } + } +} + +void GodotBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) { + if (lock) { + locked_axis |= p_axis; + } else { + locked_axis &= ~p_axis; + } +} + +bool GodotBody3D::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const { + return locked_axis & p_axis; +} + +void GodotBody3D::integrate_forces(real_t p_step) { + if (mode == PhysicsServer3D::BODY_MODE_STATIC) { + return; + } + + ERR_FAIL_NULL(get_space()); + + int ac = areas.size(); + + bool gravity_done = false; + bool linear_damp_done = false; + bool angular_damp_done = false; + + bool stopped = false; + + gravity = Vector3(0, 0, 0); + + total_linear_damp = 0.0; + total_angular_damp = 0.0; + + // Combine gravity and damping from overlapping areas in priority order. + if (ac) { + areas.sort(); + const AreaCMP *aa = &areas[0]; + for (int i = ac - 1; i >= 0 && !stopped; i--) { + if (!gravity_done) { + PhysicsServer3D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE); + if (area_gravity_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + Vector3 area_gravity; + aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity); + switch (area_gravity_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + gravity += area_gravity; + gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = area_gravity; + gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!linear_damp_done) { + PhysicsServer3D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE); + if (area_linear_damp_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + real_t area_linear_damp = aa[i].area->get_linear_damp(); + switch (area_linear_damp_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + total_linear_damp += area_linear_damp; + linear_damp_done = area_linear_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + total_linear_damp = area_linear_damp; + linear_damp_done = area_linear_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + if (!angular_damp_done) { + PhysicsServer3D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE); + if (area_angular_damp_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + real_t area_angular_damp = aa[i].area->get_angular_damp(); + switch (area_angular_damp_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + total_angular_damp += area_angular_damp; + angular_damp_done = area_angular_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + total_angular_damp = area_angular_damp; + angular_damp_done = area_angular_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + stopped = gravity_done && linear_damp_done && angular_damp_done; + } + } + + // Add default gravity and damping from space area. + if (!stopped) { + GodotArea3D *default_area = get_space()->get_default_area(); + ERR_FAIL_NULL(default_area); + + if (!gravity_done) { + Vector3 default_gravity; + default_area->compute_gravity(get_transform().get_origin(), default_gravity); + gravity += default_gravity; + } + + if (!linear_damp_done) { + total_linear_damp += default_area->get_linear_damp(); + } + + if (!angular_damp_done) { + total_angular_damp += default_area->get_angular_damp(); + } + } + + // Override linear damping with body's value. + switch (linear_damp_mode) { + case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: { + total_linear_damp += linear_damp; + } break; + case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: { + total_linear_damp = linear_damp; + } break; + } + + // Override angular damping with body's value. + switch (angular_damp_mode) { + case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: { + total_angular_damp += angular_damp; + } break; + case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: { + total_angular_damp = angular_damp; + } break; + } + + gravity *= gravity_scale; + + prev_linear_velocity = linear_velocity; + prev_angular_velocity = angular_velocity; + + Vector3 motion; + bool do_motion = false; + + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + //compute motion, angular and etc. velocities from prev transform + motion = new_transform.origin - get_transform().origin; + do_motion = true; + linear_velocity = constant_linear_velocity + motion / p_step; + + //compute a FAKE angular velocity, not so easy + Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed(); + Vector3 axis; + real_t angle; + + rot.get_axis_angle(axis, angle); + axis.normalize(); + angular_velocity = constant_angular_velocity + axis * (angle / p_step); + } else { + if (!omit_force_integration) { + //overridden by direct state query + + Vector3 force = gravity * mass + applied_force + constant_force; + Vector3 torque = applied_torque + constant_torque; + + real_t damp = 1.0 - p_step * total_linear_damp; + + if (damp < 0) { // reached zero in the given time + damp = 0; + } + + real_t angular_damp_new = 1.0 - p_step * total_angular_damp; + + if (angular_damp_new < 0) { // reached zero in the given time + angular_damp_new = 0; + } + + linear_velocity *= damp; + angular_velocity *= angular_damp_new; + + linear_velocity += _inv_mass * force * p_step; + angular_velocity += _inv_inertia_tensor.xform(torque) * p_step; + } + + if (continuous_cd) { + motion = linear_velocity * p_step; + do_motion = true; + } + } + + applied_force = Vector3(); + applied_torque = Vector3(); + + biased_angular_velocity = Vector3(); + biased_linear_velocity = Vector3(); + + if (do_motion) { //shapes temporarily extend for raycast + _update_shapes_with_motion(motion); + } + + contact_count = 0; +} + +void GodotBody3D::integrate_velocities(real_t p_step) { + if (mode == PhysicsServer3D::BODY_MODE_STATIC) { + return; + } + + ERR_FAIL_NULL(get_space()); + + if (fi_callback_data || body_state_callback.is_valid()) { + get_space()->body_add_to_state_query_list(&direct_state_query_list); + } + + //apply axis lock linear + for (int i = 0; i < 3; i++) { + if (is_axis_locked((PhysicsServer3D::BodyAxis)(1 << i))) { + linear_velocity[i] = 0; + biased_linear_velocity[i] = 0; + new_transform.origin[i] = get_transform().origin[i]; + } + } + //apply axis lock angular + for (int i = 0; i < 3; i++) { + if (is_axis_locked((PhysicsServer3D::BodyAxis)(1 << (i + 3)))) { + angular_velocity[i] = 0; + biased_angular_velocity[i] = 0; + } + } + + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + _set_transform(new_transform, false); + _set_inv_transform(new_transform.affine_inverse()); + if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3()) { + set_active(false); //stopped moving, deactivate + } + + return; + } + + Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity; + + real_t ang_vel = total_angular_velocity.length(); + Transform3D transform_new = get_transform(); + + if (!Math::is_zero_approx(ang_vel)) { + Vector3 ang_vel_axis = total_angular_velocity / ang_vel; + Basis rot(ang_vel_axis, ang_vel * p_step); + Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1); + transform_new.origin += ((identity3 - rot) * transform_new.basis).xform(center_of_mass_local); + transform_new.basis = rot * transform_new.basis; + transform_new.orthonormalize(); + } + + Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity; + /*for(int i=0;i<3;i++) { + if (axis_lock&(1<<i)) { + transform_new.origin[i]=0.0; + } + }*/ + + transform_new.origin += total_linear_velocity * p_step; + + _set_transform(transform_new); + _set_inv_transform(get_transform().inverse()); + + _update_transform_dependent(); +} + +void GodotBody3D::wakeup_neighbours() { + for (const KeyValue<GodotConstraint3D *, int> &E : constraint_map) { + const GodotConstraint3D *c = E.key; + GodotBody3D **n = c->get_body_ptr(); + int bc = c->get_body_count(); + + for (int i = 0; i < bc; i++) { + if (i == E.value) { + continue; + } + GodotBody3D *b = n[i]; + if (b->mode < PhysicsServer3D::BODY_MODE_RIGID) { + continue; + } + + if (!b->is_active()) { + b->set_active(true); + } + } + } +} + +void GodotBody3D::call_queries() { + Variant direct_state_variant = get_direct_state(); + + if (fi_callback_data) { + if (!fi_callback_data->callable.is_valid()) { + set_force_integration_callback(Callable()); + } else { + const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata }; + + Callable::CallError ce; + int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2; + Variant rv; + fi_callback_data->callable.callp(vp, argc, rv, ce); + } + } + + if (body_state_callback.is_valid()) { + body_state_callback.call(direct_state_variant); + } +} + +bool GodotBody3D::sleep_test(real_t p_step) { + if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + return true; + } else if (!can_sleep) { + return false; + } + + ERR_FAIL_NULL_V(get_space(), true); + + if (Math::abs(angular_velocity.length()) < get_space()->get_body_angular_velocity_sleep_threshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_threshold() * get_space()->get_body_linear_velocity_sleep_threshold()) { + still_time += p_step; + + return still_time > get_space()->get_body_time_to_sleep(); + } else { + still_time = 0; //maybe this should be set to 0 on set_active? + return false; + } +} + +void GodotBody3D::set_state_sync_callback(const Callable &p_callable) { + body_state_callback = p_callable; +} + +void GodotBody3D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { + if (p_callable.is_valid()) { + if (!fi_callback_data) { + fi_callback_data = memnew(ForceIntegrationCallbackData); + } + fi_callback_data->callable = p_callable; + fi_callback_data->udata = p_udata; + } else if (fi_callback_data) { + memdelete(fi_callback_data); + fi_callback_data = nullptr; + } +} + +GodotPhysicsDirectBodyState3D *GodotBody3D::get_direct_state() { + if (!direct_state) { + direct_state = memnew(GodotPhysicsDirectBodyState3D); + direct_state->body = this; + } + return direct_state; +} + +GodotBody3D::GodotBody3D() : + GodotCollisionObject3D(TYPE_BODY), + active_list(this), + mass_properties_update_list(this), + direct_state_query_list(this) { + _set_static(false); +} + +GodotBody3D::~GodotBody3D() { + if (fi_callback_data) { + memdelete(fi_callback_data); + } + if (direct_state) { + memdelete(direct_state); + } +} diff --git a/modules/godot_physics_3d/godot_body_3d.h b/modules/godot_physics_3d/godot_body_3d.h new file mode 100644 index 0000000000..81b668122a --- /dev/null +++ b/modules/godot_physics_3d/godot_body_3d.h @@ -0,0 +1,396 @@ +/**************************************************************************/ +/* godot_body_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BODY_3D_H +#define GODOT_BODY_3D_H + +#include "godot_area_3d.h" +#include "godot_collision_object_3d.h" + +#include "core/templates/vset.h" + +class GodotConstraint3D; +class GodotPhysicsDirectBodyState3D; + +class GodotBody3D : public GodotCollisionObject3D { + PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_RIGID; + + Vector3 linear_velocity; + Vector3 angular_velocity; + + Vector3 prev_linear_velocity; + Vector3 prev_angular_velocity; + + Vector3 constant_linear_velocity; + Vector3 constant_angular_velocity; + + Vector3 biased_linear_velocity; + Vector3 biased_angular_velocity; + real_t mass = 1.0; + real_t bounce = 0.0; + real_t friction = 1.0; + Vector3 inertia; + + PhysicsServer3D::BodyDampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE; + PhysicsServer3D::BodyDampMode angular_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE; + + real_t linear_damp = 0.0; + real_t angular_damp = 0.0; + + real_t total_linear_damp = 0.0; + real_t total_angular_damp = 0.0; + + real_t gravity_scale = 1.0; + + uint16_t locked_axis = 0; + + real_t _inv_mass = 1.0; + Vector3 _inv_inertia; // Relative to the principal axes of inertia + + // Relative to the local frame of reference + Basis principal_inertia_axes_local; + Vector3 center_of_mass_local; + + // In world orientation with local origin + Basis _inv_inertia_tensor; + Basis principal_inertia_axes; + Vector3 center_of_mass; + + bool calculate_inertia = true; + bool calculate_center_of_mass = true; + + Vector3 gravity; + + real_t still_time = 0.0; + + Vector3 applied_force; + Vector3 applied_torque; + + Vector3 constant_force; + Vector3 constant_torque; + + SelfList<GodotBody3D> active_list; + SelfList<GodotBody3D> mass_properties_update_list; + SelfList<GodotBody3D> direct_state_query_list; + + VSet<RID> exceptions; + bool omit_force_integration = false; + bool active = true; + + bool continuous_cd = false; + bool can_sleep = true; + bool first_time_kinematic = false; + + void _mass_properties_changed(); + virtual void _shapes_changed() override; + Transform3D new_transform; + + HashMap<GodotConstraint3D *, int> constraint_map; + + Vector<AreaCMP> areas; + + struct Contact { + Vector3 local_pos; + Vector3 local_normal; + Vector3 local_velocity_at_pos; + real_t depth = 0.0; + int local_shape = 0; + Vector3 collider_pos; + int collider_shape = 0; + ObjectID collider_instance_id; + RID collider; + Vector3 collider_velocity_at_pos; + Vector3 impulse; + }; + + Vector<Contact> contacts; //no contacts by default + int contact_count = 0; + + Callable body_state_callback; + + struct ForceIntegrationCallbackData { + Callable callable; + Variant udata; + }; + + ForceIntegrationCallbackData *fi_callback_data = nullptr; + + GodotPhysicsDirectBodyState3D *direct_state = nullptr; + + uint64_t island_step = 0; + + void _update_transform_dependent(); + + friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose + +public: + void set_state_sync_callback(const Callable &p_callable); + void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); + + GodotPhysicsDirectBodyState3D *get_direct_state(); + + _FORCE_INLINE_ void add_area(GodotArea3D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount += 1; + } else { + areas.ordered_insert(AreaCMP(p_area)); + } + } + + _FORCE_INLINE_ void remove_area(GodotArea3D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount -= 1; + if (areas[index].refCount < 1) { + areas.remove_at(index); + } + } + } + + _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { + contacts.resize(p_size); + contact_count = 0; + if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && p_size) { + set_active(true); + } + } + _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); } + + _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); } + _FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_local_velocity_at_pos, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse); + + _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } + _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } + _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } + _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } + _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraint_map.erase(p_constraint); } + const HashMap<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; } + _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } + + _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } + _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } + + _FORCE_INLINE_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; } + _FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; } + _FORCE_INLINE_ Vector3 get_center_of_mass_local() const { return center_of_mass_local; } + _FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } + + _FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; } + _FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; } + + _FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; } + _FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; } + + _FORCE_INLINE_ Vector3 get_prev_linear_velocity() const { return prev_linear_velocity; } + _FORCE_INLINE_ Vector3 get_prev_angular_velocity() const { return prev_angular_velocity; } + + _FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; } + _FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; } + + _FORCE_INLINE_ void apply_central_impulse(const Vector3 &p_impulse) { + linear_velocity += p_impulse * _inv_mass; + } + + _FORCE_INLINE_ void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) { + linear_velocity += p_impulse * _inv_mass; + angular_velocity += _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse)); + } + + _FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_impulse) { + angular_velocity += _inv_inertia_tensor.xform(p_impulse); + } + + _FORCE_INLINE_ void apply_bias_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3(), real_t p_max_delta_av = -1.0) { + biased_linear_velocity += p_impulse * _inv_mass; + if (p_max_delta_av != 0.0) { + Vector3 delta_av = _inv_inertia_tensor.xform((p_position - center_of_mass).cross(p_impulse)); + if (p_max_delta_av > 0 && delta_av.length() > p_max_delta_av) { + delta_av = delta_av.normalized() * p_max_delta_av; + } + biased_angular_velocity += delta_av; + } + } + + _FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_impulse) { + biased_angular_velocity += _inv_inertia_tensor.xform(p_impulse); + } + + _FORCE_INLINE_ void apply_central_force(const Vector3 &p_force) { + applied_force += p_force; + } + + _FORCE_INLINE_ void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) { + applied_force += p_force; + applied_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void apply_torque(const Vector3 &p_torque) { + applied_torque += p_torque; + } + + _FORCE_INLINE_ void add_constant_central_force(const Vector3 &p_force) { + constant_force += p_force; + } + + _FORCE_INLINE_ void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) { + constant_force += p_force; + constant_torque += (p_position - center_of_mass).cross(p_force); + } + + _FORCE_INLINE_ void add_constant_torque(const Vector3 &p_torque) { + constant_torque += p_torque; + } + + void set_constant_force(const Vector3 &p_force) { constant_force = p_force; } + Vector3 get_constant_force() const { return constant_force; } + + void set_constant_torque(const Vector3 &p_torque) { constant_torque = p_torque; } + Vector3 get_constant_torque() const { return constant_torque; } + + void set_active(bool p_active); + _FORCE_INLINE_ bool is_active() const { return active; } + + _FORCE_INLINE_ void wakeup() { + if ((!get_space()) || mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { + return; + } + set_active(true); + } + + void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value); + Variant get_param(PhysicsServer3D::BodyParameter p_param) const; + + void set_mode(PhysicsServer3D::BodyMode p_mode); + PhysicsServer3D::BodyMode get_mode() const; + + void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer3D::BodyState p_state) const; + + _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; } + _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } + + void set_space(GodotSpace3D *p_space) override; + + void update_mass_properties(); + void reset_mass_properties(); + + _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; } + _FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; } + _FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; } + _FORCE_INLINE_ real_t get_friction() const { return friction; } + _FORCE_INLINE_ real_t get_bounce() const { return bounce; } + + void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock); + bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const; + + void integrate_forces(real_t p_step); + void integrate_velocities(real_t p_step); + + _FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3 &rel_pos) const { + return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass); + } + + _FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const { + Vector3 r0 = p_pos - get_transform().origin - center_of_mass; + + Vector3 c0 = (r0).cross(p_normal); + + Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0); + + return _inv_mass + p_normal.dot(vec); + } + + _FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const { + return p_axis.dot(_inv_inertia_tensor.xform_inv(p_axis)); + } + + //void simulate_motion(const Transform3D& p_xform,real_t p_step); + void call_queries(); + void wakeup_neighbours(); + + bool sleep_test(real_t p_step); + + GodotBody3D(); + ~GodotBody3D(); +}; + +//add contact inline + +void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_local_velocity_at_pos, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse) { + int c_max = contacts.size(); + + if (c_max == 0) { + return; + } + + Contact *c = contacts.ptrw(); + + int idx = -1; + + if (contact_count < c_max) { + idx = contact_count++; + } else { + real_t least_depth = 1e20; + int least_deep = -1; + for (int i = 0; i < c_max; i++) { + if (i == 0 || c[i].depth < least_depth) { + least_deep = i; + least_depth = c[i].depth; + } + } + + if (least_deep >= 0 && least_depth < p_depth) { + idx = least_deep; + } + if (idx == -1) { + return; //none least deepe than this + } + } + + c[idx].local_pos = p_local_pos; + c[idx].local_normal = p_local_normal; + c[idx].local_velocity_at_pos = p_local_velocity_at_pos; + c[idx].depth = p_depth; + c[idx].local_shape = p_local_shape; + c[idx].collider_pos = p_collider_pos; + c[idx].collider_shape = p_collider_shape; + c[idx].collider_instance_id = p_collider_instance_id; + c[idx].collider = p_collider; + c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; + c[idx].impulse = p_impulse; +} + +#endif // GODOT_BODY_3D_H diff --git a/modules/godot_physics_3d/godot_body_direct_state_3d.cpp b/modules/godot_physics_3d/godot_body_direct_state_3d.cpp new file mode 100644 index 0000000000..0af746c68d --- /dev/null +++ b/modules/godot_physics_3d/godot_body_direct_state_3d.cpp @@ -0,0 +1,237 @@ +/**************************************************************************/ +/* godot_body_direct_state_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_body_direct_state_3d.h" + +#include "godot_body_3d.h" +#include "godot_space_3d.h" + +Vector3 GodotPhysicsDirectBodyState3D::get_total_gravity() const { + return body->gravity; +} + +real_t GodotPhysicsDirectBodyState3D::get_total_angular_damp() const { + return body->total_angular_damp; +} + +real_t GodotPhysicsDirectBodyState3D::get_total_linear_damp() const { + return body->total_linear_damp; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass() const { + return body->get_center_of_mass(); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass_local() const { + return body->get_center_of_mass_local(); +} + +Basis GodotPhysicsDirectBodyState3D::get_principal_inertia_axes() const { + return body->get_principal_inertia_axes(); +} + +real_t GodotPhysicsDirectBodyState3D::get_inverse_mass() const { + return body->get_inv_mass(); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_inverse_inertia() const { + return body->get_inv_inertia(); +} + +Basis GodotPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const { + return body->get_inv_inertia_tensor(); +} + +void GodotPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) { + body->wakeup(); + body->set_linear_velocity(p_velocity); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_linear_velocity() const { + return body->get_linear_velocity(); +} + +void GodotPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) { + body->wakeup(); + body->set_angular_velocity(p_velocity); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_angular_velocity() const { + return body->get_angular_velocity(); +} + +void GodotPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) { + body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); +} + +Transform3D GodotPhysicsDirectBodyState3D::get_transform() const { + return body->get_transform(); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_position) const { + return body->get_velocity_in_local_point(p_position); +} + +void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) { + body->wakeup(); + body->apply_central_impulse(p_impulse); +} + +void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { + body->wakeup(); + body->apply_impulse(p_impulse, p_position); +} + +void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) { + body->wakeup(); + body->apply_torque_impulse(p_impulse); +} + +void GodotPhysicsDirectBodyState3D::apply_central_force(const Vector3 &p_force) { + body->wakeup(); + body->apply_central_force(p_force); +} + +void GodotPhysicsDirectBodyState3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) { + body->wakeup(); + body->apply_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState3D::apply_torque(const Vector3 &p_torque) { + body->wakeup(); + body->apply_torque(p_torque); +} + +void GodotPhysicsDirectBodyState3D::add_constant_central_force(const Vector3 &p_force) { + body->wakeup(); + body->add_constant_central_force(p_force); +} + +void GodotPhysicsDirectBodyState3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) { + body->wakeup(); + body->add_constant_force(p_force, p_position); +} + +void GodotPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) { + body->wakeup(); + body->add_constant_torque(p_torque); +} + +void GodotPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) { + if (!p_force.is_zero_approx()) { + body->wakeup(); + } + body->set_constant_force(p_force); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_constant_force() const { + return body->get_constant_force(); +} + +void GodotPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) { + if (!p_torque.is_zero_approx()) { + body->wakeup(); + } + body->set_constant_torque(p_torque); +} + +Vector3 GodotPhysicsDirectBodyState3D::get_constant_torque() const { + return body->get_constant_torque(); +} + +void GodotPhysicsDirectBodyState3D::set_sleep_state(bool p_sleep) { + body->set_active(!p_sleep); +} + +bool GodotPhysicsDirectBodyState3D::is_sleeping() const { + return !body->is_active(); +} + +int GodotPhysicsDirectBodyState3D::get_contact_count() const { + return body->contact_count; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_pos; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_normal; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].impulse; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].local_velocity_at_pos; +} + +int GodotPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); + return body->contacts[p_contact_idx].local_shape; +} + +RID GodotPhysicsDirectBodyState3D::get_contact_collider(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); + return body->contacts[p_contact_idx].collider; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_collider_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_pos; +} + +ObjectID GodotPhysicsDirectBodyState3D::get_contact_collider_id(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); + return body->contacts[p_contact_idx].collider_instance_id; +} + +int GodotPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); + return body->contacts[p_contact_idx].collider_shape; +} + +Vector3 GodotPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const { + ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); + return body->contacts[p_contact_idx].collider_velocity_at_pos; +} + +PhysicsDirectSpaceState3D *GodotPhysicsDirectBodyState3D::get_space_state() { + return body->get_space()->get_direct_state(); +} + +real_t GodotPhysicsDirectBodyState3D::get_step() const { + return body->get_space()->get_last_step(); +} diff --git a/modules/godot_physics_3d/godot_body_direct_state_3d.h b/modules/godot_physics_3d/godot_body_direct_state_3d.h new file mode 100644 index 0000000000..8066050c9f --- /dev/null +++ b/modules/godot_physics_3d/godot_body_direct_state_3d.h @@ -0,0 +1,107 @@ +/**************************************************************************/ +/* godot_body_direct_state_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BODY_DIRECT_STATE_3D_H +#define GODOT_BODY_DIRECT_STATE_3D_H + +#include "servers/physics_server_3d.h" + +class GodotBody3D; + +class GodotPhysicsDirectBodyState3D : public PhysicsDirectBodyState3D { + GDCLASS(GodotPhysicsDirectBodyState3D, PhysicsDirectBodyState3D); + +public: + GodotBody3D *body = nullptr; + + virtual Vector3 get_total_gravity() const override; + virtual real_t get_total_angular_damp() const override; + virtual real_t get_total_linear_damp() const override; + + virtual Vector3 get_center_of_mass() const override; + virtual Vector3 get_center_of_mass_local() const override; + virtual Basis get_principal_inertia_axes() const override; + + virtual real_t get_inverse_mass() const override; + virtual Vector3 get_inverse_inertia() const override; + virtual Basis get_inverse_inertia_tensor() const override; + + virtual void set_linear_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_linear_velocity() const override; + + virtual void set_angular_velocity(const Vector3 &p_velocity) override; + virtual Vector3 get_angular_velocity() const override; + + virtual void set_transform(const Transform3D &p_transform) override; + virtual Transform3D get_transform() const override; + + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override; + + virtual void apply_central_impulse(const Vector3 &p_impulse) override; + virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; + virtual void apply_torque_impulse(const Vector3 &p_impulse) override; + + virtual void apply_central_force(const Vector3 &p_force) override; + virtual void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void apply_torque(const Vector3 &p_torque) override; + + virtual void add_constant_central_force(const Vector3 &p_force) override; + virtual void add_constant_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void add_constant_torque(const Vector3 &p_torque) override; + + virtual void set_constant_force(const Vector3 &p_force) override; + virtual Vector3 get_constant_force() const override; + + virtual void set_constant_torque(const Vector3 &p_torque) override; + virtual Vector3 get_constant_torque() const override; + + virtual void set_sleep_state(bool p_sleep) override; + virtual bool is_sleeping() const override; + + virtual int get_contact_count() const override; + + virtual Vector3 get_contact_local_position(int p_contact_idx) const override; + virtual Vector3 get_contact_local_normal(int p_contact_idx) const override; + virtual Vector3 get_contact_impulse(int p_contact_idx) const override; + virtual int get_contact_local_shape(int p_contact_idx) const override; + virtual Vector3 get_contact_local_velocity_at_position(int p_contact_idx) const override; + + virtual RID get_contact_collider(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_position(int p_contact_idx) const override; + virtual ObjectID get_contact_collider_id(int p_contact_idx) const override; + virtual int get_contact_collider_shape(int p_contact_idx) const override; + virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override; + + virtual PhysicsDirectSpaceState3D *get_space_state() override; + + virtual real_t get_step() const override; +}; + +#endif // GODOT_BODY_DIRECT_STATE_3D_H diff --git a/modules/godot_physics_3d/godot_body_pair_3d.cpp b/modules/godot_physics_3d/godot_body_pair_3d.cpp new file mode 100644 index 0000000000..d689271ac7 --- /dev/null +++ b/modules/godot_physics_3d/godot_body_pair_3d.cpp @@ -0,0 +1,990 @@ +/**************************************************************************/ +/* godot_body_pair_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_body_pair_3d.h" + +#include "godot_collision_solver_3d.h" +#include "godot_space_3d.h" + +#include "core/os/os.h" + +#define MIN_VELOCITY 0.0001 +#define MAX_BIAS_ROTATION (Math_PI / 8) + +void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) { + GodotBodyPair3D *pair = static_cast<GodotBodyPair3D *>(p_userdata); + pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal); +} + +void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) { + Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A); + Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B); + + int new_index = contact_count; + + ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1)); + + Contact contact; + contact.index_A = p_index_A; + contact.index_B = p_index_B; + contact.local_A = local_A; + contact.local_B = local_B; + contact.normal = (p_point_A - p_point_B).normalized(); + contact.used = true; + + // Attempt to determine if the contact will be reused. + real_t contact_recycle_radius = space->get_contact_recycle_radius(); + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && + c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + c = contact; + return; + } + } + + // Figure out if the contact amount must be reduced to fit the new contact. + if (new_index == MAX_CONTACTS) { + // Remove the contact with the minimum depth. + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; + + int least_deep = -1; + real_t min_depth; + + // Start with depth for new contact. + { + Vector3 global_A = basis_A.xform(contact.local_A); + Vector3 global_B = basis_B.xform(contact.local_B) + offset_B; + + Vector3 axis = global_A - global_B; + min_depth = axis.dot(contact.normal); + } + + for (int i = 0; i < contact_count; i++) { + const Contact &c = contacts[i]; + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; + + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < min_depth) { + min_depth = depth; + least_deep = i; + } + } + + if (least_deep > -1) { + // Replace the least deep contact by the new one. + contacts[least_deep] = contact; + } + + return; + } + + contacts[new_index] = contact; + contact_count++; +} + +void GodotBodyPair3D::validate_contacts() { + // Make sure to erase contacts that are no longer valid. + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + + bool erase = false; + if (!c.used) { + // Was left behind in previous frame. + erase = true; + } else { + c.used = false; + + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } + + if (erase) { + // Contact no longer needed, remove. + if ((i + 1) < contact_count) { + // Swap with the last one. + SWAP(contacts[i], contacts[contact_count - 1]); + } + + i--; + contact_count--; + } + } +} + +// `_test_ccd` prevents tunneling by slowing down a high velocity body that is about to collide so +// that next frame it will be at an appropriate location to collide (i.e. slight overlap). +// WARNING: The way velocity is adjusted down to cause a collision means the momentum will be +// weaker than it should for a bounce! +// Process: Only proceed if body A's motion is high relative to its size. +// Cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does. +// Adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it. +bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) { + GodotShape3D *shape_A_ptr = p_A->get_shape(p_shape_A); + + Vector3 motion = p_A->get_linear_velocity() * p_step; + real_t mlen = motion.length(); + if (mlen < CMP_EPSILON) { + return false; + } + + Vector3 mnormal = motion / mlen; + + real_t min = 0.0, max = 0.0; + shape_A_ptr->project_range(mnormal, p_xform_A, min, max); + + // Did it move enough in this direction to even attempt raycast? + // Let's say it should move more than 1/3 the size of the object in that axis. + bool fast_object = mlen > (max - min) * 0.3; + if (!fast_object) { + return false; // moving slow enough that there's no chance of tunneling. + } + + // A is moving fast enough that tunneling might occur. See if it's really about to collide. + + // Roughly predict body B's position in the next frame (ignoring collisions). + Transform3D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step); + + // Support points are the farthest forward points on A in the direction of the motion vector. + // i.e. the candidate points of which one should hit B first if any collision does occur. + static const int max_supports = 16; + Vector3 supports_A[max_supports]; + int support_count_A; + GodotShape3D::FeatureType support_type_A; + // Convert mnormal into body A's local xform because get_supports requires (and returns) local coordinates. + shape_A_ptr->get_supports(p_xform_A.basis.xform_inv(mnormal).normalized(), max_supports, supports_A, support_count_A, support_type_A); + + // Cast a segment from each support point of A in the motion direction. + int segment_support_idx = -1; + float segment_hit_length = FLT_MAX; + Vector3 segment_hit_local; + for (int i = 0; i < support_count_A; i++) { + supports_A[i] = p_xform_A.xform(supports_A[i]); + + Vector3 from = supports_A[i]; + Vector3 to = from + motion; + + Transform3D from_inv = predicted_xform_B.affine_inverse(); + + // Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast. + // At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd. + // But it still works out. + Vector3 local_from = from_inv.xform(from - motion * 0.1); + Vector3 local_to = from_inv.xform(to); + + Vector3 rpos, rnorm; + int fi = -1; + if (p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, fi, true)) { + float hit_length = local_from.distance_to(rpos); + if (hit_length < segment_hit_length) { + segment_support_idx = i; + segment_hit_length = hit_length; + segment_hit_local = rpos; + } + } + } + + if (segment_support_idx == -1) { + // There was no hit. Since the segment is the length of per-frame motion, this means the bodies will not + // actually collide yet on next frame. We'll probably check again next frame once they're closer. + return false; + } + + Vector3 hitpos = predicted_xform_B.xform(segment_hit_local); + + real_t newlen = hitpos.distance_to(supports_A[segment_support_idx]); + // Adding 1% of body length to the distance between collision and support point + // should cause body A's support point to arrive just within B's collider next frame. + newlen += (max - min) * 0.01; + // FIXME: This doesn't always work well when colliding with a triangle face of a trimesh shape. + + p_A->set_linear_velocity((mnormal * newlen) / p_step); + + return true; +} + +real_t combine_bounce(GodotBody3D *A, GodotBody3D *B) { + return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); +} + +real_t combine_friction(GodotBody3D *A, GodotBody3D *B) { + return ABS(MIN(A->get_friction(), B->get_friction())); +} + +bool GodotBodyPair3D::setup(real_t p_step) { + check_ccd = false; + + if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { + collided = false; + return false; + } + + collide_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && A->collides_with(B); + collide_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && B->collides_with(A); + + report_contacts_only = false; + if (!collide_A && !collide_B) { + if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + + offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); + + validate_contacts(); + + const Vector3 &offset_A = A->get_transform().get_origin(); + Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3()); + Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform3D xform_Bu = B->get_transform(); + xform_Bu.origin -= offset_A; + Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + GodotShape3D *shape_A_ptr = A->get_shape(shape_A); + GodotShape3D *shape_B_ptr = B->get_shape(shape_B); + + collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); + + if (!collided) { + if (A->is_continuous_collision_detection_enabled() && collide_A) { + check_ccd = true; + return true; + } + + if (B->is_continuous_collision_detection_enabled() && collide_B) { + check_ccd = true; + return true; + } + + return false; + } + + return true; +} + +bool GodotBodyPair3D::pre_solve(real_t p_step) { + if (!collided) { + if (check_ccd) { + const Vector3 &offset_A = A->get_transform().get_origin(); + Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3()); + Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform3D xform_Bu = B->get_transform(); + xform_Bu.origin -= offset_A; + Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + if (A->is_continuous_collision_detection_enabled() && collide_A) { + _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); + } + + if (B->is_continuous_collision_detection_enabled() && collide_B) { + _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); + } + } + + return false; + } + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + real_t bias = 0.8; + + GodotShape3D *shape_A_ptr = A->get_shape(shape_A); + GodotShape3D *shape_B_ptr = B->get_shape(shape_B); + + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { + if (shape_A_ptr->get_custom_bias() == 0) { + bias = shape_B_ptr->get_custom_bias(); + } else if (shape_B_ptr->get_custom_bias() == 0) { + bias = shape_A_ptr->get_custom_bias(); + } else { + bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5; + } + } + + real_t inv_dt = 1.0 / p_step; + + bool do_process = false; + + const Vector3 &offset_A = A->get_transform().get_origin(); + + const Basis &basis_A = A->get_transform().basis; + const Basis &basis_B = B->get_transform().basis; + + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis; + const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + c.active = false; + + Vector3 global_A = basis_A.xform(c.local_A); + Vector3 global_B = basis_B.xform(c.local_B) + offset_B; + + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth <= 0.0) { + continue; + } + +#ifdef DEBUG_ENABLED + if (space->is_debugging_contacts()) { + space->add_debug_contact(global_A + offset_A); + space->add_debug_contact(global_B + offset_A); + } +#endif + + c.rA = global_A - A->get_center_of_mass(); + c.rB = global_B - B->get_center_of_mass() - offset_B; + + // Precompute normal mass, tangent mass, and bias. + Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal)); + Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal)); + real_t kNormal = inv_mass_A + inv_mass_B; + kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB)); + c.mass_normal = 1.0f / kNormal; + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + c.depth = depth; + + Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; + + c.acc_impulse -= j_vec; + + // contact query reporting... + + if (A->can_report_contacts() || B->can_report_contacts()) { + Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity(); + Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity(); + + if (A->can_report_contacts()) { + A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse); + } + + if (B->can_report_contacts()) { + B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, -c.acc_impulse); + } + } + + if (report_contacts_only) { + collided = false; + continue; + } + + c.active = true; + do_process = true; + + if (collide_A) { + A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass()); + } + if (collide_B) { + B->apply_impulse(j_vec, c.rB + B->get_center_of_mass()); + } + + c.bounce = combine_bounce(A, B); + if (c.bounce) { + Vector3 crA = A->get_prev_angular_velocity().cross(c.rA); + Vector3 crB = B->get_prev_angular_velocity().cross(c.rB); + Vector3 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA; + c.bounce = c.bounce * dv.dot(c.normal); + } + } + + return do_process; +} + +void GodotBodyPair3D::solve(real_t p_step) { + if (!collided) { + return; + } + + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis; + const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis; + + real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0; + real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0; + + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (!c.active) { + continue; + } + + c.active = false; //try to deactivate, will activate itself if still needed + + //bias impulse + + Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA); + Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB); + Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + real_t vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn = (-vbn + c.bias) * c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + if (collide_A) { + A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av); + } + if (collide_B) { + B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av); + } + + crbA = A->get_biased_angular_velocity().cross(c.rA); + crbB = B->get_biased_angular_velocity().cross(c.rB); + dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); + + if (collide_A) { + A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f); + } + if (collide_B) { + B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f); + } + } + + c.active = true; + } + + Vector3 crA = A->get_angular_velocity().cross(c.rA); + Vector3 crB = B->get_angular_velocity().cross(c.rB); + Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + + //normal impulse + real_t vn = dv.dot(c.normal); + + if (Math::abs(vn) > MIN_VELOCITY) { + real_t jn = -(c.bounce + vn) * c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); + + if (collide_A) { + A->apply_impulse(-j, c.rA + A->get_center_of_mass()); + } + if (collide_B) { + B->apply_impulse(j, c.rB + B->get_center_of_mass()); + } + c.acc_impulse -= j; + + c.active = true; + } + + //friction impulse + + real_t friction = combine_friction(A, B); + + Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA); + Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB); + + Vector3 dtv = lvB - lvA; + real_t tn = c.normal.dot(dtv); + + // tangential velocity + Vector3 tv = dtv - c.normal * tn; + real_t tvl = tv.length(); + + if (tvl > MIN_VELOCITY) { + tv /= tvl; + + Vector3 temp1 = inv_inertia_tensor_A.xform(c.rA.cross(tv)); + Vector3 temp2 = inv_inertia_tensor_B.xform(c.rB.cross(tv)); + + real_t t = -tvl / (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); + + Vector3 jt = t * tv; + + Vector3 jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse += jt; + + real_t fi_len = c.acc_tangent_impulse.length(); + real_t jtMax = c.acc_normal_impulse * friction; + + if (fi_len > CMP_EPSILON && fi_len > jtMax) { + c.acc_tangent_impulse *= jtMax / fi_len; + } + + jt = c.acc_tangent_impulse - jtOld; + + if (collide_A) { + A->apply_impulse(-jt, c.rA + A->get_center_of_mass()); + } + if (collide_B) { + B->apply_impulse(jt, c.rB + B->get_center_of_mass()); + } + c.acc_impulse -= jt; + + c.active = true; + } + } +} + +GodotBodyPair3D::GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B) : + GodotBodyContact3D(_arr, 2) { + A = p_A; + B = p_B; + shape_A = p_shape_A; + shape_B = p_shape_B; + space = A->get_space(); + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotBodyPair3D::~GodotBodyPair3D() { + A->remove_constraint(this); + B->remove_constraint(this); +} + +void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) { + GodotBodySoftBodyPair3D *pair = static_cast<GodotBodySoftBodyPair3D *>(p_userdata); + pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal); +} + +void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) { + Vector3 local_A = body->get_inv_transform().xform(p_point_A); + Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B); + + Contact contact; + contact.index_A = p_index_A; + contact.index_B = p_index_B; + contact.local_A = local_A; + contact.local_B = local_B; + contact.normal = (normal.dot((p_point_A - p_point_B)) < 0 ? -normal : normal); + contact.used = true; + + // Attempt to determine if the contact will be reused. + real_t contact_recycle_radius = space->get_contact_recycle_radius(); + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + if (c.index_B == p_index_B) { + if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) && + c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) { + contact.acc_normal_impulse = c.acc_normal_impulse; + contact.acc_bias_impulse = c.acc_bias_impulse; + contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass; + contact.acc_tangent_impulse = c.acc_tangent_impulse; + } + c = contact; + return; + } + } + + contacts.push_back(contact); +} + +void GodotBodySoftBodyPair3D::validate_contacts() { + // Make sure to erase contacts that are no longer valid. + real_t max_separation = space->get_contact_max_separation(); + real_t max_separation2 = max_separation * max_separation; + + const Transform3D &transform_A = body->get_transform(); + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + + bool erase = false; + if (!c.used) { + // Was left behind in previous frame. + erase = true; + } else { + c.used = false; + + Vector3 global_A = transform_A.xform(c.local_A); + Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) { + erase = true; + } + } + + if (erase) { + // Contact no longer needed, remove. + if ((contact_index + 1) < contact_count) { + // Swap with the last one. + SWAP(c, contacts[contact_count - 1]); + } + + contact_index--; + contact_count--; + } + } + + contacts.resize(contact_count); +} + +bool GodotBodySoftBodyPair3D::setup(real_t p_step) { + if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { + collided = false; + return false; + } + + body_collides = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && body->collides_with(soft_body); + soft_body_collides = soft_body->collides_with(body); + + if (!body_collides && !soft_body_collides) { + if (body->get_max_contacts_reported() > 0) { + report_contacts_only = true; + } else { + collided = false; + return false; + } + } + + const Transform3D &xform_Au = body->get_transform(); + Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape); + + Transform3D xform_Bu = soft_body->get_transform(); + Transform3D xform_B = xform_Bu * soft_body->get_shape_transform(0); + + validate_contacts(); + + GodotShape3D *shape_A_ptr = body->get_shape(body_shape); + GodotShape3D *shape_B_ptr = soft_body->get_shape(0); + + collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); + + return collided; +} + +bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) { + if (!collided) { + return false; + } + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + real_t bias = space->get_contact_bias(); + + GodotShape3D *shape_A_ptr = body->get_shape(body_shape); + + if (shape_A_ptr->get_custom_bias()) { + bias = shape_A_ptr->get_custom_bias(); + } + + real_t inv_dt = 1.0 / p_step; + + bool do_process = false; + + const Transform3D &transform_A = body->get_transform(); + + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis; + + real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0; + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + c.active = false; + + real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0; + if ((node_inv_mass == 0.0) && (body_inv_mass == 0.0)) { + continue; + } + + Vector3 global_A = transform_A.xform(c.local_A); + Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B; + Vector3 axis = global_A - global_B; + real_t depth = axis.dot(c.normal); + + if (depth <= 0.0) { + continue; + } + +#ifdef DEBUG_ENABLED + if (space->is_debugging_contacts()) { + space->add_debug_contact(global_A); + space->add_debug_contact(global_B); + } +#endif + + c.rA = global_A - transform_A.origin - body->get_center_of_mass(); + c.rB = global_B; + + // Precompute normal mass, tangent mass, and bias. + Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal)); + real_t kNormal = body_inv_mass + node_inv_mass; + kNormal += c.normal.dot(inertia_A.cross(c.rA)); + c.mass_normal = 1.0f / kNormal; + + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + c.depth = depth; + + Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; + if (body_collides) { + body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass()); + } + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, j_vec); + } + c.acc_impulse -= j_vec; + + if (body->can_report_contacts()) { + Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity(); + Vector3 crB = soft_body->get_node_velocity(c.index_B); + body->add_contact(global_A, -c.normal, depth, body_shape, crA, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crB, c.acc_impulse); + } + if (report_contacts_only) { + collided = false; + continue; + } + + c.active = true; + do_process = true; + + if (body_collides) { + body->set_active(true); + } + + c.bounce = body->get_bounce(); + + if (c.bounce) { + Vector3 crA = body->get_angular_velocity().cross(c.rA); + Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA; + + // Normal impulse. + c.bounce = c.bounce * dv.dot(c.normal); + } + } + + return do_process; +} + +void GodotBodySoftBodyPair3D::solve(real_t p_step) { + if (!collided) { + return; + } + + const real_t max_bias_av = MAX_BIAS_ROTATION / p_step; + + Basis zero_basis; + zero_basis.set_zero(); + + const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis; + + real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0; + + uint32_t contact_count = contacts.size(); + for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) { + Contact &c = contacts[contact_index]; + if (!c.active) { + continue; + } + + c.active = false; + + real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0; + + // Bias impulse. + Vector3 crbA = body->get_biased_angular_velocity().cross(c.rA); + Vector3 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA; + + real_t vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn = (-vbn + c.bias) * c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + if (body_collides) { + body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av); + } + if (soft_body_collides) { + soft_body->apply_node_bias_impulse(c.index_B, jb); + } + + crbA = body->get_biased_angular_velocity().cross(c.rA); + dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA; + + vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) { + real_t jbn_com = (-vbn + c.bias) / (body_inv_mass + node_inv_mass); + real_t jbnOld_com = c.acc_bias_impulse_center_of_mass; + c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f); + + Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com); + + if (body_collides) { + body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f); + } + if (soft_body_collides) { + soft_body->apply_node_bias_impulse(c.index_B, jb_com); + } + } + + c.active = true; + } + + Vector3 crA = body->get_angular_velocity().cross(c.rA); + Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA; + + // Normal impulse. + real_t vn = dv.dot(c.normal); + + if (Math::abs(vn) > MIN_VELOCITY) { + real_t jn = -(c.bounce + vn) * c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + Vector3 j = c.normal * (c.acc_normal_impulse - jnOld); + + if (body_collides) { + body->apply_impulse(-j, c.rA + body->get_center_of_mass()); + } + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, j); + } + c.acc_impulse -= j; + + c.active = true; + } + + // Friction impulse. + real_t friction = body->get_friction(); + + Vector3 lvA = body->get_linear_velocity() + body->get_angular_velocity().cross(c.rA); + Vector3 lvB = soft_body->get_node_velocity(c.index_B); + Vector3 dtv = lvB - lvA; + + real_t tn = c.normal.dot(dtv); + + // Tangential velocity. + Vector3 tv = dtv - c.normal * tn; + real_t tvl = tv.length(); + + if (tvl > MIN_VELOCITY) { + tv /= tvl; + + Vector3 temp1 = body_inv_inertia_tensor.xform(c.rA.cross(tv)); + + real_t t = -tvl / (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA))); + + Vector3 jt = t * tv; + + Vector3 jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse += jt; + + real_t fi_len = c.acc_tangent_impulse.length(); + real_t jtMax = c.acc_normal_impulse * friction; + + if (fi_len > CMP_EPSILON && fi_len > jtMax) { + c.acc_tangent_impulse *= jtMax / fi_len; + } + + jt = c.acc_tangent_impulse - jtOld; + + if (body_collides) { + body->apply_impulse(-jt, c.rA + body->get_center_of_mass()); + } + if (soft_body_collides) { + soft_body->apply_node_impulse(c.index_B, jt); + } + c.acc_impulse -= jt; + + c.active = true; + } + } +} + +GodotBodySoftBodyPair3D::GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B) : + GodotBodyContact3D(&body, 1) { + body = p_A; + soft_body = p_B; + body_shape = p_shape_A; + space = p_A->get_space(); + body->add_constraint(this, 0); + soft_body->add_constraint(this); +} + +GodotBodySoftBodyPair3D::~GodotBodySoftBodyPair3D() { + body->remove_constraint(this); + soft_body->remove_constraint(this); +} diff --git a/modules/godot_physics_3d/godot_body_pair_3d.h b/modules/godot_physics_3d/godot_body_pair_3d.h new file mode 100644 index 0000000000..a8f5180dd5 --- /dev/null +++ b/modules/godot_physics_3d/godot_body_pair_3d.h @@ -0,0 +1,147 @@ +/**************************************************************************/ +/* godot_body_pair_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BODY_PAIR_3D_H +#define GODOT_BODY_PAIR_3D_H + +#include "godot_body_3d.h" +#include "godot_constraint_3d.h" +#include "godot_soft_body_3d.h" + +#include "core/templates/local_vector.h" + +class GodotBodyContact3D : public GodotConstraint3D { +protected: + struct Contact { + Vector3 position; + Vector3 normal; + int index_A = 0, index_B = 0; + Vector3 local_A, local_B; + Vector3 acc_impulse; // accumulated impulse - only one of the object's impulse is needed as impulse_a == -impulse_b + real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn) + Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) + real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb) + real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com + real_t mass_normal = 0.0; + real_t bias = 0.0; + real_t bounce = 0.0; + + real_t depth = 0.0; + bool active = false; + bool used = false; + Vector3 rA, rB; // Offset in world orientation with respect to center of mass + }; + + Vector3 sep_axis; + bool collided = false; + bool check_ccd = false; + + GodotSpace3D *space = nullptr; + + GodotBodyContact3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) : + GodotConstraint3D(p_body_ptr, p_body_count) { + } +}; + +class GodotBodyPair3D : public GodotBodyContact3D { + enum { + MAX_CONTACTS = 4 + }; + + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = { nullptr, nullptr }; + }; + + int shape_A = 0; + int shape_B = 0; + + bool collide_A = false; + bool collide_B = false; + + bool report_contacts_only = false; + + Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection + + Contact contacts[MAX_CONTACTS]; + int contact_count = 0; + + static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata); + + void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal); + + void validate_contacts(); + bool _test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B); + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B); + ~GodotBodyPair3D(); +}; + +class GodotBodySoftBodyPair3D : public GodotBodyContact3D { + GodotBody3D *body = nullptr; + GodotSoftBody3D *soft_body = nullptr; + + int body_shape = 0; + + bool body_collides = false; + bool soft_body_collides = false; + + bool report_contacts_only = false; + + LocalVector<Contact> contacts; + + static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata); + + void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal); + + void validate_contacts(); + +public: + virtual bool setup(real_t p_step) override; + virtual bool pre_solve(real_t p_step) override; + virtual void solve(real_t p_step) override; + + virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const override { return soft_body; } + virtual int get_soft_body_count() const override { return 1; } + + GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B); + ~GodotBodySoftBodyPair3D(); +}; + +#endif // GODOT_BODY_PAIR_3D_H diff --git a/modules/godot_physics_3d/godot_broad_phase_3d.cpp b/modules/godot_physics_3d/godot_broad_phase_3d.cpp new file mode 100644 index 0000000000..ebd11fb51f --- /dev/null +++ b/modules/godot_physics_3d/godot_broad_phase_3d.cpp @@ -0,0 +1,36 @@ +/**************************************************************************/ +/* godot_broad_phase_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_broad_phase_3d.h" + +GodotBroadPhase3D::CreateFunction GodotBroadPhase3D::create_func = nullptr; + +GodotBroadPhase3D::~GodotBroadPhase3D() { +} diff --git a/modules/godot_physics_3d/godot_broad_phase_3d.h b/modules/godot_physics_3d/godot_broad_phase_3d.h new file mode 100644 index 0000000000..f70321be64 --- /dev/null +++ b/modules/godot_physics_3d/godot_broad_phase_3d.h @@ -0,0 +1,72 @@ +/**************************************************************************/ +/* godot_broad_phase_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BROAD_PHASE_3D_H +#define GODOT_BROAD_PHASE_3D_H + +#include "core/math/aabb.h" +#include "core/math/math_funcs.h" + +class GodotCollisionObject3D; + +class GodotBroadPhase3D { +public: + typedef GodotBroadPhase3D *(*CreateFunction)(); + + static CreateFunction create_func; + + typedef uint32_t ID; + + typedef void *(*PairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_userdata); + typedef void (*UnpairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_userdata); + + // 0 is an invalid ID + virtual ID create(GodotCollisionObject3D *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0; + virtual void move(ID p_id, const AABB &p_aabb) = 0; + virtual void set_static(ID p_id, bool p_static) = 0; + virtual void remove(ID p_id) = 0; + + virtual GodotCollisionObject3D *get_object(ID p_id) const = 0; + virtual bool is_static(ID p_id) const = 0; + virtual int get_subindex(ID p_id) const = 0; + + virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0; + + virtual void update() = 0; + + virtual ~GodotBroadPhase3D(); +}; + +#endif // GODOT_BROAD_PHASE_3D_H diff --git a/modules/godot_physics_3d/godot_broad_phase_3d_bvh.cpp b/modules/godot_physics_3d/godot_broad_phase_3d_bvh.cpp new file mode 100644 index 0000000000..0faa56b52e --- /dev/null +++ b/modules/godot_physics_3d/godot_broad_phase_3d_bvh.cpp @@ -0,0 +1,128 @@ +/**************************************************************************/ +/* godot_broad_phase_3d_bvh.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_broad_phase_3d_bvh.h" + +#include "godot_collision_object_3d.h" + +GodotBroadPhase3DBVH::ID GodotBroadPhase3DBVH::create(GodotCollisionObject3D *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { + uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC; + uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC); + ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care? + return oid + 1; +} + +void GodotBroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) { + ERR_FAIL_COND(!p_id); + bvh.move(p_id - 1, p_aabb); +} + +void GodotBroadPhase3DBVH::set_static(ID p_id, bool p_static) { + ERR_FAIL_COND(!p_id); + uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC; + uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC); + bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false); +} + +void GodotBroadPhase3DBVH::remove(ID p_id) { + ERR_FAIL_COND(!p_id); + bvh.erase(p_id - 1); +} + +GodotCollisionObject3D *GodotBroadPhase3DBVH::get_object(ID p_id) const { + ERR_FAIL_COND_V(!p_id, nullptr); + GodotCollisionObject3D *it = bvh.get(p_id - 1); + ERR_FAIL_NULL_V(it, nullptr); + return it; +} + +bool GodotBroadPhase3DBVH::is_static(ID p_id) const { + ERR_FAIL_COND_V(!p_id, false); + uint32_t tree_id = bvh.get_tree_id(p_id - 1); + return tree_id == 0; +} + +int GodotBroadPhase3DBVH::get_subindex(ID p_id) const { + ERR_FAIL_COND_V(!p_id, 0); + return bvh.get_subindex(p_id - 1); +} + +int GodotBroadPhase3DBVH::cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_point(p_point, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); +} + +int GodotBroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_segment(p_from, p_to, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); +} + +int GodotBroadPhase3DBVH::cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) { + return bvh.cull_aabb(p_aabb, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices); +} + +void *GodotBroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B) { + GodotBroadPhase3DBVH *bpo = static_cast<GodotBroadPhase3DBVH *>(self); + if (!bpo->pair_callback) { + return nullptr; + } + + return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); +} + +void GodotBroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B, void *pairdata) { + GodotBroadPhase3DBVH *bpo = static_cast<GodotBroadPhase3DBVH *>(self); + if (!bpo->unpair_callback) { + return; + } + + bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); +} + +void GodotBroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { + pair_callback = p_pair_callback; + pair_userdata = p_userdata; +} + +void GodotBroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { + unpair_callback = p_unpair_callback; + unpair_userdata = p_userdata; +} + +void GodotBroadPhase3DBVH::update() { + bvh.update(); +} + +GodotBroadPhase3D *GodotBroadPhase3DBVH::_create() { + return memnew(GodotBroadPhase3DBVH); +} + +GodotBroadPhase3DBVH::GodotBroadPhase3DBVH() { + bvh.set_pair_callback(_pair_callback, this); + bvh.set_unpair_callback(_unpair_callback, this); +} diff --git a/modules/godot_physics_3d/godot_broad_phase_3d_bvh.h b/modules/godot_physics_3d/godot_broad_phase_3d_bvh.h new file mode 100644 index 0000000000..63968dea64 --- /dev/null +++ b/modules/godot_physics_3d/godot_broad_phase_3d_bvh.h @@ -0,0 +1,100 @@ +/**************************************************************************/ +/* godot_broad_phase_3d_bvh.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_BROAD_PHASE_3D_BVH_H +#define GODOT_BROAD_PHASE_3D_BVH_H + +#include "godot_broad_phase_3d.h" + +#include "core/math/bvh.h" + +class GodotBroadPhase3DBVH : public GodotBroadPhase3D { + template <typename T> + class UserPairTestFunction { + public: + static bool user_pair_check(const T *p_a, const T *p_b) { + // return false if no collision, decided by masks etc + return p_a->interacts_with(p_b); + } + }; + + template <typename T> + class UserCullTestFunction { + public: + static bool user_cull_check(const T *p_a, const T *p_b) { + return true; + } + }; + + enum Tree { + TREE_STATIC = 0, + TREE_DYNAMIC = 1, + }; + + enum TreeFlag { + TREE_FLAG_STATIC = 1 << TREE_STATIC, + TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC, + }; + + BVH_Manager<GodotCollisionObject3D, 2, true, 128, UserPairTestFunction<GodotCollisionObject3D>, UserCullTestFunction<GodotCollisionObject3D>> bvh; + + static void *_pair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int); + static void _unpair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int, void *); + + PairCallback pair_callback = nullptr; + void *pair_userdata = nullptr; + UnpairCallback unpair_callback = nullptr; + void *unpair_userdata = nullptr; + +public: + // 0 is an invalid ID + virtual ID create(GodotCollisionObject3D *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) override; + virtual void move(ID p_id, const AABB &p_aabb) override; + virtual void set_static(ID p_id, bool p_static) override; + virtual void remove(ID p_id) override; + + virtual GodotCollisionObject3D *get_object(ID p_id) const override; + virtual bool is_static(ID p_id) const override; + virtual int get_subindex(ID p_id) const override; + + virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) override; + virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) override; + virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) override; + + virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) override; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) override; + + virtual void update() override; + + static GodotBroadPhase3D *_create(); + GodotBroadPhase3DBVH(); +}; + +#endif // GODOT_BROAD_PHASE_3D_BVH_H diff --git a/modules/godot_physics_3d/godot_collision_object_3d.cpp b/modules/godot_physics_3d/godot_collision_object_3d.cpp new file mode 100644 index 0000000000..283614a43d --- /dev/null +++ b/modules/godot_physics_3d/godot_collision_object_3d.cpp @@ -0,0 +1,242 @@ +/**************************************************************************/ +/* godot_collision_object_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_collision_object_3d.h" + +#include "godot_physics_server_3d.h" +#include "godot_space_3d.h" + +void GodotCollisionObject3D::add_shape(GodotShape3D *p_shape, const Transform3D &p_transform, bool p_disabled) { + Shape s; + s.shape = p_shape; + s.xform = p_transform; + s.xform_inv = s.xform.affine_inverse(); + s.bpid = 0; //needs update + s.disabled = p_disabled; + shapes.push_back(s); + p_shape->add_owner(this); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject3D::set_shape(int p_index, GodotShape3D *p_shape) { + ERR_FAIL_INDEX(p_index, shapes.size()); + shapes[p_index].shape->remove_owner(this); + shapes.write[p_index].shape = p_shape; + + p_shape->add_owner(this); + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject3D::set_shape_transform(int p_index, const Transform3D &p_transform) { + ERR_FAIL_INDEX(p_index, shapes.size()); + + shapes.write[p_index].xform = p_transform; + shapes.write[p_index].xform_inv = p_transform.affine_inverse(); + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject3D::set_shape_disabled(int p_idx, bool p_disabled) { + ERR_FAIL_INDEX(p_idx, shapes.size()); + + GodotCollisionObject3D::Shape &shape = shapes.write[p_idx]; + if (shape.disabled == p_disabled) { + return; + } + + shape.disabled = p_disabled; + + if (!space) { + return; + } + + if (p_disabled && shape.bpid != 0) { + space->get_broadphase()->remove(shape.bpid); + shape.bpid = 0; + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } else if (!p_disabled && shape.bpid == 0) { + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } + } +} + +void GodotCollisionObject3D::remove_shape(GodotShape3D *p_shape) { + //remove a shape, all the times it appears + for (int i = 0; i < shapes.size(); i++) { + if (shapes[i].shape == p_shape) { + remove_shape(i); + i--; + } + } +} + +void GodotCollisionObject3D::remove_shape(int p_index) { + //remove anything from shape to be erased to end, so subindices don't change + ERR_FAIL_INDEX(p_index, shapes.size()); + for (int i = p_index; i < shapes.size(); i++) { + if (shapes[i].bpid == 0) { + continue; + } + //should never get here with a null owner + space->get_broadphase()->remove(shapes[i].bpid); + shapes.write[i].bpid = 0; + } + shapes[p_index].shape->remove_owner(this); + shapes.remove_at(p_index); + + if (!pending_shape_update_list.in_list()) { + GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list); + } +} + +void GodotCollisionObject3D::_set_static(bool p_static) { + if (_static == p_static) { + return; + } + _static = p_static; + + if (!space) { + return; + } + for (int i = 0; i < get_shape_count(); i++) { + const Shape &s = shapes[i]; + if (s.bpid > 0) { + space->get_broadphase()->set_static(s.bpid, _static); + } + } +} + +void GodotCollisionObject3D::_unregister_shapes() { + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid > 0) { + space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } +} + +void GodotCollisionObject3D::_update_shapes() { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + AABB shape_aabb = s.shape->get_aabb(); + Transform3D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05); + s.aabb_cache = shape_aabb; + + Vector3 scale = xform.get_basis().get_scale(); + s.area_cache = s.shape->get_volume() * scale.x * scale.y * scale.z; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject3D::_update_shapes_with_motion(const Vector3 &p_motion) { + if (!space) { + return; + } + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.disabled) { + continue; + } + + //not quite correct, should compute the next matrix.. + AABB shape_aabb = s.shape->get_aabb(); + Transform3D xform = transform * s.xform; + shape_aabb = xform.xform(shape_aabb); + shape_aabb.merge_with(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion + s.aabb_cache = shape_aabb; + + if (s.bpid == 0) { + s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static); + space->get_broadphase()->set_static(s.bpid, _static); + } + + space->get_broadphase()->move(s.bpid, shape_aabb); + } +} + +void GodotCollisionObject3D::_set_space(GodotSpace3D *p_space) { + GodotSpace3D *old_space = space; + space = p_space; + + if (old_space) { + old_space->remove_object(this); + + for (int i = 0; i < shapes.size(); i++) { + Shape &s = shapes.write[i]; + if (s.bpid) { + old_space->get_broadphase()->remove(s.bpid); + s.bpid = 0; + } + } + } + + if (space) { + space->add_object(this); + _update_shapes(); + } +} + +void GodotCollisionObject3D::_shape_changed() { + _update_shapes(); + _shapes_changed(); +} + +GodotCollisionObject3D::GodotCollisionObject3D(Type p_type) : + pending_shape_update_list(this) { + type = p_type; +} diff --git a/modules/godot_physics_3d/godot_collision_object_3d.h b/modules/godot_physics_3d/godot_collision_object_3d.h new file mode 100644 index 0000000000..bf28bcc45a --- /dev/null +++ b/modules/godot_physics_3d/godot_collision_object_3d.h @@ -0,0 +1,194 @@ +/**************************************************************************/ +/* godot_collision_object_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_COLLISION_OBJECT_3D_H +#define GODOT_COLLISION_OBJECT_3D_H + +#include "godot_broad_phase_3d.h" +#include "godot_shape_3d.h" + +#include "core/templates/self_list.h" +#include "servers/physics_server_3d.h" + +#ifdef DEBUG_ENABLED +#define MAX_OBJECT_DISTANCE 3.1622776601683791e+18 + +#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE) +#endif + +class GodotSpace3D; + +class GodotCollisionObject3D : public GodotShapeOwner3D { +public: + enum Type { + TYPE_AREA, + TYPE_BODY, + TYPE_SOFT_BODY, + }; + +private: + Type type; + RID self; + ObjectID instance_id; + uint32_t collision_layer = 1; + uint32_t collision_mask = 1; + real_t collision_priority = 1.0; + + struct Shape { + Transform3D xform; + Transform3D xform_inv; + GodotBroadPhase3D::ID bpid; + AABB aabb_cache; //for rayqueries + real_t area_cache = 0.0; + GodotShape3D *shape = nullptr; + bool disabled = false; + }; + + Vector<Shape> shapes; + GodotSpace3D *space = nullptr; + Transform3D transform; + Transform3D inv_transform; + bool _static = true; + + SelfList<GodotCollisionObject3D> pending_shape_update_list; + + void _update_shapes(); + +protected: + void _update_shapes_with_motion(const Vector3 &p_motion); + void _unregister_shapes(); + + _FORCE_INLINE_ void _set_transform(const Transform3D &p_transform, bool p_update_shapes = true) { +#ifdef DEBUG_ENABLED + + ERR_FAIL_COND_MSG(p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2, "Object went too far away (more than '" + itos(MAX_OBJECT_DISTANCE) + "' units from origin)."); +#endif + + transform = p_transform; + if (p_update_shapes) { + _update_shapes(); + } + } + _FORCE_INLINE_ void _set_inv_transform(const Transform3D &p_transform) { inv_transform = p_transform; } + void _set_static(bool p_static); + + virtual void _shapes_changed() = 0; + void _set_space(GodotSpace3D *p_space); + + bool ray_pickable = true; + + GodotCollisionObject3D(Type p_type); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; } + _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } + + void _shape_changed() override; + + _FORCE_INLINE_ Type get_type() const { return type; } + void add_shape(GodotShape3D *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false); + void set_shape(int p_index, GodotShape3D *p_shape); + void set_shape_transform(int p_index, const Transform3D &p_transform); + _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } + _FORCE_INLINE_ GodotShape3D *get_shape(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].shape; + } + _FORCE_INLINE_ const Transform3D &get_shape_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform; + } + _FORCE_INLINE_ const Transform3D &get_shape_inv_transform(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].xform_inv; + } + _FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].aabb_cache; + } + _FORCE_INLINE_ real_t get_shape_area(int p_index) const { + CRASH_BAD_INDEX(p_index, shapes.size()); + return shapes[p_index].area_cache; + } + + _FORCE_INLINE_ const Transform3D &get_transform() const { return transform; } + _FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ GodotSpace3D *get_space() const { return space; } + + _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } + _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } + + void set_shape_disabled(int p_idx, bool p_disabled); + _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { + ERR_FAIL_INDEX_V(p_idx, shapes.size(), false); + return shapes[p_idx].disabled; + } + + _FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { + collision_layer = p_layer; + _shape_changed(); + } + _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } + + _FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { + collision_mask = p_mask; + _shape_changed(); + } + _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } + + _FORCE_INLINE_ void set_collision_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority <= 0, "Priority must be greater than 0."); + collision_priority = p_priority; + _shape_changed(); + } + _FORCE_INLINE_ real_t get_collision_priority() const { return collision_priority; } + + _FORCE_INLINE_ bool collides_with(GodotCollisionObject3D *p_other) const { + return p_other->collision_layer & collision_mask; + } + + _FORCE_INLINE_ bool interacts_with(const GodotCollisionObject3D *p_other) const { + return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; + } + + void remove_shape(GodotShape3D *p_shape) override; + void remove_shape(int p_index); + + virtual void set_space(GodotSpace3D *p_space) = 0; + + _FORCE_INLINE_ bool is_static() const { return _static; } + + virtual ~GodotCollisionObject3D() {} +}; + +#endif // GODOT_COLLISION_OBJECT_3D_H diff --git a/modules/godot_physics_3d/godot_collision_solver_3d.cpp b/modules/godot_physics_3d/godot_collision_solver_3d.cpp new file mode 100644 index 0000000000..db48111eea --- /dev/null +++ b/modules/godot_physics_3d/godot_collision_solver_3d.cpp @@ -0,0 +1,589 @@ +/**************************************************************************/ +/* godot_collision_solver_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_collision_solver_3d.h" + +#include "godot_collision_solver_3d_sat.h" +#include "godot_soft_body_3d.h" + +#include "gjk_epa.h" + +#define collision_solver sat_calculate_penetration +//#define collision_solver gjk_epa_calculate_penetration + +bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { + const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + return false; + } + Plane p = p_transform_A.xform(world_boundary->get_plane()); + + static const int max_supports = 16; + Vector3 supports[max_supports]; + int support_count; + GodotShape3D::FeatureType support_type = GodotShape3D::FeatureType::FEATURE_POINT; + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + + if (support_type == GodotShape3D::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } + + bool found = false; + + for (int i = 0; i < support_count; i++) { + supports[i] += p_margin * supports[i].normalized(); + supports[i] = p_transform_B.xform(supports[i]); + if (p.distance_to(supports[i]) >= 0) { + continue; + } + found = true; + + Vector3 support_A = p.project(supports[i]); + + if (p_result_callback) { + if (p_swap_result) { + Vector3 normal = (support_A - supports[i]).normalized(); + p_result_callback(supports[i], 0, support_A, 0, normal, p_userdata); + } else { + Vector3 normal = (supports[i] - support_A).normalized(); + p_result_callback(support_A, 0, supports[i], 0, normal, p_userdata); + } + } + } + + return found; +} + +bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { + const GodotSeparationRayShape3D *ray = static_cast<const GodotSeparationRayShape3D *>(p_shape_A); + + Vector3 from = p_transform_A.origin; + Vector3 to = from + p_transform_A.basis.get_column(2) * (ray->get_length() + p_margin); + Vector3 support_A = to; + + Transform3D ai = p_transform_B.affine_inverse(); + + from = ai.xform(from); + to = ai.xform(to); + + Vector3 p, n; + int fi = -1; + if (!p_shape_B->intersect_segment(from, to, p, n, fi, true)) { + return false; + } + + // Discard contacts when the ray is fully contained inside the shape. + if (n == Vector3()) { + return false; + } + + // Discard contacts in the wrong direction. + if (n.dot(from - to) < CMP_EPSILON) { + return false; + } + + Vector3 support_B = p_transform_B.xform(p); + if (ray->get_slide_on_slope()) { + Vector3 global_n = ai.basis.xform_inv(n).normalized(); + support_B = support_A + (support_B - support_A).length() * global_n; + } + + if (p_result_callback) { + Vector3 normal = (support_B - support_A).normalized(); + if (p_swap_result) { + p_result_callback(support_B, 0, support_A, 0, -normal, p_userdata); + } else { + p_result_callback(support_A, 0, support_B, 0, normal, p_userdata); + } + } + return true; +} + +struct _SoftBodyContactCollisionInfo { + int node_index = 0; + GodotCollisionSolver3D::CallbackResult result_callback = nullptr; + void *userdata = nullptr; + bool swap_result = false; + int contact_count = 0; +}; + +void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) { + _SoftBodyContactCollisionInfo &cinfo = *(static_cast<_SoftBodyContactCollisionInfo *>(p_userdata)); + + ++cinfo.contact_count; + + if (!cinfo.result_callback) { + return; + } + + if (cinfo.swap_result) { + cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, -normal, cinfo.userdata); + } else { + cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, normal, cinfo.userdata); + } +} + +struct _SoftBodyQueryInfo { + GodotSoftBody3D *soft_body = nullptr; + const GodotShape3D *shape_A = nullptr; + const GodotShape3D *shape_B = nullptr; + Transform3D transform_A; + Transform3D node_transform; + _SoftBodyContactCollisionInfo contact_info; +#ifdef DEBUG_ENABLED + int node_query_count = 0; + int convex_query_count = 0; +#endif +}; + +bool GodotCollisionSolver3D::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) { + _SoftBodyQueryInfo &query_cinfo = *(static_cast<_SoftBodyQueryInfo *>(p_userdata)); + + Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index); + + Transform3D transform_B; + transform_B.origin = query_cinfo.node_transform.xform(node_position); + + query_cinfo.contact_info.node_index = p_node_index; + bool collided = solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info); + +#ifdef DEBUG_ENABLED + ++query_cinfo.node_query_count; +#endif + + // Stop at first collision if contacts are not needed. + return (collided && !query_cinfo.contact_info.result_callback); +} + +bool GodotCollisionSolver3D::soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex) { + _SoftBodyQueryInfo &query_cinfo = *(static_cast<_SoftBodyQueryInfo *>(p_userdata)); + + query_cinfo.shape_A = p_convex; + + // Calculate AABB for internal soft body query (in world space). + AABB shape_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + + real_t smin, smax; + p_convex->project_range(axis, query_cinfo.transform_A, smin, smax); + + shape_aabb.position[i] = smin; + shape_aabb.size[i] = smax - smin; + } + + shape_aabb.grow_by(query_cinfo.soft_body->get_collision_margin()); + + query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); + + bool collided = (query_cinfo.contact_info.contact_count > 0); + +#ifdef DEBUG_ENABLED + ++query_cinfo.convex_query_count; +#endif + + // Stop at first collision if contacts are not needed. + return (collided && !query_cinfo.contact_info.result_callback); +} + +bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { + const GodotSoftBodyShape3D *soft_body_shape_B = static_cast<const GodotSoftBodyShape3D *>(p_shape_B); + + GodotSoftBody3D *soft_body = soft_body_shape_B->get_soft_body(); + const Transform3D &world_to_local = soft_body->get_inv_transform(); + + const real_t collision_margin = soft_body->get_collision_margin(); + + GodotSphereShape3D sphere_shape; + sphere_shape.set_data(collision_margin); + + _SoftBodyQueryInfo query_cinfo; + query_cinfo.contact_info.result_callback = p_result_callback; + query_cinfo.contact_info.userdata = p_userdata; + query_cinfo.contact_info.swap_result = p_swap_result; + query_cinfo.soft_body = soft_body; + query_cinfo.node_transform = p_transform_B * world_to_local; + query_cinfo.shape_A = p_shape_A; + query_cinfo.transform_A = p_transform_A; + query_cinfo.shape_B = &sphere_shape; + + if (p_shape_A->is_concave()) { + // In case of concave shape, query convex shapes first. + const GodotConcaveShape3D *concave_shape_A = static_cast<const GodotConcaveShape3D *>(p_shape_A); + + AABB soft_body_aabb = soft_body->get_bounds(); + soft_body_aabb.grow_by(collision_margin); + + // Calculate AABB for internal concave shape query (in local space). + AABB local_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis(p_transform_A.basis.get_column(i)); + real_t axis_scale = 1.0 / axis.length(); + + real_t smin = soft_body_aabb.position[i]; + real_t smax = smin + soft_body_aabb.size[i]; + + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo, true); + } else { + AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb()); + shape_aabb.grow_by(collision_margin); + + soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo); + } + + return (query_cinfo.contact_info.contact_count > 0); +} + +struct _ConcaveCollisionInfo { + const Transform3D *transform_A = nullptr; + const GodotShape3D *shape_A = nullptr; + const Transform3D *transform_B = nullptr; + GodotCollisionSolver3D::CallbackResult result_callback = nullptr; + void *userdata = nullptr; + bool swap_result = false; + bool collided = false; + int aabb_tests = 0; + int collisions = 0; + bool tested = false; + real_t margin_A = 0.0f; + real_t margin_B = 0.0f; + Vector3 close_A; + Vector3 close_B; +}; + +bool GodotCollisionSolver3D::concave_callback(void *p_userdata, GodotShape3D *p_convex) { + _ConcaveCollisionInfo &cinfo = *(static_cast<_ConcaveCollisionInfo *>(p_userdata)); + cinfo.aabb_tests++; + + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, nullptr, cinfo.margin_A, cinfo.margin_B); + if (!collided) { + return false; + } + + cinfo.collided = true; + cinfo.collisions++; + + // Stop at first collision if contacts are not needed. + return !cinfo.result_callback; +} + +bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { + const GodotConcaveShape3D *concave_B = static_cast<const GodotConcaveShape3D *>(p_shape_B); + + _ConcaveCollisionInfo cinfo; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.result_callback = p_result_callback; + cinfo.userdata = p_userdata; + cinfo.swap_result = p_swap_result; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.margin_A = p_margin_A; + cinfo.margin_B = p_margin_B; + + cinfo.aabb_tests = 0; + + Transform3D rel_transform = p_transform_A; + rel_transform.origin -= p_transform_B.origin; + + //quickly compute a local AABB + + AABB local_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis(p_transform_B.basis.get_column(i)); + real_t axis_scale = 1.0 / axis.length(); + axis *= axis_scale; + + real_t smin = 0.0, smax = 0.0; + p_shape_A->project_range(axis, rel_transform, smin, smax); + smin -= p_margin_A; + smax += p_margin_A; + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_B->cull(local_aabb, concave_callback, &cinfo, false); + + return cinfo.collided; +} + +bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { + PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); + PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); + bool concave_A = p_shape_A->is_concave(); + bool concave_B = p_shape_B->is_concave(); + + bool swap = false; + + if (type_A > type_B) { + SWAP(type_A, type_B); + SWAP(concave_A, concave_B); + swap = true; + } + + if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + WARN_PRINT_ONCE("Collisions between world boundaries are not supported."); + return false; + } + if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + WARN_PRINT_ONCE("Collisions between world boundaries and rays are not supported."); + return false; + } + if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { + WARN_PRINT_ONCE("Collisions between world boundaries and soft bodies are not supported."); + return false; + } + + if (swap) { + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A); + } else { + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_B); + } + + } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) { + WARN_PRINT_ONCE("Collisions between rays are not supported."); + return false; + } + + if (swap) { + return solve_separation_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B); + } else { + return solve_separation_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A); + } + + } else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) { + if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) { + WARN_PRINT_ONCE("Collisions between soft bodies are not supported."); + return false; + } + + if (swap) { + return solve_soft_body(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + } else { + return solve_soft_body(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + } + + } else if (concave_B) { + if (concave_A) { + WARN_PRINT_ONCE("Collisions between two concave shapes are not supported."); + return false; + } + + if (!swap) { + return solve_concave(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B); + } else { + return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B); + } + + } else { + return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B); + } +} + +bool GodotCollisionSolver3D::concave_distance_callback(void *p_userdata, GodotShape3D *p_convex) { + _ConcaveCollisionInfo &cinfo = *(static_cast<_ConcaveCollisionInfo *>(p_userdata)); + cinfo.aabb_tests++; + + Vector3 close_A, close_B; + cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B); + + if (cinfo.collided) { + // No need to process any more result. + return true; + } + + if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) { + cinfo.close_A = close_A; + cinfo.close_B = close_B; + cinfo.tested = true; + } + + cinfo.collisions++; + return false; +} + +bool GodotCollisionSolver3D::solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { + const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A); + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + return false; + } + Plane p = p_transform_A.xform(world_boundary->get_plane()); + + static const int max_supports = 16; + Vector3 supports[max_supports]; + int support_count; + GodotShape3D::FeatureType support_type; + Vector3 support_direction = p_transform_B.basis.xform_inv(-p.normal).normalized(); + + p_shape_B->get_supports(support_direction, max_supports, supports, support_count, support_type); + + if (support_count == 0) { // This is a poor man's way to detect shapes that don't implement get_supports, such as GodotMotionShape3D. + Vector3 support_B = p_transform_B.xform(p_shape_B->get_support(support_direction)); + r_point_A = p.project(support_B); + r_point_B = support_B; + bool collided = p.distance_to(support_B) <= 0; + return collided; + } + + if (support_type == GodotShape3D::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } + + bool collided = false; + Vector3 closest; + real_t closest_d = 0; + + for (int i = 0; i < support_count; i++) { + supports[i] = p_transform_B.xform(supports[i]); + real_t d = p.distance_to(supports[i]); + if (i == 0 || d < closest_d) { + closest = supports[i]; + closest_d = d; + if (d <= 0) { + collided = true; + } + } + } + + r_point_A = p.project(closest); + r_point_B = closest; + + return collided; +} + +bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { + if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { + Vector3 a, b; + bool col = solve_distance_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b); + r_point_A = b; + r_point_B = a; + return !col; + + } else if (p_shape_B->is_concave()) { + if (p_shape_A->is_concave()) { + return false; + } + + const GodotConcaveShape3D *concave_B = static_cast<const GodotConcaveShape3D *>(p_shape_B); + + _ConcaveCollisionInfo cinfo; + cinfo.transform_A = &p_transform_A; + cinfo.shape_A = p_shape_A; + cinfo.transform_B = &p_transform_B; + cinfo.result_callback = nullptr; + cinfo.userdata = nullptr; + cinfo.swap_result = false; + cinfo.collided = false; + cinfo.collisions = 0; + cinfo.aabb_tests = 0; + cinfo.tested = false; + + Transform3D rel_transform = p_transform_A; + rel_transform.origin -= p_transform_B.origin; + + //quickly compute a local AABB + + bool use_cc_hint = p_concave_hint != AABB(); + AABB cc_hint_aabb; + if (use_cc_hint) { + cc_hint_aabb = p_concave_hint; + cc_hint_aabb.position -= p_transform_B.origin; + } + + AABB local_aabb; + for (int i = 0; i < 3; i++) { + Vector3 axis(p_transform_B.basis.get_column(i)); + real_t axis_scale = ((real_t)1.0) / axis.length(); + axis *= axis_scale; + + real_t smin, smax; + + if (use_cc_hint) { + cc_hint_aabb.project_range_in_plane(Plane(axis), smin, smax); + } else { + p_shape_A->project_range(axis, rel_transform, smin, smax); + } + + smin *= axis_scale; + smax *= axis_scale; + + local_aabb.position[i] = smin; + local_aabb.size[i] = smax - smin; + } + + concave_B->cull(local_aabb, concave_distance_callback, &cinfo, false); + if (!cinfo.collided) { + r_point_A = cinfo.close_A; + r_point_B = cinfo.close_B; + } + + return !cinfo.collided; + } else { + return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis.. + } +} diff --git a/modules/godot_physics_3d/godot_collision_solver_3d.h b/modules/godot_physics_3d/godot_collision_solver_3d.h new file mode 100644 index 0000000000..36ea79576e --- /dev/null +++ b/modules/godot_physics_3d/godot_collision_solver_3d.h @@ -0,0 +1,57 @@ +/**************************************************************************/ +/* godot_collision_solver_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_COLLISION_SOLVER_3D_H +#define GODOT_COLLISION_SOLVER_3D_H + +#include "godot_shape_3d.h" + +class GodotCollisionSolver3D { +public: + typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata); + +private: + static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata); + static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata); + static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex); + static bool concave_callback(void *p_userdata, GodotShape3D *p_convex); + static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); + static bool solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); + static bool solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool concave_distance_callback(void *p_userdata, GodotShape3D *p_convex); + static bool solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); + +public: + static bool solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); + static bool solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr); +}; + +#endif // GODOT_COLLISION_SOLVER_3D_H diff --git a/modules/godot_physics_3d/godot_collision_solver_3d_sat.cpp b/modules/godot_physics_3d/godot_collision_solver_3d_sat.cpp new file mode 100644 index 0000000000..c53c8481f4 --- /dev/null +++ b/modules/godot_physics_3d/godot_collision_solver_3d_sat.cpp @@ -0,0 +1,2417 @@ +/**************************************************************************/ +/* godot_collision_solver_3d_sat.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_collision_solver_3d_sat.h" + +#include "gjk_epa.h" + +#include "core/math/geometry_3d.h" + +#define fallback_collision_solver gjk_epa_calculate_penetration + +#define _BACKFACE_NORMAL_THRESHOLD -0.0002 + +// Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders. + +/* + * Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +struct _CollectorCallback { + GodotCollisionSolver3D::CallbackResult callback = nullptr; + void *userdata = nullptr; + bool swap = false; + bool collided = false; + Vector3 normal; + Vector3 *prev_axis = nullptr; + + _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B, Vector3 p_normal) { + if (p_normal.dot(p_point_B - p_point_A) < 0) + p_normal = -p_normal; + if (swap) { + callback(p_point_B, 0, p_point_A, 0, -p_normal, userdata); + } else { + callback(p_point_A, 0, p_point_B, 0, p_normal, userdata); + } + } +}; + +typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *); + +static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 1); +#endif + + p_callback->call(*p_points_A, *p_points_B, p_callback->normal); +} + +static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 2); +#endif + + Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B); + p_callback->call(*p_points_A, closest_B, p_callback->normal); +} + +static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B < 3); +#endif + + Plane plane(p_points_B[0], p_points_B[1], p_points_B[2]); + Vector3 closest_B = plane.project(*p_points_A); + p_callback->call(*p_points_A, closest_B, plane.get_normal()); +} + +static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + Plane plane(p_points_B[0], p_points_B[1], p_points_B[2]); + Vector3 closest_B = plane.project(*p_points_A); + p_callback->call(*p_points_A, closest_B, plane.get_normal()); +} + +static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix +#endif + + Vector3 rel_A = p_points_A[1] - p_points_A[0]; + Vector3 rel_B = p_points_B[1] - p_points_B[0]; + + Vector3 c = rel_A.cross(rel_B).cross(rel_B); + + if (Math::is_zero_approx(rel_A.dot(c))) { + // should handle somehow.. + //ERR_PRINT("TODO FIX"); + //return; + + Vector3 axis = rel_A.normalized(); //make an axis + Vector3 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]); + Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]); + + //sort all 4 points in axis + real_t dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; + + SortArray<real_t> sa; + sa.sort(dvec, 4); + + //use the middle ones as contacts + p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1], p_callback->normal); + p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2], p_callback->normal); + + return; + } + + real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c); + + if (d < 0.0) { + d = 0.0; + } else if (d > 1.0) { + d = 1.0; + } + + Vector3 closest_A = p_points_A[0] + rel_A * d; + Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(closest_A, p_points_B); + // The normal should be perpendicular to both edges. + Vector3 normal = rel_A.cross(rel_B); + real_t normal_len = normal.length(); + if (normal_len > 1e-3) + normal /= normal_len; + else + normal = p_callback->normal; + p_callback->call(closest_A, closest_B, normal); +} + +static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 2); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_normal, circle_B_pos); + + static const int max_clip = 2; + Vector3 contact_points[max_clip]; + int num_points = 0; + + // Project edge point in circle plane. + const Vector3 &edge_A_1 = p_points_A[0]; + Vector3 proj_point_1 = circle_plane.project(edge_A_1); + + Vector3 dist_vec = proj_point_1 - circle_B_pos; + real_t dist_sq = dist_vec.length_squared(); + + // Point 1 is inside disk, add as contact point. + if (dist_sq <= circle_B_radius * circle_B_radius) { + contact_points[num_points] = edge_A_1; + ++num_points; + } + + const Vector3 &edge_A_2 = p_points_A[1]; + Vector3 proj_point_2 = circle_plane.project(edge_A_2); + + Vector3 dist_vec_2 = proj_point_2 - circle_B_pos; + real_t dist_sq_2 = dist_vec_2.length_squared(); + + // Point 2 is inside disk, add as contact point. + if (dist_sq_2 <= circle_B_radius * circle_B_radius) { + contact_points[num_points] = edge_A_2; + ++num_points; + } + + if (num_points < 2) { + Vector3 line_vec = proj_point_2 - proj_point_1; + real_t line_length_sq = line_vec.length_squared(); + + // Create a quadratic formula of the form ax^2 + bx + c = 0 + real_t a, b, c; + + a = line_length_sq; + b = 2.0 * dist_vec.dot(line_vec); + c = dist_sq - circle_B_radius * circle_B_radius; + + // Solve for t. + real_t sqrtterm = b * b - 4.0 * a * c; + + // If the term we intend to square root is less than 0 then the answer won't be real, + // so the line doesn't intersect. + if (sqrtterm >= 0) { + sqrtterm = Math::sqrt(sqrtterm); + + Vector3 edge_dir = edge_A_2 - edge_A_1; + + real_t fraction_1 = (-b - sqrtterm) / (2.0 * a); + if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) { + Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir; + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_1; + ++num_points; + } + + real_t fraction_2 = (-b + sqrtterm) / (2.0 * a); + if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) { + Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir; + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_2; + ++num_points; + } + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B, circle_plane.get_normal()); + } +} + +static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 2); + ERR_FAIL_COND(p_point_count_B < 3); +#endif + + static const int max_clip = 32; + + Vector3 _clipbuf1[max_clip]; + Vector3 _clipbuf2[max_clip]; + Vector3 *clipbuf_src = _clipbuf1; + Vector3 *clipbuf_dst = _clipbuf2; + int clipbuf_len = p_point_count_A; + + // copy A points to clipbuf_src + for (int i = 0; i < p_point_count_A; i++) { + clipbuf_src[i] = p_points_A[i]; + } + + Plane plane_B(p_points_B[0], p_points_B[1], p_points_B[2]); + + // go through all of B points + for (int i = 0; i < p_point_count_B; i++) { + int i_n = (i + 1) % p_point_count_B; + + Vector3 edge0_B = p_points_B[i]; + Vector3 edge1_B = p_points_B[i_n]; + + Vector3 clip_normal = (edge0_B - edge1_B).cross(plane_B.normal).normalized(); + // make a clip plane + + Plane clip(clip_normal, edge0_B); + // avoid double clip if A is edge + int dst_idx = 0; + bool edge = clipbuf_len == 2; + for (int j = 0; j < clipbuf_len; j++) { + int j_n = (j + 1) % clipbuf_len; + + Vector3 edge0_A = clipbuf_src[j]; + Vector3 edge1_A = clipbuf_src[j_n]; + + real_t dist0 = clip.distance_to(edge0_A); + real_t dist1 = clip.distance_to(edge1_A); + + if (dist0 <= 0) { // behind plane + + ERR_FAIL_COND(dst_idx >= max_clip); + clipbuf_dst[dst_idx++] = clipbuf_src[j]; + } + + // check for different sides and non coplanar + //if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) { + if ((dist0 * dist1) < 0 && !(edge && j)) { + // calculate intersection + Vector3 rel = edge1_A - edge0_A; + real_t den = clip.normal.dot(rel); + real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den; + Vector3 inters = edge0_A + rel * dist; + + ERR_FAIL_COND(dst_idx >= max_clip); + clipbuf_dst[dst_idx] = inters; + dst_idx++; + } + } + + clipbuf_len = dst_idx; + SWAP(clipbuf_src, clipbuf_dst); + } + + // generate contacts + //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]); + + for (int i = 0; i < clipbuf_len; i++) { + real_t d = plane_B.distance_to(clipbuf_src[i]); + + Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d; + + if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(clipbuf_src[i], closest_B, plane_B.get_normal()); + } +} + +static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + // Clip face with circle segments. + static const int circle_segments = 8; + Vector3 circle_points[circle_segments]; + + real_t angle_delta = 2.0 * Math_PI / circle_segments; + + for (int i = 0; i < circle_segments; ++i) { + Vector3 point_pos = circle_B_pos; + point_pos += circle_B_line_1 * Math::cos(i * angle_delta); + point_pos += circle_B_line_2 * Math::sin(i * angle_delta); + circle_points[i] = point_pos; + } + + _generate_contacts_face_face(p_points_A, p_point_count_A, circle_points, circle_segments, p_callback); + + // Clip face with circle plane. + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_normal, circle_B_pos); + + static const int max_clip = 32; + Vector3 contact_points[max_clip]; + int num_points = 0; + + for (int i = 0; i < p_point_count_A; i++) { + int i_n = (i + 1) % p_point_count_A; + + const Vector3 &edge0_A = p_points_A[i]; + const Vector3 &edge1_A = p_points_A[i_n]; + + real_t dist0 = circle_plane.distance_to(edge0_A); + real_t dist1 = circle_plane.distance_to(edge1_A); + + // First point in front of plane, generate contact point. + if (dist0 * circle_plane.d >= 0) { + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = edge0_A; + ++num_points; + } + + // Points on different sides, generate contact point. + if (dist0 * dist1 < 0) { + // calculate intersection + Vector3 rel = edge1_A - edge0_A; + real_t den = circle_plane.normal.dot(rel); + real_t dist = -(circle_plane.normal.dot(edge0_A) - circle_plane.d) / den; + Vector3 inters = edge0_A + rel * dist; + + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = inters; + ++num_points; + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B, circle_plane.get_normal()); + } +} + +static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_A_pos = p_points_A[0]; + Vector3 circle_A_line_1 = p_points_A[1] - circle_A_pos; + Vector3 circle_A_line_2 = p_points_A[2] - circle_A_pos; + + real_t circle_A_radius = circle_A_line_1.length(); + Vector3 circle_A_normal = circle_A_line_1.cross(circle_A_line_2).normalized(); + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + static const int max_clip = 4; + Vector3 contact_points[max_clip]; + int num_points = 0; + + Vector3 centers_diff = circle_B_pos - circle_A_pos; + Vector3 norm_proj = circle_A_normal.dot(centers_diff) * circle_A_normal; + Vector3 comp_proj = centers_diff - norm_proj; + real_t proj_dist = comp_proj.length(); + if (!Math::is_zero_approx(proj_dist)) { + comp_proj /= proj_dist; + if ((proj_dist > circle_A_radius - circle_B_radius) && (proj_dist > circle_B_radius - circle_A_radius)) { + // Circles are overlapping, use the 2 points of intersection as contacts. + real_t radius_a_sqr = circle_A_radius * circle_A_radius; + real_t radius_b_sqr = circle_B_radius * circle_B_radius; + real_t d_sqr = proj_dist * proj_dist; + real_t s = (1.0 + (radius_a_sqr - radius_b_sqr) / d_sqr) * 0.5; + real_t h = Math::sqrt(MAX(radius_a_sqr - d_sqr * s * s, 0.0)); + Vector3 midpoint = circle_A_pos + s * comp_proj * proj_dist; + Vector3 h_vec = h * circle_A_normal.cross(comp_proj); + + Vector3 point_A = midpoint + h_vec; + contact_points[num_points] = point_A; + ++num_points; + + point_A = midpoint - h_vec; + contact_points[num_points] = point_A; + ++num_points; + + // Add 2 points from circle A and B along the line between the centers. + point_A = circle_A_pos + comp_proj * circle_A_radius; + contact_points[num_points] = point_A; + ++num_points; + + point_A = circle_B_pos - comp_proj * circle_B_radius - norm_proj; + contact_points[num_points] = point_A; + ++num_points; + } // Otherwise one circle is inside the other one, use 3 arbitrary equidistant points. + } // Otherwise circles are concentric, use 3 arbitrary equidistant points. + + if (num_points == 0) { + // Generate equidistant points. + if (circle_A_radius < circle_B_radius) { + // Circle A inside circle B. + for (int i = 0; i < 3; ++i) { + Vector3 circle_A_point = circle_A_pos; + circle_A_point += circle_A_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_A_point += circle_A_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } else { + // Circle B inside circle A. + for (int i = 0; i < 3; ++i) { + Vector3 circle_B_point = circle_B_pos; + circle_B_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_B_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + Vector3 circle_A_point = circle_B_point - norm_proj; + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } + } + + Plane circle_B_plane(circle_B_normal, circle_B_pos); + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_B_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_B_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B, circle_B_plane.get_normal()); + } +} + +static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, GodotShape3D::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, GodotShape3D::FeatureType p_feature_type_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 1); + ERR_FAIL_COND(p_point_count_B < 1); +#endif + + static const GenerateContactsFunc generate_contacts_func_table[4][4] = { + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + _generate_contacts_point_face, + _generate_contacts_point_circle, + }, + { + nullptr, + _generate_contacts_edge_edge, + _generate_contacts_face_face, + _generate_contacts_edge_circle, + }, + { + nullptr, + nullptr, + _generate_contacts_face_face, + _generate_contacts_face_circle, + }, + { + nullptr, + nullptr, + nullptr, + _generate_contacts_circle_circle, + }, + }; + + int pointcount_B; + int pointcount_A; + const Vector3 *points_A; + const Vector3 *points_B; + int version_A; + int version_B; + + if (p_feature_type_A > p_feature_type_B) { + //swap + p_callback->swap = !p_callback->swap; + p_callback->normal = -p_callback->normal; + + pointcount_B = p_point_count_A; + pointcount_A = p_point_count_B; + points_A = p_points_B; + points_B = p_points_A; + version_A = p_feature_type_B; + version_B = p_feature_type_A; + } else { + pointcount_B = p_point_count_B; + pointcount_A = p_point_count_A; + points_A = p_points_A; + points_B = p_points_B; + version_A = p_feature_type_A; + version_B = p_feature_type_B; + } + + GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; + ERR_FAIL_NULL(contacts_func); + contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback); +} + +template <typename ShapeA, typename ShapeB, bool withMargin = false> +class SeparatorAxisTest { + const ShapeA *shape_A = nullptr; + const ShapeB *shape_B = nullptr; + const Transform3D *transform_A = nullptr; + const Transform3D *transform_B = nullptr; + real_t best_depth = 1e15; + _CollectorCallback *callback = nullptr; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + Vector3 separator_axis; + +public: + Vector3 best_axis; + + _FORCE_INLINE_ bool test_previous_axis() { + if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) { + return test_axis(*callback->prev_axis); + } else { + return true; + } + } + + _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { + Vector3 axis = p_axis; + + if (axis.is_zero_approx()) { + // strange case, try an upwards separator + axis = Vector3(0.0, 1.0, 0.0); + } + + real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0; + + shape_A->project_range(axis, *transform_A, min_A, max_A); + shape_B->project_range(axis, *transform_B, min_B, max_B); + + if (withMargin) { + min_A -= margin_A; + max_A += margin_A; + min_B -= margin_B; + max_B += margin_B; + } + + min_B -= (max_A - min_A) * 0.5; + max_B += (max_A - min_A) * 0.5; + + min_B -= (min_A + max_A) * 0.5; + max_B -= (min_A + max_A) * 0.5; + + if (min_B > 0.0 || max_B < 0.0) { + separator_axis = axis; + return false; // doesn't contain 0 + } + + //use the smallest depth + + if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0 + min_B = -min_B; + } + + if (max_B < min_B) { + if (max_B < best_depth) { + best_depth = max_B; + best_axis = axis; + } + } else { + if (min_B < best_depth) { + best_depth = min_B; + best_axis = -axis; // keep it as A axis + } + } + + return true; + } + + static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) { + SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata; + Vector3 axis = (p_point_B - p_point_A); + real_t depth = axis.length(); + + // Filter out bogus directions with a threshold and re-testing axis. + if (separator->best_depth - depth > 0.001) { + separator->test_axis(axis / depth); + } + } + + _FORCE_INLINE_ void generate_contacts() { + // nothing to do, don't generate + if (best_axis == Vector3(0.0, 0.0, 0.0)) { + return; + } + + if (!callback->callback) { + //just was checking intersection? + callback->collided = true; + if (callback->prev_axis) { + *callback->prev_axis = best_axis; + } + return; + } + + static const int max_supports = 16; + + Vector3 supports_A[max_supports]; + int support_count_A; + GodotShape3D::FeatureType support_type_A; + shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A); + for (int i = 0; i < support_count_A; i++) { + supports_A[i] = transform_A->xform(supports_A[i]); + } + + if (withMargin) { + for (int i = 0; i < support_count_A; i++) { + supports_A[i] += -best_axis * margin_A; + } + } + + Vector3 supports_B[max_supports]; + int support_count_B; + GodotShape3D::FeatureType support_type_B; + shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B); + for (int i = 0; i < support_count_B; i++) { + supports_B[i] = transform_B->xform(supports_B[i]); + } + + if (withMargin) { + for (int i = 0; i < support_count_B; i++) { + supports_B[i] += best_axis * margin_B; + } + } + + callback->normal = best_axis; + if (callback->prev_axis) { + *callback->prev_axis = best_axis; + } + _generate_contacts_from_supports(supports_A, support_count_A, support_type_A, supports_B, support_count_B, support_type_B, callback); + + callback->collided = true; + } + + _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) { + shape_A = p_shape_A; + shape_B = p_shape_B; + transform_A = &p_transform_A; + transform_B = &p_transform_B; + callback = p_callback; + margin_A = p_margin_A; + margin_B = p_margin_B; + } +}; + +/****** SAT TESTS *******/ + +typedef void (*CollisionFunc)(const GodotShape3D *, const Transform3D &, const GodotShape3D *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t); + +// Perform analytic sphere-sphere collision and report results to collector +template <bool withMargin> +static void analytic_sphere_collision(const Vector3 &p_origin_a, real_t p_radius_a, const Vector3 &p_origin_b, real_t p_radius_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + // Expand the spheres by the margins if enabled + if (withMargin) { + p_radius_a += p_margin_a; + p_radius_b += p_margin_b; + } + + // Get the vector from sphere B to A + Vector3 b_to_a = p_origin_a - p_origin_b; + + // Get the length from B to A + real_t b_to_a_len = b_to_a.length(); + + // Calculate the sphere overlap, and bail if not overlapping + real_t overlap = p_radius_a + p_radius_b - b_to_a_len; + if (overlap < 0) + return; + + // Report collision + p_collector->collided = true; + + // Bail if there is no callback to receive the A and B collision points. + if (!p_collector->callback) { + return; + } + + // Normalize the B to A vector + if (b_to_a_len < CMP_EPSILON) { + b_to_a = Vector3(0, 1, 0); // Spheres coincident, use arbitrary direction + } else { + b_to_a /= b_to_a_len; + } + + // Report collision points. The operations below are intended to minimize + // floating-point precision errors. This is done by calculating the first + // collision point from the smaller sphere, and then jumping across to + // the larger spheres collision point using the overlap distance. This + // jump is usually small even if the large sphere is massive, and so the + // second point will not suffer from precision errors. + if (p_radius_a < p_radius_b) { + Vector3 point_a = p_origin_a - b_to_a * p_radius_a; + Vector3 point_b = point_a + b_to_a * overlap; + p_collector->call(point_a, point_b, b_to_a); // Consider adding b_to_a vector + } else { + Vector3 point_b = p_origin_b + b_to_a * p_radius_b; + Vector3 point_a = point_b - b_to_a * overlap; + p_collector->call(point_a, point_b, b_to_a); // Consider adding b_to_a vector + } +} + +template <bool withMargin> +static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a); + const GodotSphereShape3D *sphere_B = static_cast<const GodotSphereShape3D *>(p_b); + + // Perform an analytic sphere collision between the two spheres + analytic_sphere_collision<withMargin>( + p_transform_a.origin, + sphere_A->get_radius() * p_transform_a.basis[0].length(), + p_transform_b.origin, + sphere_B->get_radius() * p_transform_b.basis[0].length(), + p_collector, + p_margin_a, + p_margin_b); +} + +template <bool withMargin> +static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a); + const GodotBoxShape3D *box_B = static_cast<const GodotBoxShape3D *>(p_b); + + // Find the point on the box nearest to the center of the sphere. + + Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin); + Vector3 extents = box_B->get_half_extents(); + Vector3 nearest(MIN(MAX(center.x, -extents.x), extents.x), + MIN(MAX(center.y, -extents.y), extents.y), + MIN(MAX(center.z, -extents.z), extents.z)); + nearest = p_transform_b.xform(nearest); + + // See if it is inside the sphere. + + Vector3 delta = nearest - p_transform_a.origin; + real_t length = delta.length(); + real_t radius = sphere_A->get_radius() * p_transform_a.basis[0].length(); + if (length > radius + p_margin_a + p_margin_b) { + return; + } + p_collector->collided = true; + if (!p_collector->callback) { + return; + } + Vector3 axis; + if (length == 0) { + // The box passes through the sphere center. Select an axis based on the box's center. + axis = (p_transform_b.origin - nearest).normalized(); + } else { + axis = delta / length; + } + Vector3 point_a = p_transform_a.origin + (radius + p_margin_a) * axis; + Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest); + p_collector->call(point_a, point_b, axis); +} + +template <bool withMargin> +static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a); + const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b); + + real_t scale_A = p_transform_a.basis[0].length(); + real_t scale_B = p_transform_b.basis[0].length(); + + // Construct the capsule segment (ball-center to ball-center) + Vector3 capsule_segment[2]; + Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + capsule_segment[0] = p_transform_b.origin + capsule_axis; + capsule_segment[1] = p_transform_b.origin - capsule_axis; + + // Get the capsules closest segment-point to the sphere + Vector3 capsule_closest = Geometry3D::get_closest_point_to_segment(p_transform_a.origin, capsule_segment); + + // Perform an analytic sphere collision between the sphere and the sphere-collider in the capsule + analytic_sphere_collision<withMargin>( + p_transform_a.origin, + sphere_A->get_radius() * scale_A, + capsule_closest, + capsule_B->get_radius() * scale_B, + p_collector, + p_margin_a, + p_margin_b); +} + +template <bool withMargin> +static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radius_b, real_t p_height_b, const Transform3D &p_transform_a, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + // Find the point on the cylinder nearest to the center of the sphere. + + Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin); + Vector3 nearest = center; + real_t scale_A = p_transform_a.basis[0].length(); + real_t r = Math::sqrt(center.x * center.x + center.z * center.z); + if (r > p_radius_b) { + real_t scale = p_radius_b / r; + nearest.x *= scale; + nearest.z *= scale; + } + real_t half_height = p_height_b / 2; + nearest.y = MIN(MAX(center.y, -half_height), half_height); + nearest = p_transform_b.xform(nearest); + + // See if it is inside the sphere. + + Vector3 delta = nearest - p_transform_a.origin; + real_t length = delta.length(); + if (length > p_radius_a * scale_A + p_margin_a + p_margin_b) { + return; + } + p_collector->collided = true; + if (!p_collector->callback) { + return; + } + Vector3 axis; + if (length == 0) { + // The cylinder passes through the sphere center. Select an axis based on the cylinder's center. + axis = (p_transform_b.origin - nearest).normalized(); + } else { + axis = delta / length; + } + Vector3 point_a = p_transform_a.origin + (p_radius_a * scale_A + p_margin_a) * axis; + Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest); + p_collector->call(point_a, point_b, axis); +} + +template <bool withMargin> +static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a); + const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b); + + analytic_sphere_cylinder_collision<withMargin>(sphere_A->get_radius(), cylinder_B->get_radius(), cylinder_B->get_height(), p_transform_a, p_transform_b, p_collector, p_margin_a, p_margin_b); +} + +template <bool withMargin> +static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b); + + SeparatorAxisTest<GodotSphereShape3D, GodotConvexPolygonShape3D, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of B + for (int i = 0; i < face_count; i++) { + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // edges of B + for (int i = 0; i < edge_count; i++) { + Vector3 v1 = p_transform_b.xform(vertices[edges[i].vertex_a]); + Vector3 v2 = p_transform_b.xform(vertices[edges[i].vertex_b]); + Vector3 v3 = p_transform_a.origin; + + Vector3 n1 = v2 - v1; + Vector3 n2 = v2 - v3; + + Vector3 axis = n1.cross(n2).cross(n1).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // vertices of B + for (int i = 0; i < vertex_count; i++) { + Vector3 v1 = p_transform_b.xform(vertices[i]); + Vector3 v2 = p_transform_a.origin; + + Vector3 axis = (v2 - v1).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a); + const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b); + + SeparatorAxisTest<GodotSphereShape3D, GodotFaceShape3D, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal)) { + return; + } + + // edges and points of B + for (int i = 0; i < 3; i++) { + Vector3 n1 = vertex[i] - p_transform_a.origin; + if (n1.dot(normal) < 0.0) { + n1 *= -1.0; + } + + if (!separator.test_axis(n1.normalized())) { + return; + } + + Vector3 n2 = vertex[(i + 1) % 3] - vertex[i]; + + Vector3 axis = n1.cross(n2).cross(n2).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a); + const GodotBoxShape3D *box_B = static_cast<const GodotBoxShape3D *>(p_b); + + SeparatorAxisTest<GodotBoxShape3D, GodotBoxShape3D, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // test faces of A + + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // test faces of B + + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_b.basis.get_column(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // test combined edges + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + Vector3 axis = p_transform_a.basis.get_column(i).cross(p_transform_b.basis.get_column(j)); + + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + axis.normalize(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + //add endpoint test between closest vertices and edges + + // calculate closest point to sphere + + Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin; + + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); + + Vector3 support_a = p_transform_a.xform(Vector3( + + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + + Vector3 cnormal_b = p_transform_b.basis.xform_inv(-ab_vec); + + Vector3 support_b = p_transform_b.xform(Vector3( + + (cnormal_b.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, + (cnormal_b.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, + (cnormal_b.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z)); + + Vector3 axis_ab = (support_a - support_b); + + if (!separator.test_axis(axis_ab.normalized())) { + return; + } + + //now try edges, which become cylinders! + + for (int i = 0; i < 3; i++) { + //a ->b + Vector3 axis_a = p_transform_a.basis.get_column(i); + + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { + return; + } + + //b ->a + Vector3 axis_b = p_transform_b.basis.get_column(i); + + if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized())) { + return; + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a); + const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b); + + SeparatorAxisTest<GodotBoxShape3D, GodotCapsuleShape3D, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // faces of A + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized(); + + // edges of A, capsule cylinder + + for (int i = 0; i < 3; i++) { + // cylinder + Vector3 box_axis = p_transform_a.basis.get_column(i); + Vector3 axis = box_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // points of A, capsule cylinder + // this sure could be made faster somehow.. + + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 he = box_A->get_half_extents(); + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_column(l) * he[l]; + } + + //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); + Vector3 axis = Plane(cyl_axis).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + } + + // capsule balls, edges of A + + for (int i = 0; i < 2; i++) { + Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + + Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); + + Vector3 cnormal = p_transform_a.xform_inv(sphere_pos); + + Vector3 cpoint = p_transform_a.xform(Vector3( + + (cnormal.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + + // use point to test axis + Vector3 point_axis = (sphere_pos - cpoint).normalized(); + + if (!separator.test_axis(point_axis)) { + return; + } + + // test edges of A + + for (int j = 0; j < 3; j++) { + Vector3 axis = point_axis.cross(p_transform_a.basis.get_column(j)).cross(p_transform_a.basis.get_column(j)).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a); + const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b); + + SeparatorAxisTest<GodotBoxShape3D, GodotCylinderShape3D, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Faces of A. + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + Vector3 cyl_axis = p_transform_b.basis.get_column(1).normalized(); + + // Cylinder end caps. + { + if (!separator.test_axis(cyl_axis)) { + return; + } + } + + // Edges of A, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 box_axis = p_transform_a.basis.get_column(i); + Vector3 axis = box_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Gather points of A. + Vector3 vertices_A[8]; + Vector3 box_extent = box_A->get_half_extents(); + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 extent = box_extent; + extent.x *= (i * 2 - 1); + extent.y *= (j * 2 - 1); + extent.z *= (k * 2 - 1); + Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; + point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_column(l) * extent[l]; + } + } + } + } + + // Points of A, cylinder lateral surface. + for (int i = 0; i < 8; i++) { + const Vector3 &point = vertices_A[i]; + Vector3 axis = Plane(cyl_axis).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of A, cylinder end caps rim. + int edges_start_A[12] = { 0, 2, 4, 6, 0, 1, 4, 5, 0, 1, 2, 3 }; + int edges_end_A[12] = { 1, 3, 5, 7, 2, 3, 6, 7, 4, 5, 6, 7 }; + + Vector3 cap_axis = cyl_axis * (cylinder_B->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_b.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int e = 0; e < 12; e++) { + const Vector3 &edge_start = vertices_A[edges_start_A[e]]; + const Vector3 &edge_end = vertices_A[edges_end_A[e]]; + + Vector3 edge_dir = (edge_end - edge_start); + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b); + + SeparatorAxisTest<GodotBoxShape3D, GodotConvexPolygonShape3D, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + // faces of A + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of B + for (int i = 0; i < face_count; i++) { + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // A<->B edges + for (int i = 0; i < 3; i++) { + Vector3 e1 = p_transform_a.basis.get_column(i); + + for (int j = 0; j < edge_count; j++) { + Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]); + + Vector3 axis = e1.cross(e2).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + // calculate closest points between vertices and box edges + for (int v = 0; v < vertex_count; v++) { + Vector3 vtxb = p_transform_b.xform(vertices[v]); + Vector3 ab_vec = vtxb - p_transform_a.origin; + + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); + + Vector3 support_a = p_transform_a.xform(Vector3( + + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + + Vector3 axis_ab = support_a - vtxb; + + if (!separator.test_axis(axis_ab.normalized())) { + return; + } + + //now try edges, which become cylinders! + + for (int i = 0; i < 3; i++) { + //a ->b + Vector3 axis_a = p_transform_a.basis.get_column(i); + + if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) { + return; + } + } + } + + //convex edges and box points + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 he = box_A->get_half_extents(); + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_column(l) * he[l]; + } + + for (int e = 0; e < edge_count; e++) { + Vector3 p1 = p_transform_b.xform(vertices[edges[e].vertex_a]); + Vector3 p2 = p_transform_b.xform(vertices[edges[e].vertex_b]); + Vector3 n = (p2 - p1); + + if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) { + return; + } + } + } + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a); + const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b); + + SeparatorAxisTest<GodotBoxShape3D, GodotFaceShape3D, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal)) { + return; + } + + // faces of A + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_column(i).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + + // combined edges + + for (int i = 0; i < 3; i++) { + Vector3 e = vertex[i] - vertex[(i + 1) % 3]; + + for (int j = 0; j < 3; j++) { + Vector3 axis = e.cross(p_transform_a.basis.get_column(j)).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + // calculate closest points between vertices and box edges + for (int v = 0; v < 3; v++) { + Vector3 ab_vec = vertex[v] - p_transform_a.origin; + + Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec); + + Vector3 support_a = p_transform_a.xform(Vector3( + + (cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z)); + + Vector3 axis_ab = support_a - vertex[v]; + if (axis_ab.dot(normal) < 0.0) { + axis_ab *= -1.0; + } + + if (!separator.test_axis(axis_ab.normalized())) { + return; + } + + //now try edges, which become cylinders! + + for (int i = 0; i < 3; i++) { + //a ->b + Vector3 axis_a = p_transform_a.basis.get_column(i); + + Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + + //convex edges and box points, there has to be a way to speed up this (get closest point?) + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 he = box_A->get_half_extents(); + he.x *= (i * 2 - 1); + he.y *= (j * 2 - 1); + he.z *= (k * 2 - 1); + Vector3 point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_column(l) * he[l]; + } + + for (int e = 0; e < 3; e++) { + Vector3 p1 = vertex[e]; + Vector3 p2 = vertex[(e + 1) % 3]; + + Vector3 n = (p2 - p1); + + Vector3 axis = (point - p2).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + } + } + } + + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a); + const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b); + + real_t scale_A = p_transform_a.basis[0].length(); + real_t scale_B = p_transform_b.basis[0].length(); + + // Get the closest points between the capsule segments + Vector3 capsule_A_closest; + Vector3 capsule_B_closest; + Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); + Geometry3D::get_closest_points_between_segments( + p_transform_a.origin + capsule_A_axis, + p_transform_a.origin - capsule_A_axis, + p_transform_b.origin + capsule_B_axis, + p_transform_b.origin - capsule_B_axis, + capsule_A_closest, + capsule_B_closest); + + // Perform the analytic collision between the two closest capsule spheres + analytic_sphere_collision<withMargin>( + capsule_A_closest, + capsule_A->get_radius() * scale_A, + capsule_B_closest, + capsule_B->get_radius() * scale_B, + p_collector, + p_margin_a, + p_margin_b); +} + +template <bool withMargin> +static void _collision_capsule_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a); + const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b); + + // Find the closest points between the axes of the two objects. + + Vector3 capsule_A_closest; + Vector3 cylinder_B_closest; + Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1) * (cylinder_B->get_height() * 0.5); + Geometry3D::get_closest_points_between_segments( + p_transform_a.origin + capsule_A_axis, + p_transform_a.origin - capsule_A_axis, + p_transform_b.origin + cylinder_B_axis, + p_transform_b.origin - cylinder_B_axis, + capsule_A_closest, + cylinder_B_closest); + + // Perform the collision test between the cylinder and the nearest sphere on the capsule axis. + + Transform3D sphere_transform(p_transform_a.basis, capsule_A_closest); + analytic_sphere_cylinder_collision<withMargin>(capsule_A->get_radius(), cylinder_B->get_radius(), cylinder_B->get_height(), sphere_transform, p_transform_b, p_collector, p_margin_a, p_margin_b); +} + +template <bool withMargin> +static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b); + + SeparatorAxisTest<GodotCapsuleShape3D, GodotConvexPolygonShape3D, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + const Geometry3D::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of B + for (int i = 0; i < face_count; i++) { + Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // edges of B, capsule cylinder + + for (int i = 0; i < edge_count; i++) { + // cylinder + Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].vertex_a]) - p_transform_b.basis.xform(vertices[edges[i].vertex_b]); + Vector3 axis = edge_axis.cross(p_transform_a.basis.get_column(1)).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // capsule balls, edges of B + + for (int i = 0; i < 2; i++) { + // edges of B, capsule cylinder + + Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + + Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); + + for (int j = 0; j < edge_count; j++) { + Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].vertex_a]); + Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]); + + Vector3 axis = n1.cross(n2).cross(n2).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a); + const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b); + + SeparatorAxisTest<GodotCapsuleShape3D, GodotFaceShape3D, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal)) { + return; + } + + // edges of B, capsule cylinder + + Vector3 capsule_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + + for (int i = 0; i < 3; i++) { + // edge-cylinder + Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + + Vector3 axis = edge_axis.cross(capsule_axis).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + + Vector3 dir_axis = (p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized(); + if (dir_axis.dot(normal) < 0.0) { + dir_axis *= -1.0; + } + + if (!separator.test_axis(dir_axis)) { + return; + } + + for (int j = 0; j < 2; j++) { + // point-spheres + Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis); + + Vector3 n1 = sphere_pos - vertex[i]; + if (n1.dot(normal) < 0.0) { + n1 *= -1.0; + } + + if (!separator.test_axis(n1.normalized())) { + return; + } + + Vector3 n2 = edge_axis; + + axis = n1.cross(n2).cross(n2); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_cylinder_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a); + const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b); + + SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 cylinder_A_axis = p_transform_a.basis.get_column(1); + Vector3 cylinder_B_axis = p_transform_b.basis.get_column(1); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder A end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + // Cylinder B end caps. + if (!separator.test_axis(cylinder_B_axis.normalized())) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder A lateral surface. + if (!separator.test_axis(cylinder_A_axis.cross(cylinder_diff).cross(cylinder_A_axis).normalized())) { + return; + } + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + real_t proj = cylinder_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(cylinder_A_axis); + if (Math::is_zero_approx(proj)) { + // Parallel cylinders, handle with specific axes only. + // Note: GJKEPA with no margin can lead to degenerate cases in this situation. + separator.generate_contacts(); + return; + } + + GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { + return; + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_cylinder_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b); + + SeparatorAxisTest<GodotCylinderShape3D, GodotConvexPolygonShape3D, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCylinderShape3D, GodotConvexPolygonShape3D, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { + return; + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a); + const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b); + + SeparatorAxisTest<GodotCylinderShape3D, GodotFaceShape3D, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + // Face B normal. + if (!separator.test_axis(normal)) { + return; + } + + Vector3 cyl_axis = p_transform_a.basis.get_column(1).normalized(); + if (cyl_axis.dot(normal) < 0.0) { + cyl_axis *= -1.0; + } + + // Cylinder end caps. + if (!separator.test_axis(cyl_axis)) { + return; + } + + // Edges of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + Vector3 axis = edge_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Points of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + const Vector3 point = vertex[i] - p_transform_a.origin; + Vector3 axis = Plane(cyl_axis).project(point).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of B, cylinder end caps rim. + Vector3 cap_axis = cyl_axis * (cylinder_A->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_a.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int j = 0; j < 3; j++) { + const Vector3 &edge_start = vertex[j]; + const Vector3 &edge_end = vertex[(j + 1) % 3]; + Vector3 edge_dir = edge_end - edge_start; + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + + separator.generate_contacts(); +} + +static _FORCE_INLINE_ bool is_minkowski_face(const Vector3 &A, const Vector3 &B, const Vector3 &B_x_A, const Vector3 &C, const Vector3 &D, const Vector3 &D_x_C) { + // Test if arcs AB and CD intersect on the unit sphere + real_t CBA = C.dot(B_x_A); + real_t DBA = D.dot(B_x_A); + real_t ADC = A.dot(D_x_C); + real_t BDC = B.dot(D_x_C); + + return (CBA * DBA < 0.0f) && (ADC * BDC < 0.0f) && (CBA * BDC > 0.0f); +} + +template <bool withMargin> +static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a); + const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b); + + SeparatorAxisTest<GodotConvexPolygonShape3D, GodotConvexPolygonShape3D, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + const Geometry3D::MeshData &mesh_A = convex_polygon_A->get_mesh(); + + const Geometry3D::MeshData::Face *faces_A = mesh_A.faces.ptr(); + int face_count_A = mesh_A.faces.size(); + const Geometry3D::MeshData::Edge *edges_A = mesh_A.edges.ptr(); + int edge_count_A = mesh_A.edges.size(); + const Vector3 *vertices_A = mesh_A.vertices.ptr(); + int vertex_count_A = mesh_A.vertices.size(); + + const Geometry3D::MeshData &mesh_B = convex_polygon_B->get_mesh(); + + const Geometry3D::MeshData::Face *faces_B = mesh_B.faces.ptr(); + int face_count_B = mesh_B.faces.size(); + const Geometry3D::MeshData::Edge *edges_B = mesh_B.edges.ptr(); + int edge_count_B = mesh_B.edges.size(); + const Vector3 *vertices_B = mesh_B.vertices.ptr(); + int vertex_count_B = mesh_B.vertices.size(); + + // Precalculating this makes the transforms faster. + Basis a_xform_normal = p_transform_a.basis.inverse().transposed(); + + // faces of A + for (int i = 0; i < face_count_A; i++) { + Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Precalculating this makes the transforms faster. + Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + + // faces of B + for (int i = 0; i < face_count_B; i++) { + Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // A<->B edges + + for (int i = 0; i < edge_count_A; i++) { + Vector3 p1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_a]); + Vector3 q1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_b]); + Vector3 e1 = q1 - p1; + Vector3 u1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_a].plane.normal).normalized(); + Vector3 v1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_b].plane.normal).normalized(); + + for (int j = 0; j < edge_count_B; j++) { + Vector3 p2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_a]); + Vector3 q2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_b]); + Vector3 e2 = q2 - p2; + Vector3 u2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_a].plane.normal).normalized(); + Vector3 v2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_b].plane.normal).normalized(); + + if (is_minkowski_face(u1, v1, -e1, -u2, -v2, -e2)) { + Vector3 axis = e1.cross(e2).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + } + } + + if (withMargin) { + //vertex-vertex + for (int i = 0; i < vertex_count_A; i++) { + Vector3 va = p_transform_a.xform(vertices_A[i]); + + for (int j = 0; j < vertex_count_B; j++) { + if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized())) { + return; + } + } + } + //edge-vertex (shell) + + for (int i = 0; i < edge_count_A; i++) { + Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_a]); + Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_b]); + Vector3 n = (e2 - e1); + + for (int j = 0; j < vertex_count_B; j++) { + Vector3 e3 = p_transform_b.xform(vertices_B[j]); + + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { + return; + } + } + } + + for (int i = 0; i < edge_count_B; i++) { + Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_a]); + Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_b]); + Vector3 n = (e2 - e1); + + for (int j = 0; j < vertex_count_A; j++) { + Vector3 e3 = p_transform_a.xform(vertices_A[j]); + + if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) { + return; + } + } + } + } + + separator.generate_contacts(); +} + +template <bool withMargin> +static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a); + const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b); + + SeparatorAxisTest<GodotConvexPolygonShape3D, GodotFaceShape3D, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + const Geometry3D::MeshData &mesh = convex_polygon_A->get_mesh(); + + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); + + if (!separator.test_axis(normal)) { + return; + } + + // faces of A + for (int i = 0; i < face_count; i++) { + //Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; + Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + + // A<->B edges + for (int i = 0; i < edge_count; i++) { + Vector3 e1 = p_transform_a.xform(vertices[edges[i].vertex_a]) - p_transform_a.xform(vertices[edges[i].vertex_b]); + + for (int j = 0; j < 3; j++) { + Vector3 e2 = vertex[j] - vertex[(j + 1) % 3]; + + Vector3 axis = e1.cross(e2).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + + if (withMargin) { + //vertex-vertex + for (int i = 0; i < vertex_count; i++) { + Vector3 va = p_transform_a.xform(vertices[i]); + + for (int j = 0; j < 3; j++) { + Vector3 axis = (va - vertex[j]).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + //edge-vertex (shell) + + for (int i = 0; i < edge_count; i++) { + Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].vertex_a]); + Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].vertex_b]); + Vector3 n = (e2 - e1); + + for (int j = 0; j < 3; j++) { + Vector3 e3 = vertex[j]; + + Vector3 axis = (e1 - e3).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + + for (int i = 0; i < 3; i++) { + Vector3 e1 = vertex[i]; + Vector3 e2 = vertex[(i + 1) % 3]; + Vector3 n = (e2 - e1); + + for (int j = 0; j < vertex_count; j++) { + Vector3 e3 = p_transform_a.xform(vertices[j]); + + Vector3 axis = (e1 - e3).cross(n).cross(n).normalized(); + if (axis.dot(normal) < 0.0) { + axis *= -1.0; + } + + if (!separator.test_axis(axis)) { + return; + } + } + } + } + + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + + separator.generate_contacts(); +} + +bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { + PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); + + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_A->is_concave(), false); + + PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); + + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); + ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY, false); + ERR_FAIL_COND_V(p_shape_B->is_concave(), false); + + static const CollisionFunc collision_table[6][6] = { + { _collision_sphere_sphere<false>, + _collision_sphere_box<false>, + _collision_sphere_capsule<false>, + _collision_sphere_cylinder<false>, + _collision_sphere_convex_polygon<false>, + _collision_sphere_face<false> }, + { nullptr, + _collision_box_box<false>, + _collision_box_capsule<false>, + _collision_box_cylinder<false>, + _collision_box_convex_polygon<false>, + _collision_box_face<false> }, + { nullptr, + nullptr, + _collision_capsule_capsule<false>, + _collision_capsule_cylinder<false>, + _collision_capsule_convex_polygon<false>, + _collision_capsule_face<false> }, + { nullptr, + nullptr, + nullptr, + _collision_cylinder_cylinder<false>, + _collision_cylinder_convex_polygon<false>, + _collision_cylinder_face<false> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<false>, + _collision_convex_polygon_face<false> }, + { nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr }, + }; + + static const CollisionFunc collision_table_margin[6][6] = { + { _collision_sphere_sphere<true>, + _collision_sphere_box<true>, + _collision_sphere_capsule<true>, + _collision_sphere_cylinder<true>, + _collision_sphere_convex_polygon<true>, + _collision_sphere_face<true> }, + { nullptr, + _collision_box_box<true>, + _collision_box_capsule<true>, + _collision_box_cylinder<true>, + _collision_box_convex_polygon<true>, + _collision_box_face<true> }, + { nullptr, + nullptr, + _collision_capsule_capsule<true>, + _collision_capsule_cylinder<true>, + _collision_capsule_convex_polygon<true>, + _collision_capsule_face<true> }, + { nullptr, + nullptr, + nullptr, + _collision_cylinder_cylinder<true>, + _collision_cylinder_convex_polygon<true>, + _collision_cylinder_face<true> }, + { nullptr, + nullptr, + nullptr, + nullptr, + _collision_convex_polygon_convex_polygon<true>, + _collision_convex_polygon_face<true> }, + { nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr }, + }; + + _CollectorCallback callback; + callback.callback = p_result_callback; + callback.swap = p_swap; + callback.userdata = p_userdata; + callback.collided = false; + callback.prev_axis = r_prev_axis; + + const GodotShape3D *A = p_shape_A; + const GodotShape3D *B = p_shape_B; + const Transform3D *transform_A = &p_transform_A; + const Transform3D *transform_B = &p_transform_B; + real_t margin_A = p_margin_a; + real_t margin_B = p_margin_b; + + if (type_A > type_B) { + SWAP(A, B); + SWAP(transform_A, transform_B); + SWAP(type_A, type_B); + SWAP(margin_A, margin_B); + callback.swap = !callback.swap; + } + + CollisionFunc collision_func; + if (margin_A != 0.0 || margin_B != 0.0) { + collision_func = collision_table_margin[type_A - 2][type_B - 2]; + + } else { + collision_func = collision_table[type_A - 2][type_B - 2]; + } + ERR_FAIL_NULL_V(collision_func, false); + + collision_func(A, *transform_A, B, *transform_B, &callback, margin_A, margin_B); + + return callback.collided; +} diff --git a/modules/godot_physics_3d/godot_collision_solver_3d_sat.h b/modules/godot_physics_3d/godot_collision_solver_3d_sat.h new file mode 100644 index 0000000000..49fcab3933 --- /dev/null +++ b/modules/godot_physics_3d/godot_collision_solver_3d_sat.h @@ -0,0 +1,38 @@ +/**************************************************************************/ +/* godot_collision_solver_3d_sat.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_COLLISION_SOLVER_3D_SAT_H +#define GODOT_COLLISION_SOLVER_3D_SAT_H + +#include "godot_collision_solver_3d.h" + +bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); + +#endif // GODOT_COLLISION_SOLVER_3D_SAT_H diff --git a/modules/squish/image_decompress_squish.cpp b/modules/godot_physics_3d/godot_constraint_3d.h index 3841ba8db1..a833aba93f 100644 --- a/modules/squish/image_decompress_squish.cpp +++ b/modules/godot_physics_3d/godot_constraint_3d.h @@ -1,5 +1,5 @@ /**************************************************************************/ -/* image_decompress_squish.cpp */ +/* godot_constraint_3d.h */ /**************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ @@ -28,69 +28,54 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /**************************************************************************/ -#include "image_decompress_squish.h" +#ifndef GODOT_CONSTRAINT_3D_H +#define GODOT_CONSTRAINT_3D_H -#include <squish.h> +class GodotBody3D; +class GodotSoftBody3D; -void image_decompress_squish(Image *p_image) { - int w = p_image->get_width(); - int h = p_image->get_height(); +class GodotConstraint3D { + GodotBody3D **_body_ptr; + int _body_count; + uint64_t island_step; + int priority; + bool disabled_collisions_between_bodies; - Image::Format source_format = p_image->get_format(); - Image::Format target_format = Image::FORMAT_RGBA8; + RID self; - Vector<uint8_t> data; - int64_t target_size = Image::get_image_data_size(w, h, target_format, p_image->has_mipmaps()); - int mm_count = p_image->get_mipmap_count(); - data.resize(target_size); - - const uint8_t *rb = p_image->get_data().ptr(); - uint8_t *wb = data.ptrw(); - - int squish_flags = 0; - - switch (source_format) { - case Image::FORMAT_DXT1: - squish_flags = squish::kDxt1; - break; - - case Image::FORMAT_DXT3: - squish_flags = squish::kDxt3; - break; +protected: + GodotConstraint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) { + _body_ptr = p_body_ptr; + _body_count = p_body_count; + island_step = 0; + priority = 1; + disabled_collisions_between_bodies = true; + } - case Image::FORMAT_DXT5: - case Image::FORMAT_DXT5_RA_AS_RG: - squish_flags = squish::kDxt5; - break; +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } - case Image::FORMAT_RGTC_R: - squish_flags = squish::kBc4; - break; + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } - case Image::FORMAT_RGTC_RG: - squish_flags = squish::kBc5; - break; + _FORCE_INLINE_ GodotBody3D **get_body_ptr() const { return _body_ptr; } + _FORCE_INLINE_ int get_body_count() const { return _body_count; } - default: - ERR_FAIL_MSG("Squish: Can't decompress unknown format: " + itos(p_image->get_format()) + "."); - break; - } + virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const { return nullptr; } + virtual int get_soft_body_count() const { return 0; } - for (int i = 0; i <= mm_count; i++) { - int64_t src_ofs = 0, mipmap_size = 0; - int mipmap_w = 0, mipmap_h = 0; - p_image->get_mipmap_offset_size_and_dimensions(i, src_ofs, mipmap_size, mipmap_w, mipmap_h); + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } - int64_t dst_ofs = Image::get_image_mipmap_offset(p_image->get_width(), p_image->get_height(), target_format, i); - squish::DecompressImage(&wb[dst_ofs], w, h, &rb[src_ofs], squish_flags); + _FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; } + _FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; } - w >>= 1; - h >>= 1; - } + virtual bool setup(real_t p_step) = 0; + virtual bool pre_solve(real_t p_step) = 0; + virtual void solve(real_t p_step) = 0; - p_image->set_data(p_image->get_width(), p_image->get_height(), p_image->has_mipmaps(), target_format, data); + virtual ~GodotConstraint3D() {} +}; - if (source_format == Image::FORMAT_DXT5_RA_AS_RG) { - p_image->convert_ra_rgba8_to_rg(); - } -} +#endif // GODOT_CONSTRAINT_3D_H diff --git a/modules/godot_physics_3d/godot_joint_3d.h b/modules/godot_physics_3d/godot_joint_3d.h new file mode 100644 index 0000000000..3207723cb4 --- /dev/null +++ b/modules/godot_physics_3d/godot_joint_3d.h @@ -0,0 +1,101 @@ +/**************************************************************************/ +/* godot_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_JOINT_3D_H +#define GODOT_JOINT_3D_H + +#include "godot_body_3d.h" +#include "godot_constraint_3d.h" + +class GodotJoint3D : public GodotConstraint3D { +protected: + bool dynamic_A = false; + bool dynamic_B = false; + + void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { + if (Math::abs(n.z) > Math_SQRT12) { + // choose p in y-z plane + real_t a = n[1] * n[1] + n[2] * n[2]; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(0, -n[2] * k, n[1] * k); + // set q = n x p + q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]); + } else { + // choose p in x-y plane + real_t a = n.x * n.x + n.y * n.y; + real_t k = 1.0 / Math::sqrt(a); + p = Vector3(-n.y * k, n.x * k, 0); + // set q = n x p + q = Vector3(-n.z * p.y, n.z * p.x, a * k); + } + } + + _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) { + real_t coeff_1 = Math_PI / 4.0f; + real_t coeff_2 = 3.0f * coeff_1; + real_t abs_y = Math::abs(y); + real_t angle; + if (x >= 0.0f) { + real_t r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + real_t r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; + } + +public: + virtual bool setup(real_t p_step) override { return false; } + virtual bool pre_solve(real_t p_step) override { return true; } + virtual void solve(real_t p_step) override {} + + void copy_settings_from(GodotJoint3D *p_joint) { + set_self(p_joint->get_self()); + set_priority(p_joint->get_priority()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); + } + + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; } + _FORCE_INLINE_ GodotJoint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) : + GodotConstraint3D(p_body_ptr, p_body_count) { + } + + virtual ~GodotJoint3D() { + for (int i = 0; i < get_body_count(); i++) { + GodotBody3D *body = get_body_ptr()[i]; + if (body) { + body->remove_constraint(this); + } + } + } +}; + +#endif // GODOT_JOINT_3D_H diff --git a/modules/godot_physics_3d/godot_physics_server_3d.cpp b/modules/godot_physics_3d/godot_physics_server_3d.cpp new file mode 100644 index 0000000000..6d0949acbe --- /dev/null +++ b/modules/godot_physics_3d/godot_physics_server_3d.cpp @@ -0,0 +1,1773 @@ +/**************************************************************************/ +/* godot_physics_server_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_physics_server_3d.h" + +#include "godot_body_direct_state_3d.h" +#include "godot_broad_phase_3d_bvh.h" +#include "joints/godot_cone_twist_joint_3d.h" +#include "joints/godot_generic_6dof_joint_3d.h" +#include "joints/godot_hinge_joint_3d.h" +#include "joints/godot_pin_joint_3d.h" +#include "joints/godot_slider_joint_3d.h" + +#include "core/debugger/engine_debugger.h" +#include "core/os/os.h" + +#define FLUSH_QUERY_CHECK(m_object) \ + ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); + +RID GodotPhysicsServer3D::world_boundary_shape_create() { + GodotShape3D *shape = memnew(GodotWorldBoundaryShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::separation_ray_shape_create() { + GodotShape3D *shape = memnew(GodotSeparationRayShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::sphere_shape_create() { + GodotShape3D *shape = memnew(GodotSphereShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::box_shape_create() { + GodotShape3D *shape = memnew(GodotBoxShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::capsule_shape_create() { + GodotShape3D *shape = memnew(GodotCapsuleShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::cylinder_shape_create() { + GodotShape3D *shape = memnew(GodotCylinderShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::convex_polygon_shape_create() { + GodotShape3D *shape = memnew(GodotConvexPolygonShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::concave_polygon_shape_create() { + GodotShape3D *shape = memnew(GodotConcavePolygonShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::heightmap_shape_create() { + GodotShape3D *shape = memnew(GodotHeightMapShape3D); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID GodotPhysicsServer3D::custom_shape_create() { + ERR_FAIL_V(RID()); +} + +void GodotPhysicsServer3D::shape_set_data(RID p_shape, const Variant &p_data) { + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + shape->set_data(p_data); +}; + +void GodotPhysicsServer3D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + shape->set_custom_bias(p_bias); +} + +PhysicsServer3D::ShapeType GodotPhysicsServer3D::shape_get_type(RID p_shape) const { + const GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, SHAPE_CUSTOM); + return shape->get_type(); +}; + +Variant GodotPhysicsServer3D::shape_get_data(RID p_shape) const { + const GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, Variant()); + ERR_FAIL_COND_V(!shape->is_configured(), Variant()); + return shape->get_data(); +}; + +void GodotPhysicsServer3D::shape_set_margin(RID p_shape, real_t p_margin) { +} + +real_t GodotPhysicsServer3D::shape_get_margin(RID p_shape) const { + return 0.0; +} + +real_t GodotPhysicsServer3D::shape_get_custom_solver_bias(RID p_shape) const { + const GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL_V(shape, 0); + return shape->get_custom_bias(); +} + +RID GodotPhysicsServer3D::space_create() { + GodotSpace3D *space = memnew(GodotSpace3D); + RID id = space_owner.make_rid(space); + space->set_self(id); + RID area_id = area_create(); + GodotArea3D *area = area_owner.get_or_null(area_id); + ERR_FAIL_NULL_V(area, RID()); + space->set_default_area(area); + area->set_space(space); + area->set_priority(-1); + RID sgb = body_create(); + body_set_space(sgb, id); + body_set_mode(sgb, BODY_MODE_STATIC); + space->set_static_global_body(sgb); + + return id; +}; + +void GodotPhysicsServer3D::space_set_active(RID p_space, bool p_active) { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + if (p_active) { + active_spaces.insert(space); + } else { + active_spaces.erase(space); + } +} + +bool GodotPhysicsServer3D::space_is_active(RID p_space) const { + const GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, false); + + return active_spaces.has(space); +} + +void GodotPhysicsServer3D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + + space->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::space_get_param(RID p_space, SpaceParameter p_param) const { + const GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, 0); + return space->get_param(p_param); +} + +PhysicsDirectSpaceState3D *GodotPhysicsServer3D::space_get_direct_state(RID p_space) { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, nullptr); + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + + return space->get_direct_state(); +} + +void GodotPhysicsServer3D::space_set_debug_contacts(RID p_space, int p_max_contacts) { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + space->set_debug_contacts(p_max_contacts); +} + +Vector<Vector3> GodotPhysicsServer3D::space_get_contacts(RID p_space) const { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, Vector<Vector3>()); + return space->get_debug_contacts(); +} + +int GodotPhysicsServer3D::space_get_contact_count(RID p_space) const { + GodotSpace3D *space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL_V(space, 0); + return space->get_debug_contact_count(); +} + +RID GodotPhysicsServer3D::area_create() { + GodotArea3D *area = memnew(GodotArea3D); + RID rid = area_owner.make_rid(area); + area->set_self(rid); + return rid; +} + +void GodotPhysicsServer3D::area_set_space(RID p_area, RID p_space) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + GodotSpace3D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + } + + if (area->get_space() == space) { + return; //pointless + } + + area->clear_constraints(); + area->set_space(space); +} + +RID GodotPhysicsServer3D::area_get_space(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, RID()); + + GodotSpace3D *space = area->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +} + +void GodotPhysicsServer3D::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + area->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer3D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + ERR_FAIL_COND(!shape->is_configured()); + + area->set_shape(p_shape_idx, shape); +} + +void GodotPhysicsServer3D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_shape_transform(p_shape_idx, p_transform); +} + +int GodotPhysicsServer3D::area_get_shape_count(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, -1); + + return area->get_shape_count(); +} + +RID GodotPhysicsServer3D::area_get_shape(RID p_area, int p_shape_idx) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, RID()); + + GodotShape3D *shape = area->get_shape(p_shape_idx); + ERR_FAIL_NULL_V(shape, RID()); + + return shape->get_self(); +} + +Transform3D GodotPhysicsServer3D::area_get_shape_transform(RID p_area, int p_shape_idx) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, Transform3D()); + + return area->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer3D::area_remove_shape(RID p_area, int p_shape_idx) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer3D::area_clear_shapes(RID p_area) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + while (area->get_shape_count()) { + area->remove_shape(0); + } +} + +void GodotPhysicsServer3D::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count()); + FLUSH_QUERY_CHECK(area); + area->set_shape_disabled(p_shape_idx, p_disabled); +} + +void GodotPhysicsServer3D::area_attach_object_instance_id(RID p_area, ObjectID p_id) { + if (space_owner.owns(p_area)) { + GodotSpace3D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + area->set_instance_id(p_id); +} + +ObjectID GodotPhysicsServer3D::area_get_object_instance_id(RID p_area) const { + if (space_owner.owns(p_area)) { + GodotSpace3D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, ObjectID()); + return area->get_instance_id(); +} + +void GodotPhysicsServer3D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) { + if (space_owner.owns(p_area)) { + GodotSpace3D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + area->set_param(p_param, p_value); +}; + +void GodotPhysicsServer3D::area_set_transform(RID p_area, const Transform3D &p_transform) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + area->set_transform(p_transform); +}; + +Variant GodotPhysicsServer3D::area_get_param(RID p_area, AreaParameter p_param) const { + if (space_owner.owns(p_area)) { + GodotSpace3D *space = space_owner.get_or_null(p_area); + p_area = space->get_default_area()->get_self(); + } + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, Variant()); + + return area->get_param(p_param); +}; + +Transform3D GodotPhysicsServer3D::area_get_transform(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, Transform3D()); + + return area->get_transform(); +}; + +void GodotPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_collision_layer(p_layer); +} + +uint32_t GodotPhysicsServer3D::area_get_collision_layer(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, 0); + + return area->get_collision_layer(); +} + +void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_collision_mask(p_mask); +} + +uint32_t GodotPhysicsServer3D::area_get_collision_mask(RID p_area) const { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL_V(area, 0); + + return area->get_collision_mask(); +} + +void GodotPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + FLUSH_QUERY_CHECK(area); + + area->set_monitorable(p_monitorable); +} + +void GodotPhysicsServer3D::area_set_monitor_callback(RID p_area, const Callable &p_callback) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_monitor_callback(p_callback.is_valid() ? p_callback : Callable()); +} + +void GodotPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_ray_pickable(p_enable); +} + +void GodotPhysicsServer3D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) { + GodotArea3D *area = area_owner.get_or_null(p_area); + ERR_FAIL_NULL(area); + + area->set_area_monitor_callback(p_callback.is_valid() ? p_callback : Callable()); +} + +/* BODY API */ + +RID GodotPhysicsServer3D::body_create() { + GodotBody3D *body = memnew(GodotBody3D); + RID rid = body_owner.make_rid(body); + body->set_self(rid); + return rid; +}; + +void GodotPhysicsServer3D::body_set_space(RID p_body, RID p_space) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + GodotSpace3D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + } + + if (body->get_space() == space) { + return; //pointless + } + + body->clear_constraint_map(); + body->set_space(space); +}; + +RID GodotPhysicsServer3D::body_get_space(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, RID()); + + GodotSpace3D *space = body->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +}; + +void GodotPhysicsServer3D::body_set_mode(RID p_body, BodyMode p_mode) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_mode(p_mode); +}; + +PhysicsServer3D::BodyMode GodotPhysicsServer3D::body_get_mode(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, BODY_MODE_STATIC); + + return body->get_mode(); +}; + +void GodotPhysicsServer3D::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + + body->add_shape(shape, p_transform, p_disabled); +} + +void GodotPhysicsServer3D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + GodotShape3D *shape = shape_owner.get_or_null(p_shape); + ERR_FAIL_NULL(shape); + ERR_FAIL_COND(!shape->is_configured()); + + body->set_shape(p_shape_idx, shape); +} +void GodotPhysicsServer3D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_shape_transform(p_shape_idx, p_transform); +} + +int GodotPhysicsServer3D::body_get_shape_count(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, -1); + + return body->get_shape_count(); +} + +RID GodotPhysicsServer3D::body_get_shape(RID p_body, int p_shape_idx) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, RID()); + + GodotShape3D *shape = body->get_shape(p_shape_idx); + ERR_FAIL_NULL_V(shape, RID()); + + return shape->get_self(); +} + +void GodotPhysicsServer3D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count()); + FLUSH_QUERY_CHECK(body); + + body->set_shape_disabled(p_shape_idx, p_disabled); +} + +Transform3D GodotPhysicsServer3D::body_get_shape_transform(RID p_body, int p_shape_idx) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Transform3D()); + + return body->get_shape_transform(p_shape_idx); +} + +void GodotPhysicsServer3D::body_remove_shape(RID p_body, int p_shape_idx) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->remove_shape(p_shape_idx); +} + +void GodotPhysicsServer3D::body_clear_shapes(RID p_body) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + while (body->get_shape_count()) { + body->remove_shape(0); + } +} + +void GodotPhysicsServer3D::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_continuous_collision_detection(p_enable); +} + +bool GodotPhysicsServer3D::body_is_continuous_collision_detection_enabled(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + + return body->is_continuous_collision_detection_enabled(); +} + +void GodotPhysicsServer3D::body_set_collision_layer(RID p_body, uint32_t p_layer) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_layer(p_layer); +} + +uint32_t GodotPhysicsServer3D::body_get_collision_layer(RID p_body) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_layer(); +} + +void GodotPhysicsServer3D::body_set_collision_mask(RID p_body, uint32_t p_mask) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_mask(p_mask); +} + +uint32_t GodotPhysicsServer3D::body_get_collision_mask(RID p_body) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_mask(); +} + +void GodotPhysicsServer3D::body_set_collision_priority(RID p_body, real_t p_priority) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_collision_priority(p_priority); +} + +real_t GodotPhysicsServer3D::body_get_collision_priority(RID p_body) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_collision_priority(); +} + +void GodotPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) { + GodotBody3D *body = body_owner.get_or_null(p_body); + if (body) { + body->set_instance_id(p_id); + return; + } + + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + if (soft_body) { + soft_body->set_instance_id(p_id); + return; + } + + ERR_FAIL_MSG("Invalid ID."); +} + +ObjectID GodotPhysicsServer3D::body_get_object_instance_id(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, ObjectID()); + + return body->get_instance_id(); +} + +void GodotPhysicsServer3D::body_set_user_flags(RID p_body, uint32_t p_flags) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); +} + +uint32_t GodotPhysicsServer3D::body_get_user_flags(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return 0; +} + +void GodotPhysicsServer3D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_param(p_param, p_value); +} + +Variant GodotPhysicsServer3D::body_get_param(RID p_body, BodyParameter p_param) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + + return body->get_param(p_param); +} + +void GodotPhysicsServer3D::body_reset_mass_properties(RID p_body) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + return body->reset_mass_properties(); +} + +void GodotPhysicsServer3D::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_state(p_state, p_variant); +} + +Variant GodotPhysicsServer3D::body_get_state(RID p_body, BodyState p_state) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Variant()); + + return body->get_state(p_state); +} + +void GodotPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + _update_shapes(); + + body->apply_central_impulse(p_impulse); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + _update_shapes(); + + body->apply_impulse(p_impulse, p_position); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + _update_shapes(); + + body->apply_torque_impulse(p_impulse); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_apply_central_force(RID p_body, const Vector3 &p_force) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->apply_central_force(p_force); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->apply_force(p_force, p_position); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_apply_torque(RID p_body, const Vector3 &p_torque) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->apply_torque(p_torque); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_add_constant_central_force(RID p_body, const Vector3 &p_force) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_central_force(p_force); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_force(p_force, p_position); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_add_constant_torque(RID p_body, const Vector3 &p_torque) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_constant_torque(p_torque); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_set_constant_force(RID p_body, const Vector3 &p_force) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_constant_force(p_force); + if (!p_force.is_zero_approx()) { + body->wakeup(); + } +} + +Vector3 GodotPhysicsServer3D::body_get_constant_force(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Vector3()); + return body->get_constant_force(); +} + +void GodotPhysicsServer3D::body_set_constant_torque(RID p_body, const Vector3 &p_torque) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_constant_torque(p_torque); + if (!p_torque.is_zero_approx()) { + body->wakeup(); + } +} + +Vector3 GodotPhysicsServer3D::body_get_constant_torque(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, Vector3()); + + return body->get_constant_torque(); +} + +void GodotPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + _update_shapes(); + + Vector3 v = body->get_linear_velocity(); + Vector3 axis = p_axis_velocity.normalized(); + v -= axis * axis.dot(v); + v += p_axis_velocity; + body->set_linear_velocity(v); + body->wakeup(); +} + +void GodotPhysicsServer3D::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_axis_lock(p_axis, p_lock); + body->wakeup(); +} + +bool GodotPhysicsServer3D::body_is_axis_locked(RID p_body, BodyAxis p_axis) const { + const GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + return body->is_axis_locked(p_axis); +} + +void GodotPhysicsServer3D::body_add_collision_exception(RID p_body, RID p_body_b) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->add_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_remove_collision_exception(RID p_body, RID p_body_b) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->remove_exception(p_body_b); + body->wakeup(); +}; + +void GodotPhysicsServer3D::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + for (int i = 0; i < body->get_exceptions().size(); i++) { + p_exceptions->push_back(body->get_exceptions()[i]); + } +}; + +void GodotPhysicsServer3D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); +}; + +real_t GodotPhysicsServer3D::body_get_contacts_reported_depth_threshold(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, 0); + return 0; +}; + +void GodotPhysicsServer3D::body_set_omit_force_integration(RID p_body, bool p_omit) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + + body->set_omit_force_integration(p_omit); +}; + +bool GodotPhysicsServer3D::body_is_omitting_force_integration(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + return body->get_omit_force_integration(); +}; + +void GodotPhysicsServer3D::body_set_max_contacts_reported(RID p_body, int p_contacts) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_max_contacts_reported(p_contacts); +} + +int GodotPhysicsServer3D::body_get_max_contacts_reported(RID p_body) const { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, -1); + return body->get_max_contacts_reported(); +} + +void GodotPhysicsServer3D::body_set_state_sync_callback(RID p_body, const Callable &p_callable) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_state_sync_callback(p_callable); +} + +void GodotPhysicsServer3D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_force_integration_callback(p_callable, p_udata); +} + +void GodotPhysicsServer3D::body_set_ray_pickable(RID p_body, bool p_enable) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL(body); + body->set_ray_pickable(p_enable); +} + +bool GodotPhysicsServer3D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) { + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, false); + ERR_FAIL_NULL_V(body->get_space(), false); + ERR_FAIL_COND_V(body->get_space()->is_locked(), false); + + _update_shapes(); + + return body->get_space()->test_body_motion(body, p_parameters, r_result); +} + +PhysicsDirectBodyState3D *GodotPhysicsServer3D::body_get_direct_state(RID p_body) { + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + if (!body_owner.owns(p_body)) { + return nullptr; + } + + GodotBody3D *body = body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(body, nullptr); + + if (!body->get_space()) { + return nullptr; + } + + ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + + return body->get_direct_state(); +} + +/* SOFT BODY */ + +RID GodotPhysicsServer3D::soft_body_create() { + GodotSoftBody3D *soft_body = memnew(GodotSoftBody3D); + RID rid = soft_body_owner.make_rid(soft_body); + soft_body->set_self(rid); + return rid; +} + +void GodotPhysicsServer3D::soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->update_rendering_server(p_rendering_server_handler); +} + +void GodotPhysicsServer3D::soft_body_set_space(RID p_body, RID p_space) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + GodotSpace3D *space = nullptr; + if (p_space.is_valid()) { + space = space_owner.get_or_null(p_space); + ERR_FAIL_NULL(space); + } + + if (soft_body->get_space() == space) { + return; + } + + soft_body->set_space(space); +} + +RID GodotPhysicsServer3D::soft_body_get_space(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, RID()); + + GodotSpace3D *space = soft_body->get_space(); + if (!space) { + return RID(); + } + return space->get_self(); +} + +void GodotPhysicsServer3D::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_collision_layer(p_layer); +} + +uint32_t GodotPhysicsServer3D::soft_body_get_collision_layer(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, 0); + + return soft_body->get_collision_layer(); +} + +void GodotPhysicsServer3D::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_collision_mask(p_mask); +} + +uint32_t GodotPhysicsServer3D::soft_body_get_collision_mask(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, 0); + + return soft_body->get_collision_mask(); +} + +void GodotPhysicsServer3D::soft_body_add_collision_exception(RID p_body, RID p_body_b) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->add_exception(p_body_b); +} + +void GodotPhysicsServer3D::soft_body_remove_collision_exception(RID p_body, RID p_body_b) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->remove_exception(p_body_b); +} + +void GodotPhysicsServer3D::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + for (int i = 0; i < soft_body->get_exceptions().size(); i++) { + p_exceptions->push_back(soft_body->get_exceptions()[i]); + } +} + +void GodotPhysicsServer3D::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_state(p_state, p_variant); +} + +Variant GodotPhysicsServer3D::soft_body_get_state(RID p_body, BodyState p_state) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, Variant()); + + return soft_body->get_state(p_state); +} + +void GodotPhysicsServer3D::soft_body_set_transform(RID p_body, const Transform3D &p_transform) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_state(BODY_STATE_TRANSFORM, p_transform); +} + +void GodotPhysicsServer3D::soft_body_set_ray_pickable(RID p_body, bool p_enable) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_ray_pickable(p_enable); +} + +void GodotPhysicsServer3D::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_iteration_count(p_simulation_precision); +} + +int GodotPhysicsServer3D::soft_body_get_simulation_precision(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, 0.f); + + return soft_body->get_iteration_count(); +} + +void GodotPhysicsServer3D::soft_body_set_total_mass(RID p_body, real_t p_total_mass) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_total_mass(p_total_mass); +} + +real_t GodotPhysicsServer3D::soft_body_get_total_mass(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, 0.f); + + return soft_body->get_total_mass(); +} + +void GodotPhysicsServer3D::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_linear_stiffness(p_stiffness); +} + +real_t GodotPhysicsServer3D::soft_body_get_linear_stiffness(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, 0.f); + + return soft_body->get_linear_stiffness(); +} + +void GodotPhysicsServer3D::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_pressure_coefficient(p_pressure_coefficient); +} + +real_t GodotPhysicsServer3D::soft_body_get_pressure_coefficient(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, 0.f); + + return soft_body->get_pressure_coefficient(); +} + +void GodotPhysicsServer3D::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_damping_coefficient(p_damping_coefficient); +} + +real_t GodotPhysicsServer3D::soft_body_get_damping_coefficient(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, 0.f); + + return soft_body->get_damping_coefficient(); +} + +void GodotPhysicsServer3D::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_drag_coefficient(p_drag_coefficient); +} + +real_t GodotPhysicsServer3D::soft_body_get_drag_coefficient(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, 0.f); + + return soft_body->get_drag_coefficient(); +} + +void GodotPhysicsServer3D::soft_body_set_mesh(RID p_body, RID p_mesh) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_mesh(p_mesh); +} + +AABB GodotPhysicsServer3D::soft_body_get_bounds(RID p_body) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, AABB()); + + return soft_body->get_bounds(); +} + +void GodotPhysicsServer3D::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->set_vertex_position(p_point_index, p_global_position); +} + +Vector3 GodotPhysicsServer3D::soft_body_get_point_global_position(RID p_body, int p_point_index) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, Vector3()); + + return soft_body->get_vertex_position(p_point_index); +} + +void GodotPhysicsServer3D::soft_body_remove_all_pinned_points(RID p_body) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + soft_body->unpin_all_vertices(); +} + +void GodotPhysicsServer3D::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL(soft_body); + + if (p_pin) { + soft_body->pin_vertex(p_point_index); + } else { + soft_body->unpin_vertex(p_point_index); + } +} + +bool GodotPhysicsServer3D::soft_body_is_point_pinned(RID p_body, int p_point_index) const { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body); + ERR_FAIL_NULL_V(soft_body, false); + + return soft_body->is_vertex_pinned(p_point_index); +} + +/* JOINT API */ + +RID GodotPhysicsServer3D::joint_create() { + GodotJoint3D *joint = memnew(GodotJoint3D); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void GodotPhysicsServer3D::joint_clear(RID p_joint) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + GodotJoint3D *empty_joint = memnew(GodotJoint3D); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + +void GodotPhysicsServer3D::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_NULL(body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_NULL(body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_NULL(body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint3D *joint = memnew(GodotPinJoint3D(body_A, p_local_A, body_B, p_local_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint); + pin_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); + GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint); + return pin_joint->get_param(p_param); +} + +void GodotPhysicsServer3D::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint); + pin_joint->set_pos_a(p_A); +} + +Vector3 GodotPhysicsServer3D::pin_joint_get_local_a(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); + GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint); + return pin_joint->get_position_a(); +} + +void GodotPhysicsServer3D::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); + GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint); + pin_joint->set_pos_b(p_B); +} + +Vector3 GodotPhysicsServer3D::pin_joint_get_local_b(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); + GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint); + return pin_joint->get_position_b(); +} + +void GodotPhysicsServer3D::joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_NULL(body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_NULL(body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_NULL(body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint3D *joint = memnew(GodotHingeJoint3D(body_A, body_B, p_frame_A, p_frame_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_NULL(body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_NULL(body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_NULL(body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint3D *joint = memnew(GodotHingeJoint3D(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); + GodotHingeJoint3D *hinge_joint = static_cast<GodotHingeJoint3D *>(joint); + hinge_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0); + GodotHingeJoint3D *hinge_joint = static_cast<GodotHingeJoint3D *>(joint); + return hinge_joint->get_param(p_param); +} + +void GodotPhysicsServer3D::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_enabled) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); + GodotHingeJoint3D *hinge_joint = static_cast<GodotHingeJoint3D *>(joint); + hinge_joint->set_flag(p_flag, p_enabled); +} + +bool GodotPhysicsServer3D::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); + GodotHingeJoint3D *hinge_joint = static_cast<GodotHingeJoint3D *>(joint); + return hinge_joint->get_flag(p_flag); +} + +void GodotPhysicsServer3D::joint_set_solver_priority(RID p_joint, int p_priority) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + joint->set_priority(p_priority); +} + +int GodotPhysicsServer3D::joint_get_solver_priority(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + return joint->get_priority(); +} + +void GodotPhysicsServer3D::joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + + joint->disable_collisions_between_bodies(p_disable); + + if (2 == joint->get_body_count()) { + GodotBody3D *body_a = *joint->get_body_ptr(); + GodotBody3D *body_b = *(joint->get_body_ptr() + 1); + + if (p_disable) { + body_add_collision_exception(body_a->get_self(), body_b->get_self()); + body_add_collision_exception(body_b->get_self(), body_a->get_self()); + } else { + body_remove_collision_exception(body_a->get_self(), body_b->get_self()); + body_remove_collision_exception(body_b->get_self(), body_a->get_self()); + } + } +} + +bool GodotPhysicsServer3D::joint_is_disabled_collisions_between_bodies(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, true); + + return joint->is_disabled_collisions_between_bodies(); +} + +GodotPhysicsServer3D::JointType GodotPhysicsServer3D::joint_get_type(RID p_joint) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, JOINT_TYPE_PIN); + return joint->get_type(); +} + +void GodotPhysicsServer3D::joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_NULL(body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_NULL(body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_NULL(body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint3D *joint = memnew(GodotSliderJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); + GodotSliderJoint3D *slider_joint = static_cast<GodotSliderJoint3D *>(joint); + slider_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); + GodotSliderJoint3D *slider_joint = static_cast<GodotSliderJoint3D *>(joint); + return slider_joint->get_param(p_param); +} + +void GodotPhysicsServer3D::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_NULL(body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_NULL(body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_NULL(body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint3D *joint = memnew(GodotConeTwistJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); + GodotConeTwistJoint3D *cone_twist_joint = static_cast<GodotConeTwistJoint3D *>(joint); + cone_twist_joint->set_param(p_param, p_value); +} + +real_t GodotPhysicsServer3D::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); + GodotConeTwistJoint3D *cone_twist_joint = static_cast<GodotConeTwistJoint3D *>(joint); + return cone_twist_joint->get_param(p_param); +} + +void GodotPhysicsServer3D::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) { + GodotBody3D *body_A = body_owner.get_or_null(p_body_A); + ERR_FAIL_NULL(body_A); + + if (!p_body_B.is_valid()) { + ERR_FAIL_NULL(body_A->get_space()); + p_body_B = body_A->get_space()->get_static_global_body(); + } + + GodotBody3D *body_B = body_owner.get_or_null(p_body_B); + ERR_FAIL_NULL(body_B); + + ERR_FAIL_COND(body_A == body_B); + + GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(prev_joint); + + GodotJoint3D *joint = memnew(GodotGeneric6DOFJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); +} + +void GodotPhysicsServer3D::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast<GodotGeneric6DOFJoint3D *>(joint); + generic_6dof_joint->set_param(p_axis, p_param, p_value); +} + +real_t GodotPhysicsServer3D::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0); + GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast<GodotGeneric6DOFJoint3D *>(joint); + return generic_6dof_joint->get_param(p_axis, p_param); +} + +void GodotPhysicsServer3D::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL(joint); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); + GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast<GodotGeneric6DOFJoint3D *>(joint); + generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); +} + +bool GodotPhysicsServer3D::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const { + GodotJoint3D *joint = joint_owner.get_or_null(p_joint); + ERR_FAIL_NULL_V(joint, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); + GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast<GodotGeneric6DOFJoint3D *>(joint); + return generic_6dof_joint->get_flag(p_axis, p_flag); +} + +void GodotPhysicsServer3D::free(RID p_rid) { + _update_shapes(); //just in case + + if (shape_owner.owns(p_rid)) { + GodotShape3D *shape = shape_owner.get_or_null(p_rid); + + while (shape->get_owners().size()) { + GodotShapeOwner3D *so = shape->get_owners().begin()->key; + so->remove_shape(shape); + } + + shape_owner.free(p_rid); + memdelete(shape); + } else if (body_owner.owns(p_rid)) { + GodotBody3D *body = body_owner.get_or_null(p_rid); + + body->set_space(nullptr); + + while (body->get_shape_count()) { + body->remove_shape(0); + } + + body_owner.free(p_rid); + memdelete(body); + } else if (soft_body_owner.owns(p_rid)) { + GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_rid); + + soft_body->set_space(nullptr); + + soft_body_owner.free(p_rid); + memdelete(soft_body); + } else if (area_owner.owns(p_rid)) { + GodotArea3D *area = area_owner.get_or_null(p_rid); + + area->set_space(nullptr); + + while (area->get_shape_count()) { + area->remove_shape(0); + } + + area_owner.free(p_rid); + memdelete(area); + } else if (space_owner.owns(p_rid)) { + GodotSpace3D *space = space_owner.get_or_null(p_rid); + + while (space->get_objects().size()) { + GodotCollisionObject3D *co = static_cast<GodotCollisionObject3D *>(*space->get_objects().begin()); + co->set_space(nullptr); + } + + active_spaces.erase(space); + free(space->get_default_area()->get_self()); + free(space->get_static_global_body()); + + space_owner.free(p_rid); + memdelete(space); + } else if (joint_owner.owns(p_rid)) { + GodotJoint3D *joint = joint_owner.get_or_null(p_rid); + + joint_owner.free(p_rid); + memdelete(joint); + + } else { + ERR_FAIL_MSG("Invalid ID."); + } +} + +void GodotPhysicsServer3D::set_active(bool p_active) { + active = p_active; +} + +void GodotPhysicsServer3D::init() { + stepper = memnew(GodotStep3D); +} + +void GodotPhysicsServer3D::step(real_t p_step) { + if (!active) { + return; + } + + _update_shapes(); + + island_count = 0; + active_objects = 0; + collision_pairs = 0; + for (const GodotSpace3D *E : active_spaces) { + stepper->step(const_cast<GodotSpace3D *>(E), p_step); + island_count += E->get_island_count(); + active_objects += E->get_active_objects(); + collision_pairs += E->get_collision_pairs(); + } +} + +void GodotPhysicsServer3D::sync() { + doing_sync = true; +} + +void GodotPhysicsServer3D::flush_queries() { + if (!active) { + return; + } + + flushing_queries = true; + + uint64_t time_beg = OS::get_singleton()->get_ticks_usec(); + + for (const GodotSpace3D *E : active_spaces) { + GodotSpace3D *space = const_cast<GodotSpace3D *>(E); + space->call_queries(); + } + + flushing_queries = false; + + if (EngineDebugger::is_profiling("servers")) { + uint64_t total_time[GodotSpace3D::ELAPSED_TIME_MAX]; + static const char *time_name[GodotSpace3D::ELAPSED_TIME_MAX] = { + "integrate_forces", + "generate_islands", + "setup_constraints", + "solve_constraints", + "integrate_velocities" + }; + + for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { + total_time[i] = 0; + } + + for (const GodotSpace3D *E : active_spaces) { + for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { + total_time[i] += E->get_elapsed_time(GodotSpace3D::ElapsedTime(i)); + } + } + + Array values; + values.resize(GodotSpace3D::ELAPSED_TIME_MAX * 2); + for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) { + values[i * 2 + 0] = time_name[i]; + values[i * 2 + 1] = USEC_TO_SEC(total_time[i]); + } + values.push_back("flush_queries"); + values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg)); + + values.push_front("physics_3d"); + EngineDebugger::profiler_add_frame_data("servers", values); + } +} + +void GodotPhysicsServer3D::end_sync() { + doing_sync = false; +} + +void GodotPhysicsServer3D::finish() { + memdelete(stepper); +} + +int GodotPhysicsServer3D::get_process_info(ProcessInfo p_info) { + switch (p_info) { + case INFO_ACTIVE_OBJECTS: { + return active_objects; + } break; + case INFO_COLLISION_PAIRS: { + return collision_pairs; + } break; + case INFO_ISLAND_COUNT: { + return island_count; + } break; + } + + return 0; +} + +void GodotPhysicsServer3D::_update_shapes() { + while (pending_shape_update_list.first()) { + pending_shape_update_list.first()->self()->_shape_changed(); + pending_shape_update_list.remove(pending_shape_update_list.first()); + } +} + +void GodotPhysicsServer3D::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) { + CollCbkData *cbk = static_cast<CollCbkData *>(p_userdata); + + if (cbk->max == 0) { + return; + } + + if (cbk->amount == cbk->max) { + //find least deep + real_t min_depth = 1e20; + int min_depth_idx = 0; + for (int i = 0; i < cbk->amount; i++) { + real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]); + if (d < min_depth) { + min_depth = d; + min_depth_idx = i; + } + } + + real_t d = p_point_A.distance_squared_to(p_point_B); + if (d < min_depth) { + return; + } + cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; + cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; + + } else { + cbk->ptr[cbk->amount * 2 + 0] = p_point_A; + cbk->ptr[cbk->amount * 2 + 1] = p_point_B; + cbk->amount++; + } +} + +GodotPhysicsServer3D *GodotPhysicsServer3D::godot_singleton = nullptr; +GodotPhysicsServer3D::GodotPhysicsServer3D(bool p_using_threads) { + godot_singleton = this; + GodotBroadPhase3D::create_func = GodotBroadPhase3DBVH::_create; + + using_threads = p_using_threads; +}; diff --git a/modules/godot_physics_3d/godot_physics_server_3d.h b/modules/godot_physics_3d/godot_physics_server_3d.h new file mode 100644 index 0000000000..040e673dcd --- /dev/null +++ b/modules/godot_physics_3d/godot_physics_server_3d.h @@ -0,0 +1,385 @@ +/**************************************************************************/ +/* godot_physics_server_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_PHYSICS_SERVER_3D_H +#define GODOT_PHYSICS_SERVER_3D_H + +#include "godot_joint_3d.h" +#include "godot_shape_3d.h" +#include "godot_space_3d.h" +#include "godot_step_3d.h" + +#include "core/templates/rid_owner.h" +#include "servers/physics_server_3d.h" + +class GodotPhysicsServer3D : public PhysicsServer3D { + GDCLASS(GodotPhysicsServer3D, PhysicsServer3D); + + friend class GodotPhysicsDirectSpaceState3D; + bool active = true; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + bool using_threads = false; + bool doing_sync = false; + bool flushing_queries = false; + + GodotStep3D *stepper = nullptr; + HashSet<const GodotSpace3D *> active_spaces; + + mutable RID_PtrOwner<GodotShape3D, true> shape_owner; + mutable RID_PtrOwner<GodotSpace3D, true> space_owner; + mutable RID_PtrOwner<GodotArea3D, true> area_owner; + mutable RID_PtrOwner<GodotBody3D, true> body_owner; + mutable RID_PtrOwner<GodotSoftBody3D, true> soft_body_owner; + mutable RID_PtrOwner<GodotJoint3D, true> joint_owner; + + //void _clear_query(QuerySW *p_query); + friend class GodotCollisionObject3D; + SelfList<GodotCollisionObject3D>::List pending_shape_update_list; + void _update_shapes(); + + static GodotPhysicsServer3D *godot_singleton; + +public: + struct CollCbkData { + int max; + int amount; + Vector3 *ptr = nullptr; + }; + + static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata); + + virtual RID world_boundary_shape_create() override; + virtual RID separation_ray_shape_create() override; + virtual RID sphere_shape_create() override; + virtual RID box_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID cylinder_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + virtual RID heightmap_shape_create() override; + virtual RID custom_shape_create() override; + + virtual void shape_set_data(RID p_shape, const Variant &p_data) override; + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; + + virtual ShapeType shape_get_type(RID p_shape) const override; + virtual Variant shape_get_data(RID p_shape) const override; + + virtual void shape_set_margin(RID p_shape, real_t p_margin) override; + virtual real_t shape_get_margin(RID p_shape) const override; + + virtual real_t shape_get_custom_solver_bias(RID p_shape) const override; + + /* SPACE API */ + + virtual RID space_create() override; + virtual void space_set_active(RID p_space, bool p_active) override; + virtual bool space_is_active(RID p_space) const override; + + virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override; + virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override; + + virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override; + virtual Vector<Vector3> space_get_contacts(RID p_space) const override; + virtual int space_get_contact_count(RID p_space) const override; + + /* AREA API */ + + virtual RID area_create() override; + + virtual void area_set_space(RID p_area, RID p_space) override; + virtual RID area_get_space(RID p_area) const override; + + virtual void area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; + virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override; + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) override; + + virtual int area_get_shape_count(RID p_area) const override; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const override; + virtual Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const override; + + virtual void area_remove_shape(RID p_area, int p_shape_idx) override; + virtual void area_clear_shapes(RID p_area) override; + + virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) override; + + virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override; + virtual ObjectID area_get_object_instance_id(RID p_area) const override; + + virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override; + virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override; + + virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override; + virtual Transform3D area_get_transform(RID p_area) const override; + + virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; + + virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; + virtual uint32_t area_get_collision_layer(RID p_area) const override; + + virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; + virtual uint32_t area_get_collision_mask(RID p_area) const override; + + virtual void area_set_monitorable(RID p_area, bool p_monitorable) override; + + virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override; + virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override; + + /* BODY API */ + + // create a body of a given type + virtual RID body_create() override; + + virtual void body_set_space(RID p_body, RID p_space) override; + virtual RID body_get_space(RID p_body) const override; + + virtual void body_set_mode(RID p_body, BodyMode p_mode) override; + virtual BodyMode body_get_mode(RID p_body) const override; + + virtual void body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override; + virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override; + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) override; + + virtual int body_get_shape_count(RID p_body) const override; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const override; + virtual Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override; + + virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override; + + virtual void body_remove_shape(RID p_body, int p_shape_idx) override; + virtual void body_clear_shapes(RID p_body) override; + + virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override; + virtual ObjectID body_get_object_instance_id(RID p_body) const override; + + virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) override; + virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const override; + + virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t body_get_collision_layer(RID p_body) const override; + + virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t body_get_collision_mask(RID p_body) const override; + + virtual void body_set_collision_priority(RID p_body, real_t p_priority) override; + virtual real_t body_get_collision_priority(RID p_body) const override; + + virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override; + virtual uint32_t body_get_user_flags(RID p_body) const override; + + virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override; + virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override; + + virtual void body_reset_mass_properties(RID p_body) override; + + virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; + virtual Variant body_get_state(RID p_body, BodyState p_state) const override; + + virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override; + virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override; + virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override; + + virtual void body_apply_central_force(RID p_body, const Vector3 &p_force) override; + virtual void body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void body_apply_torque(RID p_body, const Vector3 &p_torque) override; + + virtual void body_add_constant_central_force(RID p_body, const Vector3 &p_force) override; + virtual void body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; + virtual void body_add_constant_torque(RID p_body, const Vector3 &p_torque) override; + + virtual void body_set_constant_force(RID p_body, const Vector3 &p_force) override; + virtual Vector3 body_get_constant_force(RID p_body) const override; + + virtual void body_set_constant_torque(RID p_body, const Vector3 &p_torque) override; + virtual Vector3 body_get_constant_torque(RID p_body) const override; + + virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override; + + virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) override; + virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const override; + + virtual void body_add_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override; + virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override; + + virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override; + virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override; + + virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override; + virtual bool body_is_omitting_force_integration(RID p_body) const override; + + virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override; + virtual int body_get_max_contacts_reported(RID p_body) const override; + + virtual void body_set_state_sync_callback(RID p_body, const Callable &p_callable) override; + virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override; + + virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; + + virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override; + + // this function only works on physics process, errors and returns null otherwise + virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override; + + /* SOFT BODY */ + + virtual RID soft_body_create() override; + + virtual void soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) override; + + virtual void soft_body_set_space(RID p_body, RID p_space) override; + virtual RID soft_body_get_space(RID p_body) const override; + + virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override; + virtual uint32_t soft_body_get_collision_layer(RID p_body) const override; + + virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override; + virtual uint32_t soft_body_get_collision_mask(RID p_body) const override; + + virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override; + virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override; + virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override; + + virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override; + virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override; + + virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override; + + virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override; + + virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override; + virtual int soft_body_get_simulation_precision(RID p_body) const override; + + virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override; + virtual real_t soft_body_get_total_mass(RID p_body) const override; + + virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override; + virtual real_t soft_body_get_linear_stiffness(RID p_body) const override; + + virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override; + virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override; + + virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override; + virtual real_t soft_body_get_damping_coefficient(RID p_body) const override; + + virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override; + virtual real_t soft_body_get_drag_coefficient(RID p_body) const override; + + virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override; + + virtual AABB soft_body_get_bounds(RID p_body) const override; + + virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override; + virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override; + + virtual void soft_body_remove_all_pinned_points(RID p_body) override; + virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override; + virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override; + + /* JOINT API */ + + virtual RID joint_create() override; + + virtual void joint_clear(RID p_joint) override; //resets type + + virtual void joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override; + + virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; + virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; + + virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) override; + virtual Vector3 pin_joint_get_local_a(RID p_joint) const override; + + virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override; + virtual Vector3 pin_joint_get_local_b(RID p_joint) const override; + + virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) override; + virtual void joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override; + + virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override; + virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const override; + + virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override; + virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override; + + virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A + + virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override; + virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override; + + virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A + + virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override; + virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override; + + virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override; //reference frame is A + + virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value) override; + virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override; + + virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override; + virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override; + + virtual JointType joint_get_type(RID p_joint) const override; + + virtual void joint_set_solver_priority(RID p_joint, int p_priority) override; + virtual int joint_get_solver_priority(RID p_joint) const override; + + virtual void joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) override; + virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override; + + /* MISC */ + + virtual void free(RID p_rid) override; + + virtual void set_active(bool p_active) override; + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void flush_queries() override; + virtual void end_sync() override; + virtual void finish() override; + + virtual bool is_flushing_queries() const override { return flushing_queries; } + + int get_process_info(ProcessInfo p_info) override; + + GodotPhysicsServer3D(bool p_using_threads = false); + ~GodotPhysicsServer3D() {} +}; + +#endif // GODOT_PHYSICS_SERVER_3D_H diff --git a/modules/godot_physics_3d/godot_shape_3d.cpp b/modules/godot_physics_3d/godot_shape_3d.cpp new file mode 100644 index 0000000000..70b6bcf19e --- /dev/null +++ b/modules/godot_physics_3d/godot_shape_3d.cpp @@ -0,0 +1,2265 @@ +/**************************************************************************/ +/* godot_shape_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_shape_3d.h" + +#include "core/io/image.h" +#include "core/math/convex_hull.h" +#include "core/math/geometry_3d.h" +#include "core/templates/sort_array.h" + +// GodotHeightMapShape3D is based on Bullet btHeightfieldTerrainShape. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 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. +*/ + +const double edge_support_threshold = 0.99999998; +const double edge_support_threshold_lower = Math::sqrt(1.0 - edge_support_threshold * edge_support_threshold); +// For a unit normal vector n, the horizontality condition +// sqrt(n.x * n.x + n.z * n.z) > edge_support_threshold +// is equivalent to the condition +// abs(n.y) < edge_support_threshold_lower, +// which is cheaper to test. +const double face_support_threshold = 0.9998; + +const double cylinder_edge_support_threshold = 0.999998; +const double cylinder_edge_support_threshold_lower = Math::sqrt(1.0 - cylinder_edge_support_threshold * cylinder_edge_support_threshold); +const double cylinder_face_support_threshold = 0.999; + +void GodotShape3D::configure(const AABB &p_aabb) { + aabb = p_aabb; + configured = true; + for (const KeyValue<GodotShapeOwner3D *, int> &E : owners) { + GodotShapeOwner3D *co = const_cast<GodotShapeOwner3D *>(E.key); + co->_shape_changed(); + } +} + +Vector3 GodotShape3D::get_support(const Vector3 &p_normal) const { + Vector3 res; + int amnt; + FeatureType type; + get_supports(p_normal, 1, &res, amnt, type); + return res; +} + +void GodotShape3D::add_owner(GodotShapeOwner3D *p_owner) { + HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner); + if (E) { + E->value++; + } else { + owners[p_owner] = 1; + } +} + +void GodotShape3D::remove_owner(GodotShapeOwner3D *p_owner) { + HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->value--; + if (E->value == 0) { + owners.remove(E); + } +} + +bool GodotShape3D::is_owner(GodotShapeOwner3D *p_owner) const { + return owners.has(p_owner); +} + +const HashMap<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const { + return owners; +} + +GodotShape3D::~GodotShape3D() { + ERR_FAIL_COND(owners.size()); +} + +Plane GodotWorldBoundaryShape3D::get_plane() const { + return plane; +} + +void GodotWorldBoundaryShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // gibberish, a plane is infinity + r_min = -1e7; + r_max = 1e7; +} + +Vector3 GodotWorldBoundaryShape3D::get_support(const Vector3 &p_normal) const { + return p_normal * 1e15; +} + +bool GodotWorldBoundaryShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + bool inters = plane.intersects_segment(p_begin, p_end, &r_result); + if (inters) { + r_normal = plane.normal; + } + return inters; +} + +bool GodotWorldBoundaryShape3D::intersect_point(const Vector3 &p_point) const { + return plane.distance_to(p_point) < 0; +} + +Vector3 GodotWorldBoundaryShape3D::get_closest_point_to(const Vector3 &p_point) const { + if (plane.is_point_over(p_point)) { + return plane.project(p_point); + } else { + return p_point; + } +} + +Vector3 GodotWorldBoundaryShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); // not applicable. +} + +void GodotWorldBoundaryShape3D::_setup(const Plane &p_plane) { + plane = p_plane; + configure(AABB(Vector3(-1e15, -1e15, -1e15), Vector3(1e15 * 2, 1e15 * 2, 1e15 * 2))); +} + +void GodotWorldBoundaryShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotWorldBoundaryShape3D::get_data() const { + return plane; +} + +GodotWorldBoundaryShape3D::GodotWorldBoundaryShape3D() { +} + +// + +real_t GodotSeparationRayShape3D::get_length() const { + return length; +} + +bool GodotSeparationRayShape3D::get_slide_on_slope() const { + return slide_on_slope; +} + +void GodotSeparationRayShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // don't think this will be even used + r_min = 0; + r_max = 1; +} + +Vector3 GodotSeparationRayShape3D::get_support(const Vector3 &p_normal) const { + if (p_normal.z > 0) { + return Vector3(0, 0, length); + } else { + return Vector3(0, 0, 0); + } +} + +void GodotSeparationRayShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + if (Math::abs(p_normal.z) < edge_support_threshold_lower) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = Vector3(0, 0, 0); + r_supports[1] = Vector3(0, 0, length); + } else if (p_normal.z > 0) { + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = Vector3(0, 0, length); + } else { + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = Vector3(0, 0, 0); + } +} + +bool GodotSeparationRayShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + return false; //simply not possible +} + +bool GodotSeparationRayShape3D::intersect_point(const Vector3 &p_point) const { + return false; //simply not possible +} + +Vector3 GodotSeparationRayShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 s[2] = { + Vector3(0, 0, 0), + Vector3(0, 0, length) + }; + + return Geometry3D::get_closest_point_to_segment(p_point, s); +} + +Vector3 GodotSeparationRayShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); +} + +void GodotSeparationRayShape3D::_setup(real_t p_length, bool p_slide_on_slope) { + length = p_length; + slide_on_slope = p_slide_on_slope; + configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length))); +} + +void GodotSeparationRayShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + _setup(d["length"], d["slide_on_slope"]); +} + +Variant GodotSeparationRayShape3D::get_data() const { + Dictionary d; + d["length"] = length; + d["slide_on_slope"] = slide_on_slope; + return d; +} + +GodotSeparationRayShape3D::GodotSeparationRayShape3D() {} + +/********** SPHERE *************/ + +real_t GodotSphereShape3D::get_radius() const { + return radius; +} + +void GodotSphereShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + real_t d = p_normal.dot(p_transform.origin); + + // figure out scale at point + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + + r_min = d - (radius)*scale; + r_max = d + (radius)*scale; +} + +Vector3 GodotSphereShape3D::get_support(const Vector3 &p_normal) const { + return p_normal * radius; +} + +void GodotSphereShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + *r_supports = p_normal * radius; + r_amount = 1; + r_type = FEATURE_POINT; +} + +bool GodotSphereShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); +} + +bool GodotSphereShape3D::intersect_point(const Vector3 &p_point) const { + return p_point.length() < radius; +} + +Vector3 GodotSphereShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 p = p_point; + real_t l = p.length(); + if (l < radius) { + return p_point; + } + return (p / l) * radius; +} + +Vector3 GodotSphereShape3D::get_moment_of_inertia(real_t p_mass) const { + real_t s = 0.4 * p_mass * radius * radius; + return Vector3(s, s, s); +} + +void GodotSphereShape3D::_setup(real_t p_radius) { + radius = p_radius; + configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0))); +} + +void GodotSphereShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotSphereShape3D::get_data() const { + return radius; +} + +GodotSphereShape3D::GodotSphereShape3D() {} + +/********** BOX *************/ + +void GodotBoxShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + // no matter the angle, the box is mirrored anyway + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + + real_t length = local_normal.abs().dot(half_extents); + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 GodotBoxShape3D::get_support(const Vector3 &p_normal) const { + Vector3 point( + (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); + + return point; +} + +void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + static const int next[3] = { 1, 2, 0 }; + static const int next2[3] = { 2, 0, 1 }; + + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + real_t dot = p_normal.dot(axis); + if (Math::abs(dot) > face_support_threshold) { + //Vector3 axis_b; + + bool neg = dot < 0; + r_amount = 4; + r_type = FEATURE_FACE; + + Vector3 point; + point[i] = half_extents[i]; + + int i_n = next[i]; + int i_n2 = next2[i]; + + static const real_t sign[4][2] = { + { -1.0, 1.0 }, + { 1.0, 1.0 }, + { 1.0, -1.0 }, + { -1.0, -1.0 }, + }; + + for (int j = 0; j < 4; j++) { + point[i_n] = sign[j][0] * half_extents[i_n]; + point[i_n2] = sign[j][1] * half_extents[i_n2]; + r_supports[j] = neg ? -point : point; + } + + if (neg) { + SWAP(r_supports[1], r_supports[2]); + SWAP(r_supports[0], r_supports[3]); + } + + return; + } + + r_amount = 0; + } + + for (int i = 0; i < 3; i++) { + Vector3 axis; + axis[i] = 1.0; + + if (Math::abs(p_normal.dot(axis)) < edge_support_threshold_lower) { + r_amount = 2; + r_type = FEATURE_EDGE; + + int i_n = next[i]; + int i_n2 = next2[i]; + + Vector3 point = half_extents; + + if (p_normal[i_n] < 0) { + point[i_n] = -point[i_n]; + } + if (p_normal[i_n2] < 0) { + point[i_n2] = -point[i_n2]; + } + + r_supports[0] = point; + point[i] = -point[i]; + r_supports[1] = point; + return; + } + } + /* USE POINT */ + + Vector3 point( + (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); + + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = point; +} + +bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + AABB aabb_ext(-half_extents, half_extents * 2.0); + + return aabb_ext.intersects_segment(p_begin, p_end, &r_result, &r_normal); +} + +bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const { + return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z); +} + +Vector3 GodotBoxShape3D::get_closest_point_to(const Vector3 &p_point) const { + int outside = 0; + Vector3 min_point; + + for (int i = 0; i < 3; i++) { + if (Math::abs(p_point[i]) > half_extents[i]) { + outside++; + if (outside == 1) { + //use plane if only one side matches + Vector3 n; + n[i] = SIGN(p_point[i]); + + Plane p(n, half_extents[i]); + min_point = p.project(p_point); + } + } + } + + if (!outside) { + return p_point; //it's inside, don't do anything else + } + + if (outside == 1) { //if only above one plane, this plane clearly wins + return min_point; + } + + //check segments + real_t min_distance = 1e20; + Vector3 closest_vertex = half_extents * p_point.sign(); + Vector3 s[2] = { + closest_vertex, + closest_vertex + }; + + for (int i = 0; i < 3; i++) { + s[1] = closest_vertex; + s[1][i] = -s[1][i]; //edge + + Vector3 closest_edge = Geometry3D::get_closest_point_to_segment(p_point, s); + + real_t d = p_point.distance_to(closest_edge); + if (d < min_distance) { + min_point = closest_edge; + min_distance = d; + } + } + + return min_point; +} + +Vector3 GodotBoxShape3D::get_moment_of_inertia(real_t p_mass) const { + real_t lx = half_extents.x; + real_t ly = half_extents.y; + real_t lz = half_extents.z; + + return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly)); +} + +void GodotBoxShape3D::_setup(const Vector3 &p_half_extents) { + half_extents = p_half_extents.abs(); + + configure(AABB(-half_extents, half_extents * 2)); +} + +void GodotBoxShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotBoxShape3D::get_data() const { + return half_extents; +} + +GodotBoxShape3D::GodotBoxShape3D() {} + +/********** CAPSULE *************/ + +void GodotCapsuleShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + + r_max = p_normal.dot(p_transform.xform(n)); + r_min = p_normal.dot(p_transform.xform(-n)); +} + +Vector3 GodotCapsuleShape3D::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + + real_t h = height * 0.5 - radius; + + n *= radius; + n.y += (n.y > 0) ? h : -h; + return n; +} + +void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + Vector3 n = p_normal; + + real_t d = n.y; + real_t h = height * 0.5 - radius; // half-height of the cylinder part + + if (h > 0 && Math::abs(d) < edge_support_threshold_lower) { + // make it flat + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += h; + r_supports[1] = n; + r_supports[1].y -= h; + } else { + n *= radius; + n.y += (d > 0) ? h : -h; + r_amount = 1; + r_type = FEATURE_POINT; + *r_supports = n; + } +} + +bool GodotCapsuleShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + Vector3 norm = (p_end - p_begin).normalized(); + real_t min_d = 1e20; + + Vector3 res, n; + bool collision = false; + + Vector3 auxres, auxn; + bool collided; + + // test against cylinder and spheres :-| + + collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1); + + if (collided) { + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn); + + if (collided) { + real_t d = norm.dot(auxres); + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn); + + if (collided) { + real_t d = norm.dot(auxres); + + if (d < min_d) { + min_d = d; + res = auxres; + n = auxn; + collision = true; + } + } + + if (collision) { + r_result = res; + r_normal = n; + } + return collision; +} + +bool GodotCapsuleShape3D::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5 - radius) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } else { + Vector3 p = p_point; + p.y = Math::abs(p.y) - height * 0.5 + radius; + return p.length() < radius; + } +} + +Vector3 GodotCapsuleShape3D::get_closest_point_to(const Vector3 &p_point) const { + Vector3 s[2] = { + Vector3(0, -height * 0.5 + radius, 0), + Vector3(0, height * 0.5 - radius, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; +} + +Vector3 GodotCapsuleShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotCapsuleShape3D::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2))); +} + +void GodotCapsuleShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant GodotCapsuleShape3D::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +GodotCapsuleShape3D::GodotCapsuleShape3D() {} + +/********** CYLINDER *************/ + +void GodotCylinderShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + Vector3 cylinder_axis = p_transform.basis.get_column(1).normalized(); + real_t axis_dot = cylinder_axis.dot(p_normal); + + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + real_t scaled_radius = radius * scale; + real_t scaled_height = height * scale; + + real_t length; + if (Math::abs(axis_dot) > 1.0) { + length = scaled_height * 0.5; + } else { + length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot); + } + + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 GodotCylinderShape3D::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + real_t h = (n.y > 0) ? height : -height; + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = radius; + n.y = h * 0.5; + n.z = 0.0; + } else { + real_t d = radius / s; + n.x = n.x * d; + n.y = h * 0.5; + n.z = n.z * d; + } + + return n; +} + +void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + real_t d = p_normal.y; + if (Math::abs(d) > cylinder_face_support_threshold) { + real_t h = (d > 0) ? height : -height; + + Vector3 n = p_normal; + n.x = 0.0; + n.z = 0.0; + n.y = h * 0.5; + + r_amount = 3; + r_type = FEATURE_CIRCLE; + r_supports[0] = n; + r_supports[1] = n; + r_supports[1].x += radius; + r_supports[2] = n; + r_supports[2].z += radius; + } else if (Math::abs(d) < cylinder_edge_support_threshold_lower) { + // make it flat + Vector3 n = p_normal; + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += height * 0.5; + r_supports[1] = n; + r_supports[1].y -= height * 0.5; + } else { + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = get_support(p_normal); + } +} + +bool GodotCylinderShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); +} + +bool GodotCylinderShape3D::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } + return false; +} + +Vector3 GodotCylinderShape3D::get_closest_point_to(const Vector3 &p_point) const { + if (Math::absf(p_point.y) > height * 0.5) { + // Project point to top disk. + real_t dir = p_point.y > 0.0 ? 1.0 : -1.0; + Vector3 circle_pos(0.0, dir * height * 0.5, 0.0); + Plane circle_plane(Vector3(0.0, dir, 0.0), circle_pos); + Vector3 proj_point = circle_plane.project(p_point); + + // Clip position. + Vector3 delta_point_1 = proj_point - circle_pos; + real_t dist_point_1 = delta_point_1.length_squared(); + if (!Math::is_zero_approx(dist_point_1)) { + dist_point_1 = Math::sqrt(dist_point_1); + proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1; + } + + return proj_point; + } else { + Vector3 s[2] = { + Vector3(0, -height * 0.5, 0), + Vector3(0, height * 0.5, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; + } +} + +Vector3 GodotCylinderShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotCylinderShape3D::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0))); +} + +void GodotCylinderShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant GodotCylinderShape3D::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +GodotCylinderShape3D::GodotCylinderShape3D() {} + +/********** CONVEX POLYGON *************/ + +void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + uint32_t vertex_count = mesh.vertices.size(); + if (vertex_count == 0) { + return; + } + + const Vector3 *vrts = &mesh.vertices[0]; + + if (vertex_count > 3 * extreme_vertices.size()) { + // For a large mesh, two calls to get_support() is faster than a full + // scan over all vertices. + + Vector3 n = p_transform.basis.xform_inv(p_normal).normalized(); + r_min = p_normal.dot(p_transform.xform(get_support(-n))); + r_max = p_normal.dot(p_transform.xform(get_support(n))); + } else { + for (uint32_t i = 0; i < vertex_count; i++) { + real_t d = p_normal.dot(p_transform.xform(vrts[i])); + + if (i == 0 || d > r_max) { + r_max = d; + } + if (i == 0 || d < r_min) { + r_min = d; + } + } + } +} + +Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const { + // Skip if there are no vertices in the mesh + if (mesh.vertices.size() == 0) { + return Vector3(); + } + + // Get the array of vertices + const Vector3 *const vertices_array = mesh.vertices.ptr(); + + // Start with an initial assumption of the first extreme vertex. + int best_vertex = extreme_vertices[0]; + real_t max_support = p_normal.dot(vertices_array[best_vertex]); + + // Check the remaining extreme vertices for a better vertex. + for (const int &vert : extreme_vertices) { + real_t s = p_normal.dot(vertices_array[vert]); + if (s > max_support) { + best_vertex = vert; + max_support = s; + } + } + + // If we checked all vertices in the mesh then we're done. + if (extreme_vertices.size() == mesh.vertices.size()) { + return vertices_array[best_vertex]; + } + + // Move along the surface until we reach the true support vertex. + int last_vertex = -1; + while (true) { + int next_vertex = -1; + + // Iterate over all the neighbors checking for a better vertex. + for (const int &vert : vertex_neighbors[best_vertex]) { + if (vert != last_vertex) { + real_t s = p_normal.dot(vertices_array[vert]); + if (s > max_support) { + next_vertex = vert; + max_support = s; + break; + } + } + } + + // No better vertex found, we have the best + if (next_vertex == -1) { + return vertices_array[best_vertex]; + } + + // Move to the better vertex and try again + last_vertex = best_vertex; + best_vertex = next_vertex; + } +} + +void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + + const Vector3 *vertices = mesh.vertices.ptr(); + int vc = mesh.vertices.size(); + + r_amount = 0; + ERR_FAIL_COND_MSG(vc == 0, "Convex polygon shape has no vertices."); + + //find vertex first + real_t max = 0; + int vtx = 0; + + for (int i = 0; i < vc; i++) { + real_t d = p_normal.dot(vertices[i]); + + if (i == 0 || d > max) { + max = d; + vtx = i; + } + } + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.normal.dot(p_normal) > face_support_threshold) { + int ic = faces[i].indices.size(); + const int *ind = faces[i].indices.ptr(); + + bool valid = false; + for (int j = 0; j < ic; j++) { + if (ind[j] == vtx) { + valid = true; + break; + } + } + + if (!valid) { + continue; + } + + int m = MIN(p_max, ic); + for (int j = 0; j < m; j++) { + r_supports[j] = vertices[ind[j]]; + } + r_amount = m; + r_type = FEATURE_FACE; + return; + } + } + + for (int i = 0; i < ec; i++) { + real_t dot = (vertices[edges[i].vertex_a] - vertices[edges[i].vertex_b]).normalized().dot(p_normal); + dot = ABS(dot); + if (dot < edge_support_threshold_lower && (edges[i].vertex_a == vtx || edges[i].vertex_b == vtx)) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = vertices[edges[i].vertex_a]; + r_supports[1] = vertices[edges[i].vertex_b]; + return; + } + } + + r_supports[0] = vertices[vtx]; + r_amount = 1; + r_type = FEATURE_POINT; +} + +bool GodotConvexPolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + const Vector3 *vertices = mesh.vertices.ptr(); + + Vector3 n = p_end - p_begin; + real_t min = 1e20; + bool col = false; + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.normal.dot(n) > 0) { + continue; //opposing face + } + + int ic = faces[i].indices.size(); + const int *ind = faces[i].indices.ptr(); + + for (int j = 1; j < ic - 1; j++) { + Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]); + Vector3 result; + if (f.intersects_segment(p_begin, p_end, &result)) { + real_t d = n.dot(result); + if (d < min) { + min = d; + r_result = result; + r_normal = faces[i].plane.normal; + col = true; + } + + break; + } + } + } + + return col; +} + +bool GodotConvexPolygonShape3D::intersect_point(const Vector3 &p_point) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + for (int i = 0; i < fc; i++) { + if (faces[i].plane.distance_to(p_point) >= 0) { + return false; + } + } + + return true; +} + +Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point) const { + const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + + bool all_inside = true; + for (int i = 0; i < fc; i++) { + if (!faces[i].plane.is_point_over(p_point)) { + continue; + } + + all_inside = false; + bool is_inside = true; + int ic = faces[i].indices.size(); + const int *indices = faces[i].indices.ptr(); + + for (int j = 0; j < ic; j++) { + Vector3 a = vertices[indices[j]]; + Vector3 b = vertices[indices[(j + 1) % ic]]; + Vector3 n = (a - b).cross(faces[i].plane.normal).normalized(); + if (Plane(n, a).is_point_over(p_point)) { + is_inside = false; + break; + } + } + + if (is_inside) { + return faces[i].plane.project(p_point); + } + } + + if (all_inside) { + return p_point; + } + + real_t min_distance = 1e20; + Vector3 min_point; + + //check edges + const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + for (int i = 0; i < ec; i++) { + Vector3 s[2] = { + vertices[edges[i].vertex_a], + vertices[edges[i].vertex_b] + }; + + Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s); + real_t d = closest.distance_to(p_point); + if (d < min_distance) { + min_distance = d; + min_point = closest; + } + } + + return min_point; +} + +Vector3 GodotConvexPolygonShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) { + Error err = ConvexHullComputer::convex_hull(p_vertices, mesh); + if (err != OK) { + ERR_PRINT("Failed to build convex hull"); + } + extreme_vertices.resize(0); + vertex_neighbors.resize(0); + + AABB _aabb; + + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { + if (i == 0) { + _aabb.position = mesh.vertices[i]; + } else { + _aabb.expand_to(mesh.vertices[i]); + } + } + + configure(_aabb); + + // Pre-compute the extreme vertices in 26 directions. This will be used + // to speed up get_support() by letting us quickly get a good guess for + // the support vertex. + + for (int x = -1; x < 2; x++) { + for (int y = -1; y < 2; y++) { + for (int z = -1; z < 2; z++) { + if (x != 0 || y != 0 || z != 0) { + Vector3 dir(x, y, z); + dir.normalize(); + real_t max_support = 0.0; + int best_vertex = -1; + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { + real_t s = dir.dot(mesh.vertices[i]); + if (best_vertex == -1 || s > max_support) { + best_vertex = i; + max_support = s; + } + } + if (!extreme_vertices.has(best_vertex)) + extreme_vertices.push_back(best_vertex); + } + } + } + } + + // Record all the neighbors of each vertex. This is used in get_support(). + + if (extreme_vertices.size() < mesh.vertices.size()) { + vertex_neighbors.resize(mesh.vertices.size()); + for (Geometry3D::MeshData::Edge &edge : mesh.edges) { + vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b); + vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a); + } + } +} + +void GodotConvexPolygonShape3D::set_data(const Variant &p_data) { + _setup(p_data); +} + +Variant GodotConvexPolygonShape3D::get_data() const { + Vector<Vector3> vertices; + vertices.resize(mesh.vertices.size()); + for (uint32_t i = 0; i < mesh.vertices.size(); i++) { + vertices.write[i] = mesh.vertices[i]; + } + return vertices; +} + +GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() { +} + +/********** FACE POLYGON *************/ + +void GodotFaceShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + for (int i = 0; i < 3; i++) { + Vector3 v = p_transform.xform(vertex[i]); + real_t d = p_normal.dot(v); + + if (i == 0 || d > r_max) { + r_max = d; + } + + if (i == 0 || d < r_min) { + r_min = d; + } + } +} + +Vector3 GodotFaceShape3D::get_support(const Vector3 &p_normal) const { + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < 3; i++) { + real_t d = p_normal.dot(vertex[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + return vertex[vert_support_idx]; +} + +void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + Vector3 n = p_normal; + + /** TEST FACE AS SUPPORT **/ + if (Math::abs(normal.dot(n)) > face_support_threshold) { + r_amount = 3; + r_type = FEATURE_FACE; + for (int i = 0; i < 3; i++) { + r_supports[i] = vertex[i]; + } + return; + } + + /** FIND SUPPORT VERTEX **/ + + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < 3; i++) { + real_t d = n.dot(vertex[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + /** TEST EDGES AS SUPPORT **/ + + for (int i = 0; i < 3; i++) { + int nx = (i + 1) % 3; + if (i != vert_support_idx && nx != vert_support_idx) { + continue; + } + + // check if edge is valid as a support + real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); + dot = ABS(dot); + if (dot < edge_support_threshold_lower) { + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = vertex[i]; + r_supports[1] = vertex[nx]; + return; + } + } + + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = vertex[vert_support_idx]; +} + +bool GodotFaceShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result); + if (c) { + r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal; + if (r_normal.dot(p_end - p_begin) > 0) { + if (backface_collision && p_hit_back_faces) { + r_normal = -r_normal; + } else { + c = false; + } + } + } + + return c; +} + +bool GodotFaceShape3D::intersect_point(const Vector3 &p_point) const { + return false; //face is flat +} + +Vector3 GodotFaceShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); +} + +Vector3 GodotFaceShape3D::get_moment_of_inertia(real_t p_mass) const { + return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! +} + +GodotFaceShape3D::GodotFaceShape3D() { + configure(AABB()); +} + +Vector<Vector3> GodotConcavePolygonShape3D::get_faces() const { + Vector<Vector3> rfaces; + rfaces.resize(faces.size() * 3); + + for (int i = 0; i < faces.size(); i++) { + Face f = faces.get(i); + + for (int j = 0; j < 3; j++) { + rfaces.set(i * 3 + j, vertices.get(f.indices[j])); + } + } + + return rfaces; +} + +void GodotConcavePolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + int count = vertices.size(); + if (count == 0) { + r_min = 0; + r_max = 0; + return; + } + const Vector3 *vptr = vertices.ptr(); + + for (int i = 0; i < count; i++) { + real_t d = p_normal.dot(p_transform.xform(vptr[i])); + + if (i == 0 || d > r_max) { + r_max = d; + } + if (i == 0 || d < r_min) { + r_min = d; + } + } +} + +Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const { + int count = vertices.size(); + if (count == 0) { + return Vector3(); + } + + const Vector3 *vptr = vertices.ptr(); + + Vector3 n = p_normal; + + int vert_support_idx = -1; + real_t support_max = 0; + + for (int i = 0; i < count; i++) { + real_t d = n.dot(vptr[i]); + + if (i == 0 || d > support_max) { + support_max = d; + vert_support_idx = i; + } + } + + return vptr[vert_support_idx]; +} + +void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const { + const BVH *params_bvh = &p_params->bvh[p_idx]; + + if (!params_bvh->aabb.intersects_segment(p_params->from, p_params->to)) { + return; + } + + if (params_bvh->face_index >= 0) { + const Face *f = &p_params->faces[params_bvh->face_index]; + GodotFaceShape3D *face = p_params->face; + face->normal = f->normal; + face->vertex[0] = p_params->vertices[f->indices[0]]; + face->vertex[1] = p_params->vertices[f->indices[1]]; + face->vertex[2] = p_params->vertices[f->indices[2]]; + + Vector3 res; + Vector3 normal; + int face_index = params_bvh->face_index; + if (face->intersect_segment(p_params->from, p_params->to, res, normal, face_index, true)) { + real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from); + if ((d > 0) && (d < p_params->min_d)) { + p_params->min_d = d; + p_params->result = res; + p_params->normal = normal; + p_params->face_index = face_index; + p_params->collisions++; + } + } + } else { + if (params_bvh->left >= 0) { + _cull_segment(params_bvh->left, p_params); + } + if (params_bvh->right >= 0) { + _cull_segment(params_bvh->right, p_params); + } + } +} + +bool GodotConcavePolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + if (faces.size() == 0) { + return false; + } + + // unlock data + const Face *fr = faces.ptr(); + const Vector3 *vr = vertices.ptr(); + const BVH *br = bvh.ptr(); + + GodotFaceShape3D face; + face.backface_collision = backface_collision && p_hit_back_faces; + + _SegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = (p_end - p_begin).normalized(); + + params.faces = fr; + params.vertices = vr; + params.bvh = br; + + params.face = &face; + + // cull + _cull_segment(0, ¶ms); + + if (params.collisions > 0) { + r_result = params.result; + r_normal = params.normal; + r_face_index = params.face_index; + return true; + } else { + return false; + } +} + +bool GodotConcavePolygonShape3D::intersect_point(const Vector3 &p_point) const { + return false; //face is flat +} + +Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Vector3(); +} + +bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { + const BVH *params_bvh = &p_params->bvh[p_idx]; + + if (!p_params->aabb.intersects(params_bvh->aabb)) { + return false; + } + + if (params_bvh->face_index >= 0) { + const Face *f = &p_params->faces[params_bvh->face_index]; + GodotFaceShape3D *face = p_params->face; + face->normal = f->normal; + face->vertex[0] = p_params->vertices[f->indices[0]]; + face->vertex[1] = p_params->vertices[f->indices[1]]; + face->vertex[2] = p_params->vertices[f->indices[2]]; + if (p_params->callback(p_params->userdata, face)) { + return true; + } + } else { + if (params_bvh->left >= 0) { + if (_cull(params_bvh->left, p_params)) { + return true; + } + } + + if (params_bvh->right >= 0) { + if (_cull(params_bvh->right, p_params)) { + return true; + } + } + } + + return false; +} + +void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const { + // make matrix local to concave + if (faces.size() == 0) { + return; + } + + AABB local_aabb = p_local_aabb; + + // unlock data + const Face *fr = faces.ptr(); + const Vector3 *vr = vertices.ptr(); + const BVH *br = bvh.ptr(); + + GodotFaceShape3D face; // use this to send in the callback + face.backface_collision = backface_collision; + face.invert_backface_collision = p_invert_backface_collision; + + _CullParams params; + params.aabb = local_aabb; + params.face = &face; + params.faces = fr; + params.vertices = vr; + params.bvh = br; + params.callback = p_callback; + params.userdata = p_userdata; + + // cull + _cull(0, ¶ms); +} + +Vector3 GodotConcavePolygonShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +struct _Volume_BVH_Element { + AABB aabb; + Vector3 center; + int face_index = 0; +}; + +struct _Volume_BVH_CompareX { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.x < b.center.x; + } +}; + +struct _Volume_BVH_CompareY { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.y < b.center.y; + } +}; + +struct _Volume_BVH_CompareZ { + _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const { + return a.center.z < b.center.z; + } +}; + +struct _Volume_BVH { + AABB aabb; + _Volume_BVH *left = nullptr; + _Volume_BVH *right = nullptr; + + int face_index = 0; +}; + +_Volume_BVH *_volume_build_bvh(_Volume_BVH_Element *p_elements, int p_size, int &count) { + _Volume_BVH *bvh = memnew(_Volume_BVH); + + if (p_size == 1) { + //leaf + bvh->aabb = p_elements[0].aabb; + bvh->left = nullptr; + bvh->right = nullptr; + bvh->face_index = p_elements->face_index; + count++; + return bvh; + } else { + bvh->face_index = -1; + } + + AABB aabb; + for (int i = 0; i < p_size; i++) { + if (i == 0) { + aabb = p_elements[i].aabb; + } else { + aabb.merge_with(p_elements[i].aabb); + } + } + bvh->aabb = aabb; + switch (aabb.get_longest_axis_index()) { + case 0: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareX> sort_x; + sort_x.sort(p_elements, p_size); + + } break; + case 1: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareY> sort_y; + sort_y.sort(p_elements, p_size); + } break; + case 2: { + SortArray<_Volume_BVH_Element, _Volume_BVH_CompareZ> sort_z; + sort_z.sort(p_elements, p_size); + } break; + } + + int split = p_size / 2; + bvh->left = _volume_build_bvh(p_elements, split, count); + bvh->right = _volume_build_bvh(&p_elements[split], p_size - split, count); + + //printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index); + count++; + return bvh; +} + +void GodotConcavePolygonShape3D::_fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) { + int idx = p_idx; + + p_bvh_array[idx].aabb = p_bvh_tree->aabb; + p_bvh_array[idx].face_index = p_bvh_tree->face_index; + //printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right); + + if (p_bvh_tree->left) { + p_bvh_array[idx].left = ++p_idx; + _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx); + + } else { + p_bvh_array[p_idx].left = -1; + } + + if (p_bvh_tree->right) { + p_bvh_array[idx].right = ++p_idx; + _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx); + + } else { + p_bvh_array[p_idx].right = -1; + } + + memdelete(p_bvh_tree); +} + +void GodotConcavePolygonShape3D::_setup(const Vector<Vector3> &p_faces, bool p_backface_collision) { + int src_face_count = p_faces.size(); + if (src_face_count == 0) { + configure(AABB()); + return; + } + ERR_FAIL_COND(src_face_count % 3); + src_face_count /= 3; + + const Vector3 *facesr = p_faces.ptr(); + + Vector<_Volume_BVH_Element> bvh_array; + bvh_array.resize(src_face_count); + + _Volume_BVH_Element *bvh_arrayw = bvh_array.ptrw(); + + faces.resize(src_face_count); + Face *facesw = faces.ptrw(); + + vertices.resize(src_face_count * 3); + + Vector3 *verticesw = vertices.ptrw(); + + AABB _aabb; + + for (int i = 0; i < src_face_count; i++) { + Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); + + bvh_arrayw[i].aabb = face.get_aabb(); + bvh_arrayw[i].center = bvh_arrayw[i].aabb.get_center(); + bvh_arrayw[i].face_index = i; + facesw[i].indices[0] = i * 3 + 0; + facesw[i].indices[1] = i * 3 + 1; + facesw[i].indices[2] = i * 3 + 2; + facesw[i].normal = face.get_plane().normal; + verticesw[i * 3 + 0] = face.vertex[0]; + verticesw[i * 3 + 1] = face.vertex[1]; + verticesw[i * 3 + 2] = face.vertex[2]; + if (i == 0) { + _aabb = bvh_arrayw[i].aabb; + } else { + _aabb.merge_with(bvh_arrayw[i].aabb); + } + } + + int count = 0; + _Volume_BVH *bvh_tree = _volume_build_bvh(bvh_arrayw, src_face_count, count); + + bvh.resize(count + 1); + + BVH *bvh_arrayw2 = bvh.ptrw(); + + int idx = 0; + _fill_bvh(bvh_tree, bvh_arrayw2, idx); + + backface_collision = p_backface_collision; + + configure(_aabb); // this type of shape has no margin +} + +void GodotConcavePolygonShape3D::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("faces")); + + _setup(d["faces"], d["backface_collision"]); +} + +Variant GodotConcavePolygonShape3D::get_data() const { + Dictionary d; + d["faces"] = get_faces(); + d["backface_collision"] = backface_collision; + + return d; +} + +GodotConcavePolygonShape3D::GodotConcavePolygonShape3D() { +} + +/* HEIGHT MAP SHAPE */ + +Vector<real_t> GodotHeightMapShape3D::get_heights() const { + return heights; +} + +int GodotHeightMapShape3D::get_width() const { + return width; +} + +int GodotHeightMapShape3D::get_depth() const { + return depth; +} + +void GodotHeightMapShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { + //not very useful, but not very used either + p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal), r_min, r_max); +} + +Vector3 GodotHeightMapShape3D::get_support(const Vector3 &p_normal) const { + //not very useful, but not very used either + return get_aabb().get_support(p_normal); +} + +struct _HeightmapSegmentCullParams { + Vector3 from; + Vector3 to; + Vector3 dir; + + Vector3 result; + Vector3 normal; + + const GodotHeightMapShape3D *heightmap = nullptr; + GodotFaceShape3D *face = nullptr; +}; + +struct _HeightmapGridCullState { + real_t length = 0.0; + real_t length_flat = 0.0; + + real_t dist = 0.0; + real_t prev_dist = 0.0; + + int x = 0; + int z = 0; +}; + +_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) { + Vector3 res; + Vector3 normal; + int fi = -1; + if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal, fi, true)) { + p_params.result = res; + p_params.normal = normal; + + return true; + } + + return false; +} + +_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) { + // First triangle. + p_params.heightmap->_get_point(p_state.x, p_state.z, p_params.face->vertex[0]); + p_params.heightmap->_get_point(p_state.x + 1, p_state.z, p_params.face->vertex[1]); + p_params.heightmap->_get_point(p_state.x, p_state.z + 1, p_params.face->vertex[2]); + p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; + if (_heightmap_face_cull_segment(p_params)) { + return true; + } + + // Second triangle. + p_params.face->vertex[0] = p_params.face->vertex[1]; + p_params.heightmap->_get_point(p_state.x + 1, p_state.z + 1, p_params.face->vertex[1]); + p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal; + if (_heightmap_face_cull_segment(p_params)) { + return true; + } + + return false; +} + +_FORCE_INLINE_ bool _heightmap_chunk_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) { + const GodotHeightMapShape3D::Range &chunk = p_params.heightmap->_get_bounds_chunk(p_state.x, p_state.z); + + Vector3 enter_pos; + Vector3 exit_pos; + + if (p_state.length_flat > CMP_EPSILON) { + real_t flat_to_3d = p_state.length / p_state.length_flat; + real_t enter_param = p_state.prev_dist * flat_to_3d; + real_t exit_param = p_state.dist * flat_to_3d; + enter_pos = p_params.from + p_params.dir * enter_param; + exit_pos = p_params.from + p_params.dir * exit_param; + } else { + // Consider the ray vertical. + // (though we shouldn't reach this often because there is an early check up-front) + enter_pos = p_params.from; + exit_pos = p_params.to; + } + + // Transform positions to heightmap space. + enter_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE; + exit_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE; + + // We did enter the flat projection of the AABB, + // but we have to check if we intersect it on the vertical axis. + if ((enter_pos.y > chunk.max) && (exit_pos.y > chunk.max)) { + return false; + } + if ((enter_pos.y < chunk.min) && (exit_pos.y < chunk.min)) { + return false; + } + + return p_params.heightmap->_intersect_grid_segment(_heightmap_cell_cull_segment, enter_pos, exit_pos, p_params.heightmap->width, p_params.heightmap->depth, p_params.heightmap->local_origin, p_params.result, p_params.normal); +} + +template <typename ProcessFunction> +bool GodotHeightMapShape3D::_intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const { + Vector3 delta = (p_end - p_begin); + real_t length = delta.length(); + + if (length < CMP_EPSILON) { + return false; + } + + Vector3 local_begin = p_begin + offset; + + GodotFaceShape3D face; + face.backface_collision = false; + + _HeightmapSegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = delta / length; + params.heightmap = this; + params.face = &face; + + _HeightmapGridCullState state; + + // Perform grid query from projected ray. + Vector2 ray_dir_flat(delta.x, delta.z); + state.length = length; + state.length_flat = ray_dir_flat.length(); + + if (state.length_flat < CMP_EPSILON) { + ray_dir_flat = Vector2(); + } else { + ray_dir_flat /= state.length_flat; + } + + const int x_step = (ray_dir_flat.x > CMP_EPSILON) ? 1 : ((ray_dir_flat.x < -CMP_EPSILON) ? -1 : 0); + const int z_step = (ray_dir_flat.y > CMP_EPSILON) ? 1 : ((ray_dir_flat.y < -CMP_EPSILON) ? -1 : 0); + + const real_t infinite = 1e20; + const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_flat.x) : infinite; + const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_flat.y) : infinite; + + real_t cross_x; // At which value of `param` we will cross a x-axis lane? + real_t cross_z; // At which value of `param` we will cross a z-axis lane? + + // X initialization. + if (x_step != 0) { + if (x_step == 1) { + cross_x = (Math::ceil(local_begin.x) - local_begin.x) * delta_x; + } else { + cross_x = (local_begin.x - Math::floor(local_begin.x)) * delta_x; + } + } else { + cross_x = infinite; // Will never cross on X. + } + + // Z initialization. + if (z_step != 0) { + if (z_step == 1) { + cross_z = (Math::ceil(local_begin.z) - local_begin.z) * delta_z; + } else { + cross_z = (local_begin.z - Math::floor(local_begin.z)) * delta_z; + } + } else { + cross_z = infinite; // Will never cross on Z. + } + + int x = Math::floor(local_begin.x); + int z = Math::floor(local_begin.z); + + // Workaround cases where the ray starts at an integer position. + if (Math::is_zero_approx(cross_x)) { + cross_x += delta_x; + // If going backwards, we should ignore the position we would get by the above flooring, + // because the ray is not heading in that direction. + if (x_step == -1) { + x -= 1; + } + } + + if (Math::is_zero_approx(cross_z)) { + cross_z += delta_z; + if (z_step == -1) { + z -= 1; + } + } + + // Start inside the grid. + int x_start = MAX(MIN(x, p_width - 2), 0); + int z_start = MAX(MIN(z, p_depth - 2), 0); + + // Adjust initial cross values. + cross_x += delta_x * x_step * (x_start - x); + cross_z += delta_z * z_step * (z_start - z); + + x = x_start; + z = z_start; + + while (true) { + state.prev_dist = state.dist; + state.x = x; + state.z = z; + + if (cross_x < cross_z) { + // X lane. + x += x_step; + // Assign before advancing the param, + // to be in sync with the initialization step. + state.dist = cross_x; + cross_x += delta_x; + } else { + // Z lane. + z += z_step; + state.dist = cross_z; + cross_z += delta_z; + } + + if (state.dist > state.length_flat) { + state.dist = state.length_flat; + if (p_process(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + break; + } + + if (p_process(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + + // Stop when outside the grid. + if ((x < 0) || (z < 0) || (x >= p_width - 1) || (z >= p_depth - 1)) { + break; + } + } + + return false; +} + +bool GodotHeightMapShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + if (heights.is_empty()) { + return false; + } + + Vector3 local_begin = p_begin + local_origin; + Vector3 local_end = p_end + local_origin; + + // Quantize the ray begin/end. + int begin_x = Math::floor(local_begin.x); + int begin_z = Math::floor(local_begin.z); + int end_x = Math::floor(local_end.x); + int end_z = Math::floor(local_end.z); + + if ((begin_x == end_x) && (begin_z == end_z)) { + // Simple case for rays that don't traverse the grid horizontally. + // Just perform a test on the given cell. + GodotFaceShape3D face; + face.backface_collision = p_hit_back_faces; + + _HeightmapSegmentCullParams params; + params.from = p_begin; + params.to = p_end; + params.dir = (p_end - p_begin).normalized(); + + params.heightmap = this; + params.face = &face; + + _HeightmapGridCullState state; + state.x = MAX(MIN(begin_x, width - 2), 0); + state.z = MAX(MIN(begin_z, depth - 2), 0); + if (_heightmap_cell_cull_segment(params, state)) { + r_point = params.result; + r_normal = params.normal; + return true; + } + } else if (bounds_grid.is_empty()) { + // Process all cells intersecting the flat projection of the ray. + return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal); + } else { + Vector3 ray_diff = (p_end - p_begin); + real_t length_flat_sqr = ray_diff.x * ray_diff.x + ray_diff.z * ray_diff.z; + if (length_flat_sqr < BOUNDS_CHUNK_SIZE * BOUNDS_CHUNK_SIZE) { + // Don't use chunks, the ray is too short in the plane. + return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal); + } else { + // The ray is long, run raycast on a higher-level grid. + Vector3 bounds_from = p_begin / BOUNDS_CHUNK_SIZE; + Vector3 bounds_to = p_end / BOUNDS_CHUNK_SIZE; + Vector3 bounds_offset = local_origin / BOUNDS_CHUNK_SIZE; + return _intersect_grid_segment(_heightmap_chunk_cull_segment, bounds_from, bounds_to, bounds_grid_width, bounds_grid_depth, bounds_offset, r_point, r_normal); + } + } + + return false; +} + +bool GodotHeightMapShape3D::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) const { + return Vector3(); +} + +void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const { + const AABB &shape_aabb = get_aabb(); + + Vector3 pos_local = shape_aabb.position + local_origin; + + Vector3 clamped_point(p_point); + clamped_point = p_point.clamp(pos_local, pos_local + shape_aabb.size); + + r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5); + r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5); + r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5); +} + +void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const { + if (heights.is_empty()) { + return; + } + + AABB local_aabb = p_local_aabb; + local_aabb.position += local_origin; + + // Quantize the aabb, and adjust the start/end ranges. + int aabb_min[3]; + int aabb_max[3]; + _get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]); + _get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]); + + // Expand the min/max quantized values. + // This is to catch the case where the input aabb falls between grid points. + for (int i = 0; i < 3; ++i) { + aabb_min[i]--; + aabb_max[i]++; + } + + int start_x = MAX(0, aabb_min[0]); + int end_x = MIN(width - 1, aabb_max[0]); + int start_z = MAX(0, aabb_min[2]); + int end_z = MIN(depth - 1, aabb_max[2]); + + GodotFaceShape3D face; + face.backface_collision = !p_invert_backface_collision; + face.invert_backface_collision = p_invert_backface_collision; + + for (int z = start_z; z < end_z; z++) { + for (int x = start_x; x < end_x; x++) { + // First triangle. + _get_point(x, z, face.vertex[0]); + _get_point(x + 1, z, face.vertex[1]); + _get_point(x, z + 1, face.vertex[2]); + face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal; + if (p_callback(p_userdata, &face)) { + return; + } + + // Second triangle. + face.vertex[0] = face.vertex[1]; + _get_point(x + 1, z + 1, face.vertex[1]); + face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal; + if (p_callback(p_userdata, &face)) { + return; + } + } + } +} + +Vector3 GodotHeightMapShape3D::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y)); +} + +void GodotHeightMapShape3D::_build_accelerator() { + bounds_grid.clear(); + + bounds_grid_width = width / BOUNDS_CHUNK_SIZE; + bounds_grid_depth = depth / BOUNDS_CHUNK_SIZE; + + if (width % BOUNDS_CHUNK_SIZE > 0) { + ++bounds_grid_width; // In case terrain size isn't dividable by chunk size. + } + + if (depth % BOUNDS_CHUNK_SIZE > 0) { + ++bounds_grid_depth; + } + + uint32_t bound_grid_size = (uint32_t)(bounds_grid_width * bounds_grid_depth); + + if (bound_grid_size < 2) { + // Grid is empty or just one chunk. + return; + } + + bounds_grid.resize(bound_grid_size); + + // Compute min and max height for all chunks. + for (int cz = 0; cz < bounds_grid_depth; ++cz) { + int z0 = cz * BOUNDS_CHUNK_SIZE; + + for (int cx = 0; cx < bounds_grid_width; ++cx) { + int x0 = cx * BOUNDS_CHUNK_SIZE; + + Range r; + + r.min = _get_height(x0, z0); + r.max = r.min; + + // Compute min and max height for this chunk. + // We have to include one extra cell to account for neighbors. + // Here is why: + // Say we have a flat terrain, and a plateau that fits a chunk perfectly. + // + // Left Right + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // x + // + // If the AABB for the Left chunk did not share vertices with the Right, + // then we would fail collision tests at x due to a gap. + // + int z_max = MIN(z0 + BOUNDS_CHUNK_SIZE + 1, depth); + int x_max = MIN(x0 + BOUNDS_CHUNK_SIZE + 1, width); + for (int z = z0; z < z_max; ++z) { + for (int x = x0; x < x_max; ++x) { + real_t height = _get_height(x, z); + if (height < r.min) { + r.min = height; + } else if (height > r.max) { + r.max = height; + } + } + } + + bounds_grid[cx + cz * bounds_grid_width] = r; + } + } +} + +void GodotHeightMapShape3D::_setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) { + heights = p_heights; + width = p_width; + depth = p_depth; + + // Initialize aabb. + AABB aabb_new; + aabb_new.position = Vector3(0.0, p_min_height, 0.0); + aabb_new.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1); + + // Initialize origin as the aabb center. + local_origin = aabb_new.position + 0.5 * aabb_new.size; + local_origin.y = 0.0; + + aabb_new.position -= local_origin; + + _build_accelerator(); + + configure(aabb_new); +} + +void GodotHeightMapShape3D::set_data(const Variant &p_data) { + ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY); + + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("width")); + ERR_FAIL_COND(!d.has("depth")); + ERR_FAIL_COND(!d.has("heights")); + + int width_new = d["width"]; + int depth_new = d["depth"]; + + ERR_FAIL_COND(width_new <= 0.0); + ERR_FAIL_COND(depth_new <= 0.0); + + Variant heights_variant = d["heights"]; + Vector<real_t> heights_buffer; +#ifdef REAL_T_IS_DOUBLE + if (heights_variant.get_type() == Variant::PACKED_FLOAT64_ARRAY) { +#else + if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) { +#endif + // Ready-to-use heights can be passed. + heights_buffer = heights_variant; + } else if (heights_variant.get_type() == Variant::OBJECT) { + // If an image is passed, we have to convert it. + // This would be expensive to do with a script, so it's nice to have it here. + Ref<Image> image = heights_variant; + ERR_FAIL_COND(image.is_null()); + ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF); + + PackedByteArray im_data = image->get_data(); + heights_buffer.resize(image->get_width() * image->get_height()); + + real_t *w = heights_buffer.ptrw(); + real_t *rp = (real_t *)im_data.ptr(); + for (int i = 0; i < heights_buffer.size(); ++i) { + w[i] = rp[i]; + } + } else { +#ifdef REAL_T_IS_DOUBLE + ERR_FAIL_MSG("Expected PackedFloat64Array or float Image."); +#else + ERR_FAIL_MSG("Expected PackedFloat32Array or float Image."); +#endif + } + + // Compute min and max heights or use precomputed values. + real_t min_height = 0.0; + real_t max_height = 0.0; + if (d.has("min_height") && d.has("max_height")) { + min_height = d["min_height"]; + max_height = d["max_height"]; + } else { + int heights_size = heights.size(); + for (int i = 0; i < heights_size; ++i) { + real_t h = heights[i]; + if (h < min_height) { + min_height = h; + } else if (h > max_height) { + max_height = h; + } + } + } + + ERR_FAIL_COND(min_height > max_height); + + ERR_FAIL_COND(heights_buffer.size() != (width_new * depth_new)); + + // If specified, min and max height will be used as precomputed values. + _setup(heights_buffer, width_new, depth_new, min_height, max_height); +} + +Variant GodotHeightMapShape3D::get_data() const { + Dictionary d; + d["width"] = width; + d["depth"] = depth; + + const AABB &shape_aabb = get_aabb(); + d["min_height"] = shape_aabb.position.y; + d["max_height"] = shape_aabb.position.y + shape_aabb.size.y; + + d["heights"] = heights; + + return d; +} + +GodotHeightMapShape3D::GodotHeightMapShape3D() { +} diff --git a/modules/godot_physics_3d/godot_shape_3d.h b/modules/godot_physics_3d/godot_shape_3d.h new file mode 100644 index 0000000000..dbd58ead68 --- /dev/null +++ b/modules/godot_physics_3d/godot_shape_3d.h @@ -0,0 +1,514 @@ +/**************************************************************************/ +/* godot_shape_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_SHAPE_3D_H +#define GODOT_SHAPE_3D_H + +#include "core/math/geometry_3d.h" +#include "core/templates/local_vector.h" +#include "servers/physics_server_3d.h" + +class GodotShape3D; + +class GodotShapeOwner3D { +public: + virtual void _shape_changed() = 0; + virtual void remove_shape(GodotShape3D *p_shape) = 0; + + virtual ~GodotShapeOwner3D() {} +}; + +class GodotShape3D { + RID self; + AABB aabb; + bool configured = false; + real_t custom_bias = 0.0; + + HashMap<GodotShapeOwner3D *, int> owners; + +protected: + void configure(const AABB &p_aabb); + +public: + enum FeatureType { + FEATURE_POINT, + FEATURE_EDGE, + FEATURE_FACE, + FEATURE_CIRCLE, + }; + + virtual real_t get_volume() const { return aabb.get_volume(); } + + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + virtual PhysicsServer3D::ShapeType get_type() const = 0; + + _FORCE_INLINE_ const AABB &get_aabb() const { return aabb; } + _FORCE_INLINE_ bool is_configured() const { return configured; } + + virtual bool is_concave() const { return false; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const = 0; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const = 0; + virtual bool intersect_point(const Vector3 &p_point) const = 0; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0; + + virtual void set_data(const Variant &p_data) = 0; + virtual Variant get_data() const = 0; + + _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } + _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; } + + void add_owner(GodotShapeOwner3D *p_owner); + void remove_owner(GodotShapeOwner3D *p_owner); + bool is_owner(GodotShapeOwner3D *p_owner) const; + const HashMap<GodotShapeOwner3D *, int> &get_owners() const; + + GodotShape3D() {} + virtual ~GodotShape3D(); +}; + +class GodotConcaveShape3D : public GodotShape3D { +public: + virtual bool is_concave() const override { return true; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + + // Returns true to stop the query. + typedef bool (*QueryCallback)(void *p_userdata, GodotShape3D *p_convex); + + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const = 0; + + GodotConcaveShape3D() {} +}; + +class GodotWorldBoundaryShape3D : public GodotShape3D { + Plane plane; + + void _setup(const Plane &p_plane); + +public: + Plane get_plane() const; + + virtual real_t get_volume() const override { return INFINITY; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_WORLD_BOUNDARY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotWorldBoundaryShape3D(); +}; + +class GodotSeparationRayShape3D : public GodotShape3D { + real_t length = 1.0; + bool slide_on_slope = false; + + void _setup(real_t p_length, bool p_slide_on_slope); + +public: + real_t get_length() const; + bool get_slide_on_slope() const; + + virtual real_t get_volume() const override { return 0.0; } + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SEPARATION_RAY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotSeparationRayShape3D(); +}; + +class GodotSphereShape3D : public GodotShape3D { + real_t radius = 0.0; + + void _setup(real_t p_radius); + +public: + real_t get_radius() const; + + virtual real_t get_volume() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SPHERE; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotSphereShape3D(); +}; + +class GodotBoxShape3D : public GodotShape3D { + Vector3 half_extents; + void _setup(const Vector3 &p_half_extents); + +public: + _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; } + virtual real_t get_volume() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_BOX; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotBoxShape3D(); +}; + +class GodotCapsuleShape3D : public GodotShape3D { + real_t height = 0.0; + real_t radius = 0.0; + + void _setup(real_t p_height, real_t p_radius); + +public: + _FORCE_INLINE_ real_t get_height() const { return height; } + _FORCE_INLINE_ real_t get_radius() const { return radius; } + + virtual real_t get_volume() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CAPSULE; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotCapsuleShape3D(); +}; + +class GodotCylinderShape3D : public GodotShape3D { + real_t height = 0.0; + real_t radius = 0.0; + + void _setup(real_t p_height, real_t p_radius); + +public: + _FORCE_INLINE_ real_t get_height() const { return height; } + _FORCE_INLINE_ real_t get_radius() const { return radius; } + + virtual real_t get_volume() const override { return height * Math_PI * radius * radius; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CYLINDER; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotCylinderShape3D(); +}; + +struct GodotConvexPolygonShape3D : public GodotShape3D { + Geometry3D::MeshData mesh; + LocalVector<int> extreme_vertices; + LocalVector<LocalVector<int>> vertex_neighbors; + + void _setup(const Vector<Vector3> &p_vertices); + +public: + const Geometry3D::MeshData &get_mesh() const { return mesh; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotConvexPolygonShape3D(); +}; + +struct _Volume_BVH; +struct GodotFaceShape3D; + +struct GodotConcavePolygonShape3D : public GodotConcaveShape3D { + // always a trimesh + + struct Face { + Vector3 normal; + int indices[3] = {}; + }; + + Vector<Face> faces; + Vector<Vector3> vertices; + + struct BVH { + AABB aabb; + int left = 0; + int right = 0; + + int face_index = 0; + }; + + Vector<BVH> bvh; + + struct _CullParams { + AABB aabb; + QueryCallback callback = nullptr; + void *userdata = nullptr; + const Face *faces = nullptr; + const Vector3 *vertices = nullptr; + const BVH *bvh = nullptr; + GodotFaceShape3D *face = nullptr; + }; + + struct _SegmentCullParams { + Vector3 from; + Vector3 to; + Vector3 dir; + const Face *faces = nullptr; + const Vector3 *vertices = nullptr; + const BVH *bvh = nullptr; + GodotFaceShape3D *face = nullptr; + + Vector3 result; + Vector3 normal; + int face_index = -1; + real_t min_d = 1e20; + int collisions = 0; + }; + + bool backface_collision = false; + + void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; + bool _cull(int p_idx, _CullParams *p_params) const; + + void _fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); + + void _setup(const Vector<Vector3> &p_faces, bool p_backface_collision); + +public: + Vector<Vector3> get_faces() const; + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotConcavePolygonShape3D(); +}; + +struct GodotHeightMapShape3D : public GodotConcaveShape3D { + Vector<real_t> heights; + int width = 0; + int depth = 0; + Vector3 local_origin; + + // Accelerator. + struct Range { + real_t min = 0.0; + real_t max = 0.0; + }; + LocalVector<Range> bounds_grid; + int bounds_grid_width = 0; + int bounds_grid_depth = 0; + + static const int BOUNDS_CHUNK_SIZE = 16; + + _FORCE_INLINE_ const Range &_get_bounds_chunk(int p_x, int p_z) const { + return bounds_grid[(p_z * bounds_grid_width) + p_x]; + } + + _FORCE_INLINE_ real_t _get_height(int p_x, int p_z) const { + return heights[(p_z * width) + p_x]; + } + + _FORCE_INLINE_ void _get_point(int p_x, int p_z, Vector3 &r_point) const { + r_point.x = p_x - 0.5 * (width - 1.0); + r_point.y = _get_height(p_x, p_z); + r_point.z = p_z - 0.5 * (depth - 1.0); + } + + void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const; + + void _build_accelerator(); + + template <typename ProcessFunction> + bool _intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const; + + void _setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height); + +public: + Vector<real_t> get_heights() const; + int get_width() const; + int get_depth() const; + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_HEIGHTMAP; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override; + virtual Variant get_data() const override; + + GodotHeightMapShape3D(); +}; + +//used internally +struct GodotFaceShape3D : public GodotShape3D { + Vector3 normal; //cache + Vector3 vertex[3]; + bool backface_collision = false; + bool invert_backface_collision = false; + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; } + + const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override; + virtual Vector3 get_support(const Vector3 &p_normal) const override; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; + + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } + + GodotFaceShape3D(); +}; + +struct GodotMotionShape3D : public GodotShape3D { + GodotShape3D *shape = nullptr; + Vector3 motion; + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } + + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { + Vector3 cast = p_transform.basis.xform(motion); + real_t mina, maxa; + real_t minb, maxb; + Transform3D ofsb = p_transform; + ofsb.origin += cast; + shape->project_range(p_normal, p_transform, mina, maxa); + shape->project_range(p_normal, ofsb, minb, maxb); + r_min = MIN(mina, minb); + r_max = MAX(maxa, maxb); + } + + virtual Vector3 get_support(const Vector3 &p_normal) const override { + Vector3 support = shape->get_support(p_normal); + if (p_normal.dot(motion) > 0) { + support += motion; + } + return support; + } + + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override { return false; } + virtual bool intersect_point(const Vector3 &p_point) const override { return false; } + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override { return p_point; } + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); } + + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } + + GodotMotionShape3D() { configure(AABB()); } +}; + +#endif // GODOT_SHAPE_3D_H diff --git a/modules/godot_physics_3d/godot_soft_body_3d.cpp b/modules/godot_physics_3d/godot_soft_body_3d.cpp new file mode 100644 index 0000000000..7284076a47 --- /dev/null +++ b/modules/godot_physics_3d/godot_soft_body_3d.cpp @@ -0,0 +1,1295 @@ +/**************************************************************************/ +/* godot_soft_body_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_soft_body_3d.h" + +#include "godot_space_3d.h" + +#include "core/math/geometry_3d.h" +#include "core/templates/rb_map.h" +#include "servers/rendering_server.h" + +// Based on Bullet soft body. + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ +///btSoftBody implementation by Nathanael Presson + +GodotSoftBody3D::GodotSoftBody3D() : + GodotCollisionObject3D(TYPE_SOFT_BODY), + active_list(this) { + _set_static(false); +} + +void GodotSoftBody3D::_shapes_changed() { +} + +void GodotSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + _set_transform(p_variant); + _set_inv_transform(get_transform().inverse()); + + apply_nodes_transform(get_transform()); + + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + // Not supported. + ERR_FAIL_MSG("Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_MSG("Angular velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies."); + } break; + } +} + +Variant GodotSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const { + switch (p_state) { + case PhysicsServer3D::BODY_STATE_TRANSFORM: { + return get_transform(); + } break; + case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Linear velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: { + ERR_FAIL_V_MSG(Vector3(), "Angular velocity is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_SLEEPING: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + case PhysicsServer3D::BODY_STATE_CAN_SLEEP: { + ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies."); + } break; + } + + return Variant(); +} + +void GodotSoftBody3D::set_space(GodotSpace3D *p_space) { + if (get_space()) { + get_space()->soft_body_remove_from_active_list(&active_list); + + deinitialize_shape(); + } + + _set_space(p_space); + + if (get_space()) { + get_space()->soft_body_add_to_active_list(&active_list); + + if (bounds != AABB()) { + initialize_shape(true); + } + } +} + +void GodotSoftBody3D::set_mesh(RID p_mesh) { + destroy(); + + soft_mesh = p_mesh; + + if (soft_mesh.is_null()) { + return; + } + + Array arrays = RenderingServer::get_singleton()->mesh_surface_get_arrays(soft_mesh, 0); + ERR_FAIL_COND(arrays.is_empty()); + + const Vector<int> &indices = arrays[RenderingServer::ARRAY_INDEX]; + const Vector<Vector3> &vertices = arrays[RenderingServer::ARRAY_VERTEX]; + ERR_FAIL_COND_MSG(indices.is_empty(), "Soft body's mesh needs to have indices"); + ERR_FAIL_COND_MSG(vertices.is_empty(), "Soft body's mesh needs to have vertices"); + + bool success = create_from_trimesh(indices, vertices); + if (!success) { + destroy(); + } +} + +void GodotSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) { + if (soft_mesh.is_null()) { + return; + } + + const uint32_t vertex_count = map_visual_to_physics.size(); + for (uint32_t i = 0; i < vertex_count; ++i) { + const uint32_t node_index = map_visual_to_physics[i]; + const Node &node = nodes[node_index]; + + p_rendering_server_handler->set_vertex(i, node.x); + p_rendering_server_handler->set_normal(i, node.n); + } + + p_rendering_server_handler->set_aabb(bounds); +} + +void GodotSoftBody3D::update_normals_and_centroids() { + for (Node &node : nodes) { + node.n = Vector3(); + } + + for (Face &face : faces) { + const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x); + face.n[0]->n += n; + face.n[1]->n += n; + face.n[2]->n += n; + face.normal = n; + face.normal.normalize(); + face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x); + } + + for (Node &node : nodes) { + real_t len = node.n.length(); + if (len > CMP_EPSILON) { + node.n /= len; + } + } +} + +void GodotSoftBody3D::update_bounds() { + AABB prev_bounds = bounds; + prev_bounds.grow_by(collision_margin); + + bounds = AABB(); + + const uint32_t nodes_count = nodes.size(); + if (nodes_count == 0) { + deinitialize_shape(); + return; + } + + bool first = true; + bool moved = false; + for (uint32_t node_index = 0; node_index < nodes_count; ++node_index) { + const Node &node = nodes[node_index]; + if (!prev_bounds.has_point(node.x)) { + moved = true; + } + if (first) { + bounds.position = node.x; + first = false; + } else { + bounds.expand_to(node.x); + } + } + + if (get_space()) { + initialize_shape(moved); + } +} + +void GodotSoftBody3D::update_constants() { + reset_link_rest_lengths(); + update_link_constants(); + update_area(); +} + +void GodotSoftBody3D::update_area() { + int i, ni; + + // Face area. + for (Face &face : faces) { + const Vector3 &x0 = face.n[0]->x; + const Vector3 &x1 = face.n[1]->x; + const Vector3 &x2 = face.n[2]->x; + + const Vector3 a = x1 - x0; + const Vector3 b = x2 - x0; + const Vector3 cr = vec3_cross(a, b); + face.ra = cr.length() * 0.5; + } + + // Node area. + LocalVector<int> counts; + if (nodes.size() > 0) { + counts.resize(nodes.size()); + memset(counts.ptr(), 0, counts.size() * sizeof(int)); + } + + for (Node &node : nodes) { + node.area = 0.0; + } + + for (const Face &face : faces) { + for (int j = 0; j < 3; ++j) { + const int index = (int)(face.n[j] - &nodes[0]); + counts[index]++; + face.n[j]->area += Math::abs(face.ra); + } + } + + for (i = 0, ni = nodes.size(); i < ni; ++i) { + if (counts[i] > 0) { + nodes[i].area /= (real_t)counts[i]; + } else { + nodes[i].area = 0.0; + } + } +} + +void GodotSoftBody3D::reset_link_rest_lengths() { + for (Link &link : links) { + link.rl = (link.n[0]->x - link.n[1]->x).length(); + link.c1 = link.rl * link.rl; + } +} + +void GodotSoftBody3D::update_link_constants() { + real_t inv_linear_stiffness = 1.0 / linear_stiffness; + for (Link &link : links) { + link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness; + } +} + +void GodotSoftBody3D::apply_nodes_transform(const Transform3D &p_transform) { + if (soft_mesh.is_null()) { + return; + } + + uint32_t node_count = nodes.size(); + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + + node.x = p_transform.xform(node.x); + node.q = node.x; + node.v = Vector3(); + node.bv = Vector3(); + + AABB node_aabb(node.x, leaf_size); + node_tree.update(node.leaf, node_aabb); + } + + face_tree.clear(); + + update_normals_and_centroids(); + update_bounds(); + update_constants(); +} + +Vector3 GodotSoftBody3D::get_vertex_position(int p_index) const { + ERR_FAIL_COND_V(p_index < 0, Vector3()); + + if (soft_mesh.is_null()) { + return Vector3(); + } + + ERR_FAIL_COND_V(p_index >= (int)map_visual_to_physics.size(), Vector3()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3()); + return nodes[node_index].x; +} + +void GodotSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) { + ERR_FAIL_COND(p_index < 0); + + if (soft_mesh.is_null()) { + return; + } + + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.q = node.x; + node.x = p_position; +} + +void GodotSoftBody3D::pin_vertex(int p_index) { + ERR_FAIL_COND(p_index < 0); + + if (is_vertex_pinned(p_index)) { + return; + } + + pinned_vertices.push_back(p_index); + + if (!soft_mesh.is_null()) { + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = 0.0; + } +} + +void GodotSoftBody3D::unpin_vertex(int p_index) { + ERR_FAIL_COND(p_index < 0); + + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + pinned_vertices.remove_at(i); + + if (!soft_mesh.is_null()) { + ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[p_index]; + + ERR_FAIL_COND(node_index >= nodes.size()); + real_t inv_node_mass = nodes.size() * inv_total_mass; + + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + + return; + } + } +} + +void GodotSoftBody3D::unpin_all_vertices() { + if (!soft_mesh.is_null()) { + real_t inv_node_mass = nodes.size() * inv_total_mass; + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + int pinned_vertex = pinned_vertices[i]; + + ERR_CONTINUE(pinned_vertex >= (int)map_visual_to_physics.size()); + uint32_t node_index = map_visual_to_physics[pinned_vertex]; + + ERR_CONTINUE(node_index >= nodes.size()); + Node &node = nodes[node_index]; + node.im = inv_node_mass; + } + } + + pinned_vertices.clear(); +} + +bool GodotSoftBody3D::is_vertex_pinned(int p_index) const { + ERR_FAIL_COND_V(p_index < 0, false); + + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + if (p_index == pinned_vertices[i]) { + return true; + } + } + + return false; +} + +uint32_t GodotSoftBody3D::get_node_count() const { + return nodes.size(); +} + +real_t GodotSoftBody3D::get_node_inv_mass(uint32_t p_node_index) const { + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), 0.0); + return nodes[p_node_index].im; +} + +Vector3 GodotSoftBody3D::get_node_position(uint32_t p_node_index) const { + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); + return nodes[p_node_index].x; +} + +Vector3 GodotSoftBody3D::get_node_velocity(uint32_t p_node_index) const { + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); + return nodes[p_node_index].v; +} + +Vector3 GodotSoftBody3D::get_node_biased_velocity(uint32_t p_node_index) const { + ERR_FAIL_UNSIGNED_INDEX_V(p_node_index, nodes.size(), Vector3()); + return nodes[p_node_index].bv; +} + +void GodotSoftBody3D::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_UNSIGNED_INDEX(p_node_index, nodes.size()); + Node &node = nodes[p_node_index]; + node.v += p_impulse * node.im; +} + +void GodotSoftBody3D::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { + ERR_FAIL_UNSIGNED_INDEX(p_node_index, nodes.size()); + Node &node = nodes[p_node_index]; + node.bv += p_impulse * node.im; +} + +uint32_t GodotSoftBody3D::get_face_count() const { + return faces.size(); +} + +void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { + ERR_FAIL_UNSIGNED_INDEX(p_face_index, faces.size()); + const Face &face = faces[p_face_index]; + r_point_1 = face.n[0]->x; + r_point_2 = face.n[1]->x; + r_point_3 = face.n[2]->x; +} + +Vector3 GodotSoftBody3D::get_face_normal(uint32_t p_face_index) const { + ERR_FAIL_UNSIGNED_INDEX_V(p_face_index, faces.size(), Vector3()); + return faces[p_face_index].normal; +} + +bool GodotSoftBody3D::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) { + ERR_FAIL_COND_V(p_indices.is_empty(), false); + ERR_FAIL_COND_V(p_vertices.is_empty(), false); + + uint32_t node_count = 0; + LocalVector<Vector3> vertices; + const int visual_vertex_count(p_vertices.size()); + + LocalVector<int> triangles; + const uint32_t triangle_count(p_indices.size() / 3); + triangles.resize(triangle_count * 3); + + // Merge all overlapping vertices and create a map of physical vertices to visual vertices. + { + // Process vertices. + { + uint32_t vertex_count = 0; + HashMap<Vector3, uint32_t> unique_vertices; + + vertices.resize(visual_vertex_count); + map_visual_to_physics.resize(visual_vertex_count); + + for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) { + const Vector3 &vertex = p_vertices[visual_vertex_index]; + + HashMap<Vector3, uint32_t>::Iterator e = unique_vertices.find(vertex); + uint32_t vertex_id; + if (e) { + // Already existing. + vertex_id = e->value; + } else { + // Create new one. + vertex_id = vertex_count++; + unique_vertices[vertex] = vertex_id; + vertices[vertex_id] = vertex; + } + + map_visual_to_physics[visual_vertex_index] = vertex_id; + } + + vertices.resize(vertex_count); + } + + // Process triangles. + { + for (uint32_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index) { + for (int i = 0; i < 3; ++i) { + int visual_index = 3 * triangle_index + i; + int physics_index = map_visual_to_physics[p_indices[visual_index]]; + triangles[visual_index] = physics_index; + node_count = MAX((int)node_count, physics_index); + } + } + } + } + + ++node_count; + + // Create nodes from vertices. + nodes.resize(node_count); + real_t inv_node_mass = node_count * inv_total_mass; + Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0; + for (uint32_t i = 0; i < node_count; ++i) { + Node &node = nodes[i]; + node.s = vertices[i]; + node.x = node.s; + node.q = node.s; + node.im = inv_node_mass; + + AABB node_aabb(node.x, leaf_size); + node.leaf = node_tree.insert(node_aabb, &node); + + node.index = i; + } + + // Create links and faces from triangles. + LocalVector<bool> chks; + chks.resize(node_count * node_count); + memset(chks.ptr(), 0, chks.size() * sizeof(bool)); + + for (uint32_t i = 0; i < triangle_count * 3; i += 3) { + const int idx[] = { triangles[i], triangles[i + 1], triangles[i + 2] }; + + for (int j = 2, k = 0; k < 3; j = k++) { + int chk = idx[k] * node_count + idx[j]; + if (!chks[chk]) { + chks[chk] = true; + int inv_chk = idx[j] * node_count + idx[k]; + chks[inv_chk] = true; + + append_link(idx[j], idx[k]); + } + } + + append_face(idx[0], idx[1], idx[2]); + } + + // Set pinned nodes. + uint32_t pinned_count = pinned_vertices.size(); + for (uint32_t i = 0; i < pinned_count; ++i) { + int pinned_vertex = pinned_vertices[i]; + + ERR_CONTINUE(pinned_vertex >= visual_vertex_count); + uint32_t node_index = map_visual_to_physics[pinned_vertex]; + + ERR_CONTINUE(node_index >= node_count); + Node &node = nodes[node_index]; + node.im = 0.0; + } + + generate_bending_constraints(2); + reoptimize_link_order(); + + update_constants(); + update_normals_and_centroids(); + update_bounds(); + + return true; +} + +void GodotSoftBody3D::generate_bending_constraints(int p_distance) { + uint32_t i, j; + + if (p_distance > 1) { + // Build graph. + const uint32_t n = nodes.size(); + const unsigned inf = (~(unsigned)0) >> 1; + const uint32_t adj_size = n * n; + unsigned *adj = memnew_arr(unsigned, adj_size); + +#define IDX(_x_, _y_) ((_y_) * n + (_x_)) + for (j = 0; j < n; ++j) { + for (i = 0; i < n; ++i) { + int idx_ij = j * n + i; + int idx_ji = i * n + j; + if (i != j) { + adj[idx_ij] = adj[idx_ji] = inf; + } else { + adj[idx_ij] = adj[idx_ji] = 0; + } + } + } + for (Link &link : links) { + const int ia = (int)(link.n[0] - &nodes[0]); + const int ib = (int)(link.n[1] - &nodes[0]); + int idx = ib * n + ia; + int idx_inv = ia * n + ib; + adj[idx] = 1; + adj[idx_inv] = 1; + } + + // Special optimized case for distance == 2. + if (p_distance == 2) { + LocalVector<LocalVector<int>> node_links; + + // Build node links. + node_links.resize(nodes.size()); + + for (Link &link : links) { + const int ia = (int)(link.n[0] - &nodes[0]); + const int ib = (int)(link.n[1] - &nodes[0]); + if (!node_links[ia].has(ib)) { + node_links[ia].push_back(ib); + } + + if (!node_links[ib].has(ia)) { + node_links[ib].push_back(ia); + } + } + for (uint32_t ii = 0; ii < node_links.size(); ii++) { + for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) { + int k = node_links[ii][jj]; + for (const int &l : node_links[k]) { + if ((int)ii != l) { + int idx_ik = k * n + ii; + int idx_kj = l * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + ERR_FAIL_COND(sum != 2); + int idx_ij = l * n + ii; + if (adj[idx_ij] > sum) { + int idx_ji = l * n + ii; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + } else { + // Generic Floyd's algorithm. + for (uint32_t k = 0; k < n; ++k) { + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ik = k * n + i; + int idx_kj = j * n + k; + const unsigned sum = adj[idx_ik] + adj[idx_kj]; + int idx_ij = j * n + i; + if (adj[idx_ij] > sum) { + int idx_ji = j * n + i; + adj[idx_ij] = adj[idx_ji] = sum; + } + } + } + } + } + + // Build links. + for (j = 0; j < n; ++j) { + for (i = j + 1; i < n; ++i) { + int idx_ij = j * n + i; + if (adj[idx_ij] == (unsigned)p_distance) { + append_link(i, j); + } + } + } + memdelete_arr(adj); + } +} + +//=================================================================== +// +// +// This function takes in a list of interdependent Links and tries +// to maximize the distance between calculation +// of dependent links. This increases the amount of parallelism that can +// be exploited by out-of-order instruction processors with large but +// (inevitably) finite instruction windows. +// +//=================================================================== + +// A small structure to track lists of dependent link calculations. +class LinkDeps { +public: + // A link calculation that is dependent on this one. + // Positive values = "input A" while negative values = "input B". + int value; + // Next dependence in the list. + LinkDeps *next; +}; +typedef LinkDeps *LinkDepsPtr; + +void GodotSoftBody3D::reoptimize_link_order() { + const int reop_not_dependent = -1; + const int reop_node_complete = -2; + + uint32_t link_count = links.size(); + uint32_t node_count = nodes.size(); + + if (link_count < 1 || node_count < 2) { + return; + } + + uint32_t i; + Link *lr; + int ar, br; + Node *node0 = &(nodes[0]); + Node *node1 = &(nodes[1]); + LinkDepsPtr link_dep; + int ready_list_head, ready_list_tail, link_num, link_dep_frees, dep_link; + + // Allocate temporary buffers. + int *node_written_at = memnew_arr(int, node_count + 1); // What link calculation produced this node's current values? + int *link_dep_A = memnew_arr(int, link_count); // Link calculation input is dependent upon prior calculation #N + int *link_dep_B = memnew_arr(int, link_count); + int *ready_list = memnew_arr(int, link_count); // List of ready-to-process link calculations (# of links, maximum) + LinkDeps *link_dep_free_list = memnew_arr(LinkDeps, 2 * link_count); // Dependent-on-me list elements (2x# of links, maximum) + LinkDepsPtr *link_dep_list_starts = memnew_arr(LinkDepsPtr, link_count); // Start nodes of dependent-on-me lists, one for each link + + // Copy the original, unsorted links to a side buffer. + Link *link_buffer = memnew_arr(Link, link_count); + memcpy(link_buffer, &(links[0]), sizeof(Link) * link_count); + + // Clear out the node setup and ready list. + for (i = 0; i < node_count + 1; i++) { + node_written_at[i] = reop_not_dependent; + } + for (i = 0; i < link_count; i++) { + link_dep_list_starts[i] = nullptr; + } + ready_list_head = ready_list_tail = link_dep_frees = 0; + + // Initial link analysis to set up data structures. + for (i = 0; i < link_count; i++) { + // Note which prior link calculations we are dependent upon & build up dependence lists. + lr = &(links[i]); + ar = (lr->n[0] - node0) / (node1 - node0); + br = (lr->n[1] - node0) / (node1 - node0); + if (node_written_at[ar] > reop_not_dependent) { + link_dep_A[i] = node_written_at[ar]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = i; + link_dep->next = link_dep_list_starts[node_written_at[ar]]; + link_dep_list_starts[node_written_at[ar]] = link_dep; + } else { + link_dep_A[i] = reop_not_dependent; + } + if (node_written_at[br] > reop_not_dependent) { + link_dep_B[i] = node_written_at[br]; + link_dep = &link_dep_free_list[link_dep_frees++]; + link_dep->value = -(int)(i + 1); + link_dep->next = link_dep_list_starts[node_written_at[br]]; + link_dep_list_starts[node_written_at[br]] = link_dep; + } else { + link_dep_B[i] = reop_not_dependent; + } + + // Add this link to the initial ready list, if it is not dependent on any other links. + if ((link_dep_A[i] == reop_not_dependent) && (link_dep_B[i] == reop_not_dependent)) { + ready_list[ready_list_tail++] = i; + link_dep_A[i] = link_dep_B[i] = reop_node_complete; // Probably not needed now. + } + + // Update the nodes to mark which ones are calculated by this link. + node_written_at[ar] = node_written_at[br] = i; + } + + // Process the ready list and create the sorted list of links: + // -- By treating the ready list as a queue, we maximize the distance between any + // inter-dependent node calculations. + // -- All other (non-related) nodes in the ready list will automatically be inserted + // in between each set of inter-dependent link calculations by this loop. + i = 0; + while (ready_list_head != ready_list_tail) { + // Use ready list to select the next link to process. + link_num = ready_list[ready_list_head++]; + // Copy the next-to-calculate link back into the original link array. + links[i++] = link_buffer[link_num]; + + // Free up any link inputs that are dependent on this one. + link_dep = link_dep_list_starts[link_num]; + while (link_dep) { + dep_link = link_dep->value; + if (dep_link >= 0) { + link_dep_A[dep_link] = reop_not_dependent; + } else { + dep_link = -dep_link - 1; + link_dep_B[dep_link] = reop_not_dependent; + } + // Add this dependent link calculation to the ready list if *both* inputs are clear. + if ((link_dep_A[dep_link] == reop_not_dependent) && (link_dep_B[dep_link] == reop_not_dependent)) { + ready_list[ready_list_tail++] = dep_link; + link_dep_A[dep_link] = link_dep_B[dep_link] = reop_node_complete; // Probably not needed now. + } + link_dep = link_dep->next; + } + } + + // Delete the temporary buffers. + memdelete_arr(node_written_at); + memdelete_arr(link_dep_A); + memdelete_arr(link_dep_B); + memdelete_arr(ready_list); + memdelete_arr(link_dep_free_list); + memdelete_arr(link_dep_list_starts); + memdelete_arr(link_buffer); +} + +void GodotSoftBody3D::append_link(uint32_t p_node1, uint32_t p_node2) { + if (p_node1 == p_node2) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + + Link link; + link.n[0] = node1; + link.n[1] = node2; + link.rl = (node1->x - node2->x).length(); + + links.push_back(link); +} + +void GodotSoftBody3D::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) { + if (p_node1 == p_node2) { + return; + } + if (p_node1 == p_node3) { + return; + } + if (p_node2 == p_node3) { + return; + } + + Node *node1 = &nodes[p_node1]; + Node *node2 = &nodes[p_node2]; + Node *node3 = &nodes[p_node3]; + + Face face; + face.n[0] = node1; + face.n[1] = node2; + face.n[2] = node3; + + face.index = faces.size(); + + faces.push_back(face); +} + +void GodotSoftBody3D::set_iteration_count(int p_val) { + iteration_count = p_val; +} + +void GodotSoftBody3D::set_total_mass(real_t p_val) { + ERR_FAIL_COND(p_val < 0.0); + + inv_total_mass = 1.0 / p_val; + real_t mass_factor = total_mass * inv_total_mass; + total_mass = p_val; + + uint32_t node_count = nodes.size(); + for (uint32_t node_index = 0; node_index < node_count; ++node_index) { + Node &node = nodes[node_index]; + node.im *= mass_factor; + } + + update_constants(); +} + +void GodotSoftBody3D::set_collision_margin(real_t p_val) { + collision_margin = p_val; +} + +void GodotSoftBody3D::set_linear_stiffness(real_t p_val) { + linear_stiffness = p_val; +} + +void GodotSoftBody3D::set_pressure_coefficient(real_t p_val) { + pressure_coefficient = p_val; +} + +void GodotSoftBody3D::set_damping_coefficient(real_t p_val) { + damping_coefficient = p_val; +} + +void GodotSoftBody3D::set_drag_coefficient(real_t p_val) { + drag_coefficient = p_val; +} + +void GodotSoftBody3D::add_velocity(const Vector3 &p_velocity) { + for (Node &node : nodes) { + if (node.im > 0) { + node.v += p_velocity; + } + } +} + +void GodotSoftBody3D::apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas) { + if (nodes.is_empty()) { + return; + } + + int32_t j; + + real_t volume = 0.0; + const Vector3 &org = nodes[0].x; + + // Iterate over faces (try not to iterate elsewhere if possible). + for (const Face &face : faces) { + Vector3 wind_force(0, 0, 0); + + // Compute volume. + volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org)); + + // Compute nodal forces from area winds. + if (!p_wind_areas.is_empty()) { + for (const GodotArea3D *area : p_wind_areas) { + wind_force += _compute_area_windforce(area, &face); + } + + for (j = 0; j < 3; j++) { + Node *current_node = face.n[j]; + current_node->f += wind_force; + } + } + } + volume /= 6.0; + + // Apply nodal pressure forces. + if (pressure_coefficient > CMP_EPSILON) { + real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient; + for (Node &node : nodes) { + if (node.im > 0) { + node.f += node.n * (node.area * ivolumetp); + } + } + } +} + +Vector3 GodotSoftBody3D::_compute_area_windforce(const GodotArea3D *p_area, const Face *p_face) { + real_t wfm = p_area->get_wind_force_magnitude(); + real_t waf = p_area->get_wind_attenuation_factor(); + const Vector3 &wd = p_area->get_wind_direction(); + const Vector3 &ws = p_area->get_wind_source(); + real_t projection_on_tri_normal = vec3_dot(p_face->normal, wd); + real_t projection_toward_centroid = vec3_dot(p_face->centroid - ws, wd); + real_t attenuation_over_distance = pow(projection_toward_centroid, -waf); + real_t nodal_force_magnitude = wfm * 0.33333333333 * p_face->ra * projection_on_tri_normal * attenuation_over_distance; + return nodal_force_magnitude * p_face->normal; +} + +void GodotSoftBody3D::predict_motion(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + ERR_FAIL_NULL(get_space()); + + bool gravity_done = false; + Vector3 gravity; + + LocalVector<GodotArea3D *> wind_areas; + + int ac = areas.size(); + if (ac) { + areas.sort(); + const AreaCMP *aa = &areas[0]; + for (int i = ac - 1; i >= 0; i--) { + if (!gravity_done) { + PhysicsServer3D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE); + if (area_gravity_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { + Vector3 area_gravity; + aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity); + switch (area_gravity_mode) { + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: { + gravity += area_gravity; + gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE; + } break; + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: + case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: { + gravity = area_gravity; + gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE; + } break; + default: { + } + } + } + } + + if (aa[i].area->get_wind_force_magnitude() > CMP_EPSILON) { + wind_areas.push_back(aa[i].area); + } + } + } + + // Add default gravity and damping from space area. + if (!gravity_done) { + GodotArea3D *default_area = get_space()->get_default_area(); + ERR_FAIL_NULL(default_area); + + Vector3 default_gravity; + default_area->compute_gravity(get_transform().get_origin(), default_gravity); + gravity += default_gravity; + } + + // Apply forces. + add_velocity(gravity * p_delta); + if (pressure_coefficient > CMP_EPSILON || !wind_areas.is_empty()) { + apply_forces(wind_areas); + } + + // Avoid soft body from 'exploding' so use some upper threshold of maximum motion + // that a node can travel per frame. + const real_t max_displacement = 1000.0; + real_t clamp_delta_v = max_displacement * inv_delta; + + // Integrate. + for (Node &node : nodes) { + node.q = node.x; + Vector3 delta_v = node.f * node.im * p_delta; + for (int c = 0; c < 3; c++) { + delta_v[c] = CLAMP(delta_v[c], -clamp_delta_v, clamp_delta_v); + } + node.v += delta_v; + node.x += node.v * p_delta; + node.f = Vector3(); + } + + // Bounds and tree update. + update_bounds(); + + // Node tree update. + for (const Node &node : nodes) { + AABB node_aabb(node.x, Vector3()); + node_aabb.expand_to(node.x + node.v * p_delta); + node_aabb.grow_by(collision_margin); + + node_tree.update(node.leaf, node_aabb); + } + + // Face tree update. + if (!face_tree.is_empty()) { + update_face_tree(p_delta); + } + + // Optimize node tree. + node_tree.optimize_incremental(1); + face_tree.optimize_incremental(1); +} + +void GodotSoftBody3D::solve_constraints(real_t p_delta) { + const real_t inv_delta = 1.0 / p_delta; + + for (Link &link : links) { + link.c3 = link.n[1]->q - link.n[0]->q; + link.c2 = 1 / (link.c3.length_squared() * link.c0); + } + + // Solve velocities. + for (Node &node : nodes) { + node.x = node.q + node.v * p_delta; + } + + // Solve positions. + for (int isolve = 0; isolve < iteration_count; ++isolve) { + const real_t ti = isolve / (real_t)iteration_count; + solve_links(1.0, ti); + } + const real_t vc = (1.0 - damping_coefficient) * inv_delta; + for (Node &node : nodes) { + node.x += node.bv * p_delta; + node.bv = Vector3(); + + node.v = (node.x - node.q) * vc; + + node.q = node.x; + } + + update_normals_and_centroids(); +} + +void GodotSoftBody3D::solve_links(real_t kst, real_t ti) { + for (Link &link : links) { + if (link.c0 > 0) { + Node &node_a = *link.n[0]; + Node &node_b = *link.n[1]; + const Vector3 del = node_b.x - node_a.x; + const real_t len = del.length_squared(); + if (link.c1 + len > CMP_EPSILON) { + const real_t k = ((link.c1 - len) / (link.c0 * (link.c1 + len))) * kst; + node_a.x -= del * (k * node_a.im); + node_b.x += del * (k * node_b.im); + } + } + } +} + +struct AABBQueryResult { + const GodotSoftBody3D *soft_body = nullptr; + void *userdata = nullptr; + GodotSoftBody3D::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_node_index(p_data), userdata); + }; +}; + +void GodotSoftBody3D::query_aabb(const AABB &p_aabb, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) { + AABBQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + node_tree.aabb_query(p_aabb, query_result); +} + +struct RayQueryResult { + const GodotSoftBody3D *soft_body = nullptr; + void *userdata = nullptr; + GodotSoftBody3D::QueryResultCallback result_callback = nullptr; + + _FORCE_INLINE_ bool operator()(void *p_data) { + return result_callback(soft_body->get_face_index(p_data), userdata); + }; +}; + +void GodotSoftBody3D::query_ray(const Vector3 &p_from, const Vector3 &p_to, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) { + if (face_tree.is_empty()) { + initialize_face_tree(); + } + + RayQueryResult query_result; + query_result.soft_body = this; + query_result.result_callback = p_result_callback; + query_result.userdata = p_userdata; + + face_tree.ray_query(p_from, p_to, query_result); +} + +void GodotSoftBody3D::initialize_face_tree() { + face_tree.clear(); + for (Face &face : faces) { + AABB face_aabb; + + face_aabb.position = face.n[0]->x; + face_aabb.expand_to(face.n[1]->x); + face_aabb.expand_to(face.n[2]->x); + + face_aabb.grow_by(collision_margin); + + face.leaf = face_tree.insert(face_aabb, &face); + } +} + +void GodotSoftBody3D::update_face_tree(real_t p_delta) { + for (const Face &face : faces) { + AABB face_aabb; + + const Node *node0 = face.n[0]; + face_aabb.position = node0->x; + face_aabb.expand_to(node0->x + node0->v * p_delta); + + const Node *node1 = face.n[1]; + face_aabb.expand_to(node1->x); + face_aabb.expand_to(node1->x + node1->v * p_delta); + + const Node *node2 = face.n[2]; + face_aabb.expand_to(node2->x); + face_aabb.expand_to(node2->x + node2->v * p_delta); + + face_aabb.grow_by(collision_margin); + + face_tree.update(face.leaf, face_aabb); + } +} + +void GodotSoftBody3D::initialize_shape(bool p_force_move) { + if (get_shape_count() == 0) { + GodotSoftBodyShape3D *soft_body_shape = memnew(GodotSoftBodyShape3D(this)); + add_shape(soft_body_shape); + } else if (p_force_move) { + GodotSoftBodyShape3D *soft_body_shape = static_cast<GodotSoftBodyShape3D *>(get_shape(0)); + soft_body_shape->update_bounds(); + } +} + +void GodotSoftBody3D::deinitialize_shape() { + if (get_shape_count() > 0) { + GodotShape3D *shape = get_shape(0); + remove_shape(shape); + memdelete(shape); + } +} + +void GodotSoftBody3D::destroy() { + soft_mesh = RID(); + + map_visual_to_physics.clear(); + + node_tree.clear(); + face_tree.clear(); + + nodes.clear(); + links.clear(); + faces.clear(); + + bounds = AABB(); + deinitialize_shape(); +} + +void GodotSoftBodyShape3D::update_bounds() { + ERR_FAIL_NULL(soft_body); + + AABB collision_aabb = soft_body->get_bounds(); + collision_aabb.grow_by(soft_body->get_collision_margin()); + configure(collision_aabb); +} + +GodotSoftBodyShape3D::GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body) { + soft_body = p_soft_body; + update_bounds(); +} + +struct _SoftBodyIntersectSegmentInfo { + const GodotSoftBody3D *soft_body = nullptr; + Vector3 from; + Vector3 dir; + Vector3 hit_position; + uint32_t hit_face_index = -1; + real_t hit_dist_sq = INFINITY; + + static bool process_hit(uint32_t p_face_index, void *p_userdata) { + _SoftBodyIntersectSegmentInfo &query_info = *(static_cast<_SoftBodyIntersectSegmentInfo *>(p_userdata)); + + Vector3 points[3]; + query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]); + + Vector3 result; + if (Geometry3D::ray_intersects_triangle(query_info.from, query_info.dir, points[0], points[1], points[2], &result)) { + real_t dist_sq = query_info.from.distance_squared_to(result); + if (dist_sq < query_info.hit_dist_sq) { + query_info.hit_dist_sq = dist_sq; + query_info.hit_position = result; + query_info.hit_face_index = p_face_index; + } + } + + // Continue with the query. + return false; + } +}; + +bool GodotSoftBodyShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const { + _SoftBodyIntersectSegmentInfo query_info; + query_info.soft_body = soft_body; + query_info.from = p_begin; + query_info.dir = (p_end - p_begin).normalized(); + + soft_body->query_ray(p_begin, p_end, _SoftBodyIntersectSegmentInfo::process_hit, &query_info); + + if (query_info.hit_dist_sq != INFINITY) { + r_result = query_info.hit_position; + r_normal = soft_body->get_face_normal(query_info.hit_face_index); + return true; + } + + return false; +} + +bool GodotSoftBodyShape3D::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 GodotSoftBodyShape3D::get_closest_point_to(const Vector3 &p_point) const { + ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies."); +} diff --git a/modules/godot_physics_3d/godot_soft_body_3d.h b/modules/godot_physics_3d/godot_soft_body_3d.h new file mode 100644 index 0000000000..e23f4bb9f5 --- /dev/null +++ b/modules/godot_physics_3d/godot_soft_body_3d.h @@ -0,0 +1,276 @@ +/**************************************************************************/ +/* godot_soft_body_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_SOFT_BODY_3D_H +#define GODOT_SOFT_BODY_3D_H + +#include "godot_area_3d.h" +#include "godot_collision_object_3d.h" + +#include "core/math/aabb.h" +#include "core/math/dynamic_bvh.h" +#include "core/math/vector3.h" +#include "core/templates/hash_set.h" +#include "core/templates/local_vector.h" +#include "core/templates/vset.h" + +class GodotConstraint3D; + +class GodotSoftBody3D : public GodotCollisionObject3D { + RID soft_mesh; + + struct Node { + Vector3 s; // Source position + Vector3 x; // Position + Vector3 q; // Previous step position/Test position + Vector3 f; // Force accumulator + Vector3 v; // Velocity + Vector3 bv; // Biased Velocity + Vector3 n; // Normal + real_t area = 0.0; // Area + real_t im = 0.0; // 1/mass + DynamicBVH::ID leaf; // Leaf data + uint32_t index = 0; + }; + + struct Link { + Vector3 c3; // gradient + Node *n[2] = { nullptr, nullptr }; // Node pointers + real_t rl = 0.0; // Rest length + real_t c0 = 0.0; // (ima+imb)*kLST + real_t c1 = 0.0; // rl^2 + real_t c2 = 0.0; // |gradient|^2/c0 + }; + + struct Face { + Vector3 centroid; + Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers + Vector3 normal; // Normal + real_t ra = 0.0; // Rest area + DynamicBVH::ID leaf; // Leaf data + uint32_t index = 0; + }; + + LocalVector<Node> nodes; + LocalVector<Link> links; + LocalVector<Face> faces; + + DynamicBVH node_tree; + DynamicBVH face_tree; + + LocalVector<uint32_t> map_visual_to_physics; + + AABB bounds; + + real_t collision_margin = 0.05; + + real_t total_mass = 1.0; + real_t inv_total_mass = 1.0; + + int iteration_count = 5; + real_t linear_stiffness = 0.5; // [0,1] + real_t pressure_coefficient = 0.0; // [-inf,+inf] + real_t damping_coefficient = 0.01; // [0,1] + real_t drag_coefficient = 0.0; // [0,1] + LocalVector<int> pinned_vertices; + + SelfList<GodotSoftBody3D> active_list; + + HashSet<GodotConstraint3D *> constraints; + + Vector<AreaCMP> areas; + + VSet<RID> exceptions; + + uint64_t island_step = 0; + + _FORCE_INLINE_ Vector3 _compute_area_windforce(const GodotArea3D *p_area, const Face *p_face); + +public: + GodotSoftBody3D(); + + const AABB &get_bounds() const { return bounds; } + + void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); + Variant get_state(PhysicsServer3D::BodyState p_state) const; + + _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const HashSet<GodotConstraint3D *> &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } + + _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } + _FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); } + _FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); } + _FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; } + + _FORCE_INLINE_ void add_area(GodotArea3D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount += 1; + } else { + areas.ordered_insert(AreaCMP(p_area)); + } + } + + _FORCE_INLINE_ void remove_area(GodotArea3D *p_area) { + int index = areas.find(AreaCMP(p_area)); + if (index > -1) { + areas.write[index].refCount -= 1; + if (areas[index].refCount < 1) { + areas.remove_at(index); + } + } + } + + virtual void set_space(GodotSpace3D *p_space) override; + + void set_mesh(RID p_mesh); + + void update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler); + + Vector3 get_vertex_position(int p_index) const; + void set_vertex_position(int p_index, const Vector3 &p_position); + + void pin_vertex(int p_index); + void unpin_vertex(int p_index); + void unpin_all_vertices(); + bool is_vertex_pinned(int p_index) const; + + uint32_t get_node_count() const; + real_t get_node_inv_mass(uint32_t p_node_index) const; + Vector3 get_node_position(uint32_t p_node_index) const; + Vector3 get_node_velocity(uint32_t p_node_index) const; + Vector3 get_node_biased_velocity(uint32_t p_node_index) const; + void apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse); + void apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse); + + uint32_t get_face_count() const; + void get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const; + Vector3 get_face_normal(uint32_t p_face_index) const; + + void set_iteration_count(int p_val); + _FORCE_INLINE_ real_t get_iteration_count() const { return iteration_count; } + + void set_total_mass(real_t p_val); + _FORCE_INLINE_ real_t get_total_mass() const { return total_mass; } + _FORCE_INLINE_ real_t get_total_inv_mass() const { return inv_total_mass; } + + void set_collision_margin(real_t p_val); + _FORCE_INLINE_ real_t get_collision_margin() const { return collision_margin; } + + void set_linear_stiffness(real_t p_val); + _FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; } + + void set_pressure_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; } + + void set_damping_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; } + + void set_drag_coefficient(real_t p_val); + _FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; } + + void predict_motion(real_t p_delta); + void solve_constraints(real_t p_delta); + + _FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return static_cast<Node *>(p_node)->index; } + _FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return static_cast<Face *>(p_face)->index; } + + // Return true to stop the query. + // p_index is the node index for AABB query, face index for Ray query. + typedef bool (*QueryResultCallback)(uint32_t p_index, void *p_userdata); + + void query_aabb(const AABB &p_aabb, QueryResultCallback p_result_callback, void *p_userdata); + void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata); + +protected: + virtual void _shapes_changed() override; + +private: + void update_normals_and_centroids(); + void update_bounds(); + void update_constants(); + void update_area(); + void reset_link_rest_lengths(); + void update_link_constants(); + + void apply_nodes_transform(const Transform3D &p_transform); + + void add_velocity(const Vector3 &p_velocity); + + void apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas); + + bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices); + void generate_bending_constraints(int p_distance); + void reoptimize_link_order(); + void append_link(uint32_t p_node1, uint32_t p_node2); + void append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3); + + void solve_links(real_t kst, real_t ti); + + void initialize_face_tree(); + void update_face_tree(real_t p_delta); + + void initialize_shape(bool p_force_move = true); + void deinitialize_shape(); + + void destroy(); +}; + +class GodotSoftBodyShape3D : public GodotShape3D { + GodotSoftBody3D *soft_body = nullptr; + +public: + GodotSoftBody3D *get_soft_body() const { return soft_body; } + + virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SOFT_BODY; } + virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { r_min = r_max = 0.0; } + virtual Vector3 get_support(const Vector3 &p_normal) const override { return Vector3(); } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } + + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const override; + virtual bool intersect_point(const Vector3 &p_point) const override; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; + virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); } + + virtual void set_data(const Variant &p_data) override {} + virtual Variant get_data() const override { return Variant(); } + + void update_bounds(); + + GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body); + ~GodotSoftBodyShape3D() {} +}; + +#endif // GODOT_SOFT_BODY_3D_H diff --git a/modules/godot_physics_3d/godot_space_3d.cpp b/modules/godot_physics_3d/godot_space_3d.cpp new file mode 100644 index 0000000000..9a6ba776b4 --- /dev/null +++ b/modules/godot_physics_3d/godot_space_3d.cpp @@ -0,0 +1,1277 @@ +/**************************************************************************/ +/* godot_space_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_space_3d.h" + +#include "godot_collision_solver_3d.h" +#include "godot_physics_server_3d.h" + +#include "core/config/project_settings.h" + +#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001 +#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 + +_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject3D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { + if (!(p_object->get_collision_layer() & p_collision_mask)) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject3D::TYPE_AREA && !p_collide_with_areas) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject3D::TYPE_BODY && !p_collide_with_bodies) { + return false; + } + + if (p_object->get_type() == GodotCollisionObject3D::TYPE_SOFT_BODY && !p_collide_with_bodies) { + return false; + } + + return true; +} + +int GodotPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) { + ERR_FAIL_COND_V(space->locked, false); + int amount = space->broadphase->cull_point(p_parameters.position, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + int cc = 0; + + //Transform3D ai = p_xform.affine_inverse(); + + for (int i = 0; i < amount; i++) { + if (cc >= p_result_max) { + break; + } + + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + //area can't be picked by ray (default) + + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + inv_xform.affine_invert(); + + if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_parameters.position))) { + continue; + } + + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } else { + r_results[cc].collider = nullptr; + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + + cc++; + } + + return cc; +} + +bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) { + ERR_FAIL_COND_V(space->locked, false); + + Vector3 begin, end; + Vector3 normal; + begin = p_parameters.from; + end = p_parameters.to; + normal = (end - begin).normalized(); + + int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision + + bool collided = false; + Vector3 res_point, res_normal; + int res_face_index = -1; + int res_shape = -1; + const GodotCollisionObject3D *res_obj = nullptr; + real_t min_d = 1e10; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + if (p_parameters.pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) { + continue; + } + + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + + int shape_idx = space->intersection_query_subindex_results[i]; + Transform3D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); + + Vector3 local_from = inv_xform.xform(begin); + Vector3 local_to = inv_xform.xform(end); + + const GodotShape3D *shape = col_obj->get_shape(shape_idx); + + Vector3 shape_point, shape_normal; + int shape_face_index = -1; + + if (shape->intersect_point(local_from)) { + if (p_parameters.hit_from_inside) { + // Hit shape at starting point. + min_d = 0; + res_point = begin; + res_normal = Vector3(); + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + break; + } else { + // Ignore shape when starting inside. + continue; + } + } + + if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal, shape_face_index, p_parameters.hit_back_faces)) { + Transform3D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + shape_point = xform.xform(shape_point); + + real_t ld = normal.dot(shape_point); + + if (ld < min_d) { + min_d = ld; + res_point = shape_point; + res_normal = inv_xform.basis.xform_inv(shape_normal).normalized(); + res_face_index = shape_face_index; + res_shape = shape_idx; + res_obj = col_obj; + collided = true; + } + } + } + + if (!collided) { + return false; + } + ERR_FAIL_NULL_V(res_obj, false); // Shouldn't happen but silences warning. + + r_result.collider_id = res_obj->get_instance_id(); + if (r_result.collider_id.is_valid()) { + r_result.collider = ObjectDB::get_instance(r_result.collider_id); + } else { + r_result.collider = nullptr; + } + r_result.normal = res_normal; + r_result.face_index = res_face_index; + r_result.position = res_point; + r_result.rid = res_obj->get_self(); + r_result.shape = res_shape; + + return true; +} + +int GodotPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) { + if (p_result_max <= 0) { + return 0; + } + + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, 0); + + AABB aabb = p_parameters.transform.xform(shape->get_aabb()); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + int cc = 0; + + //Transform3D ai = p_xform.affine_inverse(); + + for (int i = 0; i < amount; i++) { + if (cc >= p_result_max) { + break; + } + + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + //area can't be picked by ray (default) + + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + if (!GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_parameters.margin, 0)) { + continue; + } + + if (r_results) { + r_results[cc].collider_id = col_obj->get_instance_id(); + if (r_results[cc].collider_id.is_valid()) { + r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id); + } else { + r_results[cc].collider = nullptr; + } + r_results[cc].rid = col_obj->get_self(); + r_results[cc].shape = shape_idx; + } + + cc++; + } + + return cc; +} + +bool GodotPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe, ShapeRestInfo *r_info) { + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, false); + + AABB aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.merge(AABB(aabb.position + p_parameters.motion, aabb.size)); //motion + aabb = aabb.grow(p_parameters.margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + real_t best_safe = 1; + real_t best_unsafe = 1; + + Transform3D xform_inv = p_parameters.transform.affine_inverse(); + GodotMotionShape3D mshape; + mshape.shape = shape; + mshape.motion = xform_inv.basis.xform(p_parameters.motion); + + bool best_first = true; + + Vector3 motion_normal = p_parameters.motion.normalized(); + + Vector3 closest_A, closest_B; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) { + continue; //ignore excluded + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + int shape_idx = space->intersection_query_subindex_results[i]; + + Vector3 point_A, point_B; + Vector3 sep_axis = motion_normal; + + Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + //test initial overlap, does it collide if going all the way? + if (GodotCollisionSolver3D::solve_distance(&mshape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { + continue; + } + + //test initial overlap, ignore objects it's inside of. + sep_axis = motion_normal; + + if (!GodotCollisionSolver3D::solve_distance(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { + continue; + } + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int j = 0; j < 8; j++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + mshape.motion = xform_inv.basis.xform(p_parameters.motion * fraction); + + Vector3 lA, lB; + Vector3 sep = motion_normal; //important optimization for this to work fast enough + bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); + + if (collided) { + hi = fraction; + if ((j == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + point_A = lA; + point_B = lB; + low = fraction; + if ((j == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (low < best_safe) { + best_first = true; //force reset + best_safe = low; + best_unsafe = hi; + } + + if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low <= best_safe))) { + closest_A = point_A; + closest_B = point_B; + r_info->collider_id = col_obj->get_instance_id(); + r_info->rid = col_obj->get_self(); + r_info->shape = shape_idx; + r_info->point = closest_B; + r_info->normal = (closest_A - closest_B).normalized(); + best_first = false; + if (col_obj->get_type() == GodotCollisionObject3D::TYPE_BODY) { + const GodotBody3D *body = static_cast<const GodotBody3D *>(col_obj); + Vector3 rel_vec = closest_B - (body->get_transform().origin + body->get_center_of_mass()); + r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); + } + } + } + + p_closest_safe = best_safe; + p_closest_unsafe = best_unsafe; + + return true; +} + +bool GodotPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) { + if (p_result_max <= 0) { + return false; + } + + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, 0); + + AABB aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.grow(p_parameters.margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + bool collided = false; + r_result_count = 0; + + GodotPhysicsServer3D::CollCbkData cbk; + cbk.max = p_result_max; + cbk.amount = 0; + cbk.ptr = r_results; + GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk; + + GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk; + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + + if (p_parameters.exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + if (GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) { + collided = true; + } + } + + r_result_count = cbk.amount; + + return collided; +} + +struct _RestResultData { + const GodotCollisionObject3D *object = nullptr; + int local_shape = 0; + int shape = 0; + Vector3 contact; + Vector3 normal; + real_t len = 0.0; +}; + +struct _RestCallbackData { + const GodotCollisionObject3D *object = nullptr; + int local_shape = 0; + int shape = 0; + + real_t min_allowed_depth = 0.0; + + _RestResultData best_result; + + int max_results = 0; + int result_count = 0; + _RestResultData *other_results = nullptr; +}; + +static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) { + _RestCallbackData *rd = static_cast<_RestCallbackData *>(p_userdata); + + Vector3 contact_rel = p_point_B - p_point_A; + real_t len = contact_rel.length(); + if (len < rd->min_allowed_depth) { + return; + } + + bool is_best_result = (len > rd->best_result.len); + + if (rd->other_results && rd->result_count > 0) { + // Consider as new result by default. + int prev_result_count = rd->result_count++; + + int result_index = 0; + real_t tested_len = is_best_result ? rd->best_result.len : len; + for (; result_index < prev_result_count - 1; ++result_index) { + if (tested_len > rd->other_results[result_index].len) { + // Re-using a previous result. + rd->result_count--; + break; + } + } + + if (result_index < rd->max_results - 1) { + _RestResultData &result = rd->other_results[result_index]; + + if (is_best_result) { + // Keep the previous best result as separate result. + result = rd->best_result; + } else { + // Keep this result as separate result. + result.len = len; + result.contact = p_point_B; + result.normal = normal; + result.object = rd->object; + result.shape = rd->shape; + result.local_shape = rd->local_shape; + } + } else { + // Discarding this result. + rd->result_count--; + } + } else if (is_best_result) { + rd->result_count = 1; + } + + if (!is_best_result) { + return; + } + + rd->best_result.len = len; + rd->best_result.contact = p_point_B; + rd->best_result.normal = normal; + rd->best_result.object = rd->object; + rd->best_result.shape = rd->shape; + rd->best_result.local_shape = rd->local_shape; +} + +bool GodotPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) { + GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid); + ERR_FAIL_NULL_V(shape, 0); + + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); + + AABB aabb = p_parameters.transform.xform(shape->get_aabb()); + aabb = aabb.grow(margin); + + int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); + + _RestCallbackData rcd; + + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + real_t motion_length = p_parameters.motion.length(); + real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); + + for (int i = 0; i < amount; i++) { + if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) { + continue; + } + + const GodotCollisionObject3D *col_obj = space->intersection_query_results[i]; + + if (p_parameters.exclude.has(col_obj->get_self())) { + continue; + } + + int shape_idx = space->intersection_query_subindex_results[i]; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, margin); + if (!sc) { + continue; + } + } + + if (rcd.best_result.len == 0 || !rcd.best_result.object) { + return false; + } + + r_info->collider_id = rcd.best_result.object->get_instance_id(); + r_info->shape = rcd.best_result.shape; + r_info->normal = rcd.best_result.normal; + r_info->point = rcd.best_result.contact; + r_info->rid = rcd.best_result.object->get_self(); + if (rcd.best_result.object->get_type() == GodotCollisionObject3D::TYPE_BODY) { + const GodotBody3D *body = static_cast<const GodotBody3D *>(rcd.best_result.object); + Vector3 rel_vec = rcd.best_result.contact - (body->get_transform().origin + body->get_center_of_mass()); + r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); + + } else { + r_info->linear_velocity = Vector3(); + } + + return true; +} + +Vector3 GodotPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { + GodotCollisionObject3D *obj = GodotPhysicsServer3D::godot_singleton->area_owner.get_or_null(p_object); + if (!obj) { + obj = GodotPhysicsServer3D::godot_singleton->body_owner.get_or_null(p_object); + } + ERR_FAIL_NULL_V(obj, Vector3()); + + ERR_FAIL_COND_V(obj->get_space() != space, Vector3()); + + real_t min_distance = 1e20; + Vector3 min_point; + + bool shapes_found = false; + + for (int i = 0; i < obj->get_shape_count(); i++) { + if (obj->is_shape_disabled(i)) { + continue; + } + + Transform3D shape_xform = obj->get_transform() * obj->get_shape_transform(i); + GodotShape3D *shape = obj->get_shape(i); + + Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); + point = shape_xform.xform(point); + + real_t dist = point.distance_to(p_point); + if (dist < min_distance) { + min_distance = dist; + min_point = point; + } + shapes_found = true; + } + + if (!shapes_found) { + return obj->get_transform().origin; //no shapes found, use distance to origin. + } else { + return min_point; + } +} + +GodotPhysicsDirectSpaceState3D::GodotPhysicsDirectSpaceState3D() { + space = nullptr; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////// + +int GodotSpace3D::_cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb) { + int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); + + for (int i = 0; i < amount; i++) { + bool keep = true; + + if (intersection_query_results[i] == p_body) { + keep = false; + } else if (intersection_query_results[i]->get_type() == GodotCollisionObject3D::TYPE_AREA) { + keep = false; + } else if (intersection_query_results[i]->get_type() == GodotCollisionObject3D::TYPE_SOFT_BODY) { + keep = false; + } else if (!p_body->collides_with(static_cast<GodotBody3D *>(intersection_query_results[i]))) { + keep = false; + } else if (static_cast<GodotBody3D *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { + keep = false; + } + + if (!keep) { + if (i < amount - 1) { + SWAP(intersection_query_results[i], intersection_query_results[amount - 1]); + SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]); + } + + amount--; + i--; + } + } + + return amount; +} + +bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) { + //give me back regular physics engine logic + //this is madness + //and most people using this function will think + //what it does is simpler than using physics + //this took about a week to get right.. + //but is it right? who knows at this point.. + + ERR_FAIL_COND_V(p_parameters.max_collisions < 0 || p_parameters.max_collisions > PhysicsServer3D::MotionResult::MAX_COLLISIONS, false); + + if (r_result) { + *r_result = PhysicsServer3D::MotionResult(); + } + + AABB body_aabb; + bool shapes_found = false; + + for (int i = 0; i < p_body->get_shape_count(); i++) { + if (p_body->is_shape_disabled(i)) { + continue; + } + + if (!shapes_found) { + body_aabb = p_body->get_shape_aabb(i); + shapes_found = true; + } else { + body_aabb = body_aabb.merge(p_body->get_shape_aabb(i)); + } + } + + if (!shapes_found) { + if (r_result) { + r_result->travel = p_parameters.motion; + } + + return false; + } + + real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE); + + // Undo the currently transform the physics server is aware of and apply the provided one + body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb)); + body_aabb = body_aabb.grow(margin); + + real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; + + real_t motion_length = p_parameters.motion.length(); + Vector3 motion_normal = p_parameters.motion / motion_length; + + Transform3D body_transform = p_parameters.from; + + bool recovered = false; + + { + //STEP 1, FREE BODY IF STUCK + + const int max_results = 32; + int recover_attempts = 4; + Vector3 sr[max_results * 2]; + real_t priorities[max_results]; + + do { + GodotPhysicsServer3D::CollCbkData cbk; + cbk.max = max_results; + cbk.amount = 0; + cbk.ptr = sr; + + GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk; + GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk; + int priority_amount = 0; + + bool collided = false; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); + GodotShape3D *body_shape = p_body->get_shape(j); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject3D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, margin)) { + collided = cbk.amount > 0; + } + while (cbk.amount > priority_amount) { + priorities[priority_amount] = col_obj->get_collision_priority(); + priority_amount++; + } + } + } + + if (!collided) { + break; + } + + real_t inv_total_weight = 0.0; + for (int i = 0; i < cbk.amount; i++) { + inv_total_weight += priorities[i]; + } + inv_total_weight = Math::is_zero_approx(inv_total_weight) ? 1.0 : (real_t)cbk.amount / inv_total_weight; + + recovered = true; + + Vector3 recover_motion; + for (int i = 0; i < cbk.amount; i++) { + Vector3 a = sr[i * 2 + 0]; + Vector3 b = sr[i * 2 + 1]; + + // Compute plane on b towards a. + Vector3 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > min_contact_depth + CMP_EPSILON) { + // Only recover if there is penetration. + recover_motion -= n * (depth - min_contact_depth) * 0.4 * priorities[i] * inv_total_weight; + } + } + + if (recover_motion == Vector3()) { + collided = false; + break; + } + + body_transform.origin += recover_motion; + body_aabb.position += recover_motion; + + recover_attempts--; + + } while (recover_attempts); + } + + real_t safe = 1.0; + real_t unsafe = 1.0; + int best_shape = -1; + + { + // STEP 2 ATTEMPT MOTION + + AABB motion_aabb = body_aabb; + motion_aabb.position += p_parameters.motion; + motion_aabb = motion_aabb.merge(body_aabb); + + int amount = _cull_aabb_for_body(p_body, motion_aabb); + + for (int j = 0; j < p_body->get_shape_count(); j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + GodotShape3D *body_shape = p_body->get_shape(j); + + // Colliding separation rays allows to properly snap to the ground, + // otherwise it's not needed in regular motion. + if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) { + // When slide on slope is on, separation ray shape acts like a regular shape. + if (!static_cast<GodotSeparationRayShape3D *>(body_shape)->get_slide_on_slope()) { + continue; + } + } + + Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); + + Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); + GodotMotionShape3D mshape; + mshape.shape = body_shape; + mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion); + + bool stuck = false; + + real_t best_safe = 1; + real_t best_unsafe = 1; + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject3D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + //test initial overlap, does it collide if going all the way? + Vector3 point_A, point_B; + Vector3 sep_axis = motion_normal; + + Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); + //test initial overlap, does it collide if going all the way? + if (GodotCollisionSolver3D::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { + continue; + } + sep_axis = motion_normal; + + if (!GodotCollisionSolver3D::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { + stuck = true; + break; + } + + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; + for (int k = 0; k < 8; k++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; + + mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion * fraction); + + Vector3 lA, lB; + Vector3 sep = motion_normal; //important optimization for this to work fast enough + bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); + + if (collided) { + hi = fraction; + if ((k == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } + } else { + point_A = lA; + point_B = lB; + low = fraction; + if ((k == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } + } + } + + if (low < best_safe) { + best_safe = low; + best_unsafe = hi; + } + } + + if (stuck) { + safe = 0; + unsafe = 0; + best_shape = j; //sadly it's the best + break; + } + if (best_safe == 1.0) { + continue; + } + if (best_safe < safe) { + safe = best_safe; + unsafe = best_unsafe; + best_shape = j; + } + } + } + + bool collided = false; + if ((p_parameters.recovery_as_collision && recovered) || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 + } + + //it collided, let's get the rest info in unsafe advance + Transform3D ugt = body_transform; + ugt.origin += p_parameters.motion * unsafe; + + _RestResultData results[PhysicsServer3D::MotionResult::MAX_COLLISIONS]; + + _RestCallbackData rcd; + if (p_parameters.max_collisions > 1) { + rcd.max_results = p_parameters.max_collisions; + rcd.other_results = results; + } + + // Allowed depth can't be lower than motion length, in order to handle contacts at low speed. + rcd.min_allowed_depth = MIN(motion_length, min_contact_depth); + + body_aabb.position += p_parameters.motion * unsafe; + int amount = _cull_aabb_for_body(p_body, body_aabb); + + int from_shape = best_shape != -1 ? best_shape : 0; + int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); + + for (int j = from_shape; j < to_shape; j++) { + if (p_body->is_shape_disabled(j)) { + continue; + } + + Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j); + GodotShape3D *body_shape = p_body->get_shape(j); + + for (int i = 0; i < amount; i++) { + const GodotCollisionObject3D *col_obj = intersection_query_results[i]; + if (p_parameters.exclude_bodies.has(col_obj->get_self())) { + continue; + } + if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) { + continue; + } + + int shape_idx = intersection_query_subindex_results[i]; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, margin); + if (!sc) { + continue; + } + } + } + + if (rcd.result_count > 0) { + if (r_result) { + for (int collision_index = 0; collision_index < rcd.result_count; ++collision_index) { + const _RestResultData &result = (collision_index > 0) ? rcd.other_results[collision_index - 1] : rcd.best_result; + + PhysicsServer3D::MotionCollision &collision = r_result->collisions[collision_index]; + + collision.collider = result.object->get_self(); + collision.collider_id = result.object->get_instance_id(); + collision.collider_shape = result.shape; + collision.local_shape = result.local_shape; + collision.normal = result.normal; + collision.position = result.contact; + collision.depth = result.len; + + const GodotBody3D *body = static_cast<const GodotBody3D *>(result.object); + + Vector3 rel_vec = result.contact - (body->get_transform().origin + body->get_center_of_mass()); + collision.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); + collision.collider_angular_velocity = body->get_angular_velocity(); + } + + r_result->travel = safe * p_parameters.motion; + r_result->remainder = p_parameters.motion - safe * p_parameters.motion; + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; + + r_result->collision_count = rcd.result_count; + r_result->collision_depth = rcd.best_result.len; + } + + collided = true; + } + } + + if (!collided && r_result) { + r_result->travel = p_parameters.motion; + r_result->remainder = Vector3(); + r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin()); + + r_result->collision_safe_fraction = 1.0; + r_result->collision_unsafe_fraction = 1.0; + r_result->collision_depth = 0.0; + } + + return collided; +} + +// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree. +void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self) { + GodotCollisionObject3D::Type type_A = A->get_type(); + GodotCollisionObject3D::Type type_B = B->get_type(); + if (type_A > type_B) { + SWAP(A, B); + SWAP(p_subindex_A, p_subindex_B); + SWAP(type_A, type_B); + } + + GodotSpace3D *self = static_cast<GodotSpace3D *>(p_self); + + self->collision_pairs++; + + if (type_A == GodotCollisionObject3D::TYPE_AREA) { + GodotArea3D *area = static_cast<GodotArea3D *>(A); + if (type_B == GodotCollisionObject3D::TYPE_AREA) { + GodotArea3D *area_b = static_cast<GodotArea3D *>(B); + GodotArea2Pair3D *area2_pair = memnew(GodotArea2Pair3D(area_b, p_subindex_B, area, p_subindex_A)); + return area2_pair; + } else if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) { + GodotSoftBody3D *softbody = static_cast<GodotSoftBody3D *>(B); + GodotAreaSoftBodyPair3D *soft_area_pair = memnew(GodotAreaSoftBodyPair3D(softbody, p_subindex_B, area, p_subindex_A)); + return soft_area_pair; + } else { + GodotBody3D *body = static_cast<GodotBody3D *>(B); + GodotAreaPair3D *area_pair = memnew(GodotAreaPair3D(body, p_subindex_B, area, p_subindex_A)); + return area_pair; + } + } else if (type_A == GodotCollisionObject3D::TYPE_BODY) { + if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) { + GodotBodySoftBodyPair3D *soft_pair = memnew(GodotBodySoftBodyPair3D(static_cast<GodotBody3D *>(A), p_subindex_A, static_cast<GodotSoftBody3D *>(B))); + return soft_pair; + } else { + GodotBodyPair3D *b = memnew(GodotBodyPair3D(static_cast<GodotBody3D *>(A), p_subindex_A, static_cast<GodotBody3D *>(B), p_subindex_B)); + return b; + } + } else { + // Soft Body/Soft Body, not supported. + } + + return nullptr; +} + +void GodotSpace3D::_broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self) { + if (!p_data) { + return; + } + + GodotSpace3D *self = static_cast<GodotSpace3D *>(p_self); + self->collision_pairs--; + GodotConstraint3D *c = static_cast<GodotConstraint3D *>(p_data); + memdelete(c); +} + +const SelfList<GodotBody3D>::List &GodotSpace3D::get_active_body_list() const { + return active_list; +} + +void GodotSpace3D::body_add_to_active_list(SelfList<GodotBody3D> *p_body) { + active_list.add(p_body); +} + +void GodotSpace3D::body_remove_from_active_list(SelfList<GodotBody3D> *p_body) { + active_list.remove(p_body); +} + +void GodotSpace3D::body_add_to_mass_properties_update_list(SelfList<GodotBody3D> *p_body) { + mass_properties_update_list.add(p_body); +} + +void GodotSpace3D::body_remove_from_mass_properties_update_list(SelfList<GodotBody3D> *p_body) { + mass_properties_update_list.remove(p_body); +} + +GodotBroadPhase3D *GodotSpace3D::get_broadphase() { + return broadphase; +} + +void GodotSpace3D::add_object(GodotCollisionObject3D *p_object) { + ERR_FAIL_COND(objects.has(p_object)); + objects.insert(p_object); +} + +void GodotSpace3D::remove_object(GodotCollisionObject3D *p_object) { + ERR_FAIL_COND(!objects.has(p_object)); + objects.erase(p_object); +} + +const HashSet<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const { + return objects; +} + +void GodotSpace3D::body_add_to_state_query_list(SelfList<GodotBody3D> *p_body) { + state_query_list.add(p_body); +} + +void GodotSpace3D::body_remove_from_state_query_list(SelfList<GodotBody3D> *p_body) { + state_query_list.remove(p_body); +} + +void GodotSpace3D::area_add_to_monitor_query_list(SelfList<GodotArea3D> *p_area) { + monitor_query_list.add(p_area); +} + +void GodotSpace3D::area_remove_from_monitor_query_list(SelfList<GodotArea3D> *p_area) { + monitor_query_list.remove(p_area); +} + +void GodotSpace3D::area_add_to_moved_list(SelfList<GodotArea3D> *p_area) { + area_moved_list.add(p_area); +} + +void GodotSpace3D::area_remove_from_moved_list(SelfList<GodotArea3D> *p_area) { + area_moved_list.remove(p_area); +} + +const SelfList<GodotArea3D>::List &GodotSpace3D::get_moved_area_list() const { + return area_moved_list; +} + +const SelfList<GodotSoftBody3D>::List &GodotSpace3D::get_active_soft_body_list() const { + return active_soft_body_list; +} + +void GodotSpace3D::soft_body_add_to_active_list(SelfList<GodotSoftBody3D> *p_soft_body) { + active_soft_body_list.add(p_soft_body); +} + +void GodotSpace3D::soft_body_remove_from_active_list(SelfList<GodotSoftBody3D> *p_soft_body) { + active_soft_body_list.remove(p_soft_body); +} + +void GodotSpace3D::call_queries() { + while (state_query_list.first()) { + GodotBody3D *b = state_query_list.first()->self(); + state_query_list.remove(state_query_list.first()); + b->call_queries(); + } + + while (monitor_query_list.first()) { + GodotArea3D *a = monitor_query_list.first()->self(); + monitor_query_list.remove(monitor_query_list.first()); + a->call_queries(); + } +} + +void GodotSpace3D::setup() { + contact_debug_count = 0; + while (mass_properties_update_list.first()) { + mass_properties_update_list.first()->self()->update_mass_properties(); + mass_properties_update_list.remove(mass_properties_update_list.first()); + } +} + +void GodotSpace3D::update() { + broadphase->update(); +} + +void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + contact_recycle_radius = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + contact_max_separation = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: + contact_max_allowed_penetration = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + contact_bias = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + body_linear_velocity_sleep_threshold = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + body_angular_velocity_sleep_threshold = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + body_time_to_sleep = p_value; + break; + case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: + solver_iterations = p_value; + break; + } +} + +real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const { + switch (p_param) { + case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: + return contact_recycle_radius; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: + return contact_max_separation; + case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: + return contact_max_allowed_penetration; + case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: + return contact_bias; + case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: + return body_linear_velocity_sleep_threshold; + case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: + return body_angular_velocity_sleep_threshold; + case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: + return body_time_to_sleep; + case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: + return solver_iterations; + } + return 0; +} + +void GodotSpace3D::lock() { + locked = true; +} + +void GodotSpace3D::unlock() { + locked = false; +} + +bool GodotSpace3D::is_locked() const { + return locked; +} + +GodotPhysicsDirectSpaceState3D *GodotSpace3D::get_direct_state() { + return direct_access; +} + +GodotSpace3D::GodotSpace3D() { + body_linear_velocity_sleep_threshold = GLOBAL_GET("physics/3d/sleep_threshold_linear"); + body_angular_velocity_sleep_threshold = GLOBAL_GET("physics/3d/sleep_threshold_angular"); + body_time_to_sleep = GLOBAL_GET("physics/3d/time_before_sleep"); + solver_iterations = GLOBAL_GET("physics/3d/solver/solver_iterations"); + contact_recycle_radius = GLOBAL_GET("physics/3d/solver/contact_recycle_radius"); + contact_max_separation = GLOBAL_GET("physics/3d/solver/contact_max_separation"); + contact_max_allowed_penetration = GLOBAL_GET("physics/3d/solver/contact_max_allowed_penetration"); + contact_bias = GLOBAL_GET("physics/3d/solver/default_contact_bias"); + + broadphase = GodotBroadPhase3D::create_func(); + broadphase->set_pair_callback(_broadphase_pair, this); + broadphase->set_unpair_callback(_broadphase_unpair, this); + + direct_access = memnew(GodotPhysicsDirectSpaceState3D); + direct_access->space = this; +} + +GodotSpace3D::~GodotSpace3D() { + memdelete(broadphase); + memdelete(direct_access); +} diff --git a/modules/godot_physics_3d/godot_space_3d.h b/modules/godot_physics_3d/godot_space_3d.h new file mode 100644 index 0000000000..f476be5934 --- /dev/null +++ b/modules/godot_physics_3d/godot_space_3d.h @@ -0,0 +1,218 @@ +/**************************************************************************/ +/* godot_space_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_SPACE_3D_H +#define GODOT_SPACE_3D_H + +#include "godot_area_3d.h" +#include "godot_area_pair_3d.h" +#include "godot_body_3d.h" +#include "godot_body_pair_3d.h" +#include "godot_broad_phase_3d.h" +#include "godot_collision_object_3d.h" +#include "godot_soft_body_3d.h" + +#include "core/config/project_settings.h" +#include "core/templates/hash_map.h" +#include "core/typedefs.h" + +class GodotPhysicsDirectSpaceState3D : public PhysicsDirectSpaceState3D { + GDCLASS(GodotPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D); + +public: + GodotSpace3D *space = nullptr; + + virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override; + virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override; + virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe, ShapeRestInfo *r_info = nullptr) override; + virtual bool collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) override; + virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override; + virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override; + + GodotPhysicsDirectSpaceState3D(); +}; + +class GodotSpace3D { +public: + enum ElapsedTime { + ELAPSED_TIME_INTEGRATE_FORCES, + ELAPSED_TIME_GENERATE_ISLANDS, + ELAPSED_TIME_SETUP_CONSTRAINTS, + ELAPSED_TIME_SOLVE_CONSTRAINTS, + ELAPSED_TIME_INTEGRATE_VELOCITIES, + ELAPSED_TIME_MAX + + }; + +private: + uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; + + GodotPhysicsDirectSpaceState3D *direct_access = nullptr; + RID self; + + GodotBroadPhase3D *broadphase = nullptr; + SelfList<GodotBody3D>::List active_list; + SelfList<GodotBody3D>::List mass_properties_update_list; + SelfList<GodotBody3D>::List state_query_list; + SelfList<GodotArea3D>::List monitor_query_list; + SelfList<GodotArea3D>::List area_moved_list; + SelfList<GodotSoftBody3D>::List active_soft_body_list; + + static void *_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self); + static void _broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self); + + HashSet<GodotCollisionObject3D *> objects; + + GodotArea3D *area = nullptr; + + int solver_iterations = 0; + + real_t contact_recycle_radius = 0.0; + real_t contact_max_separation = 0.0; + real_t contact_max_allowed_penetration = 0.0; + real_t contact_bias = 0.0; + + enum { + INTERSECTION_QUERY_MAX = 2048 + }; + + GodotCollisionObject3D *intersection_query_results[INTERSECTION_QUERY_MAX]; + int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; + + real_t body_linear_velocity_sleep_threshold = 0.0; + real_t body_angular_velocity_sleep_threshold = 0.0; + real_t body_time_to_sleep = 0.0; + + bool locked = false; + + real_t last_step = 0.001; + + int island_count = 0; + int active_objects = 0; + int collision_pairs = 0; + + RID static_global_body; + + Vector<Vector3> contact_debug; + int contact_debug_count = 0; + + friend class GodotPhysicsDirectSpaceState3D; + + int _cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb); + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + void set_default_area(GodotArea3D *p_area) { area = p_area; } + GodotArea3D *get_default_area() const { return area; } + + const SelfList<GodotBody3D>::List &get_active_body_list() const; + void body_add_to_active_list(SelfList<GodotBody3D> *p_body); + void body_remove_from_active_list(SelfList<GodotBody3D> *p_body); + void body_add_to_mass_properties_update_list(SelfList<GodotBody3D> *p_body); + void body_remove_from_mass_properties_update_list(SelfList<GodotBody3D> *p_body); + + void body_add_to_state_query_list(SelfList<GodotBody3D> *p_body); + void body_remove_from_state_query_list(SelfList<GodotBody3D> *p_body); + + void area_add_to_monitor_query_list(SelfList<GodotArea3D> *p_area); + void area_remove_from_monitor_query_list(SelfList<GodotArea3D> *p_area); + void area_add_to_moved_list(SelfList<GodotArea3D> *p_area); + void area_remove_from_moved_list(SelfList<GodotArea3D> *p_area); + const SelfList<GodotArea3D>::List &get_moved_area_list() const; + + const SelfList<GodotSoftBody3D>::List &get_active_soft_body_list() const; + void soft_body_add_to_active_list(SelfList<GodotSoftBody3D> *p_soft_body); + void soft_body_remove_from_active_list(SelfList<GodotSoftBody3D> *p_soft_body); + + GodotBroadPhase3D *get_broadphase(); + + void add_object(GodotCollisionObject3D *p_object); + void remove_object(GodotCollisionObject3D *p_object); + const HashSet<GodotCollisionObject3D *> &get_objects() const; + + _FORCE_INLINE_ int get_solver_iterations() const { return solver_iterations; } + _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } + _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; } + _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; } + _FORCE_INLINE_ real_t get_contact_bias() const { return contact_bias; } + _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_threshold() const { return body_linear_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_threshold() const { return body_angular_velocity_sleep_threshold; } + _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; } + + void update(); + void setup(); + void call_queries(); + + bool is_locked() const; + void lock(); + void unlock(); + + real_t get_last_step() const { return last_step; } + void set_last_step(real_t p_step) { last_step = p_step; } + + void set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value); + real_t get_param(PhysicsServer3D::SpaceParameter p_param) const; + + void set_island_count(int p_island_count) { island_count = p_island_count; } + int get_island_count() const { return island_count; } + + void set_active_objects(int p_active_objects) { active_objects = p_active_objects; } + int get_active_objects() const { return active_objects; } + + int get_collision_pairs() const { return collision_pairs; } + + GodotPhysicsDirectSpaceState3D *get_direct_state(); + + void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } + _FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); } + _FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) { + if (contact_debug_count < contact_debug.size()) { + contact_debug.write[contact_debug_count++] = p_contact; + } + } + _FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; } + _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } + + void set_static_global_body(RID p_body) { static_global_body = p_body; } + RID get_static_global_body() { return static_global_body; } + + void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } + uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; } + + bool test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result); + + GodotSpace3D(); + ~GodotSpace3D(); +}; + +#endif // GODOT_SPACE_3D_H diff --git a/modules/godot_physics_3d/godot_step_3d.cpp b/modules/godot_physics_3d/godot_step_3d.cpp new file mode 100644 index 0000000000..b6cec4ba59 --- /dev/null +++ b/modules/godot_physics_3d/godot_step_3d.cpp @@ -0,0 +1,418 @@ +/**************************************************************************/ +/* godot_step_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "godot_step_3d.h" + +#include "godot_joint_3d.h" + +#include "core/object/worker_thread_pool.h" +#include "core/os/os.h" + +#define BODY_ISLAND_COUNT_RESERVE 128 +#define BODY_ISLAND_SIZE_RESERVE 512 +#define ISLAND_COUNT_RESERVE 128 +#define ISLAND_SIZE_RESERVE 512 +#define CONSTRAINT_COUNT_RESERVE 1024 + +void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) { + p_body->set_island_step(_step); + + if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { + // Only rigid bodies are tested for activation. + p_body_island.push_back(p_body); + } + + for (const KeyValue<GodotConstraint3D *, int> &E : p_body->get_constraint_map()) { + GodotConstraint3D *constraint = const_cast<GodotConstraint3D *>(E.key); + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { + if (i == E.value) { + continue; + } + GodotBody3D *other_body = constraint->get_body_ptr()[i]; + if (other_body->get_island_step() == _step) { + continue; // Already processed. + } + if (other_body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(other_body, p_body_island, p_constraint_island); + } + + // Find connected soft bodies. + for (int i = 0; i < constraint->get_soft_body_count(); i++) { + GodotSoftBody3D *soft_body = constraint->get_soft_body_ptr(i); + if (soft_body->get_island_step() == _step) { + continue; // Already processed. + } + _populate_island_soft_body(soft_body, p_body_island, p_constraint_island); + } + } +} + +void GodotStep3D::_populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) { + p_soft_body->set_island_step(_step); + + for (const GodotConstraint3D *E : p_soft_body->get_constraints()) { + GodotConstraint3D *constraint = const_cast<GodotConstraint3D *>(E); + if (constraint->get_island_step() == _step) { + continue; // Already processed. + } + constraint->set_island_step(_step); + p_constraint_island.push_back(constraint); + + all_constraints.push_back(constraint); + + // Find connected rigid bodies. + for (int i = 0; i < constraint->get_body_count(); i++) { + GodotBody3D *body = constraint->get_body_ptr()[i]; + if (body->get_island_step() == _step) { + continue; // Already processed. + } + if (body->get_mode() == PhysicsServer3D::BODY_MODE_STATIC) { + continue; // Static bodies don't connect islands. + } + _populate_island(body, p_body_island, p_constraint_island); + } + } +} + +void GodotStep3D::_setup_constraint(uint32_t p_constraint_index, void *p_userdata) { + GodotConstraint3D *constraint = all_constraints[p_constraint_index]; + constraint->setup(delta); +} + +void GodotStep3D::_pre_solve_island(LocalVector<GodotConstraint3D *> &p_constraint_island) const { + uint32_t constraint_count = p_constraint_island.size(); + uint32_t valid_constraint_count = 0; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + GodotConstraint3D *constraint = p_constraint_island[constraint_index]; + if (p_constraint_island[constraint_index]->pre_solve(delta)) { + // Keep this constraint for solving. + p_constraint_island[valid_constraint_count++] = constraint; + } + } + p_constraint_island.resize(valid_constraint_count); +} + +void GodotStep3D::_solve_island(uint32_t p_island_index, void *p_userdata) { + LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[p_island_index]; + + int current_priority = 1; + + uint32_t constraint_count = constraint_island.size(); + while (constraint_count > 0) { + for (int i = 0; i < iterations; i++) { + // Go through all iterations. + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + constraint_island[constraint_index]->solve(delta); + } + } + + // Check priority to keep only higher priority constraints. + uint32_t priority_constraint_count = 0; + ++current_priority; + for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { + GodotConstraint3D *constraint = constraint_island[constraint_index]; + if (constraint->get_priority() >= current_priority) { + // Keep this constraint for the next iteration. + constraint_island[priority_constraint_count++] = constraint; + } + } + constraint_count = priority_constraint_count; + } +} + +void GodotStep3D::_check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const { + bool can_sleep = true; + + uint32_t body_count = p_body_island.size(); + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody3D *body = p_body_island[body_index]; + + if (!body->sleep_test(delta)) { + can_sleep = false; + } + } + + // Put all to sleep or wake up everyone. + for (uint32_t body_index = 0; body_index < body_count; ++body_index) { + GodotBody3D *body = p_body_island[body_index]; + + bool active = body->is_active(); + + if (active == can_sleep) { + body->set_active(!can_sleep); + } + } +} + +void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta) { + p_space->lock(); // can't access space during this + + p_space->setup(); //update inertias, etc + + p_space->set_last_step(p_delta); + + iterations = p_space->get_solver_iterations(); + delta = p_delta; + + const SelfList<GodotBody3D>::List *body_list = &p_space->get_active_body_list(); + + const SelfList<GodotSoftBody3D>::List *soft_body_list = &p_space->get_active_soft_body_list(); + + /* INTEGRATE FORCES */ + + uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec(); + uint64_t profile_endtime = 0; + + int active_count = 0; + + const SelfList<GodotBody3D> *b = body_list->first(); + while (b) { + b->self()->integrate_forces(p_delta); + b = b->next(); + active_count++; + } + + /* UPDATE SOFT BODY MOTION */ + + const SelfList<GodotSoftBody3D> *sb = soft_body_list->first(); + while (sb) { + sb->self()->predict_motion(p_delta); + sb = sb->next(); + active_count++; + } + + p_space->set_active_objects(active_count); + + // Update the broadphase to register collision pairs. + p_space->update(); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* GENERATE CONSTRAINT ISLANDS FOR MOVING AREAS */ + + uint32_t island_count = 0; + + const SelfList<GodotArea3D>::List &aml = p_space->get_moved_area_list(); + + while (aml.first()) { + for (GodotConstraint3D *E : aml.first()->self()->get_constraints()) { + GodotConstraint3D *constraint = E; + if (constraint->get_island_step() == _step) { + continue; + } + constraint->set_island_step(_step); + + // Each constraint can be on a separate island for areas as there's no solving phase. + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + + all_constraints.push_back(constraint); + constraint_island.push_back(constraint); + } + p_space->area_remove_from_moved_list((SelfList<GodotArea3D> *)aml.first()); //faster to remove here + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ + + b = body_list->first(); + + uint32_t body_island_count = 0; + + while (b) { + GodotBody3D *body = b->self(); + + if (body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector<GodotBody3D *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island(body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } + } + b = b->next(); + } + + /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE SOFT BODIES */ + + sb = soft_body_list->first(); + while (sb) { + GodotSoftBody3D *soft_body = sb->self(); + + if (soft_body->get_island_step() != _step) { + ++body_island_count; + if (body_islands.size() < body_island_count) { + body_islands.resize(body_island_count); + } + LocalVector<GodotBody3D *> &body_island = body_islands[body_island_count - 1]; + body_island.clear(); + body_island.reserve(BODY_ISLAND_SIZE_RESERVE); + + ++island_count; + if (constraint_islands.size() < island_count) { + constraint_islands.resize(island_count); + } + LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1]; + constraint_island.clear(); + constraint_island.reserve(ISLAND_SIZE_RESERVE); + + _populate_island_soft_body(soft_body, body_island, constraint_island); + + if (body_island.is_empty()) { + --body_island_count; + } + + if (constraint_island.is_empty()) { + --island_count; + } + } + sb = sb->next(); + } + + p_space->set_island_count((int)island_count); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* SETUP CONSTRAINTS / PROCESS COLLISIONS */ + + uint32_t total_constraint_count = all_constraints.size(); + WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep3D::_setup_constraint, nullptr, total_constraint_count, -1, true, SNAME("Physics3DConstraintSetup")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* PRE-SOLVE CONSTRAINT ISLANDS */ + + // WARNING: This doesn't run on threads, because it involves thread-unsafe processing. + for (uint32_t island_index = 0; island_index < island_count; ++island_index) { + _pre_solve_island(constraint_islands[island_index]); + } + + /* SOLVE CONSTRAINT ISLANDS */ + + // WARNING: `_solve_island` modifies the constraint islands for optimization purpose, + // their content is not reliable after these calls and shouldn't be used anymore. + group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep3D::_solve_island, nullptr, island_count, -1, true, SNAME("Physics3DConstraintSolveIslands")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + /* INTEGRATE VELOCITIES */ + + b = body_list->first(); + while (b) { + const SelfList<GodotBody3D> *n = b->next(); + b->self()->integrate_velocities(p_delta); + b = n; + } + + /* SLEEP / WAKE UP ISLANDS */ + + for (uint32_t island_index = 0; island_index < body_island_count; ++island_index) { + _check_suspend(body_islands[island_index]); + } + + /* UPDATE SOFT BODY CONSTRAINTS */ + + sb = soft_body_list->first(); + while (sb) { + sb->self()->solve_constraints(p_delta); + sb = sb->next(); + } + + { //profile + profile_endtime = OS::get_singleton()->get_ticks_usec(); + p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); + profile_begtime = profile_endtime; + } + + all_constraints.clear(); + + p_space->unlock(); + _step++; +} + +GodotStep3D::GodotStep3D() { + body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); + constraint_islands.reserve(ISLAND_COUNT_RESERVE); + all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); +} + +GodotStep3D::~GodotStep3D() { +} diff --git a/modules/godot_physics_3d/godot_step_3d.h b/modules/godot_physics_3d/godot_step_3d.h new file mode 100644 index 0000000000..1c9b0af422 --- /dev/null +++ b/modules/godot_physics_3d/godot_step_3d.h @@ -0,0 +1,61 @@ +/**************************************************************************/ +/* godot_step_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_STEP_3D_H +#define GODOT_STEP_3D_H + +#include "godot_space_3d.h" + +#include "core/templates/local_vector.h" + +class GodotStep3D { + uint64_t _step = 1; + + int iterations = 0; + real_t delta = 0.0; + + LocalVector<LocalVector<GodotBody3D *>> body_islands; + LocalVector<LocalVector<GodotConstraint3D *>> constraint_islands; + LocalVector<GodotConstraint3D *> all_constraints; + + void _populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island); + void _populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island); + void _setup_constraint(uint32_t p_constraint_index, void *p_userdata = nullptr); + void _pre_solve_island(LocalVector<GodotConstraint3D *> &p_constraint_island) const; + void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr); + void _check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const; + +public: + void step(GodotSpace3D *p_space, real_t p_delta); + GodotStep3D(); + ~GodotStep3D(); +}; + +#endif // GODOT_STEP_3D_H diff --git a/modules/godot_physics_3d/joints/SCsub b/modules/godot_physics_3d/joints/SCsub new file mode 100644 index 0000000000..39eb469978 --- /dev/null +++ b/modules/godot_physics_3d/joints/SCsub @@ -0,0 +1,6 @@ +#!/usr/bin/env python +from misc.utility.scons_hints import * + +Import("env") + +env.add_source_files(env.modules_sources, "*.cpp") diff --git a/modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.cpp b/modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.cpp new file mode 100644 index 0000000000..4091422789 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.cpp @@ -0,0 +1,326 @@ +/**************************************************************************/ +/* godot_cone_twist_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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. */ +/**************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios + +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. + +Written by: Marcus Hennix +*/ + +#include "godot_cone_twist_joint_3d.h" + +GodotConeTwistJoint3D::GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) : + GodotJoint3D(_arr, 2) { + A = rbA; + B = rbB; + + m_rbAFrame = rbAFrame; + m_rbBFrame = rbBFrame; + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +bool GodotConeTwistJoint3D::setup(real_t p_timestep) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + m_appliedImpulse = real_t(0.); + + //set bias, sign, clear accumulator + m_swingCorrection = real_t(0.); + m_twistLimitSign = real_t(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + m_accTwistLimitImpulse = real_t(0.); + m_accSwingLimitImpulse = real_t(0.); + + if (!m_angularOnly) { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + Vector3 relPos = pivotBInW - pivotAInW; + + Vector3 normal[3]; + if (Math::is_zero_approx(relPos.length_squared())) { + normal[0] = Vector3(real_t(1.0), 0, 0); + } else { + normal[0] = relPos.normalized(); + } + + plane_space(normal[0], normal[1], normal[2]); + + for (int i = 0; i < 3; i++) { + memnew_placement( + &m_jac[i], + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + } + } + + Vector3 b1Axis1, b1Axis2, b1Axis3; + Vector3 b2Axis1, b2Axis2; + + b1Axis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0)); + b2Axis1 = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(0)); + + real_t swing1 = real_t(0.), swing2 = real_t(0.); + + real_t swx = real_t(0.), swy = real_t(0.); + real_t thresh = real_t(10.); + real_t fact; + + // Get Frame into world space + if (m_swingSpan1 >= real_t(0.05f)) { + b1Axis2 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1)); + //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis2); + swing1 = atan2fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; + fact = fact / (fact + real_t(1.0)); + swing1 *= fact; + } + + if (m_swingSpan2 >= real_t(0.05f)) { + b1Axis3 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); + //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis3); + swing2 = atan2fast(swy, swx); + fact = (swy * swy + swx * swx) * thresh * thresh; + fact = fact / (fact + real_t(1.0)); + swing2 *= fact; + } + + real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); + real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); + real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq; + + if (EllipseAngle > 1.0f) { + m_swingCorrection = EllipseAngle - 1.0f; + m_solveSwingLimit = true; + + // Calculate necessary axis & factors + m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3)); + m_swingAxis.normalize(); + + real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; + m_swingAxis *= swingAxisSign; + + m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + B->compute_angular_impulse_denominator(m_swingAxis)); + } + + // Twist limits + if (m_twistSpan >= real_t(0.)) { + Vector3 b2Axis22 = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1)); + Quaternion rotationArc = Quaternion(b2Axis1, b1Axis1); + Vector3 TwistRef = rotationArc.xform(b2Axis22); + real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); + + real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.); + if (twist <= -m_twistSpan * lockedFreeFactor) { + m_twistCorrection = -(twist + m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + m_twistAxis *= -1.0f; + + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis)); + + } else if (twist > m_twistSpan * lockedFreeFactor) { + m_twistCorrection = (twist - m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + + m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis)); + } + } + + return true; +} + +void GodotConeTwistJoint3D::solve(real_t p_timestep) { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + + real_t tau = real_t(0.3); + + //linear part + if (!m_angularOnly) { + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + for (int i = 0; i < 3; i++) { + const Vector3 &normal = m_jac[i].m_linearJointAxis; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + real_t rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + real_t impulse = depth * tau / p_timestep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } + } + } + + { + ///solve angular part + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); + + // solve swing limit + if (m_solveSwingLimit) { + real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / p_timestep) * m_biasFactor); + real_t impulseMag = amplitude * m_kSwing; + + // Clamp the accumulated impulse + real_t temp = m_accSwingLimitImpulse; + m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0)); + impulseMag = m_accSwingLimitImpulse - temp; + + Vector3 impulse = m_swingAxis * impulseMag; + + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } + } + + // solve twist limit + if (m_solveTwistLimit) { + real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / p_timestep) * m_biasFactor); + real_t impulseMag = amplitude * m_kTwist; + + // Clamp the accumulated impulse + real_t temp = m_accTwistLimitImpulse; + m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0)); + impulseMag = m_accTwistLimitImpulse - temp; + + Vector3 impulse = m_twistAxis * impulseMag; + + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } + } + } +} + +void GodotConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { + m_swingSpan1 = p_value; + m_swingSpan2 = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { + m_twistSpan = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { + m_biasFactor = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { + m_limitSoftness = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { + m_relaxationFactor = p_value; + } break; + case PhysicsServer3D::CONE_TWIST_MAX: + break; // Can't happen, but silences warning + } +} + +real_t GodotConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { + return m_swingSpan1; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: { + return m_twistSpan; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: { + return m_biasFactor; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: { + return m_limitSoftness; + } break; + case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: { + return m_relaxationFactor; + } break; + case PhysicsServer3D::CONE_TWIST_MAX: + break; // Can't happen, but silences warning + } + + return 0; +} diff --git a/modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.h b/modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.h new file mode 100644 index 0000000000..f3b683a8f3 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_cone_twist_joint_3d.h @@ -0,0 +1,142 @@ +/**************************************************************************/ +/* godot_cone_twist_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_CONE_TWIST_JOINT_3D_H +#define GODOT_CONE_TWIST_JOINT_3D_H + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +GodotConeTwistJoint3D is Copyright (c) 2007 Starbreeze Studios + +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. + +Written by: Marcus Hennix +*/ + +#include "../godot_joint_3d.h" +#include "godot_jacobian_entry_3d.h" + +// GodotConeTwistJoint3D can be used to simulate ragdoll joints (upper arm, leg etc). +class GodotConeTwistJoint3D : public GodotJoint3D { +#ifdef IN_PARALLELL_SOLVER +public: +#endif + + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = { nullptr, nullptr }; + }; + + GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints + + real_t m_appliedImpulse = 0.0; + Transform3D m_rbAFrame; + Transform3D m_rbBFrame; + + real_t m_limitSoftness = 0.0; + real_t m_biasFactor = 0.3; + real_t m_relaxationFactor = 1.0; + + real_t m_swingSpan1 = Math_TAU / 8.0; + real_t m_swingSpan2 = 0.0; + real_t m_twistSpan = 0.0; + + Vector3 m_swingAxis; + Vector3 m_twistAxis; + + real_t m_kSwing = 0.0; + real_t m_kTwist = 0.0; + + real_t m_twistLimitSign = 0.0; + real_t m_swingCorrection = 0.0; + real_t m_twistCorrection = 0.0; + + real_t m_accSwingLimitImpulse = 0.0; + real_t m_accTwistLimitImpulse = 0.0; + + bool m_angularOnly = false; + bool m_solveTwistLimit = false; + bool m_solveSwingLimit = false; + +public: + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame); + + void setAngularOnly(bool angularOnly) { + m_angularOnly = angularOnly; + } + + void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) { + m_swingSpan1 = _swingSpan1; + m_swingSpan2 = _swingSpan2; + m_twistSpan = _twistSpan; + + m_limitSoftness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; + } + + inline int getSolveTwistLimit() { + return m_solveTwistLimit; + } + + inline int getSolveSwingLimit() { + return m_solveTwistLimit; + } + + inline real_t getTwistLimitSign() { + return m_twistLimitSign; + } + + void set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const; +}; + +#endif // GODOT_CONE_TWIST_JOINT_3D_H diff --git a/modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.cpp b/modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.cpp new file mode 100644 index 0000000000..226f8a0f7f --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.cpp @@ -0,0 +1,675 @@ +/**************************************************************************/ +/* godot_generic_6dof_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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. */ +/**************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +/* +2007-09-09 +GodotGeneric6DOFJoint3D Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + +#include "godot_generic_6dof_joint_3d.h" + +#define GENERIC_D6_DISABLE_WARMSTARTING 1 + +//////////////////////////// GodotG6DOFRotationalLimitMotor3D //////////////////////////////////// + +int GodotG6DOFRotationalLimitMotor3D::testLimitValue(real_t test_value) { + if (m_loLimit > m_hiLimit) { + m_currentLimit = 0; //Free from violation + return 0; + } + + if (test_value < m_loLimit) { + m_currentLimit = 1; //low limit violation + m_currentLimitError = test_value - m_loLimit; + return 1; + } else if (test_value > m_hiLimit) { + m_currentLimit = 2; //High limit violation + m_currentLimitError = test_value - m_hiLimit; + return 2; + }; + + m_currentLimit = 0; //Free from violation + return 0; +} + +real_t GodotG6DOFRotationalLimitMotor3D::solveAngularLimits( + real_t timeStep, Vector3 &axis, real_t jacDiagABInv, + GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic) { + if (!needApplyTorques()) { + return 0.0f; + } + + real_t target_velocity = m_targetVelocity; + real_t maxMotorForce = m_maxMotorForce; + + //current error correction + if (m_currentLimit != 0) { + target_velocity = -m_ERP * m_currentLimitError / (timeStep); + maxMotorForce = m_maxLimitForce; + } + + maxMotorForce *= timeStep; + + // current velocity difference + Vector3 vel_diff = body0->get_angular_velocity(); + if (body1) { + vel_diff -= body1->get_angular_velocity(); + } + + real_t rel_vel = axis.dot(vel_diff); + + // correction velocity + real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel); + + if (Math::is_zero_approx(motor_relvel)) { + return 0.0f; //no need for applying force + } + + // correction impulse + real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv; + + // clip correction impulse + real_t clippedMotorImpulse; + + ///@todo: should clip against accumulated impulse + if (unclippedMotorImpulse > 0.0f) { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; + } else { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; + } + + // sort with accumulated impulses + real_t lo = real_t(-1e30); + real_t hi = real_t(1e30); + + real_t oldaccumImpulse = m_accumulatedImpulse; + real_t sum = oldaccumImpulse + clippedMotorImpulse; + m_accumulatedImpulse = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); + + clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + + Vector3 motorImp = clippedMotorImpulse * axis; + + if (p_body0_dynamic) { + body0->apply_torque_impulse(motorImp); + } + if (body1 && p_body1_dynamic) { + body1->apply_torque_impulse(-motorImp); + } + + return clippedMotorImpulse; +} + +//////////////////////////// GodotG6DOFTranslationalLimitMotor3D //////////////////////////////////// + +real_t GodotG6DOFTranslationalLimitMotor3D::solveLinearAxis( + real_t timeStep, + real_t jacDiagABInv, + GodotBody3D *body1, const Vector3 &pointInA, + GodotBody3D *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, + int limit_index, + const Vector3 &axis_normal_on_a, + const Vector3 &anchorPos) { + ///find relative velocity + // Vector3 rel_pos1 = pointInA - body1->get_transform().origin; + // Vector3 rel_pos2 = pointInB - body2->get_transform().origin; + Vector3 rel_pos1 = anchorPos - body1->get_transform().origin; + Vector3 rel_pos2 = anchorPos - body2->get_transform().origin; + + Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + real_t rel_vel = axis_normal_on_a.dot(vel); + + /// apply displacement correction + + //positional error (zeroth order error) + real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a); + real_t lo = real_t(-1e30); + real_t hi = real_t(1e30); + + real_t minLimit = m_lowerLimit[limit_index]; + real_t maxLimit = m_upperLimit[limit_index]; + + //handle the limits + if (minLimit < maxLimit) { + { + if (depth > maxLimit) { + depth -= maxLimit; + lo = real_t(0.); + + } else { + if (depth < minLimit) { + depth -= minLimit; + hi = real_t(0.); + } else { + return 0.0f; + } + } + } + } + + real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv; + + real_t oldNormalImpulse = m_accumulatedImpulse[limit_index]; + real_t sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : (sum < lo ? real_t(0.) : sum); + normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; + + Vector3 impulse_vector = axis_normal_on_a * normalImpulse; + if (p_body1_dynamic) { + body1->apply_impulse(impulse_vector, rel_pos1); + } + if (p_body2_dynamic) { + body2->apply_impulse(-impulse_vector, rel_pos2); + } + return normalImpulse; +} + +//////////////////////////// GodotGeneric6DOFJoint3D //////////////////////////////////// + +GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) : + GodotJoint3D(_arr, 2), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) { + A = rbA; + B = rbB; + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +void GodotGeneric6DOFJoint3D::calculateAngleInfo() { + Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis; + + m_calculatedAxisAngleDiff = relative_frame.get_euler(EulerOrder::XYZ); + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + Vector3 axis0 = m_calculatedTransformB.basis.get_column(0); + Vector3 axis2 = m_calculatedTransformA.basis.get_column(2); + + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + + /* + if(m_debugDrawer) + { + char buff[300]; + sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", + m_calculatedAxisAngleDiff[0], + m_calculatedAxisAngleDiff[1], + m_calculatedAxisAngleDiff[2]); + m_debugDrawer->reportErrorWarning(buff); + } + */ +} + +void GodotGeneric6DOFJoint3D::calculateTransforms() { + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + + calculateAngleInfo(); +} + +void GodotGeneric6DOFJoint3D::buildLinearJacobian( + GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld, + const Vector3 &pivotAInW, const Vector3 &pivotBInW) { + memnew_placement( + &jacLinear, + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); +} + +void GodotGeneric6DOFJoint3D::buildAngularJacobian( + GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) { + memnew_placement( + &jacAngular, + GodotJacobianEntry3D( + jointAxisW, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); +} + +bool GodotGeneric6DOFJoint3D::testAngularLimitMotor(int axis_index) { + real_t angle = m_calculatedAxisAngleDiff[axis_index]; + + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); +} + +bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + // Clear accumulated impulses for the next simulation step + m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.)); + int i; + for (i = 0; i < 3; i++) { + m_angularLimits[i].m_accumulatedImpulse = real_t(0.); + } + //calculates transform + calculateTransforms(); + + // const Vector3& pivotAInW = m_calculatedTransformA.origin; + // const Vector3& pivotBInW = m_calculatedTransformB.origin; + calcAnchorPos(); + Vector3 pivotAInW = m_AnchorPos; + Vector3 pivotBInW = m_AnchorPos; + + // not used here + // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 normalWorld; + //linear part + for (i = 0; i < 3; i++) { + if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { + if (m_useLinearReferenceFrameA) { + normalWorld = m_calculatedTransformA.basis.get_column(i); + } else { + normalWorld = m_calculatedTransformB.basis.get_column(i); + } + + buildLinearJacobian( + m_jacLinear[i], normalWorld, + pivotAInW, pivotBInW); + } + } + + // angular part + for (i = 0; i < 3; i++) { + //calculates error angle + if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) { + normalWorld = getAxis(i); + // Create angular atom + buildAngularJacobian(m_jacAng[i], normalWorld); + } + } + + return true; +} + +void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) { + m_timeStep = p_timestep; + + //calculateTransforms(); + + int i; + + // linear + + Vector3 pointInA = m_calculatedTransformA.origin; + Vector3 pointInB = m_calculatedTransformB.origin; + + real_t jacDiagABInv; + Vector3 linear_axis; + for (i = 0; i < 3; i++) { + if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { + jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); + + if (m_useLinearReferenceFrameA) { + linear_axis = m_calculatedTransformA.basis.get_column(i); + } else { + linear_axis = m_calculatedTransformB.basis.get_column(i); + } + + m_linearLimits.solveLinearAxis( + m_timeStep, + jacDiagABInv, + A, pointInA, + B, pointInB, + dynamic_A, dynamic_B, + i, linear_axis, m_AnchorPos); + } + } + + // angular + Vector3 angular_axis; + real_t angularJacDiagABInv; + for (i = 0; i < 3; i++) { + if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) { + // get axis + angular_axis = getAxis(i); + + angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); + + m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B, dynamic_A, dynamic_B); + } + } +} + +void GodotGeneric6DOFJoint3D::updateRHS(real_t timeStep) { + (void)timeStep; +} + +Vector3 GodotGeneric6DOFJoint3D::getAxis(int axis_index) const { + return m_calculatedAxis[axis_index]; +} + +real_t GodotGeneric6DOFJoint3D::getAngle(int axis_index) const { + return m_calculatedAxisAngleDiff[axis_index]; +} + +void GodotGeneric6DOFJoint3D::calcAnchorPos() { + real_t imA = A->get_inv_mass(); + real_t imB = B->get_inv_mass(); + real_t weight; + if (imB == real_t(0.0)) { + weight = real_t(1.0); + } else { + weight = imA / (imA + imB); + } + const Vector3 &pA = m_calculatedTransformA.origin; + const Vector3 &pB = m_calculatedTransformB.origin; + m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight); +} + +void GodotGeneric6DOFJoint3D::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) { + ERR_FAIL_INDEX(p_axis, 3); + switch (p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + m_linearLimits.m_lowerLimit[p_axis] = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + m_linearLimits.m_upperLimit[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + m_linearLimits.m_limitSoftness[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { + m_linearLimits.m_restitution[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { + m_linearLimits.m_damping[p_axis] = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + m_angularLimits[p_axis].m_loLimit = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + m_angularLimits[p_axis].m_hiLimit = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + m_angularLimits[p_axis].m_limitSoftness = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { + m_angularLimits[p_axis].m_damping = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { + m_angularLimits[p_axis].m_bounce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + m_angularLimits[p_axis].m_maxLimitForce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { + m_angularLimits[p_axis].m_ERP = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + m_angularLimits[p_axis].m_targetVelocity = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + m_angularLimits[p_axis].m_maxLimitForce = p_value; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_MAX: + break; // Can't happen, but silences warning + } +} + +real_t GodotGeneric6DOFJoint3D::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const { + ERR_FAIL_INDEX_V(p_axis, 3, 0); + switch (p_param) { + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { + return m_linearLimits.m_lowerLimit[p_axis]; + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: { + return m_linearLimits.m_upperLimit[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: { + return m_linearLimits.m_limitSoftness[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: { + return m_linearLimits.m_restitution[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: { + return m_linearLimits.m_damping[p_axis]; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: { + return m_angularLimits[p_axis].m_loLimit; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: { + return m_angularLimits[p_axis].m_hiLimit; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: { + return m_angularLimits[p_axis].m_limitSoftness; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: { + return m_angularLimits[p_axis].m_damping; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: { + return m_angularLimits[p_axis].m_bounce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: { + return m_angularLimits[p_axis].m_maxLimitForce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: { + return m_angularLimits[p_axis].m_ERP; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: { + return m_angularLimits[p_axis].m_targetVelocity; + + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: { + return m_angularLimits[p_axis].m_maxMotorForce; + + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_MAX: + break; // Can't happen, but silences warning + } + return 0; +} + +void GodotGeneric6DOFJoint3D::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) { + ERR_FAIL_INDEX(p_axis, 3); + + switch (p_flag) { + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + m_linearLimits.enable_limit[p_axis] = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + m_angularLimits[p_axis].m_enableLimit = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + m_angularLimits[p_axis].m_enableMotor = p_value; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX: + break; // Can't happen, but silences warning + } +} + +bool GodotGeneric6DOFJoint3D::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { + ERR_FAIL_INDEX_V(p_axis, 3, 0); + switch (p_flag) { + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { + return m_linearLimits.enable_limit[p_axis]; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: { + return m_angularLimits[p_axis].m_enableLimit; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: { + return m_angularLimits[p_axis].m_enableMotor; + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: { + // Not implemented in GodotPhysics3D backend + } break; + case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX: + break; // Can't happen, but silences warning + } + + return false; +} diff --git a/modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.h b/modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.h new file mode 100644 index 0000000000..9ee6dd2791 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_generic_6dof_joint_3d.h @@ -0,0 +1,322 @@ +/**************************************************************************/ +/* godot_generic_6dof_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_GENERIC_6DOF_JOINT_3D_H +#define GODOT_GENERIC_6DOF_JOINT_3D_H + +/* +Adapted to Godot from the Bullet library. +*/ + +#include "../godot_joint_3d.h" +#include "godot_jacobian_entry_3d.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +/* +2007-09-09 +GodotGeneric6DOFJoint3D Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + +//! Rotation Limit structure for generic joints +class GodotG6DOFRotationalLimitMotor3D { +public: + //! limit_parameters + //!@{ + real_t m_loLimit = -1e30; //!< joint limit + real_t m_hiLimit = 1e30; //!< joint limit + real_t m_targetVelocity = 0.0; //!< target motor velocity + real_t m_maxMotorForce = 0.1; //!< max force on motor + real_t m_maxLimitForce = 300.0; //!< max force on limit + real_t m_damping = 1.0; //!< Damping. + real_t m_limitSoftness = 0.5; //! Relaxation factor + real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit + real_t m_bounce = 0.0; //!< restitution factor + bool m_enableMotor = false; + bool m_enableLimit = false; + + //!@} + + //! temp_variables + //!@{ + real_t m_currentLimitError = 0.0; //!< How much is violated this limit + int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit + real_t m_accumulatedImpulse = 0.0; + //!@} + + GodotG6DOFRotationalLimitMotor3D() {} + + bool isLimited() { + return (m_loLimit < m_hiLimit); + } + + // Need apply correction. + bool needApplyTorques() { + return (m_enableMotor || m_currentLimit != 0); + } + + // Calculates m_currentLimit and m_currentLimitError. + int testLimitValue(real_t test_value); + + // Apply the correction impulses for two bodies. + real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic); +}; + +class GodotG6DOFTranslationalLimitMotor3D { +public: + Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits + Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits + Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0); + //! Linear_Limit_parameters + //!@{ + Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit + Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit + Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit + //!@} + bool enable_limit[3] = { true, true, true }; + + //! Test limit + /*! + * - free means upper < lower, + * - locked means upper == lower + * - limited means upper > lower + * - limitIndex: first 3 are linear, next 3 are angular + */ + inline bool isLimited(int limitIndex) { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + real_t solveLinearAxis( + real_t timeStep, + real_t jacDiagABInv, + GodotBody3D *body1, const Vector3 &pointInA, + GodotBody3D *body2, const Vector3 &pointInB, + bool p_body1_dynamic, bool p_body2_dynamic, + int limit_index, + const Vector3 &axis_normal_on_a, + const Vector3 &anchorPos); +}; + +class GodotGeneric6DOFJoint3D : public GodotJoint3D { +protected: + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = { nullptr, nullptr }; + }; + + //! relative_frames + //!@{ + Transform3D m_frameInA; //!< the constraint space w.r.t body A + Transform3D m_frameInB; //!< the constraint space w.r.t body B + //!@} + + //! Jacobians + //!@{ + GodotJacobianEntry3D m_jacLinear[3]; //!< 3 orthogonal linear constraints + GodotJacobianEntry3D m_jacAng[3]; //!< 3 orthogonal angular constraints + //!@} + + //! Linear_Limit_parameters + //!@{ + GodotG6DOFTranslationalLimitMotor3D m_linearLimits; + //!@} + + //! hinge_parameters + //!@{ + GodotG6DOFRotationalLimitMotor3D m_angularLimits[3]; + //!@} + +protected: + //! temporal variables + //!@{ + real_t m_timeStep = 0.0; + Transform3D m_calculatedTransformA; + Transform3D m_calculatedTransformB; + Vector3 m_calculatedAxisAngleDiff; + Vector3 m_calculatedAxis[3]; + + Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes + + bool m_useLinearReferenceFrameA = false; + + //!@} + + GodotGeneric6DOFJoint3D(GodotGeneric6DOFJoint3D const &) = delete; + void operator=(GodotGeneric6DOFJoint3D const &) = delete; + + void buildLinearJacobian( + GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld, + const Vector3 &pivotAInW, const Vector3 &pivotBInW); + + void buildAngularJacobian(GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW); + + //! calcs the euler angles between the two bodies. + void calculateAngleInfo(); + +public: + GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA); + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + // Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies. + void calculateTransforms(); + + // Gets the global transform of the offset for body A. + const Transform3D &getCalculatedTransformA() const { + return m_calculatedTransformA; + } + + // Gets the global transform of the offset for body B. + const Transform3D &getCalculatedTransformB() const { + return m_calculatedTransformB; + } + + const Transform3D &getFrameOffsetA() const { + return m_frameInA; + } + + const Transform3D &getFrameOffsetB() const { + return m_frameInB; + } + + Transform3D &getFrameOffsetA() { + return m_frameInA; + } + + Transform3D &getFrameOffsetB() { + return m_frameInB; + } + + // Performs Jacobian calculation, and also calculates angle differences and axis. + void updateRHS(real_t timeStep); + + // Get the rotation axis in global coordinates. + Vector3 getAxis(int axis_index) const; + + // Get the relative Euler angle. + real_t getAngle(int axis_index) const; + + // Calculates angular correction and returns true if limit needs to be corrected. + bool testAngularLimitMotor(int axis_index); + + void setLinearLowerLimit(const Vector3 &linearLower) { + m_linearLimits.m_lowerLimit = linearLower; + } + + void setLinearUpperLimit(const Vector3 &linearUpper) { + m_linearLimits.m_upperLimit = linearUpper; + } + + void setAngularLowerLimit(const Vector3 &angularLower) { + m_angularLimits[0].m_loLimit = angularLower.x; + m_angularLimits[1].m_loLimit = angularLower.y; + m_angularLimits[2].m_loLimit = angularLower.z; + } + + void setAngularUpperLimit(const Vector3 &angularUpper) { + m_angularLimits[0].m_hiLimit = angularUpper.x; + m_angularLimits[1].m_hiLimit = angularUpper.y; + m_angularLimits[2].m_hiLimit = angularUpper.z; + } + + // Retrieves the angular limit information. + GodotG6DOFRotationalLimitMotor3D *getRotationalLimitMotor(int index) { + return &m_angularLimits[index]; + } + + // Retrieves the limit information. + GodotG6DOFTranslationalLimitMotor3D *getTranslationalLimitMotor() { + return &m_linearLimits; + } + + // First 3 are linear, next 3 are angular. + void setLimit(int axis, real_t lo, real_t hi) { + if (axis < 3) { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } else { + m_angularLimits[axis - 3].m_loLimit = lo; + m_angularLimits[axis - 3].m_hiLimit = hi; + } + } + + //! Test limit + /*! + * - free means upper < lower, + * - locked means upper == lower + * - limited means upper > lower + * - limitIndex: first 3 are linear, next 3 are angular + */ + bool isLimited(int limitIndex) { + if (limitIndex < 3) { + return m_linearLimits.isLimited(limitIndex); + } + return m_angularLimits[limitIndex - 3].isLimited(); + } + + const GodotBody3D *getRigidBodyA() const { + return A; + } + const GodotBody3D *getRigidBodyB() const { + return B; + } + + virtual void calcAnchorPos(); // overridable + + void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value); + real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const; + + void set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value); + bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const; +}; + +#endif // GODOT_GENERIC_6DOF_JOINT_3D_H diff --git a/modules/godot_physics_3d/joints/godot_hinge_joint_3d.cpp b/modules/godot_physics_3d/joints/godot_hinge_joint_3d.cpp new file mode 100644 index 0000000000..3d423f70e2 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_hinge_joint_3d.cpp @@ -0,0 +1,441 @@ +/**************************************************************************/ +/* godot_hinge_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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. */ +/**************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +#include "godot_hinge_joint_3d.h" + +GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB) : + GodotJoint3D(_arr, 2) { + A = rbA; + B = rbB; + + m_rbAFrame = frameA; + m_rbBFrame = frameB; + // flip axis + m_rbBFrame.basis[0][2] *= real_t(-1.); + m_rbBFrame.basis[1][2] *= real_t(-1.); + m_rbBFrame.basis[2][2] *= real_t(-1.); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, + const Vector3 &axisInA, const Vector3 &axisInB) : + GodotJoint3D(_arr, 2) { + A = rbA; + B = rbB; + + m_rbAFrame.origin = pivotInA; + + // since no frame is given, assume this to be zero angle and just pick rb transform axis + Vector3 rbAxisA1 = rbA->get_transform().basis.get_column(0); + + Vector3 rbAxisA2; + real_t projection = axisInA.dot(rbAxisA1); + if (projection >= 1.0f - CMP_EPSILON) { + rbAxisA1 = -rbA->get_transform().basis.get_column(2); + rbAxisA2 = rbA->get_transform().basis.get_column(1); + } else if (projection <= -1.0f + CMP_EPSILON) { + rbAxisA1 = rbA->get_transform().basis.get_column(2); + rbAxisA2 = rbA->get_transform().basis.get_column(1); + } else { + rbAxisA2 = axisInA.cross(rbAxisA1); + rbAxisA1 = rbAxisA2.cross(axisInA); + } + + m_rbAFrame.basis = Basis(rbAxisA1.x, rbAxisA2.x, axisInA.x, + rbAxisA1.y, rbAxisA2.y, axisInA.y, + rbAxisA1.z, rbAxisA2.z, axisInA.z); + + Quaternion rotationArc = Quaternion(axisInA, axisInB); + Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1); + Vector3 rbAxisB2 = axisInB.cross(rbAxisB1); + + m_rbBFrame.origin = pivotInB; + m_rbBFrame.basis = Basis(rbAxisB1.x, rbAxisB2.x, -axisInB.x, + rbAxisB1.y, rbAxisB2.y, -axisInB.y, + rbAxisB1.z, rbAxisB2.z, -axisInB.z); + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +bool GodotHingeJoint3D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + m_appliedImpulse = real_t(0.); + + if (!m_angularOnly) { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + Vector3 relPos = pivotBInW - pivotAInW; + + Vector3 normal[3]; + if (Math::is_zero_approx(relPos.length_squared())) { + normal[0] = Vector3(real_t(1.0), 0, 0); + } else { + normal[0] = relPos.normalized(); + } + + plane_space(normal[0], normal[1], normal[2]); + + for (int i = 0; i < 3; i++) { + memnew_placement( + &m_jac[i], + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + pivotAInW - A->get_transform().origin - A->get_center_of_mass(), + pivotBInW - B->get_transform().origin - B->get_center_of_mass(), + normal[i], + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + } + } + + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies + + //this is unused for now, it's a todo + Vector3 jointAxis0local; + Vector3 jointAxis1local; + + plane_space(m_rbAFrame.basis.get_column(2), jointAxis0local, jointAxis1local); + + Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local); + Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); + Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); + + memnew_placement( + &m_jacAng[0], + GodotJacobianEntry3D( + jointAxis0, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + memnew_placement( + &m_jacAng[1], + GodotJacobianEntry3D( + jointAxis1, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + memnew_placement( + &m_jacAng[2], + GodotJacobianEntry3D( + hingeAxisWorld, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + + // Compute limit information + real_t hingeAngle = get_hinge_angle(); + + //set bias, sign, clear accumulator + m_correction = real_t(0.); + m_limitSign = real_t(0.); + m_solveLimit = false; + m_accLimitImpulse = real_t(0.); + + if (m_useLimit && m_lowerLimit <= m_upperLimit) { + if (hingeAngle <= m_lowerLimit) { + m_correction = (m_lowerLimit - hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } else if (hingeAngle >= m_upperLimit) { + m_correction = m_upperLimit - hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } + } + + //Compute K = J*W*J' for hinge axis + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); + m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); + + return true; +} + +void GodotHingeJoint3D::solve(real_t p_step) { + Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); + Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); + + //real_t tau = real_t(0.3); + + //linear part + if (!m_angularOnly) { + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + for (int i = 0; i < 3; i++) { + const Vector3 &normal = m_jac[i].m_linearJointAxis; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + real_t rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } + } + } + + { + ///solve angular part + + // get axes in world space + Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(2)); + Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(2)); + + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); + + Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); + + Vector3 angAorthog = angVelA - angVelAroundHingeAxisA; + Vector3 angBorthog = angVelB - angVelAroundHingeAxisB; + Vector3 velrelOrthog = angAorthog - angBorthog; + { + //solve orthogonal angular velocity correction + real_t relaxation = real_t(1.); + real_t len = velrelOrthog.length(); + if (len > real_t(0.00001)) { + Vector3 normal = velrelOrthog.normalized(); + real_t denom = A->compute_angular_impulse_denominator(normal) + + B->compute_angular_impulse_denominator(normal); + // scale for mass and relaxation + velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor; + } + + //solve angular positional correction + Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step); + real_t len2 = angularError.length(); + if (len2 > real_t(0.00001)) { + Vector3 normal2 = angularError.normalized(); + real_t denom2 = A->compute_angular_impulse_denominator(normal2) + + B->compute_angular_impulse_denominator(normal2); + angularError *= (real_t(1.) / denom2) * relaxation; + } + + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } + + // solve limit + if (m_solveLimit) { + real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign; + + real_t impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + real_t temp = m_accLimitImpulse; + m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0)); + impulseMag = m_accLimitImpulse - temp; + + Vector3 impulse = axisA * impulseMag * m_limitSign; + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } + } + } + + //apply motor + if (m_enableAngularMotor) { + //todo: add limits too + Vector3 angularLimit(0, 0, 0); + + Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + real_t projRelVel = velrel.dot(axisA); + + real_t desiredMotorVel = m_motorTargetVelocity; + real_t motor_relvel = desiredMotorVel - projRelVel; + + real_t unclippedMotorImpulse = m_kHinge * motor_relvel; + //todo: should clip against accumulated impulse + real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + Vector3 motorImp = clippedMotorImpulse * axisA; + + if (dynamic_A) { + A->apply_torque_impulse(motorImp + angularLimit); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp - angularLimit); + } + } + } +} + +/* +void HingeJointSW::updateRHS(real_t timeStep) +{ + (void)timeStep; +} + +*/ + +real_t GodotHingeJoint3D::get_hinge_angle() { + const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(0)); + const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_column(1)); + const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_column(1)); + + return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); +} + +void GodotHingeJoint3D::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::HINGE_JOINT_BIAS: + tau = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: + m_upperLimit = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: + m_lowerLimit = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: + m_biasFactor = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: + m_limitSoftness = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: + m_relaxationFactor = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: + m_motorTargetVelocity = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: + m_maxMotorImpulse = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_MAX: + break; // Can't happen, but silences warning + } +} + +real_t GodotHingeJoint3D::get_param(PhysicsServer3D::HingeJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::HINGE_JOINT_BIAS: + return tau; + case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: + return m_upperLimit; + case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: + return m_lowerLimit; + case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: + return m_biasFactor; + case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: + return m_limitSoftness; + case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: + return m_relaxationFactor; + case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: + return m_motorTargetVelocity; + case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: + return m_maxMotorImpulse; + case PhysicsServer3D::HINGE_JOINT_MAX: + break; // Can't happen, but silences warning + } + + return 0; +} + +void GodotHingeJoint3D::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) { + switch (p_flag) { + case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: + m_useLimit = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: + m_enableAngularMotor = p_value; + break; + case PhysicsServer3D::HINGE_JOINT_FLAG_MAX: + break; // Can't happen, but silences warning + } +} + +bool GodotHingeJoint3D::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const { + switch (p_flag) { + case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: + return m_useLimit; + case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: + return m_enableAngularMotor; + case PhysicsServer3D::HINGE_JOINT_FLAG_MAX: + break; // Can't happen, but silences warning + } + + return false; +} diff --git a/modules/godot_physics_3d/joints/godot_hinge_joint_3d.h b/modules/godot_physics_3d/joints/godot_hinge_joint_3d.h new file mode 100644 index 0000000000..7f83509468 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_hinge_joint_3d.h @@ -0,0 +1,116 @@ +/**************************************************************************/ +/* godot_hinge_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_HINGE_JOINT_3D_H +#define GODOT_HINGE_JOINT_3D_H + +/* +Adapted to Godot from the Bullet library. +*/ + +#include "../godot_joint_3d.h" +#include "godot_jacobian_entry_3d.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +class GodotHingeJoint3D : public GodotJoint3D { + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = {}; + }; + + GodotJacobianEntry3D m_jac[3]; //3 orthogonal linear constraints + GodotJacobianEntry3D m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor + + Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis. + Transform3D m_rbBFrame; + + real_t m_motorTargetVelocity = 0.0; + real_t m_maxMotorImpulse = 0.0; + + real_t m_limitSoftness = 0.9; + real_t m_biasFactor = 0.3; + real_t m_relaxationFactor = 1.0; + + real_t m_lowerLimit = Math_PI; + real_t m_upperLimit = -Math_PI; + + real_t m_kHinge = 0.0; + + real_t m_limitSign = 0.0; + real_t m_correction = 0.0; + + real_t m_accLimitImpulse = 0.0; + + real_t tau = 0.3; + + bool m_useLimit = false; + bool m_angularOnly = false; + bool m_enableAngularMotor = false; + bool m_solveLimit = false; + + real_t m_appliedImpulse = 0.0; + +public: + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; } + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + real_t get_hinge_angle(); + + void set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::HingeJointParam p_param) const; + + void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value); + bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const; + + GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB); + GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); +}; + +#endif // GODOT_HINGE_JOINT_3D_H diff --git a/modules/godot_physics_3d/joints/godot_jacobian_entry_3d.h b/modules/godot_physics_3d/joints/godot_jacobian_entry_3d.h new file mode 100644 index 0000000000..d0c3c48ae6 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_jacobian_entry_3d.h @@ -0,0 +1,169 @@ +/**************************************************************************/ +/* godot_jacobian_entry_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_JACOBIAN_ENTRY_3D_H +#define GODOT_JACOBIAN_ENTRY_3D_H + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +#include "core/math/transform_3d.h" + +class GodotJacobianEntry3D { +public: + GodotJacobianEntry3D() {} + //constraint between two different rigidbodies + GodotJacobianEntry3D( + const Basis &world2A, + const Basis &world2B, + const Vector3 &rel_pos1, const Vector3 &rel_pos2, + const Vector3 &jointAxis, + const Vector3 &inertiaInvA, + const real_t massInvA, + const Vector3 &inertiaInvB, + const real_t massInvB) : + m_linearJointAxis(jointAxis) { + m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //angular constraint between two different rigidbodies + GodotJacobianEntry3D(const Vector3 &jointAxis, + const Basis &world2A, + const Basis &world2B, + const Vector3 &inertiaInvA, + const Vector3 &inertiaInvB) : + m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) { + m_aJ = world2A.xform(jointAxis); + m_bJ = world2B.xform(-jointAxis); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //angular constraint between two different rigidbodies + GodotJacobianEntry3D(const Vector3 &axisInA, + const Vector3 &axisInB, + const Vector3 &inertiaInvA, + const Vector3 &inertiaInvB) : + m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), + m_aJ(axisInA), + m_bJ(-axisInB) { + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //constraint on one rigidbody + GodotJacobianEntry3D( + const Basis &world2A, + const Vector3 &rel_pos1, const Vector3 &rel_pos2, + const Vector3 &jointAxis, + const Vector3 &inertiaInvA, + const real_t massInvA) : + m_linearJointAxis(jointAxis) { + m_aJ = world2A.xform(rel_pos1.cross(jointAxis)); + m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.)); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + real_t getDiagonal() const { return m_Adiag; } + + // for two constraints on the same rigidbody (for example vehicle friction) + real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA) const { + const GodotJacobianEntry3D &jacA = *this; + real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); + real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); + return lin + ang; + } + + // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) + real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA, const real_t massInvB) const { + const GodotJacobianEntry3D &jacA = *this; + Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; + Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; + Vector3 lin0 = massInvA * lin; + Vector3 lin1 = massInvB * lin; + Vector3 sum = ang0 + ang1 + lin0 + lin1; + return sum[0] + sum[1] + sum[2]; + } + + real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) { + Vector3 linrel = linvelA - linvelB; + Vector3 angvela = angvelA * m_aJ; + Vector3 angvelb = angvelB * m_bJ; + linrel *= m_linearJointAxis; + angvela += angvelb; + angvela += linrel; + real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2]; + return rel_vel2 + CMP_EPSILON; + } + //private: + + Vector3 m_linearJointAxis; + Vector3 m_aJ; + Vector3 m_bJ; + Vector3 m_0MinvJt; + Vector3 m_1MinvJt; + //Optimization: can be stored in the w/last component of one of the vectors + real_t m_Adiag = 1.0; +}; + +#endif // GODOT_JACOBIAN_ENTRY_3D_H diff --git a/modules/godot_physics_3d/joints/godot_pin_joint_3d.cpp b/modules/godot_physics_3d/joints/godot_pin_joint_3d.cpp new file mode 100644 index 0000000000..05ae0839e4 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_pin_joint_3d.cpp @@ -0,0 +1,181 @@ +/**************************************************************************/ +/* godot_pin_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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. */ +/**************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +#include "godot_pin_joint_3d.h" + +bool GodotPinJoint3D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + m_appliedImpulse = real_t(0.); + + Vector3 normal(0, 0, 0); + + for (int i = 0; i < 3; i++) { + normal[i] = 1; + memnew_placement( + &m_jac[i], + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), + B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(), + normal, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + normal[i] = 0; + } + + return true; +} + +void GodotPinJoint3D::solve(real_t p_step) { + Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); + Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); + + Vector3 normal(0, 0, 0); + + //Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity(); + //Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity(); + + for (int i = 0; i < 3; i++) { + normal[i] = 1; + real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal(); + + Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; + Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; + //this jacobian entry could be re-used for all iterations + + Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1); + Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2); + Vector3 vel = vel1 - vel2; + + real_t rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA, + B->getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv; + + real_t impulseClamp = m_impulseClamp; + if (impulseClamp > 0) { + if (impulse < -impulseClamp) { + impulse = -impulseClamp; + } + if (impulse > impulseClamp) { + impulse = impulseClamp; + } + } + + m_appliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, pivotAInW - A->get_transform().origin); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, pivotBInW - B->get_transform().origin); + } + + normal[i] = 0; + } +} + +void GodotPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: + m_tau = p_value; + break; + case PhysicsServer3D::PIN_JOINT_DAMPING: + m_damping = p_value; + break; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: + m_impulseClamp = p_value; + break; + } +} + +real_t GodotPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::PIN_JOINT_BIAS: + return m_tau; + case PhysicsServer3D::PIN_JOINT_DAMPING: + return m_damping; + case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: + return m_impulseClamp; + } + + return 0; +} + +GodotPinJoint3D::GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b) : + GodotJoint3D(_arr, 2) { + A = p_body_a; + B = p_body_b; + m_pivotInA = p_pos_a; + m_pivotInB = p_pos_b; + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +GodotPinJoint3D::~GodotPinJoint3D() { +} diff --git a/modules/godot_physics_3d/joints/godot_pin_joint_3d.h b/modules/godot_physics_3d/joints/godot_pin_joint_3d.h new file mode 100644 index 0000000000..62d3068e09 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_pin_joint_3d.h @@ -0,0 +1,95 @@ +/**************************************************************************/ +/* godot_pin_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_PIN_JOINT_3D_H +#define GODOT_PIN_JOINT_3D_H + +/* +Adapted to Godot from the Bullet library. +*/ + +#include "../godot_joint_3d.h" +#include "godot_jacobian_entry_3d.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +class GodotPinJoint3D : public GodotJoint3D { + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = {}; + }; + + real_t m_tau = 0.3; //bias + real_t m_damping = 1.0; + real_t m_impulseClamp = 0.0; + real_t m_appliedImpulse = 0.0; + + GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints + + Vector3 m_pivotInA; + Vector3 m_pivotInB; + +public: + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; } + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::PinJointParam p_param) const; + + void set_pos_a(const Vector3 &p_pos) { m_pivotInA = p_pos; } + void set_pos_b(const Vector3 &p_pos) { m_pivotInB = p_pos; } + + Vector3 get_position_a() { return m_pivotInA; } + Vector3 get_position_b() { return m_pivotInB; } + + GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b); + ~GodotPinJoint3D(); +}; + +#endif // GODOT_PIN_JOINT_3D_H diff --git a/modules/godot_physics_3d/joints/godot_slider_joint_3d.cpp b/modules/godot_physics_3d/joints/godot_slider_joint_3d.cpp new file mode 100644 index 0000000000..b9dca94b37 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_slider_joint_3d.cpp @@ -0,0 +1,478 @@ +/**************************************************************************/ +/* godot_slider_joint_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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. */ +/**************************************************************************/ + +/* +Adapted to Godot from the Bullet library. +*/ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +*/ + +#include "godot_slider_joint_3d.h" + +//----------------------------------------------------------------------------- + +GodotSliderJoint3D::GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB) : + GodotJoint3D(_arr, 2), + m_frameInA(frameInA), + m_frameInB(frameInB) { + A = rbA; + B = rbB; + + A->add_constraint(this, 0); + B->add_constraint(this, 1); +} + +//----------------------------------------------------------------------------- + +bool GodotSliderJoint3D::setup(real_t p_step) { + dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); + + if (!dynamic_A && !dynamic_B) { + return false; + } + + //calculate transforms + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + m_realPivotAInW = m_calculatedTransformA.origin; + m_realPivotBInW = m_calculatedTransformB.origin; + m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + m_relPosA = m_projPivotInW - A->get_transform().origin; + m_relPosB = m_realPivotBInW - B->get_transform().origin; + Vector3 normalWorld; + int i; + //linear part + for (i = 0; i < 3; i++) { + normalWorld = m_calculatedTransformA.basis.get_column(i); + memnew_placement( + &m_jacLin[i], + GodotJacobianEntry3D( + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + m_relPosA - A->get_center_of_mass(), + m_relPosB - B->get_center_of_mass(), + normalWorld, + A->get_inv_inertia(), + A->get_inv_mass(), + B->get_inv_inertia(), + B->get_inv_mass())); + m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); + m_depth[i] = m_delta.dot(normalWorld); + } + testLinLimits(); + // angular part + for (i = 0; i < 3; i++) { + normalWorld = m_calculatedTransformA.basis.get_column(i); + memnew_placement( + &m_jacAng[i], + GodotJacobianEntry3D( + normalWorld, + A->get_principal_inertia_axes().transposed(), + B->get_principal_inertia_axes().transposed(), + A->get_inv_inertia(), + B->get_inv_inertia())); + } + testAngLimits(); + Vector3 axisA = m_calculatedTransformA.basis.get_column(0); + m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); + // clear accumulator for motors + m_accumulatedLinMotorImpulse = real_t(0.0); + m_accumulatedAngMotorImpulse = real_t(0.0); + + return true; +} + +//----------------------------------------------------------------------------- + +void GodotSliderJoint3D::solve(real_t p_step) { + int i; + // linear + Vector3 velA = A->get_velocity_in_local_point(m_relPosA); + Vector3 velB = B->get_velocity_in_local_point(m_relPosB); + Vector3 vel = velA - velB; + for (i = 0; i < 3; i++) { + const Vector3 &normal = m_jacLin[i].m_linearJointAxis; + real_t rel_vel = normal.dot(vel); + // calculate positional error + real_t depth = m_depth[i]; + // get parameters + real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); + real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); + real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); + // Calculate and apply impulse. + real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i]; + Vector3 impulse_vector = normal * normalImpulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } + if (m_poweredLinMotor && (!i)) { // apply linear motor + if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { + real_t desiredMotorVel = m_targetLinMotorVelocity; + real_t motor_relvel = desiredMotorVel + rel_vel; + normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; + // clamp accumulated impulse + real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse); + if (new_acc > m_maxLinMotorForce) { + new_acc = m_maxLinMotorForce; + } + real_t del = new_acc - m_accumulatedLinMotorImpulse; + if (normalImpulse < real_t(0.0)) { + normalImpulse = -del; + } else { + normalImpulse = del; + } + m_accumulatedLinMotorImpulse = new_acc; + // apply clamped impulse + impulse_vector = normal * normalImpulse; + if (dynamic_A) { + A->apply_impulse(impulse_vector, m_relPosA); + } + if (dynamic_B) { + B->apply_impulse(-impulse_vector, m_relPosB); + } + } + } + } + // angular + // get axes in world space + Vector3 axisA = m_calculatedTransformA.basis.get_column(0); + Vector3 axisB = m_calculatedTransformB.basis.get_column(0); + + const Vector3 &angVelA = A->get_angular_velocity(); + const Vector3 &angVelB = B->get_angular_velocity(); + + Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); + Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); + + Vector3 angAorthog = angVelA - angVelAroundAxisA; + Vector3 angBorthog = angVelB - angVelAroundAxisB; + Vector3 velrelOrthog = angAorthog - angBorthog; + //solve orthogonal angular velocity correction + real_t len = velrelOrthog.length(); + if (len > real_t(0.00001)) { + Vector3 normal = velrelOrthog.normalized(); + real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal); + velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng; + } + //solve angular positional correction + Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step); + real_t len2 = angularError.length(); + if (len2 > real_t(0.00001)) { + Vector3 normal2 = angularError.normalized(); + real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2); + angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + } + // apply impulse + if (dynamic_A) { + A->apply_torque_impulse(-velrelOrthog + angularError); + } + if (dynamic_B) { + B->apply_torque_impulse(velrelOrthog - angularError); + } + real_t impulseMag; + //solve angular limits + if (m_solveAngLim) { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step; + impulseMag *= m_kAngle * m_softnessLimAng; + } else { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step; + impulseMag *= m_kAngle * m_softnessDirAng; + } + Vector3 impulse = axisA * impulseMag; + if (dynamic_A) { + A->apply_torque_impulse(impulse); + } + if (dynamic_B) { + B->apply_torque_impulse(-impulse); + } + //apply angular motor + if (m_poweredAngMotor) { + if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { + Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB; + real_t projRelVel = velrel.dot(axisA); + + real_t desiredMotorVel = m_targetAngMotorVelocity; + real_t motor_relvel = desiredMotorVel - projRelVel; + + real_t angImpulse = m_kAngle * motor_relvel; + // clamp accumulated impulse + real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse); + if (new_acc > m_maxAngMotorForce) { + new_acc = m_maxAngMotorForce; + } + real_t del = new_acc - m_accumulatedAngMotorImpulse; + if (angImpulse < real_t(0.0)) { + angImpulse = -del; + } else { + angImpulse = del; + } + m_accumulatedAngMotorImpulse = new_acc; + // apply clamped impulse + Vector3 motorImp = angImpulse * axisA; + if (dynamic_A) { + A->apply_torque_impulse(motorImp); + } + if (dynamic_B) { + B->apply_torque_impulse(-motorImp); + } + } + } +} + +//----------------------------------------------------------------------------- + +void GodotSliderJoint3D::calculateTransforms() { + m_calculatedTransformA = A->get_transform() * m_frameInA; + m_calculatedTransformB = B->get_transform() * m_frameInB; + m_realPivotAInW = m_calculatedTransformA.origin; + m_realPivotBInW = m_calculatedTransformB.origin; + m_sliderAxis = m_calculatedTransformA.basis.get_column(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + Vector3 normalWorld; + int i; + //linear part + for (i = 0; i < 3; i++) { + normalWorld = m_calculatedTransformA.basis.get_column(i); + m_depth[i] = m_delta.dot(normalWorld); + } +} + +//----------------------------------------------------------------------------- + +void GodotSliderJoint3D::testLinLimits() { + m_solveLinLim = false; + m_linPos = m_depth[0]; + if (m_lowerLinLimit <= m_upperLinLimit) { + if (m_depth[0] > m_upperLinLimit) { + m_depth[0] -= m_upperLinLimit; + m_solveLinLim = true; + } else if (m_depth[0] < m_lowerLinLimit) { + m_depth[0] -= m_lowerLinLimit; + m_solveLinLim = true; + } else { + m_depth[0] = real_t(0.); + } + } else { + m_depth[0] = real_t(0.); + } +} + +//----------------------------------------------------------------------------- + +void GodotSliderJoint3D::testAngLimits() { + m_angDepth = real_t(0.); + m_solveAngLim = false; + if (m_lowerAngLimit <= m_upperAngLimit) { + const Vector3 axisA0 = m_calculatedTransformA.basis.get_column(1); + const Vector3 axisA1 = m_calculatedTransformA.basis.get_column(2); + const Vector3 axisB0 = m_calculatedTransformB.basis.get_column(1); + real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + if (rot < m_lowerAngLimit) { + m_angDepth = rot - m_lowerAngLimit; + m_solveAngLim = true; + } else if (rot > m_upperAngLimit) { + m_angDepth = rot - m_upperAngLimit; + m_solveAngLim = true; + } + } +} + +//----------------------------------------------------------------------------- + +Vector3 GodotSliderJoint3D::getAncorInA() { + Vector3 ancorInA; + ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; + ancorInA = A->get_transform().inverse().xform(ancorInA); + return ancorInA; +} + +//----------------------------------------------------------------------------- + +Vector3 GodotSliderJoint3D::getAncorInB() { + Vector3 ancorInB; + ancorInB = m_frameInB.origin; + return ancorInB; +} + +void GodotSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) { + switch (p_param) { + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: + m_upperLinLimit = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: + m_lowerLinLimit = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: + m_softnessLimLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: + m_restitutionLimLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: + m_dampingLimLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: + m_softnessDirLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: + m_restitutionDirLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: + m_dampingDirLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: + m_softnessOrthoLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: + m_restitutionOrthoLin = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: + m_dampingOrthoLin = p_value; + break; + + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: + m_upperAngLimit = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: + m_lowerAngLimit = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: + m_softnessLimAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: + m_restitutionLimAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: + m_dampingLimAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: + m_softnessDirAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: + m_restitutionDirAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: + m_dampingDirAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: + m_softnessOrthoAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: + m_restitutionOrthoAng = p_value; + break; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: + m_dampingOrthoAng = p_value; + break; + + case PhysicsServer3D::SLIDER_JOINT_MAX: + break; // Can't happen, but silences warning + } +} + +real_t GodotSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const { + switch (p_param) { + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: + return m_upperLinLimit; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: + return m_lowerLinLimit; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: + return m_softnessLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: + return m_restitutionLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: + return m_dampingLimLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: + return m_softnessDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: + return m_restitutionDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: + return m_dampingDirLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: + return m_softnessOrthoLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: + return m_restitutionOrthoLin; + case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: + return m_dampingOrthoLin; + + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: + return m_upperAngLimit; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: + return m_lowerAngLimit; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: + return m_softnessLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: + return m_restitutionLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: + return m_dampingLimAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: + return m_softnessDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: + return m_restitutionDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: + return m_dampingDirAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: + return m_softnessOrthoAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: + return m_restitutionOrthoAng; + case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: + return m_dampingOrthoAng; + + case PhysicsServer3D::SLIDER_JOINT_MAX: + break; // Can't happen, but silences warning + } + + return 0; +} diff --git a/modules/godot_physics_3d/joints/godot_slider_joint_3d.h b/modules/godot_physics_3d/joints/godot_slider_joint_3d.h new file mode 100644 index 0000000000..99fabf8638 --- /dev/null +++ b/modules/godot_physics_3d/joints/godot_slider_joint_3d.h @@ -0,0 +1,246 @@ +/**************************************************************************/ +/* godot_slider_joint_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_SLIDER_JOINT_3D_H +#define GODOT_SLIDER_JOINT_3D_H + +/* +Adapted to Godot from the Bullet library. +*/ + +#include "../godot_joint_3d.h" +#include "godot_jacobian_entry_3d.h" + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +*/ + +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7)) + +//----------------------------------------------------------------------------- + +class GodotSliderJoint3D : public GodotJoint3D { +protected: + union { + struct { + GodotBody3D *A; + GodotBody3D *B; + }; + + GodotBody3D *_arr[2] = { nullptr, nullptr }; + }; + + Transform3D m_frameInA; + Transform3D m_frameInB; + + // linear limits + real_t m_lowerLinLimit = 1.0; + real_t m_upperLinLimit = -1.0; + // angular limits + real_t m_lowerAngLimit = 0.0; + real_t m_upperAngLimit = 0.0; + // softness, restitution and damping for different cases + // DirLin - moving inside linear limits + // LimLin - hitting linear limit + // DirAng - moving inside angular limits + // LimAng - hitting angular limit + // OrthoLin, OrthoAng - against constraint axis + real_t m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingDirLin = 0.0; + real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingDirAng = 0.0; + real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; + + // for interlal use + bool m_solveLinLim = false; + bool m_solveAngLim = false; + + GodotJacobianEntry3D m_jacLin[3] = {}; + real_t m_jacLinDiagABInv[3] = {}; + + GodotJacobianEntry3D m_jacAng[3] = {}; + + real_t m_timeStep = 0.0; + Transform3D m_calculatedTransformA; + Transform3D m_calculatedTransformB; + + Vector3 m_sliderAxis; + Vector3 m_realPivotAInW; + Vector3 m_realPivotBInW; + Vector3 m_projPivotInW; + Vector3 m_delta; + Vector3 m_depth; + Vector3 m_relPosA; + Vector3 m_relPosB; + + real_t m_linPos = 0.0; + + real_t m_angDepth = 0.0; + real_t m_kAngle = 0.0; + + bool m_poweredLinMotor = false; + real_t m_targetLinMotorVelocity = 0.0; + real_t m_maxLinMotorForce = 0.0; + real_t m_accumulatedLinMotorImpulse = 0.0; + + bool m_poweredAngMotor = false; + real_t m_targetAngMotorVelocity = 0.0; + real_t m_maxAngMotorForce = 0.0; + real_t m_accumulatedAngMotorImpulse = 0.0; + +public: + // constructors + GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB); + //SliderJointSW(); + // overrides + + // access + const GodotBody3D *getRigidBodyA() const { return A; } + const GodotBody3D *getRigidBodyB() const { return B; } + const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; } + const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; } + const Transform3D &getFrameOffsetA() const { return m_frameInA; } + const Transform3D &getFrameOffsetB() const { return m_frameInB; } + Transform3D &getFrameOffsetA() { return m_frameInA; } + Transform3D &getFrameOffsetB() { return m_frameInB; } + real_t getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; } + real_t getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; } + real_t getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; } + real_t getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; } + + real_t getSoftnessDirLin() { return m_softnessDirLin; } + real_t getRestitutionDirLin() { return m_restitutionDirLin; } + real_t getDampingDirLin() { return m_dampingDirLin; } + real_t getSoftnessDirAng() { return m_softnessDirAng; } + real_t getRestitutionDirAng() { return m_restitutionDirAng; } + real_t getDampingDirAng() { return m_dampingDirAng; } + real_t getSoftnessLimLin() { return m_softnessLimLin; } + real_t getRestitutionLimLin() { return m_restitutionLimLin; } + real_t getDampingLimLin() { return m_dampingLimLin; } + real_t getSoftnessLimAng() { return m_softnessLimAng; } + real_t getRestitutionLimAng() { return m_restitutionLimAng; } + real_t getDampingLimAng() { return m_dampingLimAng; } + real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; } + real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; } + real_t getDampingOrthoLin() { return m_dampingOrthoLin; } + real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; } + real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; } + real_t getDampingOrthoAng() { return m_dampingOrthoAng; } + void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; } + void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } + void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; } + void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; } + void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } + void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; } + void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; } + void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } + void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; } + void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; } + void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } + void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; } + void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } + void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } + void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } + void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } + void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } + void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } + void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } + bool getPoweredLinMotor() { return m_poweredLinMotor; } + void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } + real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } + void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } + real_t getMaxLinMotorForce() { return m_maxLinMotorForce; } + void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } + bool getPoweredAngMotor() { return m_poweredAngMotor; } + void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } + real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } + void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } + real_t getMaxAngMotorForce() { return m_maxAngMotorForce; } + real_t getLinearPos() { return m_linPos; } + + // access for ODE solver + bool getSolveLinLimit() { return m_solveLinLim; } + real_t getLinDepth() { return m_depth[0]; } + bool getSolveAngLimit() { return m_solveAngLim; } + real_t getAngDepth() { return m_angDepth; } + // shared code used by ODE solver + void calculateTransforms(); + void testLinLimits(); + void testAngLimits(); + // access for PE Solver + Vector3 getAncorInA(); + Vector3 getAncorInB(); + + void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value); + real_t get_param(PhysicsServer3D::SliderJointParam p_param) const; + + virtual bool setup(real_t p_step) override; + virtual void solve(real_t p_step) override; + + virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; } +}; + +#endif // GODOT_SLIDER_JOINT_3D_H diff --git a/modules/godot_physics_3d/register_types.cpp b/modules/godot_physics_3d/register_types.cpp new file mode 100644 index 0000000000..1b1690cf59 --- /dev/null +++ b/modules/godot_physics_3d/register_types.cpp @@ -0,0 +1,61 @@ +/**************************************************************************/ +/* register_types.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 "register_types.h" + +#include "godot_physics_server_3d.h" +#include "servers/physics_server_3d.h" +#include "servers/physics_server_3d_wrap_mt.h" + +static PhysicsServer3D *_createGodotPhysics3DCallback() { +#ifdef THREADS_ENABLED + bool using_threads = GLOBAL_GET("physics/3d/run_on_separate_thread"); +#else + bool using_threads = false; +#endif + + PhysicsServer3D *physics_server_3d = memnew(GodotPhysicsServer3D(using_threads)); + + return memnew(PhysicsServer3DWrapMT(physics_server_3d, using_threads)); +} + +void initialize_godot_physics_3d_module(ModuleInitializationLevel p_level) { + if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) { + return; + } + PhysicsServer3DManager::get_singleton()->register_server("GodotPhysics3D", callable_mp_static(_createGodotPhysics3DCallback)); + PhysicsServer3DManager::get_singleton()->set_default_server("GodotPhysics3D"); +} + +void uninitialize_godot_physics_3d_module(ModuleInitializationLevel p_level) { + if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) { + return; + } +} diff --git a/modules/godot_physics_3d/register_types.h b/modules/godot_physics_3d/register_types.h new file mode 100644 index 0000000000..998fb4a1ee --- /dev/null +++ b/modules/godot_physics_3d/register_types.h @@ -0,0 +1,39 @@ +/**************************************************************************/ +/* register_types.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 GODOT_PHYSICS_3D_REGISTER_TYPES_H +#define GODOT_PHYSICS_3D_REGISTER_TYPES_H + +#include "modules/register_module_types.h" + +void initialize_godot_physics_3d_module(ModuleInitializationLevel p_level); +void uninitialize_godot_physics_3d_module(ModuleInitializationLevel p_level); + +#endif // GODOT_PHYSICS_3D_REGISTER_TYPES_H diff --git a/modules/gridmap/SCsub b/modules/gridmap/SCsub index 282d772592..d4baa9000e 100644 --- a/modules/gridmap/SCsub +++ b/modules/gridmap/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/hdr/SCsub b/modules/hdr/SCsub index 10629bda3c..739b2caecf 100644 --- a/modules/hdr/SCsub +++ b/modules/hdr/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/interactive_music/SCsub b/modules/interactive_music/SCsub index 2950a30854..f2546747a0 100644 --- a/modules/interactive_music/SCsub +++ b/modules/interactive_music/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/jpg/SCsub b/modules/jpg/SCsub index b840542c1b..2d948d3355 100644 --- a/modules/jpg/SCsub +++ b/modules/jpg/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/jsonrpc/SCsub b/modules/jsonrpc/SCsub index 8ee4f8bfea..923567b138 100644 --- a/modules/jsonrpc/SCsub +++ b/modules/jsonrpc/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/ktx/SCsub b/modules/ktx/SCsub index c4cb732498..f4c394d734 100644 --- a/modules/ktx/SCsub +++ b/modules/ktx/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/lightmapper_rd/SCsub b/modules/lightmapper_rd/SCsub index fe9737b36f..157381ae98 100644 --- a/modules/lightmapper_rd/SCsub +++ b/modules/lightmapper_rd/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/lightmapper_rd/lm_compute.glsl b/modules/lightmapper_rd/lm_compute.glsl index 2c85fff6f3..31b721bb20 100644 --- a/modules/lightmapper_rd/lm_compute.glsl +++ b/modules/lightmapper_rd/lm_compute.glsl @@ -485,7 +485,7 @@ void trace_direct_light(vec3 p_position, vec3 p_normal, uint p_light_index, bool float a = randomize(r_noise) * 2.0 * PI; float vogel_index = float(total_ray_count - 1 - (i * shadowing_ray_count + j)); // Start from (total_ray_count - 1) so we check the outer points first. - vec2 light_disk_sample = (get_vogel_disk(vogel_index, a, shadowing_ray_count_sqrt)) * soft_shadowing_disk_size * light_data.shadow_blur; + vec2 light_disk_sample = get_vogel_disk(vogel_index, a, shadowing_ray_count_sqrt) * soft_shadowing_disk_size * light_data.shadow_blur; vec3 light_disk_to_point = normalize(light_to_point + light_disk_sample.x * light_to_point_tan + light_disk_sample.y * light_to_point_bitan); // Offset the ray origin for AA, offset the light position for soft shadows. if (trace_ray_any_hit(origin - light_disk_to_point * (bake_params.bias + length(disk_sample)), p_position - light_disk_to_point * dist) == RAY_MISS) { diff --git a/modules/mbedtls/SCsub b/modules/mbedtls/SCsub index 90ce98c751..6183fa5944 100644 --- a/modules/mbedtls/SCsub +++ b/modules/mbedtls/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") @@ -65,6 +66,22 @@ if env["builtin_mbedtls"]: "platform.c", "platform_util.c", "poly1305.c", + "psa_crypto.c", + "psa_crypto_aead.c", + "psa_crypto_cipher.c", + "psa_crypto_client.c", + "psa_crypto_driver_wrappers_no_static.c", + "psa_crypto_ecp.c", + "psa_crypto_ffdh.c", + "psa_crypto_hash.c", + "psa_crypto_mac.c", + "psa_crypto_pake.c", + "psa_crypto_rsa.c", + "psa_crypto_se.c", + "psa_crypto_slot_management.c", + "psa_crypto_storage.c", + "psa_its_file.c", + "psa_util.c", "ripemd160.c", "rsa.c", "rsa_alt_helpers.c", diff --git a/modules/mbedtls/crypto_mbedtls.cpp b/modules/mbedtls/crypto_mbedtls.cpp index 0d97b5fc1a..be7aaef9d4 100644 --- a/modules/mbedtls/crypto_mbedtls.cpp +++ b/modules/mbedtls/crypto_mbedtls.cpp @@ -314,10 +314,6 @@ Crypto *CryptoMbedTLS::create(bool p_notify_postinitialize) { } void CryptoMbedTLS::initialize_crypto() { -#ifdef DEBUG_ENABLED - mbedtls_debug_set_threshold(1); -#endif - Crypto::_create = create; Crypto::_load_default_certificates = load_default_certificates; X509CertificateMbedTLS::make_default(); diff --git a/modules/mbedtls/register_types.cpp b/modules/mbedtls/register_types.cpp index df5bce05e4..bf65dfb0b7 100644 --- a/modules/mbedtls/register_types.cpp +++ b/modules/mbedtls/register_types.cpp @@ -35,15 +35,34 @@ #include "packet_peer_mbed_dtls.h" #include "stream_peer_mbedtls.h" +#if MBEDTLS_VERSION_MAJOR >= 3 +#include <psa/crypto.h> +#endif + #ifdef TESTS_ENABLED #include "tests/test_crypto_mbedtls.h" #endif +static bool godot_mbedtls_initialized = false; + void initialize_mbedtls_module(ModuleInitializationLevel p_level) { if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) { return; } +#if MBEDTLS_VERSION_MAJOR >= 3 + int status = psa_crypto_init(); + ERR_FAIL_COND_MSG(status != PSA_SUCCESS, "Failed to initialize psa crypto. The mbedTLS modules will not work."); +#endif + +#ifdef DEBUG_ENABLED + if (OS::get_singleton()->is_stdout_verbose()) { + mbedtls_debug_set_threshold(1); + } +#endif + + godot_mbedtls_initialized = true; + CryptoMbedTLS::initialize_crypto(); StreamPeerMbedTLS::initialize_tls(); PacketPeerMbedDTLS::initialize_dtls(); @@ -55,6 +74,14 @@ void uninitialize_mbedtls_module(ModuleInitializationLevel p_level) { return; } + if (!godot_mbedtls_initialized) { + return; + } + +#if MBEDTLS_VERSION_MAJOR >= 3 + mbedtls_psa_crypto_free(); +#endif + DTLSServerMbedTLS::finalize(); PacketPeerMbedDTLS::finalize_dtls(); StreamPeerMbedTLS::finalize_tls(); diff --git a/modules/meshoptimizer/SCsub b/modules/meshoptimizer/SCsub index 3f86bb4f00..b335b5db3a 100644 --- a/modules/meshoptimizer/SCsub +++ b/modules/meshoptimizer/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/minimp3/SCsub b/modules/minimp3/SCsub index 09e84f71e9..e9491bb72f 100644 --- a/modules/minimp3/SCsub +++ b/modules/minimp3/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/mobile_vr/SCsub b/modules/mobile_vr/SCsub index e6c43228b4..b237f31209 100644 --- a/modules/mobile_vr/SCsub +++ b/modules/mobile_vr/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/mono/SCsub b/modules/mono/SCsub index d267df938a..f74f0fb9c1 100644 --- a/modules/mono/SCsub +++ b/modules/mono/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * import build_scripts.mono_configure as mono_configure diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/TestData/GeneratedSources/EventSignals_ScriptSignals.generated.cs b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/TestData/GeneratedSources/EventSignals_ScriptSignals.generated.cs index cc45e5746f..188972e6fe 100644 --- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/TestData/GeneratedSources/EventSignals_ScriptSignals.generated.cs +++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/TestData/GeneratedSources/EventSignals_ScriptSignals.generated.cs @@ -32,6 +32,10 @@ partial class EventSignals add => backing_MySignal += value; remove => backing_MySignal -= value; } + protected void OnMySignal(string str, int num) + { + EmitSignal(SignalName.MySignal, str, num); + } /// <inheritdoc/> [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] protected override void RaiseGodotClassSignalCallbacks(in godot_string_name signal, NativeVariantPtrArgs args) diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ExtensionMethods.cs b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ExtensionMethods.cs index 957d5789df..62fa7b0a36 100644 --- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ExtensionMethods.cs +++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ExtensionMethods.cs @@ -155,6 +155,32 @@ namespace Godot.SourceGenerators }; } + public static string GetAccessibilityKeyword(this INamedTypeSymbol namedTypeSymbol) + { + if (namedTypeSymbol.DeclaredAccessibility == Accessibility.NotApplicable) + { + // Accessibility not specified. Get the default accessibility. + return namedTypeSymbol.ContainingSymbol switch + { + null or INamespaceSymbol => "internal", + ITypeSymbol { TypeKind: TypeKind.Class or TypeKind.Struct } => "private", + ITypeSymbol { TypeKind: TypeKind.Interface } => "public", + _ => "", + }; + } + + return namedTypeSymbol.DeclaredAccessibility switch + { + Accessibility.Private => "private", + Accessibility.Protected => "protected", + Accessibility.Internal => "internal", + Accessibility.ProtectedAndInternal => "private", + Accessibility.ProtectedOrInternal => "private", + Accessibility.Public => "public", + _ => "", + }; + } + public static string NameWithTypeParameters(this INamedTypeSymbol symbol) { return symbol.IsGenericType ? diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ScriptSignalsGenerator.cs b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ScriptSignalsGenerator.cs index 54f2212339..0dda43ab4c 100644 --- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ScriptSignalsGenerator.cs +++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ScriptSignalsGenerator.cs @@ -5,13 +5,6 @@ using Microsoft.CodeAnalysis; using Microsoft.CodeAnalysis.CSharp.Syntax; using Microsoft.CodeAnalysis.Text; -// TODO: -// Determine a proper way to emit the signal. -// 'Emit(nameof(TheEvent))' creates a StringName every time and has the overhead of string marshaling. -// I haven't decided on the best option yet. Some possibilities: -// - Expose the generated StringName fields to the user, for use with 'Emit(...)'. -// - Generate a 'EmitSignalName' method for each event signal. - namespace Godot.SourceGenerators { [Generator] @@ -276,7 +269,7 @@ namespace Godot.SourceGenerators source.Append( $" /// <inheritdoc cref=\"{signalDelegate.DelegateSymbol.FullQualifiedNameIncludeGlobal()}\"/>\n"); - source.Append(" public event ") + source.Append($" {signalDelegate.DelegateSymbol.GetAccessibilityKeyword()} event ") .Append(signalDelegate.DelegateSymbol.FullQualifiedNameIncludeGlobal()) .Append(" @") .Append(signalName) @@ -288,6 +281,43 @@ namespace Godot.SourceGenerators .Append(signalName) .Append(" -= value;\n") .Append("}\n"); + + // Generate On{EventName} method to raise the event + + var invokeMethodSymbol = signalDelegate.InvokeMethodData.Method; + int paramCount = invokeMethodSymbol.Parameters.Length; + + string raiseMethodModifiers = signalDelegate.DelegateSymbol.ContainingType.IsSealed ? + "private" : + "protected"; + + source.Append($" {raiseMethodModifiers} void On{signalName}("); + for (int i = 0; i < paramCount; i++) + { + var paramSymbol = invokeMethodSymbol.Parameters[i]; + source.Append($"{paramSymbol.Type.FullQualifiedNameIncludeGlobal()} {paramSymbol.Name}"); + if (i < paramCount - 1) + { + source.Append(", "); + } + } + source.Append(")\n"); + source.Append(" {\n"); + source.Append($" EmitSignal(SignalName.{signalName}"); + foreach (var paramSymbol in invokeMethodSymbol.Parameters) + { + // Enums must be converted to the underlying type before they can be implicitly converted to Variant + if (paramSymbol.Type.TypeKind == TypeKind.Enum) + { + var underlyingType = ((INamedTypeSymbol)paramSymbol.Type).EnumUnderlyingType; + source.Append($", ({underlyingType.FullQualifiedNameIncludeGlobal()}){paramSymbol.Name}"); + continue; + } + + source.Append($", {paramSymbol.Name}"); + } + source.Append(");\n"); + source.Append(" }\n"); } // Generate RaiseGodotClassSignalCallbacks diff --git a/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs b/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs index e84b4e92c7..788b46ab9a 100644 --- a/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs +++ b/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs @@ -7,6 +7,7 @@ using System.Collections.Generic; using System.Globalization; using System.IO; using System.Linq; +using System.Reflection; using GodotTools.Build; using GodotTools.Ides; using GodotTools.Ides.Rider; @@ -701,6 +702,23 @@ namespace GodotTools private static IntPtr InternalCreateInstance(IntPtr unmanagedCallbacks, int unmanagedCallbacksSize) { Internal.Initialize(unmanagedCallbacks, unmanagedCallbacksSize); + + var populateConstructorMethod = + AppDomain.CurrentDomain + .GetAssemblies() + .First(x => x.GetName().Name == "GodotSharpEditor") + .GetType("Godot.EditorConstructors")? + .GetMethod("AddEditorConstructors", + BindingFlags.Static | BindingFlags.NonPublic | BindingFlags.Public); + + if (populateConstructorMethod == null) + { + throw new MissingMethodException("Godot.EditorConstructors", + "AddEditorConstructors"); + } + + populateConstructorMethod.Invoke(null, null); + return new GodotSharpEditor().NativeInstance; } } diff --git a/modules/mono/editor/bindings_generator.cpp b/modules/mono/editor/bindings_generator.cpp index d0adf39fb2..89655e0b56 100644 --- a/modules/mono/editor/bindings_generator.cpp +++ b/modules/mono/editor/bindings_generator.cpp @@ -77,6 +77,10 @@ StringBuilder &operator<<(StringBuilder &r_sb, const char *p_cstring) { #define BINDINGS_GLOBAL_SCOPE_CLASS "GD" #define BINDINGS_NATIVE_NAME_FIELD "NativeName" +#define BINDINGS_CLASS_CONSTRUCTOR "Constructors" +#define BINDINGS_CLASS_CONSTRUCTOR_EDITOR "EditorConstructors" +#define BINDINGS_CLASS_CONSTRUCTOR_DICTIONARY "BuiltInMethodConstructors" + #define CS_PARAM_MEMORYOWN "memoryOwn" #define CS_PARAM_METHODBIND "method" #define CS_PARAM_INSTANCE "ptr" @@ -1737,6 +1741,69 @@ Error BindingsGenerator::generate_cs_core_project(const String &p_proj_dir) { compile_items.push_back(output_file); } + // Generate source file for built-in type constructor dictionary. + + { + StringBuilder cs_built_in_ctors_content; + + cs_built_in_ctors_content.append("namespace " BINDINGS_NAMESPACE ";\n\n"); + cs_built_in_ctors_content.append("using System;\n" + "using System.Collections.Generic;\n" + "\n"); + cs_built_in_ctors_content.append("internal static class " BINDINGS_CLASS_CONSTRUCTOR "\n{"); + + cs_built_in_ctors_content.append(MEMBER_BEGIN "internal static readonly Dictionary<string, Func<IntPtr, GodotObject>> " BINDINGS_CLASS_CONSTRUCTOR_DICTIONARY ";\n"); + + cs_built_in_ctors_content.append(MEMBER_BEGIN "public static GodotObject Invoke(string nativeTypeNameStr, IntPtr nativeObjectPtr)\n"); + cs_built_in_ctors_content.append(INDENT1 OPEN_BLOCK); + cs_built_in_ctors_content.append(INDENT2 "if (!" BINDINGS_CLASS_CONSTRUCTOR_DICTIONARY ".TryGetValue(nativeTypeNameStr, out var constructor))\n"); + cs_built_in_ctors_content.append(INDENT3 "throw new InvalidOperationException(\"Wrapper class not found for type: \" + nativeTypeNameStr);\n"); + cs_built_in_ctors_content.append(INDENT2 "return constructor(nativeObjectPtr);\n"); + cs_built_in_ctors_content.append(INDENT1 CLOSE_BLOCK); + + cs_built_in_ctors_content.append(MEMBER_BEGIN "static " BINDINGS_CLASS_CONSTRUCTOR "()\n"); + cs_built_in_ctors_content.append(INDENT1 OPEN_BLOCK); + cs_built_in_ctors_content.append(INDENT2 BINDINGS_CLASS_CONSTRUCTOR_DICTIONARY " = new();\n"); + + for (const KeyValue<StringName, TypeInterface> &E : obj_types) { + const TypeInterface &itype = E.value; + + if (itype.api_type != ClassDB::API_CORE || itype.is_singleton_instance) { + continue; + } + + if (itype.is_deprecated) { + cs_built_in_ctors_content.append("#pragma warning disable CS0618\n"); + } + + cs_built_in_ctors_content.append(INDENT2 BINDINGS_CLASS_CONSTRUCTOR_DICTIONARY ".Add(\""); + cs_built_in_ctors_content.append(itype.name); + cs_built_in_ctors_content.append("\", " CS_PARAM_INSTANCE " => new "); + cs_built_in_ctors_content.append(itype.proxy_name); + if (itype.is_singleton && !itype.is_compat_singleton) { + cs_built_in_ctors_content.append("Instance"); + } + cs_built_in_ctors_content.append("(" CS_PARAM_INSTANCE "));\n"); + + if (itype.is_deprecated) { + cs_built_in_ctors_content.append("#pragma warning restore CS0618\n"); + } + } + + cs_built_in_ctors_content.append(INDENT1 CLOSE_BLOCK); + + cs_built_in_ctors_content.append(CLOSE_BLOCK); + + String constructors_file = path::join(base_gen_dir, BINDINGS_CLASS_CONSTRUCTOR ".cs"); + Error err = _save_file(constructors_file, cs_built_in_ctors_content); + + if (err != OK) { + return err; + } + + compile_items.push_back(constructors_file); + } + // Generate native calls StringBuilder cs_icalls_content; @@ -1844,6 +1911,57 @@ Error BindingsGenerator::generate_cs_editor_project(const String &p_proj_dir) { compile_items.push_back(output_file); } + // Generate source file for editor type constructor dictionary. + + { + StringBuilder cs_built_in_ctors_content; + + cs_built_in_ctors_content.append("namespace " BINDINGS_NAMESPACE ";\n\n"); + cs_built_in_ctors_content.append("internal static class " BINDINGS_CLASS_CONSTRUCTOR_EDITOR "\n{"); + + cs_built_in_ctors_content.append(MEMBER_BEGIN "private static void AddEditorConstructors()\n"); + cs_built_in_ctors_content.append(INDENT1 OPEN_BLOCK); + cs_built_in_ctors_content.append(INDENT2 "var builtInMethodConstructors = " BINDINGS_CLASS_CONSTRUCTOR "." BINDINGS_CLASS_CONSTRUCTOR_DICTIONARY ";\n"); + + for (const KeyValue<StringName, TypeInterface> &E : obj_types) { + const TypeInterface &itype = E.value; + + if (itype.api_type != ClassDB::API_EDITOR || itype.is_singleton_instance) { + continue; + } + + if (itype.is_deprecated) { + cs_built_in_ctors_content.append("#pragma warning disable CS0618\n"); + } + + cs_built_in_ctors_content.append(INDENT2 "builtInMethodConstructors.Add(\""); + cs_built_in_ctors_content.append(itype.name); + cs_built_in_ctors_content.append("\", " CS_PARAM_INSTANCE " => new "); + cs_built_in_ctors_content.append(itype.proxy_name); + if (itype.is_singleton && !itype.is_compat_singleton) { + cs_built_in_ctors_content.append("Instance"); + } + cs_built_in_ctors_content.append("(" CS_PARAM_INSTANCE "));\n"); + + if (itype.is_deprecated) { + cs_built_in_ctors_content.append("#pragma warning restore CS0618\n"); + } + } + + cs_built_in_ctors_content.append(INDENT1 CLOSE_BLOCK); + + cs_built_in_ctors_content.append(CLOSE_BLOCK); + + String constructors_file = path::join(base_gen_dir, BINDINGS_CLASS_CONSTRUCTOR_EDITOR ".cs"); + Error err = _save_file(constructors_file, cs_built_in_ctors_content); + + if (err != OK) { + return err; + } + + compile_items.push_back(constructors_file); + } + // Generate native calls StringBuilder cs_icalls_content; @@ -2210,6 +2328,15 @@ Error BindingsGenerator::_generate_cs_type(const TypeInterface &itype, const Str << CLOSE_BLOCK_L2 CLOSE_BLOCK_L1; } + output << MEMBER_BEGIN "internal " << itype.proxy_name << "(IntPtr " CS_PARAM_INSTANCE ") : this(" + << (itype.memory_own ? "true" : "false") << ")\n" OPEN_BLOCK_L1 + << INDENT2 "NativePtr = " CS_PARAM_INSTANCE ";\n" + << INDENT2 "unsafe\n" INDENT2 OPEN_BLOCK + << INDENT3 "ConstructAndInitialize(null, " + << BINDINGS_NATIVE_NAME_FIELD ", CachedType, refCounted: " + << (itype.is_ref_counted ? "true" : "false") << ");\n" + << CLOSE_BLOCK_L2 CLOSE_BLOCK_L1; + // Add.. em.. trick constructor. Sort of. output.append(MEMBER_BEGIN "internal "); output.append(itype.proxy_name); @@ -2934,11 +3061,6 @@ Error BindingsGenerator::_generate_cs_method(const BindingsGenerator::TypeInterf Error BindingsGenerator::_generate_cs_signal(const BindingsGenerator::TypeInterface &p_itype, const BindingsGenerator::SignalInterface &p_isignal, StringBuilder &p_output) { String arguments_sig; - String delegate_type_params; - - if (!p_isignal.arguments.is_empty()) { - delegate_type_params += "<"; - } // Retrieve information from the arguments const ArgumentInterface &first = p_isignal.arguments.front()->get(); @@ -2959,18 +3081,13 @@ Error BindingsGenerator::_generate_cs_signal(const BindingsGenerator::TypeInterf if (&iarg != &first) { arguments_sig += ", "; - delegate_type_params += ", "; } - arguments_sig += arg_type->cs_type; + String arg_cs_type = arg_type->cs_type + _get_generic_type_parameters(*arg_type, iarg.type.generic_type_parameters); + + arguments_sig += arg_cs_type; arguments_sig += " "; arguments_sig += iarg.name; - - delegate_type_params += arg_type->cs_type; - } - - if (!p_isignal.arguments.is_empty()) { - delegate_type_params += ">"; } // Generate signal @@ -3019,8 +3136,14 @@ Error BindingsGenerator::_generate_cs_signal(const BindingsGenerator::TypeInterf p_output << ", "; } - p_output << sformat(arg_type->cs_variant_to_managed, - "args[" + itos(idx) + "]", arg_type->cs_type, arg_type->name); + if (arg_type->cname == name_cache.type_Array_generic || arg_type->cname == name_cache.type_Dictionary_generic) { + String arg_cs_type = arg_type->cs_type + _get_generic_type_parameters(*arg_type, iarg.type.generic_type_parameters); + + p_output << "new " << arg_cs_type << "(" << sformat(arg_type->cs_variant_to_managed, "args[" + itos(idx) + "]", arg_type->cs_type, arg_type->name) << ")"; + } else { + p_output << sformat(arg_type->cs_variant_to_managed, + "args[" + itos(idx) + "]", arg_type->cs_type, arg_type->name); + } idx++; } @@ -3104,6 +3227,46 @@ Error BindingsGenerator::_generate_cs_signal(const BindingsGenerator::TypeInterf } p_output.append(CLOSE_BLOCK_L1); + + // Generate On{EventName} method to raise the event. + if (!p_itype.is_singleton) { + p_output.append(MEMBER_BEGIN "protected void "); + p_output << "On" << p_isignal.proxy_name; + if (is_parameterless) { + p_output.append("()\n" OPEN_BLOCK_L1 INDENT2); + p_output << "EmitSignal(SignalName." << p_isignal.proxy_name << ");\n"; + p_output.append(CLOSE_BLOCK_L1); + } else { + p_output.append("("); + + StringBuilder cs_emitsignal_params; + + int idx = 0; + for (const ArgumentInterface &iarg : p_isignal.arguments) { + const TypeInterface *arg_type = _get_type_or_null(iarg.type); + ERR_FAIL_NULL_V_MSG(arg_type, ERR_BUG, "Argument type '" + iarg.type.cname + "' was not found."); + + if (idx != 0) { + p_output << ", "; + cs_emitsignal_params << ", "; + } + + p_output << arg_type->cs_type << " " << iarg.name; + + if (arg_type->is_enum) { + cs_emitsignal_params << "(long)"; + } + + cs_emitsignal_params << iarg.name; + + idx++; + } + + p_output.append(")\n" OPEN_BLOCK_L1 INDENT2); + p_output << "EmitSignal(SignalName." << p_isignal.proxy_name << ", " << cs_emitsignal_params << ");\n"; + p_output.append(CLOSE_BLOCK_L1); + } + } } return OK; diff --git a/modules/mono/editor/script_templates/SCsub b/modules/mono/editor/script_templates/SCsub index 01c293c25d..f465374758 100644 --- a/modules/mono/editor/script_templates/SCsub +++ b/modules/mono/editor/script_templates/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Bridge/ScriptManagerBridge.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Bridge/ScriptManagerBridge.cs index 901700067d..91d49854c7 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Bridge/ScriptManagerBridge.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Bridge/ScriptManagerBridge.cs @@ -93,27 +93,15 @@ namespace Godot.Bridge internal static unsafe IntPtr CreateManagedForGodotObjectBinding(godot_string_name* nativeTypeName, IntPtr godotObject) { - // TODO: Optimize with source generators and delegate pointers. - try { using var stringName = StringName.CreateTakingOwnershipOfDisposableValue( NativeFuncs.godotsharp_string_name_new_copy(CustomUnsafe.AsRef(nativeTypeName))); string nativeTypeNameStr = stringName.ToString(); - Type nativeType = TypeGetProxyClass(nativeTypeNameStr) ?? throw new InvalidOperationException( - "Wrapper class not found for type: " + nativeTypeNameStr); - var obj = (GodotObject)FormatterServices.GetUninitializedObject(nativeType); + var instance = Constructors.Invoke(nativeTypeNameStr, godotObject); - var ctor = nativeType.GetConstructor( - BindingFlags.Public | BindingFlags.NonPublic | BindingFlags.Instance, - null, Type.EmptyTypes, null); - - obj.NativePtr = godotObject; - - _ = ctor!.Invoke(obj, null); - - return GCHandle.ToIntPtr(CustomGCHandle.AllocStrong(obj)); + return GCHandle.ToIntPtr(CustomGCHandle.AllocStrong(instance)); } catch (Exception e) { @@ -308,66 +296,6 @@ namespace Godot.Bridge } } - private static Type? TypeGetProxyClass(string nativeTypeNameStr) - { - // Performance is not critical here as this will be replaced with a generated dictionary. - - if (nativeTypeNameStr[0] == '_') - nativeTypeNameStr = nativeTypeNameStr.Substring(1); - - Type? wrapperType = typeof(GodotObject).Assembly.GetType("Godot." + nativeTypeNameStr); - - if (wrapperType == null) - { - wrapperType = GetTypeByGodotClassAttr(typeof(GodotObject).Assembly, nativeTypeNameStr); - } - - if (wrapperType == null) - { - var editorAssembly = AppDomain.CurrentDomain.GetAssemblies() - .FirstOrDefault(a => a.GetName().Name == "GodotSharpEditor"); - - if (editorAssembly != null) - { - wrapperType = editorAssembly.GetType("Godot." + nativeTypeNameStr); - - if (wrapperType == null) - { - wrapperType = GetTypeByGodotClassAttr(editorAssembly, nativeTypeNameStr); - } - } - } - - static Type? GetTypeByGodotClassAttr(Assembly assembly, string nativeTypeNameStr) - { - var types = assembly.GetTypes(); - foreach (var type in types) - { - var attr = type.GetCustomAttribute<GodotClassNameAttribute>(); - if (attr?.Name == nativeTypeNameStr) - { - return type; - } - } - return null; - } - - static bool IsStatic(Type type) => type.IsAbstract && type.IsSealed; - - if (wrapperType != null && IsStatic(wrapperType)) - { - // A static class means this is a Godot singleton class. Try to get the Instance proxy type. - wrapperType = TypeGetProxyClass($"{wrapperType.Name}Instance"); - if (wrapperType == null) - { - // Otherwise, fallback to GodotObject. - return typeof(GodotObject); - } - } - - return wrapperType; - } - // Called from GodotPlugins // ReSharper disable once UnusedMember.Local public static void LookupScriptsInAssembly(Assembly assembly) @@ -732,15 +660,7 @@ namespace Godot.Bridge { Type native = GodotObject.InternalGetClassNativeBase(scriptType); - string typeName = scriptType.Name; - if (scriptType.IsGenericType) - { - var sb = new StringBuilder(); - AppendTypeName(sb, scriptType); - typeName = sb.ToString(); - } - - godot_string className = Marshaling.ConvertStringToNative(typeName); + godot_string className = Marshaling.ConvertStringToNative(ReflectionUtils.ConstructTypeName(scriptType)); bool isTool = scriptType.IsDefined(typeof(ToolAttribute), inherit: false); @@ -773,24 +693,6 @@ namespace Godot.Bridge outTypeInfo->IsGenericTypeDefinition = scriptType.IsGenericTypeDefinition.ToGodotBool(); outTypeInfo->IsConstructedGenericType = scriptType.IsConstructedGenericType.ToGodotBool(); - static void AppendTypeName(StringBuilder sb, Type type) - { - sb.Append(type.Name); - if (type.IsGenericType) - { - sb.Append('<'); - for (int i = 0; i < type.GenericTypeArguments.Length; i++) - { - Type typeArg = type.GenericTypeArguments[i]; - AppendTypeName(sb, typeArg); - if (i != type.GenericTypeArguments.Length - 1) - { - sb.Append(", "); - } - } - sb.Append('>'); - } - } } [UnmanagedCallersOnly] @@ -1104,7 +1006,7 @@ namespace Godot.Bridge interopProperties[i] = interopProperty; } - using godot_string currentClassName = Marshaling.ConvertStringToNative(type.Name); + using godot_string currentClassName = Marshaling.ConvertStringToNative(ReflectionUtils.ConstructTypeName(type)); addPropInfoFunc(scriptPtr, ¤tClassName, interopProperties, length); diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotObject.base.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotObject.base.cs index c094eaed77..a429931399 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotObject.base.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotObject.base.cs @@ -29,6 +29,17 @@ namespace Godot } } + internal GodotObject(IntPtr nativePtr) : this(false) + { + // NativePtr must be non-zero before calling ConstructAndInitialize to avoid invoking the constructor NativeCtor. + // We don't want to invoke the constructor, because we already have a constructed instance in nativePtr. + NativePtr = nativePtr; + unsafe + { + ConstructAndInitialize(NativeCtor, NativeName, _cachedType, refCounted: false); + } + } + internal unsafe void ConstructAndInitialize( delegate* unmanaged<godot_bool, IntPtr> nativeCtor, StringName nativeName, diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/ReflectionUtils.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/ReflectionUtils.cs index ee605f8d8f..27989b5c81 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/ReflectionUtils.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/ReflectionUtils.cs @@ -1,5 +1,8 @@ using System; +using System.Collections.Generic; +using System.Diagnostics; using System.Linq; +using System.Text; #nullable enable @@ -7,10 +10,186 @@ namespace Godot; internal class ReflectionUtils { + private static readonly HashSet<Type>? _tupleTypeSet; + private static readonly Dictionary<Type, string>? _builtinTypeNameDictionary; + private static readonly bool _isEditorHintCached; + + static ReflectionUtils() + { + _isEditorHintCached = Engine.IsEditorHint(); + if (!_isEditorHintCached) + { + return; + } + + _tupleTypeSet = new HashSet<Type> + { + // ValueTuple with only one element should be treated as normal generic type. + //typeof(ValueTuple<>), + typeof(ValueTuple<,>), + typeof(ValueTuple<,,>), + typeof(ValueTuple<,,,>), + typeof(ValueTuple<,,,,>), + typeof(ValueTuple<,,,,,>), + typeof(ValueTuple<,,,,,,>), + typeof(ValueTuple<,,,,,,,>), + }; + + _builtinTypeNameDictionary ??= new Dictionary<Type, string> + { + { typeof(sbyte), "sbyte" }, + { typeof(byte), "byte" }, + { typeof(short), "short" }, + { typeof(ushort), "ushort" }, + { typeof(int), "int" }, + { typeof(uint), "uint" }, + { typeof(long), "long" }, + { typeof(ulong), "ulong" }, + { typeof(nint), "nint" }, + { typeof(nuint), "nuint" }, + { typeof(float), "float" }, + { typeof(double), "double" }, + { typeof(decimal), "decimal" }, + { typeof(bool), "bool" }, + { typeof(char), "char" }, + { typeof(string), "string" }, + { typeof(object), "object" }, + }; + } + public static Type? FindTypeInLoadedAssemblies(string assemblyName, string typeFullName) { return AppDomain.CurrentDomain.GetAssemblies() .FirstOrDefault(a => a.GetName().Name == assemblyName)? .GetType(typeFullName); } + + public static string ConstructTypeName(Type type) + { + if (!_isEditorHintCached) + { + return type.Name; + } + + if (type is { IsArray: false, IsGenericType: false }) + { + return GetSimpleTypeName(type); + } + + var typeNameBuilder = new StringBuilder(); + AppendType(typeNameBuilder, type); + return typeNameBuilder.ToString(); + + static void AppendType(StringBuilder sb, Type type) + { + if (type.IsArray) + { + AppendArray(sb, type); + } + else if (type.IsGenericType) + { + AppendGeneric(sb, type); + } + else + { + sb.Append(GetSimpleTypeName(type)); + } + } + + static void AppendArray(StringBuilder sb, Type type) + { + // Append inner most non-array element. + var elementType = type.GetElementType()!; + while (elementType.IsArray) + { + elementType = elementType.GetElementType()!; + } + + AppendType(sb, elementType); + // Append brackets. + AppendArrayBrackets(sb, type); + + static void AppendArrayBrackets(StringBuilder sb, Type type) + { + while (type != null && type.IsArray) + { + int rank = type.GetArrayRank(); + sb.Append('['); + sb.Append(',', rank - 1); + sb.Append(']'); + type = type.GetElementType(); + } + } + } + + static void AppendGeneric(StringBuilder sb, Type type) + { + var genericArgs = type.GenericTypeArguments; + var genericDefinition = type.GetGenericTypeDefinition(); + + // Nullable<T> + if (genericDefinition == typeof(Nullable<>)) + { + AppendType(sb, genericArgs[0]); + sb.Append('?'); + return; + } + + // ValueTuple + Debug.Assert(_tupleTypeSet != null); + if (_tupleTypeSet.Contains(genericDefinition)) + { + sb.Append('('); + while (true) + { + // We assume that ValueTuple has 1~8 elements. + // And the 8th element (TRest) is always another ValueTuple. + + // This is a hard coded tuple element length check. + if (genericArgs.Length != 8) + { + AppendParamTypes(sb, genericArgs); + break; + } + else + { + AppendParamTypes(sb, genericArgs.AsSpan(0, 7)); + sb.Append(", "); + + // TRest should be a ValueTuple! + var nextTuple = genericArgs[7]; + + genericArgs = nextTuple.GenericTypeArguments; + } + } + sb.Append(')'); + return; + } + + // Normal generic + var typeName = type.Name.AsSpan(); + sb.Append(typeName[..typeName.LastIndexOf('`')]); + sb.Append('<'); + AppendParamTypes(sb, genericArgs); + sb.Append('>'); + + static void AppendParamTypes(StringBuilder sb, ReadOnlySpan<Type> genericArgs) + { + int n = genericArgs.Length - 1; + for (int i = 0; i < n; i += 1) + { + AppendType(sb, genericArgs[i]); + sb.Append(", "); + } + + AppendType(sb, genericArgs[n]); + } + } + + static string GetSimpleTypeName(Type type) + { + Debug.Assert(_builtinTypeNameDictionary != null); + return _builtinTypeNameDictionary.TryGetValue(type, out string? name) ? name : type.Name; + } + } } diff --git a/modules/mono/mono_gd/gd_mono.cpp b/modules/mono/mono_gd/gd_mono.cpp index 6abf0193cf..c81aa91fa5 100644 --- a/modules/mono/mono_gd/gd_mono.cpp +++ b/modules/mono/mono_gd/gd_mono.cpp @@ -388,7 +388,7 @@ godot_plugins_initialize_fn initialize_hostfxr_and_godot_plugins(bool &r_runtime String assembly_name = path::get_csharp_project_name(); HostFxrCharString assembly_path = str_to_hostfxr(GodotSharpDirs::get_api_assemblies_dir() - .path_join(assembly_name + ".dll")); + .path_join(assembly_name + ".dll")); load_assembly_and_get_function_pointer_fn load_assembly_and_get_function_pointer = initialize_hostfxr_self_contained(get_data(assembly_path)); diff --git a/modules/msdfgen/SCsub b/modules/msdfgen/SCsub index f4316a74e7..844b0980ac 100644 --- a/modules/msdfgen/SCsub +++ b/modules/msdfgen/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/multiplayer/SCsub b/modules/multiplayer/SCsub index e89038c3e0..f9f4e579e8 100644 --- a/modules/multiplayer/SCsub +++ b/modules/multiplayer/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") @@ -12,3 +13,10 @@ if env.editor_build: env_mp.add_source_files(module_obj, "editor/*.cpp") env.modules_sources += module_obj + +if env["tests"]: + env_mp.Append(CPPDEFINES=["TESTS_ENABLED"]) + env_mp.add_source_files(env.modules_sources, "./tests/*.cpp") + + if env["disable_exceptions"]: + env_mp.Append(CPPDEFINES=["DOCTEST_CONFIG_NO_EXCEPTIONS_BUT_WITH_ALL_ASSERTS"]) diff --git a/modules/multiplayer/doc_classes/MultiplayerSpawner.xml b/modules/multiplayer/doc_classes/MultiplayerSpawner.xml index b620292519..198e2378c9 100644 --- a/modules/multiplayer/doc_classes/MultiplayerSpawner.xml +++ b/modules/multiplayer/doc_classes/MultiplayerSpawner.xml @@ -63,13 +63,13 @@ <signal name="despawned"> <param index="0" name="node" type="Node" /> <description> - Emitted when a spawnable scene or custom spawn was despawned by the multiplayer authority. Only called on puppets. + Emitted when a spawnable scene or custom spawn was despawned by the multiplayer authority. Only called on remote peers. </description> </signal> <signal name="spawned"> <param index="0" name="node" type="Node" /> <description> - Emitted when a spawnable scene or custom spawn was spawned by the multiplayer authority. Only called on puppets. + Emitted when a spawnable scene or custom spawn was spawned by the multiplayer authority. Only called on remote peers. </description> </signal> </signals> diff --git a/modules/multiplayer/doc_classes/SceneMultiplayer.xml b/modules/multiplayer/doc_classes/SceneMultiplayer.xml index 42f32d4848..3277f1ff3e 100644 --- a/modules/multiplayer/doc_classes/SceneMultiplayer.xml +++ b/modules/multiplayer/doc_classes/SceneMultiplayer.xml @@ -65,7 +65,7 @@ [b]Warning:[/b] Deserialized objects can contain code which gets executed. Do not use this option if the serialized object comes from untrusted sources to avoid potential security threat such as remote code execution. </member> <member name="auth_callback" type="Callable" setter="set_auth_callback" getter="get_auth_callback" default="Callable()"> - The callback to execute when when receiving authentication data sent via [method send_auth]. If the [Callable] is empty (default), peers will be automatically accepted as soon as they connect. + The callback to execute when receiving authentication data sent via [method send_auth]. If the [Callable] is empty (default), peers will be automatically accepted as soon as they connect. </member> <member name="auth_timeout" type="float" setter="set_auth_timeout" getter="get_auth_timeout" default="3.0"> If set to a value greater than [code]0.0[/code], the maximum duration in seconds peers can stay in the authenticating state, after which the authentication will automatically fail. See the [signal peer_authenticating] and [signal peer_authentication_failed] signals. diff --git a/modules/multiplayer/editor/editor_network_profiler.cpp b/modules/multiplayer/editor/editor_network_profiler.cpp index 3a51712c70..f5f20d6931 100644 --- a/modules/multiplayer/editor/editor_network_profiler.cpp +++ b/modules/multiplayer/editor/editor_network_profiler.cpp @@ -193,6 +193,9 @@ void EditorNetworkProfiler::_update_button_text() { } void EditorNetworkProfiler::started() { + _clear_pressed(); + activate->set_disabled(false); + if (EditorSettings::get_singleton()->get_project_metadata("debug_options", "autostart_network_profiler", false)) { set_profiling(true); refresh_timer->start(); @@ -200,6 +203,7 @@ void EditorNetworkProfiler::started() { } void EditorNetworkProfiler::stopped() { + activate->set_disabled(true); set_profiling(false); refresh_timer->stop(); } @@ -218,6 +222,7 @@ void EditorNetworkProfiler::_clear_pressed() { set_bandwidth(0, 0); refresh_rpc_data(); refresh_replication_data(); + clear_button->set_disabled(true); } void EditorNetworkProfiler::_autostart_toggled(bool p_toggled_on) { @@ -235,6 +240,9 @@ void EditorNetworkProfiler::_replication_button_clicked(TreeItem *p_item, int p_ } void EditorNetworkProfiler::add_rpc_frame_data(const RPCNodeInfo &p_frame) { + if (clear_button->is_disabled()) { + clear_button->set_disabled(false); + } dirty = true; if (!rpc_data.has(p_frame.node)) { rpc_data.insert(p_frame.node, p_frame); @@ -251,6 +259,9 @@ void EditorNetworkProfiler::add_rpc_frame_data(const RPCNodeInfo &p_frame) { } void EditorNetworkProfiler::add_sync_frame_data(const SyncInfo &p_frame) { + if (clear_button->is_disabled()) { + clear_button->set_disabled(false); + } dirty = true; if (!sync_data.has(p_frame.synchronizer)) { sync_data[p_frame.synchronizer] = p_frame; @@ -292,11 +303,13 @@ EditorNetworkProfiler::EditorNetworkProfiler() { activate = memnew(Button); activate->set_toggle_mode(true); activate->set_text(TTR("Start")); + activate->set_disabled(true); activate->connect(SceneStringName(pressed), callable_mp(this, &EditorNetworkProfiler::_activate_pressed)); hb->add_child(activate); clear_button = memnew(Button); clear_button->set_text(TTR("Clear")); + clear_button->set_disabled(true); clear_button->connect(SceneStringName(pressed), callable_mp(this, &EditorNetworkProfiler::_clear_pressed)); hb->add_child(clear_button); diff --git a/modules/multiplayer/tests/test_scene_multiplayer.h b/modules/multiplayer/tests/test_scene_multiplayer.h new file mode 100644 index 0000000000..5e526c9be6 --- /dev/null +++ b/modules/multiplayer/tests/test_scene_multiplayer.h @@ -0,0 +1,284 @@ +/**************************************************************************/ +/* test_scene_multiplayer.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* 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 TEST_SCENE_MULTIPLAYER_H +#define TEST_SCENE_MULTIPLAYER_H + +#include "tests/test_macros.h" +#include "tests/test_utils.h" + +#include "../scene_multiplayer.h" + +namespace TestSceneMultiplayer { + +static inline Array build_array() { + return Array(); +} +template <typename... Targs> +static inline Array build_array(Variant item, Targs... Fargs) { + Array a = build_array(Fargs...); + a.push_front(item); + return a; +} + +TEST_CASE("[Multiplayer][SceneMultiplayer] Defaults") { + Ref<SceneMultiplayer> scene_multiplayer; + scene_multiplayer.instantiate(); + + REQUIRE(scene_multiplayer->has_multiplayer_peer()); + Ref<MultiplayerPeer> multiplayer_peer = scene_multiplayer->get_multiplayer_peer(); + REQUIRE_MESSAGE(Object::cast_to<OfflineMultiplayerPeer>(multiplayer_peer.ptr()) != nullptr, "By default it must be an OfflineMultiplayerPeer instance."); + CHECK_EQ(scene_multiplayer->poll(), Error::OK); + CHECK_EQ(scene_multiplayer->get_unique_id(), MultiplayerPeer::TARGET_PEER_SERVER); + CHECK_EQ(scene_multiplayer->get_peer_ids(), Vector<int>()); + CHECK_EQ(scene_multiplayer->get_remote_sender_id(), 0); + CHECK_EQ(scene_multiplayer->get_root_path(), NodePath()); + CHECK(scene_multiplayer->get_connected_peers().is_empty()); + CHECK_FALSE(scene_multiplayer->is_refusing_new_connections()); + CHECK_FALSE(scene_multiplayer->is_object_decoding_allowed()); + CHECK(scene_multiplayer->is_server_relay_enabled()); + CHECK_EQ(scene_multiplayer->get_max_sync_packet_size(), 1350); + CHECK_EQ(scene_multiplayer->get_max_delta_packet_size(), 65535); + CHECK(scene_multiplayer->is_server()); +} + +TEST_CASE("[Multiplayer][SceneMultiplayer][SceneTree] SceneTree has a OfflineMultiplayerPeer by default") { + Ref<SceneMultiplayer> scene_multiplayer = SceneTree::get_singleton()->get_multiplayer(); + REQUIRE(scene_multiplayer->has_multiplayer_peer()); + + Ref<MultiplayerPeer> multiplayer_peer = scene_multiplayer->get_multiplayer_peer(); + REQUIRE_MESSAGE(Object::cast_to<OfflineMultiplayerPeer>(multiplayer_peer.ptr()) != nullptr, "By default it must be an OfflineMultiplayerPeer instance."); +} + +TEST_CASE("[Multiplayer][SceneMultiplayer][SceneTree] Object configuration add/remove") { + Ref<SceneMultiplayer> scene_multiplayer; + scene_multiplayer.instantiate(); + + SUBCASE("Returns invalid parameter") { + CHECK_EQ(scene_multiplayer->object_configuration_add(nullptr, "ImInvalid"), Error::ERR_INVALID_PARAMETER); + CHECK_EQ(scene_multiplayer->object_configuration_remove(nullptr, "ImInvalid"), Error::ERR_INVALID_PARAMETER); + + NodePath foo_path("/Foo"); + NodePath bar_path("/Bar"); + CHECK_EQ(scene_multiplayer->object_configuration_add(nullptr, foo_path), Error::OK); + ERR_PRINT_OFF; + CHECK_EQ(scene_multiplayer->object_configuration_remove(nullptr, bar_path), Error::ERR_INVALID_PARAMETER); + ERR_PRINT_ON; + } + + SUBCASE("Sets root path") { + NodePath foo_path("/Foo"); + CHECK_EQ(scene_multiplayer->object_configuration_add(nullptr, foo_path), Error::OK); + + CHECK_EQ(scene_multiplayer->get_root_path(), foo_path); + } + + SUBCASE("Unsets root path") { + NodePath foo_path("/Foo"); + CHECK_EQ(scene_multiplayer->object_configuration_add(nullptr, foo_path), Error::OK); + + CHECK_EQ(scene_multiplayer->object_configuration_remove(nullptr, foo_path), Error::OK); + CHECK_EQ(scene_multiplayer->get_root_path(), NodePath()); + } + + SUBCASE("Add/Remove a MultiplayerSpawner") { + Node2D *node = memnew(Node2D); + MultiplayerSpawner *spawner = memnew(MultiplayerSpawner); + + CHECK_EQ(scene_multiplayer->object_configuration_add(node, spawner), Error::OK); + CHECK_EQ(scene_multiplayer->object_configuration_remove(node, spawner), Error::OK); + + memdelete(spawner); + memdelete(node); + } + + SUBCASE("Add/Remove a MultiplayerSynchronizer") { + Node2D *node = memnew(Node2D); + MultiplayerSynchronizer *synchronizer = memnew(MultiplayerSynchronizer); + + CHECK_EQ(scene_multiplayer->object_configuration_add(node, synchronizer), Error::OK); + CHECK_EQ(scene_multiplayer->object_configuration_remove(node, synchronizer), Error::OK); + + memdelete(synchronizer); + memdelete(node); + } +} + +TEST_CASE("[Multiplayer][SceneMultiplayer] Root Path") { + Ref<SceneMultiplayer> scene_multiplayer; + scene_multiplayer.instantiate(); + + SUBCASE("Is set") { + NodePath foo_path("/Foo"); + scene_multiplayer->set_root_path(foo_path); + + CHECK_EQ(scene_multiplayer->get_root_path(), foo_path); + } + + SUBCASE("Fails when path is empty") { + ERR_PRINT_OFF; + scene_multiplayer->set_root_path(NodePath()); + ERR_PRINT_ON; + } + + SUBCASE("Fails when path is relative") { + NodePath foo_path("Foo"); + ERR_PRINT_OFF; + scene_multiplayer->set_root_path(foo_path); + ERR_PRINT_ON; + + CHECK_EQ(scene_multiplayer->get_root_path(), NodePath()); + } +} + +// This one could be a dummy callback because the current set of test is not actually testing the full auth flow. +static Variant auth_callback(Variant sv, Variant pvav) { + return Variant(); +} + +TEST_CASE("[Multiplayer][SceneMultiplayer][SceneTree] Send Authentication") { + Ref<SceneMultiplayer> scene_multiplayer; + scene_multiplayer.instantiate(); + SceneTree::get_singleton()->set_multiplayer(scene_multiplayer); + scene_multiplayer->set_auth_callback(callable_mp_static(auth_callback)); + + SUBCASE("Is properly sent") { + SIGNAL_WATCH(scene_multiplayer.ptr(), "peer_authenticating"); + + // Adding a peer to MultiplayerPeer. + Ref<MultiplayerPeer> multiplayer_peer = scene_multiplayer->get_multiplayer_peer(); + int peer_id = 42; + multiplayer_peer->emit_signal(SNAME("peer_connected"), peer_id); + SIGNAL_CHECK("peer_authenticating", build_array(build_array(peer_id))); + + CHECK_EQ(scene_multiplayer->send_auth(peer_id, String("It's me").to_ascii_buffer()), Error::OK); + + Vector<int> expected_peer_ids = { peer_id }; + CHECK_EQ(scene_multiplayer->get_authenticating_peer_ids(), expected_peer_ids); + + SIGNAL_UNWATCH(scene_multiplayer.ptr(), "peer_authenticating"); + } + + SUBCASE("peer_authentication_failed is emitted when a peer is deleted before authentication is completed") { + SIGNAL_WATCH(scene_multiplayer.ptr(), "peer_authentication_failed"); + + // Adding a peer to MultiplayerPeer. + Ref<MultiplayerPeer> multiplayer_peer = scene_multiplayer->get_multiplayer_peer(); + int peer_id = 42; + multiplayer_peer->emit_signal(SNAME("peer_connected"), peer_id); + multiplayer_peer->emit_signal(SNAME("peer_disconnected"), peer_id); + SIGNAL_CHECK("peer_authentication_failed", build_array(build_array(peer_id))); + + SIGNAL_UNWATCH(scene_multiplayer.ptr(), "peer_authentication_failed"); + } + + SUBCASE("peer_authentication_failed is emitted when authentication timeout") { + SIGNAL_WATCH(scene_multiplayer.ptr(), "peer_authentication_failed"); + scene_multiplayer->set_auth_timeout(0.01); + CHECK_EQ(scene_multiplayer->get_auth_timeout(), 0.01); + + // Adding two peesr to MultiplayerPeer. + Ref<MultiplayerPeer> multiplayer_peer = scene_multiplayer->get_multiplayer_peer(); + int first_peer_id = 42; + int second_peer_id = 84; + multiplayer_peer->emit_signal(SNAME("peer_connected"), first_peer_id); + multiplayer_peer->emit_signal(SNAME("peer_connected"), second_peer_id); + + // Let timeout happens. + OS::get_singleton()->delay_usec(500000); + + CHECK_EQ(scene_multiplayer->poll(), Error::OK); + + SIGNAL_CHECK("peer_authentication_failed", build_array(build_array(first_peer_id), build_array(second_peer_id))); + + SIGNAL_UNWATCH(scene_multiplayer.ptr(), "peer_authentication_failed"); + } + + SUBCASE("Fails when there is no MultiplayerPeer configured") { + scene_multiplayer->set_multiplayer_peer(nullptr); + + ERR_PRINT_OFF; + CHECK_EQ(scene_multiplayer->send_auth(42, Vector<uint8_t>()), Error::ERR_UNCONFIGURED); + ERR_PRINT_ON; + } + + SUBCASE("Fails when the peer to send the auth is not pending") { + ERR_PRINT_OFF; + CHECK_EQ(scene_multiplayer->send_auth(42, String("It's me").to_ascii_buffer()), Error::ERR_INVALID_PARAMETER); + ERR_PRINT_ON; + } +} + +TEST_CASE("[Multiplayer][SceneMultiplayer][SceneTree] Complete Authentication") { + Ref<SceneMultiplayer> scene_multiplayer; + scene_multiplayer.instantiate(); + SceneTree::get_singleton()->set_multiplayer(scene_multiplayer); + scene_multiplayer->set_auth_callback(callable_mp_static(auth_callback)); + + SUBCASE("Is properly completed") { + Ref<MultiplayerPeer> multiplayer_peer = scene_multiplayer->get_multiplayer_peer(); + int peer_id = 42; + multiplayer_peer->emit_signal(SNAME("peer_connected"), peer_id); + CHECK_EQ(scene_multiplayer->send_auth(peer_id, String("It's me").to_ascii_buffer()), Error::OK); + + CHECK_EQ(scene_multiplayer->complete_auth(peer_id), Error::OK); + } + + SUBCASE("Fails when there is no MultiplayerPeer configured") { + scene_multiplayer->set_multiplayer_peer(nullptr); + + ERR_PRINT_OFF; + CHECK_EQ(scene_multiplayer->complete_auth(42), Error::ERR_UNCONFIGURED); + ERR_PRINT_ON; + } + + SUBCASE("Fails when the peer to complete the auth is not pending") { + ERR_PRINT_OFF; + CHECK_EQ(scene_multiplayer->complete_auth(42), Error::ERR_INVALID_PARAMETER); + ERR_PRINT_ON; + } + + SUBCASE("Fails to send auth or completed for a second time") { + Ref<MultiplayerPeer> multiplayer_peer = scene_multiplayer->get_multiplayer_peer(); + int peer_id = 42; + multiplayer_peer->emit_signal(SNAME("peer_connected"), peer_id); + CHECK_EQ(scene_multiplayer->send_auth(peer_id, String("It's me").to_ascii_buffer()), Error::OK); + CHECK_EQ(scene_multiplayer->complete_auth(peer_id), Error::OK); + + ERR_PRINT_OFF; + CHECK_EQ(scene_multiplayer->send_auth(peer_id, String("It's me").to_ascii_buffer()), Error::ERR_FILE_CANT_WRITE); + CHECK_EQ(scene_multiplayer->complete_auth(peer_id), Error::ERR_FILE_CANT_WRITE); + ERR_PRINT_ON; + } +} + +} // namespace TestSceneMultiplayer + +#endif // TEST_SCENE_MULTIPLAYER_H diff --git a/modules/navigation/SCsub b/modules/navigation/SCsub index 02d3b7487e..ab578252c1 100644 --- a/modules/navigation/SCsub +++ b/modules/navigation/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/noise/SCsub b/modules/noise/SCsub index f309fd2dd4..dcf51b03e3 100644 --- a/modules/noise/SCsub +++ b/modules/noise/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/ogg/SCsub b/modules/ogg/SCsub index f15525648f..fabd4f936a 100644 --- a/modules/ogg/SCsub +++ b/modules/ogg/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/openxr/SCsub b/modules/openxr/SCsub index 77922045eb..dd6a921440 100644 --- a/modules/openxr/SCsub +++ b/modules/openxr/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/openxr/action_map/SCsub b/modules/openxr/action_map/SCsub index 7a493011ec..d659be1d99 100644 --- a/modules/openxr/action_map/SCsub +++ b/modules/openxr/action_map/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_openxr") diff --git a/modules/openxr/doc_classes/OpenXRExtensionWrapperExtension.xml b/modules/openxr/doc_classes/OpenXRExtensionWrapperExtension.xml index 338d632524..813c9d582e 100644 --- a/modules/openxr/doc_classes/OpenXRExtensionWrapperExtension.xml +++ b/modules/openxr/doc_classes/OpenXRExtensionWrapperExtension.xml @@ -178,6 +178,15 @@ [param layer] is a pointer to an [code]XrCompositionLayerBaseHeader[/code] struct. </description> </method> + <method name="_set_android_surface_swapchain_create_info_and_get_next_pointer" qualifiers="virtual"> + <return type="int" /> + <param index="0" name="property_values" type="Dictionary" /> + <param index="1" name="next_pointer" type="void*" /> + <description> + Adds additional data structures to Android surface swapchains created by [OpenXRCompositionLayer]. + [param property_values] contains the values of the properties returned by [method _get_viewport_composition_layer_extension_properties]. + </description> + </method> <method name="_set_hand_joint_locations_and_get_next_pointer" qualifiers="virtual"> <return type="int" /> <param index="0" name="hand_index" type="int" /> diff --git a/modules/openxr/editor/SCsub b/modules/openxr/editor/SCsub index ccf67a80d0..39eb469978 100644 --- a/modules/openxr/editor/SCsub +++ b/modules/openxr/editor/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") diff --git a/modules/openxr/editor/openxr_select_action_dialog.cpp b/modules/openxr/editor/openxr_select_action_dialog.cpp index a4ccc98408..89dea09be4 100644 --- a/modules/openxr/editor/openxr_select_action_dialog.cpp +++ b/modules/openxr/editor/openxr_select_action_dialog.cpp @@ -66,7 +66,7 @@ void OpenXRSelectActionDialog::_on_select_action(const String p_action) { void OpenXRSelectActionDialog::open() { ERR_FAIL_COND(action_map.is_null()); - // out with the old... + // Out with the old. while (main_vb->get_child_count() > 0) { memdelete(main_vb->get_child(0)); } @@ -74,6 +74,7 @@ void OpenXRSelectActionDialog::open() { selected_action = ""; action_buttons.clear(); + // In with the new. Array action_sets = action_map->get_action_sets(); for (int i = 0; i < action_sets.size(); i++) { Ref<OpenXRActionSet> action_set = action_sets[i]; diff --git a/modules/openxr/editor/openxr_select_interaction_profile_dialog.cpp b/modules/openxr/editor/openxr_select_interaction_profile_dialog.cpp index 53b8cbd401..ee8940f30b 100644 --- a/modules/openxr/editor/openxr_select_interaction_profile_dialog.cpp +++ b/modules/openxr/editor/openxr_select_interaction_profile_dialog.cpp @@ -66,15 +66,15 @@ void OpenXRSelectInteractionProfileDialog::_on_select_interaction_profile(const void OpenXRSelectInteractionProfileDialog::open(PackedStringArray p_do_not_include) { int available_count = 0; - // out with the old... - while (main_vb->get_child_count() > 0) { - memdelete(main_vb->get_child(0)); + // Out with the old. + while (main_vb->get_child_count() > 1) { + memdelete(main_vb->get_child(1)); } selected_interaction_profile = ""; ip_buttons.clear(); - // in with the new + // In with the new. PackedStringArray interaction_profiles = OpenXRInteractionProfileMetadata::get_singleton()->get_interaction_profile_paths(); for (int i = 0; i < interaction_profiles.size(); i++) { const String &path = interaction_profiles[i]; @@ -82,6 +82,7 @@ void OpenXRSelectInteractionProfileDialog::open(PackedStringArray p_do_not_inclu Button *ip_button = memnew(Button); ip_button->set_flat(true); ip_button->set_text(OpenXRInteractionProfileMetadata::get_singleton()->get_profile(path)->display_name); + ip_button->set_text_alignment(HORIZONTAL_ALIGNMENT_LEFT); ip_button->connect(SceneStringName(pressed), callable_mp(this, &OpenXRSelectInteractionProfileDialog::_on_select_interaction_profile).bind(path)); main_vb->add_child(ip_button); @@ -90,23 +91,16 @@ void OpenXRSelectInteractionProfileDialog::open(PackedStringArray p_do_not_inclu } } - if (available_count == 0) { - // give warning that we have all profiles selected - - } else { - // TODO maybe if we only have one, auto select it? - - popup_centered(); - } + all_selected->set_visible(available_count == 0); + get_cancel_button()->set_visible(available_count > 0); + popup_centered(); } void OpenXRSelectInteractionProfileDialog::ok_pressed() { - if (selected_interaction_profile == "") { - return; + if (selected_interaction_profile != "") { + emit_signal("interaction_profile_selected", selected_interaction_profile); } - emit_signal("interaction_profile_selected", selected_interaction_profile); - hide(); } @@ -118,6 +112,10 @@ OpenXRSelectInteractionProfileDialog::OpenXRSelectInteractionProfileDialog() { add_child(scroll); main_vb = memnew(VBoxContainer); - // main_vb->set_h_size_flags(Control::SIZE_EXPAND_FILL); + main_vb->set_h_size_flags(Control::SIZE_EXPAND_FILL); scroll->add_child(main_vb); + + all_selected = memnew(Label); + all_selected->set_text(TTR("All interaction profiles have been added to the action map.")); + main_vb->add_child(all_selected); } diff --git a/modules/openxr/editor/openxr_select_interaction_profile_dialog.h b/modules/openxr/editor/openxr_select_interaction_profile_dialog.h index 1d1615321c..d85e4cd4d6 100644 --- a/modules/openxr/editor/openxr_select_interaction_profile_dialog.h +++ b/modules/openxr/editor/openxr_select_interaction_profile_dialog.h @@ -51,6 +51,7 @@ private: VBoxContainer *main_vb = nullptr; ScrollContainer *scroll = nullptr; + Label *all_selected = nullptr; protected: static void _bind_methods(); diff --git a/modules/openxr/extensions/SCsub b/modules/openxr/extensions/SCsub index 1bd9cfaa22..95b75ccd65 100644 --- a/modules/openxr/extensions/SCsub +++ b/modules/openxr/extensions/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_openxr") diff --git a/modules/openxr/extensions/openxr_composition_layer_extension.cpp b/modules/openxr/extensions/openxr_composition_layer_extension.cpp index 83e45ffe7f..dc30b95b27 100644 --- a/modules/openxr/extensions/openxr_composition_layer_extension.cpp +++ b/modules/openxr/extensions/openxr_composition_layer_extension.cpp @@ -144,7 +144,6 @@ bool OpenXRCompositionLayerExtension::create_android_surface_swapchain(XrSwapcha OpenXRAPI *openxr_api = OpenXRAPI::get_singleton(); ERR_FAIL_NULL_V(openxr_api, false); - // @todo We need a way to add to the next pointer chain. XrResult result = xrCreateSwapchainAndroidSurfaceKHR(openxr_api->get_session(), p_info, r_swapchain, r_surface); if (XR_FAILED(result)) { print_line("OpenXR: Failed to create Android surface swapchain [", openxr_api->get_error_string(result), "]"); @@ -254,11 +253,19 @@ void OpenXRViewportCompositionLayerProvider::create_android_surface() { ERR_FAIL_COND(android_surface.swapchain != XR_NULL_HANDLE || android_surface.surface.is_valid()); ERR_FAIL_COND(!openxr_api || !openxr_api->is_running()); + void *next_pointer = nullptr; + for (OpenXRExtensionWrapper *wrapper : openxr_api->get_registered_extension_wrappers()) { + void *np = wrapper->set_android_surface_swapchain_create_info_and_get_next_pointer(extension_property_values, next_pointer); + if (np != nullptr) { + next_pointer = np; + } + } + // The XR_FB_android_surface_swapchain_create extension mandates that format, sampleCount, // faceCount, arraySize, and mipCount must be zero. XrSwapchainCreateInfo info = { XR_TYPE_SWAPCHAIN_CREATE_INFO, // type - nullptr, // next + next_pointer, // next 0, // createFlags XR_SWAPCHAIN_USAGE_SAMPLED_BIT | XR_SWAPCHAIN_USAGE_COLOR_ATTACHMENT_BIT | XR_SWAPCHAIN_USAGE_MUTABLE_FORMAT_BIT, // usageFlags 0, // format diff --git a/modules/openxr/extensions/openxr_extension_wrapper.h b/modules/openxr/extensions/openxr_extension_wrapper.h index 09a9556dfa..95b537d1b4 100644 --- a/modules/openxr/extensions/openxr_extension_wrapper.h +++ b/modules/openxr/extensions/openxr_extension_wrapper.h @@ -97,10 +97,11 @@ public: virtual void on_state_loss_pending() {} // `on_state_loss_pending` is called when the OpenXR session state is changed to loss pending. virtual void on_state_exiting() {} // `on_state_exiting` is called when the OpenXR session state is changed to exiting. - virtual void *set_viewport_composition_layer_and_get_next_pointer(const XrCompositionLayerBaseHeader *p_layer, Dictionary p_property_values, void *p_next_pointer) { return p_next_pointer; } // Add additional data structures to composition layers created via OpenXRCompositionLayer. + virtual void *set_viewport_composition_layer_and_get_next_pointer(const XrCompositionLayerBaseHeader *p_layer, const Dictionary &p_property_values, void *p_next_pointer) { return p_next_pointer; } // Add additional data structures to composition layers created via OpenXRCompositionLayer. virtual void on_viewport_composition_layer_destroyed(const XrCompositionLayerBaseHeader *p_layer) {} // `on_viewport_composition_layer_destroyed` is called when a composition layer created via OpenXRCompositionLayer is destroyed. virtual void get_viewport_composition_layer_extension_properties(List<PropertyInfo> *p_property_list) {} // Get additional property definitions for OpenXRCompositionLayer. virtual Dictionary get_viewport_composition_layer_extension_property_defaults() { return Dictionary(); } // Get the default values for the additional property definitions for OpenXRCompositionLayer. + virtual void *set_android_surface_swapchain_create_info_and_get_next_pointer(const Dictionary &p_property_values, void *p_next_pointer) { return p_next_pointer; } // `on_event_polled` is called when there is an OpenXR event to process. // Should return true if the event was handled, false otherwise. diff --git a/modules/openxr/extensions/openxr_extension_wrapper_extension.cpp b/modules/openxr/extensions/openxr_extension_wrapper_extension.cpp index e09ca484d5..07ca476421 100644 --- a/modules/openxr/extensions/openxr_extension_wrapper_extension.cpp +++ b/modules/openxr/extensions/openxr_extension_wrapper_extension.cpp @@ -65,6 +65,7 @@ void OpenXRExtensionWrapperExtension::_bind_methods() { GDVIRTUAL_BIND(_get_viewport_composition_layer_extension_properties); GDVIRTUAL_BIND(_get_viewport_composition_layer_extension_property_defaults); GDVIRTUAL_BIND(_on_viewport_composition_layer_destroyed, "layer"); + GDVIRTUAL_BIND(_set_android_surface_swapchain_create_info_and_get_next_pointer, "property_values", "next_pointer"); ClassDB::bind_method(D_METHOD("get_openxr_api"), &OpenXRExtensionWrapperExtension::get_openxr_api); ClassDB::bind_method(D_METHOD("register_extension_wrapper"), &OpenXRExtensionWrapperExtension::register_extension_wrapper); @@ -249,7 +250,7 @@ bool OpenXRExtensionWrapperExtension::on_event_polled(const XrEventDataBuffer &p return false; } -void *OpenXRExtensionWrapperExtension::set_viewport_composition_layer_and_get_next_pointer(const XrCompositionLayerBaseHeader *p_layer, Dictionary p_property_values, void *p_next_pointer) { +void *OpenXRExtensionWrapperExtension::set_viewport_composition_layer_and_get_next_pointer(const XrCompositionLayerBaseHeader *p_layer, const Dictionary &p_property_values, void *p_next_pointer) { uint64_t pointer = 0; if (GDVIRTUAL_CALL(_set_viewport_composition_layer_and_get_next_pointer, GDExtensionConstPtr<void>(p_layer), p_property_values, GDExtensionPtr<void>(p_next_pointer), pointer)) { @@ -279,6 +280,16 @@ Dictionary OpenXRExtensionWrapperExtension::get_viewport_composition_layer_exten return property_defaults; } +void *OpenXRExtensionWrapperExtension::set_android_surface_swapchain_create_info_and_get_next_pointer(const Dictionary &p_property_values, void *p_next_pointer) { + uint64_t pointer = 0; + + if (GDVIRTUAL_CALL(_set_android_surface_swapchain_create_info_and_get_next_pointer, p_property_values, GDExtensionPtr<void>(p_next_pointer), pointer)) { + return reinterpret_cast<void *>(pointer); + } + + return p_next_pointer; +} + Ref<OpenXRAPIExtension> OpenXRExtensionWrapperExtension::get_openxr_api() { return openxr_api; } diff --git a/modules/openxr/extensions/openxr_extension_wrapper_extension.h b/modules/openxr/extensions/openxr_extension_wrapper_extension.h index e37853903b..5cdf288c93 100644 --- a/modules/openxr/extensions/openxr_extension_wrapper_extension.h +++ b/modules/openxr/extensions/openxr_extension_wrapper_extension.h @@ -121,15 +121,17 @@ public: GDVIRTUAL1R(bool, _on_event_polled, GDExtensionConstPtr<void>); - virtual void *set_viewport_composition_layer_and_get_next_pointer(const XrCompositionLayerBaseHeader *p_layer, Dictionary p_property_values, void *p_next_pointer) override; + virtual void *set_viewport_composition_layer_and_get_next_pointer(const XrCompositionLayerBaseHeader *p_layer, const Dictionary &p_property_values, void *p_next_pointer) override; virtual void on_viewport_composition_layer_destroyed(const XrCompositionLayerBaseHeader *p_layer) override; virtual void get_viewport_composition_layer_extension_properties(List<PropertyInfo> *p_property_list) override; virtual Dictionary get_viewport_composition_layer_extension_property_defaults() override; + virtual void *set_android_surface_swapchain_create_info_and_get_next_pointer(const Dictionary &p_property_values, void *p_next_pointer) override; GDVIRTUAL3R(uint64_t, _set_viewport_composition_layer_and_get_next_pointer, GDExtensionConstPtr<void>, Dictionary, GDExtensionPtr<void>); GDVIRTUAL1(_on_viewport_composition_layer_destroyed, GDExtensionConstPtr<void>); GDVIRTUAL0R(TypedArray<Dictionary>, _get_viewport_composition_layer_extension_properties); GDVIRTUAL0R(Dictionary, _get_viewport_composition_layer_extension_property_defaults); + GDVIRTUAL2R(uint64_t, _set_android_surface_swapchain_create_info_and_get_next_pointer, Dictionary, GDExtensionPtr<void>); Ref<OpenXRAPIExtension> get_openxr_api(); diff --git a/modules/openxr/scene/SCsub b/modules/openxr/scene/SCsub index 7a493011ec..d659be1d99 100644 --- a/modules/openxr/scene/SCsub +++ b/modules/openxr/scene/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_openxr") diff --git a/modules/raycast/SCsub b/modules/raycast/SCsub index f3a8e30763..bbf5ff7983 100644 --- a/modules/raycast/SCsub +++ b/modules/raycast/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/regex/SCsub b/modules/regex/SCsub index f5e2dd5dfc..5d70604e76 100644 --- a/modules/regex/SCsub +++ b/modules/regex/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/squish/SCsub b/modules/squish/SCsub deleted file mode 100644 index c9e29911d8..0000000000 --- a/modules/squish/SCsub +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python - -Import("env") -Import("env_modules") - -env_squish = env_modules.Clone() - -# Thirdparty source files - -thirdparty_obj = [] - -if env["builtin_squish"]: - thirdparty_dir = "#thirdparty/squish/" - thirdparty_sources = [ - "alpha.cpp", - "clusterfit.cpp", - "colourblock.cpp", - "colourfit.cpp", - "colourset.cpp", - "maths.cpp", - "rangefit.cpp", - "singlecolourfit.cpp", - "squish.cpp", - ] - - thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] - - env_squish.Prepend(CPPPATH=[thirdparty_dir]) - - env_thirdparty = env_squish.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_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/svg/SCsub b/modules/svg/SCsub index a32be0e41a..af8f6c14f4 100644 --- a/modules/svg/SCsub +++ b/modules/svg/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/text_server_adv/SCsub b/modules/text_server_adv/SCsub index 4112b81622..304a09515c 100644 --- a/modules/text_server_adv/SCsub +++ b/modules/text_server_adv/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/text_server_adv/gdextension_build/SConstruct b/modules/text_server_adv/gdextension_build/SConstruct index effed1e772..8f4f2cba40 100644 --- a/modules/text_server_adv/gdextension_build/SConstruct +++ b/modules/text_server_adv/gdextension_build/SConstruct @@ -1,4 +1,6 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * + import atexit import sys import time diff --git a/modules/text_server_adv/text_server_adv.cpp b/modules/text_server_adv/text_server_adv.cpp index 3322300dda..1c6e62650a 100644 --- a/modules/text_server_adv/text_server_adv.cpp +++ b/modules/text_server_adv/text_server_adv.cpp @@ -442,6 +442,8 @@ bool TextServerAdvanced::_load_support_data(const String &p_filename) { } #else if (!icu_data_loaded) { + UErrorCode err = U_ZERO_ERROR; +#ifdef ICU_DATA_NAME String filename = (p_filename.is_empty()) ? String("res://") + _MKSTR(ICU_DATA_NAME) : p_filename; Ref<FileAccess> f = FileAccess::open(filename, FileAccess::READ); @@ -451,13 +453,13 @@ bool TextServerAdvanced::_load_support_data(const String &p_filename) { uint64_t len = f->get_length(); icu_data = f->get_buffer(len); - UErrorCode err = U_ZERO_ERROR; udata_setCommonData(icu_data.ptr(), &err); if (U_FAILURE(err)) { ERR_FAIL_V_MSG(false, u_errorName(err)); } err = U_ZERO_ERROR; +#endif u_init(&err); if (U_FAILURE(err)) { ERR_FAIL_V_MSG(false, u_errorName(err)); @@ -1357,7 +1359,7 @@ _FORCE_INLINE_ bool TextServerAdvanced::_ensure_glyph(FontAdvanced *p_font_data, return false; } -_FORCE_INLINE_ bool TextServerAdvanced::_ensure_cache_for_size(FontAdvanced *p_font_data, const Vector2i &p_size, FontForSizeAdvanced *&r_cache_for_size) const { +_FORCE_INLINE_ bool TextServerAdvanced::_ensure_cache_for_size(FontAdvanced *p_font_data, const Vector2i &p_size, FontForSizeAdvanced *&r_cache_for_size, bool p_silent) const { ERR_FAIL_COND_V(p_size.x <= 0, false); HashMap<Vector2i, FontForSizeAdvanced *>::Iterator E = p_font_data->cache.find(p_size); @@ -1378,7 +1380,11 @@ _FORCE_INLINE_ bool TextServerAdvanced::_ensure_cache_for_size(FontAdvanced *p_f error = FT_Init_FreeType(&ft_library); if (error != 0) { memdelete(fd); - ERR_FAIL_V_MSG(false, "FreeType: Error initializing library: '" + String(FT_Error_String(error)) + "'."); + if (p_silent) { + return false; + } else { + ERR_FAIL_V_MSG(false, "FreeType: Error initializing library: '" + String(FT_Error_String(error)) + "'."); + } } #ifdef MODULE_SVG_ENABLED FT_Property_Set(ft_library, "ot-svg", "svg-hooks", get_tvg_svg_in_ot_hooks()); @@ -1412,7 +1418,11 @@ _FORCE_INLINE_ bool TextServerAdvanced::_ensure_cache_for_size(FontAdvanced *p_f FT_Done_Face(fd->face); fd->face = nullptr; memdelete(fd); - ERR_FAIL_V_MSG(false, "FreeType: Error loading font: '" + String(FT_Error_String(error)) + "'."); + if (p_silent) { + return false; + } else { + ERR_FAIL_V_MSG(false, "FreeType: Error loading font: '" + String(FT_Error_String(error)) + "'."); + } } } @@ -1847,7 +1857,11 @@ _FORCE_INLINE_ bool TextServerAdvanced::_ensure_cache_for_size(FontAdvanced *p_f } #else memdelete(fd); - ERR_FAIL_V_MSG(false, "FreeType: Can't load dynamic font, engine is compiled without FreeType support!"); + if (p_silent) { + return false; + } else { + ERR_FAIL_V_MSG(false, "FreeType: Can't load dynamic font, engine is compiled without FreeType support!"); + } #endif } else { // Init bitmap font. @@ -1858,6 +1872,16 @@ _FORCE_INLINE_ bool TextServerAdvanced::_ensure_cache_for_size(FontAdvanced *p_f return true; } +_FORCE_INLINE_ bool TextServerAdvanced::_font_validate(const RID &p_font_rid) const { + FontAdvanced *fd = _get_font_data(p_font_rid); + ERR_FAIL_NULL_V(fd, false); + + MutexLock lock(fd->mutex); + Vector2i size = _get_size(fd, 16); + FontForSizeAdvanced *ffsd = nullptr; + return _ensure_cache_for_size(fd, size, ffsd, true); +} + _FORCE_INLINE_ void TextServerAdvanced::_font_clear_cache(FontAdvanced *p_font_data) { MutexLock ftlock(ft_mutex); @@ -5106,6 +5130,10 @@ RID TextServerAdvanced::_find_sys_font_for_text(const RID &p_fdef, const String SystemFontCacheRec sysf; sysf.rid = _create_font(); _font_set_data_ptr(sysf.rid, font_data.ptr(), font_data.size()); + if (!_font_validate(sysf.rid)) { + _free_rid(sysf.rid); + continue; + } Dictionary var = dvar; // Select matching style from collection. diff --git a/modules/text_server_adv/text_server_adv.h b/modules/text_server_adv/text_server_adv.h index c63389b1c6..9c8d75b358 100644 --- a/modules/text_server_adv/text_server_adv.h +++ b/modules/text_server_adv/text_server_adv.h @@ -365,7 +365,8 @@ class TextServerAdvanced : public TextServerExtension { _FORCE_INLINE_ FontGlyph rasterize_bitmap(FontForSizeAdvanced *p_data, int p_rect_margin, FT_Bitmap p_bitmap, int p_yofs, int p_xofs, const Vector2 &p_advance, bool p_bgra) const; #endif _FORCE_INLINE_ bool _ensure_glyph(FontAdvanced *p_font_data, const Vector2i &p_size, int32_t p_glyph, FontGlyph &r_glyph) const; - _FORCE_INLINE_ bool _ensure_cache_for_size(FontAdvanced *p_font_data, const Vector2i &p_size, FontForSizeAdvanced *&r_cache_for_size) const; + _FORCE_INLINE_ bool _ensure_cache_for_size(FontAdvanced *p_font_data, const Vector2i &p_size, FontForSizeAdvanced *&r_cache_for_size, bool p_silent = false) const; + _FORCE_INLINE_ bool _font_validate(const RID &p_font_rid) const; _FORCE_INLINE_ void _font_clear_cache(FontAdvanced *p_font_data); static void _generateMTSDF_threaded(void *p_td, uint32_t p_y); diff --git a/modules/text_server_fb/SCsub b/modules/text_server_fb/SCsub index fc0a8727c6..b56df192c2 100644 --- a/modules/text_server_fb/SCsub +++ b/modules/text_server_fb/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/text_server_fb/gdextension_build/SConstruct b/modules/text_server_fb/gdextension_build/SConstruct index a3c2052040..dc849d5814 100644 --- a/modules/text_server_fb/gdextension_build/SConstruct +++ b/modules/text_server_fb/gdextension_build/SConstruct @@ -1,4 +1,6 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * + import atexit import sys import time diff --git a/modules/text_server_fb/text_server_fb.cpp b/modules/text_server_fb/text_server_fb.cpp index 540ba19cac..ce95622f09 100644 --- a/modules/text_server_fb/text_server_fb.cpp +++ b/modules/text_server_fb/text_server_fb.cpp @@ -791,7 +791,7 @@ _FORCE_INLINE_ bool TextServerFallback::_ensure_glyph(FontFallback *p_font_data, return false; } -_FORCE_INLINE_ bool TextServerFallback::_ensure_cache_for_size(FontFallback *p_font_data, const Vector2i &p_size, FontForSizeFallback *&r_cache_for_size) const { +_FORCE_INLINE_ bool TextServerFallback::_ensure_cache_for_size(FontFallback *p_font_data, const Vector2i &p_size, FontForSizeFallback *&r_cache_for_size, bool p_silent) const { ERR_FAIL_COND_V(p_size.x <= 0, false); HashMap<Vector2i, FontForSizeFallback *>::Iterator E = p_font_data->cache.find(p_size); @@ -813,7 +813,11 @@ _FORCE_INLINE_ bool TextServerFallback::_ensure_cache_for_size(FontFallback *p_f error = FT_Init_FreeType(&ft_library); if (error != 0) { memdelete(fd); - ERR_FAIL_V_MSG(false, "FreeType: Error initializing library: '" + String(FT_Error_String(error)) + "'."); + if (p_silent) { + return false; + } else { + ERR_FAIL_V_MSG(false, "FreeType: Error initializing library: '" + String(FT_Error_String(error)) + "'."); + } } #ifdef MODULE_SVG_ENABLED FT_Property_Set(ft_library, "ot-svg", "svg-hooks", get_tvg_svg_in_ot_hooks()); @@ -847,7 +851,11 @@ _FORCE_INLINE_ bool TextServerFallback::_ensure_cache_for_size(FontFallback *p_f FT_Done_Face(fd->face); fd->face = nullptr; memdelete(fd); - ERR_FAIL_V_MSG(false, "FreeType: Error loading font: '" + String(FT_Error_String(error)) + "'."); + if (p_silent) { + return false; + } else { + ERR_FAIL_V_MSG(false, "FreeType: Error loading font: '" + String(FT_Error_String(error)) + "'."); + } } } @@ -980,7 +988,11 @@ _FORCE_INLINE_ bool TextServerFallback::_ensure_cache_for_size(FontFallback *p_f } #else memdelete(fd); - ERR_FAIL_V_MSG(false, "FreeType: Can't load dynamic font, engine is compiled without FreeType support!"); + if (p_silent) { + return false; + } else { + ERR_FAIL_V_MSG(false, "FreeType: Can't load dynamic font, engine is compiled without FreeType support!"); + } #endif } @@ -989,6 +1001,16 @@ _FORCE_INLINE_ bool TextServerFallback::_ensure_cache_for_size(FontFallback *p_f return true; } +_FORCE_INLINE_ bool TextServerFallback::_font_validate(const RID &p_font_rid) const { + FontFallback *fd = _get_font_data(p_font_rid); + ERR_FAIL_NULL_V(fd, false); + + MutexLock lock(fd->mutex); + Vector2i size = _get_size(fd, 16); + FontForSizeFallback *ffsd = nullptr; + return _ensure_cache_for_size(fd, size, ffsd, true); +} + _FORCE_INLINE_ void TextServerFallback::_font_clear_cache(FontFallback *p_font_data) { MutexLock ftlock(ft_mutex); @@ -3920,6 +3942,10 @@ RID TextServerFallback::_find_sys_font_for_text(const RID &p_fdef, const String SystemFontCacheRec sysf; sysf.rid = _create_font(); _font_set_data_ptr(sysf.rid, font_data.ptr(), font_data.size()); + if (!_font_validate(sysf.rid)) { + _free_rid(sysf.rid); + continue; + } Dictionary var = dvar; // Select matching style from collection. diff --git a/modules/text_server_fb/text_server_fb.h b/modules/text_server_fb/text_server_fb.h index 7f12ad593b..56626c1f6c 100644 --- a/modules/text_server_fb/text_server_fb.h +++ b/modules/text_server_fb/text_server_fb.h @@ -314,7 +314,8 @@ class TextServerFallback : public TextServerExtension { _FORCE_INLINE_ FontGlyph rasterize_bitmap(FontForSizeFallback *p_data, int p_rect_margin, FT_Bitmap p_bitmap, int p_yofs, int p_xofs, const Vector2 &p_advance, bool p_bgra) const; #endif _FORCE_INLINE_ bool _ensure_glyph(FontFallback *p_font_data, const Vector2i &p_size, int32_t p_glyph, FontGlyph &r_glyph) const; - _FORCE_INLINE_ bool _ensure_cache_for_size(FontFallback *p_font_data, const Vector2i &p_size, FontForSizeFallback *&r_cache_for_size) const; + _FORCE_INLINE_ bool _ensure_cache_for_size(FontFallback *p_font_data, const Vector2i &p_size, FontForSizeFallback *&r_cache_for_size, bool p_silent = false) const; + _FORCE_INLINE_ bool _font_validate(const RID &p_font_rid) const; _FORCE_INLINE_ void _font_clear_cache(FontFallback *p_font_data); static void _generateMTSDF_threaded(void *p_td, uint32_t p_y); diff --git a/modules/tga/SCsub b/modules/tga/SCsub index ccd7d2ee37..c7f58e87f7 100644 --- a/modules/tga/SCsub +++ b/modules/tga/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/theora/SCsub b/modules/theora/SCsub index ca666050dd..be557c1c24 100644 --- a/modules/theora/SCsub +++ b/modules/theora/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/tinyexr/SCsub b/modules/tinyexr/SCsub index bf9242cc16..434e99bf84 100644 --- a/modules/tinyexr/SCsub +++ b/modules/tinyexr/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/upnp/SCsub b/modules/upnp/SCsub index 98c03e9ee9..6657d75cae 100644 --- a/modules/upnp/SCsub +++ b/modules/upnp/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/upnp/upnp.cpp b/modules/upnp/upnp.cpp index 6bdb261b50..4305bf842a 100644 --- a/modules/upnp/upnp.cpp +++ b/modules/upnp/upnp.cpp @@ -131,7 +131,11 @@ void UPNP::parse_igd(Ref<UPNPDevice> dev, UPNPDev *devlist) { GetUPNPUrls(&urls, &data, dev->get_description_url().utf8().get_data(), 0); char addr[16]; +#if MINIUPNPC_API_VERSION >= 18 + int i = UPNP_GetValidIGD(devlist, &urls, &data, (char *)&addr, 16, nullptr, 0); +#else int i = UPNP_GetValidIGD(devlist, &urls, &data, (char *)&addr, 16); +#endif if (i != 1) { FreeUPNPUrls(&urls); diff --git a/modules/vhacd/SCsub b/modules/vhacd/SCsub index 1ff4122114..926cc5b16f 100644 --- a/modules/vhacd/SCsub +++ b/modules/vhacd/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/vorbis/SCsub b/modules/vorbis/SCsub index 322314487f..f063d97fee 100644 --- a/modules/vorbis/SCsub +++ b/modules/vorbis/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/vorbis/resource_importer_ogg_vorbis.cpp b/modules/vorbis/resource_importer_ogg_vorbis.cpp index 7b8d14741b..729a6f5561 100644 --- a/modules/vorbis/resource_importer_ogg_vorbis.cpp +++ b/modules/vorbis/resource_importer_ogg_vorbis.cpp @@ -155,7 +155,6 @@ Ref<AudioStreamOggVorbis> ResourceImporterOggVorbis::load_from_buffer(const Vect char *sync_buf = ogg_sync_buffer(&sync_state, OGG_SYNC_BUFFER_SIZE); err = ogg_sync_check(&sync_state); ERR_FAIL_COND_V_MSG(err != 0, Ref<AudioStreamOggVorbis>(), "Ogg sync error " + itos(err)); - ERR_FAIL_COND_V(cursor > size_t(file_data.size()), Ref<AudioStreamOggVorbis>()); size_t copy_size = file_data.size() - cursor; if (copy_size > OGG_SYNC_BUFFER_SIZE) { copy_size = OGG_SYNC_BUFFER_SIZE; diff --git a/modules/webp/SCsub b/modules/webp/SCsub index dde4450c23..a939e2f90e 100644 --- a/modules/webp/SCsub +++ b/modules/webp/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/webrtc/SCsub b/modules/webrtc/SCsub index e315633f55..0c5f2c9dda 100644 --- a/modules/webrtc/SCsub +++ b/modules/webrtc/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/websocket/SCsub b/modules/websocket/SCsub index 8b469fe5be..acaa0d3ceb 100644 --- a/modules/websocket/SCsub +++ b/modules/websocket/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/websocket/editor/editor_debugger_server_websocket.cpp b/modules/websocket/editor/editor_debugger_server_websocket.cpp index a28fc53440..344a0356c5 100644 --- a/modules/websocket/editor/editor_debugger_server_websocket.cpp +++ b/modules/websocket/editor/editor_debugger_server_websocket.cpp @@ -77,8 +77,8 @@ Error EditorDebuggerServerWebSocket::start(const String &p_uri) { // Optionally override if (!p_uri.is_empty() && p_uri != "ws://") { - String scheme, path; - Error err = p_uri.parse_url(scheme, bind_host, bind_port, path); + String scheme, path, fragment; + Error err = p_uri.parse_url(scheme, bind_host, bind_port, path, fragment); ERR_FAIL_COND_V(err != OK, ERR_INVALID_PARAMETER); ERR_FAIL_COND_V(!bind_host.is_valid_ip_address() && bind_host != "*", ERR_INVALID_PARAMETER); } diff --git a/modules/websocket/emws_peer.cpp b/modules/websocket/emws_peer.cpp index 03a530909b..c5768c9f0b 100644 --- a/modules/websocket/emws_peer.cpp +++ b/modules/websocket/emws_peer.cpp @@ -68,8 +68,9 @@ Error EMWSPeer::connect_to_url(const String &p_url, Ref<TLSOptions> p_tls_option String host; String path; String scheme; + String fragment; int port = 0; - Error err = p_url.parse_url(scheme, host, port, path); + Error err = p_url.parse_url(scheme, host, port, path, fragment); ERR_FAIL_COND_V_MSG(err != OK, err, "Invalid URL: " + p_url); if (scheme.is_empty()) { diff --git a/modules/websocket/wsl_peer.cpp b/modules/websocket/wsl_peer.cpp index 0a9a4053e3..0c0a046805 100644 --- a/modules/websocket/wsl_peer.cpp +++ b/modules/websocket/wsl_peer.cpp @@ -482,8 +482,9 @@ Error WSLPeer::connect_to_url(const String &p_url, Ref<TLSOptions> p_options) { String host; String path; String scheme; + String fragment; int port = 0; - Error err = p_url.parse_url(scheme, host, port, path); + Error err = p_url.parse_url(scheme, host, port, path, fragment); ERR_FAIL_COND_V_MSG(err != OK, err, "Invalid URL: " + p_url); if (scheme.is_empty()) { scheme = "ws://"; diff --git a/modules/webxr/SCsub b/modules/webxr/SCsub index 81caa4a279..9fe4e03ea6 100644 --- a/modules/webxr/SCsub +++ b/modules/webxr/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/webxr/doc_classes/WebXRInterface.xml b/modules/webxr/doc_classes/WebXRInterface.xml index bd7192520a..829279e7bb 100644 --- a/modules/webxr/doc_classes/WebXRInterface.xml +++ b/modules/webxr/doc_classes/WebXRInterface.xml @@ -283,7 +283,7 @@ </signals> <constants> <constant name="TARGET_RAY_MODE_UNKNOWN" value="0" enum="TargetRayMode"> - We don't know the the target ray mode. + We don't know the target ray mode. </constant> <constant name="TARGET_RAY_MODE_GAZE" value="1" enum="TargetRayMode"> Target ray originates at the viewer's eyes and points in the direction they are looking. diff --git a/modules/xatlas_unwrap/SCsub b/modules/xatlas_unwrap/SCsub index aa6bdaea33..ae82a53bd9 100644 --- a/modules/xatlas_unwrap/SCsub +++ b/modules/xatlas_unwrap/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") diff --git a/modules/zip/SCsub b/modules/zip/SCsub index b7710123fd..0bab3ff5f9 100644 --- a/modules/zip/SCsub +++ b/modules/zip/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") |