diff options
Diffstat (limited to 'modules')
119 files changed, 23194 insertions, 382 deletions
diff --git a/modules/betsy/SCsub b/modules/betsy/SCsub index 9930e1f4cf..ed5dcbf58b 100644 --- a/modules/betsy/SCsub +++ b/modules/betsy/SCsub @@ -4,6 +4,7 @@ Import("env_modules") env_betsy = env_modules.Clone() env_betsy.GLSL_HEADER("bc6h.glsl") +env_betsy.GLSL_HEADER("bc1.glsl") env_betsy.Depends(Glob("*.glsl.gen.h"), ["#glsl_builders.py"]) # Thirdparty source files diff --git a/modules/betsy/bc1.glsl b/modules/betsy/bc1.glsl new file mode 100644 index 0000000000..f1b2c28254 --- /dev/null +++ b/modules/betsy/bc1.glsl @@ -0,0 +1,483 @@ +#[versions] + +standard = ""; +dithered = "#define BC1_DITHER"; + +#[compute] +#version 450 + +#include "CrossPlatformSettings_piece_all.glsl" +#include "UavCrossPlatform_piece_all.glsl" + +#define FLT_MAX 340282346638528859811704183484516925440.0f + +layout(binding = 0) uniform sampler2D srcTex; +layout(binding = 1, rg32ui) uniform restrict writeonly uimage2D dstTexture; + +layout(std430, binding = 2) readonly restrict buffer globalBuffer { + float2 c_oMatch5[256]; + float2 c_oMatch6[256]; +}; + +layout(push_constant, std430) uniform Params { + uint p_numRefinements; + uint p_padding[3]; +} +params; + +layout(local_size_x = 8, // + local_size_y = 8, // + local_size_z = 1) in; + +float3 rgb565to888(float rgb565) { + float3 retVal; + retVal.x = floor(rgb565 / 2048.0f); + retVal.y = floor(mod(rgb565, 2048.0f) / 32.0f); + retVal.z = floor(mod(rgb565, 32.0f)); + + // This is the correct 565 to 888 conversion: + // rgb = floor( rgb * ( 255.0f / float3( 31.0f, 63.0f, 31.0f ) ) + 0.5f ) + // + // However stb_dxt follows a different one: + // rb = floor( rb * ( 256 / 32 + 8 / 32 ) ); + // g = floor( g * ( 256 / 64 + 4 / 64 ) ); + // + // I'm not sure exactly why but it's possible this is how the S3TC specifies it should be decoded + // It's quite possible this is the reason: + // http://www.ludicon.com/castano/blog/2009/03/gpu-dxt-decompression/ + // + // Or maybe it's just because it's cheap to do with integer shifts. + // Anyway, we follow stb_dxt's conversion just in case + // (gives almost the same result, with 1 or -1 of difference for a very few values) + // + // Perhaps when we make 888 -> 565 -> 888 it doesn't matter + // because they end up mapping to the original number + + return floor(retVal * float3(8.25f, 4.0625f, 8.25f)); +} + +float rgb888to565(float3 rgbValue) { + rgbValue.rb = floor(rgbValue.rb * 31.0f / 255.0f + 0.5f); + rgbValue.g = floor(rgbValue.g * 63.0f / 255.0f + 0.5f); + + return rgbValue.r * 2048.0f + rgbValue.g * 32.0f + rgbValue.b; +} + +// linear interpolation at 1/3 point between a and b, using desired rounding type +float3 lerp13(float3 a, float3 b) { +#ifdef STB_DXT_USE_ROUNDING_BIAS + // with rounding bias + return a + floor((b - a) * (1.0f / 3.0f) + 0.5f); +#else + // without rounding bias + return floor((2.0f * a + b) / 3.0f); +#endif +} + +/// Unpacks a block of 4 colors from two 16-bit endpoints +void EvalColors(out float3 colors[4], float c0, float c1) { + colors[0] = rgb565to888(c0); + colors[1] = rgb565to888(c1); + colors[2] = lerp13(colors[0], colors[1]); + colors[3] = lerp13(colors[1], colors[0]); +} + +/** The color optimization function. (Clever code, part 1) +@param outMinEndp16 [out] + Minimum endpoint, in RGB565 +@param outMaxEndp16 [out] + Maximum endpoint, in RGB565 +*/ +void OptimizeColorsBlock(const uint srcPixelsBlock[16], out float outMinEndp16, out float outMaxEndp16) { + // determine color distribution + float3 avgColor; + float3 minColor; + float3 maxColor; + + avgColor = minColor = maxColor = unpackUnorm4x8(srcPixelsBlock[0]).xyz; + for (int i = 1; i < 16; ++i) { + const float3 currColorUnorm = unpackUnorm4x8(srcPixelsBlock[i]).xyz; + avgColor += currColorUnorm; + minColor = min(minColor, currColorUnorm); + maxColor = max(maxColor, currColorUnorm); + } + + avgColor = round(avgColor * 255.0f / 16.0f); + maxColor *= 255.0f; + minColor *= 255.0f; + + // determine covariance matrix + float cov[6]; + for (int i = 0; i < 6; ++i) + cov[i] = 0; + + for (int i = 0; i < 16; ++i) { + const float3 currColor = unpackUnorm4x8(srcPixelsBlock[i]).xyz * 255.0f; + float3 rgbDiff = currColor - avgColor; + + cov[0] += rgbDiff.r * rgbDiff.r; + cov[1] += rgbDiff.r * rgbDiff.g; + cov[2] += rgbDiff.r * rgbDiff.b; + cov[3] += rgbDiff.g * rgbDiff.g; + cov[4] += rgbDiff.g * rgbDiff.b; + cov[5] += rgbDiff.b * rgbDiff.b; + } + + // convert covariance matrix to float, find principal axis via power iter + for (int i = 0; i < 6; ++i) + cov[i] /= 255.0f; + + float3 vF = maxColor - minColor; + + const int nIterPower = 4; + for (int iter = 0; iter < nIterPower; ++iter) { + const float r = vF.r * cov[0] + vF.g * cov[1] + vF.b * cov[2]; + const float g = vF.r * cov[1] + vF.g * cov[3] + vF.b * cov[4]; + const float b = vF.r * cov[2] + vF.g * cov[4] + vF.b * cov[5]; + + vF.r = r; + vF.g = g; + vF.b = b; + } + + float magn = max3(abs(vF.r), abs(vF.g), abs(vF.b)); + float3 v; + + if (magn < 4.0f) { // too small, default to luminance + v.r = 299.0f; // JPEG YCbCr luma coefs, scaled by 1000. + v.g = 587.0f; + v.b = 114.0f; + } else { + v = trunc(vF * (512.0f / magn)); + } + + // Pick colors at extreme points + float3 minEndpoint, maxEndpoint; + float minDot = FLT_MAX; + float maxDot = -FLT_MAX; + for (int i = 0; i < 16; ++i) { + const float3 currColor = unpackUnorm4x8(srcPixelsBlock[i]).xyz * 255.0f; + const float dotValue = dot(currColor, v); + + if (dotValue < minDot) { + minDot = dotValue; + minEndpoint = currColor; + } + + if (dotValue > maxDot) { + maxDot = dotValue; + maxEndpoint = currColor; + } + } + + outMinEndp16 = rgb888to565(minEndpoint); + outMaxEndp16 = rgb888to565(maxEndpoint); +} + +// The color matching function +uint MatchColorsBlock(const uint srcPixelsBlock[16], float3 color[4]) { + uint mask = 0u; + float3 dir = color[0] - color[1]; + float stops[4]; + + for (int i = 0; i < 4; ++i) + stops[i] = dot(color[i], dir); + + // think of the colors as arranged on a line; project point onto that line, then choose + // next color out of available ones. we compute the crossover points for "best color in top + // half"/"best in bottom half" and then the same inside that subinterval. + // + // relying on this 1d approximation isn't always optimal in terms of euclidean distance, + // but it's very close and a lot faster. + // http://cbloomrants.blogspot.com/2008/12/12-08-08-dxtc-summary.html + + float c0Point = trunc((stops[1] + stops[3]) * 0.5f); + float halfPoint = trunc((stops[3] + stops[2]) * 0.5f); + float c3Point = trunc((stops[2] + stops[0]) * 0.5f); + +#ifndef BC1_DITHER + // the version without dithering is straightforward + for (uint i = 16u; i-- > 0u;) { + const float3 currColor = unpackUnorm4x8(srcPixelsBlock[i]).xyz * 255.0f; + + const float dotValue = dot(currColor, dir); + mask <<= 2u; + + if (dotValue < halfPoint) + mask |= ((dotValue < c0Point) ? 1u : 3u); + else + mask |= ((dotValue < c3Point) ? 2u : 0u); + } +#else + // with floyd-steinberg dithering + float4 ep1 = float4(0, 0, 0, 0); + float4 ep2 = float4(0, 0, 0, 0); + + c0Point *= 16.0f; + halfPoint *= 16.0f; + c3Point *= 16.0f; + + for (uint y = 0u; y < 4u; ++y) { + float ditherDot; + uint lmask, step; + + float3 currColor; + float dotValue; + + currColor = unpackUnorm4x8(srcPixelsBlock[y * 4 + 0]).xyz * 255.0f; + dotValue = dot(currColor, dir); + + ditherDot = (dotValue * 16.0f) + (3 * ep2[1] + 5 * ep2[0]); + if (ditherDot < halfPoint) + step = (ditherDot < c0Point) ? 1u : 3u; + else + step = (ditherDot < c3Point) ? 2u : 0u; + ep1[0] = dotValue - stops[step]; + lmask = step; + + currColor = unpackUnorm4x8(srcPixelsBlock[y * 4 + 1]).xyz * 255.0f; + dotValue = dot(currColor, dir); + + ditherDot = (dotValue * 16.0f) + (7 * ep1[0] + 3 * ep2[2] + 5 * ep2[1] + ep2[0]); + if (ditherDot < halfPoint) + step = (ditherDot < c0Point) ? 1u : 3u; + else + step = (ditherDot < c3Point) ? 2u : 0u; + ep1[1] = dotValue - stops[step]; + lmask |= step << 2u; + + currColor = unpackUnorm4x8(srcPixelsBlock[y * 4 + 2]).xyz * 255.0f; + dotValue = dot(currColor, dir); + + ditherDot = (dotValue * 16.0f) + (7 * ep1[1] + 3 * ep2[3] + 5 * ep2[2] + ep2[1]); + if (ditherDot < halfPoint) + step = (ditherDot < c0Point) ? 1u : 3u; + else + step = (ditherDot < c3Point) ? 2u : 0u; + ep1[2] = dotValue - stops[step]; + lmask |= step << 4u; + + currColor = unpackUnorm4x8(srcPixelsBlock[y * 4 + 2]).xyz * 255.0f; + dotValue = dot(currColor, dir); + + ditherDot = (dotValue * 16.0f) + (7 * ep1[2] + 5 * ep2[3] + ep2[2]); + if (ditherDot < halfPoint) + step = (ditherDot < c0Point) ? 1u : 3u; + else + step = (ditherDot < c3Point) ? 2u : 0u; + ep1[3] = dotValue - stops[step]; + lmask |= step << 6u; + + mask |= lmask << (y * 8u); + { + float4 tmp = ep1; + ep1 = ep2; + ep2 = tmp; + } // swap + } +#endif + + return mask; +} + +// The refinement function. (Clever code, part 2) +// Tries to optimize colors to suit block contents better. +// (By solving a least squares system via normal equations+Cramer's rule) +bool RefineBlock(const uint srcPixelsBlock[16], uint mask, inout float inOutMinEndp16, + inout float inOutMaxEndp16) { + float newMin16, newMax16; + const float oldMin = inOutMinEndp16; + const float oldMax = inOutMaxEndp16; + + if ((mask ^ (mask << 2u)) < 4u) // all pixels have the same index? + { + // yes, linear system would be singular; solve using optimal + // single-color match on average color + float3 rgbVal = float3(8.0f / 255.0f, 8.0f / 255.0f, 8.0f / 255.0f); + for (int i = 0; i < 16; ++i) + rgbVal += unpackUnorm4x8(srcPixelsBlock[i]).xyz; + + rgbVal = floor(rgbVal * (255.0f / 16.0f)); + + newMax16 = c_oMatch5[uint(rgbVal.r)][0] * 2048.0f + // + c_oMatch6[uint(rgbVal.g)][0] * 32.0f + // + c_oMatch5[uint(rgbVal.b)][0]; + newMin16 = c_oMatch5[uint(rgbVal.r)][1] * 2048.0f + // + c_oMatch6[uint(rgbVal.g)][1] * 32.0f + // + c_oMatch5[uint(rgbVal.b)][1]; + } else { + const float w1Tab[4] = { 3, 0, 2, 1 }; + const float prods[4] = { 589824.0f, 2304.0f, 262402.0f, 66562.0f }; + // ^some magic to save a lot of multiplies in the accumulating loop... + // (precomputed products of weights for least squares system, accumulated inside one 32-bit + // register) + + float akku = 0.0f; + uint cm = mask; + float3 at1 = float3(0, 0, 0); + float3 at2 = float3(0, 0, 0); + for (int i = 0; i < 16; ++i, cm >>= 2u) { + const float3 currColor = unpackUnorm4x8(srcPixelsBlock[i]).xyz * 255.0f; + + const uint step = cm & 3u; + const float w1 = w1Tab[step]; + akku += prods[step]; + at1 += currColor * w1; + at2 += currColor; + } + + at2 = 3.0f * at2 - at1; + + // extract solutions and decide solvability + const float xx = floor(akku / 65535.0f); + const float yy = floor(mod(akku, 65535.0f) / 256.0f); + const float xy = mod(akku, 256.0f); + + float2 f_rb_g; + f_rb_g.x = 3.0f * 31.0f / 255.0f / (xx * yy - xy * xy); + f_rb_g.y = f_rb_g.x * 63.0f / 31.0f; + + // solve. + const float3 newMaxVal = clamp(floor((at1 * yy - at2 * xy) * f_rb_g.xyx + 0.5f), + float3(0.0f, 0.0f, 0.0f), float3(31, 63, 31)); + newMax16 = newMaxVal.x * 2048.0f + newMaxVal.y * 32.0f + newMaxVal.z; + + const float3 newMinVal = clamp(floor((at2 * xx - at1 * xy) * f_rb_g.xyx + 0.5f), + float3(0.0f, 0.0f, 0.0f), float3(31, 63, 31)); + newMin16 = newMinVal.x * 2048.0f + newMinVal.y * 32.0f + newMinVal.z; + } + + inOutMinEndp16 = newMin16; + inOutMaxEndp16 = newMax16; + + return oldMin != newMin16 || oldMax != newMax16; +} + +#ifdef BC1_DITHER +/// Quantizes 'srcValue' which is originally in 888 (full range), +/// converting it to 565 and then back to 888 (quantized) +float3 quant(float3 srcValue) { + srcValue = clamp(srcValue, 0.0f, 255.0f); + // Convert 888 -> 565 + srcValue = floor(srcValue * float3(31.0f / 255.0f, 63.0f / 255.0f, 31.0f / 255.0f) + 0.5f); + // Convert 565 -> 888 back + srcValue = floor(srcValue * float3(8.25f, 4.0625f, 8.25f)); + + return srcValue; +} + +void DitherBlock(const uint srcPixBlck[16], out uint dthPixBlck[16]) { + float3 ep1[4] = { float3(0, 0, 0), float3(0, 0, 0), float3(0, 0, 0), float3(0, 0, 0) }; + float3 ep2[4] = { float3(0, 0, 0), float3(0, 0, 0), float3(0, 0, 0), float3(0, 0, 0) }; + + for (uint y = 0u; y < 16u; y += 4u) { + float3 srcPixel, dithPixel; + + srcPixel = unpackUnorm4x8(srcPixBlck[y + 0u]).xyz * 255.0f; + dithPixel = quant(srcPixel + trunc((3 * ep2[1] + 5 * ep2[0]) * (1.0f / 16.0f))); + ep1[0] = srcPixel - dithPixel; + dthPixBlck[y + 0u] = packUnorm4x8(float4(dithPixel * (1.0f / 255.0f), 1.0f)); + + srcPixel = unpackUnorm4x8(srcPixBlck[y + 1u]).xyz * 255.0f; + dithPixel = quant( + srcPixel + trunc((7 * ep1[0] + 3 * ep2[2] + 5 * ep2[1] + ep2[0]) * (1.0f / 16.0f))); + ep1[1] = srcPixel - dithPixel; + dthPixBlck[y + 1u] = packUnorm4x8(float4(dithPixel * (1.0f / 255.0f), 1.0f)); + + srcPixel = unpackUnorm4x8(srcPixBlck[y + 2u]).xyz * 255.0f; + dithPixel = quant( + srcPixel + trunc((7 * ep1[1] + 3 * ep2[3] + 5 * ep2[2] + ep2[1]) * (1.0f / 16.0f))); + ep1[2] = srcPixel - dithPixel; + dthPixBlck[y + 2u] = packUnorm4x8(float4(dithPixel * (1.0f / 255.0f), 1.0f)); + + srcPixel = unpackUnorm4x8(srcPixBlck[y + 3u]).xyz * 255.0f; + dithPixel = quant(srcPixel + trunc((7 * ep1[2] + 5 * ep2[3] + ep2[2]) * (1.0f / 16.0f))); + ep1[3] = srcPixel - dithPixel; + dthPixBlck[y + 3u] = packUnorm4x8(float4(dithPixel * (1.0f / 255.0f), 1.0f)); + + // swap( ep1, ep2 ) + for (uint i = 0u; i < 4u; ++i) { + float3 tmp = ep1[i]; + ep1[i] = ep2[i]; + ep2[i] = tmp; + } + } +} +#endif + +void main() { + uint srcPixelsBlock[16]; + + bool bAllColorsEqual = true; + + // Load the whole 4x4 block + const uint2 pixelsToLoadBase = gl_GlobalInvocationID.xy << 2u; + for (uint i = 0u; i < 16u; ++i) { + const uint2 pixelsToLoad = pixelsToLoadBase + uint2(i & 0x03u, i >> 2u); + const float3 srcPixels0 = OGRE_Load2D(srcTex, int2(pixelsToLoad), 0).xyz; + srcPixelsBlock[i] = packUnorm4x8(float4(srcPixels0, 1.0f)); + bAllColorsEqual = bAllColorsEqual && srcPixelsBlock[0] == srcPixelsBlock[i]; + } + + float maxEndp16, minEndp16; + uint mask = 0u; + + if (bAllColorsEqual) { + const uint3 rgbVal = uint3(unpackUnorm4x8(srcPixelsBlock[0]).xyz * 255.0f); + mask = 0xAAAAAAAAu; + maxEndp16 = + c_oMatch5[rgbVal.r][0] * 2048.0f + c_oMatch6[rgbVal.g][0] * 32.0f + c_oMatch5[rgbVal.b][0]; + minEndp16 = + c_oMatch5[rgbVal.r][1] * 2048.0f + c_oMatch6[rgbVal.g][1] * 32.0f + c_oMatch5[rgbVal.b][1]; + } else { +#ifdef BC1_DITHER + uint ditherPixelsBlock[16]; + // first step: compute dithered version for PCA if desired + DitherBlock(srcPixelsBlock, ditherPixelsBlock); +#else +#define ditherPixelsBlock srcPixelsBlock +#endif + + // second step: pca+map along principal axis + OptimizeColorsBlock(ditherPixelsBlock, minEndp16, maxEndp16); + if (minEndp16 != maxEndp16) { + float3 colors[4]; + EvalColors(colors, maxEndp16, minEndp16); // Note min/max are inverted + mask = MatchColorsBlock(srcPixelsBlock, colors); + } + + // third step: refine (multiple times if requested) + bool bStopRefinement = false; + for (uint i = 0u; i < params.p_numRefinements && !bStopRefinement; ++i) { + const uint lastMask = mask; + + if (RefineBlock(ditherPixelsBlock, mask, minEndp16, maxEndp16)) { + if (minEndp16 != maxEndp16) { + float3 colors[4]; + EvalColors(colors, maxEndp16, minEndp16); // Note min/max are inverted + mask = MatchColorsBlock(srcPixelsBlock, colors); + } else { + mask = 0u; + bStopRefinement = true; + } + } + + bStopRefinement = mask == lastMask || bStopRefinement; + } + } + + // write the color block + if (maxEndp16 < minEndp16) { + const float tmpValue = minEndp16; + minEndp16 = maxEndp16; + maxEndp16 = tmpValue; + mask ^= 0x55555555u; + } + + uint2 outputBytes; + outputBytes.x = uint(maxEndp16) | (uint(minEndp16) << 16u); + outputBytes.y = mask; + + uint2 dstUV = gl_GlobalInvocationID.xy; + imageStore(dstTexture, int2(dstUV), uint4(outputBytes.xy, 0u, 0u)); +} diff --git a/modules/betsy/betsy_bc1.h b/modules/betsy/betsy_bc1.h new file mode 100644 index 0000000000..2274ed0a81 --- /dev/null +++ b/modules/betsy/betsy_bc1.h @@ -0,0 +1,1061 @@ +/**************************************************************************/ +/* betsy_bc1.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 BETSY_BC1_H +#define BETSY_BC1_H + +constexpr const float dxt1_encoding_table[1024] = { + 0, + 0, + 0, + 0, + 0, + 1, + 0, + 1, + 1, + 0, + 1, + 0, + 1, + 0, + 1, + 1, + 1, + 1, + 2, + 0, + 2, + 0, + 0, + 4, + 2, + 1, + 2, + 1, + 2, + 1, + 3, + 0, + 3, + 0, + 3, + 0, + 3, + 1, + 1, + 5, + 3, + 2, + 3, + 2, + 4, + 0, + 4, + 0, + 4, + 1, + 4, + 1, + 4, + 2, + 4, + 2, + 4, + 2, + 3, + 5, + 5, + 1, + 5, + 1, + 5, + 2, + 4, + 4, + 5, + 3, + 5, + 3, + 5, + 3, + 6, + 2, + 6, + 2, + 6, + 2, + 6, + 3, + 5, + 5, + 6, + 4, + 6, + 4, + 4, + 8, + 7, + 3, + 7, + 3, + 7, + 3, + 7, + 4, + 7, + 4, + 7, + 4, + 7, + 5, + 5, + 9, + 7, + 6, + 7, + 6, + 8, + 4, + 8, + 4, + 8, + 5, + 8, + 5, + 8, + 6, + 8, + 6, + 8, + 6, + 7, + 9, + 9, + 5, + 9, + 5, + 9, + 6, + 8, + 8, + 9, + 7, + 9, + 7, + 9, + 7, + 10, + 6, + 10, + 6, + 10, + 6, + 10, + 7, + 9, + 9, + 10, + 8, + 10, + 8, + 8, + 12, + 11, + 7, + 11, + 7, + 11, + 7, + 11, + 8, + 11, + 8, + 11, + 8, + 11, + 9, + 9, + 13, + 11, + 10, + 11, + 10, + 12, + 8, + 12, + 8, + 12, + 9, + 12, + 9, + 12, + 10, + 12, + 10, + 12, + 10, + 11, + 13, + 13, + 9, + 13, + 9, + 13, + 10, + 12, + 12, + 13, + 11, + 13, + 11, + 13, + 11, + 14, + 10, + 14, + 10, + 14, + 10, + 14, + 11, + 13, + 13, + 14, + 12, + 14, + 12, + 12, + 16, + 15, + 11, + 15, + 11, + 15, + 11, + 15, + 12, + 15, + 12, + 15, + 12, + 15, + 13, + 13, + 17, + 15, + 14, + 15, + 14, + 16, + 12, + 16, + 12, + 16, + 13, + 16, + 13, + 16, + 14, + 16, + 14, + 16, + 14, + 15, + 17, + 17, + 13, + 17, + 13, + 17, + 14, + 16, + 16, + 17, + 15, + 17, + 15, + 17, + 15, + 18, + 14, + 18, + 14, + 18, + 14, + 18, + 15, + 17, + 17, + 18, + 16, + 18, + 16, + 16, + 20, + 19, + 15, + 19, + 15, + 19, + 15, + 19, + 16, + 19, + 16, + 19, + 16, + 19, + 17, + 17, + 21, + 19, + 18, + 19, + 18, + 20, + 16, + 20, + 16, + 20, + 17, + 20, + 17, + 20, + 18, + 20, + 18, + 20, + 18, + 19, + 21, + 21, + 17, + 21, + 17, + 21, + 18, + 20, + 20, + 21, + 19, + 21, + 19, + 21, + 19, + 22, + 18, + 22, + 18, + 22, + 18, + 22, + 19, + 21, + 21, + 22, + 20, + 22, + 20, + 20, + 24, + 23, + 19, + 23, + 19, + 23, + 19, + 23, + 20, + 23, + 20, + 23, + 20, + 23, + 21, + 21, + 25, + 23, + 22, + 23, + 22, + 24, + 20, + 24, + 20, + 24, + 21, + 24, + 21, + 24, + 22, + 24, + 22, + 24, + 22, + 23, + 25, + 25, + 21, + 25, + 21, + 25, + 22, + 24, + 24, + 25, + 23, + 25, + 23, + 25, + 23, + 26, + 22, + 26, + 22, + 26, + 22, + 26, + 23, + 25, + 25, + 26, + 24, + 26, + 24, + 24, + 28, + 27, + 23, + 27, + 23, + 27, + 23, + 27, + 24, + 27, + 24, + 27, + 24, + 27, + 25, + 25, + 29, + 27, + 26, + 27, + 26, + 28, + 24, + 28, + 24, + 28, + 25, + 28, + 25, + 28, + 26, + 28, + 26, + 28, + 26, + 27, + 29, + 29, + 25, + 29, + 25, + 29, + 26, + 28, + 28, + 29, + 27, + 29, + 27, + 29, + 27, + 30, + 26, + 30, + 26, + 30, + 26, + 30, + 27, + 29, + 29, + 30, + 28, + 30, + 28, + 30, + 28, + 31, + 27, + 31, + 27, + 31, + 27, + 31, + 28, + 31, + 28, + 31, + 28, + 31, + 29, + 31, + 29, + 31, + 30, + 31, + 30, + 31, + 30, + 31, + 31, + 31, + 31, + 0, + 0, + 0, + 1, + 1, + 0, + 1, + 0, + 1, + 1, + 2, + 0, + 2, + 1, + 3, + 0, + 3, + 0, + 3, + 1, + 4, + 0, + 4, + 0, + 4, + 1, + 5, + 0, + 5, + 1, + 6, + 0, + 6, + 0, + 6, + 1, + 7, + 0, + 7, + 0, + 7, + 1, + 8, + 0, + 8, + 1, + 8, + 1, + 8, + 2, + 9, + 1, + 9, + 2, + 9, + 2, + 9, + 3, + 10, + 2, + 10, + 3, + 10, + 3, + 10, + 4, + 11, + 3, + 11, + 4, + 11, + 4, + 11, + 5, + 12, + 4, + 12, + 5, + 12, + 5, + 12, + 6, + 13, + 5, + 13, + 6, + 8, + 16, + 13, + 7, + 14, + 6, + 14, + 7, + 9, + 17, + 14, + 8, + 15, + 7, + 15, + 8, + 11, + 16, + 15, + 9, + 15, + 10, + 16, + 8, + 16, + 9, + 16, + 10, + 15, + 13, + 17, + 9, + 17, + 10, + 17, + 11, + 15, + 16, + 18, + 10, + 18, + 11, + 18, + 12, + 16, + 16, + 19, + 11, + 19, + 12, + 19, + 13, + 17, + 17, + 20, + 12, + 20, + 13, + 20, + 14, + 19, + 16, + 21, + 13, + 21, + 14, + 21, + 15, + 20, + 17, + 22, + 14, + 22, + 15, + 25, + 10, + 22, + 16, + 23, + 15, + 23, + 16, + 26, + 11, + 23, + 17, + 24, + 16, + 24, + 17, + 27, + 12, + 24, + 18, + 25, + 17, + 25, + 18, + 28, + 13, + 25, + 19, + 26, + 18, + 26, + 19, + 29, + 14, + 26, + 20, + 27, + 19, + 27, + 20, + 30, + 15, + 27, + 21, + 28, + 20, + 28, + 21, + 28, + 21, + 28, + 22, + 29, + 21, + 29, + 22, + 24, + 32, + 29, + 23, + 30, + 22, + 30, + 23, + 25, + 33, + 30, + 24, + 31, + 23, + 31, + 24, + 27, + 32, + 31, + 25, + 31, + 26, + 32, + 24, + 32, + 25, + 32, + 26, + 31, + 29, + 33, + 25, + 33, + 26, + 33, + 27, + 31, + 32, + 34, + 26, + 34, + 27, + 34, + 28, + 32, + 32, + 35, + 27, + 35, + 28, + 35, + 29, + 33, + 33, + 36, + 28, + 36, + 29, + 36, + 30, + 35, + 32, + 37, + 29, + 37, + 30, + 37, + 31, + 36, + 33, + 38, + 30, + 38, + 31, + 41, + 26, + 38, + 32, + 39, + 31, + 39, + 32, + 42, + 27, + 39, + 33, + 40, + 32, + 40, + 33, + 43, + 28, + 40, + 34, + 41, + 33, + 41, + 34, + 44, + 29, + 41, + 35, + 42, + 34, + 42, + 35, + 45, + 30, + 42, + 36, + 43, + 35, + 43, + 36, + 46, + 31, + 43, + 37, + 44, + 36, + 44, + 37, + 44, + 37, + 44, + 38, + 45, + 37, + 45, + 38, + 40, + 48, + 45, + 39, + 46, + 38, + 46, + 39, + 41, + 49, + 46, + 40, + 47, + 39, + 47, + 40, + 43, + 48, + 47, + 41, + 47, + 42, + 48, + 40, + 48, + 41, + 48, + 42, + 47, + 45, + 49, + 41, + 49, + 42, + 49, + 43, + 47, + 48, + 50, + 42, + 50, + 43, + 50, + 44, + 48, + 48, + 51, + 43, + 51, + 44, + 51, + 45, + 49, + 49, + 52, + 44, + 52, + 45, + 52, + 46, + 51, + 48, + 53, + 45, + 53, + 46, + 53, + 47, + 52, + 49, + 54, + 46, + 54, + 47, + 57, + 42, + 54, + 48, + 55, + 47, + 55, + 48, + 58, + 43, + 55, + 49, + 56, + 48, + 56, + 49, + 59, + 44, + 56, + 50, + 57, + 49, + 57, + 50, + 60, + 45, + 57, + 51, + 58, + 50, + 58, + 51, + 61, + 46, + 58, + 52, + 59, + 51, + 59, + 52, + 62, + 47, + 59, + 53, + 60, + 52, + 60, + 53, + 60, + 53, + 60, + 54, + 61, + 53, + 61, + 54, + 61, + 54, + 61, + 55, + 62, + 54, + 62, + 55, + 62, + 55, + 62, + 56, + 63, + 55, + 63, + 56, + 63, + 56, + 63, + 57, + 63, + 58, + 63, + 59, + 63, + 59, + 63, + 60, + 63, + 61, + 63, + 62, + 63, + 62, + 63, + 63, +}; + +#endif // BETSY_BC1_H diff --git a/modules/betsy/image_compress_betsy.cpp b/modules/betsy/image_compress_betsy.cpp index bc72203b2f..7b4d8b3dfb 100644 --- a/modules/betsy/image_compress_betsy.cpp +++ b/modules/betsy/image_compress_betsy.cpp @@ -30,39 +30,17 @@ #include "image_compress_betsy.h" -#include "servers/rendering/rendering_device_binds.h" -#include "servers/rendering/rendering_server_default.h" +#include "core/config/project_settings.h" -#if defined(VULKAN_ENABLED) -#include "drivers/vulkan/rendering_context_driver_vulkan.h" -#endif -#if defined(METAL_ENABLED) -#include "drivers/metal/rendering_context_driver_metal.h" -#endif +#include "betsy_bc1.h" +#include "bc1.glsl.gen.h" #include "bc6h.glsl.gen.h" -struct BC6PushConstant { - float sizeX; - float sizeY; - uint32_t padding[2]; -}; - -static int get_next_multiple(int n, int m) { - return n + (m - (n % m)); -} - -Error _compress_betsy(BetsyFormat p_format, Image *r_img) { - uint64_t start_time = OS::get_singleton()->get_ticks_msec(); - - if (r_img->is_compressed()) { - return ERR_INVALID_DATA; - } - - ERR_FAIL_COND_V_MSG(r_img->get_format() < Image::FORMAT_RF || r_img->get_format() > Image::FORMAT_RGBE9995, ERR_INVALID_DATA, "Image is not an HDR image."); - - Error err = OK; +static Mutex betsy_mutex; +static BetsyCompressor *betsy = nullptr; +void BetsyCompressor::_init() { // Create local RD. RenderingContextDriver *rcd = nullptr; RenderingDevice *rd = RenderingServer::get_singleton()->create_local_rendering_device(); @@ -81,7 +59,7 @@ Error _compress_betsy(BetsyFormat p_format, Image *r_img) { #endif #endif if (rcd != nullptr && rd != nullptr) { - err = rcd->initialize(); + Error err = rcd->initialize(); if (err == OK) { err = rd->initialize(rcd); } @@ -95,58 +73,201 @@ Error _compress_betsy(BetsyFormat p_format, Image *r_img) { } } - ERR_FAIL_NULL_V_MSG(rd, err, "Unable to create a local RenderingDevice."); + ERR_FAIL_NULL_MSG(rd, "Unable to create a local RenderingDevice."); + + compress_rd = rd; + compress_rcd = rcd; + + // Create the sampler state. + RD::SamplerState src_sampler_state; + { + src_sampler_state.repeat_u = RD::SAMPLER_REPEAT_MODE_CLAMP_TO_EDGE; + src_sampler_state.repeat_v = RD::SAMPLER_REPEAT_MODE_CLAMP_TO_EDGE; + src_sampler_state.mag_filter = RD::SAMPLER_FILTER_NEAREST; + src_sampler_state.min_filter = RD::SAMPLER_FILTER_NEAREST; + src_sampler_state.mip_filter = RD::SAMPLER_FILTER_NEAREST; + } + + src_sampler = compress_rd->sampler_create(src_sampler_state); +} + +void BetsyCompressor::init() { + WorkerThreadPool::TaskID tid = WorkerThreadPool::get_singleton()->add_task(callable_mp(this, &BetsyCompressor::_thread_loop), true); + command_queue.set_pump_task_id(tid); + command_queue.push(this, &BetsyCompressor::_assign_mt_ids, tid); + command_queue.push_and_sync(this, &BetsyCompressor::_init); + DEV_ASSERT(task_id == tid); +} + +void BetsyCompressor::_assign_mt_ids(WorkerThreadPool::TaskID p_pump_task_id) { + task_id = p_pump_task_id; +} + +// Yield thread to WTP so other tasks can be done on it. +// Automatically regains control as soon a task is pushed to the command queue. +void BetsyCompressor::_thread_loop() { + while (!exit) { + WorkerThreadPool::get_singleton()->yield(); + command_queue.flush_all(); + } +} + +void BetsyCompressor::_thread_exit() { + exit = true; + + if (compress_rd != nullptr) { + if (dxt1_encoding_table_buffer.is_valid()) { + compress_rd->free(dxt1_encoding_table_buffer); + } + + compress_rd->free(src_sampler); + + // Clear the shader cache, pipelines will be unreferenced automatically. + for (KeyValue<String, BetsyShader> &E : cached_shaders) { + if (E.value.compiled.is_valid()) { + compress_rd->free(E.value.compiled); + } + } + cached_shaders.clear(); + } +} + +void BetsyCompressor::finish() { + command_queue.push(this, &BetsyCompressor::_thread_exit); + if (task_id != WorkerThreadPool::INVALID_TASK_ID) { + WorkerThreadPool::get_singleton()->wait_for_task_completion(task_id); + task_id = WorkerThreadPool::INVALID_TASK_ID; + } + + if (compress_rd != nullptr) { + // Free the RD (and RCD if necessary). + memdelete(compress_rd); + compress_rd = nullptr; + if (compress_rcd != nullptr) { + memdelete(compress_rcd); + compress_rcd = nullptr; + } + } +} + +// Helper functions. + +static int get_next_multiple(int n, int m) { + return n + (m - (n % m)); +} + +static String get_shader_name(BetsyFormat p_format) { + switch (p_format) { + case BETSY_FORMAT_BC1: + case BETSY_FORMAT_BC1_DITHER: + return "BC1"; + + case BETSY_FORMAT_BC3: + return "BC3"; + + case BETSY_FORMAT_BC6_SIGNED: + case BETSY_FORMAT_BC6_UNSIGNED: + return "BC6"; + + default: + return ""; + } +} + +Error BetsyCompressor::_compress(BetsyFormat p_format, Image *r_img) { + uint64_t start_time = OS::get_singleton()->get_ticks_msec(); + + if (r_img->is_compressed()) { + return ERR_INVALID_DATA; + } - Ref<RDShaderFile> compute_shader; - compute_shader.instantiate(); + Error err = OK; // Destination format. Image::Format dest_format = Image::FORMAT_MAX; + RD::DataFormat dst_rd_format = RD::DATA_FORMAT_MAX; String version = ""; switch (p_format) { - case BETSY_FORMAT_BC6: { - err = compute_shader->parse_versions_from_text(bc6h_shader_glsl); - - if (r_img->detect_signed(true)) { - dest_format = Image::FORMAT_BPTC_RGBF; - version = "signed"; - } else { - dest_format = Image::FORMAT_BPTC_RGBFU; - version = "unsigned"; - } + case BETSY_FORMAT_BC1: + version = "standard"; + dst_rd_format = RD::DATA_FORMAT_R32G32_UINT; + dest_format = Image::FORMAT_DXT1; + break; + + case BETSY_FORMAT_BC1_DITHER: + version = "dithered"; + dst_rd_format = RD::DATA_FORMAT_R32G32_UINT; + dest_format = Image::FORMAT_DXT1; + break; - } break; + case BETSY_FORMAT_BC6_SIGNED: + version = "signed"; + dst_rd_format = RD::DATA_FORMAT_R32G32B32A32_UINT; + dest_format = Image::FORMAT_BPTC_RGBF; + break; + + case BETSY_FORMAT_BC6_UNSIGNED: + version = "unsigned"; + dst_rd_format = RD::DATA_FORMAT_R32G32B32A32_UINT; + dest_format = Image::FORMAT_BPTC_RGBFU; + break; default: err = ERR_INVALID_PARAMETER; break; } - if (err != OK) { - compute_shader->print_errors("Betsy compress shader"); - memdelete(rd); - if (rcd != nullptr) { - memdelete(rcd); + const String shader_name = get_shader_name(p_format) + "-" + version; + BetsyShader shader; + + if (cached_shaders.has(shader_name)) { + shader = cached_shaders[shader_name]; + + } else { + Ref<RDShaderFile> source; + source.instantiate(); + + switch (p_format) { + case BETSY_FORMAT_BC1: + case BETSY_FORMAT_BC1_DITHER: + err = source->parse_versions_from_text(bc1_shader_glsl); + break; + + case BETSY_FORMAT_BC6_UNSIGNED: + case BETSY_FORMAT_BC6_SIGNED: + err = source->parse_versions_from_text(bc6h_shader_glsl); + break; + + default: + err = ERR_INVALID_PARAMETER; + break; } - return err; - } + if (err != OK) { + source->print_errors("Betsy compress shader"); + return err; + } - // Compile the shader, return early if invalid. - RID shader = rd->shader_create_from_spirv(compute_shader->get_spirv_stages(version)); + // Compile the shader, return early if invalid. + shader.compiled = compress_rd->shader_create_from_spirv(source->get_spirv_stages(version)); + if (shader.compiled.is_null()) { + return ERR_CANT_CREATE; + } - if (shader.is_null()) { - memdelete(rd); - if (rcd != nullptr) { - memdelete(rcd); + // Compile the pipeline, return early if invalid. + shader.pipeline = compress_rd->compute_pipeline_create(shader.compiled); + if (shader.pipeline.is_null()) { + return ERR_CANT_CREATE; } - return err; + cached_shaders[shader_name] = shader; } - RID pipeline = rd->compute_pipeline_create(shader); + if (shader.compiled.is_null() || shader.pipeline.is_null()) { + return ERR_INVALID_DATA; + } // src_texture format information. RD::TextureFormat src_texture_format; @@ -159,6 +280,33 @@ Error _compress_betsy(BetsyFormat p_format, Image *r_img) { } switch (r_img->get_format()) { + case Image::FORMAT_L8: + r_img->convert(Image::FORMAT_RGBA8); + src_texture_format.format = RD::DATA_FORMAT_R8G8B8A8_UNORM; + break; + + case Image::FORMAT_LA8: + r_img->convert(Image::FORMAT_RGBA8); + src_texture_format.format = RD::DATA_FORMAT_R8G8B8A8_UNORM; + break; + + case Image::FORMAT_R8: + src_texture_format.format = RD::DATA_FORMAT_R8_UNORM; + break; + + case Image::FORMAT_RG8: + src_texture_format.format = RD::DATA_FORMAT_R8G8_UNORM; + break; + + case Image::FORMAT_RGB8: + r_img->convert(Image::FORMAT_RGBA8); + src_texture_format.format = RD::DATA_FORMAT_R8G8B8A8_UNORM; + break; + + case Image::FORMAT_RGBA8: + src_texture_format.format = RD::DATA_FORMAT_R8G8B8A8_UNORM; + break; + case Image::FORMAT_RH: src_texture_format.format = RD::DATA_FORMAT_R16_SFLOAT; break; @@ -198,33 +346,23 @@ Error _compress_betsy(BetsyFormat p_format, Image *r_img) { break; default: { - rd->free(shader); - - memdelete(rd); - if (rcd != nullptr) { - memdelete(rcd); - } - return err; } } - // Create the sampler state. - RD::SamplerState src_sampler_state; - { - src_sampler_state.repeat_u = RD::SAMPLER_REPEAT_MODE_CLAMP_TO_EDGE; - src_sampler_state.repeat_v = RD::SAMPLER_REPEAT_MODE_CLAMP_TO_EDGE; - src_sampler_state.mag_filter = RD::SAMPLER_FILTER_NEAREST; - src_sampler_state.min_filter = RD::SAMPLER_FILTER_NEAREST; - src_sampler_state.mip_filter = RD::SAMPLER_FILTER_NEAREST; - } - - RID src_sampler = rd->sampler_create(src_sampler_state); - // For the destination format just copy the source format and change the usage bits. RD::TextureFormat dst_texture_format = src_texture_format; dst_texture_format.usage_bits = RD::TEXTURE_USAGE_COLOR_ATTACHMENT_BIT | RD::TEXTURE_USAGE_STORAGE_BIT | RD::TEXTURE_USAGE_CAN_COPY_FROM_BIT | RD::TEXTURE_USAGE_CAN_COPY_TO_BIT | RD::TEXTURE_USAGE_CAN_UPDATE_BIT; - dst_texture_format.format = RD::DATA_FORMAT_R32G32B32A32_UINT; + dst_texture_format.format = dst_rd_format; + + // Encoding table setup. + if (dest_format == Image::FORMAT_DXT1 && dxt1_encoding_table_buffer.is_null()) { + Vector<uint8_t> data; + data.resize(1024 * 4); + memcpy(data.ptrw(), dxt1_encoding_table, 1024 * 4); + + dxt1_encoding_table_buffer = compress_rd->storage_buffer_create(1024 * 4, data); + } const int mip_count = r_img->get_mipmap_count() + 1; @@ -256,8 +394,41 @@ Error _compress_betsy(BetsyFormat p_format, Image *r_img) { memcpy(src_image_ptr[0].ptrw(), r_img->ptr() + ofs, size); // Create the textures on the GPU. - RID src_texture = rd->texture_create(src_texture_format, RD::TextureView(), src_images); - RID dst_texture = rd->texture_create(dst_texture_format, RD::TextureView()); + RID src_texture = compress_rd->texture_create(src_texture_format, RD::TextureView(), src_images); + RID dst_texture = compress_rd->texture_create(dst_texture_format, RD::TextureView()); + + Vector<RD::Uniform> uniforms; + { + { + RD::Uniform u; + u.uniform_type = RD::UNIFORM_TYPE_SAMPLER_WITH_TEXTURE; + u.binding = 0; + u.append_id(src_sampler); + u.append_id(src_texture); + uniforms.push_back(u); + } + { + RD::Uniform u; + u.uniform_type = RD::UNIFORM_TYPE_IMAGE; + u.binding = 1; + u.append_id(dst_texture); + uniforms.push_back(u); + } + + if (dest_format == Image::FORMAT_DXT1) { + RD::Uniform u; + u.uniform_type = RD::UNIFORM_TYPE_STORAGE_BUFFER; + u.binding = 2; + u.append_id(dxt1_encoding_table_buffer); + uniforms.push_back(u); + } + } + + RID uniform_set = compress_rd->uniform_set_create(uniforms, shader.compiled, 0); + RD::ComputeListID compute_list = compress_rd->compute_list_begin(); + + compress_rd->compute_list_bind_compute_pipeline(compute_list, shader.pipeline); + compress_rd->compute_list_bind_uniform_set(compute_list, uniform_set, 0); if (dest_format == Image::FORMAT_BPTC_RGBFU || dest_format == Image::FORMAT_BPTC_RGBF) { BC6PushConstant push_constant; @@ -266,47 +437,33 @@ Error _compress_betsy(BetsyFormat p_format, Image *r_img) { push_constant.padding[0] = 0; push_constant.padding[1] = 0; - Vector<RD::Uniform> uniforms; - { - { - RD::Uniform u; - u.uniform_type = RD::UNIFORM_TYPE_SAMPLER_WITH_TEXTURE; - u.binding = 0; - u.append_id(src_sampler); - u.append_id(src_texture); - uniforms.push_back(u); - } - { - RD::Uniform u; - u.uniform_type = RD::UNIFORM_TYPE_IMAGE; - u.binding = 1; - u.append_id(dst_texture); - uniforms.push_back(u); - } - } + compress_rd->compute_list_set_push_constant(compute_list, &push_constant, sizeof(BC6PushConstant)); - RID uniform_set = rd->uniform_set_create(uniforms, shader, 0); - RD::ComputeListID compute_list = rd->compute_list_begin(); + } else { + BC1PushConstant push_constant; + push_constant.num_refines = 2; + push_constant.padding[0] = 0; + push_constant.padding[1] = 0; + push_constant.padding[2] = 0; - rd->compute_list_bind_compute_pipeline(compute_list, pipeline); - rd->compute_list_bind_uniform_set(compute_list, uniform_set, 0); - rd->compute_list_set_push_constant(compute_list, &push_constant, sizeof(BC6PushConstant)); - rd->compute_list_dispatch(compute_list, get_next_multiple(width, 32) / 32, get_next_multiple(height, 32) / 32, 1); - rd->compute_list_end(); + compress_rd->compute_list_set_push_constant(compute_list, &push_constant, sizeof(BC1PushConstant)); } - rd->submit(); - rd->sync(); + compress_rd->compute_list_dispatch(compute_list, get_next_multiple(width, 32) / 32, get_next_multiple(height, 32) / 32, 1); + compress_rd->compute_list_end(); + + compress_rd->submit(); + compress_rd->sync(); // Copy data from the GPU to the buffer. - const Vector<uint8_t> texture_data = rd->texture_get_data(dst_texture, 0); + const Vector<uint8_t> texture_data = compress_rd->texture_get_data(dst_texture, 0); int64_t dst_ofs = Image::get_image_mipmap_offset(r_img->get_width(), r_img->get_height(), dest_format, i); memcpy(dst_data_ptr + dst_ofs, texture_data.ptr(), texture_data.size()); // Free the source and dest texture. - rd->free(dst_texture); - rd->free(src_texture); + compress_rd->free(dst_texture); + compress_rd->free(src_texture); } src_images.clear(); @@ -314,26 +471,67 @@ Error _compress_betsy(BetsyFormat p_format, Image *r_img) { // Set the compressed data to the image. r_img->set_data(r_img->get_width(), r_img->get_height(), r_img->has_mipmaps(), dest_format, dst_data); - // Free the shader (dependencies will be cleared automatically). - rd->free(src_sampler); - rd->free(shader); - - memdelete(rd); - if (rcd != nullptr) { - memdelete(rcd); - } - print_verbose(vformat("Betsy: Encoding took %d ms.", OS::get_singleton()->get_ticks_msec() - start_time)); return OK; } +void ensure_betsy_exists() { + betsy_mutex.lock(); + if (betsy == nullptr) { + betsy = memnew(BetsyCompressor); + betsy->init(); + } + betsy_mutex.unlock(); +} + Error _betsy_compress_bptc(Image *r_img, Image::UsedChannels p_channels) { + ensure_betsy_exists(); Image::Format format = r_img->get_format(); + Error result = ERR_UNAVAILABLE; if (format >= Image::FORMAT_RF && format <= Image::FORMAT_RGBE9995) { - return _compress_betsy(BETSY_FORMAT_BC6, r_img); + if (r_img->detect_signed()) { + result = betsy->compress(BETSY_FORMAT_BC6_SIGNED, r_img); + } else { + result = betsy->compress(BETSY_FORMAT_BC6_UNSIGNED, r_img); + } + } + + if (!GLOBAL_GET("rendering/textures/vram_compression/cache_gpu_compressor")) { + free_device(); + } + + return result; +} + +Error _betsy_compress_s3tc(Image *r_img, Image::UsedChannels p_channels) { + ensure_betsy_exists(); + Error result = ERR_UNAVAILABLE; + + switch (p_channels) { + case Image::USED_CHANNELS_RGB: + result = betsy->compress(BETSY_FORMAT_BC1_DITHER, r_img); + break; + + case Image::USED_CHANNELS_L: + result = betsy->compress(BETSY_FORMAT_BC1, r_img); + break; + + default: + break; } - return ERR_UNAVAILABLE; + if (!GLOBAL_GET("rendering/textures/vram_compression/cache_gpu_compressor")) { + free_device(); + } + + return result; +} + +void free_device() { + if (betsy != nullptr) { + betsy->finish(); + memdelete(betsy); + } } diff --git a/modules/betsy/image_compress_betsy.h b/modules/betsy/image_compress_betsy.h index a64e586c76..70e4ae85ed 100644 --- a/modules/betsy/image_compress_betsy.h +++ b/modules/betsy/image_compress_betsy.h @@ -32,13 +32,79 @@ #define IMAGE_COMPRESS_BETSY_H #include "core/io/image.h" +#include "core/object/worker_thread_pool.h" +#include "core/os/thread.h" +#include "core/templates/command_queue_mt.h" + +#include "servers/rendering/rendering_device_binds.h" +#include "servers/rendering/rendering_server_default.h" + +#if defined(VULKAN_ENABLED) +#include "drivers/vulkan/rendering_context_driver_vulkan.h" +#endif +#if defined(METAL_ENABLED) +#include "drivers/metal/rendering_context_driver_metal.h" +#endif enum BetsyFormat { - BETSY_FORMAT_BC6, + BETSY_FORMAT_BC1, + BETSY_FORMAT_BC1_DITHER, + BETSY_FORMAT_BC3, + BETSY_FORMAT_BC6_SIGNED, + BETSY_FORMAT_BC6_UNSIGNED, +}; + +struct BC6PushConstant { + float sizeX; + float sizeY; + uint32_t padding[2]; }; -Error _compress_betsy(BetsyFormat p_format, Image *r_img); +struct BC1PushConstant { + uint32_t num_refines; + uint32_t padding[3]; +}; + +void free_device(); Error _betsy_compress_bptc(Image *r_img, Image::UsedChannels p_channels); +Error _betsy_compress_s3tc(Image *r_img, Image::UsedChannels p_channels); + +class BetsyCompressor : public Object { + mutable CommandQueueMT command_queue; + bool exit = false; + WorkerThreadPool::TaskID task_id = WorkerThreadPool::INVALID_TASK_ID; + + struct BetsyShader { + RID compiled; + RID pipeline; + }; + + // Resources shared by all compression formats. + RenderingDevice *compress_rd = nullptr; + RenderingContextDriver *compress_rcd = nullptr; + HashMap<String, BetsyShader> cached_shaders; + RID src_sampler = RID(); + + // Format-specific resources. + RID dxt1_encoding_table_buffer = RID(); + + void _init(); + void _assign_mt_ids(WorkerThreadPool::TaskID p_pump_task_id); + void _thread_loop(); + void _thread_exit(); + + Error _compress(BetsyFormat p_format, Image *r_img); + +public: + void init(); + void finish(); + + Error compress(BetsyFormat p_format, Image *r_img) { + Error err; + command_queue.push_and_ret(this, &BetsyCompressor::_compress, p_format, r_img, &err); + return err; + } +}; #endif // IMAGE_COMPRESS_BETSY_H diff --git a/modules/betsy/register_types.cpp b/modules/betsy/register_types.cpp index 019099e67c..a3a3b5a99b 100644 --- a/modules/betsy/register_types.cpp +++ b/modules/betsy/register_types.cpp @@ -38,10 +38,13 @@ void initialize_betsy_module(ModuleInitializationLevel p_level) { } Image::_image_compress_bptc_rd_func = _betsy_compress_bptc; + Image::_image_compress_bc_rd_func = _betsy_compress_s3tc; } void uninitialize_betsy_module(ModuleInitializationLevel p_level) { if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) { return; } + + free_device(); } diff --git a/modules/bmp/image_loader_bmp.cpp b/modules/bmp/image_loader_bmp.cpp index 72b540496d..1804d73a69 100644 --- a/modules/bmp/image_loader_bmp.cpp +++ b/modules/bmp/image_loader_bmp.cpp @@ -59,30 +59,6 @@ Error ImageLoaderBMP::convert_to_image(Ref<Image> p_image, size_t height = (size_t)p_header.bmp_info_header.bmp_height; size_t bits_per_pixel = (size_t)p_header.bmp_info_header.bmp_bit_count; - // Check whether we can load it - - if (bits_per_pixel == 1) { - // Requires bit unpacking... - ERR_FAIL_COND_V_MSG(width % 8 != 0, ERR_UNAVAILABLE, - vformat("1-bpp BMP images must have a width that is a multiple of 8, but the imported BMP is %d pixels wide.", int(width))); - ERR_FAIL_COND_V_MSG(height % 8 != 0, ERR_UNAVAILABLE, - vformat("1-bpp BMP images must have a height that is a multiple of 8, but the imported BMP is %d pixels tall.", int(height))); - - } else if (bits_per_pixel == 2) { - // Requires bit unpacking... - ERR_FAIL_COND_V_MSG(width % 4 != 0, ERR_UNAVAILABLE, - vformat("2-bpp BMP images must have a width that is a multiple of 4, but the imported BMP is %d pixels wide.", int(width))); - ERR_FAIL_COND_V_MSG(height % 4 != 0, ERR_UNAVAILABLE, - vformat("2-bpp BMP images must have a height that is a multiple of 4, but the imported BMP is %d pixels tall.", int(height))); - - } else if (bits_per_pixel == 4) { - // Requires bit unpacking... - ERR_FAIL_COND_V_MSG(width % 2 != 0, ERR_UNAVAILABLE, - vformat("4-bpp BMP images must have a width that is a multiple of 2, but the imported BMP is %d pixels wide.", int(width))); - ERR_FAIL_COND_V_MSG(height % 2 != 0, ERR_UNAVAILABLE, - vformat("4-bpp BMP images must have a height that is a multiple of 2, but the imported BMP is %d pixels tall.", int(height))); - } - // Image data (might be indexed) Vector<uint8_t> data; int data_len = 0; @@ -98,55 +74,32 @@ Error ImageLoaderBMP::convert_to_image(Ref<Image> p_image, uint8_t *data_w = data.ptrw(); uint8_t *write_buffer = data_w; - const uint32_t width_bytes = width * bits_per_pixel / 8; - const uint32_t line_width = (width_bytes + 3) & ~3; + const uint32_t width_bytes = (width * bits_per_pixel + 7) / 8; + const uint32_t line_width = (width_bytes + 3) & ~3; // Padded to 4 bytes. - // The actual data traversal is determined by - // the data width in case of 8/4/2/1 bit images - const uint32_t w = bits_per_pixel >= 16 ? width : width_bytes; const uint8_t *line = p_buffer + (line_width * (height - 1)); const uint8_t *end_buffer = p_buffer + p_header.bmp_file_header.bmp_file_size - p_header.bmp_file_header.bmp_file_offset; + ERR_FAIL_COND_V(line + line_width > end_buffer, ERR_FILE_CORRUPT); for (uint64_t i = 0; i < height; i++) { const uint8_t *line_ptr = line; - for (unsigned int j = 0; j < w; j++) { - ERR_FAIL_COND_V(line_ptr >= end_buffer, ERR_FILE_CORRUPT); + for (unsigned int j = 0; j < width; j++) { switch (bits_per_pixel) { case 1: { - uint8_t color_index = *line_ptr; + write_buffer[index] = (line[(j * bits_per_pixel) / 8] >> (8 - bits_per_pixel * (1 + j % 8))) & 0x01; - write_buffer[index + 0] = (color_index >> 7) & 1; - write_buffer[index + 1] = (color_index >> 6) & 1; - write_buffer[index + 2] = (color_index >> 5) & 1; - write_buffer[index + 3] = (color_index >> 4) & 1; - write_buffer[index + 4] = (color_index >> 3) & 1; - write_buffer[index + 5] = (color_index >> 2) & 1; - write_buffer[index + 6] = (color_index >> 1) & 1; - write_buffer[index + 7] = (color_index >> 0) & 1; - - index += 8; - line_ptr += 1; + index++; } break; case 2: { - uint8_t color_index = *line_ptr; + write_buffer[index] = (line[(j * bits_per_pixel) / 8] >> (8 - bits_per_pixel * (1 + j % 4))) & 0x03; - write_buffer[index + 0] = (color_index >> 6) & 3; - write_buffer[index + 1] = (color_index >> 4) & 3; - write_buffer[index + 2] = (color_index >> 2) & 3; - write_buffer[index + 3] = color_index & 3; - - index += 4; - line_ptr += 1; + index++; } break; case 4: { - uint8_t color_index = *line_ptr; - - write_buffer[index + 0] = (color_index >> 4) & 0x0f; - write_buffer[index + 1] = color_index & 0x0f; + write_buffer[index] = (line[(j * bits_per_pixel) / 8] >> (8 - bits_per_pixel * (1 + j % 2))) & 0x0f; - index += 2; - line_ptr += 1; + index++; } break; case 8: { uint8_t color_index = *line_ptr; diff --git a/modules/gdscript/.editorconfig b/modules/gdscript/.editorconfig index 640c205093..b380846f86 100644 --- a/modules/gdscript/.editorconfig +++ b/modules/gdscript/.editorconfig @@ -1,8 +1,3 @@ [*.gd] -indent_style = tab indent_size = 4 -insert_final_newline = true trim_trailing_whitespace = true - -[*.out] -insert_final_newline = true diff --git a/modules/gdscript/gdscript.cpp b/modules/gdscript/gdscript.cpp index 276a12f5de..7b9aa70686 100644 --- a/modules/gdscript/gdscript.cpp +++ b/modules/gdscript/gdscript.cpp @@ -880,6 +880,11 @@ Error GDScript::reload(bool p_keep_state) { if (can_run && p_keep_state) { _restore_old_static_data(); } + + if (p_keep_state) { + // Update the properties in the inspector. + update_exports(); + } #endif reloading = false; @@ -906,7 +911,7 @@ void GDScript::get_members(HashSet<StringName> *p_members) { } } -const Variant GDScript::get_rpc_config() const { +Variant GDScript::get_rpc_config() const { return rpc_config; } diff --git a/modules/gdscript/gdscript.h b/modules/gdscript/gdscript.h index 4d21651365..9bb39aac0f 100644 --- a/modules/gdscript/gdscript.h +++ b/modules/gdscript/gdscript.h @@ -334,7 +334,7 @@ public: virtual void get_constants(HashMap<StringName, Variant> *p_constants) override; virtual void get_members(HashSet<StringName> *p_members) override; - virtual const Variant get_rpc_config() const override; + virtual Variant get_rpc_config() const override; void unload_static() const; diff --git a/modules/gdscript/gdscript_lambda_callable.cpp b/modules/gdscript/gdscript_lambda_callable.cpp index 2de5811bca..d6fd5d043b 100644 --- a/modules/gdscript/gdscript_lambda_callable.cpp +++ b/modules/gdscript/gdscript_lambda_callable.cpp @@ -97,25 +97,25 @@ void GDScriptLambdaCallable::call(const Variant **p_arguments, int p_argcount, V } if (captures_amount > 0) { - Vector<const Variant *> args; - args.resize(p_argcount + captures_amount); + const int total_argcount = p_argcount + captures_amount; + const Variant **args = (const Variant **)alloca(sizeof(Variant *) * total_argcount); for (int i = 0; i < captures_amount; i++) { - args.write[i] = &captures[i]; + args[i] = &captures[i]; if (captures[i].get_type() == Variant::OBJECT) { bool was_freed = false; captures[i].get_validated_object_with_check(was_freed); if (was_freed) { ERR_PRINT(vformat(R"(Lambda capture at index %d was freed. Passed "null" instead.)", i)); static Variant nil; - args.write[i] = &nil; + args[i] = &nil; } } } for (int i = 0; i < p_argcount; i++) { - args.write[i + captures_amount] = p_arguments[i]; + args[i + captures_amount] = p_arguments[i]; } - r_return_value = function->call(nullptr, args.ptrw(), args.size(), r_call_error); + r_return_value = function->call(nullptr, args, total_argcount, r_call_error); switch (r_call_error.error) { case Callable::CallError::CALL_ERROR_INVALID_ARGUMENT: r_call_error.argument -= captures_amount; @@ -229,25 +229,25 @@ void GDScriptLambdaSelfCallable::call(const Variant **p_arguments, int p_argcoun } if (captures_amount > 0) { - Vector<const Variant *> args; - args.resize(p_argcount + captures_amount); + const int total_argcount = p_argcount + captures_amount; + const Variant **args = (const Variant **)alloca(sizeof(Variant *) * total_argcount); for (int i = 0; i < captures_amount; i++) { - args.write[i] = &captures[i]; + args[i] = &captures[i]; if (captures[i].get_type() == Variant::OBJECT) { bool was_freed = false; captures[i].get_validated_object_with_check(was_freed); if (was_freed) { ERR_PRINT(vformat(R"(Lambda capture at index %d was freed. Passed "null" instead.)", i)); static Variant nil; - args.write[i] = &nil; + args[i] = &nil; } } } for (int i = 0; i < p_argcount; i++) { - args.write[i + captures_amount] = p_arguments[i]; + args[i + captures_amount] = p_arguments[i]; } - r_return_value = function->call(static_cast<GDScriptInstance *>(object->get_script_instance()), args.ptrw(), args.size(), r_call_error); + r_return_value = function->call(static_cast<GDScriptInstance *>(object->get_script_instance()), args, total_argcount, r_call_error); switch (r_call_error.error) { case Callable::CallError::CALL_ERROR_INVALID_ARGUMENT: r_call_error.argument -= captures_amount; diff --git a/modules/gdscript/language_server/gdscript_extend_parser.h b/modules/gdscript/language_server/gdscript_extend_parser.h index a808f19e5b..239f7d9f43 100644 --- a/modules/gdscript/language_server/gdscript_extend_parser.h +++ b/modules/gdscript/language_server/gdscript_extend_parser.h @@ -37,10 +37,10 @@ #include "core/variant/variant.h" #ifndef LINE_NUMBER_TO_INDEX -#define LINE_NUMBER_TO_INDEX(p_line) ((p_line)-1) +#define LINE_NUMBER_TO_INDEX(p_line) ((p_line) - 1) #endif #ifndef COLUMN_NUMBER_TO_INDEX -#define COLUMN_NUMBER_TO_INDEX(p_column) ((p_column)-1) +#define COLUMN_NUMBER_TO_INDEX(p_column) ((p_column) - 1) #endif #ifndef SYMBOL_SEPERATOR diff --git a/modules/gdscript/language_server/gdscript_text_document.cpp b/modules/gdscript/language_server/gdscript_text_document.cpp index fa5f279db9..06e9775360 100644 --- a/modules/gdscript/language_server/gdscript_text_document.cpp +++ b/modules/gdscript/language_server/gdscript_text_document.cpp @@ -229,19 +229,6 @@ Array GDScriptTextDocument::completion(const Dictionary &p_params) { arr[i] = item.to_json(); i++; } - } else if (GDScriptLanguageProtocol::get_singleton()->is_smart_resolve_enabled()) { - arr = native_member_completions.duplicate(); - - for (KeyValue<String, ExtendGDScriptParser *> &E : GDScriptLanguageProtocol::get_singleton()->get_workspace()->scripts) { - ExtendGDScriptParser *scr = E.value; - const Array &items = scr->get_member_completions(); - - const int start_size = arr.size(); - arr.resize(start_size + items.size()); - for (int i = start_size; i < arr.size(); i++) { - arr[i] = items[i - start_size]; - } - } } return arr; } @@ -485,8 +472,6 @@ GDScriptTextDocument::GDScriptTextDocument() { void GDScriptTextDocument::sync_script_content(const String &p_path, const String &p_content) { String path = GDScriptLanguageProtocol::get_singleton()->get_workspace()->get_file_path(p_path); GDScriptLanguageProtocol::get_singleton()->get_workspace()->parse_script(path, p_content); - - EditorFileSystem::get_singleton()->update_file(path); } void GDScriptTextDocument::show_native_symbol_in_editor(const String &p_symbol_id) { diff --git a/modules/gdscript/language_server/godot_lsp.h b/modules/gdscript/language_server/godot_lsp.h index bdf339f5fe..6e19cd7a23 100644 --- a/modules/gdscript/language_server/godot_lsp.h +++ b/modules/gdscript/language_server/godot_lsp.h @@ -958,28 +958,30 @@ struct CompletionItem { /** * A string that should be used when comparing this item - * with other items. When `falsy` the label is used. + * with other items. When omitted the label is used + * as the filter text for this item. */ String sortText; /** * A string that should be used when filtering a set of - * completion items. When `falsy` the label is used. + * completion items. When omitted the label is used as the + * filter text for this item. */ String filterText; /** * A string that should be inserted into a document when selecting - * this completion. When `falsy` the label is used. + * this completion. When omitted the label is used as the insert text + * for this item. * * The `insertText` is subject to interpretation by the client side. * Some tools might not take the string literally. For example - * VS Code when code complete is requested in this example `con<cursor position>` - * and a completion item with an `insertText` of `console` is provided it - * will only insert `sole`. Therefore it is recommended to use `textEdit` instead - * since it avoids additional client side interpretation. - * - * @deprecated Use textEdit instead. + * VS Code when code complete is requested in this example + * `con<cursor position>` and a completion item with an `insertText` of + * `console` is provided it will only insert `sole`. Therefore it is + * recommended to use `textEdit` instead since it avoids additional client + * side interpretation. */ String insertText; @@ -1034,14 +1036,20 @@ struct CompletionItem { dict["label"] = label; dict["kind"] = kind; dict["data"] = data; - dict["insertText"] = insertText; + if (!insertText.is_empty()) { + dict["insertText"] = insertText; + } if (resolved) { dict["detail"] = detail; dict["documentation"] = documentation.to_json(); dict["deprecated"] = deprecated; dict["preselect"] = preselect; - dict["sortText"] = sortText; - dict["filterText"] = filterText; + if (!sortText.is_empty()) { + dict["sortText"] = sortText; + } + if (!filterText.is_empty()) { + dict["filterText"] = filterText; + } if (commitCharacters.size()) { dict["commitCharacters"] = commitCharacters; } diff --git a/modules/gdscript/tests/scripts/.editorconfig b/modules/gdscript/tests/scripts/.editorconfig new file mode 100644 index 0000000000..da1efefe3c --- /dev/null +++ b/modules/gdscript/tests/scripts/.editorconfig @@ -0,0 +1,12 @@ +# This file is required to workaround `.editorconfig` autogeneration (see #96845). + +# Some tests handle invalid syntax deliberately; exclude relevant attributes. + +[parser/features/mixed_indentation_on_blank_lines.gd] +trim_trailing_whitespace = false + +[parser/warnings/empty_file_newline.notest.gd] +insert_final_newline = false + +[parser/warnings/empty_file_newline_comment.notest.gd] +insert_final_newline = false diff --git a/modules/gdscript/tests/scripts/parser/.editorconfig b/modules/gdscript/tests/scripts/parser/.editorconfig deleted file mode 100644 index fa43b3ad78..0000000000 --- a/modules/gdscript/tests/scripts/parser/.editorconfig +++ /dev/null @@ -1,2 +0,0 @@ -[*.{gd,out}] -trim_trailing_whitespace = false diff --git a/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_key.gd b/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_key.gd new file mode 100644 index 0000000000..2f0b3bd0eb --- /dev/null +++ b/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_key.gd @@ -0,0 +1,7 @@ +func get_key() -> Variant: + return "key" + +func test(): + var typed: Dictionary[int, int] + typed[get_key()] = 0 + print('not ok') diff --git a/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_key.out b/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_key.out new file mode 100644 index 0000000000..5f6dd7f641 --- /dev/null +++ b/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_key.out @@ -0,0 +1,6 @@ +GDTEST_RUNTIME_ERROR +>> SCRIPT ERROR +>> on function: test() +>> runtime/errors/typed_dictionary_assign_differently_typed_key.gd +>> 6 +>> Invalid assignment of property or key 'key' with value of type 'int' on a base object of type 'Dictionary[int, int]'. diff --git a/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_value.gd b/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_value.gd new file mode 100644 index 0000000000..b171159aed --- /dev/null +++ b/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_value.gd @@ -0,0 +1,7 @@ +func get_value() -> Variant: + return "value" + +func test(): + var typed: Dictionary[int, int] + typed[0] = get_value() + print("not ok") diff --git a/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_value.out b/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_value.out new file mode 100644 index 0000000000..f766d14261 --- /dev/null +++ b/modules/gdscript/tests/scripts/runtime/errors/typed_dictionary_assign_differently_typed_value.out @@ -0,0 +1,6 @@ +GDTEST_RUNTIME_ERROR +>> SCRIPT ERROR +>> on function: test() +>> runtime/errors/typed_dictionary_assign_differently_typed_value.gd +>> 6 +>> Invalid assignment of property or key '0' with value of type 'String' on a base object of type 'Dictionary[int, int]'. diff --git a/modules/gdscript/tests/scripts/runtime/features/self_destruction.gd b/modules/gdscript/tests/scripts/runtime/features/self_destruction.gd new file mode 100644 index 0000000000..442335faeb --- /dev/null +++ b/modules/gdscript/tests/scripts/runtime/features/self_destruction.gd @@ -0,0 +1,50 @@ +# https://github.com/godotengine/godot/issues/75658 + +class MyObj: + var callable: Callable + + func run(): + callable.call() + + var prop: + set(value): + callable.call() + get: + callable.call() + return 0 + + func _on_some_signal(): + callable.call() + + func _init(p_callable: Callable): + self.callable = p_callable + +signal some_signal + +var obj: MyObj + +func test(): + # Call. + obj = MyObj.new(nullify_obj) + obj.run() + print(obj) + + # Get. + obj = MyObj.new(nullify_obj) + var _aux = obj.prop + print(obj) + + # Set. + obj = MyObj.new(nullify_obj) + obj.prop = 1 + print(obj) + + # Signal handling. + obj = MyObj.new(nullify_obj) + @warning_ignore("return_value_discarded") + some_signal.connect(obj._on_some_signal) + some_signal.emit() + print(obj) + +func nullify_obj(): + obj = null diff --git a/modules/gdscript/tests/scripts/runtime/features/self_destruction.out b/modules/gdscript/tests/scripts/runtime/features/self_destruction.out new file mode 100644 index 0000000000..ee4024a524 --- /dev/null +++ b/modules/gdscript/tests/scripts/runtime/features/self_destruction.out @@ -0,0 +1,5 @@ +GDTEST_OK +<null> +<null> +<null> +<null> diff --git a/modules/gltf/doc_classes/GLTFDocumentExtension.xml b/modules/gltf/doc_classes/GLTFDocumentExtension.xml index 5c548c472f..b33e296e1c 100644 --- a/modules/gltf/doc_classes/GLTFDocumentExtension.xml +++ b/modules/gltf/doc_classes/GLTFDocumentExtension.xml @@ -18,7 +18,7 @@ <param index="1" name="gltf_node" type="GLTFNode" /> <param index="2" name="scene_node" type="Node" /> <description> - Part of the export process. This method is run after [method _export_preflight] and before [method _export_preserialize]. + Part of the export process. This method is run after [method _export_preflight] and before [method _export_post_convert]. Runs when converting the data from a Godot scene node. This method can be used to process the Godot scene node data into a format that can be used by [method _export_node]. </description> </method> @@ -41,6 +41,15 @@ This method can be used to modify the final JSON of the generated glTF file. </description> </method> + <method name="_export_post_convert" qualifiers="virtual"> + <return type="int" enum="Error" /> + <param index="0" name="state" type="GLTFState" /> + <param index="1" name="root" type="Node" /> + <description> + Part of the export process. This method is run after [method _convert_scene_node] and before [method _export_preserialize]. + This method can be used to modify the converted node data structures before serialization with any additional data from the scene tree. + </description> + </method> <method name="_export_preflight" qualifiers="virtual"> <return type="int" enum="Error" /> <param index="0" name="state" type="GLTFState" /> @@ -54,7 +63,7 @@ <return type="int" enum="Error" /> <param index="0" name="state" type="GLTFState" /> <description> - Part of the export process. This method is run after [method _convert_scene_node] and before [method _get_saveable_image_formats]. + Part of the export process. This method is run after [method _export_post_convert] and before [method _get_saveable_image_formats]. This method can be used to alter the state before performing serialization. It runs every time when generating a buffer with [method GLTFDocument.generate_buffer] or writing to the file system with [method GLTFDocument.write_to_filesystem]. </description> </method> @@ -64,7 +73,7 @@ <param index="1" name="gltf_node" type="GLTFNode" /> <param index="2" name="scene_parent" type="Node" /> <description> - Part of the import process. This method is run after [method _import_post_parse] and before [method _import_node]. + Part of the import process. This method is run after [method _import_pre_generate] and before [method _import_node]. Runs when generating a Godot scene node from a GLTFNode. The returned node will be added to the scene tree. Multiple nodes can be generated in this step if they are added as a child of the returned node. [b]Note:[/b] The [param scene_parent] parameter may be null if this is the single root node. </description> @@ -113,8 +122,16 @@ <return type="int" enum="Error" /> <param index="0" name="state" type="GLTFState" /> <description> - Part of the import process. This method is run after [method _parse_node_extensions] and before [method _generate_scene_node]. - This method can be used to modify any of the data imported so far after parsing, before generating the nodes and then running the final per-node import step. + Part of the import process. This method is run after [method _parse_node_extensions] and before [method _import_pre_generate]. + This method can be used to modify any of the data imported so far after parsing each node, but before generating the scene or any of its nodes. + </description> + </method> + <method name="_import_pre_generate" qualifiers="virtual"> + <return type="int" enum="Error" /> + <param index="0" name="state" type="GLTFState" /> + <description> + Part of the import process. This method is run after [method _import_post_parse] and before [method _generate_scene_node]. + This method can be used to modify or read from any of the processed data structures, before generating the nodes and then running the final per-node import step. </description> </method> <method name="_import_preflight" qualifiers="virtual"> diff --git a/modules/gltf/doc_classes/GLTFNode.xml b/modules/gltf/doc_classes/GLTFNode.xml index 2786c25e9a..a242a0d1d8 100644 --- a/modules/gltf/doc_classes/GLTFNode.xml +++ b/modules/gltf/doc_classes/GLTFNode.xml @@ -12,6 +12,13 @@ <link title="glTF scene and node spec">https://github.com/KhronosGroup/glTF-Tutorials/blob/master/gltfTutorial/gltfTutorial_004_ScenesNodes.md"</link> </tutorials> <methods> + <method name="append_child_index"> + <return type="void" /> + <param index="0" name="child_index" type="int" /> + <description> + Appends the given child node index to the [member children] array. + </description> + </method> <method name="get_additional_data"> <return type="Variant" /> <param index="0" name="extension_name" type="StringName" /> diff --git a/modules/gltf/doc_classes/GLTFState.xml b/modules/gltf/doc_classes/GLTFState.xml index c049acf557..de7ec2a4ca 100644 --- a/modules/gltf/doc_classes/GLTFState.xml +++ b/modules/gltf/doc_classes/GLTFState.xml @@ -28,6 +28,17 @@ Appends the given byte array data to the buffers and creates a [GLTFBufferView] for it. The index of the destination [GLTFBufferView] is returned. If [param deduplication] is true, the buffers will first be searched for duplicate data, otherwise new bytes will always be appended. </description> </method> + <method name="append_gltf_node"> + <return type="int" /> + <param index="0" name="gltf_node" type="GLTFNode" /> + <param index="1" name="godot_scene_node" type="Node" /> + <param index="2" name="parent_node_index" type="int" /> + <description> + Append the given [GLTFNode] to the state, and return its new index. This can be used to export one Godot node as multiple glTF nodes, or inject new glTF nodes at import time. On import, this must be called before [method GLTFDocumentExtension._generate_scene_node] finishes for the parent node. On export, this must be called before [method GLTFDocumentExtension._export_node] runs for the parent node. + The [param godot_scene_node] parameter is the Godot scene node that corresponds to this glTF node. This is highly recommended to be set to a valid node, but may be null if there is no corresponding Godot scene node. One Godot scene node may be used for multiple glTF nodes, so if exporting multiple glTF nodes for one Godot scene node, use the same Godot scene node for each. + The [param parent_node_index] parameter is the index of the parent [GLTFNode] in the state. If [code]-1[/code], the node will be a root node, otherwise the new node will be added to the parent's list of children. The index will also be written to the [member GLTFNode.parent] property of the new node. + </description> + </method> <method name="get_accessors"> <return type="GLTFAccessor[]" /> <description> diff --git a/modules/gltf/extensions/gltf_document_extension.cpp b/modules/gltf/extensions/gltf_document_extension.cpp index c6540ebb22..6e611762b6 100644 --- a/modules/gltf/extensions/gltf_document_extension.cpp +++ b/modules/gltf/extensions/gltf_document_extension.cpp @@ -38,13 +38,15 @@ void GLTFDocumentExtension::_bind_methods() { GDVIRTUAL_BIND(_parse_image_data, "state", "image_data", "mime_type", "ret_image"); GDVIRTUAL_BIND(_get_image_file_extension); GDVIRTUAL_BIND(_parse_texture_json, "state", "texture_json", "ret_gltf_texture"); - GDVIRTUAL_BIND(_generate_scene_node, "state", "gltf_node", "scene_parent"); GDVIRTUAL_BIND(_import_post_parse, "state"); + GDVIRTUAL_BIND(_import_pre_generate, "state"); + GDVIRTUAL_BIND(_generate_scene_node, "state", "gltf_node", "scene_parent"); GDVIRTUAL_BIND(_import_node, "state", "gltf_node", "json", "node"); GDVIRTUAL_BIND(_import_post, "state", "root"); // Export process. GDVIRTUAL_BIND(_export_preflight, "state", "root"); GDVIRTUAL_BIND(_convert_scene_node, "state", "gltf_node", "scene_node"); + GDVIRTUAL_BIND(_export_post_convert, "state", "root"); GDVIRTUAL_BIND(_export_preserialize, "state"); GDVIRTUAL_BIND(_get_saveable_image_formats); GDVIRTUAL_BIND(_serialize_image_to_bytes, "state", "image", "image_dict", "image_format", "lossy_quality"); @@ -98,6 +100,20 @@ Error GLTFDocumentExtension::parse_texture_json(Ref<GLTFState> p_state, const Di return err; } +Error GLTFDocumentExtension::import_post_parse(Ref<GLTFState> p_state) { + ERR_FAIL_COND_V(p_state.is_null(), ERR_INVALID_PARAMETER); + Error err = OK; + GDVIRTUAL_CALL(_import_post_parse, p_state, err); + return err; +} + +Error GLTFDocumentExtension::import_pre_generate(Ref<GLTFState> p_state) { + ERR_FAIL_COND_V(p_state.is_null(), ERR_INVALID_PARAMETER); + Error err = OK; + GDVIRTUAL_CALL(_import_pre_generate, p_state, err); + return err; +} + Node3D *GLTFDocumentExtension::generate_scene_node(Ref<GLTFState> p_state, Ref<GLTFNode> p_gltf_node, Node *p_scene_parent) { ERR_FAIL_COND_V(p_state.is_null(), nullptr); ERR_FAIL_COND_V(p_gltf_node.is_null(), nullptr); @@ -106,13 +122,6 @@ Node3D *GLTFDocumentExtension::generate_scene_node(Ref<GLTFState> p_state, Ref<G return ret_node; } -Error GLTFDocumentExtension::import_post_parse(Ref<GLTFState> p_state) { - ERR_FAIL_COND_V(p_state.is_null(), ERR_INVALID_PARAMETER); - Error err = OK; - GDVIRTUAL_CALL(_import_post_parse, p_state, err); - return err; -} - Error GLTFDocumentExtension::import_node(Ref<GLTFState> p_state, Ref<GLTFNode> p_gltf_node, Dictionary &r_dict, Node *p_node) { ERR_FAIL_COND_V(p_state.is_null(), ERR_INVALID_PARAMETER); ERR_FAIL_COND_V(p_gltf_node.is_null(), ERR_INVALID_PARAMETER); @@ -145,6 +154,14 @@ void GLTFDocumentExtension::convert_scene_node(Ref<GLTFState> p_state, Ref<GLTFN GDVIRTUAL_CALL(_convert_scene_node, p_state, p_gltf_node, p_scene_node); } +Error GLTFDocumentExtension::export_post_convert(Ref<GLTFState> p_state, Node *p_root) { + ERR_FAIL_COND_V(p_state.is_null(), ERR_INVALID_PARAMETER); + ERR_FAIL_NULL_V(p_root, ERR_INVALID_PARAMETER); + Error err = OK; + GDVIRTUAL_CALL(_export_post_convert, p_state, p_root, err); + return err; +} + Error GLTFDocumentExtension::export_preserialize(Ref<GLTFState> p_state) { ERR_FAIL_COND_V(p_state.is_null(), ERR_INVALID_PARAMETER); Error err = OK; diff --git a/modules/gltf/extensions/gltf_document_extension.h b/modules/gltf/extensions/gltf_document_extension.h index 761dff725c..b70710e015 100644 --- a/modules/gltf/extensions/gltf_document_extension.h +++ b/modules/gltf/extensions/gltf_document_extension.h @@ -50,12 +50,14 @@ public: virtual String get_image_file_extension(); virtual Error parse_texture_json(Ref<GLTFState> p_state, const Dictionary &p_texture_json, Ref<GLTFTexture> r_gltf_texture); virtual Error import_post_parse(Ref<GLTFState> p_state); + virtual Error import_pre_generate(Ref<GLTFState> p_state); virtual Node3D *generate_scene_node(Ref<GLTFState> p_state, Ref<GLTFNode> p_gltf_node, Node *p_scene_parent); virtual Error import_node(Ref<GLTFState> p_state, Ref<GLTFNode> p_gltf_node, Dictionary &r_json, Node *p_node); virtual Error import_post(Ref<GLTFState> p_state, Node *p_node); // Export process. virtual Error export_preflight(Ref<GLTFState> p_state, Node *p_root); virtual void convert_scene_node(Ref<GLTFState> p_state, Ref<GLTFNode> p_gltf_node, Node *p_scene_node); + virtual Error export_post_convert(Ref<GLTFState> p_state, Node *p_root); virtual Error export_preserialize(Ref<GLTFState> p_state); virtual Vector<String> get_saveable_image_formats(); virtual PackedByteArray serialize_image_to_bytes(Ref<GLTFState> p_state, Ref<Image> p_image, Dictionary p_image_dict, const String &p_image_format, float p_lossy_quality); @@ -71,13 +73,15 @@ public: GDVIRTUAL4R(Error, _parse_image_data, Ref<GLTFState>, PackedByteArray, String, Ref<Image>); GDVIRTUAL0R(String, _get_image_file_extension); GDVIRTUAL3R(Error, _parse_texture_json, Ref<GLTFState>, Dictionary, Ref<GLTFTexture>); - GDVIRTUAL3R(Node3D *, _generate_scene_node, Ref<GLTFState>, Ref<GLTFNode>, Node *); GDVIRTUAL1R(Error, _import_post_parse, Ref<GLTFState>); + GDVIRTUAL1R(Error, _import_pre_generate, Ref<GLTFState>); + GDVIRTUAL3R(Node3D *, _generate_scene_node, Ref<GLTFState>, Ref<GLTFNode>, Node *); GDVIRTUAL4R(Error, _import_node, Ref<GLTFState>, Ref<GLTFNode>, Dictionary, Node *); GDVIRTUAL2R(Error, _import_post, Ref<GLTFState>, Node *); // Export process. GDVIRTUAL2R(Error, _export_preflight, Ref<GLTFState>, Node *); GDVIRTUAL3(_convert_scene_node, Ref<GLTFState>, Ref<GLTFNode>, Node *); + GDVIRTUAL2R(Error, _export_post_convert, Ref<GLTFState>, Node *); GDVIRTUAL1R(Error, _export_preserialize, Ref<GLTFState>); GDVIRTUAL0R(Vector<String>, _get_saveable_image_formats); GDVIRTUAL5R(PackedByteArray, _serialize_image_to_bytes, Ref<GLTFState>, Ref<Image>, Dictionary, String, float); diff --git a/modules/gltf/gltf_document.cpp b/modules/gltf/gltf_document.cpp index cf1a1ea4b3..992075e980 100644 --- a/modules/gltf/gltf_document.cpp +++ b/modules/gltf/gltf_document.cpp @@ -414,7 +414,6 @@ static Vector<real_t> _xform_to_array(const Transform3D p_transform) { Error GLTFDocument::_serialize_nodes(Ref<GLTFState> p_state) { Array nodes; - const int scene_node_count = p_state->scene_nodes.size(); for (int i = 0; i < p_state->nodes.size(); i++) { Dictionary node; Ref<GLTFNode> gltf_node = p_state->nodes[i]; @@ -465,7 +464,7 @@ Error GLTFDocument::_serialize_nodes(Ref<GLTFState> p_state) { } Node *scene_node = nullptr; - if (i < scene_node_count) { + if (i < (int)p_state->scene_nodes.size()) { scene_node = p_state->scene_nodes[i]; } for (Ref<GLTFDocumentExtension> ext : document_extensions) { @@ -5258,6 +5257,7 @@ void GLTFDocument::_convert_scene_node(Ref<GLTFState> p_state, Node *p_current, gltf_node.instantiate(); gltf_node->set_original_name(p_current->get_name()); gltf_node->set_name(_gen_unique_name(p_state, p_current->get_name())); + gltf_node->merge_meta_from(p_current); if (cast_to<Node3D>(p_current)) { Node3D *spatial = cast_to<Node3D>(p_current); _convert_spatial(p_state, spatial, gltf_node); @@ -5303,14 +5303,18 @@ void GLTFDocument::_convert_scene_node(Ref<GLTFState> p_state, Node *p_current, ERR_CONTINUE(ext.is_null()); ext->convert_scene_node(p_state, gltf_node, p_current); } - GLTFNodeIndex current_node_i = p_state->nodes.size(); - GLTFNodeIndex gltf_root = p_gltf_root; - if (gltf_root == -1) { - gltf_root = current_node_i; - p_state->root_nodes.push_back(gltf_root); + GLTFNodeIndex current_node_i; + if (gltf_node->get_parent() == -1) { + current_node_i = p_state->append_gltf_node(gltf_node, p_current, p_gltf_parent); + } else if (gltf_node->get_parent() < -1) { + return; + } else { + current_node_i = p_state->nodes.size() - 1; + while (gltf_node != p_state->nodes[current_node_i]) { + current_node_i--; + } } - gltf_node->merge_meta_from(p_current); - _create_gltf_node(p_state, p_current, current_node_i, p_gltf_parent, gltf_root, gltf_node); + const GLTFNodeIndex gltf_root = (p_gltf_root == -1) ? current_node_i : p_gltf_root; for (int node_i = 0; node_i < p_current->get_child_count(); node_i++) { _convert_scene_node(p_state, p_current->get_child(node_i), current_node_i, gltf_root); } @@ -5371,18 +5375,6 @@ void GLTFDocument::_convert_csg_shape_to_gltf(CSGShape3D *p_current, GLTFNodeInd } #endif // MODULE_CSG_ENABLED -void GLTFDocument::_create_gltf_node(Ref<GLTFState> p_state, Node *p_scene_parent, GLTFNodeIndex p_current_node_i, - GLTFNodeIndex p_parent_node_index, GLTFNodeIndex p_root_gltf_node, Ref<GLTFNode> p_gltf_node) { - p_state->scene_nodes.insert(p_current_node_i, p_scene_parent); - p_state->nodes.push_back(p_gltf_node); - ERR_FAIL_COND(p_current_node_i == p_parent_node_index); - p_state->nodes.write[p_current_node_i]->parent = p_parent_node_index; - if (p_parent_node_index == -1) { - return; - } - p_state->nodes.write[p_parent_node_index]->children.push_back(p_current_node_i); -} - void GLTFDocument::_convert_animation_player_to_gltf(AnimationPlayer *p_animation_player, Ref<GLTFState> p_state, GLTFNodeIndex p_gltf_current, GLTFNodeIndex p_gltf_root_index, Ref<GLTFNode> p_gltf_node, Node *p_scene_parent) { ERR_FAIL_NULL(p_animation_player); p_state->animation_players.push_back(p_animation_player); @@ -5542,6 +5534,10 @@ void GLTFDocument::_convert_skeleton_to_gltf(Skeleton3D *p_skeleton3d, Ref<GLTFS joint_node->set_name(_gen_unique_name(p_state, skeleton->get_bone_name(bone_i))); joint_node->transform = skeleton->get_bone_pose(bone_i); joint_node->joint = true; + + if (p_skeleton3d->has_bone_meta(bone_i, "extras")) { + joint_node->set_meta("extras", p_skeleton3d->get_bone_meta(bone_i, "extras")); + } GLTFNodeIndex current_node_i = p_state->nodes.size(); p_state->scene_nodes.insert(current_node_i, skeleton); p_state->nodes.push_back(joint_node); @@ -7209,6 +7205,12 @@ Node *GLTFDocument::_generate_scene_node_tree(Ref<GLTFState> p_state) { ERR_FAIL_COND_V_MSG(err != OK, nullptr, "glTF: Failed to create skeletons."); err = _create_skins(p_state); ERR_FAIL_COND_V_MSG(err != OK, nullptr, "glTF: Failed to create skins."); + // Run pre-generate for each extension, in case an extension needs to do something before generating the scene. + for (Ref<GLTFDocumentExtension> ext : document_extensions) { + ERR_CONTINUE(ext.is_null()); + err = ext->import_pre_generate(p_state); + ERR_CONTINUE(err != OK); + } // Generate the node tree. Node *single_root; if (p_state->extensions_used.has("GODOT_single_root")) { @@ -7443,6 +7445,12 @@ Error GLTFDocument::append_from_scene(Node *p_node, Ref<GLTFState> p_state, uint state->extensions_used.append("GODOT_single_root"); } _convert_scene_node(state, p_node, -1, -1); + // Run post-convert for each extension, in case an extension needs to do something after converting the scene. + for (Ref<GLTFDocumentExtension> ext : document_extensions) { + ERR_CONTINUE(ext.is_null()); + Error err = ext->export_post_convert(p_state, p_node); + ERR_CONTINUE(err != OK); + } return OK; } diff --git a/modules/gltf/gltf_document.h b/modules/gltf/gltf_document.h index b3e6dcf54a..d347d49102 100644 --- a/modules/gltf/gltf_document.h +++ b/modules/gltf/gltf_document.h @@ -342,12 +342,6 @@ public: void _convert_csg_shape_to_gltf(CSGShape3D *p_current, GLTFNodeIndex p_gltf_parent, Ref<GLTFNode> p_gltf_node, Ref<GLTFState> p_state); #endif // MODULE_CSG_ENABLED - void _create_gltf_node(Ref<GLTFState> p_state, - Node *p_scene_parent, - GLTFNodeIndex p_current_node_i, - GLTFNodeIndex p_parent_node_index, - GLTFNodeIndex p_root_gltf_node, - Ref<GLTFNode> p_gltf_node); void _convert_animation_player_to_gltf( AnimationPlayer *p_animation_player, Ref<GLTFState> p_state, GLTFNodeIndex p_gltf_current, diff --git a/modules/gltf/gltf_state.cpp b/modules/gltf/gltf_state.cpp index 73a61ff77f..7763874d02 100644 --- a/modules/gltf/gltf_state.cpp +++ b/modules/gltf/gltf_state.cpp @@ -35,6 +35,7 @@ void GLTFState::_bind_methods() { ClassDB::bind_method(D_METHOD("add_used_extension", "extension_name", "required"), &GLTFState::add_used_extension); ClassDB::bind_method(D_METHOD("append_data_to_buffers", "data", "deduplication"), &GLTFState::append_data_to_buffers); + ClassDB::bind_method(D_METHOD("append_gltf_node", "gltf_node", "godot_scene_node", "parent_node_index"), &GLTFState::append_gltf_node); ClassDB::bind_method(D_METHOD("get_json"), &GLTFState::get_json); ClassDB::bind_method(D_METHOD("set_json", "json"), &GLTFState::set_json); @@ -441,3 +442,16 @@ GLTFBufferViewIndex GLTFState::append_data_to_buffers(const Vector<uint8_t> &p_d buffer_views.push_back(buffer_view); return new_index; } + +GLTFNodeIndex GLTFState::append_gltf_node(Ref<GLTFNode> p_gltf_node, Node *p_godot_scene_node, GLTFNodeIndex p_parent_node_index) { + p_gltf_node->set_parent(p_parent_node_index); + const GLTFNodeIndex new_index = nodes.size(); + nodes.append(p_gltf_node); + scene_nodes.insert(new_index, p_godot_scene_node); + if (p_parent_node_index == -1) { + root_nodes.append(new_index); + } else if (p_parent_node_index < new_index) { + nodes.write[p_parent_node_index]->append_child_index(new_index); + } + return new_index; +} diff --git a/modules/gltf/gltf_state.h b/modules/gltf/gltf_state.h index 07efafe13b..7954049192 100644 --- a/modules/gltf/gltf_state.h +++ b/modules/gltf/gltf_state.h @@ -119,6 +119,7 @@ public: void add_used_extension(const String &p_extension, bool p_required = false); GLTFBufferViewIndex append_data_to_buffers(const Vector<uint8_t> &p_data, const bool p_deduplication); + GLTFNodeIndex append_gltf_node(Ref<GLTFNode> p_gltf_node, Node *p_godot_scene_node, GLTFNodeIndex p_parent_node_index); enum GLTFHandleBinary { HANDLE_BINARY_DISCARD_TEXTURES = 0, diff --git a/modules/gltf/skin_tool.cpp b/modules/gltf/skin_tool.cpp index a344334d93..1522c0e324 100644 --- a/modules/gltf/skin_tool.cpp +++ b/modules/gltf/skin_tool.cpp @@ -602,6 +602,11 @@ Error SkinTool::_create_skeletons( skeleton->set_bone_pose_rotation(bone_index, node->transform.basis.get_rotation_quaternion()); skeleton->set_bone_pose_scale(bone_index, node->transform.basis.get_scale()); + // Store bone-level GLTF extras in skeleton per bone meta. + if (node->has_meta("extras")) { + skeleton->set_bone_meta(bone_index, "extras", node->get_meta("extras")); + } + if (node->parent >= 0 && nodes[node->parent]->skeleton == skel_i) { const int bone_parent = skeleton->find_bone(nodes[node->parent]->get_name()); ERR_FAIL_COND_V(bone_parent < 0, FAILED); diff --git a/modules/gltf/structures/gltf_node.cpp b/modules/gltf/structures/gltf_node.cpp index 2934e4b5ee..ccee5e8ca4 100644 --- a/modules/gltf/structures/gltf_node.cpp +++ b/modules/gltf/structures/gltf_node.cpp @@ -55,6 +55,7 @@ void GLTFNode::_bind_methods() { ClassDB::bind_method(D_METHOD("set_scale", "scale"), &GLTFNode::set_scale); ClassDB::bind_method(D_METHOD("get_children"), &GLTFNode::get_children); ClassDB::bind_method(D_METHOD("set_children", "children"), &GLTFNode::set_children); + ClassDB::bind_method(D_METHOD("append_child_index", "child_index"), &GLTFNode::append_child_index); ClassDB::bind_method(D_METHOD("get_light"), &GLTFNode::get_light); ClassDB::bind_method(D_METHOD("set_light", "light"), &GLTFNode::set_light); ClassDB::bind_method(D_METHOD("get_additional_data", "extension_name"), &GLTFNode::get_additional_data); @@ -170,6 +171,10 @@ void GLTFNode::set_children(Vector<int> p_children) { children = p_children; } +void GLTFNode::append_child_index(int p_child_index) { + children.append(p_child_index); +} + GLTFLightIndex GLTFNode::get_light() { return light; } diff --git a/modules/gltf/structures/gltf_node.h b/modules/gltf/structures/gltf_node.h index 63399fb32b..f3f6bfa2f1 100644 --- a/modules/gltf/structures/gltf_node.h +++ b/modules/gltf/structures/gltf_node.h @@ -97,6 +97,7 @@ public: Vector<int> get_children(); void set_children(Vector<int> p_children); + void append_child_index(int p_child_index); GLTFLightIndex get_light(); void set_light(GLTFLightIndex p_light); diff --git a/modules/gltf/tests/test_gltf_extras.h b/modules/gltf/tests/test_gltf_extras.h index 96aadf3023..37c8f6925c 100644 --- a/modules/gltf/tests/test_gltf_extras.h +++ b/modules/gltf/tests/test_gltf_extras.h @@ -41,6 +41,7 @@ #include "modules/gltf/gltf_document.h" #include "modules/gltf/gltf_state.h" #include "scene/3d/mesh_instance_3d.h" +#include "scene/3d/skeleton_3d.h" #include "scene/main/window.h" #include "scene/resources/3d/primitive_meshes.h" #include "scene/resources/material.h" @@ -158,6 +159,62 @@ TEST_CASE("[SceneTree][Node] GLTF test mesh and material meta export and import" memdelete(original); memdelete(loaded); } + +TEST_CASE("[SceneTree][Node] GLTF test skeleton and bone export and import") { + // Setup scene. + Skeleton3D *skeleton = memnew(Skeleton3D); + skeleton->set_name("skeleton"); + Dictionary skeleton_extras; + skeleton_extras["node_type"] = "skeleton"; + skeleton->set_meta("extras", skeleton_extras); + + skeleton->add_bone("parent"); + skeleton->set_bone_rest(0, Transform3D()); + Dictionary parent_bone_extras; + parent_bone_extras["bone"] = "i_am_parent_bone"; + skeleton->set_bone_meta(0, "extras", parent_bone_extras); + + skeleton->add_bone("child"); + skeleton->set_bone_rest(1, Transform3D()); + skeleton->set_bone_parent(1, 0); + Dictionary child_bone_extras; + child_bone_extras["bone"] = "i_am_child_bone"; + skeleton->set_bone_meta(1, "extras", child_bone_extras); + + // We have to have a mesh to link with skeleton or it will not get imported. + Ref<PlaneMesh> meshdata = memnew(PlaneMesh); + meshdata->set_name("planemesh"); + + MeshInstance3D *mesh = memnew(MeshInstance3D); + mesh->set_mesh(meshdata); + mesh->set_name("mesh_instance_3d"); + + Node3D *scene = memnew(Node3D); + SceneTree::get_singleton()->get_root()->add_child(scene); + scene->add_child(skeleton); + scene->add_child(mesh); + scene->set_name("node3d"); + + // Now that both skeleton and mesh are part of scene, link them. + mesh->set_skeleton_path(mesh->get_path_to(skeleton)); + + // Convert to GLFT and back. + String tempfile = OS::get_singleton()->get_cache_path().path_join("gltf_bone_extras"); + Node *loaded = _gltf_export_then_import(scene, tempfile); + + // Compare the results. + CHECK(loaded->get_name() == "node3d"); + Skeleton3D *result = Object::cast_to<Skeleton3D>(loaded->find_child("Skeleton3D", false, true)); + CHECK(result->get_bone_name(0) == "parent"); + CHECK(Dictionary(result->get_bone_meta(0, "extras"))["bone"] == "i_am_parent_bone"); + CHECK(result->get_bone_name(1) == "child"); + CHECK(Dictionary(result->get_bone_meta(1, "extras"))["bone"] == "i_am_child_bone"); + + memdelete(skeleton); + memdelete(mesh); + memdelete(scene); + memdelete(loaded); +} } // namespace TestGltfExtras #endif // TOOLS_ENABLED diff --git a/modules/godot_physics_3d/SCsub b/modules/godot_physics_3d/SCsub new file mode 100644 index 0000000000..41a59cd24e --- /dev/null +++ b/modules/godot_physics_3d/SCsub @@ -0,0 +1,7 @@ +#!/usr/bin/env python + +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..5d93da5ecf --- /dev/null +++ b/modules/godot_physics_3d/joints/SCsub @@ -0,0 +1,5 @@ +#!/usr/bin/env python + +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/editor/grid_map_editor_plugin.cpp b/modules/gridmap/editor/grid_map_editor_plugin.cpp index 1c658b9e9b..4c11565c51 100644 --- a/modules/gridmap/editor/grid_map_editor_plugin.cpp +++ b/modules/gridmap/editor/grid_map_editor_plugin.cpp @@ -816,13 +816,14 @@ void GridMapEditor::_text_changed(const String &p_text) { update_palette(); } -void GridMapEditor::_sbox_input(const Ref<InputEvent> &p_ie) { - const Ref<InputEventKey> k = p_ie; - - if (k.is_valid() && (k->get_keycode() == Key::UP || k->get_keycode() == Key::DOWN || k->get_keycode() == Key::PAGEUP || k->get_keycode() == Key::PAGEDOWN)) { - // Forward the key input to the ItemList so it can be scrolled - mesh_library_palette->gui_input(k); - search_box->accept_event(); +void GridMapEditor::_sbox_input(const Ref<InputEvent> &p_event) { + // Redirect navigational key events to the item list. + Ref<InputEventKey> key = p_event; + if (key.is_valid()) { + if (key->is_action("ui_up", true) || key->is_action("ui_down", true) || key->is_action("ui_page_up") || key->is_action("ui_page_down")) { + mesh_library_palette->gui_input(key); + search_box->accept_event(); + } } } diff --git a/modules/gridmap/editor/grid_map_editor_plugin.h b/modules/gridmap/editor/grid_map_editor_plugin.h index cfa0f0c35c..4294c93c93 100644 --- a/modules/gridmap/editor/grid_map_editor_plugin.h +++ b/modules/gridmap/editor/grid_map_editor_plugin.h @@ -199,7 +199,7 @@ class GridMapEditor : public VBoxContainer { void _update_theme(); void _text_changed(const String &p_text); - void _sbox_input(const Ref<InputEvent> &p_ie); + void _sbox_input(const Ref<InputEvent> &p_event); void _mesh_library_palette_input(const Ref<InputEvent> &p_ie); void _icon_size_changed(float p_value); diff --git a/modules/minimp3/audio_stream_mp3.cpp b/modules/minimp3/audio_stream_mp3.cpp index e29fe9295a..4963cfdf1a 100644 --- a/modules/minimp3/audio_stream_mp3.cpp +++ b/modules/minimp3/audio_stream_mp3.cpp @@ -221,10 +221,9 @@ void AudioStreamMP3::clear_data() { void AudioStreamMP3::set_data(const Vector<uint8_t> &p_data) { int src_data_len = p_data.size(); - const uint8_t *src_datar = p_data.ptr(); mp3dec_ex_t *mp3d = memnew(mp3dec_ex_t); - int err = mp3dec_ex_open_buf(mp3d, src_datar, src_data_len, MP3D_SEEK_TO_SAMPLE); + int err = mp3dec_ex_open_buf(mp3d, p_data.ptr(), src_data_len, MP3D_SEEK_TO_SAMPLE); if (err || mp3d->info.hz == 0) { memdelete(mp3d); ERR_FAIL_MSG("Failed to decode mp3 file. Make sure it is a valid mp3 audio file."); @@ -237,10 +236,7 @@ void AudioStreamMP3::set_data(const Vector<uint8_t> &p_data) { mp3dec_ex_close(mp3d); memdelete(mp3d); - clear_data(); - - data.resize(src_data_len); - memcpy(data.ptrw(), src_datar, src_data_len); + data = p_data; data_len = src_data_len; } diff --git a/modules/minimp3/audio_stream_mp3.h b/modules/minimp3/audio_stream_mp3.h index 39d389b8cd..bb4c7f524c 100644 --- a/modules/minimp3/audio_stream_mp3.h +++ b/modules/minimp3/audio_stream_mp3.h @@ -96,7 +96,7 @@ class AudioStreamMP3 : public AudioStream { friend class AudioStreamPlaybackMP3; - PackedByteArray data; + LocalVector<uint8_t> data; uint32_t data_len = 0; float sample_rate = 1.0; diff --git a/modules/mono/csharp_script.cpp b/modules/mono/csharp_script.cpp index 5d59c33636..3d12994469 100644 --- a/modules/mono/csharp_script.cpp +++ b/modules/mono/csharp_script.cpp @@ -1497,11 +1497,23 @@ bool CSharpInstance::get(const StringName &p_name, Variant &r_ret) const { void CSharpInstance::get_property_list(List<PropertyInfo> *p_properties) const { List<PropertyInfo> props; - script->get_script_property_list(&props); + ERR_FAIL_COND(!script.is_valid()); +#ifdef TOOLS_ENABLED + for (const PropertyInfo &prop : script->exported_members_cache) { + props.push_back(prop); + } +#else + for (const KeyValue<StringName, PropertyInfo> &E : script->member_info) { + props.push_front(E.value); + } +#endif - // Call _get_property_list + for (PropertyInfo &prop : props) { + validate_property(prop); + p_properties->push_back(prop); + } - ERR_FAIL_COND(!script.is_valid()); + // Call _get_property_list StringName method = SNAME("_get_property_list"); @@ -1524,10 +1536,25 @@ void CSharpInstance::get_property_list(List<PropertyInfo> *p_properties) const { } } - props.reverse(); - for (PropertyInfo &prop : props) { - validate_property(prop); - p_properties->push_front(prop); + CSharpScript *top = script.ptr()->base_script.ptr(); + while (top != nullptr) { + props.clear(); +#ifdef TOOLS_ENABLED + for (const PropertyInfo &prop : top->exported_members_cache) { + props.push_back(prop); + } +#else + for (const KeyValue<StringName, PropertyInfo> &E : top->member_info) { + props.push_front(E.value); + } +#endif + + for (PropertyInfo &prop : props) { + validate_property(prop); + p_properties->push_back(prop); + } + + top = top->base_script.ptr(); } } @@ -2717,7 +2744,7 @@ int CSharpScript::get_member_line(const StringName &p_member) const { return -1; } -const Variant CSharpScript::get_rpc_config() const { +Variant CSharpScript::get_rpc_config() const { return rpc_config; } diff --git a/modules/mono/csharp_script.h b/modules/mono/csharp_script.h index c48e1a95c9..ec7328be4a 100644 --- a/modules/mono/csharp_script.h +++ b/modules/mono/csharp_script.h @@ -284,7 +284,7 @@ public: int get_member_line(const StringName &p_member) const override; - const Variant get_rpc_config() const override; + Variant get_rpc_config() const override; #ifdef TOOLS_ENABLED bool is_placeholder_fallback_enabled() const override { diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Godot.NET.Sdk.csproj b/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Godot.NET.Sdk.csproj index ee624a443d..c5f2dfee4b 100644 --- a/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Godot.NET.Sdk.csproj +++ b/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Godot.NET.Sdk.csproj @@ -30,6 +30,7 @@ <None Include="$(GodotSdkPackageVersionsFilePath)" Pack="true" PackagePath="Sdk"> <Link>Sdk\SdkPackageVersions.props</Link> </None> + <None Include="Sdk\Android.props" Pack="true" PackagePath="Sdk" /> <None Include="Sdk\iOSNativeAOT.props" Pack="true" PackagePath="Sdk" /> <None Include="Sdk\iOSNativeAOT.targets" Pack="true" PackagePath="Sdk" /> </ItemGroup> diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Sdk/Android.props b/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Sdk/Android.props new file mode 100644 index 0000000000..3926a4b22a --- /dev/null +++ b/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Sdk/Android.props @@ -0,0 +1,5 @@ +<Project> + <PropertyGroup> + <UseMonoRuntime Condition=" '$(UseMonoRuntime)' == '' and '$(PublishAot)' != 'true' ">true</UseMonoRuntime> + </PropertyGroup> +</Project> diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Sdk/Sdk.props b/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Sdk/Sdk.props index c4034f1f9f..d10f9ae0ab 100644 --- a/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Sdk/Sdk.props +++ b/modules/mono/editor/Godot.NET.Sdk/Godot.NET.Sdk/Sdk/Sdk.props @@ -112,5 +112,6 @@ <DefineConstants>$(GodotDefineConstants);$(DefineConstants)</DefineConstants> </PropertyGroup> + <Import Project="$(MSBuildThisFileDirectory)\Android.props" Condition=" '$(GodotTargetPlatform)' == 'android' " /> <Import Project="$(MSBuildThisFileDirectory)\iOSNativeAOT.props" Condition=" '$(GodotTargetPlatform)' == 'ios' " /> </Project> diff --git a/modules/mono/editor/GodotTools/GodotTools.OpenVisualStudio/GodotTools.OpenVisualStudio.csproj b/modules/mono/editor/GodotTools/GodotTools.OpenVisualStudio/GodotTools.OpenVisualStudio.csproj index f23f2b9a8c..208e6d8f41 100644 --- a/modules/mono/editor/GodotTools/GodotTools.OpenVisualStudio/GodotTools.OpenVisualStudio.csproj +++ b/modules/mono/editor/GodotTools/GodotTools.OpenVisualStudio/GodotTools.OpenVisualStudio.csproj @@ -5,7 +5,6 @@ <TargetFramework>net6.0-windows</TargetFramework> <LangVersion>10</LangVersion> <Nullable>enable</Nullable> - <RuntimeIdentifier>win-x86</RuntimeIdentifier> <SelfContained>False</SelfContained> <RollForward>LatestMajor</RollForward> </PropertyGroup> diff --git a/modules/mono/editor/GodotTools/GodotTools/Export/ExportPlugin.cs b/modules/mono/editor/GodotTools/GodotTools/Export/ExportPlugin.cs index a5f24fb67b..6fd84d3834 100644 --- a/modules/mono/editor/GodotTools/GodotTools/Export/ExportPlugin.cs +++ b/modules/mono/editor/GodotTools/GodotTools/Export/ExportPlugin.cs @@ -245,7 +245,6 @@ namespace GodotTools.Export { publishOutputDir = Path.Combine(GodotSharpDirs.ProjectBaseOutputPath, "godot-publish-dotnet", $"{buildConfig}-{runtimeIdentifier}"); - } outputPaths.Add(publishOutputDir); @@ -322,6 +321,30 @@ namespace GodotTools.Export { if (embedBuildResults) { + if (platform == OS.Platforms.Android) + { + if (IsSharedObject(Path.GetFileName(path))) + { + AddSharedObject(path, tags: new string[] { arch }, + Path.Join(projectDataDirName, + Path.GetRelativePath(publishOutputDir, + Path.GetDirectoryName(path)!))); + + return; + } + + static bool IsSharedObject(string fileName) + { + if (fileName.EndsWith(".so") || fileName.EndsWith(".a") + || fileName.EndsWith(".jar") || fileName.EndsWith(".dex")) + { + return true; + } + + return false; + } + } + string filePath = SanitizeSlashes(Path.GetRelativePath(publishOutputDir, path)); byte[] fileData = File.ReadAllBytes(path); string hash = Convert.ToBase64String(SHA512.HashData(fileData)); diff --git a/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs b/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs index 673f98ee14..788b46ab9a 100644 --- a/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs +++ b/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs @@ -260,11 +260,12 @@ namespace GodotTools var args = new List<string> { + Path.Combine(GodotSharpDirs.DataEditorToolsDir, "GodotTools.OpenVisualStudio.dll"), GodotSharpDirs.ProjectSlnPath, line >= 0 ? $"{scriptPath};{line + 1};{col + 1}" : scriptPath }; - string command = Path.Combine(GodotSharpDirs.DataEditorToolsDir, "GodotTools.OpenVisualStudio.exe"); + string command = DotNetFinder.FindDotNetExe() ?? "dotnet"; try { diff --git a/modules/mono/editor/bindings_generator.cpp b/modules/mono/editor/bindings_generator.cpp index 4b8bbc04cb..a467aae2e9 100644 --- a/modules/mono/editor/bindings_generator.cpp +++ b/modules/mono/editor/bindings_generator.cpp @@ -1456,7 +1456,7 @@ Error BindingsGenerator::_populate_method_icalls_table(const TypeInterface &p_it } const TypeInterface *return_type = _get_type_or_null(imethod.return_type); - ERR_FAIL_NULL_V(return_type, ERR_BUG); // Return type not found + ERR_FAIL_NULL_V_MSG(return_type, ERR_BUG, "Return type '" + imethod.return_type.cname + "' was not found."); String im_unique_sig = get_ret_unique_sig(return_type) + ",CallMethodBind"; @@ -1467,7 +1467,7 @@ Error BindingsGenerator::_populate_method_icalls_table(const TypeInterface &p_it // Get arguments information for (const ArgumentInterface &iarg : imethod.arguments) { const TypeInterface *arg_type = _get_type_or_null(iarg.type); - ERR_FAIL_NULL_V(arg_type, ERR_BUG); // Argument type not found + ERR_FAIL_NULL_V_MSG(arg_type, ERR_BUG, "Argument type '" + iarg.type.cname + "' was not found."); im_unique_sig += ","; im_unique_sig += get_arg_unique_sig(*arg_type); @@ -2440,7 +2440,7 @@ Error BindingsGenerator::_generate_cs_type(const TypeInterface &itype, const Str const ArgumentInterface &iarg = *itr; const TypeInterface *arg_type = _get_type_or_null(iarg.type); - ERR_FAIL_NULL_V(arg_type, ERR_BUG); // Argument type not found + ERR_FAIL_NULL_V_MSG(arg_type, ERR_BUG, "Argument type '" + iarg.type.cname + "' was not found."); if (i != 0) { output << ", "; @@ -2460,7 +2460,7 @@ Error BindingsGenerator::_generate_cs_type(const TypeInterface &itype, const Str if (imethod.return_type.cname != name_cache.type_void) { const TypeInterface *return_type = _get_type_or_null(imethod.return_type); - ERR_FAIL_NULL_V(return_type, ERR_BUG); // Return type not found + ERR_FAIL_NULL_V_MSG(return_type, ERR_BUG, "Return type '" + imethod.return_type.cname + "' was not found."); output << INDENT3 "ret = " << sformat(return_type->cs_managed_to_variant, "callRet", return_type->cs_type, return_type->name) @@ -2679,7 +2679,7 @@ Error BindingsGenerator::_generate_cs_property(const BindingsGenerator::TypeInte const TypeReference &proptype_name = getter ? getter->return_type : setter->arguments.back()->get().type; const TypeInterface *prop_itype = _get_type_or_singleton_or_null(proptype_name); - ERR_FAIL_NULL_V(prop_itype, ERR_BUG); // Property type not found + ERR_FAIL_NULL_V_MSG(prop_itype, ERR_BUG, "Property type '" + proptype_name.cname + "' was not found."); ERR_FAIL_COND_V_MSG(prop_itype->is_singleton, ERR_BUG, "Property type is a singleton: '" + p_itype.name + "." + String(p_iprop.cname) + "'."); @@ -2778,7 +2778,7 @@ Error BindingsGenerator::_generate_cs_property(const BindingsGenerator::TypeInte Error BindingsGenerator::_generate_cs_method(const BindingsGenerator::TypeInterface &p_itype, const BindingsGenerator::MethodInterface &p_imethod, int &p_method_bind_count, StringBuilder &p_output) { const TypeInterface *return_type = _get_type_or_singleton_or_null(p_imethod.return_type); - ERR_FAIL_NULL_V(return_type, ERR_BUG); // Return type not found + ERR_FAIL_NULL_V_MSG(return_type, ERR_BUG, "Return type '" + p_imethod.return_type.cname + "' was not found."); ERR_FAIL_COND_V_MSG(return_type->is_singleton, ERR_BUG, "Method return type is a singleton: '" + p_itype.name + "." + p_imethod.name + "'."); @@ -2817,7 +2817,7 @@ Error BindingsGenerator::_generate_cs_method(const BindingsGenerator::TypeInterf const ArgumentInterface &first = p_imethod.arguments.front()->get(); for (const ArgumentInterface &iarg : p_imethod.arguments) { const TypeInterface *arg_type = _get_type_or_singleton_or_null(iarg.type); - ERR_FAIL_NULL_V(arg_type, ERR_BUG); // Argument type not found + ERR_FAIL_NULL_V_MSG(arg_type, ERR_BUG, "Argument type '" + iarg.type.cname + "' was not found."); ERR_FAIL_COND_V_MSG(arg_type->is_singleton, ERR_BUG, "Argument type is a singleton: '" + iarg.name + "' of method '" + p_itype.name + "." + p_imethod.name + "'."); @@ -3061,17 +3061,12 @@ 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(); for (const ArgumentInterface &iarg : p_isignal.arguments) { const TypeInterface *arg_type = _get_type_or_singleton_or_null(iarg.type); - ERR_FAIL_NULL_V(arg_type, ERR_BUG); // Argument type not found + ERR_FAIL_NULL_V_MSG(arg_type, ERR_BUG, "Argument type '" + iarg.type.cname + "' was not found."); ERR_FAIL_COND_V_MSG(arg_type->is_singleton, ERR_BUG, "Argument type is a singleton: '" + iarg.name + "' of signal '" + p_itype.name + "." + p_isignal.name + "'."); @@ -3086,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 @@ -3140,14 +3130,20 @@ Error BindingsGenerator::_generate_cs_signal(const BindingsGenerator::TypeInterf int idx = 0; for (const ArgumentInterface &iarg : p_isignal.arguments) { const TypeInterface *arg_type = _get_type_or_null(iarg.type); - ERR_FAIL_NULL_V(arg_type, ERR_BUG); // Argument type not found + ERR_FAIL_NULL_V_MSG(arg_type, ERR_BUG, "Argument type '" + iarg.type.cname + "' was not found."); if (idx != 0) { 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++; } @@ -3240,7 +3236,7 @@ Error BindingsGenerator::_generate_cs_native_calls(const InternalCall &p_icall, bool ret_void = p_icall.return_type.cname == name_cache.type_void; const TypeInterface *return_type = _get_type_or_null(p_icall.return_type); - ERR_FAIL_NULL_V(return_type, ERR_BUG); // Return type not found + ERR_FAIL_NULL_V_MSG(return_type, ERR_BUG, "Return type '" + p_icall.return_type.cname + "' was not found."); StringBuilder c_func_sig; StringBuilder c_in_statements; @@ -3256,7 +3252,7 @@ Error BindingsGenerator::_generate_cs_native_calls(const InternalCall &p_icall, int i = 0; for (const TypeReference &arg_type_ref : p_icall.argument_types) { const TypeInterface *arg_type = _get_type_or_null(arg_type_ref); - ERR_FAIL_NULL_V(arg_type, ERR_BUG); // Return type not found + ERR_FAIL_NULL_V_MSG(arg_type, ERR_BUG, "Argument type '" + arg_type_ref.cname + "' was not found."); String c_param_name = "arg" + itos(i + 1); @@ -3516,7 +3512,7 @@ const String BindingsGenerator::_get_generic_type_parameters(const TypeInterface String params = "<"; for (const TypeReference ¶m_type : p_generic_type_parameters) { const TypeInterface *param_itype = _get_type_or_singleton_or_null(param_type); - ERR_FAIL_NULL_V(param_itype, ""); // Parameter type not found + ERR_FAIL_NULL_V_MSG(param_itype, "", "Parameter type '" + param_type.cname + "' was not found."); ERR_FAIL_COND_V_MSG(param_itype->is_singleton, "", "Generic type parameter is a singleton: '" + param_itype->name + "'."); diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs index 0f534d477f..0dc143edea 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs @@ -468,8 +468,8 @@ namespace Godot { for (int j = 0; j < 3; j++) { - real_t e = transform.Basis[i][j] * min[j]; - real_t f = transform.Basis[i][j] * max[j]; + real_t e = transform.Basis[j][i] * min[j]; + real_t f = transform.Basis[j][i] * max[j]; if (e < f) { tmin[i] += e; diff --git a/modules/mono/mono_gd/gd_mono.cpp b/modules/mono/mono_gd/gd_mono.cpp index 48caae8523..6abf0193cf 100644 --- a/modules/mono/mono_gd/gd_mono.cpp +++ b/modules/mono/mono_gd/gd_mono.cpp @@ -61,6 +61,14 @@ hostfxr_initialize_for_runtime_config_fn hostfxr_initialize_for_runtime_config = hostfxr_get_runtime_delegate_fn hostfxr_get_runtime_delegate = nullptr; hostfxr_close_fn hostfxr_close = nullptr; +#ifndef TOOLS_ENABLED +typedef int(CORECLR_DELEGATE_CALLTYPE *coreclr_create_delegate_fn)(void *hostHandle, unsigned int domainId, const char *entryPointAssemblyName, const char *entryPointTypeName, const char *entryPointMethodName, void **delegate); +typedef int(CORECLR_DELEGATE_CALLTYPE *coreclr_initialize_fn)(const char *exePath, const char *appDomainFriendlyName, int propertyCount, const char **propertyKeys, const char **propertyValues, void **hostHandle, unsigned int *domainId); + +coreclr_create_delegate_fn coreclr_create_delegate = nullptr; +coreclr_initialize_fn coreclr_initialize = nullptr; +#endif + #ifdef _WIN32 static_assert(sizeof(char_t) == sizeof(char16_t)); using HostFxrCharString = Char16String; @@ -142,6 +150,56 @@ String find_hostfxr() { #endif } +#ifndef TOOLS_ENABLED +String find_monosgen() { +#if defined(ANDROID_ENABLED) + // Android includes all native libraries in the libs directory of the APK + // so we assume it exists and use only the name to dlopen it. + return "libmonosgen-2.0.so"; +#else +#if defined(WINDOWS_ENABLED) + String probe_path = GodotSharpDirs::get_api_assemblies_dir() + .path_join("monosgen-2.0.dll"); +#elif defined(MACOS_ENABLED) + String probe_path = GodotSharpDirs::get_api_assemblies_dir() + .path_join("libmonosgen-2.0.dylib"); +#elif defined(UNIX_ENABLED) + String probe_path = GodotSharpDirs::get_api_assemblies_dir() + .path_join("libmonosgen-2.0.so"); +#else +#error "Platform not supported (yet?)" +#endif + + if (FileAccess::exists(probe_path)) { + return probe_path; + } + + return String(); +#endif +} + +String find_coreclr() { +#if defined(WINDOWS_ENABLED) + String probe_path = GodotSharpDirs::get_api_assemblies_dir() + .path_join("coreclr.dll"); +#elif defined(MACOS_ENABLED) + String probe_path = GodotSharpDirs::get_api_assemblies_dir() + .path_join("libcoreclr.dylib"); +#elif defined(UNIX_ENABLED) + String probe_path = GodotSharpDirs::get_api_assemblies_dir() + .path_join("libcoreclr.so"); +#else +#error "Platform not supported (yet?)" +#endif + + if (FileAccess::exists(probe_path)) { + return probe_path; + } + + return String(); +} +#endif + bool load_hostfxr(void *&r_hostfxr_dll_handle) { String hostfxr_path = find_hostfxr(); @@ -182,6 +240,47 @@ bool load_hostfxr(void *&r_hostfxr_dll_handle) { hostfxr_close); } +#ifndef TOOLS_ENABLED +bool load_coreclr(void *&r_coreclr_dll_handle) { + String coreclr_path = find_coreclr(); + + bool is_monovm = false; + if (coreclr_path.is_empty()) { + // Fallback to MonoVM (should have the same API as CoreCLR). + coreclr_path = find_monosgen(); + is_monovm = true; + } + + if (coreclr_path.is_empty()) { + return false; + } + + const String coreclr_name = is_monovm ? "monosgen" : "coreclr"; + print_verbose("Found " + coreclr_name + ": " + coreclr_path); + + Error err = OS::get_singleton()->open_dynamic_library(coreclr_path, r_coreclr_dll_handle); + + if (err != OK) { + return false; + } + + void *lib = r_coreclr_dll_handle; + + void *symbol = nullptr; + + err = OS::get_singleton()->get_dynamic_library_symbol_handle(lib, "coreclr_initialize", symbol); + ERR_FAIL_COND_V(err != OK, false); + coreclr_initialize = (coreclr_initialize_fn)symbol; + + err = OS::get_singleton()->get_dynamic_library_symbol_handle(lib, "coreclr_create_delegate", symbol); + ERR_FAIL_COND_V(err != OK, false); + coreclr_create_delegate = (coreclr_create_delegate_fn)symbol; + + return (coreclr_initialize && + coreclr_create_delegate); +} +#endif + #ifdef TOOLS_ENABLED load_assembly_and_get_function_pointer_fn initialize_hostfxr_for_config(const char_t *p_config_path) { hostfxr_handle cxt = nullptr; @@ -339,6 +438,56 @@ godot_plugins_initialize_fn try_load_native_aot_library(void *&r_aot_dll_handle) } #endif +#ifndef TOOLS_ENABLED +String make_tpa_list() { + String tpa_list; + +#if defined(WINDOWS_ENABLED) + String separator = ";"; +#else + String separator = ":"; +#endif + + String assemblies_dir = GodotSharpDirs::get_api_assemblies_dir(); + PackedStringArray files = DirAccess::get_files_at(assemblies_dir); + for (const String &file : files) { + tpa_list += assemblies_dir.path_join(file); + tpa_list += separator; + } + + return tpa_list; +} + +godot_plugins_initialize_fn initialize_coreclr_and_godot_plugins(bool &r_runtime_initialized) { + godot_plugins_initialize_fn godot_plugins_initialize = nullptr; + + String assembly_name = path::get_csharp_project_name(); + + String tpa_list = make_tpa_list(); + const char *prop_keys[] = { "TRUSTED_PLATFORM_ASSEMBLIES" }; + const char *prop_values[] = { tpa_list.utf8().get_data() }; + int nprops = sizeof(prop_keys) / sizeof(prop_keys[0]); + + void *coreclr_handle = nullptr; + unsigned int domain_id = 0; + int rc = coreclr_initialize(nullptr, nullptr, nprops, (const char **)&prop_keys, (const char **)&prop_values, &coreclr_handle, &domain_id); + ERR_FAIL_COND_V_MSG(rc != 0, nullptr, ".NET: Failed to initialize CoreCLR."); + + r_runtime_initialized = true; + + print_verbose(".NET: CoreCLR initialized"); + + coreclr_create_delegate(coreclr_handle, domain_id, + assembly_name.utf8().get_data(), + "GodotPlugins.Game.Main", + "InitializeFromGameProject", + (void **)&godot_plugins_initialize); + ERR_FAIL_NULL_V_MSG(godot_plugins_initialize, nullptr, ".NET: Failed to get GodotPlugins initialization function pointer"); + + return godot_plugins_initialize; +} +#endif + } // namespace bool GDMono::should_initialize() { @@ -382,14 +531,21 @@ void GDMono::initialize() { } #endif - if (!load_hostfxr(hostfxr_dll_handle)) { + if (load_hostfxr(hostfxr_dll_handle)) { + godot_plugins_initialize = initialize_hostfxr_and_godot_plugins(runtime_initialized); + ERR_FAIL_NULL(godot_plugins_initialize); + } else { #if !defined(TOOLS_ENABLED) - godot_plugins_initialize = try_load_native_aot_library(hostfxr_dll_handle); - - if (godot_plugins_initialize != nullptr) { - is_native_aot = true; - runtime_initialized = true; + if (load_coreclr(coreclr_dll_handle)) { + godot_plugins_initialize = initialize_coreclr_and_godot_plugins(runtime_initialized); } else { + godot_plugins_initialize = try_load_native_aot_library(hostfxr_dll_handle); + if (godot_plugins_initialize != nullptr) { + runtime_initialized = true; + } + } + + if (godot_plugins_initialize == nullptr) { ERR_FAIL_MSG(".NET: Failed to load hostfxr"); } #else @@ -400,11 +556,6 @@ void GDMono::initialize() { #endif } - if (!is_native_aot) { - godot_plugins_initialize = initialize_hostfxr_and_godot_plugins(runtime_initialized); - ERR_FAIL_NULL(godot_plugins_initialize); - } - int32_t interop_funcs_size = 0; const void **interop_funcs = godotsharp::get_runtime_interop_funcs(interop_funcs_size); @@ -553,6 +704,9 @@ GDMono::~GDMono() { if (hostfxr_dll_handle) { OS::get_singleton()->close_dynamic_library(hostfxr_dll_handle); } + if (coreclr_dll_handle) { + OS::get_singleton()->close_dynamic_library(coreclr_dll_handle); + } finalizing_scripts_domain = false; runtime_initialized = false; diff --git a/modules/mono/mono_gd/gd_mono.h b/modules/mono/mono_gd/gd_mono.h index 614bfc63fb..fae3421ac9 100644 --- a/modules/mono/mono_gd/gd_mono.h +++ b/modules/mono/mono_gd/gd_mono.h @@ -64,7 +64,7 @@ class GDMono { bool finalizing_scripts_domain = false; void *hostfxr_dll_handle = nullptr; - bool is_native_aot = false; + void *coreclr_dll_handle = nullptr; String project_assembly_path; uint64_t project_assembly_modified_time = 0; diff --git a/modules/mono/thirdparty/libSystem.Security.Cryptography.Native.Android.jar b/modules/mono/thirdparty/libSystem.Security.Cryptography.Native.Android.jar Binary files differnew file mode 100755 index 0000000000..7366030881 --- /dev/null +++ b/modules/mono/thirdparty/libSystem.Security.Cryptography.Native.Android.jar diff --git a/modules/multiplayer/doc_classes/SceneMultiplayer.xml b/modules/multiplayer/doc_classes/SceneMultiplayer.xml index 7abee8e2c8..42f32d4848 100644 --- a/modules/multiplayer/doc_classes/SceneMultiplayer.xml +++ b/modules/multiplayer/doc_classes/SceneMultiplayer.xml @@ -68,7 +68,7 @@ The callback to execute when when receiving authentication data sent via [method send_auth]. If the [Callable] is empty (default), peers will be automatically accepted as soon as they connect. </member> <member name="auth_timeout" type="float" setter="set_auth_timeout" getter="get_auth_timeout" default="3.0"> - If set to a value greater than [code]0.0[/code], the maximum amount of time peers can stay in the authenticating state, after which the authentication will automatically fail. See the [signal peer_authenticating] and [signal peer_authentication_failed] signals. + If set to a value greater than [code]0.0[/code], the maximum duration in seconds peers can stay in the authenticating state, after which the authentication will automatically fail. See the [signal peer_authenticating] and [signal peer_authentication_failed] signals. </member> <member name="max_delta_packet_size" type="int" setter="set_max_delta_packet_size" getter="get_max_delta_packet_size" default="65535"> Maximum size of each delta packet. Higher values increase the chance of receiving full updates in a single frame, but also the chance of causing networking congestion (higher latency, disconnections). See [MultiplayerSynchronizer]. diff --git a/modules/multiplayer/editor/replication_editor.cpp b/modules/multiplayer/editor/replication_editor.cpp index 851ad85876..386feae4f9 100644 --- a/modules/multiplayer/editor/replication_editor.cpp +++ b/modules/multiplayer/editor/replication_editor.cpp @@ -93,24 +93,6 @@ void ReplicationEditor::_pick_node_select_recursive(TreeItem *p_item, const Stri } } -void ReplicationEditor::_pick_node_filter_input(const Ref<InputEvent> &p_ie) { - Ref<InputEventKey> k = p_ie; - - if (k.is_valid()) { - switch (k->get_keycode()) { - case Key::UP: - case Key::DOWN: - case Key::PAGEUP: - case Key::PAGEDOWN: { - pick_node->get_scene_tree()->get_scene_tree()->gui_input(k); - pick_node->get_filter_line_edit()->accept_event(); - } break; - default: - break; - } - } -} - void ReplicationEditor::_pick_node_selected(NodePath p_path) { Node *root = current->get_node(current->get_root_path()); ERR_FAIL_NULL(root); @@ -184,11 +166,9 @@ ReplicationEditor::ReplicationEditor() { pick_node = memnew(SceneTreeDialog); add_child(pick_node); - pick_node->register_text_enter(pick_node->get_filter_line_edit()); pick_node->set_title(TTR("Pick a node to synchronize:")); pick_node->connect("selected", callable_mp(this, &ReplicationEditor::_pick_node_selected)); pick_node->get_filter_line_edit()->connect(SceneStringName(text_changed), callable_mp(this, &ReplicationEditor::_pick_node_filter_text_changed)); - pick_node->get_filter_line_edit()->connect("gui_input", callable_mp(this, &ReplicationEditor::_pick_node_filter_input)); prop_selector = memnew(PropertySelector); add_child(prop_selector); diff --git a/modules/multiplayer/editor/replication_editor.h b/modules/multiplayer/editor/replication_editor.h index 8f11774292..017fa73967 100644 --- a/modules/multiplayer/editor/replication_editor.h +++ b/modules/multiplayer/editor/replication_editor.h @@ -81,7 +81,6 @@ private: void _pick_node_filter_text_changed(const String &p_newtext); void _pick_node_select_recursive(TreeItem *p_item, const String &p_filter, Vector<Node *> &p_select_candidates); - void _pick_node_filter_input(const Ref<InputEvent> &p_ie); void _pick_node_selected(NodePath p_path); void _pick_new_property(); diff --git a/modules/multiplayer/scene_rpc_interface.cpp b/modules/multiplayer/scene_rpc_interface.cpp index 69bb19c01c..0938d7ef99 100644 --- a/modules/multiplayer/scene_rpc_interface.cpp +++ b/modules/multiplayer/scene_rpc_interface.cpp @@ -118,7 +118,7 @@ const SceneRPCInterface::RPCConfigCache &SceneRPCInterface::_get_node_config(con return rpc_cache[oid]; } RPCConfigCache cache; - _parse_rpc_config(p_node->get_node_rpc_config(), true, cache); + _parse_rpc_config(p_node->get_rpc_config(), true, cache); if (p_node->get_script_instance()) { _parse_rpc_config(p_node->get_script_instance()->get_rpc_config(), false, cache); } diff --git a/modules/navigation/nav_base.h b/modules/navigation/nav_base.h index c28392acf7..d2308abfaf 100644 --- a/modules/navigation/nav_base.h +++ b/modules/navigation/nav_base.h @@ -64,7 +64,7 @@ public: void set_owner_id(ObjectID p_owner_id) { owner_id = p_owner_id; } ObjectID get_owner_id() const { return owner_id; } - virtual ~NavBase(){}; + virtual ~NavBase() {} }; #endif // NAV_BASE_H 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/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/extensions/platform/openxr_opengl_extension.cpp b/modules/openxr/extensions/platform/openxr_opengl_extension.cpp index de4a9e4b8e..caded14ca7 100644 --- a/modules/openxr/extensions/platform/openxr_opengl_extension.cpp +++ b/modules/openxr/extensions/platform/openxr_opengl_extension.cpp @@ -240,8 +240,8 @@ bool OpenXROpenGLExtension::get_swapchain_image_data(XrSwapchain p_swapchain, in Vector<RID> texture_rids; for (uint64_t i = 0; i < swapchain_length; i++) { - RID texture_rid = texture_storage->texture_create_external( - p_array_size == 1 ? GLES3::Texture::TYPE_2D : GLES3::Texture::TYPE_LAYERED, + RID texture_rid = texture_storage->texture_create_from_native_handle( + p_array_size == 1 ? RS::TEXTURE_TYPE_2D : RS::TEXTURE_TYPE_LAYERED, format, images[i].image, p_width, diff --git a/modules/openxr/openxr_api.cpp b/modules/openxr/openxr_api.cpp index 73b6f6c1c9..c67be5a2b3 100644 --- a/modules/openxr/openxr_api.cpp +++ b/modules/openxr/openxr_api.cpp @@ -1247,7 +1247,7 @@ bool OpenXRAPI::create_main_swapchains(Size2i p_size) { return false; } - set_object_name(XR_OBJECT_TYPE_SWAPCHAIN, uint64_t(render_state.main_swapchains[OPENXR_SWAPCHAIN_COLOR].get_swapchain()), "Main depth swapchain"); + set_object_name(XR_OBJECT_TYPE_SWAPCHAIN, uint64_t(render_state.main_swapchains[OPENXR_SWAPCHAIN_DEPTH].get_swapchain()), "Main depth swapchain"); } // We create our velocity swapchain if: diff --git a/modules/raycast/godot_update_embree.py b/modules/raycast/godot_update_embree.py index c179060365..c4fff330c9 100644 --- a/modules/raycast/godot_update_embree.py +++ b/modules/raycast/godot_update_embree.py @@ -4,8 +4,8 @@ import re import shutil import stat import subprocess -from types import TracebackType -from typing import Any, Callable, Tuple, Type +import sys +from typing import Any, Callable git_tag = "v4.3.1" @@ -100,9 +100,7 @@ subprocess.run(["git", "checkout", git_tag]) commit_hash = str(subprocess.check_output(["git", "rev-parse", "HEAD"], universal_newlines=True)).strip() -def on_rm_error( - function: Callable[..., Any], path: str, excinfo: Tuple[Type[Exception], Exception, TracebackType] -) -> None: +def on_rm_error(function: Callable[..., Any], path: str, excinfo: Exception) -> None: """ Error handler for `shutil.rmtree()`. @@ -113,10 +111,12 @@ def on_rm_error( os.unlink(path) -# 3.12 Python and beyond should replace `onerror` with `onexc`. # We remove the .git directory because it contains # a lot of read-only files that are problematic on Windows. -shutil.rmtree(".git", onerror=on_rm_error) +if sys.version_info >= (3, 12): + shutil.rmtree(".git", onexc=on_rm_error) +else: + shutil.rmtree(".git", onerror=on_rm_error) # type: ignore all_files = set(cpp_files) diff --git a/modules/text_server_adv/text_server_adv.h b/modules/text_server_adv/text_server_adv.h index 448be9ebe4..c63389b1c6 100644 --- a/modules/text_server_adv/text_server_adv.h +++ b/modules/text_server_adv/text_server_adv.h @@ -705,7 +705,7 @@ class TextServerAdvanced : public TextServerExtension { }; protected: - static void _bind_methods(){}; + static void _bind_methods() {} void full_copy(ShapedTextDataAdvanced *p_shaped); void invalidate(ShapedTextDataAdvanced *p_shaped, bool p_text = false); diff --git a/modules/text_server_fb/text_server_fb.h b/modules/text_server_fb/text_server_fb.h index ee1f72401f..7f12ad593b 100644 --- a/modules/text_server_fb/text_server_fb.h +++ b/modules/text_server_fb/text_server_fb.h @@ -574,7 +574,7 @@ class TextServerFallback : public TextServerExtension { Mutex ft_mutex; protected: - static void _bind_methods(){}; + static void _bind_methods() {} void full_copy(ShapedTextDataFallback *p_shaped); void invalidate(ShapedTextDataFallback *p_shaped); 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/webxr/webxr_interface_js.cpp b/modules/webxr/webxr_interface_js.cpp index 78a9db9b19..352d495dd4 100644 --- a/modules/webxr/webxr_interface_js.cpp +++ b/modules/webxr/webxr_interface_js.cpp @@ -561,8 +561,8 @@ RID WebXRInterfaceJS::_get_texture(unsigned int p_texture_id) { uint32_t view_count = godot_webxr_get_view_count(); Size2 texture_size = get_render_target_size(); - RID texture = texture_storage->texture_create_external( - view_count == 1 ? GLES3::Texture::TYPE_2D : GLES3::Texture::TYPE_LAYERED, + RID texture = texture_storage->texture_create_from_native_handle( + view_count == 1 ? RS::TEXTURE_TYPE_2D : RS::TEXTURE_TYPE_LAYERED, Image::FORMAT_RGBA8, p_texture_id, (int)texture_size.width, |