diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp index 5aadfb6e11bc..3cbc28a49819 100644 --- a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp +++ b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp @@ -78,6 +78,7 @@ void JoltSoftBody3D::_space_changing() { if (jolt_body != nullptr) { jolt_settings = new JPH::SoftBodyCreationSettings(jolt_body->GetSoftBodyCreationSettings()); jolt_settings->mSettings = nullptr; + jolt_settings->mVertexRadius = JoltProjectSettings::soft_body_point_radius; } _deref_shared_data(); @@ -146,8 +147,6 @@ bool JoltSoftBody3D::_ref_shared_data() { LocalVector &mesh_to_physics = iter_shared_data->value.mesh_to_physics; JPH::SoftBodySharedSettings &settings = *iter_shared_data->value.settings; - settings.mVertexRadius = JoltProjectSettings::soft_body_point_radius; - JPH::Array &physics_vertices = settings.mVertices; JPH::Array &physics_faces = settings.mFaces; diff --git a/thirdparty/README.md b/thirdparty/README.md index 1cd27a5af28f..0a551cc90f58 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -492,7 +492,7 @@ Files generated from upstream source: ## jolt_physics - Upstream: https://github.com/jrouwe/JoltPhysics -- Version: 5.3.0 (0373ec0dd762e4bc2f6acdb08371ee84fa23c6db, 2025) +- Version: 5.4.0 (036ea7b1d717b3e713ac9d8cbd47118fb9cd5d60, 2025) - License: MIT Files extracted from upstream source: diff --git a/thirdparty/jolt_physics/Jolt/ConfigurationString.h b/thirdparty/jolt_physics/Jolt/ConfigurationString.h index 1ff1969b2412..ef502e0db844 100644 --- a/thirdparty/jolt_physics/Jolt/ConfigurationString.h +++ b/thirdparty/jolt_physics/Jolt/ConfigurationString.h @@ -83,6 +83,9 @@ inline const char *GetConfigurationString() #ifdef JPH_PROFILE_ENABLED "(Profile) " #endif +#ifdef JPH_EXTERNAL_PROFILE + "(External Profile) " +#endif #if defined(JPH_OBJECT_LAYER_BITS) && JPH_OBJECT_LAYER_BITS == 32 "(32-bit ObjectLayer) " #else diff --git a/thirdparty/jolt_physics/Jolt/Core/Array.h b/thirdparty/jolt_physics/Jolt/Core/Array.h index b8dbce595355..5f5f4e2c0b40 100644 --- a/thirdparty/jolt_physics/Jolt/Core/Array.h +++ b/thirdparty/jolt_physics/Jolt/Core/Array.h @@ -69,8 +69,8 @@ class [[nodiscard]] Array : private Allocator rev_it & operator -- () { ++mValue; return *this; } rev_it operator -- (int) { return rev_it(mValue++); } - rev_it operator + (int inValue) { return rev_it(mValue - inValue); } - rev_it operator - (int inValue) { return rev_it(mValue + inValue); } + rev_it operator + (int inValue) const { return rev_it(mValue - inValue); } + rev_it operator - (int inValue) const { return rev_it(mValue + inValue); } rev_it & operator += (int inValue) { mValue -= inValue; return *this; } rev_it & operator -= (int inValue) { mValue += inValue; return *this; } diff --git a/thirdparty/jolt_physics/Jolt/Core/Core.h b/thirdparty/jolt_physics/Jolt/Core/Core.h index dfd2a04d2f4a..b306f5a6868a 100644 --- a/thirdparty/jolt_physics/Jolt/Core/Core.h +++ b/thirdparty/jolt_physics/Jolt/Core/Core.h @@ -6,7 +6,7 @@ // Jolt library version #define JPH_VERSION_MAJOR 5 -#define JPH_VERSION_MINOR 3 +#define JPH_VERSION_MINOR 4 #define JPH_VERSION_PATCH 0 // Determine which features the library was compiled with @@ -437,6 +437,11 @@ #define JPH_SUPPRESS_WARNINGS_STD_END \ JPH_SUPPRESS_WARNING_POP +// MSVC STL requires _HAS_EXCEPTIONS=0 if exceptions are turned off +#if defined(JPH_COMPILER_MSVC) && (!defined(__cpp_exceptions) || !__cpp_exceptions) && !defined(_HAS_EXCEPTIONS) + #define _HAS_EXCEPTIONS 0 +#endif + // Standard C++ includes JPH_SUPPRESS_WARNINGS_STD_BEGIN #include @@ -448,7 +453,7 @@ JPH_SUPPRESS_WARNINGS_STD_BEGIN #include #include #include -#ifdef JPH_COMPILER_MSVC +#if defined(JPH_COMPILER_MSVC) || (defined(JPH_COMPILER_CLANG) && defined(_MSC_VER)) // MSVC or clang-cl #include // for alloca #endif #if defined(JPH_USE_SSE) diff --git a/thirdparty/jolt_physics/Jolt/Core/HashTable.h b/thirdparty/jolt_physics/Jolt/Core/HashTable.h index d2d766fe0f21..822cb284ebec 100644 --- a/thirdparty/jolt_physics/Jolt/Core/HashTable.h +++ b/thirdparty/jolt_physics/Jolt/Core/HashTable.h @@ -364,6 +364,8 @@ class HashTable using Base = IteratorBase; public: + using IteratorBase::operator ==; + /// Properties using reference = typename Base::value_type &; using pointer = typename Base::value_type *; @@ -401,6 +403,8 @@ class HashTable using Base = IteratorBase; public: + using IteratorBase::operator ==; + /// Properties using reference = const typename Base::value_type &; using pointer = const typename Base::value_type *; diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.h b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h index 7701bd2dcd8a..22b47c1e9444 100644 --- a/thirdparty/jolt_physics/Jolt/Core/TickCounter.h +++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h @@ -11,6 +11,8 @@ #include #elif defined(JPH_CPU_E2K) #include +#elif defined(JPH_CPU_LOONGARCH) + #include #endif JPH_NAMESPACE_BEGIN @@ -35,7 +37,16 @@ JPH_INLINE uint64 GetProcessorTickCount() uint64 val; asm volatile("mrs %0, cntvct_el0" : "=r" (val)); return val; -#elif defined(JPH_CPU_ARM) || defined(JPH_CPU_RISCV) || defined(JPH_CPU_WASM) || defined(JPH_CPU_PPC) || defined(JPH_CPU_LOONGARCH) +#elif defined(JPH_CPU_LOONGARCH) + #if JPH_CPU_ADDRESS_BITS == 64 + __drdtime_t t = __rdtime_d(); + return t.dvalue; + #else + __rdtime_t h = __rdtimeh_w(); + __rdtime_t l = __rdtimel_w(); + return ((uint64)h.value << 32) + l.value; + #endif +#elif defined(JPH_CPU_ARM) || defined(JPH_CPU_RISCV) || defined(JPH_CPU_WASM) || defined(JPH_CPU_PPC) return 0; // Not supported #else #error Undefined diff --git a/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h index 49897019396e..3100952026ad 100644 --- a/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h +++ b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h @@ -103,8 +103,6 @@ class EPAPenetrationDepth template EStatus GetPenetrationDepthStepGJK(const AE &inAExcludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, float inConvexRadiusB, float inTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) { - JPH_PROFILE_FUNCTION(); - JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inTolerance;) // Don't supply a zero ioV, we only want to get points on the hull of the Minkowsky sum and not internal points. diff --git a/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h b/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h index b375e1675476..2622d6fc19dd 100644 --- a/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h +++ b/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h @@ -551,7 +551,7 @@ class GJKClosestPoint : public NonCopyable #ifdef JPH_GJK_DEBUG Trace("v . r = %g", (double)v_dot_r); #endif - if (v_dot_r >= 0.0f) + if (v_dot_r >= -1.0e-18f) // Instead of checking >= 0, check with epsilon as we don't want the division below to overflow to infinity as it can cause a float exception return false; // Update the lower bound for lambda @@ -744,7 +744,7 @@ class GJKClosestPoint : public NonCopyable #ifdef JPH_GJK_DEBUG Trace("v . r = %g", (double)v_dot_r); #endif - if (v_dot_r >= 0.0f) + if (v_dot_r >= -1.0e-18f) // Instead of checking >= 0, check with epsilon as we don't want the division below to overflow to infinity as it can cause a float exception return false; // Update the lower bound for lambda diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Plane.h b/thirdparty/jolt_physics/Jolt/Geometry/Plane.h index 7e3b8a8c4fed..e62507eb9bca 100644 --- a/thirdparty/jolt_physics/Jolt/Geometry/Plane.h +++ b/thirdparty/jolt_physics/Jolt/Geometry/Plane.h @@ -32,6 +32,9 @@ class [[nodiscard]] Plane float GetConstant() const { return mNormalAndConstant.GetW(); } void SetConstant(float inConstant) { mNormalAndConstant.SetW(inConstant); } + /// Store as 4 floats + void StoreFloat4(Float4 *outV) const { mNormalAndConstant.StoreFloat4(outV); } + /// Offset the plane (positive value means move it in the direction of the plane normal) Plane Offset(float inDistance) const { return Plane(mNormalAndConstant - Vec4(Vec3::sZero(), inDistance)); } diff --git a/thirdparty/jolt_physics/Jolt/Math/DMat44.h b/thirdparty/jolt_physics/Jolt/Math/DMat44.h index cd8042107ada..cf585c8f3bd6 100644 --- a/thirdparty/jolt_physics/Jolt/Math/DMat44.h +++ b/thirdparty/jolt_physics/Jolt/Math/DMat44.h @@ -9,7 +9,7 @@ JPH_NAMESPACE_BEGIN /// Holds a 4x4 matrix of floats with the last column consisting of doubles -class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DMat44 +class [[nodiscard]] alignas(max(JPH_VECTOR_ALIGNMENT, JPH_DVECTOR_ALIGNMENT)) DMat44 { public: JPH_OVERRIDE_NEW_DELETE diff --git a/thirdparty/jolt_physics/Jolt/Math/Float4.h b/thirdparty/jolt_physics/Jolt/Math/Float4.h index ca292b3ac3e4..454c9c279362 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Float4.h +++ b/thirdparty/jolt_physics/Jolt/Math/Float4.h @@ -15,6 +15,7 @@ class [[nodiscard]] Float4 Float4() = default; ///< Intentionally not initialized for performance reasons Float4(const Float4 &inRHS) = default; Float4(float inX, float inY, float inZ, float inW) : x(inX), y(inY), z(inZ), w(inW) { } + Float4 & operator = (const Float4 &inRHS) = default; float operator [] (int inCoordinate) const { @@ -22,6 +23,16 @@ class [[nodiscard]] Float4 return *(&x + inCoordinate); } + bool operator == (const Float4 &inRHS) const + { + return x == inRHS.x && y == inRHS.y && z == inRHS.z && w == inRHS.w; + } + + bool operator != (const Float4 &inRHS) const + { + return x != inRHS.x || y != inRHS.y || z != inRHS.z || w != inRHS.w; + } + float x; float y; float z; diff --git a/thirdparty/jolt_physics/Jolt/Math/Mat44.inl b/thirdparty/jolt_physics/Jolt/Math/Mat44.inl index 76577b7153a6..f04533f27f6e 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Mat44.inl +++ b/thirdparty/jolt_physics/Jolt/Math/Mat44.inl @@ -838,18 +838,18 @@ Quat Mat44::GetQuaternion() const Mat44 Mat44::sQuatLeftMultiply(QuatArg inQ) { return Mat44( - Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle(), - Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle(), - Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle(), + inQ.mValue.Swizzle().FlipSign<1, 1, -1, -1>(), + inQ.mValue.Swizzle().FlipSign<-1, 1, 1, -1>(), + inQ.mValue.Swizzle().FlipSign<1, -1, 1, -1>(), inQ.mValue); } Mat44 Mat44::sQuatRightMultiply(QuatArg inQ) { return Mat44( - Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle(), - Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle(), - Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle(), + inQ.mValue.Swizzle().FlipSign<1, -1, 1, -1>(), + inQ.mValue.Swizzle().FlipSign<1, 1, -1, -1>(), + inQ.mValue.Swizzle().FlipSign<-1, 1, 1, -1>(), inQ.mValue); } diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.h b/thirdparty/jolt_physics/Jolt/Math/Quat.h index a68e45be294f..6e7e3bef6ce1 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Quat.h +++ b/thirdparty/jolt_physics/Jolt/Math/Quat.h @@ -40,6 +40,7 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat Quat(const Quat &inRHS) = default; Quat & operator = (const Quat &inRHS) = default; inline Quat(float inX, float inY, float inZ, float inW) : mValue(inX, inY, inZ, inW) { } + inline explicit Quat(const Float4 &inV) : mValue(Vec4::sLoadFloat4(&inV)) { } inline explicit Quat(Vec4Arg inV) : mValue(inV) { } ///@} @@ -159,6 +160,9 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat /// Rotate a vector by this quaternion JPH_INLINE Vec3 operator * (Vec3Arg inValue) const; + /// Multiply a quaternion with imaginary components and no real component (x, y, z, 0) with a quaternion + static JPH_INLINE Quat sMultiplyImaginary(Vec3Arg inLHS, QuatArg inRHS); + /// Rotate a vector by the inverse of this quaternion JPH_INLINE Vec3 InverseRotate(Vec3Arg inValue) const; @@ -175,7 +179,7 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat JPH_INLINE float Dot(QuatArg inRHS) const { return mValue.Dot(inRHS.mValue); } /// The conjugate [w, -x, -y, -z] is the same as the inverse for unit quaternions - JPH_INLINE Quat Conjugated() const { return Quat(Vec4::sXor(mValue, UVec4(0x80000000, 0x80000000, 0x80000000, 0).ReinterpretAsFloat())); } + JPH_INLINE Quat Conjugated() const { return Quat(mValue.FlipSign<-1, -1, -1, 1>()); } /// Get inverse quaternion JPH_INLINE Quat Inversed() const { return Conjugated() / Length(); } @@ -184,7 +188,7 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat JPH_INLINE Quat EnsureWPositive() const { return Quat(Vec4::sXor(mValue, Vec4::sAnd(mValue.SplatW(), UVec4::sReplicate(0x80000000).ReinterpretAsFloat()))); } /// Get a quaternion that is perpendicular to this quaternion - JPH_INLINE Quat GetPerpendicular() const { return Quat(Vec4(1, -1, 1, -1) * mValue.Swizzle()); } + JPH_INLINE Quat GetPerpendicular() const { return Quat(mValue.Swizzle().FlipSign<1, -1, 1, -1>()); } /// Get rotation angle around inAxis (uses Swing Twist Decomposition to get the twist quaternion and uses q(axis, angle) = [cos(angle / 2), axis * sin(angle / 2)]) JPH_INLINE float GetRotationAngle(Vec3Arg inAxis) const { return GetW() == 0.0f? JPH_PI : 2.0f * ATan(GetXYZ().Dot(inAxis) / GetW()); } @@ -238,9 +242,18 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat /// Load 3 floats from memory (X, Y and Z component and then calculates W) reads 32 bits extra which it doesn't use static JPH_INLINE Quat sLoadFloat3Unsafe(const Float3 &inV); - /// Store 3 as floats to memory (X, Y and Z component) + /// Store as 3 floats to memory (X, Y and Z component). Ensures that W is positive before storing. JPH_INLINE void StoreFloat3(Float3 *outV) const; + /// Store as 4 floats + JPH_INLINE void StoreFloat4(Float4 *outV) const; + + /// Compress a unit quaternion to a 32 bit value, precision is around 0.5 degree + JPH_INLINE uint32 CompressUnitQuat() const { return mValue.CompressUnitVector(); } + + /// Decompress a unit quaternion from a 32 bit value + JPH_INLINE static Quat sDecompressUnitQuat(uint32 inValue) { return Quat(Vec4::sDecompressUnitVector(inValue)); } + /// To String friend ostream & operator << (ostream &inStream, QuatArg inQ) { inStream << inQ.mValue; return inStream; } diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.inl b/thirdparty/jolt_physics/Jolt/Math/Quat.inl index 201481929d55..57e1947b3066 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Quat.inl +++ b/thirdparty/jolt_physics/Jolt/Math/Quat.inl @@ -71,6 +71,60 @@ Quat Quat::operator * (QuatArg inRHS) const #endif } +Quat Quat::sMultiplyImaginary(Vec3Arg inLHS, QuatArg inRHS) +{ +#if defined(JPH_USE_SSE4_1) + __m128 abc0 = inLHS.mValue; + __m128 xyzw = inRHS.mValue.mValue; + + // [a,a,a,a] * [w,y,z,x] = [aw,ay,az,ax] + __m128 aaaa = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(0, 0, 0, 0)); + __m128 xzyw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 1, 2, 0)); + __m128 axazayaw = _mm_mul_ps(aaaa, xzyw); + + // [b,b,b,b] * [z,x,w,y] = [bz,bx,bw,by] + __m128 bbbb = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(1, 1, 1, 1)); + __m128 ywxz = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 0, 3, 1)); + __m128 bybwbxbz = _mm_mul_ps(bbbb, ywxz); + + // [c,c,c,c] * [w,z,x,y] = [cw,cz,cx,cy] + __m128 cccc = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(2, 2, 2, 2)); + __m128 yxzw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 2, 0, 1)); + __m128 cycxczcw = _mm_mul_ps(cccc, yxzw); + + // [+aw,+ay,-az,-ax] + __m128 e = _mm_xor_ps(axazayaw, _mm_set_ps(0.0f, 0.0f, -0.0f, -0.0f)); + + // [+aw,+ay,-az,-ax] + -[bz,bx,bw,by] = [+aw+bz,+ay-bx,-az+bw,-ax-by] + e = _mm_addsub_ps(e, bybwbxbz); + + // [+ay-bx,-ax-by,-az+bw,+aw+bz] + e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 0, 1, 3)); + + // [+ay-bx,-ax-by,-az+bw,+aw+bz] + -[cw,cz,cx,cy] = [+ay-bx+cw,-ax-by-cz,-az+bw+cx,+aw+bz-cy] + e = _mm_addsub_ps(e, cycxczcw); + + // [-ax-by-cz,+ay-bx+cw,-az+bw+cx,+aw+bz-cy] + return Quat(Vec4(_mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 3, 1, 0)))); +#else + float lx = inLHS.GetX(); + float ly = inLHS.GetY(); + float lz = inLHS.GetZ(); + + float rx = inRHS.mValue.GetX(); + float ry = inRHS.mValue.GetY(); + float rz = inRHS.mValue.GetZ(); + float rw = inRHS.mValue.GetW(); + + float x = (lx * rw) + ly * rz - lz * ry; + float y = -(lx * rz) + ly * rw + lz * rx; + float z = (lx * ry) - ly * rx + lz * rw; + float w = -(lx * rx) - ly * ry - lz * rz; + + return Quat(x, y, z, w); +#endif +} + Quat Quat::sRotation(Vec3Arg inAxis, float inAngle) { // returns [inAxis * sin(0.5f * inAngle), cos(0.5f * inAngle)] @@ -274,42 +328,61 @@ Quat Quat::SLERP(QuatArg inDestination, float inFraction) const Vec3 Quat::operator * (Vec3Arg inValue) const { - // Rotating a vector by a quaternion is done by: p' = q * p * q^-1 (q^-1 = conjugated(q) for a unit quaternion) + // Rotating a vector by a quaternion is done by: p' = q * (p, 0) * q^-1 (q^-1 = conjugated(q) for a unit quaternion) + // Using Rodrigues formula: https://en.m.wikipedia.org/wiki/Euler%E2%80%93Rodrigues_formula + // This is equivalent to: p' = p + 2 * (q.w * q.xyz x p + q.xyz x (q.xyz x p)) + // + // This is: + // + // Vec3 xyz = GetXYZ(); + // Vec3 q_cross_p = xyz.Cross(inValue); + // Vec3 q_cross_q_cross_p = xyz.Cross(q_cross_p); + // Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p; + // return inValue + (v + v); + // + // But we can write out the cross products in a more efficient way: JPH_ASSERT(IsNormalized()); - return Vec3((*this * Quat(Vec4(inValue, 0)) * Conjugated()).mValue); + Vec3 xyz = GetXYZ(); + Vec3 yzx = xyz.Swizzle(); + Vec3 q_cross_p = (inValue.Swizzle() * xyz - yzx * inValue).Swizzle(); + Vec3 q_cross_q_cross_p = (q_cross_p.Swizzle() * xyz - yzx * q_cross_p).Swizzle(); + Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p; + return inValue + (v + v); } Vec3 Quat::InverseRotate(Vec3Arg inValue) const { JPH_ASSERT(IsNormalized()); - return Vec3((Conjugated() * Quat(Vec4(inValue, 0)) * *this).mValue); + Vec3 xyz = GetXYZ(); // Needs to be negated, but we do this in the equations below + Vec3 yzx = xyz.Swizzle(); + Vec3 q_cross_p = (yzx * inValue - inValue.Swizzle() * xyz).Swizzle(); + Vec3 q_cross_q_cross_p = (yzx * q_cross_p - q_cross_p.Swizzle() * xyz).Swizzle(); + Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p; + return inValue + (v + v); } Vec3 Quat::RotateAxisX() const { // This is *this * Vec3::sAxisX() written out: JPH_ASSERT(IsNormalized()); - float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); - float tx = 2.0f * x, tw = 2.0f * w; - return Vec3(tx * x + tw * w - 1.0f, tx * y + z * tw, tx * z - y * tw); + Vec4 t = mValue + mValue; + return Vec3(t.SplatX() * mValue + (t.SplatW() * mValue.Swizzle()).FlipSign<1, 1, -1, 1>() - Vec4(1, 0, 0, 0)); } Vec3 Quat::RotateAxisY() const { // This is *this * Vec3::sAxisY() written out: JPH_ASSERT(IsNormalized()); - float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); - float ty = 2.0f * y, tw = 2.0f * w; - return Vec3(x * ty - z * tw, tw * w + ty * y - 1.0f, x * tw + ty * z); + Vec4 t = mValue + mValue; + return Vec3(t.SplatY() * mValue + (t.SplatW() * mValue.Swizzle()).FlipSign<-1, 1, 1, 1>() - Vec4(0, 1, 0, 0)); } Vec3 Quat::RotateAxisZ() const { // This is *this * Vec3::sAxisZ() written out: JPH_ASSERT(IsNormalized()); - float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); - float tz = 2.0f * z, tw = 2.0f * w; - return Vec3(x * tz + y * tw, y * tz - x * tw, tw * w + tz * z - 1.0f); + Vec4 t = mValue + mValue; + return Vec3(t.SplatZ() * mValue + (t.SplatW() * mValue.Swizzle()).FlipSign<1, -1, 1, 1>() - Vec4(0, 0, 1, 0)); } void Quat::StoreFloat3(Float3 *outV) const @@ -318,6 +391,11 @@ void Quat::StoreFloat3(Float3 *outV) const EnsureWPositive().GetXYZ().StoreFloat3(outV); } +void Quat::StoreFloat4(Float4 *outV) const +{ + mValue.StoreFloat4(outV); +} + Quat Quat::sLoadFloat3Unsafe(const Float3 &inV) { Vec3 v = Vec3::sLoadFloat3Unsafe(inV); diff --git a/thirdparty/jolt_physics/Jolt/Math/UVec4.h b/thirdparty/jolt_physics/Jolt/Math/UVec4.h index 3f22d43ca6e1..0c30f1f2a01c 100644 --- a/thirdparty/jolt_physics/Jolt/Math/UVec4.h +++ b/thirdparty/jolt_physics/Jolt/Math/UVec4.h @@ -115,15 +115,21 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) UVec4 JPH_INLINE uint32 operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 4); return mU32[inCoordinate]; } JPH_INLINE uint32 & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 4); return mU32[inCoordinate]; } - /// Multiplies each of the 4 integer components with an integer (discards any overflow) + /// Component wise multiplication of two integer vectors (stores low 32 bits of result only) JPH_INLINE UVec4 operator * (UVec4Arg inV2) const; - /// Adds an integer value to all integer components (discards any overflow) - JPH_INLINE UVec4 operator + (UVec4Arg inV2); + /// Add two integer vectors (component wise) + JPH_INLINE UVec4 operator + (UVec4Arg inV2) const; /// Add two integer vectors (component wise) JPH_INLINE UVec4 & operator += (UVec4Arg inV2); + /// Subtract two integer vectors (component wise) + JPH_INLINE UVec4 operator - (UVec4Arg inV2) const; + + /// Subtract two integer vectors (component wise) + JPH_INLINE UVec4 & operator -= (UVec4Arg inV2); + /// Replicate the X component to all components JPH_INLINE UVec4 SplatX() const; @@ -142,6 +148,12 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) UVec4 /// Reinterpret UVec4 as a Vec4 (doesn't change the bits) JPH_INLINE Vec4 ReinterpretAsFloat() const; + /// Dot product, returns the dot product in X, Y, Z and W components + JPH_INLINE UVec4 DotV(UVec4Arg inV2) const; + + /// Dot product + JPH_INLINE uint32 Dot(UVec4Arg inV2) const; + /// Store 4 ints to memory JPH_INLINE void StoreInt4(uint32 *outV) const; diff --git a/thirdparty/jolt_physics/Jolt/Math/UVec4.inl b/thirdparty/jolt_physics/Jolt/Math/UVec4.inl index f9014acc28dc..6a0be910b375 100644 --- a/thirdparty/jolt_physics/Jolt/Math/UVec4.inl +++ b/thirdparty/jolt_physics/Jolt/Math/UVec4.inl @@ -255,7 +255,7 @@ UVec4 UVec4::operator * (UVec4Arg inV2) const #endif } -UVec4 UVec4::operator + (UVec4Arg inV2) +UVec4 UVec4::operator + (UVec4Arg inV2) const { #if defined(JPH_USE_SSE) return _mm_add_epi32(mValue, inV2.mValue); @@ -282,6 +282,33 @@ UVec4 &UVec4::operator += (UVec4Arg inV2) return *this; } +UVec4 UVec4::operator - (UVec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vsubq_u32(mValue, inV2.mValue); +#else + return UVec4(mU32[0] - inV2.mU32[0], + mU32[1] - inV2.mU32[1], + mU32[2] - inV2.mU32[2], + mU32[3] - inV2.mU32[3]); +#endif +} + +UVec4 &UVec4::operator -= (UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_sub_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vsubq_u32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mU32[i] -= inV2.mU32[i]; +#endif + return *this; +} + UVec4 UVec4::SplatX() const { #if defined(JPH_USE_SSE) @@ -348,6 +375,34 @@ Vec4 UVec4::ReinterpretAsFloat() const #endif } +UVec4 UVec4::DotV(UVec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + __m128i mul = _mm_mullo_epi32(mValue, inV2.mValue); + __m128i sum = _mm_add_epi32(mul, _mm_shuffle_epi32(mul, _MM_SHUFFLE(2, 3, 0, 1))); + return _mm_add_epi32(sum, _mm_shuffle_epi32(sum, _MM_SHUFFLE(1, 0, 3, 2))); +#elif defined(JPH_USE_NEON) + uint32x4_t mul = vmulq_u32(mValue, inV2.mValue); + return vdupq_n_u32(vaddvq_u32(mul)); +#else + return UVec4::sReplicate(mU32[0] * inV2.mU32[0] + mU32[1] * inV2.mU32[1] + mU32[2] * inV2.mU32[2] + mU32[3] * inV2.mU32[3]); +#endif +} + +uint32 UVec4::Dot(UVec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + __m128i mul = _mm_mullo_epi32(mValue, inV2.mValue); + __m128i sum = _mm_add_epi32(mul, _mm_shuffle_epi32(mul, _MM_SHUFFLE(2, 3, 0, 1))); + return _mm_cvtsi128_si32(_mm_add_epi32(sum, _mm_shuffle_epi32(sum, _MM_SHUFFLE(1, 0, 3, 2)))); +#elif defined(JPH_USE_NEON) + uint32x4_t mul = vmulq_u32(mValue, inV2.mValue); + return vaddvq_u32(mul); +#else + return mU32[0] * inV2.mU32[0] + mU32[1] * inV2.mU32[1] + mU32[2] * inV2.mU32[2] + mU32[3] * inV2.mU32[3]; +#endif +} + void UVec4::StoreInt4(uint32 *outV) const { #if defined(JPH_USE_SSE) diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp b/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp index c865387f79e6..636efa2bece5 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp @@ -12,7 +12,7 @@ static void sAddVertex(StaticArray &ioVertices, Vec3Arg inVertex) { bool found = false; for (const Vec3 &v : ioVertices) - if (v == inVertex) + if (v.IsClose(inVertex, 1.0e-6f)) { found = true; break; diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.h b/thirdparty/jolt_physics/Jolt/Math/Vec3.h index 76d42fb48c03..60df89d81430 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Vec3.h +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.h @@ -271,6 +271,16 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec3 /// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative) JPH_INLINE Vec3 GetSign() const; + /// Flips the signs of the components, e.g. FlipSign<-1, 1, -1>() will flip the signs of the X and Z components + template + JPH_INLINE Vec3 FlipSign() const; + + /// Compress a unit vector to a 32 bit value, precision is around 10^-4 + JPH_INLINE uint32 CompressUnitVector() const; + + /// Decompress a unit vector from a 32 bit value + JPH_INLINE static Vec3 sDecompressUnitVector(uint32 inValue); + /// To String friend ostream & operator << (ostream &inStream, Vec3Arg inV) { diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.inl b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl index cea60f4d8853..bd40c6875447 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Vec3.inl +++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl @@ -857,4 +857,82 @@ Vec3 Vec3::GetSign() const #endif } +template +JPH_INLINE Vec3 Vec3::FlipSign() const +{ + static_assert(X == 1 || X == -1, "X must be 1 or -1"); + static_assert(Y == 1 || Y == -1, "Y must be 1 or -1"); + static_assert(Z == 1 || Z == -1, "Z must be 1 or -1"); + return Vec3::sXor(*this, Vec3(X > 0? 0.0f : -0.0f, Y > 0? 0.0f : -0.0f, Z > 0? 0.0f : -0.0f)); +} + +uint32 Vec3::CompressUnitVector() const +{ + constexpr float cOneOverSqrt2 = 0.70710678f; + constexpr uint cNumBits = 14; + constexpr uint cMask = (1 << cNumBits) - 1; + + // Store sign bit + Vec3 v = *this; + uint32 max_element = v.Abs().GetHighestComponentIndex(); + uint32 value = 0; + if (v[max_element] < 0.0f) + { + value = 0x80000000u; + v = -v; + } + + // Store highest component + value |= max_element << 29; + + // Store the other two components in a compressed format + UVec4 compressed = Vec3::sClamp((v + Vec3::sReplicate(cOneOverSqrt2)) * (float(cMask) / (2.0f * cOneOverSqrt2)) + Vec3::sReplicate(0.5f), Vec3::sZero(), Vec3::sReplicate(cMask)).ToInt(); + switch (max_element) + { + case 0: + compressed = compressed.Swizzle(); + break; + + case 1: + compressed = compressed.Swizzle(); + break; + } + + value |= compressed.GetX(); + value |= compressed.GetY() << cNumBits; + return value; +} + +Vec3 Vec3::sDecompressUnitVector(uint32 inValue) +{ + constexpr float cOneOverSqrt2 = 0.70710678f; + constexpr uint cNumBits = 14; + constexpr uint cMask = (1u << cNumBits) - 1; + + // Restore two components + Vec3 v = Vec3(UVec4(inValue & cMask, (inValue >> cNumBits) & cMask, 0, 0).ToFloat()) * (2.0f * cOneOverSqrt2 / float(cMask)) - Vec3(cOneOverSqrt2, cOneOverSqrt2, 0.0f); + JPH_ASSERT(v.GetZ() == 0.0f); + + // Restore the highest component + v.SetZ(sqrt(max(1.0f - v.LengthSq(), 0.0f))); + + // Extract sign + if ((inValue & 0x80000000u) != 0) + v = -v; + + // Swizzle the components in place + switch ((inValue >> 29) & 3) + { + case 0: + v = v.Swizzle(); + break; + + case 1: + v = v.Swizzle(); + break; + } + + return v; +} + JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.h b/thirdparty/jolt_physics/Jolt/Math/Vec4.h index 4098626a7805..535956446682 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Vec4.h +++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.h @@ -63,6 +63,9 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4 /// Return the maximum of each of the components static JPH_INLINE Vec4 sMax(Vec4Arg inV1, Vec4Arg inV2); + /// Clamp a vector between min and max (component wise) + static JPH_INLINE Vec4 sClamp(Vec4Arg inV, Vec4Arg inMin, Vec4Arg inMax); + /// Equals (component wise) static JPH_INLINE UVec4 sEquals(Vec4Arg inV1, Vec4Arg inV2); @@ -139,6 +142,9 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4 /// Test if two vectors are close JPH_INLINE bool IsClose(Vec4Arg inV2, float inMaxDistSq = 1.0e-12f) const; + /// Test if vector is near zero + JPH_INLINE bool IsNearZero(float inMaxDistSq = 1.0e-12f) const; + /// Test if vector is normalized JPH_INLINE bool IsNormalized(float inTolerance = 1.0e-6f) const; @@ -200,13 +206,31 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4 /// Replicate the W component to all components JPH_INLINE Vec4 SplatW() const; + /// Replicate the X component to all components + JPH_INLINE Vec3 SplatX3() const; + + /// Replicate the Y component to all components + JPH_INLINE Vec3 SplatY3() const; + + /// Replicate the Z component to all components + JPH_INLINE Vec3 SplatZ3() const; + + /// Replicate the W component to all components + JPH_INLINE Vec3 SplatW3() const; + + /// Get index of component with lowest value + JPH_INLINE int GetLowestComponentIndex() const; + + /// Get index of component with highest value + JPH_INLINE int GetHighestComponentIndex() const; + /// Return the absolute value of each of the components JPH_INLINE Vec4 Abs() const; /// Reciprocal vector (1 / value) for each of the components JPH_INLINE Vec4 Reciprocal() const; - /// Dot product, returns the dot product in X, Y and Z components + /// Dot product, returns the dot product in X, Y, Z and W components JPH_INLINE Vec4 DotV(Vec4Arg inV2) const; /// Dot product @@ -245,6 +269,10 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4 /// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative) JPH_INLINE Vec4 GetSign() const; + /// Flips the signs of the components, e.g. FlipSign<-1, 1, -1, 1>() will flip the signs of the X and Z components + template + JPH_INLINE Vec4 FlipSign() const; + /// Calculate the sine and cosine for each element of this vector (input in radians) inline void SinCos(Vec4 &outSin, Vec4 &outCos) const; @@ -265,6 +293,12 @@ class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4 /// Calculate the arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI]) inline static Vec4 sATan2(Vec4Arg inY, Vec4Arg inX); + /// Compress a unit vector to a 32 bit value, precision is around 0.5 * 10^-3 + JPH_INLINE uint32 CompressUnitVector() const; + + /// Decompress a unit vector from a 32 bit value + JPH_INLINE static Vec4 sDecompressUnitVector(uint32 inValue); + /// To String friend ostream & operator << (ostream &inStream, Vec4Arg inV) { diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.inl b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl index bcbd2fb38630..392c266864d0 100644 --- a/thirdparty/jolt_physics/Jolt/Math/Vec4.inl +++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl @@ -168,6 +168,11 @@ Vec4 Vec4::sMax(Vec4Arg inV1, Vec4Arg inV2) #endif } +Vec4 Vec4::sClamp(Vec4Arg inV, Vec4Arg inMin, Vec4Arg inMax) +{ + return sMax(sMin(inV, inMax), inMin); +} + UVec4 Vec4::sEquals(Vec4Arg inV1, Vec4Arg inV2) { #if defined(JPH_USE_SSE) @@ -364,6 +369,11 @@ bool Vec4::IsClose(Vec4Arg inV2, float inMaxDistSq) const return (inV2 - *this).LengthSq() <= inMaxDistSq; } +bool Vec4::IsNearZero(float inMaxDistSq) const +{ + return LengthSq() <= inMaxDistSq; +} + bool Vec4::IsNormalized(float inTolerance) const { return abs(LengthSq() - 1.0f) <= inTolerance; @@ -604,6 +614,70 @@ Vec4 Vec4::SplatW() const #endif } +Vec3 Vec4::SplatX3() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 0, 0)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 0); +#else + return Vec3(mF32[0], mF32[0], mF32[0]); +#endif +} + +Vec3 Vec4::SplatY3() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(1, 1, 1, 1)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 1); +#else + return Vec3(mF32[1], mF32[1], mF32[1]); +#endif +} + +Vec3 Vec4::SplatZ3() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(2, 2, 2, 2)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 2); +#else + return Vec3(mF32[2], mF32[2], mF32[2]); +#endif +} + +Vec3 Vec4::SplatW3() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(3, 3, 3, 3)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 3); +#else + return Vec3(mF32[3], mF32[3], mF32[3]); +#endif +} + +int Vec4::GetLowestComponentIndex() const +{ + // Get the minimum value in all 4 components + Vec4 value = Vec4::sMin(*this, Swizzle()); + value = Vec4::sMin(value, value.Swizzle()); + + // Compare with the original vector to find which component is equal to the minimum value + return CountTrailingZeros(Vec4::sEquals(*this, value).GetTrues()); +} + +int Vec4::GetHighestComponentIndex() const +{ + // Get the maximum value in all 4 components + Vec4 value = Vec4::sMax(*this, Swizzle()); + value = Vec4::sMax(value, value.Swizzle()); + + // Compare with the original vector to find which component is equal to the maximum value + return CountTrailingZeros(Vec4::sEquals(*this, value).GetTrues()); +} + Vec4 Vec4::Abs() const { #if defined(JPH_USE_AVX512) @@ -707,6 +781,16 @@ Vec4 Vec4::GetSign() const #endif } +template +JPH_INLINE Vec4 Vec4::FlipSign() const +{ + static_assert(X == 1 || X == -1, "X must be 1 or -1"); + static_assert(Y == 1 || Y == -1, "Y must be 1 or -1"); + static_assert(Z == 1 || Z == -1, "Z must be 1 or -1"); + static_assert(W == 1 || W == -1, "W must be 1 or -1"); + return Vec4::sXor(*this, Vec4(X > 0? 0.0f : -0.0f, Y > 0? 0.0f : -0.0f, Z > 0? 0.0f : -0.0f, W > 0? 0.0f : -0.0f)); +} + Vec4 Vec4::Normalized() const { #if defined(JPH_USE_SSE4_1) @@ -983,4 +1067,82 @@ Vec4 Vec4::sATan2(Vec4Arg inY, Vec4Arg inX) return atan; } +uint32 Vec4::CompressUnitVector() const +{ + constexpr float cOneOverSqrt2 = 0.70710678f; + constexpr uint cNumBits = 9; + constexpr uint cMask = (1 << cNumBits) - 1; + + // Store sign bit + Vec4 v = *this; + uint32 max_element = v.Abs().GetHighestComponentIndex(); + uint32 value = 0; + if (v[max_element] < 0.0f) + { + value = 0x80000000u; + v = -v; + } + + // Store highest component + value |= max_element << 29; + + // Store the other three components in a compressed format + UVec4 compressed = Vec4::sClamp((v + Vec4::sReplicate(cOneOverSqrt2)) * (float(cMask) / (2.0f * cOneOverSqrt2)) + Vec4::sReplicate(0.5f), Vec4::sZero(), Vec4::sReplicate(cMask)).ToInt(); + switch (max_element) + { + case 0: + compressed = compressed.Swizzle(); + break; + + case 1: + compressed = compressed.Swizzle(); + break; + + case 2: + compressed = compressed.Swizzle(); + break; + } + + value |= compressed.GetX(); + value |= compressed.GetY() << cNumBits; + value |= compressed.GetZ() << 2 * cNumBits; + return value; +} + +Vec4 Vec4::sDecompressUnitVector(uint32 inValue) +{ + constexpr float cOneOverSqrt2 = 0.70710678f; + constexpr uint cNumBits = 9; + constexpr uint cMask = (1u << cNumBits) - 1; + + // Restore three components + Vec4 v = Vec4(UVec4(inValue & cMask, (inValue >> cNumBits) & cMask, (inValue >> (2 * cNumBits)) & cMask, 0).ToFloat()) * (2.0f * cOneOverSqrt2 / float(cMask)) - Vec4(cOneOverSqrt2, cOneOverSqrt2, cOneOverSqrt2, 0.0f); + JPH_ASSERT(v.GetW() == 0.0f); + + // Restore the highest component + v.SetW(sqrt(max(1.0f - v.LengthSq(), 0.0f))); + + // Extract sign + if ((inValue & 0x80000000u) != 0) + v = -v; + + // Swizzle the components in place + switch ((inValue >> 29) & 3) + { + case 0: + v = v.Swizzle(); + break; + + case 1: + v = v.Swizzle(); + break; + + case 2: + v = v.Swizzle(); + break; + } + + return v; +} + JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h index adbfbb773c66..88ff21e00412 100644 --- a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h @@ -58,10 +58,12 @@ class JPH_EXPORT IObjectStreamIn : public ObjectStream virtual bool ReadPrimitiveData(bool &outPrimitive) = 0; virtual bool ReadPrimitiveData(String &outPrimitive) = 0; virtual bool ReadPrimitiveData(Float3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Float4 &outPrimitive) = 0; virtual bool ReadPrimitiveData(Double3 &outPrimitive) = 0; virtual bool ReadPrimitiveData(Vec3 &outPrimitive) = 0; virtual bool ReadPrimitiveData(DVec3 &outPrimitive) = 0; virtual bool ReadPrimitiveData(Vec4 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(UVec4 &outPrimitive) = 0; virtual bool ReadPrimitiveData(Quat &outPrimitive) = 0; virtual bool ReadPrimitiveData(Mat44 &outPrimitive) = 0; virtual bool ReadPrimitiveData(DMat44 &outPrimitive) = 0; @@ -92,10 +94,12 @@ class JPH_EXPORT IObjectStreamOut : public ObjectStream virtual void WritePrimitiveData(const bool &inPrimitive) = 0; virtual void WritePrimitiveData(const String &inPrimitive) = 0; virtual void WritePrimitiveData(const Float3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Float4 &inPrimitive) = 0; virtual void WritePrimitiveData(const Double3 &inPrimitive) = 0; virtual void WritePrimitiveData(const Vec3 &inPrimitive) = 0; virtual void WritePrimitiveData(const DVec3 &inPrimitive) = 0; virtual void WritePrimitiveData(const Vec4 &inPrimitive) = 0; + virtual void WritePrimitiveData(const UVec4 &inPrimitive) = 0; virtual void WritePrimitiveData(const Quat &inPrimitive) = 0; virtual void WritePrimitiveData(const Mat44 &inPrimitive) = 0; virtual void WritePrimitiveData(const DMat44 &inPrimitive) = 0; diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h index 86b8830c259a..280d8d14c06d 100644 --- a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h @@ -152,13 +152,19 @@ public: \ /// Classes must be derived from SerializableObject if you want to be able to save pointers or /// reference counting pointers to objects of this or derived classes. The type will automatically /// be determined during serialization and upon deserialization it will be restored correctly. -class JPH_EXPORT SerializableObject : public NonCopyable +class JPH_EXPORT SerializableObject { JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(JPH_EXPORT, SerializableObject) public: - /// Constructor + /// Destructor virtual ~SerializableObject() = default; + +protected: + /// Don't allow (copy) constructing this base class, but allow derived classes to (copy) construct themselves + SerializableObject() = default; + SerializableObject(const SerializableObject &) = default; + SerializableObject & operator = (const SerializableObject &) = default; }; JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h index 015e1dbf1445..26238aea10d2 100644 --- a/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h +++ b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h @@ -22,16 +22,19 @@ JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, double); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, bool); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, String); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Float3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Float4); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Double3); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Vec3); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, DVec3); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Vec4); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, UVec4); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Quat); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Mat44); JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, DMat44); JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Color); JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, AABox); JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Triangle); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, IndexedTriangleNoMaterial); JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, IndexedTriangle); JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Plane); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp index 5fdb9220255f..034f34528638 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp @@ -242,7 +242,7 @@ bool Body::ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Ve float relative_center_of_buoyancy_velocity_len_sq = relative_center_of_buoyancy_velocity.LengthSq(); if (relative_center_of_buoyancy_velocity_len_sq > 1.0e-12f) { - Vec3 local_relative_center_of_buoyancy_velocity = GetRotation().Conjugated() * relative_center_of_buoyancy_velocity; + Vec3 local_relative_center_of_buoyancy_velocity = GetRotation().InverseRotate(relative_center_of_buoyancy_velocity); area = local_relative_center_of_buoyancy_velocity.Abs().Dot(size.Swizzle() * size.Swizzle()) / sqrt(relative_center_of_buoyancy_velocity_len_sq); } @@ -415,6 +415,9 @@ SoftBodyCreationSettings Body::GetSoftBodyCreationSettings() const result.mGravityFactor = mp->GetGravityFactor(); result.mPressure = mp->GetPressure(); result.mUpdatePosition = mp->GetUpdatePosition(); + result.mVertexRadius = mp->GetVertexRadius(); + result.mAllowSleeping = mp->GetAllowSleeping(); + result.mFacesDoubleSided = mp->GetFacesDoubleSided(); result.mSettings = mp->GetSettings(); return result; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h index 849187101709..44d71eb99998 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h @@ -33,7 +33,7 @@ class SoftBodyCreationSettings; /// The linear velocity is also velocity of the center of mass, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$. class #ifndef JPH_PLATFORM_DOXYGEN // Doxygen gets confused here - JPH_EXPORT_GCC_BUG_WORKAROUND alignas(JPH_RVECTOR_ALIGNMENT) + JPH_EXPORT_GCC_BUG_WORKAROUND alignas(max(JPH_VECTOR_ALIGNMENT, JPH_RVECTOR_ALIGNMENT)) #endif Body : public NonCopyable { @@ -444,8 +444,8 @@ class // 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer) }; -static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect"); -static_assert(alignof(Body) == JPH_RVECTOR_ALIGNMENT, "Body should properly align"); +static_assert(JPH_CPU_ADDRESS_BITS != 64 || JPH_RVECTOR_ALIGNMENT < 16 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect"); +static_assert(alignof(Body) == max(JPH_VECTOR_ALIGNMENT, JPH_RVECTOR_ALIGNMENT), "Body should properly align"); JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp index 988094c8e21e..4a079937985b 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp @@ -308,12 +308,12 @@ void BodyInterface::SetShape(const BodyID &inBodyID, const Shape *inShape, bool // Update the shape body.SetShapeInternal(inShape, inUpdateMassProperties); - // Flag collision cache invalid for this body - mBodyManager->InvalidateContactCacheForBody(body); - // Notify broadphase of change if (body.IsInBroadPhase()) { + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + BodyID id = body.GetID(); mBroadPhase->NotifyBodiesAABBChanged(&id, 1); @@ -338,12 +338,12 @@ void BodyInterface::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviou // Recalculate bounding box body.CalculateWorldSpaceBoundsInternal(); - // Flag collision cache invalid for this body - mBodyManager->InvalidateContactCacheForBody(body); - // Notify broadphase of change if (body.IsInBroadPhase()) { + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + BodyID id = body.GetID(); mBroadPhase->NotifyBodiesAABBChanged(&id, 1); @@ -553,7 +553,7 @@ void BodyInterface::MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosit body.MoveKinematic(inTargetPosition, inTargetRotation, inDeltaTime); - if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero())) + if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero()) && body.IsInBroadPhase()) mBodyManager->ActivateBodies(&inBodyID, 1); } } @@ -569,7 +569,7 @@ void BodyInterface::SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg body.SetLinearVelocityClamped(inLinearVelocity); body.SetAngularVelocityClamped(inAngularVelocity); - if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero())) + if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero()) && body.IsInBroadPhase()) mBodyManager->ActivateBodies(&inBodyID, 1); } } @@ -602,7 +602,7 @@ void BodyInterface::SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVe { body.SetLinearVelocityClamped(inLinearVelocity); - if (!body.IsActive() && !inLinearVelocity.IsNearZero()) + if (!body.IsActive() && !inLinearVelocity.IsNearZero() && body.IsInBroadPhase()) mBodyManager->ActivateBodies(&inBodyID, 1); } } @@ -631,7 +631,7 @@ void BodyInterface::AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVe { body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity); - if (!body.IsActive() && !body.GetLinearVelocity().IsNearZero()) + if (!body.IsActive() && !body.GetLinearVelocity().IsNearZero() && body.IsInBroadPhase()) mBodyManager->ActivateBodies(&inBodyID, 1); } } @@ -648,7 +648,7 @@ void BodyInterface::AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity); body.SetAngularVelocityClamped(body.GetAngularVelocity() + inAngularVelocity); - if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero())) + if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero()) && body.IsInBroadPhase()) mBodyManager->ActivateBodies(&inBodyID, 1); } } @@ -664,7 +664,7 @@ void BodyInterface::SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngular { body.SetAngularVelocityClamped(inAngularVelocity); - if (!body.IsActive() && !inAngularVelocity.IsNearZero()) + if (!body.IsActive() && !inAngularVelocity.IsNearZero() && body.IsInBroadPhase()) mBodyManager->ActivateBodies(&inBodyID, 1); } } @@ -849,7 +849,7 @@ void BodyInterface::SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3 body.SetAngularVelocityClamped(inAngularVelocity); // Optionally activate body - if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero())) + if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero()) && body.IsInBroadPhase()) mBodyManager->ActivateBodies(&inBodyID, 1); } } @@ -869,7 +869,7 @@ void BodyInterface::SetMotionType(const BodyID &inBodyID, EMotionType inMotionTy body.SetMotionType(inMotionType); // Activate body if requested - if (inMotionType != EMotionType::Static && inActivationMode == EActivation::Activate) + if (inMotionType != EMotionType::Static && inActivationMode == EActivation::Activate && body.IsInBroadPhase()) ActivateBodyInternal(body); } } @@ -965,6 +965,38 @@ float BodyInterface::GetGravityFactor(const BodyID &inBodyID) const return 1.0f; } +void BodyInterface::SetMaxLinearVelocity(const BodyID &inBodyID, float inLinearVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + lock.GetBody().GetMotionPropertiesUnchecked()->SetMaxLinearVelocity(inLinearVelocity); +} + +float BodyInterface::GetMaxLinearVelocity(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + return lock.GetBody().GetMotionPropertiesUnchecked()->GetMaxLinearVelocity(); + else + return 500.0f; +} + +void BodyInterface::SetMaxAngularVelocity(const BodyID &inBodyID, float inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + lock.GetBody().GetMotionPropertiesUnchecked()->SetMaxAngularVelocity(inAngularVelocity); +} + +float BodyInterface::GetMaxAngularVelocity(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + return lock.GetBody().GetMotionPropertiesUnchecked()->GetMaxAngularVelocity(); + else + return 0.25f * JPH_PI * 60.0f; +} + void BodyInterface::SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction) { BodyLockWrite lock(*mBodyLockInterface, inBodyID); @@ -976,7 +1008,8 @@ void BodyInterface::SetUseManifoldReduction(const BodyID &inBodyID, bool inUseRe body.SetUseManifoldReduction(inUseReduction); // Flag collision cache invalid for this body - mBodyManager->InvalidateContactCacheForBody(body); + if (body.IsInBroadPhase()) + mBodyManager->InvalidateContactCacheForBody(body); } } } @@ -990,6 +1023,22 @@ bool BodyInterface::GetUseManifoldReduction(const BodyID &inBodyID) const return true; } +void BodyInterface::SetIsSensor(const BodyID &inBodyID, bool inIsSensor) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetIsSensor(inIsSensor); +} + +bool BodyInterface::IsSensor(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().IsSensor(); + else + return false; +} + void BodyInterface::SetCollisionGroup(const BodyID &inBodyID, const CollisionGroup &inCollisionGroup) { BodyLockWrite lock(*mBodyLockInterface, inBodyID); @@ -1043,7 +1092,7 @@ const PhysicsMaterial *BodyInterface::GetMaterial(const BodyID &inBodyID, const void BodyInterface::InvalidateContactCache(const BodyID &inBodyID) { BodyLockWrite lock(*mBodyLockInterface, inBodyID); - if (lock.Succeeded()) + if (lock.SucceededAndIsInBroadPhase()) mBodyManager->InvalidateContactCacheForBody(lock.GetBody()); } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h index cc92e93772f1..cae78b245313 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h @@ -98,7 +98,7 @@ class JPH_EXPORT BodyInterface : public NonCopyable /// After adding, to get a body by ID use the BodyLockRead or BodyLockWrite interface! void AddBody(const BodyID &inBodyID, EActivation inActivationMode); - /// Remove body from the physics system. + /// Remove body from the physics system. Note that you need to add a body to the physics system before you can remove it. void RemoveBody(const BodyID &inBodyID); /// Check if a body has been added to the physics system. @@ -132,12 +132,12 @@ class JPH_EXPORT BodyInterface : public NonCopyable /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. void AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState); - /// Remove inNumber bodies in ioBodies from the PhysicsSystem. + /// Remove inNumber bodies in ioBodies from the PhysicsSystem. Note that bodies need to be added to the physics system before they can be removed. /// ioBodies may be shuffled around by this function. void RemoveBodies(BodyID *ioBodies, int inNumber); ///@} - ///@name Activate / deactivate a body + ///@name Activate / deactivate a body. Note that you need to add a body to the physics system before you can activate it. ///@{ void ActivateBody(const BodyID &inBodyID); void ActivateBodies(const BodyID *inBodyIDs, int inNumber); @@ -151,7 +151,8 @@ class JPH_EXPORT BodyInterface : public NonCopyable /// Create a two body constraint TwoBodyConstraint * CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2); - /// Activate non-static bodies attached to a constraint + /// Activate non-static bodies attached to a constraint. + /// Note that the bodies involved in the constraint should be added to the physics system before activating a constraint. void ActivateConstraint(const TwoBodyConstraint *inConstraint); ///@name Access to the shape of a body @@ -214,7 +215,7 @@ class JPH_EXPORT BodyInterface : public NonCopyable /// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$ void SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); - ///@name Add forces to the body + ///@name Add forces to the body. Note that you should add a body to the physics system before applying forces or torques. ///@{ void AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddForce void AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode = EActivation::Activate); ///< Applied at inPoint @@ -222,7 +223,7 @@ class JPH_EXPORT BodyInterface : public NonCopyable void AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< A combination of Body::AddForce and Body::AddTorque ///@} - ///@name Add an impulse to the body + ///@name Add an impulse to the body. Note that you should add a body to the physics system before applying impulses. ///@{ void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at inPoint @@ -268,12 +269,30 @@ class JPH_EXPORT BodyInterface : public NonCopyable float GetGravityFactor(const BodyID &inBodyID) const; ///@} + ///@name Max linear velocity + ///@{ + void SetMaxLinearVelocity(const BodyID &inBodyID, float inLinearVelocity); + float GetMaxLinearVelocity(const BodyID &inBodyID) const; + ///@} + + ///@name Max angular velocity + ///@{ + void SetMaxAngularVelocity(const BodyID &inBodyID, float inAngularVelocity); + float GetMaxAngularVelocity(const BodyID &inBodyID) const; + ///@} + ///@name Manifold reduction ///@{ void SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction); bool GetUseManifoldReduction(const BodyID &inBodyID) const; ///@} + ///@name Sensor + ///@{ + void SetIsSensor(const BodyID &inBodyID, bool inIsSensor); + bool IsSensor(const BodyID &inBodyID) const; + ///@} + ///@name Collision group ///@{ void SetCollisionGroup(const BodyID &inBodyID, const CollisionGroup &inCollisionGroup); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h index 5872729c029c..4c7a186967b5 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h @@ -33,8 +33,8 @@ class BodyLockMultiBase : public NonCopyable } } - /// Destructor will unlock the bodies - ~BodyLockMultiBase() + /// Explicitly release the locks on all bodies (normally this is done in the destructor) + inline void ReleaseLocks() { if (mMutexMask != 0) { @@ -42,9 +42,25 @@ class BodyLockMultiBase : public NonCopyable mBodyLockInterface.UnlockWrite(mMutexMask); else mBodyLockInterface.UnlockRead(mMutexMask); + + mMutexMask = 0; + mBodyIDs = nullptr; + mNumBodyIDs = 0; } } + /// Destructor will unlock the bodies + ~BodyLockMultiBase() + { + ReleaseLocks(); + } + + /// Returns the number of bodies that were locked + inline int GetNumBodies() const + { + return mNumBodyIDs; + } + /// Access the body (returns null if body was not properly locked) inline BodyType * GetBody(int inBodyIndex) const { diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp index e64a446fbc65..ebf524ba0542 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp @@ -1092,6 +1092,15 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings if (inDrawSettings.mDrawSoftBodyEdgeConstraints) mp->DrawEdgeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + if (inDrawSettings.mDrawSoftBodyRods) + mp->DrawRods(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyRodStates) + mp->DrawRodStates(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyRodBendTwistConstraints) + mp->DrawRodBendTwistConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + if (inDrawSettings.mDrawSoftBodyBendConstraints) mp->DrawBendConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h index 518574707a00..3a91b621671b 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h @@ -248,6 +248,9 @@ class JPH_EXPORT BodyManager : public NonCopyable bool mDrawSoftBodyVolumeConstraints = false; ///< Draw the volume constraints of soft bodies bool mDrawSoftBodySkinConstraints = false; ///< Draw the skin constraints of soft bodies bool mDrawSoftBodyLRAConstraints = false; ///< Draw the LRA constraints of soft bodies + bool mDrawSoftBodyRods = false; ///< Draw the rods of soft bodies + bool mDrawSoftBodyRodStates = false; ///< Draw the rod states (orientation and angular velocity) of soft bodies + bool mDrawSoftBodyRodBendTwistConstraints = false; ///< Draw the rod bend twist constraints of soft bodies bool mDrawSoftBodyPredictedBounds = false; ///< Draw the predicted bounds of soft bodies ESoftBodyConstraintColor mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints }; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl index 200ab680a17b..474ab15c18c3 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl +++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl @@ -113,7 +113,7 @@ void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, floa // Calculate local space angular momentum Quat inertia_space_to_world_space = inBodyRotation * mInertiaRotation; - Vec3 local_angular_velocity = inertia_space_to_world_space.Conjugated() * mAngularVelocity; + Vec3 local_angular_velocity = inertia_space_to_world_space.InverseRotate(mAngularVelocity); Vec3 local_momentum = local_inertia * local_angular_velocity; // The gyroscopic force applies a torque: T = -w x I w where w is angular velocity and I the inertia tensor diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp index 4cb3e95e25b0..84c4d38a2138 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp @@ -330,4 +330,25 @@ TransformedShape Character::GetTransformedShape(bool inLockBodies) const return sCharacterGetBodyInterface(mSystem, inLockBodies).GetTransformedShape(mBodyID); } +CharacterSettings Character::GetCharacterSettings(bool inLockBodies) const +{ + BodyLockRead lock(sCharacterGetBodyLockInterface(mSystem, inLockBodies), mBodyID); + JPH_ASSERT(lock.Succeeded()); + const Body &body = lock.GetBody(); + + CharacterSettings settings; + settings.mUp = mUp; + settings.mSupportingVolume = mSupportingVolume; + settings.mMaxSlopeAngle = ACos(mCosMaxSlopeAngle); + settings.mEnhancedInternalEdgeRemoval = body.GetEnhancedInternalEdgeRemoval(); + settings.mShape = mShape; + settings.mLayer = mLayer; + const MotionProperties *mp = body.GetMotionProperties(); + settings.mMass = 1.0f / mp->GetInverseMass(); + settings.mFriction = body.GetFriction(); + settings.mGravityFactor = mp->GetGravityFactor(); + settings.mAllowedDOFs = mp->GetAllowedDOFs(); + return settings; +} + JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h index 0f37f33b79e0..5d276c5e59ed 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h @@ -18,6 +18,11 @@ class JPH_EXPORT CharacterSettings : public CharacterBaseSettings public: JPH_OVERRIDE_NEW_DELETE + /// Constructor + CharacterSettings() = default; + CharacterSettings(const CharacterSettings &) = default; + CharacterSettings & operator = (const CharacterSettings &) = default; + /// Layer that this character will be added to ObjectLayer mLayer = 0; @@ -134,6 +139,9 @@ class JPH_EXPORT Character : public CharacterBase /// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false) void CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies = true) const; + /// Get the character settings that can recreate this character + CharacterSettings GetCharacterSettings(bool inLockBodies = true) const; + private: /// Check collisions between inShape and the world using the center of mass transform void CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h index fc19c3d9949b..ce938eb6d4d4 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h @@ -24,8 +24,8 @@ class JPH_EXPORT CharacterBaseSettings : public RefTarget /// Constructor CharacterBaseSettings() = default; - CharacterBaseSettings(const CharacterBaseSettings &inSettings) = default; - CharacterBaseSettings & operator = (const CharacterBaseSettings &inSettings) = default; + CharacterBaseSettings(const CharacterBaseSettings &) = default; + CharacterBaseSettings & operator = (const CharacterBaseSettings &) = default; /// Virtual destructor virtual ~CharacterBaseSettings() = default; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp index 58614d402b73..b7029d2d9052 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -542,6 +543,14 @@ inline static bool sCorrectFractionForCharacterPadding(const Shape *inShape, Mat const ScaledShape *scaled_shape = static_cast(inShape); return sCorrectFractionForCharacterPadding(scaled_shape->GetInnerShape(), inStart, inDisplacement, inScale * scaled_shape->GetScale(), inPolygon, ioFraction); } + else if (inShape->GetType() == EShapeType::Compound) + { + const CompoundShape *compound = static_cast(inShape); + bool return_value = false; + for (const CompoundShape::SubShape &sub_shape : compound->GetSubShapes()) + return_value |= sCorrectFractionForCharacterPadding(sub_shape.mShape, inStart * sub_shape.GetLocalTransformNoScale(inScale), inDisplacement, sub_shape.TransformScale(inScale), inPolygon, ioFraction); + return return_value; + } else { JPH_ASSERT(false, "Not supported yet!"); @@ -1888,4 +1897,37 @@ void CharacterVirtual::RestoreState(StateRecorder &inStream) c.RestoreState(inStream); } +CharacterVirtualSettings CharacterVirtual::GetCharacterVirtualSettings() const +{ + CharacterVirtualSettings settings; + settings.mUp = mUp; + settings.mSupportingVolume = mSupportingVolume; + settings.mMaxSlopeAngle = ACos(mCosMaxSlopeAngle); + settings.mEnhancedInternalEdgeRemoval = mEnhancedInternalEdgeRemoval; + settings.mShape = mShape; + settings.mID = mID; + settings.mMass = mMass; + settings.mMaxStrength = mMaxStrength; + settings.mShapeOffset = mShapeOffset; + settings.mBackFaceMode = mBackFaceMode; + settings.mPredictiveContactDistance = mPredictiveContactDistance; + settings.mMaxCollisionIterations = mMaxCollisionIterations; + settings.mMaxConstraintIterations = mMaxConstraintIterations; + settings.mMinTimeRemaining = mMinTimeRemaining; + settings.mCollisionTolerance = mCollisionTolerance; + settings.mCharacterPadding = mCharacterPadding; + settings.mMaxNumHits = mMaxNumHits; + settings.mHitReductionCosMaxAngle = mHitReductionCosMaxAngle; + settings.mPenetrationRecoverySpeed = mPenetrationRecoverySpeed; + BodyLockRead lock(mSystem->GetBodyLockInterface(), mInnerBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + settings.mInnerBodyShape = body.GetShape(); + settings.mInnerBodyIDOverride = body.GetID(); + settings.mInnerBodyLayer = body.GetObjectLayer(); + } + return settings; +} + JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h index e7a5b749005e..4f6d63a1c296 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h @@ -24,6 +24,11 @@ class JPH_EXPORT CharacterVirtualSettings : public CharacterBaseSettings public: JPH_OVERRIDE_NEW_DELETE + /// Constructor + CharacterVirtualSettings() = default; + CharacterVirtualSettings(const CharacterVirtualSettings &) = default; + CharacterVirtualSettings & operator = (const CharacterVirtualSettings &) = default; + /// ID to give to this character. This is used for deterministically sorting and as an identifier to represent the character in the contact removal callback. CharacterID mID = CharacterID::sNextCharacterID(); @@ -419,6 +424,9 @@ class JPH_EXPORT CharacterVirtual : public CharacterBase /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. void CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + /// Get the character settings that can recreate this character + CharacterVirtualSettings GetCharacterVirtualSettings() const; + // Saving / restoring state for replay virtual void SaveState(StateRecorder &inStream) const override; virtual void RestoreState(StateRecorder &inStream) override; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp index 0446027b9f19..a70f362022f1 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp @@ -225,10 +225,6 @@ uint32 QuadTree::AllocateNode(bool inIsChanged) // you could still be running out of nodes because the tree is not being maintained. If your application is paused, // consider still calling PhysicsSystem::Update with a delta time of 0 to keep the tree in good shape. // - // The system keeps track of a previous and a current tree, this allows for queries to continue using the old tree - // while the new tree is being built. If you completely clean the PhysicsSystem and rebuild it from scratch, you may - // want to call PhysicsSystem::OptimizeBroadPhase two times after clearing to completely get rid of any lingering nodes. - // // The number of nodes that is allocated is related to the max number of bodies that is passed in PhysicsSystem::Init. // For normal situations there are plenty of nodes available. If all else fails, you can increase the number of nodes // by increasing the maximum number of bodies. diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp index a69208a04dd7..e8a8c2145912 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp @@ -29,8 +29,6 @@ CastConvexVsTriangles::CastConvexVsTriangles(const ShapeCast &inShapeCast, const void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) { - JPH_PROFILE_FUNCTION(); - // Scale triangle Vec3 v0 = mScale * inV0; Vec3 v1 = mScale * inV1; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp index 69abfbaf3119..8e1010641944 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp @@ -117,8 +117,6 @@ float CastSphereVsTriangles::RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylin void CastSphereVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) { - JPH_PROFILE_FUNCTION(); - // Scale triangle and make it relative to the start of the cast Vec3 v0 = mScale * inV0 - mStart; Vec3 v1 = mScale * inV1 - mStart; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp index c57e750be27b..e03659454de5 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp @@ -40,8 +40,6 @@ CollideConvexVsTriangles::CollideConvexVsTriangles(const ConvexShape *inShape1, void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) { - JPH_PROFILE_FUNCTION(); - // Scale triangle and transform it to the space of 1 Vec3 v0 = mTransform2To1 * (mScale2 * inV0); Vec3 v1 = mTransform2To1 * (mScale2 * inV1); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp index 92ed28a7b495..18441ea73b35 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp @@ -47,8 +47,6 @@ CollideSphereVsTriangles::CollideSphereVsTriangles(const SphereShape *inShape1, void CollideSphereVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) { - JPH_PROFILE_FUNCTION(); - // Scale triangle and make it relative to the center of the sphere Vec3 v0 = mScale2 * inV0 - mSphereCenterIn2; Vec3 v1 = mScale2 * inV1 - mSphereCenterIn2; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h index 1a24b4fb88a5..07dacf3a5f7c 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h @@ -61,10 +61,20 @@ enum class ValidateResult RejectAllContactsForThisBodyPair ///< Rejects this and any further contact points for this body pair }; -/// A listener class that receives collision contact events. -/// It can be registered with the ContactConstraintManager (or PhysicsSystem). -/// Note that contact listener callbacks are called from multiple threads at the same time when all bodies are locked, you're only allowed to read from the bodies and you can't change physics state. +/// A listener class that receives collision contact events. It can be registered through PhysicsSystem::SetContactListener. +/// Only a single contact listener can be registered. A common pattern is to create a contact listener that casts Body::GetUserData +/// to a game object and then forwards the call to a handler specific for that game object. +/// Typically this is done on both objects involved in a collision event. +/// +/// Note that contact listener callbacks are called from multiple threads at the same time when all bodies are locked, this means you cannot +/// use PhysicsSystem::GetBodyInterface / PhysicsSystem::GetBodyLockInterface but must use PhysicsSystem::GetBodyInterfaceNoLock / PhysicsSystem::GetBodyLockInterfaceNoLock instead. +/// If you use a locking interface, the simulation will deadlock. You're only allowed to read from the bodies and you can't change physics state. /// During OnContactRemoved you cannot access the bodies at all, see the comments at that function. +/// +/// While a callback can come from multiple threads, all callbacks relating to a single body pair are serialized. +/// For EMotionQuality::Discrete bodies, during every 'collision step' in a PhysicsSystem::Update, you will receive at most one OnContactAdded/Persisted/Removed call per body/sub shape pair. +/// For EMotionQuality::LinearCast bodies, you may get an OnContactAdded followed by an OnContactPersisted for the same body/sub shape pair. +/// This happens when a body collides both in the discrete and the continuous collision detection stage. class ContactListener { public: @@ -72,29 +82,39 @@ class ContactListener virtual ~ContactListener() = default; /// Called after detecting a collision between a body pair, but before calling OnContactAdded and before adding the contact constraint. - /// If the function rejects the contact, the contact will not be added and any other contacts between this body pair will not be processed. - /// This function will only be called once per PhysicsSystem::Update per body pair and may not be called again the next update - /// if a contact persists and no new contact pairs between sub shapes are found. + /// If the function rejects the contact, the contact will not be processed by the simulation. /// This is a rather expensive time to reject a contact point since a lot of the collision detection has happened already, make sure you /// filter out the majority of undesired body pairs through the ObjectLayerPairFilter that is registered on the PhysicsSystem. - /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// + /// This function may not be called again the next update if a contact persists and no new contact pairs between sub shapes are found. + /// + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! See detailed class description of ContactListener. + /// /// Body 1 will have a motion type that is larger or equal than body 2's motion type (order from large to small: dynamic -> kinematic -> static). When motion types are equal, they are ordered by BodyID. + /// /// The collision result (inCollisionResult) is reported relative to inBaseOffset. virtual ValidateResult OnContactValidate([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] RVec3Arg inBaseOffset, [[maybe_unused]] const CollideShapeResult &inCollisionResult) { return ValidateResult::AcceptAllContactsForThisBodyPair; } /// Called whenever a new contact point is detected. - /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! See detailed class description of ContactListener. + /// /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. + /// /// Note that only active bodies will report contacts, as soon as a body goes to sleep the contacts between that body and all other /// bodies will receive an OnContactRemoved callback, if this is the case then Body::IsActive() will return false during the callback. + /// /// When contacts are added, the constraint solver has not run yet, so the collision impulse is unknown at that point. /// The velocities of inBody1 and inBody2 are the velocities before the contact has been resolved, so you can use this to /// estimate the collision impulse to e.g. determine the volume of the impact sound to play (see: EstimateCollisionResponse). virtual void OnContactAdded([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ } /// Called whenever a contact is detected that was also detected last update. - /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! See detailed class description of ContactListener. + /// /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. + /// /// If the structure of the shape of a body changes between simulation steps (e.g. by adding/removing a child shape of a compound shape), /// it is possible that the same sub shape ID used to identify the removed child shape is now reused for a different child shape. The physics /// system cannot detect this, so may send a 'contact persisted' callback even though the contact is now on a different child shape. You can @@ -103,14 +123,18 @@ class ContactListener virtual void OnContactPersisted([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ } /// Called whenever a contact was detected last update but is not detected anymore. + /// /// You cannot access the bodies at the time of this callback because: /// - All bodies are locked at the time of this callback. /// - Some properties of the bodies are being modified from another thread at the same time. /// - The body may have been removed and destroyed (you'll receive an OnContactRemoved callback in the PhysicsSystem::Update after the body has been removed). + /// /// Cache what you need in the OnContactAdded and OnContactPersisted callbacks and store it in a separate structure to use during this callback. - /// Alternatively, you could just record that the contact was removed and process it after PhysicsSimulation::Update. + /// Alternatively, you could just record that the contact was removed and process it after PhysicsSystem::Update. + /// /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. - /// The sub shape ID were created in the previous simulation step too, so if the structure of a shape changes (e.g. by adding/removing a child shape of a compound shape), + /// + /// The sub shape IDs were created in the previous simulation step, so if the structure of a shape changes (e.g. by adding/removing a child shape of a compound shape), /// the sub shape ID may not be valid / may not point to the same sub shape anymore. /// If you want to know if this is the last contact between the two bodies, use PhysicsSystem::WereBodiesInContact. virtual void OnContactRemoved([[maybe_unused]] const SubShapeIDPair &inSubShapePair) { /* Do nothing */ } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h index 5e01043a8f37..86651a640df8 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h @@ -34,6 +34,11 @@ class JPH_EXPORT GroupFilter : public SerializableObject, public RefTargetDrawWirePolygon(RMat44::sIdentity(), inResult.mShape2Face, Color::sGreen); - DebugRenderer::sInstance->DrawArrow(RVec3(inResult.mContactPointOn2), RVec3(inResult.mContactPointOn2) + inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f); + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sTranslation(mBaseOffset), inResult.mShape2Face, Color::sGreen); + DebugRenderer::sInstance->DrawArrow(mBaseOffset + inResult.mContactPointOn2, mBaseOffset + inResult.mContactPointOn2 + inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f); #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG } public: /// Constructor, configures a collector to be called with all the results that do not hit internal edges - explicit InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector) : + explicit InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + , RVec3Arg inBaseOffset + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + ) : CollideShapeCollector(inChainedCollector), mChainedCollector(inChainedCollector) + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + , mBaseOffset(inBaseOffset) + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG { // Initialize arrays to full capacity to avoid needless reallocation calls mVoidedFeatures.reserve(cMaxLocalVoidedFeatures); @@ -205,11 +212,11 @@ class InternalEdgeRemovingCollector : public CollideShapeCollector #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG Color color = voided? Color::sRed : Color::sYellow; - DebugRenderer::sInstance->DrawText3D(RVec3(r.mContactPointOn2), StringFormat("%d: %g", i, r.mPenetrationDepth), color, 0.1f); - DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), r.mShape2Face, color); - DebugRenderer::sInstance->DrawArrow(RVec3(r.mContactPointOn2), RVec3(r.mContactPointOn2) + r.mPenetrationAxis.NormalizedOr(Vec3::sZero()), color, 0.1f); - DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v1_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f); - DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v2_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f); + DebugRenderer::sInstance->DrawText3D(mBaseOffset + r.mContactPointOn2, StringFormat("%d: %g", i, r.mPenetrationDepth), color, 0.1f); + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sTranslation(mBaseOffset), r.mShape2Face, color); + DebugRenderer::sInstance->DrawArrow(mBaseOffset + r.mContactPointOn2, mBaseOffset + r.mContactPointOn2 + r.mPenetrationAxis.NormalizedOr(Vec3::sZero()), color, 0.1f); + DebugRenderer::sInstance->DrawMarker(mBaseOffset + r.mShape2Face[best_v1_idx], IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f); + DebugRenderer::sInstance->DrawMarker(mBaseOffset + r.mShape2Face[best_v2_idx], IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f); #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG // No voided features, accept the contact @@ -233,12 +240,20 @@ class InternalEdgeRemovingCollector : public CollideShapeCollector } /// Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges - static void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) + static void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { } +#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + , RVec3Arg inBaseOffset = RVec3::sZero() +#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + ) { JPH_ASSERT(inCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideWithAll); // Won't work without colliding with all edges JPH_ASSERT(inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces); // Won't work without collecting faces - InternalEdgeRemovingCollector wrapper(ioCollector); + InternalEdgeRemovingCollector wrapper(ioCollector + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + , inBaseOffset + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + ); CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter); wrapper.Flush(); } @@ -256,6 +271,9 @@ class InternalEdgeRemovingCollector : public CollideShapeCollector CollideShapeCollector & mChainedCollector; Array> mVoidedFeatures; Array> mDelayedResults; +#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + RVec3 mBaseOffset; // Base offset for the query, used to draw the results in the right place +#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG }; JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp index 5ac90ab99644..670cee332e29 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp @@ -301,7 +301,11 @@ void NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval(const Shape *inShape, settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll; settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; - InternalEdgeRemovingCollector wrapper(ioCollector); + InternalEdgeRemovingCollector wrapper(ioCollector + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + , inBaseOffset + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + ); CollideShape(inShape, inShapeScale, inCenterOfMassTransform, settings, inBaseOffset, wrapper, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h index d13c9644b6f8..518d148e05a1 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h @@ -24,7 +24,8 @@ class JPH_EXPORT PhysicsMaterial : public SerializableObject, public RefTarget

GetLocalBounds().Scaled(inScale2).Transformed(transform2_to_1); + mBoundsOf2InSpaceOf1.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); } /// Returns true when collision detection should abort because it's not possible to find a better hit diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp index 5f1dc651af23..c6682777c984 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp @@ -1106,12 +1106,12 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, } } } - bool is_outside = max_distance > 0.0f; - // Project point onto that plane - Vec3 closest_point = local_pos - max_distance * max_plane_normal; + // Project point onto that plane, in local space to the vertex + Vec3 closest_point = -max_distance * max_plane_normal; // Check edges if we're outside the hull (when inside we know the closest face is also the closest point to the surface) + bool is_outside = max_distance > 0.0f; if (is_outside) { // Loop over edges @@ -1137,13 +1137,16 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3 closest = ClosestPoint::GetClosestPointOnLine(p1 - local_pos, p2 - local_pos, set); float distance_sq = closest.LengthSq(); if (distance_sq < closest_point_dist_sq) - closest_point = local_pos + closest; + { + closest_point_dist_sq = distance_sq; + closest_point = closest; + } } } } // Check if this is the largest penetration - Vec3 normal = local_pos - closest_point; + Vec3 normal = -closest_point; float normal_length = normal.Length(); float penetration = normal_length; if (is_outside) @@ -1153,8 +1156,8 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, if (v.UpdatePenetration(penetration)) { // Calculate contact plane - normal = normal_length > 0.0f? normal / normal_length : max_plane_normal; - Plane plane = Plane::sFromPointAndNormal(closest_point, normal); + normal = normal_length > 1.0e-12f? normal / normal_length : max_plane_normal; + Plane plane = Plane::sFromPointAndNormal(local_pos + closest_point, normal); // Store collision v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp index 497f14e19679..557078ada1b6 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp @@ -319,6 +319,7 @@ void MeshShape::sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTri uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT); JPH_ASSERT((triangle.mMaterialIndex & mask) == 0); triangle.mMaterialIndex |= mask; + indices.mNumTriangles = 3; // Indicate that we have 3 or more triangles } } } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp index 3f9a3ebfbd0f..5c4f2002ceda 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp @@ -381,7 +381,7 @@ void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMa } // Preserve flip along y axis but make sure we're not inside out - Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale; + Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale; RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale); AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp index daeba9181ce9..bec0c47423b1 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp @@ -536,7 +536,7 @@ void TaperedCylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, con JPH_ASSERT(IsAligned(&ioContext, alignof(TCSGetTrianglesContext))); // Make sure the scale is not inside out - Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale; + Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale; // Mark top and bottom processed if their radius is too small TCSGetTrianglesContext *context = new (&ioContext) TCSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale)); @@ -645,7 +645,7 @@ int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int i void TaperedCylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const { // Preserve flip along y axis but make sure we're not inside out - Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale; + Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale; RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale); DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp index ee22c7ce25cd..ae82a4cf02d3 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp @@ -414,8 +414,12 @@ void TriangleShape::sRegister() CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle); CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle); - CollisionDispatch::sRegisterCollideShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCollideShape); - CollisionDispatch::sRegisterCastShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCastShape); + // Avoid registering triangle vs triangle as a reversed test to prevent infinite recursion + if (s != EShapeSubType::Triangle) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCollideShape); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCastShape); + } } // Specialized collision functions diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h index 02e4f0dcfea3..b56725b9340a 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h @@ -11,7 +11,7 @@ JPH_NAMESPACE_BEGIN /// Helper class to forward ShapeFilter calls to a SimShapeFilter /// INTERNAL CLASS DO NOT USE! -class SimShapeFilterWrapper : public ShapeFilter +class SimShapeFilterWrapper : private ShapeFilter { public: /// Constructor @@ -19,6 +19,8 @@ class SimShapeFilterWrapper : public ShapeFilter mFilter(inFilter), mBody1(inBody1) { + // Fall back to an empty filter if no simulation shape filter is set, this reduces the virtual call to 'return true' + mFinalFilter = inFilter != nullptr? this : &mDefault; } /// Forward to the simulation shape filter @@ -39,43 +41,18 @@ class SimShapeFilterWrapper : public ShapeFilter mBody2 = inBody2; } -private: - const SimShapeFilter * mFilter; - const Body * mBody1; - const Body * mBody2; -}; - -/// In case we don't have a simulation shape filter, we fall back to using a default shape filter that always returns true -/// INTERNAL CLASS DO NOT USE! -union SimShapeFilterWrapperUnion -{ -public: - /// Constructor - SimShapeFilterWrapperUnion(const SimShapeFilter *inFilter, const Body *inBody1) + /// Returns the actual filter to use for collision detection + const ShapeFilter & GetFilter() const { - // Dirty trick: if we don't have a filter, placement new a standard ShapeFilter so that we - // don't have to check for nullptr in the ShouldCollide function - if (inFilter != nullptr) - new (&mSimShapeFilterWrapper) SimShapeFilterWrapper(inFilter, inBody1); - else - new (&mSimShapeFilterWrapper) ShapeFilter(); - } - - /// Destructor - ~SimShapeFilterWrapperUnion() - { - // Doesn't need to be destructed - } - - /// Accessor - SimShapeFilterWrapper & GetSimShapeFilterWrapper() - { - return mSimShapeFilterWrapper; + return *mFinalFilter; } private: - SimShapeFilterWrapper mSimShapeFilterWrapper; - ShapeFilter mShapeFilter; + const ShapeFilter * mFinalFilter; + const SimShapeFilter * mFilter; + const Body * mBody1; + const Body * mBody2 = nullptr; + const ShapeFilter mDefault; }; JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h index 4c410b3b6d60..21557b1e7770 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h @@ -188,7 +188,7 @@ class JPH_EXPORT TransformedShape SubShapeIDCreator mSubShapeIDCreator; ///< Optional sub shape ID creator for the shape (can be used when expanding compound shapes into multiple transformed shapes) }; -static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(TransformedShape) == JPH_IF_SINGLE_PRECISION_ELSE(64, 96), "Not properly packed"); -static_assert(alignof(TransformedShape) == JPH_RVECTOR_ALIGNMENT, "Not properly aligned"); +static_assert(JPH_CPU_ADDRESS_BITS != 64 || JPH_RVECTOR_ALIGNMENT < 16 || sizeof(TransformedShape) == JPH_IF_SINGLE_PRECISION_ELSE(64, 96), "Not properly packed"); +static_assert(alignof(TransformedShape) == max(JPH_VECTOR_ALIGNMENT, JPH_RVECTOR_ALIGNMENT), "Not properly aligned"); JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h index 5127f3989577..a30a838ca122 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h @@ -94,6 +94,11 @@ class JPH_EXPORT ConstraintSettings : public SerializableObject, public RefTarge uint64 mUserData = 0; protected: + /// Don't allow (copy) constructing this base class, but allow derived classes to (copy) construct themselves + ConstraintSettings() = default; + ConstraintSettings(const ConstraintSettings &) = default; + ConstraintSettings & operator = (const ConstraintSettings &) = default; + /// This function should not be called directly, it is used by sRestoreFromBinaryState. virtual void RestoreBinaryState(StreamIn &inStream); }; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp index 687ab8957285..9e572dd8292b 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp @@ -794,8 +794,6 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated) { - JPH_PROFILE_FUNCTION(); - // Start with nothing found and not handled outConstraintCreated = false; outPairHandled = false; @@ -923,7 +921,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA const CachedContactPoint &ccp = output_cm->mContactPoints[i]; manifold.mRelativeContactPointsOn1[i] = transform_body1.Multiply3x3(Vec3::sLoadFloat3Unsafe(ccp.mPosition1)); manifold.mRelativeContactPointsOn2[i] = local_transform_body2 * Vec3::sLoadFloat3Unsafe(ccp.mPosition2); - penetration_depth = max(penetration_depth, (manifold.mRelativeContactPointsOn1[0] - manifold.mRelativeContactPointsOn2[0]).Dot(world_space_normal)); + penetration_depth = max(penetration_depth, (manifold.mRelativeContactPointsOn1[i] - manifold.mRelativeContactPointsOn2[i]).Dot(world_space_normal)); } manifold.mPenetrationDepth = penetration_depth; // We don't have the penetration depth anymore, estimate it @@ -996,8 +994,6 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2) { - JPH_PROFILE_FUNCTION(); - // Swap bodies so that body 1 id < body 2 id const Body *body1, *body2; if (inBody1.GetID() < inBody2.GetID()) diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp index 118e18ad0f69..86bc828eab3f 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp @@ -134,6 +134,25 @@ float HingeConstraint::GetCurrentAngle() const return diff.GetRotationAngle(rotation1 * mLocalSpaceHingeAxis1); } +void HingeConstraint::SetTargetOrientationBS(QuatArg inOrientation) +{ + // See: CalculateA1AndTheta + // + // The rotation between body 1 and 2 can be written as: + // + // q2 = q1 rh1 r0 + // + // where rh1 is a rotation around local hinge axis 1, also: + // + // q2 = q1 inOrientation + // + // This means: + // + // rh1 r0 = inOrientation <=> rh1 = inOrientation * r0^-1 + Quat rh1 = inOrientation * mInvInitialOrientation; + SetTargetAngle(rh1.GetRotationAngle(mLocalSpaceHingeAxis1)); +} + void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax) { JPH_ASSERT(inLimitsMin <= 0.0f && inLimitsMin >= -JPH_PI); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h index 691bb1f5f4b5..398ccc1a0626 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h @@ -123,6 +123,11 @@ class JPH_EXPORT HingeConstraint final : public TwoBodyConstraint void SetTargetAngle(float inAngle) { mTargetAngle = mHasLimits? Clamp(inAngle, mLimitsMin, mLimitsMax) : inAngle; } ///< rad float GetTargetAngle() const { return mTargetAngle; } + /// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2). + /// Calculates the local space target angle and calls SetTargetAngle. Motor state must be EMotorState::Position for this to have any effect. + /// May set the wrong angle if inOrientation contains large rotations around other axis than the hinge axis. + void SetTargetOrientationBS(QuatArg inOrientation); + /// Update the rotation limits of the hinge, value in radians (see HingeConstraintSettings) void SetLimits(float inLimitsMin, float inLimitsMax); float GetLimitsMin() const { return mLimitsMin; } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h index 2e05c2ce6f6f..abf432522b6b 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h @@ -60,6 +60,11 @@ class JPH_EXPORT PathConstraintPath : public SerializableObject, public RefTarge static PathResult sRestoreFromBinaryState(StreamIn &inStream); protected: + /// Don't allow (copy) constructing this base class, but allow derived classes to (copy) construct themselves + PathConstraintPath() = default; + PathConstraintPath(const PathConstraintPath &) = default; + PathConstraintPath &operator = (const PathConstraintPath &) = default; + /// This function should not be called directly, it is used by sRestoreFromBinaryState. virtual void RestoreBinaryState(StreamIn &inStream); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h index 092be68c1b46..75939f42b853 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h @@ -61,7 +61,7 @@ class JPH_EXPORT PulleyConstraint final : public TwoBodyConstraint public: JPH_OVERRIDE_NEW_DELETE - /// Construct distance constraint + /// Construct pulley constraint PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings); // Generic interface of a constraint diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp index 070a45e213d4..6ec73d689329 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp @@ -444,12 +444,12 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime) if (IsRotationFullyConstrained()) { // All rotation locked: Setup rotation constraint - mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), *mBody2, Mat44::sRotation(rotation2)); } else if (IsRotationConstrained() || mRotationMotorActive) { // GetRotationInConstraintSpace without redoing the calculation of constraint_body1_to_world - Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + Quat constraint_body2_to_world = rotation2 * mConstraintToBody2; Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world; // Use swing twist constraint part diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp index ed1064df9965..c5e9f56f36a2 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp @@ -97,8 +97,6 @@ uint32 IslandBuilder::GetLowestBodyIndex(uint32 inActiveBodyIndex) const void IslandBuilder::LinkBodies(uint32 inFirst, uint32 inSecond) { - JPH_PROFILE_FUNCTION(); - // Both need to be active, we don't want to create an island with static objects if (inFirst >= mMaxActiveBodies || inSecond >= mMaxActiveBodies) return; diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp index 507ed4c15d63..d4618a750a2b 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp @@ -973,7 +973,11 @@ void PhysicsSystem::sDefaultSimCollideBodyVsBody(const Body &inBody1, const Body { // Collide with enhanced internal edge removal ioCollideShapeSettings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll; - InternalEdgeRemovingCollector::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter); + InternalEdgeRemovingCollector::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + , inBody1.GetCenterOfMassPosition() // Query is done relative to the position of body 1 + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + ); } else { @@ -1033,8 +1037,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const settings.mActiveEdgeMovementDirection = body1->GetLinearVelocity() - body2->GetLinearVelocity(); // Create shape filter - SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, body1); - SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper(); + SimShapeFilterWrapper shape_filter(mSimShapeFilter, body1); shape_filter.SetBody2(body2); // Get transforms relative to body1 @@ -1156,7 +1159,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const ReductionCollideShapeCollector collector(this, body1, body2); // Perform collision detection between the two shapes - mSimCollideBodyVsBody(*body1, *body2, transform1, transform2, settings, collector, shape_filter); + mSimCollideBodyVsBody(*body1, *body2, transform1, transform2, settings, collector, shape_filter.GetFilter()); // Add the contacts for (ContactManifold &manifold : collector.mManifolds) @@ -1255,7 +1258,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle); // Perform collision detection between the two shapes - mSimCollideBodyVsBody(*body1, *body2, transform1, transform2, settings, collector, shape_filter); + mSimCollideBodyVsBody(*body1, *body2, transform1, transform2, settings, collector, shape_filter.GetFilter()); constraint_created = collector.mConstraintCreated; } @@ -1908,7 +1911,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph // Do narrow phase collision check RShapeCast relative_cast(mShapeCast.mShape, mShapeCast.mScale, mShapeCast.mCenterOfMassStart, direction, mShapeCast.mShapeWorldBounds); - body2.GetTransformedShape().CastShape(relative_cast, mShapeCastSettings, mShapeCast.mCenterOfMassStart.GetTranslation(), mCollector, mShapeFilter); + body2.GetTransformedShape().CastShape(relative_cast, mShapeCastSettings, mShapeCast.mCenterOfMassStart.GetTranslation(), mCollector, mShapeFilter.GetFilter()); // Update early out fraction based on narrow phase collector if (!mCollector.mRejectAll) @@ -1928,8 +1931,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph }; // Create shape filter - SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, &body); - SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper(); + SimShapeFilterWrapper shape_filter(mSimShapeFilter, &body); // Check if we collide with any other body. Note that we use the non-locking interface as we know the broadphase cannot be modified at this point. RShapeCast shape_cast(body.GetShape(), Vec3::sOne(), body.GetCenterOfMassTransform(), ccd_body.mDeltaPosition); diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h index d29cedababac..8ba7b1e1f39f 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h +++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h @@ -200,6 +200,12 @@ class JPH_EXPORT PhysicsSystem : public NonCopyable /// Returns a locking interface that locks the body so other threads cannot modify it. inline const BodyLockInterfaceLocking & GetBodyLockInterface() const { return mBodyLockInterfaceLocking; } + /// Broadphase layer filter that decides if two objects can collide, this was passed to the Init function. + const ObjectVsBroadPhaseLayerFilter &GetObjectVsBroadPhaseLayerFilter() const { return *mObjectVsBroadPhaseLayerFilter; } + + /// Object layer filter that decides if two objects can collide, this was passed to the Init function. + const ObjectLayerPairFilter &GetObjectLayerPairFilter() const { return *mObjectLayerPairFilter; } + /// Get an broadphase layer filter that uses the default pair filter and a specified object layer to determine if broadphase layers collide DefaultBroadPhaseLayerFilter GetDefaultBroadPhaseLayerFilter(ObjectLayer inLayer) const { return DefaultBroadPhaseLayerFilter(*mObjectVsBroadPhaseLayerFilter, inLayer); } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp index d3c7083637dd..ec9e3aa44893 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -635,6 +636,12 @@ void Ragdoll::DriveToPoseUsingMotors(const SkeletonPose &inPose) st_constraint->SetTwistMotorState(EMotorState::Position); st_constraint->SetTargetOrientationBS(joint_state.mRotation); } + else if (sub_type == EConstraintSubType::Hinge) + { + HingeConstraint *h_constraint = static_cast(constraint); + h_constraint->SetMotorState(EMotorState::Position); + h_constraint->SetTargetOrientationBS(joint_state.mRotation); + } else JPH_ASSERT(false, "Constraint type not implemented!"); } diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h index 27e185375f34..99dacd3ef6d5 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h @@ -44,11 +44,11 @@ class SoftBodyContactListener /// @return Whether the contact should be processed or not. virtual SoftBodyValidateResult OnSoftBodyContactValidate([[maybe_unused]] const Body &inSoftBody, [[maybe_unused]] const Body &inOtherBody, [[maybe_unused]] SoftBodyContactSettings &ioSettings) { return SoftBodyValidateResult::AcceptContact; } - /// Called after all contact points for a soft body have been handled. You only receive one callback per body pair per simulation step and can use inManifold to iterate through all contacts. + /// Called after all contact points for a soft body have been handled. /// Note that this callback is called when all bodies are locked, so don't use any locking functions! /// You will receive a single callback for a soft body per simulation step for performance reasons, this callback will apply to all vertices in the soft body. /// @param inSoftBody The soft body that collided. It is safe to access this as the soft body is only updated on the current thread. - /// @param inManifold The manifold that describes the contact surface between the two bodies. Other bodies may be modified by other threads during this callback. + /// @param inManifold The manifold that describes which vertices collide and with what body they collide. Other bodies may be modified by other threads during this callback. virtual void OnSoftBodyContactAdded([[maybe_unused]] const Body &inSoftBody, const SoftBodyManifold &inManifold) { /* Do nothing */ } }; diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp index 3ae026df64a0..d6a64ad0aac8 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp @@ -26,9 +26,11 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodyCreationSettings) JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mFriction) JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPressure) JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mGravityFactor) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mVertexRadius) JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUpdatePosition) JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMakeRotationIdentity) JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mAllowSleeping) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mFacesDoubleSided) } void SoftBodyCreationSettings::SaveBinaryState(StreamOut &inStream) const @@ -45,9 +47,11 @@ void SoftBodyCreationSettings::SaveBinaryState(StreamOut &inStream) const inStream.Write(mFriction); inStream.Write(mPressure); inStream.Write(mGravityFactor); + inStream.Write(mVertexRadius); inStream.Write(mUpdatePosition); inStream.Write(mMakeRotationIdentity); inStream.Write(mAllowSleeping); + inStream.Write(mFacesDoubleSided); } void SoftBodyCreationSettings::RestoreBinaryState(StreamIn &inStream) @@ -64,9 +68,11 @@ void SoftBodyCreationSettings::RestoreBinaryState(StreamIn &inStream) inStream.Read(mFriction); inStream.Read(mPressure); inStream.Read(mGravityFactor); + inStream.Read(mVertexRadius); inStream.Read(mUpdatePosition); inStream.Read(mMakeRotationIdentity); inStream.Read(mAllowSleeping); + inStream.Read(mFacesDoubleSided); } void SoftBodyCreationSettings::SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h index b5dc163767ec..537e1112ffd6 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h @@ -65,9 +65,11 @@ class JPH_EXPORT SoftBodyCreationSettings float mFriction = 0.2f; ///< Friction coefficient when colliding float mPressure = 0.0f; ///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure float mGravityFactor = 1.0f; ///< Value to multiply gravity with for this body + float mVertexRadius = 0.0f; ///< How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting bool mUpdatePosition = true; ///< Update the position of the body while simulating (set to false for something that is attached to the static world) bool mMakeRotationIdentity = true; ///< Bake specified mRotation in the vertices and set the body rotation to identity (simulation is slightly more accurate if the rotation of a soft body is kept to identity) bool mAllowSleeping = true; ///< If this body can go to sleep or not + bool mFacesDoubleSided = false; ///< If the faces in this soft body should be treated as double sided for the purpose of collision detection (ray cast / collide shape / cast shape) }; JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp index 1982ffcb55ab..4aa88d20c961 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp @@ -63,6 +63,8 @@ void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSett mNumIterations = inSettings.mNumIterations; mPressure = inSettings.mPressure; mUpdatePosition = inSettings.mUpdatePosition; + mFacesDoubleSided = inSettings.mFacesDoubleSided; + SetVertexRadius(inSettings.mVertexRadius); // Initialize vertices mVertices.resize(inSettings.mSettings->mVertices.size()); @@ -78,6 +80,20 @@ void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSett mLocalBounds.Encapsulate(out_vertex.mPosition); } + // Initialize rods + if (!inSettings.mSettings->mRodStretchShearConstraints.empty()) + { + mRodStates.resize(inSettings.mSettings->mRodStretchShearConstraints.size()); + Quat rotation_q = rotation.GetQuaternion(); + for (Array::size_type r = 0, s = mRodStates.size(); r < s; ++r) + { + const SoftBodySharedSettings::RodStretchShear &in_rod = inSettings.mSettings->mRodStretchShearConstraints[r]; + RodState &out_rod = mRodStates[r]; + out_rod.mRotation = rotation_q * in_rod.mBishop; + out_rod.mAngularVelocity = Vec3::sZero(); + } + } + // Allocate space for skinned vertices if (!inSettings.mSettings->mSkinnedConstraints.empty()) mSkinState.resize(mVertices.size()); @@ -170,7 +186,7 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont Array mHits; }; LeafShapeCollector collector; - body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sOne(), SubShapeIDCreator(), collector, mShapeFilter); + body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sOne(), SubShapeIDCreator(), collector, mShapeFilter.GetFilter()); if (collector.mHits.empty()) return; @@ -222,14 +238,13 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont // Calculate local bounding box AABox local_bounds = mLocalBounds; local_bounds.Encapsulate(mLocalPredictedBounds); - local_bounds.ExpandBy(Vec3::sReplicate(mSettings->mVertexRadius)); + local_bounds.ExpandBy(Vec3::sReplicate(mVertexRadius)); // Calculate world space bounding box AABox world_bounds = local_bounds.Transformed(inContext.mCenterOfMassTransform); // Create shape filter - SimShapeFilterWrapperUnion shape_filter_union(inContext.mSimShapeFilter, inContext.mBody); - SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper(); + SimShapeFilterWrapper shape_filter(inContext.mSimShapeFilter, inContext.mBody); Collector collector(inContext, inSystem, inBodyLockInterface, local_bounds, shape_filter, mCollidingShapes, mCollidingSensors); ObjectLayer layer = inContext.mBody->GetObjectLayer(); @@ -318,6 +333,7 @@ void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &i Vec3 sub_step_gravity = inContext.mGravity * dt; Vec3 sub_step_impulse = GetAccumulatedForce() * dt / max(float(mVertices.size()), 1.0f); for (Vertex &v : mVertices) + { if (v.mInvMass > 0.0f) { // Gravity @@ -325,17 +341,27 @@ void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &i // Damping v.mVelocity *= linear_damping; - - // Integrate - v.mPreviousPosition = v.mPosition; - v.mPosition += v.mVelocity * dt; - } - else - { - // Integrate - v.mPreviousPosition = v.mPosition; - v.mPosition += v.mVelocity * dt; } + + // Integrate + Vec3 position = v.mPosition; + v.mPreviousPosition = position; + v.mPosition = position + v.mVelocity * dt; + } + + // Integrate rod orientations + float half_dt = 0.5f * dt; + for (RodState &r : mRodStates) + { + // Damping + r.mAngularVelocity *= linear_damping; + + // Integrate + Quat rotation = r.mRotation; + Quat delta_rotation = half_dt * Quat::sMultiplyImaginary(r.mAngularVelocity, rotation); + r.mPreviousRotationInternal = rotation; // Overwrites mAngularVelocity + r.mRotation = (rotation + delta_rotation).Normalized(); + } } void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) @@ -574,6 +600,82 @@ void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext } } +void SoftBodyMotionProperties::ApplyRodStretchShearConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + RodState *rod_state = mRodStates.data() + inStartIndex; + for (const RodStretchShear *r = mSettings->mRodStretchShearConstraints.data() + inStartIndex, *r_end = mSettings->mRodStretchShearConstraints.data() + inEndIndex; r < r_end; ++r, ++rod_state) + { + // Get positions + Vertex &v0 = mVertices[r->mVertex[0]]; + Vertex &v1 = mVertices[r->mVertex[1]]; + + // Apply stretch and shear constraint + // Equation 37 from "Position and Orientation Based Cosserat Rods" - Kugelstadt and Schoemer - SIGGRAPH 2016 + float denom = v0.mInvMass + v1.mInvMass + 4.0f * r->mInvMass * Square(r->mLength) + r->mCompliance * inv_dt_sq; + if (denom < 1.0e-12f) + continue; + Vec3 x0 = v0.mPosition; + Vec3 x1 = v1.mPosition; + Quat rotation = rod_state->mRotation; + Vec3 d3 = rotation.RotateAxisZ(); + Vec3 delta = (x1 - x0 - d3 * r->mLength) / denom; + v0.mPosition = x0 + v0.mInvMass * delta; + v1.mPosition = x1 - v1.mInvMass * delta; + // q * e3_bar = q * (0, 0, -1, 0) = [-qy, qx, -qw, qz] + Quat q_e3_bar(rotation.GetXYZW().Swizzle().FlipSign<-1, 1, -1, 1>()); + rotation += (2.0f * r->mInvMass * r->mLength) * Quat::sMultiplyImaginary(delta, q_e3_bar); + + // Renormalize + rod_state->mRotation = rotation.Normalized(); + } +} + +void SoftBodyMotionProperties::ApplyRodBendTwistConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + const Array &rods = mSettings->mRodStretchShearConstraints; + + for (const RodBendTwist *r = mSettings->mRodBendTwistConstraints.data() + inStartIndex, *r_end = mSettings->mRodBendTwistConstraints.data() + inEndIndex; r < r_end; ++r) + { + uint32 rod1_index = r->mRod[0]; + uint32 rod2_index = r->mRod[1]; + const RodStretchShear &rod1 = rods[rod1_index]; + const RodStretchShear &rod2 = rods[rod2_index]; + RodState &rod1_state = mRodStates[rod1_index]; + RodState &rod2_state = mRodStates[rod2_index]; + + // Apply bend and twist constraint + // Equation 40 from "Position and Orientation Based Cosserat Rods" - Kugelstadt and Schoemer - SIGGRAPH 2016 + float denom = rod1.mInvMass + rod2.mInvMass + r->mCompliance * inv_dt_sq; + if (denom < 1.0e-12f) + continue; + Quat rotation1 = rod1_state.mRotation; + Quat rotation2 = rod2_state.mRotation; + Quat omega = rotation1.Conjugated() * rotation2; + Quat omega0 = r->mOmega0; + Vec4 omega_min_omega0 = (omega - omega0).GetXYZW(); + Vec4 omega_plus_omega0 = (omega + omega0).GetXYZW(); + // Take the shortest of the two rotations + Quat delta_omega(Vec4::sSelect(omega_min_omega0, omega_plus_omega0, Vec4::sLess(omega_plus_omega0.DotV(omega_plus_omega0), omega_min_omega0.DotV(omega_min_omega0)))); + delta_omega /= denom; + delta_omega.SetW(0.0f); // Scalar part needs to be zero because the real part of the Darboux vector doesn't vanish, see text between eq. 39 and 40. + Quat delta_rod2 = rod2.mInvMass * rotation1 * delta_omega; + rotation1 += rod1.mInvMass * rotation2 * delta_omega; + rotation2 -= delta_rod2; + + // Renormalize + rod1_state.mRotation = rotation1.Normalized(); + rod2_state.mRotation = rotation2.Normalized(); + } +} + void SoftBodyMotionProperties::ApplyLRAConstraints(uint inStartIndex, uint inEndIndex) { JPH_PROFILE_FUNCTION(); @@ -601,7 +703,7 @@ void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(cons float dt = inContext.mSubStepDeltaTime; float restitution_threshold = -2.0f * inContext.mGravity.Length() * dt; - float vertex_radius = mSettings->mVertexRadius; + float vertex_radius = mVertexRadius; for (Vertex &v : mVertices) if (v.mInvMass > 0.0f) { @@ -714,6 +816,11 @@ void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(cons } } } + + // Calculate the new angular velocity for all rods + float two_div_dt = 2.0f / dt; + for (RodState &r : mRodStates) + r.mAngularVelocity = two_div_dt * (r.mRotation * r.mPreviousRotationInternal.Conjugated()).GetXYZ(); // Overwrites mPreviousRotationInternal } void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) @@ -920,7 +1027,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSen void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex) { // Determine start and end - SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0 }; + SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0, 0, 0 }; const SoftBodySharedSettings::UpdateGroup &prev = inGroupIndex > 0? mSettings->mUpdateGroups[inGroupIndex - 1] : start; const SoftBodySharedSettings::UpdateGroup ¤t = mSettings->mUpdateGroups[inGroupIndex]; @@ -936,6 +1043,10 @@ void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioConte // Process edges ApplyEdgeConstraints(ioContext, prev.mEdgeEndIndex, current.mEdgeEndIndex); + // Process rods + ApplyRodStretchShearConstraints(ioContext, prev.mRodStretchShearEndIndex, current.mRodStretchShearEndIndex); + ApplyRodBendTwistConstraints(ioContext, prev.mRodBendTwistEndIndex, current.mRodBendTwistEndIndex); + // Process LRA constraints ApplyLRAConstraints(prev.mLRAEndIndex, current.mLRAEndIndex); } @@ -1192,6 +1303,62 @@ void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RM Color::sWhite); } +void SoftBodyMotionProperties::DrawRods(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mRodStretchShearEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const RodStretchShear &r = mSettings->mRodStretchShearConstraints[inIndex]; + inRenderer->DrawLine(inCenterOfMassTransform * mVertices[r.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[r.mVertex[1]].mPosition, inColor); + }, + Color::sWhite); +} + +void SoftBodyMotionProperties::DrawRodStates(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mRodStretchShearEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const RodState &state = mRodStates[inIndex]; + const RodStretchShear &rod = mSettings->mRodStretchShearConstraints[inIndex]; + + RVec3 x0 = inCenterOfMassTransform * mVertices[rod.mVertex[0]].mPosition; + RVec3 x1 = inCenterOfMassTransform * mVertices[rod.mVertex[1]].mPosition; + + RMat44 rod_center = inCenterOfMassTransform; + rod_center.SetTranslation(0.5_r * (x0 + x1)); + inRenderer->DrawArrow(rod_center.GetTranslation(), rod_center.GetTranslation() + state.mAngularVelocity, inColor, 0.01f * rod.mLength); + + RMat44 rod_frame = rod_center * RMat44::sRotation(state.mRotation); + inRenderer->DrawCoordinateSystem(rod_frame, 0.3f * rod.mLength); + }, + Color::sOrange); +} + +void SoftBodyMotionProperties::DrawRodBendTwistConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mRodBendTwistEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + uint r1 = mSettings->mRodBendTwistConstraints[inIndex].mRod[0]; + uint r2 = mSettings->mRodBendTwistConstraints[inIndex].mRod[1]; + const RodStretchShear &rod1 = mSettings->mRodStretchShearConstraints[r1]; + const RodStretchShear &rod2 = mSettings->mRodStretchShearConstraints[r2]; + + RVec3 x0 = inCenterOfMassTransform * (0.4f * mVertices[rod1.mVertex[0]].mPosition + 0.6f * mVertices[rod1.mVertex[1]].mPosition); + RVec3 x1 = inCenterOfMassTransform * (0.6f * mVertices[rod2.mVertex[0]].mPosition + 0.4f * mVertices[rod2.mVertex[1]].mPosition); + + inRenderer->DrawLine(x0, x1, inColor); + }, + Color::sGreen); +} + void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const { DrawConstraints(inConstraintColor, @@ -1279,11 +1446,16 @@ void SoftBodyMotionProperties::SaveState(StateRecorder &inStream) const for (const Vertex &v : mVertices) { - inStream.Write(v.mPreviousPosition); inStream.Write(v.mPosition); inStream.Write(v.mVelocity); } + for (const RodState &r : mRodStates) + { + inStream.Write(r.mRotation); + inStream.Write(r.mAngularVelocity); + } + for (const SkinState &s : mSkinState) { inStream.Write(s.mPreviousPosition); @@ -1303,11 +1475,16 @@ void SoftBodyMotionProperties::RestoreState(StateRecorder &inStream) for (Vertex &v : mVertices) { - inStream.Read(v.mPreviousPosition); inStream.Read(v.mPosition); inStream.Read(v.mVelocity); } + for (RodState &r : mRodStates) + { + inStream.Read(r.mRotation); + inStream.Read(r.mAngularVelocity); + } + for (SkinState &s : mSkinState) { inStream.Read(s.mPreviousPosition); diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h index 7ecf7cfce6fb..8768efd623bf 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h @@ -36,6 +36,8 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties public: using Vertex = SoftBodyVertex; using Edge = SoftBodySharedSettings::Edge; + using RodStretchShear = SoftBodySharedSettings::RodStretchShear; + using RodBendTwist = SoftBodySharedSettings::RodBendTwist; using Face = SoftBodySharedSettings::Face; using DihedralBend = SoftBodySharedSettings::DihedralBend; using Volume = SoftBodySharedSettings::Volume; @@ -58,6 +60,10 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties const Vertex & GetVertex(uint inIndex) const { return mVertices[inIndex]; } Vertex & GetVertex(uint inIndex) { return mVertices[inIndex]; } + /// Access to the state of rods + Quat GetRodRotation(uint inIndex) const { return mRodStates[inIndex].mRotation; } + Vec3 GetRodAngularVelocity(uint inIndex) const { return mRodStates[inIndex].mAngularVelocity; } + /// Get the materials of the soft body const PhysicsMaterialList & GetMaterials() const { return mSettings->mMaterials; } @@ -79,6 +85,10 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties bool GetUpdatePosition() const { return mUpdatePosition; } void SetUpdatePosition(bool inUpdatePosition) { mUpdatePosition = inUpdatePosition; } + /// If the faces in this soft body should be treated as double sided for the purpose of collision detection (ray cast / collide shape / cast shape) + bool GetFacesDoubleSided() const { return mFacesDoubleSided; } + void SetFacesDoubleSided(bool inDoubleSided) { mFacesDoubleSided = inDoubleSided; } + /// Global setting to turn on/off skin constraints bool GetEnableSkinConstraints() const { return mEnableSkinConstraints; } void SetEnableSkinConstraints(bool inEnableSkinConstraints) { mEnableSkinConstraints = inEnableSkinConstraints; } @@ -87,6 +97,10 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties float GetSkinnedMaxDistanceMultiplier() const { return mSkinnedMaxDistanceMultiplier; } void SetSkinnedMaxDistanceMultiplier(float inSkinnedMaxDistanceMultiplier) { mSkinnedMaxDistanceMultiplier = inSkinnedMaxDistanceMultiplier; } + /// How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting + float GetVertexRadius() const { return mVertexRadius; } + void SetVertexRadius(float inVertexRadius) { JPH_ASSERT(mVertexRadius >= 0.0f); mVertexRadius = inVertexRadius; } + /// Get local bounding box const AABox & GetLocalBounds() const { return mLocalBounds; } @@ -101,6 +115,9 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties void DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; void DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; void DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawRods(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawRodStates(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawRodBendTwistConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; void DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; void DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; void DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; @@ -208,6 +225,17 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties bool mHasContact; ///< If the sensor collided with the soft body }; + // Information about the current state of a rod. + struct RodState + { + Quat mRotation; ///< Rotation of the rod, relative to center of mass transform + union + { + Vec3 mAngularVelocity; ///< Angular velocity of the rod, relative to center of mass transform, valid only outside of the simulation. + Quat mPreviousRotationInternal; ///< Internal use only. Previous rotation of the rod, relative to center of mass transform, valid only during the simulation. + }; + }; + // Information about the state of all skinned vertices struct SkinState { @@ -240,6 +268,10 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties /// Enforce all edge constraints void ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + /// Enforce all rod constraints + void ApplyRodStretchShearConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + void ApplyRodBendTwistConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + /// Enforce all LRA constraints void ApplyLRAConstraints(uint inStartIndex, uint inEndIndex); @@ -280,6 +312,7 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties RefConst mSettings; ///< Configuration of the particles and constraints Array mVertices; ///< Current state of all vertices in the simulation + Array mRodStates; ///< Current state of all rods in the simulation Array mCollidingShapes; ///< List of colliding shapes retrieved during the last update Array mCollidingSensors; ///< List of colliding sensors retrieved during the last update Array mSkinState; ///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in) @@ -289,7 +322,9 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties uint mNumSensors; ///< Workaround for TSAN false positive: store mCollidingSensors.size() in a separate variable. float mPressure; ///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure float mSkinnedMaxDistanceMultiplier = 1.0f; ///< Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints + float mVertexRadius = 0.0f; ///< How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting bool mUpdatePosition; ///< Update the position of the body while simulating (set to false for something that is attached to the static world) + bool mFacesDoubleSided; ///< If the faces in this soft body should be treated as double sided for the purpose of collision detection (ray cast / collide shape / cast shape) atomic mNeedContactCallback = false; ///< True if the soft body has collided with anything in the last update bool mEnableSkinConstraints = true; ///< If skin constraints are enabled bool mSkinStatePreviousPositionValid = false; ///< True if the skinning was updated in the last update so that the previous position of the skin state is valid diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp index 890a0baac418..01b3da9ea360 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp @@ -83,6 +83,7 @@ void SoftBodyShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCa return; uint num_triangle_bits = GetSubShapeIDBits(); + bool check_backfaces = inRayCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && !mSoftBodyMotionProperties->GetFacesDoubleSided(); const Array &vertices = mSoftBodyMotionProperties->GetVertices(); for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) @@ -92,7 +93,7 @@ void SoftBodyShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCa Vec3 x3 = vertices[f.mVertex[2]].mPosition; // Back facing check - if (inRayCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && (x2 - x1).Cross(x3 - x1).Dot(inRay.mDirection) > 0.0f) + if (check_backfaces && (x2 - x1).Cross(x3 - x1).Dot(inRay.mDirection) > 0.0f) continue; // Test ray against triangle @@ -245,7 +246,10 @@ void SoftBodyShape::sCollideConvexVsSoftBody(const Shape *inShape1, const Shape const Array &faces = shape2->mSoftBodyMotionProperties->GetFaces(); uint num_triangle_bits = shape2->GetSubShapeIDBits(); - CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + CollideShapeSettings settings(inCollideShapeSettings); + if (shape2->mSoftBodyMotionProperties->GetFacesDoubleSided()) + settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces; + CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), settings, ioCollector); for (const SoftBodyMotionProperties::Face &f : faces) { Vec3 x1 = vertices[f.mVertex[0]].mPosition; @@ -267,7 +271,10 @@ void SoftBodyShape::sCollideSphereVsSoftBody(const Shape *inShape1, const Shape const Array &faces = shape2->mSoftBodyMotionProperties->GetFaces(); uint num_triangle_bits = shape2->GetSubShapeIDBits(); - CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + CollideShapeSettings settings(inCollideShapeSettings); + if (shape2->mSoftBodyMotionProperties->GetFacesDoubleSided()) + settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces; + CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), settings, ioCollector); for (const SoftBodyMotionProperties::Face &f : faces) { Vec3 x1 = vertices[f.mVertex[0]].mPosition; @@ -287,7 +294,10 @@ void SoftBodyShape::sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const Sh const Array &faces = shape->mSoftBodyMotionProperties->GetFaces(); uint num_triangle_bits = shape->GetSubShapeIDBits(); - CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + ShapeCastSettings settings(inShapeCastSettings); + if (shape->mSoftBodyMotionProperties->GetFacesDoubleSided()) + settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces; + CastConvexVsTriangles caster(inShapeCast, settings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); for (const SoftBodyMotionProperties::Face &f : faces) { Vec3 x1 = vertices[f.mVertex[0]].mPosition; @@ -307,7 +317,10 @@ void SoftBodyShape::sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const Sh const Array &faces = shape->mSoftBodyMotionProperties->GetFaces(); uint num_triangle_bits = shape->GetSubShapeIDBits(); - CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + ShapeCastSettings settings(inShapeCastSettings); + if (shape->mSoftBodyMotionProperties->GetFacesDoubleSided()) + settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces; + CastSphereVsTriangles caster(inShapeCast, settings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); for (const SoftBodyMotionProperties::Face &f : faces) { Vec3 x1 = vertices[f.mVertex[0]].mPosition; diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp index 59f8a2bc0885..403aee1d4c4a 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp @@ -36,6 +36,22 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Edge) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mCompliance) } +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::RodStretchShear) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mLength) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mInvMass) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mCompliance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mBishop) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::RodBendTwist) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mRod) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mCompliance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mOmega0) +} + JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::DihedralBend) { JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mVertex) @@ -87,8 +103,9 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mLRAConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mRodStretchShearConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mRodBendTwistConstraints) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials) - JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius) } void SoftBodySharedSettings::CalculateClosestKinematic() @@ -108,6 +125,11 @@ void SoftBodySharedSettings::CalculateClosestKinematic() connectivity[e.mVertex[0]].push_back(e.mVertex[1]); connectivity[e.mVertex[1]].push_back(e.mVertex[0]); } + for (const RodStretchShear &r : mRodStretchShearConstraints) + { + connectivity[r.mVertex[0]].push_back(r.mVertex[1]); + connectivity[r.mVertex[1]].push_back(r.mVertex[0]); + } // Use Dijkstra's algorithm to find the closest kinematic vertex for each vertex // See: https://en.wikipedia.org/wiki/Dijkstra's_algorithm @@ -131,6 +153,7 @@ void SoftBodySharedSettings::CalculateClosestKinematic() if (mVertices[v].mInvMass == 0.0f) { mClosestKinematic[v].mVertex = v; + mClosestKinematic[v].mHops = 0; mClosestKinematic[v].mDistance = 0.0f; to_visit.push_back({ v, 0.0f }); BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less { }); @@ -156,6 +179,7 @@ void SoftBodySharedSettings::CalculateClosestKinematic() { // Remember new closest vertex mClosestKinematic[v].mVertex = mClosestKinematic[current.mVertex].mVertex; + mClosestKinematic[v].mHops = mClosestKinematic[current.mVertex].mHops + 1; mClosestKinematic[v].mDistance = new_distance; to_visit.push_back({ v, new_distance }); BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less { }); @@ -241,10 +265,16 @@ void SoftBodySharedSettings::CreateConstraints(const VertexAttributes *inVertexA const VertexAttributes &a_opposite0 = attr(vopposite0); const VertexAttributes &a_opposite1 = attr(vopposite1); + // If the opposite vertices happen to be the same vertex then we have 2 triangles back to back and we skip creating shear / bend constraints + if (vopposite0 == vopposite1) + continue; + // Faces should be roughly in a plane Vec3 n0 = (Vec3(mVertices[f0.mVertex[2]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f0.mVertex[1]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition)); Vec3 n1 = (Vec3(mVertices[f1.mVertex[2]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f1.mVertex[1]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition)); - if (Square(n0.Dot(n1)) > sq_cos_tolerance * n0.LengthSq() * n1.LengthSq()) + float n0_dot_n1 = n0.Dot(n1); + if (n0_dot_n1 > 0.0f + && Square(n0_dot_n1) > sq_cos_tolerance * n0.LengthSq() * n1.LengthSq()) { // Faces should approximately form a quad Vec3 e0_dir = Vec3(mVertices[vopposite0].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition); @@ -344,15 +374,119 @@ void SoftBodySharedSettings::CalculateEdgeLengths() { for (Edge &e : mEdgeConstraints) { + JPH_ASSERT(e.mVertex[0] != e.mVertex[1], "Edges need to connect 2 different vertices"); e.mRestLength = (Vec3(mVertices[e.mVertex[1]].mPosition) - Vec3(mVertices[e.mVertex[0]].mPosition)).Length(); JPH_ASSERT(e.mRestLength > 0.0f); } } +void SoftBodySharedSettings::CalculateRodProperties() +{ + // Mark connections through bend twist constraints + Array> connections; + connections.resize(mRodStretchShearConstraints.size()); + for (const RodBendTwist &c : mRodBendTwistConstraints) + { + JPH_ASSERT(c.mRod[0] != c.mRod[1], "A bend twist constraint needs to be attached to different rods"); + connections[c.mRod[1]].push_back(c.mRod[0]); + connections[c.mRod[0]].push_back(c.mRod[1]); + } + + // Now calculate the Bishop frames for all rods + struct Entry + { + uint32 mFrom; // Rod we're coming from + uint32 mTo; // Rod we're going to + }; + Array stack; + stack.reserve(mRodStretchShearConstraints.size()); + for (uint32 r0_idx = 0; r0_idx < mRodStretchShearConstraints.size(); ++r0_idx) + { + RodStretchShear &r0 = mRodStretchShearConstraints[r0_idx]; + + // Do not calculate a 2nd time + if (r0.mBishop == Quat::sZero()) + { + // Calculate the frame for this rod + { + Vec3 tangent = Vec3(mVertices[r0.mVertex[1]].mPosition) - Vec3(mVertices[r0.mVertex[0]].mPosition); + r0.mLength = tangent.Length(); + JPH_ASSERT(r0.mLength > 0.0f, "Rods of zero length are not supported!"); + tangent /= r0.mLength; + Vec3 normal = tangent.GetNormalizedPerpendicular(); + Vec3 binormal = tangent.Cross(normal); + r0.mBishop = Mat44(Vec4(normal, 0), Vec4(binormal, 0), Vec4(tangent, 0), Vec4(0, 0, 0, 1)).GetQuaternion().Normalized(); + } + + // Add connected rods to the stack if they haven't been calculated yet + for (uint32 r1_idx : connections[r0_idx]) + if (mRodStretchShearConstraints[r1_idx].mBishop == Quat::sZero()) + stack.push_back({ r0_idx, r1_idx }); + + // Now connect the bishop frame for all connected rods on the stack + // This follows the procedure outlined in "Discrete Elastic Rods" - M. Bergou et al. + // See: https://www.cs.columbia.edu/cg/pdfs/143-rods.pdf + while (!stack.empty()) + { + uint32 r1_idx = stack.back().mFrom; + uint32 r2_idx = stack.back().mTo; + stack.pop_back(); + + const RodStretchShear &r1 = mRodStretchShearConstraints[r1_idx]; + RodStretchShear &r2 = mRodStretchShearConstraints[r2_idx]; + + // Get the normal and tangent of the first rod's Bishop frame (that was already calculated) + Mat44 r1_frame = Mat44::sRotation(r1.mBishop); + Vec3 tangent1 = r1_frame.GetAxisZ(); + Vec3 normal1 = r1_frame.GetAxisX(); + + // Calculate the Bishop frame for the 2nd rod + Vec3 tangent2 = Vec3(mVertices[r2.mVertex[1]].mPosition) - Vec3(mVertices[r2.mVertex[0]].mPosition); + if (tangent1.Dot(tangent2) < 0.0f) + { + // Edge is oriented in the opposite direction of the previous edge, flip it + std::swap(r2.mVertex[0], r2.mVertex[1]); + tangent2 = -tangent2; + } + r2.mLength = tangent2.Length(); + JPH_ASSERT(r2.mLength > 0.0f, "Rods of zero length are not supported!"); + tangent2 /= r2.mLength; + Vec3 t1_cross_t2 = tangent1.Cross(tangent2); + float sin_angle = t1_cross_t2.Length(); + Vec3 normal2 = normal1; + if (sin_angle > 1.0e-6f) + { + t1_cross_t2 /= sin_angle; + normal2 = Quat::sRotation(t1_cross_t2, ASin(sin_angle)) * normal2; + } + Vec3 binormal2 = tangent2.Cross(normal2); + r2.mBishop = Mat44(Vec4(normal2, 0), Vec4(binormal2, 0), Vec4(tangent2, 0), Vec4(0, 0, 0, 1)).GetQuaternion().Normalized(); + + // Add connected rods to the stack if they haven't been calculated yet + for (uint32 r3_idx : connections[r2_idx]) + if (mRodStretchShearConstraints[r3_idx].mBishop == Quat::sZero()) + stack.push_back({ r2_idx, r3_idx }); + } + } + } + + // Calculate inverse mass for all rods by taking the minimum inverse mass (aka the heaviest vertex) of both vertices + for (RodStretchShear &r : mRodStretchShearConstraints) + { + JPH_ASSERT(r.mVertex[0] != r.mVertex[1], "A rod stretch shear constraint requires two different vertices"); + r.mInvMass = min(mVertices[r.mVertex[0]].mInvMass, mVertices[r.mVertex[1]].mInvMass); + } + + // Calculate the initial rotation between the rods + for (RodBendTwist &r : mRodBendTwistConstraints) + r.mOmega0 = (mRodStretchShearConstraints[r.mRod[0]].mBishop.Conjugated() * mRodStretchShearConstraints[r.mRod[1]].mBishop).Normalized(); +} + void SoftBodySharedSettings::CalculateLRALengths(float inMaxDistanceMultiplier) { for (LRA &l : mLRAConstraints) { + JPH_ASSERT(l.mVertex[0] != l.mVertex[1], "LRA constraints need to connect 2 different vertices"); l.mMaxDistance = inMaxDistanceMultiplier * (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length(); JPH_ASSERT(l.mMaxDistance > 0.0f); } @@ -362,6 +496,10 @@ void SoftBodySharedSettings::CalculateBendConstraintConstants() { for (DihedralBend &b : mDihedralBendConstraints) { + JPH_ASSERT(b.mVertex[0] != b.mVertex[1] && b.mVertex[0] != b.mVertex[2] && b.mVertex[0] != b.mVertex[3] + && b.mVertex[1] != b.mVertex[2] && b.mVertex[1] != b.mVertex[3] + && b.mVertex[2] != b.mVertex[3], "Bend constraints need 4 different vertices"); + // Get positions Vec3 x0 = Vec3(mVertices[b.mVertex[0]].mPosition); Vec3 x1 = Vec3(mVertices[b.mVertex[1]].mPosition); @@ -401,6 +539,10 @@ void SoftBodySharedSettings::CalculateVolumeConstraintVolumes() { for (Volume &v : mVolumeConstraints) { + JPH_ASSERT(v.mVertex[0] != v.mVertex[1] && v.mVertex[0] != v.mVertex[2] && v.mVertex[0] != v.mVertex[3] + && v.mVertex[1] != v.mVertex[2] && v.mVertex[1] != v.mVertex[3] + && v.mVertex[2] != v.mVertex[3], "Volume constraints need 4 different vertices"); + Vec3 x1(mVertices[v.mVertex[0]].mPosition); Vec3 x2(mVertices[v.mVertex[1]].mPosition); Vec3 x3(mVertices[v.mVertex[2]].mPosition); @@ -504,6 +646,15 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) add_connection(c.mVertex[0], c.mVertex[1]); for (const LRA &c : mLRAConstraints) add_connection(c.mVertex[0], c.mVertex[1]); + for (const RodStretchShear &c : mRodStretchShearConstraints) + add_connection(c.mVertex[0], c.mVertex[1]); + for (const RodBendTwist &c : mRodBendTwistConstraints) + { + add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[0], mRodStretchShearConstraints[c.mRod[1]].mVertex[0]); + add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[1], mRodStretchShearConstraints[c.mRod[1]].mVertex[0]); + add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[0], mRodStretchShearConstraints[c.mRod[1]].mVertex[1]); + add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[1], mRodStretchShearConstraints[c.mRod[1]].mVertex[1]); + } for (const DihedralBend &c : mDihedralBendConstraints) { add_connection(c.mVertex[0], c.mVertex[1]); @@ -653,11 +804,13 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) { uint GetSize() const { - return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size(); + return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mRodStretchShearConstraints.size() + (uint)mRodBendTwistConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size(); } Array mEdgeConstraints; Array mLRAConstraints; + Array mRodStretchShearConstraints; + Array mRodBendTwistConstraints; Array mDihedralBendConstraints; Array mVolumeConstraints; Array mSkinnedConstraints; @@ -684,6 +837,28 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) else // In different groups -> parallel group groups.back().mLRAConstraints.push_back(uint(&l - mLRAConstraints.data())); } + for (const RodStretchShear &r : mRodStretchShearConstraints) + { + int g1 = group_idx[r.mVertex[0]]; + int g2 = group_idx[r.mVertex[1]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0); + if (g1 == g2) // In the same group + groups[g1].mRodStretchShearConstraints.push_back(uint(&r - mRodStretchShearConstraints.data())); + else // In different groups -> parallel group + groups.back().mRodStretchShearConstraints.push_back(uint(&r - mRodStretchShearConstraints.data())); + } + for (const RodBendTwist &r : mRodBendTwistConstraints) + { + int g1 = group_idx[mRodStretchShearConstraints[r.mRod[0]].mVertex[0]]; + int g2 = group_idx[mRodStretchShearConstraints[r.mRod[0]].mVertex[1]]; + int g3 = group_idx[mRodStretchShearConstraints[r.mRod[1]].mVertex[0]]; + int g4 = group_idx[mRodStretchShearConstraints[r.mRod[1]].mVertex[1]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0); + if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group + groups[g1].mRodBendTwistConstraints.push_back(uint(&r - mRodBendTwistConstraints.data())); + else // In different groups -> parallel group + groups.back().mRodBendTwistConstraints.push_back(uint(&r - mRodBendTwistConstraints.data())); + } for (const DihedralBend &d : mDihedralBendConstraints) { int g1 = group_idx[d.mVertex[0]]; @@ -767,6 +942,81 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) return inLHS < inRHS; }); + // Sort the rod stretch shear constraints + QuickSort(group.mRodStretchShearConstraints.begin(), group.mRodStretchShearConstraints.end(), [this](uint inLHS, uint inRHS) + { + const RodStretchShear &r1 = mRodStretchShearConstraints[inLHS]; + const RodStretchShear &r2 = mRodStretchShearConstraints[inRHS]; + + // First sort so that the rod with the smallest distance to a kinematic vertex comes first + float d1 = min(mClosestKinematic[r1.mVertex[0]].mDistance, mClosestKinematic[r1.mVertex[1]].mDistance); + float d2 = min(mClosestKinematic[r2.mVertex[0]].mDistance, mClosestKinematic[r2.mVertex[1]].mDistance); + if (d1 != d2) + return d1 < d2; + + // Then sort on the rod that connects to the smallest kinematic vertex + uint32 m1 = min(mClosestKinematic[r1.mVertex[0]].mVertex, mClosestKinematic[r1.mVertex[1]].mVertex); + uint32 m2 = min(mClosestKinematic[r2.mVertex[0]].mVertex, mClosestKinematic[r2.mVertex[1]].mVertex); + if (m1 != m2) + return m1 < m2; + + // Order the rods so that the ones with the smallest index go first (hoping to get better cache locality when we process the rods). + m1 = r1.GetMinVertexIndex(); + m2 = r2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the rod bend twist constraints + QuickSort(group.mRodBendTwistConstraints.begin(), group.mRodBendTwistConstraints.end(), [this](uint inLHS, uint inRHS) + { + const RodBendTwist &b1 = mRodBendTwistConstraints[inLHS]; + const RodStretchShear &b1_r1 = mRodStretchShearConstraints[b1.mRod[0]]; + const RodStretchShear &b1_r2 = mRodStretchShearConstraints[b1.mRod[1]]; + + const RodBendTwist &b2 = mRodBendTwistConstraints[inRHS]; + const RodStretchShear &b2_r1 = mRodStretchShearConstraints[b2.mRod[0]]; + const RodStretchShear &b2_r2 = mRodStretchShearConstraints[b2.mRod[1]]; + + // First sort so that the rod with the smallest number of hops to a kinematic vertex comes first. + // Note that we don't use distance because of the bilateral interleaving below. + uint32 m1 = min( + min(mClosestKinematic[b1_r1.mVertex[0]].mHops, mClosestKinematic[b1_r1.mVertex[1]].mHops), + min(mClosestKinematic[b1_r2.mVertex[0]].mHops, mClosestKinematic[b1_r2.mVertex[1]].mHops)); + uint32 m2 = min( + min(mClosestKinematic[b2_r1.mVertex[0]].mHops, mClosestKinematic[b2_r1.mVertex[1]].mHops), + min(mClosestKinematic[b2_r2.mVertex[0]].mHops, mClosestKinematic[b2_r2.mVertex[1]].mHops)); + if (m1 != m2) + return m1 < m2; + + // Then sort on the rod that connects to the kinematic vertex with lowest index. + // This ensures that we consistently order the rods that are attached to other kinematic constraints. + // Again, this helps bilateral interleaving below. + m1 = min( + min(mClosestKinematic[b1_r1.mVertex[0]].mVertex, mClosestKinematic[b1_r1.mVertex[1]].mVertex), + min(mClosestKinematic[b1_r2.mVertex[0]].mVertex, mClosestKinematic[b1_r2.mVertex[1]].mVertex)); + m2 = min( + min(mClosestKinematic[b2_r1.mVertex[0]].mVertex, mClosestKinematic[b2_r1.mVertex[1]].mVertex), + min(mClosestKinematic[b2_r2.mVertex[0]].mVertex, mClosestKinematic[b2_r2.mVertex[1]].mVertex)); + if (m1 != m2) + return m1 < m2; + + // Finally order so that the smallest vertex index goes first + m1 = min(b1_r1.GetMinVertexIndex(), b1_r2.GetMinVertexIndex()); + m2 = min(b2_r1.GetMinVertexIndex(), b2_r2.GetMinVertexIndex()); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Bilateral interleaving, see figure 4 of "Position and Orientation Based Cosserat Rods" - Kugelstadt and Schoemer - SIGGRAPH 2016 + // Keeping the twist constraints sorted often results in an unstable simulation + for (Array::size_type i = 1, s = group.mRodBendTwistConstraints.size(), s2 = s >> 1; i < s2; i += 2) + std::swap(group.mRodBendTwistConstraints[i], group.mRodBendTwistConstraints[s - i]); + // Sort the dihedral bend constraints QuickSort(group.mDihedralBendConstraints.begin(), group.mDihedralBendConstraints.end(), [this](uint inLHS, uint inRHS) { @@ -783,7 +1033,7 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) if (d1 != d2) return d1 < d2; - // Order constraints so that the ones with the smallest index go first + // Finally order so that the smallest vertex index goes first uint32 m1 = b1.GetMinVertexIndex(); uint32 m2 = b2.GetMinVertexIndex(); if (m1 != m2) @@ -835,27 +1085,37 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) Array temp_edges; temp_edges.swap(mEdgeConstraints); mEdgeConstraints.reserve(temp_edges.size()); - outResults.mEdgeRemap.reserve(temp_edges.size()); + outResults.mEdgeRemap.resize(temp_edges.size(), ~uint(0)); Array temp_lra; temp_lra.swap(mLRAConstraints); mLRAConstraints.reserve(temp_lra.size()); - outResults.mLRARemap.reserve(temp_lra.size()); + outResults.mLRARemap.resize(temp_lra.size(), ~uint(0)); + + Array temp_rod_stretch_shear; + temp_rod_stretch_shear.swap(mRodStretchShearConstraints); + mRodStretchShearConstraints.reserve(temp_rod_stretch_shear.size()); + outResults.mRodStretchShearConstraintRemap.resize(temp_rod_stretch_shear.size(), ~uint(0)); + + Array temp_rod_bend_twist; + temp_rod_bend_twist.swap(mRodBendTwistConstraints); + mRodBendTwistConstraints.reserve(temp_rod_bend_twist.size()); + outResults.mRodBendTwistConstraintRemap.resize(temp_rod_bend_twist.size(), ~uint(0)); Array temp_dihedral_bend; temp_dihedral_bend.swap(mDihedralBendConstraints); mDihedralBendConstraints.reserve(temp_dihedral_bend.size()); - outResults.mDihedralBendRemap.reserve(temp_dihedral_bend.size()); + outResults.mDihedralBendRemap.resize(temp_dihedral_bend.size(), ~uint(0)); Array temp_volume; temp_volume.swap(mVolumeConstraints); mVolumeConstraints.reserve(temp_volume.size()); - outResults.mVolumeRemap.reserve(temp_volume.size()); + outResults.mVolumeRemap.resize(temp_volume.size(), ~uint(0)); Array temp_skinned; temp_skinned.swap(mSkinnedConstraints); mSkinnedConstraints.reserve(temp_skinned.size()); - outResults.mSkinnedRemap.reserve(temp_skinned.size()); + outResults.mSkinnedRemap.resize(temp_skinned.size(), ~uint(0)); // Finalize update groups for (const Group &group : groups) @@ -863,42 +1123,61 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) // Reorder edge constraints for this group for (uint idx : group.mEdgeConstraints) { + outResults.mEdgeRemap[idx] = (uint)mEdgeConstraints.size(); mEdgeConstraints.push_back(temp_edges[idx]); - outResults.mEdgeRemap.push_back(idx); } // Reorder LRA constraints for this group for (uint idx : group.mLRAConstraints) { + outResults.mLRARemap[idx] = (uint)mLRAConstraints.size(); mLRAConstraints.push_back(temp_lra[idx]); - outResults.mLRARemap.push_back(idx); + } + + // Reorder rod stretch shear constraints for this group + for (uint idx : group.mRodStretchShearConstraints) + { + outResults.mRodStretchShearConstraintRemap[idx] = (uint)mRodStretchShearConstraints.size(); + mRodStretchShearConstraints.push_back(temp_rod_stretch_shear[idx]); + } + + // Reorder rod bend twist constraints for this group + for (uint idx : group.mRodBendTwistConstraints) + { + outResults.mRodBendTwistConstraintRemap[idx] = (uint)mRodBendTwistConstraints.size(); + mRodBendTwistConstraints.push_back(temp_rod_bend_twist[idx]); } // Reorder dihedral bend constraints for this group for (uint idx : group.mDihedralBendConstraints) { + outResults.mDihedralBendRemap[idx] = (uint)mDihedralBendConstraints.size(); mDihedralBendConstraints.push_back(temp_dihedral_bend[idx]); - outResults.mDihedralBendRemap.push_back(idx); } // Reorder volume constraints for this group for (uint idx : group.mVolumeConstraints) { + outResults.mVolumeRemap[idx] = (uint)mVolumeConstraints.size(); mVolumeConstraints.push_back(temp_volume[idx]); - outResults.mVolumeRemap.push_back(idx); } // Reorder skinned constraints for this group for (uint idx : group.mSkinnedConstraints) { + outResults.mSkinnedRemap[idx] = (uint)mSkinnedConstraints.size(); mSkinnedConstraints.push_back(temp_skinned[idx]); - outResults.mSkinnedRemap.push_back(idx); } // Store end indices - mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() }); + mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mRodStretchShearConstraints.size(), (uint)mRodBendTwistConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() }); } + // Remap bend twist indices because mRodStretchShearConstraints has been reordered + for (RodBendTwist &r : mRodBendTwistConstraints) + for (int i = 0; i < 2; ++i) + r.mRod[i] = outResults.mRodStretchShearConstraintRemap[r.mRod[i]]; + // Free closest kinematic buffer mClosestKinematic.clear(); mClosestKinematic.shrink_to_fit(); @@ -916,8 +1195,9 @@ Ref SoftBodySharedSettings::Clone() const clone->mSkinnedConstraintNormals = mSkinnedConstraintNormals; clone->mInvBindMatrices = mInvBindMatrices; clone->mLRAConstraints = mLRAConstraints; + clone->mRodStretchShearConstraints = mRodStretchShearConstraints; + clone->mRodBendTwistConstraints = mRodBendTwistConstraints; clone->mMaterials = mMaterials; - clone->mVertexRadius = mVertexRadius; clone->mUpdateGroups = mUpdateGroups; return clone; } @@ -932,9 +1212,24 @@ void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const inStream.Write(mSkinnedConstraints); inStream.Write(mSkinnedConstraintNormals); inStream.Write(mLRAConstraints); - inStream.Write(mVertexRadius); inStream.Write(mUpdateGroups); + // Can't write mRodStretchShearConstraints directly because the class contains padding + inStream.Write(mRodStretchShearConstraints, [](const RodStretchShear &inElement, StreamOut &inS) { + inS.Write(inElement.mVertex); + inS.Write(inElement.mLength); + inS.Write(inElement.mInvMass); + inS.Write(inElement.mCompliance); + inS.Write(inElement.mBishop); + }); + + // Can't write mRodBendTwistConstraints directly because the class contains padding + inStream.Write(mRodBendTwistConstraints, [](const RodBendTwist &inElement, StreamOut &inS) { + inS.Write(inElement.mRod); + inS.Write(inElement.mCompliance); + inS.Write(inElement.mOmega0); + }); + // Can't write mInvBindMatrices directly because the class contains padding inStream.Write(mInvBindMatrices, [](const InvBind &inElement, StreamOut &inS) { inS.Write(inElement.mJointIndex); @@ -952,9 +1247,22 @@ void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream) inStream.Read(mSkinnedConstraints); inStream.Read(mSkinnedConstraintNormals); inStream.Read(mLRAConstraints); - inStream.Read(mVertexRadius); inStream.Read(mUpdateGroups); + inStream.Read(mRodStretchShearConstraints, [](StreamIn &inS, RodStretchShear &outElement) { + inS.Read(outElement.mVertex); + inS.Read(outElement.mLength); + inS.Read(outElement.mInvMass); + inS.Read(outElement.mCompliance); + inS.Read(outElement.mBishop); + }); + + inStream.Read(mRodBendTwistConstraints, [](StreamIn &inS, RodBendTwist &outElement) { + inS.Read(outElement.mRod); + inS.Read(outElement.mCompliance); + inS.Read(outElement.mOmega0); + }); + inStream.Read(mInvBindMatrices, [](StreamIn &inS, InvBind &outElement) { inS.Read(outElement.mJointIndex); inS.Read(outElement.mInvBind); diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h index 3fc7342d3813..14997a4915a5 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h +++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h @@ -59,6 +59,10 @@ class JPH_EXPORT SoftBodySharedSettings : public RefTarget mEdgeRemap; ///< Maps old edge index to new edge index Array mLRARemap; ///< Maps old LRA index to new LRA index + Array mRodStretchShearConstraintRemap; ///< Maps old rod stretch shear constraint index to new stretch shear rod constraint index + Array mRodBendTwistConstraintRemap; ///< Maps old rod bend twist constraint index to new bend twist rod constraint index Array mDihedralBendRemap; ///< Maps old dihedral bend index to new dihedral bend index Array mVolumeRemap; ///< Maps old volume constraint index to new volume constraint index Array mSkinnedRemap; ///< Maps old skinned constraint index to new skinned constraint index @@ -160,7 +166,7 @@ class JPH_EXPORT SoftBodySharedSettings : public RefTarget= mMaxDistance. The faces surrounding mVertex determine an average normal. mBackStopDistance behind the vertex in the opposite direction of this normal, the back stop sphere starts. The simulated vertex will be pushed out of this sphere and it can be used to approximate the volume of the skinned mesh behind the skinned vertex. float mBackStopRadius = 40.0f; ///< Radius of the backstop sphere. By default this is a fairly large radius so the sphere approximates a plane. - uint32 mNormalInfo = 0; ///< Information needed to calculate the normal of this vertex, lowest 24 bit is start index in mSkinnedConstraintNormals, highest 8 bit is number of faces (generated by CalculateSkinnedConstraintNormals()) + uint32 mNormalInfo = 0; ///< Information needed to calculate the normal of this vertex, lowest 24 bit is start index in mSkinnedConstraintNormals, highest 8 bit is number of faces (generated by CalculateSkinnedConstraintNormals) }; /// A long range attachment constraint, this is a constraint that sets a max distance between a kinematic vertex and a dynamic vertex @@ -293,7 +299,46 @@ class JPH_EXPORT SoftBodySharedSettings : public RefTarget mSkinnedConstraints; ///< The list of vertices that are constrained to a skinned vertex Array mInvBindMatrices; ///< The list of inverse bind matrices for skinning vertices Array mLRAConstraints; ///< The list of long range attachment constraints + Array mRodStretchShearConstraints; ///< The list of Cosserat rod constraints that connect two vertices and that limit stretch and shear + Array mRodBendTwistConstraints; ///< The list of Cosserat rod constraints that connect two rods and limit the bend and twist PhysicsMaterialList mMaterials { PhysicsMaterial::sDefault }; ///< The materials of the faces of the body, referenced by Face::mMaterialIndex - float mVertexRadius = 0.0f; ///< How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting private: friend class SoftBodyMotionProperties; @@ -320,6 +366,7 @@ class JPH_EXPORT SoftBodySharedSettings : public RefTarget mClosestKinematic; ///< The closest kinematic vertex to each vertex in mVertices Array mUpdateGroups; ///< The end indices for each group of constraints that can be updated in parallel - Array mSkinnedConstraintNormals; ///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals() + Array mSkinnedConstraintNormals; ///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals }; JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp index 1bfa564f76fb..acfa1a4d4545 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp @@ -17,7 +17,7 @@ JPH_NAMESPACE_BEGIN JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MotorcycleControllerSettings) { - JPH_ADD_BASE_CLASS(MotorcycleControllerSettings, VehicleControllerSettings) + JPH_ADD_BASE_CLASS(MotorcycleControllerSettings, WheeledVehicleControllerSettings) JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mMaxLeanAngle) JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringConstant) @@ -290,4 +290,17 @@ void MotorcycleController::Draw(DebugRenderer *inRenderer) const #endif // JPH_DEBUG_RENDERER +Ref MotorcycleController::GetSettings() const +{ + MotorcycleControllerSettings *settings = new MotorcycleControllerSettings; + ToSettings(*settings); + settings->mMaxLeanAngle = mMaxLeanAngle; + settings->mLeanSpringConstant = mLeanSpringConstant; + settings->mLeanSpringDamping = settings->mLeanSpringDamping; + settings->mLeanSpringIntegrationCoefficient = mLeanSpringIntegrationCoefficient; + settings->mLeanSpringIntegrationCoefficientDecay = mLeanSpringIntegrationCoefficientDecay; + settings->mLeanSmoothingFactor = mLeanSmoothingFactor; + return settings; +} + JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h index bf8ebfa43f4b..374befff64ca 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h @@ -83,6 +83,9 @@ class JPH_EXPORT MotorcycleController : public WheeledVehicleController void SetLeanSmoothingFactor(float inFactor) { mLeanSmoothingFactor = inFactor; } float GetLeanSmoothingFactor() const { return mLeanSmoothingFactor; } + // See: VehicleController + virtual Ref GetSettings() const override; + protected: // See: VehicleController virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp index d3cfcffc54e0..69acaa694c59 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp @@ -26,18 +26,24 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TrackedVehicleControllerSettings) JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsTV) { + JPH_ADD_BASE_CLASS(WheelSettingsTV, WheelSettings) + JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLongitudinalFriction) JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLateralFriction) } void WheelSettingsTV::SaveBinaryState(StreamOut &inStream) const { + WheelSettings::SaveBinaryState(inStream); + inStream.Write(mLongitudinalFriction); inStream.Write(mLateralFriction); } void WheelSettingsTV::RestoreBinaryState(StreamIn &inStream) { + WheelSettings::RestoreBinaryState(inStream); + inStream.Read(mLongitudinalFriction); inStream.Read(mLateralFriction); } @@ -528,4 +534,14 @@ void TrackedVehicleController::RestoreState(StateRecorder &inStream) t.RestoreState(inStream); } +Ref TrackedVehicleController::GetSettings() const +{ + TrackedVehicleControllerSettings *settings = new TrackedVehicleControllerSettings; + settings->mEngine = static_cast(mEngine); + settings->mTransmission = static_cast(mTransmission); + for (size_t i = 0; i < std::size(mTracks); ++i) + settings->mTracks[i] = static_cast(mTracks[i]); + return settings; +} + JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h index 0bca9de5cd33..8fc7869580da 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h @@ -129,6 +129,9 @@ class JPH_EXPORT TrackedVehicleController : public VehicleController void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; } #endif // JPH_DEBUG_RENDERER + // See: VehicleController + virtual Ref GetSettings() const override; + protected: /// Synchronize angular velocities of left and right tracks according to their ratios void SyncLeftRightTracks(); diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp index e3ec501158fa..1d84ea3eb00f 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp @@ -687,8 +687,17 @@ void VehicleConstraint::RestoreState(StateRecorder &inStream) Ref VehicleConstraint::GetConstraintSettings() const { - JPH_ASSERT(false); // Not implemented yet - return nullptr; + VehicleConstraintSettings *settings = new VehicleConstraintSettings; + ToConstraintSettings(*settings); + settings->mUp = mUp; + settings->mForward = mForward; + settings->mMaxPitchRollAngle = ACos(mCosMaxPitchRollAngle); + settings->mWheels.resize(mWheels.size()); + for (Wheels::size_type w = 0; w < mWheels.size(); ++w) + settings->mWheels[w] = const_cast(mWheels[w]->mSettings.GetPtr()); + settings->mAntiRollBars = mAntiRollBars; + settings->mController = mController->GetSettings(); + return settings; } JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h index 6489b2bf0494..3c1ac78ce60a 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h @@ -74,9 +74,11 @@ class JPH_EXPORT VehicleConstraint : public Constraint, public PhysicsStepListen /// Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off. void SetMaxPitchRollAngle(float inMaxPitchRollAngle) { mCosMaxPitchRollAngle = Cos(inMaxPitchRollAngle); } + float GetMaxPitchRollAngle() const { return ACos(mCosMaxPitchRollAngle); } /// Set the interface that tests collision between wheel and ground void SetVehicleCollisionTester(const VehicleCollisionTester *inTester) { mVehicleCollisionTester = inTester; } + const VehicleCollisionTester *GetVehicleCollisionTester() const { return mVehicleCollisionTester; } /// Callback function to combine the friction of a tire with the friction of the body it is colliding with. /// On input ioLongitudinalFriction and ioLateralFriction contain the friction of the tire, on output they should contain the combined friction with inBody2. diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h index a07f4fd9a325..155ff22f6231 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h @@ -50,6 +50,9 @@ class JPH_EXPORT VehicleController : public NonCopyable VehicleConstraint & GetConstraint() { return mConstraint; } const VehicleConstraint & GetConstraint() const { return mConstraint; } + /// Recreate the settings for this controller + virtual Ref GetSettings() const = 0; + protected: // The functions below are only for the VehicleConstraint friend class VehicleConstraint; diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp index 8e4a71a4fb85..8f9448ede11d 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp @@ -31,6 +31,8 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheeledVehicleControllerSettings) JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsWV) { + JPH_ADD_BASE_CLASS(WheelSettingsWV, WheelSettings) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mInertia) JPH_ADD_ATTRIBUTE(WheelSettingsWV, mAngularDamping) JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxSteerAngle) @@ -55,6 +57,8 @@ WheelSettingsWV::WheelSettingsWV() void WheelSettingsWV::SaveBinaryState(StreamOut &inStream) const { + WheelSettings::SaveBinaryState(inStream); + inStream.Write(mInertia); inStream.Write(mAngularDamping); inStream.Write(mMaxSteerAngle); @@ -66,6 +70,8 @@ void WheelSettingsWV::SaveBinaryState(StreamOut &inStream) const void WheelSettingsWV::RestoreBinaryState(StreamIn &inStream) { + WheelSettings::RestoreBinaryState(inStream); + inStream.Read(mInertia); inStream.Read(mAngularDamping); inStream.Read(mMaxSteerAngle); @@ -842,4 +848,19 @@ void WheeledVehicleController::RestoreState(StateRecorder &inStream) mTransmission.RestoreState(inStream); } +void WheeledVehicleController::ToSettings(WheeledVehicleControllerSettings &outSettings) const +{ + outSettings.mEngine = static_cast(mEngine); + outSettings.mTransmission = static_cast(mTransmission); + outSettings.mDifferentials = mDifferentials; + outSettings.mDifferentialLimitedSlipRatio = mDifferentialLimitedSlipRatio; +} + +Ref WheeledVehicleController::GetSettings() const +{ + WheeledVehicleControllerSettings *settings = new WheeledVehicleControllerSettings; + ToSettings(*settings); + return settings; +} + JPH_NAMESPACE_END diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h index 0bfd66fd8b4c..487f60c56aba 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h @@ -155,7 +155,13 @@ class JPH_EXPORT WheeledVehicleController : public VehicleController void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; } #endif // JPH_DEBUG_RENDERER + // See: VehicleController + virtual Ref GetSettings() const override; + protected: + /// Convert controller back to settings + void ToSettings(WheeledVehicleControllerSettings &outSettings) const; + // See: VehicleController virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsWV))); return new WheelWV(static_cast(inWheel)); } virtual bool AllowSleep() const override;