diff options
Diffstat (limited to 'modules')
64 files changed, 20693 insertions, 112 deletions
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/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/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/mono/csharp_script.cpp b/modules/mono/csharp_script.cpp index f47e6d209a..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(); } } 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/GodotSharpEditor.cs b/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs index d3899c809a..e84b4e92c7 100644 --- a/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs +++ b/modules/mono/editor/GodotTools/GodotTools/GodotSharpEditor.cs @@ -259,11 +259,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 3222c58c4e..8081c21ea0 100644 --- a/modules/mono/editor/bindings_generator.cpp +++ b/modules/mono/editor/bindings_generator.cpp @@ -1452,7 +1452,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"; @@ -1463,7 +1463,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); @@ -2313,7 +2313,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 << ", "; @@ -2333,7 +2333,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) @@ -2552,7 +2552,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) + "'."); @@ -2651,7 +2651,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 + "'."); @@ -2690,7 +2690,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 + "'."); @@ -2934,17 +2934,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 + "'."); @@ -2959,18 +2954,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 @@ -3013,14 +3003,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++; } @@ -3113,7 +3109,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; @@ -3129,7 +3125,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); @@ -3389,7 +3385,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/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/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/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, |