diff options
Diffstat (limited to 'modules')
170 files changed, 33645 insertions, 146 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/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..024a68f080 --- /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_imgs(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_img(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_img(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_img(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_img(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..9ed8eb0d0a --- /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() { + 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 { + return 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."); + + parameters = p_parameters.duplicate(); + selected_format = p_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); + + emit_signal(SNAME("format_changed")); + + return true; +} + +CameraFeedLinux::CameraFeedLinux(const String &p_device_name) : + CameraFeed() { + device_name = p_device_name; + _query_device(device_name); + set_format(0, Dictionary()); +} + +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/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/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/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/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/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_editor.cpp b/modules/gdscript/gdscript_editor.cpp index cf1cd55355..73f2b1d618 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; 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..6c2d28dc92 --- /dev/null +++ b/modules/godot_physics_2d/godot_body_pair_2d.cpp @@ -0,0 +1,608 @@ +/**************************************************************************/ +/* 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/godot_physics_2d/godot_broad_phase_2d.cpp b/modules/godot_physics_2d/godot_broad_phase_2d.cpp new file mode 100644 index 0000000000..eb6bc21d60 --- /dev/null +++ b/modules/godot_physics_2d/godot_broad_phase_2d.cpp @@ -0,0 +1,36 @@ +/**************************************************************************/ +/* godot_broad_phase_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_broad_phase_2d.h" + +GodotBroadPhase2D::CreateFunction GodotBroadPhase2D::create_func = nullptr; + +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..bbaec8be2b --- /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..84fae73616 --- /dev/null +++ b/modules/godot_physics_3d/godot_body_pair_3d.cpp @@ -0,0 +1,988 @@ +/**************************************************************************/ +/* 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/godot_physics_3d/godot_constraint_3d.h b/modules/godot_physics_3d/godot_constraint_3d.h new file mode 100644 index 0000000000..a833aba93f --- /dev/null +++ b/modules/godot_physics_3d/godot_constraint_3d.h @@ -0,0 +1,81 @@ +/**************************************************************************/ +/* godot_constraint_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_CONSTRAINT_3D_H +#define GODOT_CONSTRAINT_3D_H + +class GodotBody3D; +class GodotSoftBody3D; + +class GodotConstraint3D { + GodotBody3D **_body_ptr; + int _body_count; + uint64_t island_step; + int priority; + bool disabled_collisions_between_bodies; + + RID self; + +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; + } + +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_ GodotBody3D **get_body_ptr() const { return _body_ptr; } + _FORCE_INLINE_ int get_body_count() const { return _body_count; } + + virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const { return nullptr; } + virtual int get_soft_body_count() const { return 0; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _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 ~GodotConstraint3D() {} +}; + +#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..d09a3b4e6d --- /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/mbedtls/SCsub b/modules/mbedtls/SCsub index 90ce98c751..e217ca5ca4 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") 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/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..a467aae2e9 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++; } 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/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..97f91c5674 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") 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/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 index c9e29911d8..d8e7fbc142 100644 --- a/modules/squish/SCsub +++ b/modules/squish/SCsub @@ -1,4 +1,5 @@ #!/usr/bin/env python +from misc.utility.scons_hints import * Import("env") Import("env_modules") 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_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/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/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/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/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") |